Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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];
00116 float half[3];
00117
00118 float linearDamping;
00119 float angularDamping;
00120
00121 float deltaLinearVelocity[3];
00122 float deltaAngularVelocity[3];
00123
00124 float fX[3];
00125 float fQ[4];
00126 float fV[3];
00127 float fOmega[3];
00128
00129 inline void setZero();
00130 inline void setIdentity();
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 ¢erLocal,const vmVector3 &halfLocal);
00193 inline void setAuxilsCcd(const vmVector3 ¢erLocal,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 ¢erLocal,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 ¢erLocal,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