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
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{
31public:
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));
40#else
41 //Exponential map
42 //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
43
44 btVector3 axis;
46 btScalar fAngle = 0;
48 {
50 }
51
52 //limit the angular motion
53 if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD)
54 {
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
73#endif
74 if (predictedOrn.length2() > SIMD_EPSILON)
75 {
77 }
78 else
79 {
80 predictedTransform.setBasis(curTrans.getBasis());
81 }
82 }
83
85 {
86 linVel = (pos1 - pos0) / timeStep;
87 btVector3 axis;
88 btScalar angle;
89 if (orn0 != orn1)
90 {
92 angVel = axis * angle / timeStep;
93 }
94 else
95 {
96 angVel.setValue(0, 0, 0);
97 }
98 }
99
101 {
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
116 {
117 linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
118 btVector3 axis;
119 btScalar angle;
121 angVel = axis * angle / timeStep;
122 }
123
125 {
126 btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
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
160public:
165 {
166 }
167
169 {
171 }
172
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 {
188 if (relLinVelocLength < 0.f)
189 {
190 relLinVelocLength = 0.f;
191 }
192
195 }
196
197 m_posA = toPosA;
198 m_posB = toPosB;
199 m_ornA = toOrnA;
200 m_ornB = toOrnB;
201 }
202
204 {
206
207 if (m_separatingDistance > 0.f)
208 {
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
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btScalar btSqrt(btScalar y)
Definition btScalar.h:466
btScalar btSin(btScalar x)
Definition btScalar.h:499
#define SIMD_FORCE_INLINE
Definition btScalar.h:98
btScalar btCos(btScalar x)
Definition btScalar.h:498
#define SIMD_EPSILON
Definition btScalar.h:543
#define ANGULAR_MOTION_THRESHOLD
btVector3 btAabbSupport(const btVector3 &halfExtents, const btVector3 &supportDir)
The btConvexSeparatingDistanceUtil can help speed up convex collision detection by conservatively upd...
void initSeparatingDistance(const btVector3 &separatingVector, btScalar separatingDistance, const btTransform &transA, const btTransform &transB)
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
void updateSeparatingDistance(const btTransform &transA, const btTransform &transB)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3 inverse() const
Return the inverse of the matrix.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
btQuaternion nearest(const btQuaternion &qd) const
btQuaternion & safeNormalize()
btQuaternion inverse() const
Return the inverse of this quaternion.
void setRotation(const btVector3 &axis, const btScalar &_angle)
Set the rotation using axis angle notation.
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Utils related to temporal transforms.
static void calculateDiffAxisAngleQuaternion(const btQuaternion &orn0, const btQuaternion &orn1a, btVector3 &axis, btScalar &angle)
static void calculateVelocityQuaternion(const btVector3 &pos0, const btVector3 &pos1, const btQuaternion &orn0, const btQuaternion &orn1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
static void calculateDiffAxisAngle(const btTransform &transform0, const btTransform &transform1, btVector3 &axis, btScalar &angle)
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
const btScalar & z() const
Return the z value.
Definition btVector3.h:579
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition btVector3.h:640
btScalar length2() const
Return the length of the vector squared.
Definition btVector3.h:251
const btScalar & x() const
Return the x value.
Definition btVector3.h:575
const btScalar & y() const
Return the y value.
Definition btVector3.h:577