src/BulletMultiThreaded/TrbStateVec.h

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