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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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.normalize();
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 fAngle = angvel.length();
57 //limit the angular motion
58 if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
59 {
60 fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
61 }
62
63 if ( fAngle < btScalar(0.001) )
64 {
65 // use Taylor's expansions of sync function
66 axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
67 }
68 else
69 {
70 // sync(fAngle) = sin(c*fAngle)/t
71 axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
72 }
73 btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
74 btQuaternion orn0 = curTrans.getRotation();
75
76 btQuaternion predictedOrn = dorn * orn0;
77 predictedOrn.normalize();
78 #endif
79 predictedTransform.setRotation(predictedOrn);
80 }
81
82 static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
83 {
84 linVel = (pos1 - pos0) / timeStep;
85 btVector3 axis;
86 btScalar angle;
87 if (orn0 != orn1)
88 {
89 calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
90 angVel = axis * angle / timeStep;
91 } else
92 {
93 angVel.setValue(0,0,0);
94 }
95 }
96
97 static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
98 {
99 btQuaternion orn1 = orn0.nearest(orn1a);
100 btQuaternion dorn = orn1 * orn0.inverse();
101 angle = dorn.getAngle();
102 axis = btVector3(dorn.x(),dorn.y(),dorn.z());
103 axis[3] = btScalar(0.);
104 //check for axis length
105 btScalar len = axis.length2();
106 if (len < SIMD_EPSILON*SIMD_EPSILON)
107 axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
108 else
109 axis /= btSqrt(len);
110 }
111
112 static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
113 {
114 linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
115 btVector3 axis;
116 btScalar angle;
117 calculateDiffAxisAngle(transform0,transform1,axis,angle);
118 angVel = axis * angle / timeStep;
119 }
120
121 static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
122 {
123 btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
124 btQuaternion dorn;
125 dmat.getRotation(dorn);
126
128 dorn.normalize();
129
130 angle = dorn.getAngle();
131 axis = btVector3(dorn.x(),dorn.y(),dorn.z());
132 axis[3] = btScalar(0.);
133 //check for axis length
134 btScalar len = axis.length2();
135 if (len < SIMD_EPSILON*SIMD_EPSILON)
136 axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
137 else
138 axis /= btSqrt(len);
139 }
140
141 };
142
143
146 class btConvexSeparatingDistanceUtil
147 {
148 btQuaternion m_ornA;
149 btQuaternion m_ornB;
150 btVector3 m_posA;
151 btVector3 m_posB;
152
153 btVector3 m_separatingNormal;
154
155 btScalar m_boundingRadiusA;
156 btScalar m_boundingRadiusB;
157 btScalar m_separatingDistance;
158
159 public:
160
161 btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB)
162 :m_boundingRadiusA(boundingRadiusA),
163 m_boundingRadiusB(boundingRadiusB),
164 m_separatingDistance(0.f)
165 {
166 }
167
168 btScalar getConservativeSeparatingDistance()
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
183
184 btVector3 linVelA,angVelA,linVelB,angVelB;
185 btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
186 btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
187 btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
188 btVector3 relLinVel = (linVelB-linVelA);
189 btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
190 if (relLinVelocLength<0.f)
191 {
192 relLinVelocLength = 0.f;
193 }
194
195 btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
196 m_separatingDistance -= projectedMotion;
197 }
198
199 m_posA = toPosA;
200 m_posB = toPosB;
201 m_ornA = toOrnA;
202 m_ornB = toOrnB;
203 }
204
205 void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
206 {
207 m_separatingDistance = separatingDistance;
208
209 if (m_separatingDistance>0.f)
210 {
211 m_separatingNormal = separatingVector;
212
213 const btVector3& toPosA = transA.getOrigin();
214 const btVector3& toPosB = transB.getOrigin();
215 btQuaternion toOrnA = transA.getRotation();
216 btQuaternion toOrnB = transB.getRotation();
217 m_posA = toPosA;
218 m_posB = toPosB;
219 m_ornA = toOrnA;
220 m_ornB = toOrnB;
221 }
222 }
223
224 };
225
226
227 #endif //BT_TRANSFORM_UTIL_H
228
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1046
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btConvexSeparatingDistanceUtil::m_separatingNormal
btVector3 m_separatingNormal
Definition: btTransformUtil.h:153
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
btQuaternion::getAngle
btScalar getAngle() const
Return the angle of rotation represented by this quaternion.
Definition: btQuaternion.h:415
btConvexSeparatingDistanceUtil::m_posA
btVector3 m_posA
Definition: btTransformUtil.h:150
btConvexSeparatingDistanceUtil::m_posB
btVector3 m_posB
Definition: btTransformUtil.h:151
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:451
btConvexSeparatingDistanceUtil::m_ornA
btQuaternion m_ornA
Definition: btTransformUtil.h:148
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btConvexSeparatingDistanceUtil::btConvexSeparatingDistanceUtil
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
Definition: btTransformUtil.h:161
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
btTransformUtil::calculateDiffAxisAngle
static void calculateDiffAxisAngle(const btTransform &transform0, const btTransform &transform1, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:121
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
btTransform.h
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
btConvexSeparatingDistanceUtil::m_boundingRadiusB
btScalar m_boundingRadiusB
Definition: btTransformUtil.h:156
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btQuaternion::nearest
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:512
btTransformUtil::calculateDiffAxisAngleQuaternion
static void calculateDiffAxisAngleQuaternion(const btQuaternion &orn0, const btQuaternion &orn1a, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:97
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
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:82
btMatrix3x3::getRotation
void getRotation(btQuaternion &q) const
Get the matrix represented as a quaternion.
Definition: btMatrix3x3.h:400
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:173
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:332
btConvexSeparatingDistanceUtil::m_boundingRadiusA
btScalar m_boundingRadiusA
Definition: btTransformUtil.h:155
btConvexSeparatingDistanceUtil
The btConvexSeparatingDistanceUtil can help speed up convex collision detection by conservatively upd...
Definition: btTransformUtil.h:146
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
btConvexSeparatingDistanceUtil::getConservativeSeparatingDistance
btScalar getConservativeSeparatingDistance()
Definition: btTransformUtil.h:168
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btConvexSeparatingDistanceUtil::m_separatingDistance
btScalar m_separatingDistance
Definition: btTransformUtil.h:157
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:446
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btConvexSeparatingDistanceUtil::m_ornB
btQuaternion m_ornB
Definition: btTransformUtil.h:149
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
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
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:112
btConvexSeparatingDistanceUtil::initSeparatingDistance
void initSeparatingDistance(const btVector3 &separatingVector, btScalar separatingDistance, const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:205
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:450
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579