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 #ifndef BT_TRANSFORM_UTIL_H
16 #define BT_TRANSFORM_UTIL_H
17 
18 #include "btTransform.h"
19 #define ANGULAR_MOTION_THRESHOLD btScalar(0.5) * SIMD_HALF_PI
20 
21 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents, const btVector3& supportDir)
22 {
23  return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
24  supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
25  supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
26 }
27 
30 {
31 public:
32  static void integrateTransform(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btTransform& predictedTransform)
33  {
34  predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
35  // #define QUATERNION_DERIVATIVE
36 #ifdef QUATERNION_DERIVATIVE
37  btQuaternion predictedOrn = curTrans.getRotation();
38  predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
39  predictedOrn.safeNormalize();
40 #else
41  //Exponential map
42  //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
43 
44  btVector3 axis;
45  btScalar fAngle2 = angvel.length2();
46  btScalar fAngle = 0;
47  if (fAngle2 > SIMD_EPSILON)
48  {
49  fAngle = btSqrt(fAngle2);
50  }
51 
52  //limit the angular motion
53  if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD)
54  {
55  fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
56  }
57 
58  if (fAngle < btScalar(0.001))
59  {
60  // use Taylor's expansions of sync function
61  axis = angvel * (btScalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (btScalar(0.020833333333)) * fAngle * fAngle);
62  }
63  else
64  {
65  // sync(fAngle) = sin(c*fAngle)/t
66  axis = angvel * (btSin(btScalar(0.5) * fAngle * timeStep) / fAngle);
67  }
68  btQuaternion dorn(axis.x(), axis.y(), axis.z(), btCos(fAngle * timeStep * btScalar(0.5)));
69  btQuaternion orn0 = curTrans.getRotation();
70 
71  btQuaternion predictedOrn = dorn * orn0;
72  predictedOrn.safeNormalize();
73 #endif
74  if (predictedOrn.length2() > SIMD_EPSILON)
75  {
76  predictedTransform.setRotation(predictedOrn);
77  }
78  else
79  {
80  predictedTransform.setBasis(curTrans.getBasis());
81  }
82  }
83 
84  static void calculateVelocityQuaternion(const btVector3& pos0, const btVector3& pos1, const btQuaternion& orn0, const btQuaternion& orn1, btScalar timeStep, btVector3& linVel, btVector3& angVel)
85  {
86  linVel = (pos1 - pos0) / timeStep;
87  btVector3 axis;
88  btScalar angle;
89  if (orn0 != orn1)
90  {
91  calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
92  angVel = axis * angle / timeStep;
93  }
94  else
95  {
96  angVel.setValue(0, 0, 0);
97  }
98  }
99 
100  static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0, const btQuaternion& orn1a, btVector3& axis, btScalar& angle)
101  {
102  btQuaternion orn1 = orn0.nearest(orn1a);
103  btQuaternion dorn = orn1 * orn0.inverse();
104  angle = dorn.getAngle();
105  axis = btVector3(dorn.x(), dorn.y(), dorn.z());
106  axis[3] = btScalar(0.);
107  //check for axis length
108  btScalar len = axis.length2();
109  if (len < SIMD_EPSILON * SIMD_EPSILON)
110  axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.));
111  else
112  axis /= btSqrt(len);
113  }
114 
115  static void calculateVelocity(const btTransform& transform0, const btTransform& transform1, btScalar timeStep, btVector3& linVel, btVector3& angVel)
116  {
117  linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
118  btVector3 axis;
119  btScalar angle;
120  calculateDiffAxisAngle(transform0, transform1, axis, angle);
121  angVel = axis * angle / timeStep;
122  }
123 
124  static void calculateDiffAxisAngle(const btTransform& transform0, const btTransform& transform1, btVector3& axis, btScalar& angle)
125  {
126  btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
127  btQuaternion dorn;
128  dmat.getRotation(dorn);
129 
131  dorn.normalize();
132 
133  angle = dorn.getAngle();
134  axis = btVector3(dorn.x(), dorn.y(), dorn.z());
135  axis[3] = btScalar(0.);
136  //check for axis length
137  btScalar len = axis.length2();
138  if (len < SIMD_EPSILON * SIMD_EPSILON)
139  axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.));
140  else
141  axis /= btSqrt(len);
142  }
143 };
144 
148 {
153 
155 
159 
160 public:
161  btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
162  : m_boundingRadiusA(boundingRadiusA),
163  m_boundingRadiusB(boundingRadiusB),
165  {
166  }
167 
169  {
170  return m_separatingDistance;
171  }
172 
173  void updateSeparatingDistance(const btTransform& transA, const btTransform& transB)
174  {
175  const btVector3& toPosA = transA.getOrigin();
176  const btVector3& toPosB = transB.getOrigin();
177  btQuaternion toOrnA = transA.getRotation();
178  btQuaternion toOrnB = transB.getRotation();
179 
180  if (m_separatingDistance > 0.f)
181  {
182  btVector3 linVelA, angVelA, linVelB, angVelB;
183  btTransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, btScalar(1.), linVelA, angVelA);
184  btTransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, btScalar(1.), linVelB, angVelB);
185  btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
186  btVector3 relLinVel = (linVelB - linVelA);
187  btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
188  if (relLinVelocLength < 0.f)
189  {
190  relLinVelocLength = 0.f;
191  }
192 
193  btScalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
194  m_separatingDistance -= projectedMotion;
195  }
196 
197  m_posA = toPosA;
198  m_posB = toPosB;
199  m_ornA = toOrnA;
200  m_ornB = toOrnB;
201  }
202 
203  void initSeparatingDistance(const btVector3& separatingVector, btScalar separatingDistance, const btTransform& transA, const btTransform& transB)
204  {
205  m_separatingDistance = separatingDistance;
206 
207  if (m_separatingDistance > 0.f)
208  {
209  m_separatingNormal = separatingVector;
210 
211  const btVector3& toPosA = transA.getOrigin();
212  const btVector3& toPosB = transB.getOrigin();
213  btQuaternion toOrnA = transA.getRotation();
214  btQuaternion toOrnB = transB.getRotation();
215  m_posA = toPosA;
216  m_posB = toPosB;
217  m_ornA = toOrnA;
218  m_ornB = toOrnB;
219  }
220  }
221 };
222 
223 #endif //BT_TRANSFORM_UTIL_H
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btQuaternion::getAngle
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:468
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btTransformUtil::calculateDiffAxisAngleQuaternion
static void calculateDiffAxisAngleQuaternion(const btQuaternion &orn0, const btQuaternion &orn1a, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:100
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1070
btConvexSeparatingDistanceUtil::m_posA
btVector3 m_posA
Definition: btTransformUtil.h:151
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btTransform::setBasis
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:154
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btTransformUtil::calculateVelocityQuaternion
static void calculateVelocityQuaternion(const btVector3 &pos0, const btVector3 &pos1, const btQuaternion &orn0, const btQuaternion &orn1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:84
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
btConvexSeparatingDistanceUtil::m_posB
btVector3 m_posB
Definition: btTransformUtil.h:152
btQuaternion::safeNormalize
btQuaternion & safeNormalize()
Definition: btQuaternion.h:374
btQuaternion::length2
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:364
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btConvexSeparatingDistanceUtil::btConvexSeparatingDistanceUtil
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
Definition: btTransformUtil.h:161
btConvexSeparatingDistanceUtil::getConservativeSeparatingDistance
btScalar getConservativeSeparatingDistance()
Definition: btTransformUtil.h:168
btConvexSeparatingDistanceUtil::m_boundingRadiusA
btScalar m_boundingRadiusA
Definition: btTransformUtil.h:156
btTransformUtil::calculateDiffAxisAngle
static void calculateDiffAxisAngle(const btTransform &transform0, const btTransform &transform1, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:124
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:479
btConvexSeparatingDistanceUtil::m_ornB
btQuaternion m_ornB
Definition: btTransformUtil.h:150
btConvexSeparatingDistanceUtil::m_ornA
btQuaternion m_ornA
Definition: btTransformUtil.h:149
btQuaternion::nearest
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:563
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:160
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:478
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btTransform.h
btConvexSeparatingDistanceUtil
The btConvexSeparatingDistanceUtil can help speed up convex collision detection by conservatively upd...
Definition: btTransformUtil.h:147
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btConvexSeparatingDistanceUtil::m_boundingRadiusB
btScalar m_boundingRadiusB
Definition: btTransformUtil.h:157
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btConvexSeparatingDistanceUtil::updateSeparatingDistance
void updateSeparatingDistance(const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:173
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:83
btConvexSeparatingDistanceUtil::m_separatingDistance
btScalar m_separatingDistance
Definition: btTransformUtil.h:158
btAabbSupport
btVector3 btAabbSupport(const btVector3 &halfExtents, const btVector3 &supportDir)
Definition: btTransformUtil.h:21
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btMatrix3x3::getRotation
void getRotation(btQuaternion &q) const
Get the matrix represented as a quaternion.
Definition: btMatrix3x3.h:397
btConvexSeparatingDistanceUtil::initSeparatingDistance
void initSeparatingDistance(const btVector3 &separatingVector, btScalar separatingDistance, const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:203
ANGULAR_MOTION_THRESHOLD
#define ANGULAR_MOTION_THRESHOLD
Definition: btTransformUtil.h:19
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btConvexSeparatingDistanceUtil::m_separatingNormal
btVector3 m_separatingNormal
Definition: btTransformUtil.h:154
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:446
btTransformUtil
Utils related to temporal transforms.
Definition: btTransformUtil.h:29
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:115
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btTransformUtil::integrateTransform
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
Definition: btTransformUtil.h:32
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251