Bullet Collision Detection & Physics Library: btTransformUtil.h Source File

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
38 class btTransformUtil
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
159 class btConvexSeparatingDistanceUtil
160 {
161 btQuaternion m_ornA;
162 btQuaternion m_ornB;
163 btVector3 m_posA;
164 btVector3 m_posB;
165
166 btVector3 m_separatingNormal;
167
168 btScalar m_boundingRadiusA;
169 btScalar m_boundingRadiusB;
170 btScalar m_separatingDistance;
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
181 btScalar getConservativeSeparatingDistance()
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
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btConvexSeparatingDistanceUtil::m_separatingNormal
btVector3 m_separatingNormal
Definition: btTransformUtil.h:166
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
btConvexSeparatingDistanceUtil::m_posA
btVector3 m_posA
Definition: btTransformUtil.h:163
btConvexSeparatingDistanceUtil::m_posB
btVector3 m_posB
Definition: btTransformUtil.h:164
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:477
btConvexSeparatingDistanceUtil::m_ornA
btQuaternion m_ornA
Definition: btTransformUtil.h:161
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btTransform::setBasis
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:159
btQuaternion::safeNormalize
btQuaternion & safeNormalize()
Definition: btQuaternion.h:358
btConvexSeparatingDistanceUtil::btConvexSeparatingDistanceUtil
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
Definition: btTransformUtil.h:174
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btTransformUtil::calculateDiffAxisAngle
static void calculateDiffAxisAngle(const btTransform &transform0, const btTransform &transform1, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:134
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
btTransform.h
btConvexSeparatingDistanceUtil::m_boundingRadiusB
btScalar m_boundingRadiusB
Definition: btTransformUtil.h:169
btTransformUtil::calculateDiffAxisAngleQuaternion
static void calculateDiffAxisAngleQuaternion(const btQuaternion &orn0, const btQuaternion &orn1a, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:110
ANGULAR_MOTION_THRESHOLD
#define ANGULAR_MOTION_THRESHOLD
Definition: btTransformUtil.h:20
btAabbSupport
btVector3 btAabbSupport(const btVector3 &halfExtents, const btVector3 &supportDir)
Definition: btTransformUtil.h:25
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:95
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:482
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btConvexSeparatingDistanceUtil::updateSeparatingDistance
void updateSeparatingDistance(const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:186
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:369
btConvexSeparatingDistanceUtil::m_boundingRadiusA
btScalar m_boundingRadiusA
Definition: btTransformUtil.h:168
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btConvexSeparatingDistanceUtil
The btConvexSeparatingDistanceUtil can help speed up convex collision detection by conservatively upd...
Definition: btTransformUtil.h:159
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btConvexSeparatingDistanceUtil::getConservativeSeparatingDistance
btScalar getConservativeSeparatingDistance()
Definition: btTransformUtil.h:181
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btConvexSeparatingDistanceUtil::m_separatingDistance
btScalar m_separatingDistance
Definition: btTransformUtil.h:170
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btConvexSeparatingDistanceUtil::m_ornB
btQuaternion m_ornB
Definition: btTransformUtil.h:162
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btTransformUtil::integrateTransform
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
Definition: btTransformUtil.h:43
btQuaternion::getAngle
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:452
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btQuaternion::nearest
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:548
btQuaternion::length2
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:348
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
btTransformUtil
Utils related to temporal transforms.
Definition: btTransformUtil.h:38
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:125
btConvexSeparatingDistanceUtil::initSeparatingDistance
void initSeparatingDistance(const btVector3 &separatingVector, btScalar separatingDistance, const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:218
btMatrix3x3::getRotation
void getRotation(btQuaternion &q) const
Get the matrix represented as a quaternion.
Definition: btMatrix3x3.h:400
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:476
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263