Bullet Collision Detection & Physics Library
btTransformUtil.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3 
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 
10 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.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14 
15 
16 #ifndef BT_TRANSFORM_UTIL_H
17 #define BT_TRANSFORM_UTIL_H
18 
19 #include "btTransform.h"
20 #define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
21 
22 
23 
24 
25 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
26 {
27  return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
28  supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
29  supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
30 }
31 
32 
33 
34 
35 
36 
39 {
40 
41 public:
42 
43  static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
44  {
45  predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
46 // #define QUATERNION_DERIVATIVE
47  #ifdef QUATERNION_DERIVATIVE
48  btQuaternion predictedOrn = curTrans.getRotation();
49  predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
50  predictedOrn.safeNormalize();
51  #else
52  //Exponential map
53  //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
54 
55  btVector3 axis;
56  btScalar fAngle2 = angvel.length2();
57  btScalar fAngle = 0;
58  if (fAngle2>SIMD_EPSILON)
59  {
60  fAngle = btSqrt(fAngle2);
61  }
62 
63  //limit the angular motion
64  if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
65  {
66  fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
67  }
68 
69  if ( fAngle < btScalar(0.001) )
70  {
71  // use Taylor's expansions of sync function
72  axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
73  }
74  else
75  {
76  // sync(fAngle) = sin(c*fAngle)/t
77  axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
78  }
79  btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
80  btQuaternion orn0 = curTrans.getRotation();
81 
82  btQuaternion predictedOrn = dorn * orn0;
83  predictedOrn.safeNormalize();
84  #endif
85  if (predictedOrn.length2()>SIMD_EPSILON)
86  {
87  predictedTransform.setRotation(predictedOrn);
88  }
89  else
90  {
91  predictedTransform.setBasis(curTrans.getBasis());
92  }
93  }
94 
95  static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
96  {
97  linVel = (pos1 - pos0) / timeStep;
98  btVector3 axis;
99  btScalar angle;
100  if (orn0 != orn1)
101  {
102  calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
103  angVel = axis * angle / timeStep;
104  } else
105  {
106  angVel.setValue(0,0,0);
107  }
108  }
109 
110  static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
111  {
112  btQuaternion orn1 = orn0.nearest(orn1a);
113  btQuaternion dorn = orn1 * orn0.inverse();
114  angle = dorn.getAngle();
115  axis = btVector3(dorn.x(),dorn.y(),dorn.z());
116  axis[3] = btScalar(0.);
117  //check for axis length
118  btScalar len = axis.length2();
119  if (len < SIMD_EPSILON*SIMD_EPSILON)
120  axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
121  else
122  axis /= btSqrt(len);
123  }
124 
125  static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
126  {
127  linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
128  btVector3 axis;
129  btScalar angle;
130  calculateDiffAxisAngle(transform0,transform1,axis,angle);
131  angVel = axis * angle / timeStep;
132  }
133 
134  static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
135  {
136  btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
137  btQuaternion dorn;
138  dmat.getRotation(dorn);
139 
141  dorn.normalize();
142 
143  angle = dorn.getAngle();
144  axis = btVector3(dorn.x(),dorn.y(),dorn.z());
145  axis[3] = btScalar(0.);
146  //check for axis length
147  btScalar len = axis.length2();
148  if (len < SIMD_EPSILON*SIMD_EPSILON)
149  axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
150  else
151  axis /= btSqrt(len);
152  }
153 
154 };
155 
156 
160 {
165 
167 
171 
172 public:
173 
174  btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB)
175  :m_boundingRadiusA(boundingRadiusA),
176  m_boundingRadiusB(boundingRadiusB),
177  m_separatingDistance(0.f)
178  {
179  }
180 
182  {
183  return m_separatingDistance;
184  }
185 
186  void updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
187  {
188  const btVector3& toPosA = transA.getOrigin();
189  const btVector3& toPosB = transB.getOrigin();
190  btQuaternion toOrnA = transA.getRotation();
191  btQuaternion toOrnB = transB.getRotation();
192 
193  if (m_separatingDistance>0.f)
194  {
195 
196 
197  btVector3 linVelA,angVelA,linVelB,angVelB;
198  btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
199  btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
200  btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
201  btVector3 relLinVel = (linVelB-linVelA);
202  btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
203  if (relLinVelocLength<0.f)
204  {
205  relLinVelocLength = 0.f;
206  }
207 
208  btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
209  m_separatingDistance -= projectedMotion;
210  }
211 
212  m_posA = toPosA;
213  m_posB = toPosB;
214  m_ornA = toOrnA;
215  m_ornB = toOrnB;
216  }
217 
218  void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
219  {
220  m_separatingDistance = separatingDistance;
221 
222  if (m_separatingDistance>0.f)
223  {
224  m_separatingNormal = separatingVector;
225 
226  const btVector3& toPosA = transA.getOrigin();
227  const btVector3& toPosB = transB.getOrigin();
228  btQuaternion toOrnA = transA.getRotation();
229  btQuaternion toOrnB = transB.getRotation();
230  m_posA = toPosA;
231  m_posB = toPosB;
232  m_ornA = toOrnA;
233  m_ornB = toOrnB;
234  }
235  }
236 
237 };
238 
239 
240 #endif //BT_TRANSFORM_UTIL_H
241 
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1075
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
#define SIMD_EPSILON
Definition: btScalar.h:521
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:470
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btScalar btSin(btScalar x)
Definition: btScalar.h:477
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:159
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btQuaternion & safeNormalize()
Definition: btQuaternion.h:376
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
static void calculateDiffAxisAngle(const btTransform &transform0, const btTransform &transform1, btVector3 &axis, btScalar &angle)
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:566
static void calculateDiffAxisAngleQuaternion(const btQuaternion &orn0, const btQuaternion &orn1a, btVector3 &axis, btScalar &angle)
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
#define ANGULAR_MOTION_THRESHOLD
btVector3 btAabbSupport(const btVector3 &halfExtents, const btVector3 &supportDir)
static void calculateVelocityQuaternion(const btVector3 &pos0, const btVector3 &pos1, const btQuaternion &orn0, const btQuaternion &orn1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
void getRotation(btQuaternion &q) const
Get the matrix represented as a quaternion.
Definition: btMatrix3x3.h:400
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
void updateSeparatingDistance(const btTransform &transA, const btTransform &transB)
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:387
The btConvexSeparatingDistanceUtil can help speed up convex collision detection by conservatively upd...
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:500
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:366
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
Utils related to temporal transforms.
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
void initSeparatingDistance(const btVector3 &separatingVector, btScalar separatingDistance, const btTransform &transA, const btTransform &transB)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btCos(btScalar x)
Definition: btScalar.h:476
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591