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

Bullet Collision Detection & Physics Library

btTransformUtil.h
Go to the documentation of this file.
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans https://bulletphysics.org
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. 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
29 class btTransformUtil
30{
31public:
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
147 class btConvexSeparatingDistanceUtil
148{
149 btQuaternion m_ornA;
150 btQuaternion m_ornB;
151 btVector3 m_posA;
152 btVector3 m_posB;
153
154 btVector3 m_separatingNormal;
155
156 btScalar m_boundingRadiusA;
157 btScalar m_boundingRadiusB;
158 btScalar m_separatingDistance;
159
160public:
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 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
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:499
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:498
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
ANGULAR_MOTION_THRESHOLD
#define ANGULAR_MOTION_THRESHOLD
Definition: btTransformUtil.h:19
btAabbSupport
btVector3 btAabbSupport(const btVector3 &halfExtents, const btVector3 &supportDir)
Definition: btTransformUtil.h:21
btTransform.h
btConvexSeparatingDistanceUtil
The btConvexSeparatingDistanceUtil can help speed up convex collision detection by conservatively upd...
Definition: btTransformUtil.h:148
btConvexSeparatingDistanceUtil::m_separatingNormal
btVector3 m_separatingNormal
Definition: btTransformUtil.h:154
btConvexSeparatingDistanceUtil::m_ornA
btQuaternion m_ornA
Definition: btTransformUtil.h:149
btConvexSeparatingDistanceUtil::m_posB
btVector3 m_posB
Definition: btTransformUtil.h:152
btConvexSeparatingDistanceUtil::m_posA
btVector3 m_posA
Definition: btTransformUtil.h:151
btConvexSeparatingDistanceUtil::m_boundingRadiusB
btScalar m_boundingRadiusB
Definition: btTransformUtil.h:157
btConvexSeparatingDistanceUtil::m_separatingDistance
btScalar m_separatingDistance
Definition: btTransformUtil.h:158
btConvexSeparatingDistanceUtil::getConservativeSeparatingDistance
btScalar getConservativeSeparatingDistance()
Definition: btTransformUtil.h:168
btConvexSeparatingDistanceUtil::initSeparatingDistance
void initSeparatingDistance(const btVector3 &separatingVector, btScalar separatingDistance, const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:203
btConvexSeparatingDistanceUtil::m_ornB
btQuaternion m_ornB
Definition: btTransformUtil.h:150
btConvexSeparatingDistanceUtil::btConvexSeparatingDistanceUtil
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
Definition: btTransformUtil.h:161
btConvexSeparatingDistanceUtil::m_boundingRadiusA
btScalar m_boundingRadiusA
Definition: btTransformUtil.h:156
btConvexSeparatingDistanceUtil::updateSeparatingDistance
void updateSeparatingDistance(const btTransform &transA, const btTransform &transB)
Definition: btTransformUtil.h:173
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1093
btMatrix3x3::getRotation
void getRotation(btQuaternion &q) const
Get the matrix represented as a quaternion.
Definition: btMatrix3x3.h:420
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btQuaternion::getAngle
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:468
btQuaternion::nearest
btQuaternion nearest(const btQuaternion &qd) const
Definition: btQuaternion.h:563
btQuaternion::length2
btScalar length2() const
Return the length squared of the quaternion.
Definition: btQuaternion.h:364
btQuaternion::safeNormalize
btQuaternion & safeNormalize()
Definition: btQuaternion.h:374
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
btTransformUtil
Utils related to temporal transforms.
Definition: btTransformUtil.h:30
btTransformUtil::calculateDiffAxisAngleQuaternion
static void calculateDiffAxisAngleQuaternion(const btQuaternion &orn0, const btQuaternion &orn1a, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:100
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
btTransformUtil::calculateDiffAxisAngle
static void calculateDiffAxisAngle(const btTransform &transform0, const btTransform &transform1, btVector3 &axis, btScalar &angle)
Definition: btTransformUtil.h:124
btTransformUtil::integrateTransform
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
Definition: btTransformUtil.h:32
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:115
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:109
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:161
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:119
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:114
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:147
btTransform::setBasis
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:155
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577