TrbStateVec.h

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2009 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 
00015 */
00016 
00017 #ifndef BT_TRBSTATEVEC_H__
00018 #define BT_TRBSTATEVEC_H__
00019 
00020 #include <stdlib.h>
00021 #ifdef PFX_USE_FREE_VECTORMATH
00022 #include "vecmath/vmInclude.h"
00023 #else
00024 #include "vectormath/vmInclude.h"
00025 #endif //PFX_USE_FREE_VECTORMATH
00026 
00027 
00028 #include "PlatformDefinitions.h"
00029 
00030 
00031 static inline vmVector3 read_Vector3(const float* p)
00032 {
00033         vmVector3 v;
00034         loadXYZ(v, p);
00035         return v;
00036 }
00037 
00038 static inline vmQuat read_Quat(const float* p)
00039 {
00040         vmQuat vq;
00041         loadXYZW(vq, p);
00042         return vq;
00043 }
00044 
00045 static inline void store_Vector3(const vmVector3 &src, float* p)
00046 {
00047         vmVector3 v = src;
00048         storeXYZ(v, p);
00049 }
00050 
00051 static inline void store_Quat(const vmQuat &src, float* p)
00052 {
00053         vmQuat vq = src;
00054         storeXYZW(vq, p);
00055 }
00056 
00057 // Motion Type
00058 enum {
00059         PfxMotionTypeFixed = 0,
00060         PfxMotionTypeActive,
00061         PfxMotionTypeKeyframe,
00062         PfxMotionTypeOneWay,
00063         PfxMotionTypeTrigger,
00064         PfxMotionTypeCount
00065 };
00066 
00067 #define PFX_MOTION_MASK_DYNAMIC 0x0a // Active,OneWay
00068 #define PFX_MOTION_MASK_STATIC  0x95 // Fixed,Keyframe,Trigger,Sleeping
00069 #define PFX_MOTION_MASK_SLEEP   0x0e // Can sleep
00070 #define PFX_MOTION_MASK_TYPE    0x7f
00071 
00072 //
00073 // Rigid Body state
00074 //
00075 
00076 #ifdef __CELLOS_LV2__
00077 ATTRIBUTE_ALIGNED128(class) TrbState
00078 #else
00079 ATTRIBUTE_ALIGNED16(class) TrbState
00080 #endif
00081 
00082 {
00083 public:
00084         TrbState()
00085         {
00086                 setMotionType(PfxMotionTypeActive);
00087                 contactFilterSelf=contactFilterTarget=0xffffffff;
00088                 deleted = 0;
00089                 mSleeping = 0;
00090                 useSleep = 1;
00091                 trbBodyIdx=0;
00092                 mSleepCount=0;
00093                 useCcd = 0;
00094                 useContactCallback = 0;
00095                 useSleepCallback = 0;
00096                 linearDamping = 1.0f;
00097                 angularDamping = 0.99f;
00098         }
00099 
00100         TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega );
00101         
00102         uint16_t        mSleepCount;
00103         uint8_t         mMotionType;
00104         uint8_t         deleted            : 1;
00105         uint8_t         mSleeping           : 1;
00106         uint8_t         useSleep           : 1;
00107         uint8_t         useCcd                 : 1;
00108         uint8_t         useContactCallback : 1;
00109         uint8_t         useSleepCallback   : 1;
00110 
00111         uint16_t        trbBodyIdx;
00112         uint32_t        contactFilterSelf;
00113         uint32_t        contactFilterTarget;
00114 
00115         float           center[3];              // AABB center(World)
00116         float           half[3];                // AABB half(World)
00117 
00118         float           linearDamping;
00119         float           angularDamping;
00120         
00121         float           deltaLinearVelocity[3];
00122         float           deltaAngularVelocity[3];
00123 
00124         float     fX[3];                                // position
00125         float     fQ[4];                                // orientation
00126         float     fV[3];                                // velocity
00127         float     fOmega[3];                    // angular velocity
00128 
00129         inline void setZero();      // Zeroes out the elements
00130         inline void setIdentity();  // Sets the rotation to identity and zeroes out the other elements
00131 
00132         bool            isDeleted() const {return deleted==1;}
00133 
00134         uint16_t        getRigidBodyId() const {return trbBodyIdx;}
00135         void            setRigidBodyId(uint16_t i) {trbBodyIdx = i;}
00136 
00137 
00138         uint32_t        getContactFilterSelf() const {return contactFilterSelf;}
00139         void            setContactFilterSelf(uint32_t filter) {contactFilterSelf = filter;}
00140 
00141         uint32_t        getContactFilterTarget() const {return contactFilterTarget;}
00142         void            setContactFilterTarget(uint32_t filter) {contactFilterTarget = filter;}
00143 
00144         float getLinearDamping() const {return linearDamping;}
00145         float getAngularDamping() const {return angularDamping;}
00146 
00147         void setLinearDamping(float damping) {linearDamping=damping;}
00148         void setAngularDamping(float damping) {angularDamping=damping;}
00149 
00150 
00151         uint8_t         getMotionType() const {return mMotionType;}
00152         void            setMotionType(uint8_t t) {mMotionType = t;mSleeping=0;mSleepCount=0;}
00153 
00154         uint8_t         getMotionMask() const {return (1<<mMotionType)|(mSleeping<<7);}
00155 
00156         bool            isAsleep() const {return mSleeping==1;}
00157         bool            isAwake() const {return mSleeping==0;}
00158 
00159         void            wakeup() {mSleeping=0;mSleepCount=0;}
00160         void            sleep() {if(useSleep) {mSleeping=1;mSleepCount=0;}}
00161 
00162         uint8_t         getUseSleep() const {return useSleep;}
00163         void            setUseSleep(uint8_t b) {useSleep=b;}
00164 
00165         uint8_t         getUseCcd() const {return useCcd;}
00166         void            setUseCcd(uint8_t b) {useCcd=b;}
00167 
00168         uint8_t         getUseContactCallback() const {return useContactCallback;}
00169         void            setUseContactCallback(uint8_t b) {useContactCallback=b;}
00170 
00171         uint8_t         getUseSleepCallback() const {return useSleepCallback;}
00172         void            setUseSleepCallback(uint8_t b) {useSleepCallback=b;}
00173 
00174         void            incrementSleepCount() {mSleepCount++;}
00175         void            resetSleepCount() {mSleepCount=0;}
00176         uint16_t        getSleepCount() const {return mSleepCount;}
00177 
00178         vmVector3 getPosition() const {return read_Vector3(fX);}
00179         vmQuat    getOrientation() const {return read_Quat(fQ);}
00180         vmVector3 getLinearVelocity() const {return read_Vector3(fV);}
00181         vmVector3 getAngularVelocity() const {return read_Vector3(fOmega);}
00182         vmVector3 getDeltaLinearVelocity() const {return read_Vector3(deltaLinearVelocity);}
00183         vmVector3 getDeltaAngularVelocity() const {return read_Vector3(deltaAngularVelocity);}
00184 
00185         void setPosition(const vmVector3 &pos) {store_Vector3(pos, fX);}
00186         void setLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, fV);}
00187         void setAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, fOmega);}
00188         void setDeltaLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaLinearVelocity);}
00189         void setDeltaAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaAngularVelocity);}
00190         void setOrientation(const vmQuat &rot) {store_Quat(rot, fQ);}
00191 
00192         inline void setAuxils(const vmVector3 &centerLocal,const vmVector3 &halfLocal);
00193         inline void     setAuxilsCcd(const vmVector3 &centerLocal,const vmVector3 &halfLocal,float timeStep);
00194         inline  void reset();
00195 };
00196 
00197 inline
00198 TrbState::TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega)
00199 {
00200         setMotionType(m);
00201         fX[0] = x[0];
00202         fX[1] = x[1];
00203         fX[2] = x[2];
00204         fQ[0] = q[0];
00205         fQ[1] = q[1];
00206         fQ[2] = q[2];
00207         fQ[3] = q[3];
00208         fV[0] = v[0];
00209         fV[1] = v[1];
00210         fV[2] = v[2];
00211         fOmega[0] = omega[0];
00212         fOmega[1] = omega[1];
00213         fOmega[2] = omega[2];
00214         contactFilterSelf=contactFilterTarget=0xffff;
00215         trbBodyIdx=0;
00216         mSleeping = 0;
00217         deleted = 0;
00218         useSleep = 1;
00219         useCcd = 0;
00220         useContactCallback = 0;
00221         useSleepCallback = 0;
00222         mSleepCount=0;
00223         linearDamping = 1.0f;
00224         angularDamping = 0.99f;
00225 }
00226 
00227 inline void
00228 TrbState::setIdentity()
00229 {
00230         fX[0] = 0.0f;
00231         fX[1] = 0.0f;
00232         fX[2] = 0.0f;
00233         fQ[0] = 0.0f;
00234         fQ[1] = 0.0f;
00235         fQ[2] = 0.0f;
00236         fQ[3] = 1.0f;
00237         fV[0] = 0.0f;
00238         fV[1] = 0.0f;
00239         fV[2] = 0.0f;
00240         fOmega[0] = 0.0f;
00241         fOmega[1] = 0.0f;
00242         fOmega[2] = 0.0f;
00243 }
00244 
00245 inline void
00246 TrbState::setZero()
00247 {
00248         fX[0] = 0.0f;
00249         fX[1] = 0.0f;
00250         fX[2] = 0.0f;
00251         fQ[0] = 0.0f;
00252         fQ[1] = 0.0f;
00253         fQ[2] = 0.0f;
00254         fQ[3] = 0.0f;
00255         fV[0] = 0.0f;
00256         fV[1] = 0.0f;
00257         fV[2] = 0.0f;
00258         fOmega[0] = 0.0f;
00259         fOmega[1] = 0.0f;
00260         fOmega[2] = 0.0f;
00261 }
00262 
00263 inline void
00264 TrbState::setAuxils(const vmVector3 &centerLocal,const vmVector3 &halfLocal)
00265 {
00266         vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);
00267         vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;
00268         center[0] = centerW[0];
00269         center[1] = centerW[1];
00270         center[2] = centerW[2];
00271         half[0] = halfW[0];
00272         half[1] = halfW[1];
00273         half[2] = halfW[2];
00274 }
00275 
00276 inline void
00277 TrbState::setAuxilsCcd(const vmVector3 &centerLocal,const vmVector3 &halfLocal,float timeStep)
00278 {
00279         vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);
00280         vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;
00281 
00282         vmVector3 diffvec = getLinearVelocity()*timeStep;
00283 
00284         vmVector3 newCenter = centerW + diffvec;
00285         vmVector3 aabbMin = minPerElem(newCenter - halfW,centerW - halfW);
00286         vmVector3 aabbMax = maxPerElem(newCenter + halfW,centerW + halfW);
00287         
00288         centerW = 0.5f * (aabbMin + aabbMax);
00289         halfW =0.5f * (aabbMax - aabbMin);
00290 
00291         center[0] = centerW[0];
00292         center[1] = centerW[1];
00293         center[2] = centerW[2];
00294 
00295         half[0] = halfW[0];
00296         half[1] = halfW[1];
00297         half[2] = halfW[2];
00298 }
00299 
00300 inline
00301 void TrbState::reset()
00302 {
00303 #if 0
00304         mSleepCount = 0;
00305         mMotionType = PfxMotionTypeActive;
00306         mDeleted = 0;
00307         mSleeping = 0;
00308         mUseSleep = 1;
00309         mUseCcd = 0;
00310         mUseContactCallback = 0;
00311         mUseSleepCallback = 0;
00312         mRigidBodyId = 0;
00313         mContactFilterSelf = 0xffffffff;
00314         mContactFilterTarget = 0xffffffff;
00315         mLinearDamping = 1.0f;
00316         mAngularDamping = 0.99f;
00317         mPosition = vmVector3(0.0f);
00318         mOrientation = vmQuat::identity();
00319         mLinearVelocity = vmVector3(0.0f);
00320         mAngularVelocity = vmVector3(0.0f);
00321 #endif
00322 
00323         setMotionType(PfxMotionTypeActive);
00324         contactFilterSelf=contactFilterTarget=0xffffffff;
00325         deleted = 0;
00326         mSleeping = 0;
00327         useSleep = 1;
00328         trbBodyIdx=0;
00329         mSleepCount=0;
00330         useCcd = 0;
00331         useContactCallback = 0;
00332         useSleepCallback = 0;
00333         linearDamping = 1.0f;
00334         angularDamping = 0.99f;
00335 }
00336 
00337 #endif //BT_TRBSTATEVEC_H__
00338 
00339