src/BulletMultiThreaded/TrbStateVec.h

   1 /*\r
   2    Copyright (C) 2009 Sony Computer Entertainment Inc.\r
   3    All rights reserved.\r
   4 \r
   5 This software is provided 'as-is', without any express or implied warranty.\r
   6 In no event will the authors be held liable for any damages arising from the use of this software.\r
   7 Permission is granted to anyone to use this software for any purpose, \r
   8 including commercial applications, and to alter it and redistribute it freely, \r
   9 subject to the following restrictions:\r
  10 \r
  11 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.\r
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\r
  13 3. This notice may not be removed or altered from any source distribution.\r
  14 \r
  15 */\r
  16 \r
  17 #ifndef BT_TRBSTATEVEC_H__\r
  18 #define BT_TRBSTATEVEC_H__\r
  19 \r
  20 #include <stdlib.h>\r
  21 #ifdef PFX_USE_FREE_VECTORMATH\r
  22 #include "vecmath/vmInclude.h"\r
  23 #else\r
  24 #include "vectormath/vmInclude.h"\r
  25 #endif //PFX_USE_FREE_VECTORMATH\r
  26 \r
  27 \r
  28 #include "PlatformDefinitions.h"\r
  29 \r
  30 \r
  31 static inline vmVector3 read_Vector3(const float* p)\r
  32 {\r
  33         vmVector3 v;\r
  34         loadXYZ(v, p);\r
  35         return v;\r
  36 }\r
  37 \r
  38 static inline vmQuat read_Quat(const float* p)\r
  39 {\r
  40         vmQuat vq;\r
  41         loadXYZW(vq, p);\r
  42         return vq;\r
  43 }\r
  44 \r
  45 static inline void store_Vector3(const vmVector3 &src, float* p)\r
  46 {\r
  47         vmVector3 v = src;\r
  48         storeXYZ(v, p);\r
  49 }\r
  50 \r
  51 static inline void store_Quat(const vmQuat &src, float* p)\r
  52 {\r
  53         vmQuat vq = src;\r
  54         storeXYZW(vq, p);\r
  55 }\r
  56 \r
  57 // Motion Type\r
  58 enum {\r
  59         PfxMotionTypeFixed = 0,\r
  60         PfxMotionTypeActive,\r
  61         PfxMotionTypeKeyframe,\r
  62         PfxMotionTypeOneWay,\r
  63         PfxMotionTypeTrigger,\r
  64         PfxMotionTypeCount\r
  65 };\r
  66 \r
  67 #define PFX_MOTION_MASK_DYNAMIC 0x0a // Active,OneWay\r
  68 #define PFX_MOTION_MASK_STATIC  0x95 // Fixed,Keyframe,Trigger,Sleeping\r
  69 #define PFX_MOTION_MASK_SLEEP   0x0e // Can sleep\r
  70 #define PFX_MOTION_MASK_TYPE    0x7f\r
  71 \r
  72 //\r
  73 // Rigid Body state\r
  74 //\r
  75 \r
  76 #ifdef __CELLOS_LV2__\r
  77 ATTRIBUTE_ALIGNED128(class) TrbState\r
  78 #else\r
  79 ATTRIBUTE_ALIGNED16(class) TrbState\r
  80 #endif\r
  81 \r
  82 {\r
  83 public:\r
  84         TrbState()\r
  85         {\r
  86                 setMotionType(PfxMotionTypeActive);\r
  87                 contactFilterSelf=contactFilterTarget=0xffffffff;\r
  88                 deleted = 0;\r
  89                 mSleeping = 0;\r
  90                 useSleep = 1;\r
  91                 trbBodyIdx=0;\r
  92                 mSleepCount=0;\r
  93                 useCcd = 0;\r
  94                 useContactCallback = 0;\r
  95                 useSleepCallback = 0;\r
  96                 linearDamping = 1.0f;\r
  97                 angularDamping = 0.99f;\r
  98         }\r
  99 \r
 100         TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega );\r
 101         \r
 102         uint16_t        mSleepCount;\r
 103         uint8_t         mMotionType;\r
 104         uint8_t         deleted            : 1;\r
 105         uint8_t         mSleeping           : 1;\r
 106         uint8_t         useSleep           : 1;\r
 107         uint8_t         useCcd                 : 1;\r
 108         uint8_t         useContactCallback : 1;\r
 109         uint8_t         useSleepCallback   : 1;\r
 110 \r
 111         uint16_t        trbBodyIdx;\r
 112         uint32_t        contactFilterSelf;\r
 113         uint32_t        contactFilterTarget;\r
 114 \r
 115         float           center[3];              // AABB center(World)\r
 116         float           half[3];                // AABB half(World)\r
 117 \r
 118         float           linearDamping;\r
 119         float           angularDamping;\r
 120         \r
 121         float           deltaLinearVelocity[3];\r
 122         float           deltaAngularVelocity[3];\r
 123 \r
 124         float     fX[3];                                // position\r
 125         float     fQ[4];                                // orientation\r
 126         float     fV[3];                                // velocity\r
 127         float     fOmega[3];                    // angular velocity\r
 128 \r
 129         inline void setZero();      // Zeroes out the elements\r
 130         inline void setIdentity();  // Sets the rotation to identity and zeroes out the other elements\r
 131 \r
 132         bool            isDeleted() const {return deleted==1;}\r
 133 \r
 134         uint16_t        getRigidBodyId() const {return trbBodyIdx;}\r
 135         void            setRigidBodyId(uint16_t i) {trbBodyIdx = i;}\r
 136 \r
 137 \r
 138         uint32_t        getContactFilterSelf() const {return contactFilterSelf;}\r
 139         void            setContactFilterSelf(uint32_t filter) {contactFilterSelf = filter;}\r
 140 \r
 141         uint32_t        getContactFilterTarget() const {return contactFilterTarget;}\r
 142         void            setContactFilterTarget(uint32_t filter) {contactFilterTarget = filter;}\r
 143 \r
 144         float getLinearDamping() const {return linearDamping;}\r
 145         float getAngularDamping() const {return angularDamping;}\r
 146 \r
 147         void setLinearDamping(float damping) {linearDamping=damping;}\r
 148         void setAngularDamping(float damping) {angularDamping=damping;}\r
 149 \r
 150 \r
 151         uint8_t         getMotionType() const {return mMotionType;}\r
 152         void            setMotionType(uint8_t t) {mMotionType = t;mSleeping=0;mSleepCount=0;}\r
 153 \r
 154         uint8_t         getMotionMask() const {return (1<<mMotionType)|(mSleeping<<7);}\r
 155 \r
 156         bool            isAsleep() const {return mSleeping==1;}\r
 157         bool            isAwake() const {return mSleeping==0;}\r
 158 \r
 159         void            wakeup() {mSleeping=0;mSleepCount=0;}\r
 160         void            sleep() {if(useSleep) {mSleeping=1;mSleepCount=0;}}\r
 161 \r
 162         uint8_t         getUseSleep() const {return useSleep;}\r
 163         void            setUseSleep(uint8_t b) {useSleep=b;}\r
 164 \r
 165         uint8_t         getUseCcd() const {return useCcd;}\r
 166         void            setUseCcd(uint8_t b) {useCcd=b;}\r
 167 \r
 168         uint8_t         getUseContactCallback() const {return useContactCallback;}\r
 169         void            setUseContactCallback(uint8_t b) {useContactCallback=b;}\r
 170 \r
 171         uint8_t         getUseSleepCallback() const {return useSleepCallback;}\r
 172         void            setUseSleepCallback(uint8_t b) {useSleepCallback=b;}\r
 173 \r
 174         void            incrementSleepCount() {mSleepCount++;}\r
 175         void            resetSleepCount() {mSleepCount=0;}\r
 176         uint16_t        getSleepCount() const {return mSleepCount;}\r
 177 \r
 178         vmVector3 getPosition() const {return read_Vector3(fX);}\r
 179         vmQuat    getOrientation() const {return read_Quat(fQ);}\r
 180         vmVector3 getLinearVelocity() const {return read_Vector3(fV);}\r
 181         vmVector3 getAngularVelocity() const {return read_Vector3(fOmega);}\r
 182         vmVector3 getDeltaLinearVelocity() const {return read_Vector3(deltaLinearVelocity);}\r
 183         vmVector3 getDeltaAngularVelocity() const {return read_Vector3(deltaAngularVelocity);}\r
 184 \r
 185         void setPosition(const vmVector3 &pos) {store_Vector3(pos, fX);}\r
 186         void setLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, fV);}\r
 187         void setAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, fOmega);}\r
 188         void setDeltaLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaLinearVelocity);}\r
 189         void setDeltaAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaAngularVelocity);}\r
 190         void setOrientation(const vmQuat &rot) {store_Quat(rot, fQ);}\r
 191 \r
 192         inline void setAuxils(const vmVector3 &centerLocal,const vmVector3 &halfLocal);\r
 193         inline void     setAuxilsCcd(const vmVector3 &centerLocal,const vmVector3 &halfLocal,float timeStep);\r
 194         inline  void reset();\r
 195 };\r
 196 \r
 197 inline\r
 198 TrbState::TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega)\r
 199 {\r
 200         setMotionType(m);\r
 201         fX[0] = x[0];\r
 202         fX[1] = x[1];\r
 203         fX[2] = x[2];\r
 204         fQ[0] = q[0];\r
 205         fQ[1] = q[1];\r
 206         fQ[2] = q[2];\r
 207         fQ[3] = q[3];\r
 208         fV[0] = v[0];\r
 209         fV[1] = v[1];\r
 210         fV[2] = v[2];\r
 211         fOmega[0] = omega[0];\r
 212         fOmega[1] = omega[1];\r
 213         fOmega[2] = omega[2];\r
 214         contactFilterSelf=contactFilterTarget=0xffff;\r
 215         trbBodyIdx=0;\r
 216         mSleeping = 0;\r
 217         deleted = 0;\r
 218         useSleep = 1;\r
 219         useCcd = 0;\r
 220         useContactCallback = 0;\r
 221         useSleepCallback = 0;\r
 222         mSleepCount=0;\r
 223         linearDamping = 1.0f;\r
 224         angularDamping = 0.99f;\r
 225 }\r
 226 \r
 227 inline void\r
 228 TrbState::setIdentity()\r
 229 {\r
 230         fX[0] = 0.0f;\r
 231         fX[1] = 0.0f;\r
 232         fX[2] = 0.0f;\r
 233         fQ[0] = 0.0f;\r
 234         fQ[1] = 0.0f;\r
 235         fQ[2] = 0.0f;\r
 236         fQ[3] = 1.0f;\r
 237         fV[0] = 0.0f;\r
 238         fV[1] = 0.0f;\r
 239         fV[2] = 0.0f;\r
 240         fOmega[0] = 0.0f;\r
 241         fOmega[1] = 0.0f;\r
 242         fOmega[2] = 0.0f;\r
 243 }\r
 244 \r
 245 inline void\r
 246 TrbState::setZero()\r
 247 {\r
 248         fX[0] = 0.0f;\r
 249         fX[1] = 0.0f;\r
 250         fX[2] = 0.0f;\r
 251         fQ[0] = 0.0f;\r
 252         fQ[1] = 0.0f;\r
 253         fQ[2] = 0.0f;\r
 254         fQ[3] = 0.0f;\r
 255         fV[0] = 0.0f;\r
 256         fV[1] = 0.0f;\r
 257         fV[2] = 0.0f;\r
 258         fOmega[0] = 0.0f;\r
 259         fOmega[1] = 0.0f;\r
 260         fOmega[2] = 0.0f;\r
 261 }\r
 262 \r
 263 inline void\r
 264 TrbState::setAuxils(const vmVector3 &centerLocal,const vmVector3 &halfLocal)\r
 265 {\r
 266         vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);\r
 267         vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;\r
 268         center[0] = centerW[0];\r
 269         center[1] = centerW[1];\r
 270         center[2] = centerW[2];\r
 271         half[0] = halfW[0];\r
 272         half[1] = halfW[1];\r
 273         half[2] = halfW[2];\r
 274 }\r
 275 \r
 276 inline void\r
 277 TrbState::setAuxilsCcd(const vmVector3 &centerLocal,const vmVector3 &halfLocal,float timeStep)\r
 278 {\r
 279         vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);\r
 280         vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;\r
 281 \r
 282         vmVector3 diffvec = getLinearVelocity()*timeStep;\r
 283 \r
 284         vmVector3 newCenter = centerW + diffvec;\r
 285         vmVector3 aabbMin = minPerElem(newCenter - halfW,centerW - halfW);\r
 286         vmVector3 aabbMax = maxPerElem(newCenter + halfW,centerW + halfW);\r
 287         \r
 288         centerW = 0.5f * (aabbMin + aabbMax);\r
 289         halfW =0.5f * (aabbMax - aabbMin);\r
 290 \r
 291         center[0] = centerW[0];\r
 292         center[1] = centerW[1];\r
 293         center[2] = centerW[2];\r
 294 \r
 295         half[0] = halfW[0];\r
 296         half[1] = halfW[1];\r
 297         half[2] = halfW[2];\r
 298 }\r
 299 \r
 300 inline\r
 301 void TrbState::reset()\r
 302 {\r
 303 #if 0\r
 304         mSleepCount = 0;\r
 305         mMotionType = PfxMotionTypeActive;\r
 306         mDeleted = 0;\r
 307         mSleeping = 0;\r
 308         mUseSleep = 1;\r
 309         mUseCcd = 0;\r
 310         mUseContactCallback = 0;\r
 311         mUseSleepCallback = 0;\r
 312         mRigidBodyId = 0;\r
 313         mContactFilterSelf = 0xffffffff;\r
 314         mContactFilterTarget = 0xffffffff;\r
 315         mLinearDamping = 1.0f;\r
 316         mAngularDamping = 0.99f;\r
 317         mPosition = vmVector3(0.0f);\r
 318         mOrientation = vmQuat::identity();\r
 319         mLinearVelocity = vmVector3(0.0f);\r
 320         mAngularVelocity = vmVector3(0.0f);\r
 321 #endif\r
 322 \r
 323         setMotionType(PfxMotionTypeActive);\r
 324         contactFilterSelf=contactFilterTarget=0xffffffff;\r
 325         deleted = 0;\r
 326         mSleeping = 0;\r
 327         useSleep = 1;\r
 328         trbBodyIdx=0;\r
 329         mSleepCount=0;\r
 330         useCcd = 0;\r
 331         useContactCallback = 0;\r
 332         useSleepCallback = 0;\r
 333         linearDamping = 1.0f;\r
 334         angularDamping = 0.99f;\r
 335 }\r
 336 \r
 337 #endif //BT_TRBSTATEVEC_H__\r
 338 \r
 339 \r