Bullet Collision Detection & Physics Library
TrbStateVec.h
Go to the documentation of this file.
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 
Generated on Thu Oct 3 2013 18:25:08 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4