summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Common/b3TransformUtil.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Common/b3TransformUtil.h')
-rw-r--r--thirdparty/bullet/Bullet3Common/b3TransformUtil.h146
1 files changed, 64 insertions, 82 deletions
diff --git a/thirdparty/bullet/Bullet3Common/b3TransformUtil.h b/thirdparty/bullet/Bullet3Common/b3TransformUtil.h
index 6ce580c132..1850a9be5f 100644
--- a/thirdparty/bullet/Bullet3Common/b3TransformUtil.h
+++ b/thirdparty/bullet/Bullet3Common/b3TransformUtil.h
@@ -12,204 +12,189 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
#ifndef B3_TRANSFORM_UTIL_H
#define B3_TRANSFORM_UTIL_H
#include "b3Transform.h"
-#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5)*B3_HALF_PI
-
-
-
+#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
-B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir)
+B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
{
return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
- supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
- supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
+ supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
+ supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
}
-
-
-
-
-
/// Utils related to temporal transforms
class b3TransformUtil
{
-
public:
-
- static void integrateTransform(const b3Transform& curTrans,const b3Vector3& linvel,const b3Vector3& angvel,b3Scalar timeStep,b3Transform& predictedTransform)
+ static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
-// #define QUATERNION_DERIVATIVE
- #ifdef QUATERNION_DERIVATIVE
+ // #define QUATERNION_DERIVATIVE
+#ifdef QUATERNION_DERIVATIVE
b3Quaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
predictedOrn.normalize();
- #else
+#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
b3Vector3 axis;
- b3Scalar fAngle = angvel.length();
+ b3Scalar fAngle = angvel.length();
//limit the angular motion
- if (fAngle*timeStep > B3_ANGULAR_MOTION_THRESHOLD)
+ if (fAngle * timeStep > B3_ANGULAR_MOTION_THRESHOLD)
{
fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep;
}
- if ( fAngle < b3Scalar(0.001) )
+ if (fAngle < b3Scalar(0.001))
{
// use Taylor's expansions of sync function
- axis = angvel*( b3Scalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(b3Scalar(0.020833333333))*fAngle*fAngle );
+ axis = angvel * (b3Scalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (b3Scalar(0.020833333333)) * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
- axis = angvel*( b3Sin(b3Scalar(0.5)*fAngle*timeStep)/fAngle );
+ axis = angvel * (b3Sin(b3Scalar(0.5) * fAngle * timeStep) / fAngle);
}
- b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),b3Cos( fAngle*timeStep*b3Scalar(0.5) ));
+ b3Quaternion dorn(axis.getX(), axis.getY(), axis.getZ(), b3Cos(fAngle * timeStep * b3Scalar(0.5)));
b3Quaternion orn0 = curTrans.getRotation();
b3Quaternion predictedOrn = dorn * orn0;
predictedOrn.normalize();
- #endif
+#endif
predictedTransform.setRotation(predictedOrn);
}
- static void calculateVelocityQuaternion(const b3Vector3& pos0,const b3Vector3& pos1,const b3Quaternion& orn0,const b3Quaternion& orn1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
+ static void calculateVelocityQuaternion(const b3Vector3& pos0, const b3Vector3& pos1, const b3Quaternion& orn0, const b3Quaternion& orn1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
{
linVel = (pos1 - pos0) / timeStep;
b3Vector3 axis;
- b3Scalar angle;
+ b3Scalar angle;
if (orn0 != orn1)
{
- calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
+ calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
angVel = axis * angle / timeStep;
- } else
+ }
+ else
{
- angVel.setValue(0,0,0);
+ angVel.setValue(0, 0, 0);
}
}
- static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)
+ static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0, const b3Quaternion& orn1a, b3Vector3& axis, b3Scalar& angle)
{
b3Quaternion orn1 = orn0.nearest(orn1a);
b3Quaternion dorn = orn1 * orn0.inverse();
angle = dorn.getAngle();
- axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ());
+ axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
axis[3] = b3Scalar(0.);
//check for axis length
b3Scalar len = axis.length2();
- if (len < B3_EPSILON*B3_EPSILON)
- axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
+ if (len < B3_EPSILON * B3_EPSILON)
+ axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
else
axis /= b3Sqrt(len);
}
- static void calculateVelocity(const b3Transform& transform0,const b3Transform& transform1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
+ static void calculateVelocity(const b3Transform& transform0, const b3Transform& transform1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
{
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
b3Vector3 axis;
- b3Scalar angle;
- calculateDiffAxisAngle(transform0,transform1,axis,angle);
+ b3Scalar angle;
+ calculateDiffAxisAngle(transform0, transform1, axis, angle);
angVel = axis * angle / timeStep;
}
- static void calculateDiffAxisAngle(const b3Transform& transform0,const b3Transform& transform1,b3Vector3& axis,b3Scalar& angle)
+ static void calculateDiffAxisAngle(const b3Transform& transform0, const b3Transform& transform1, b3Vector3& axis, b3Scalar& angle)
{
b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
b3Quaternion dorn;
dmat.getRotation(dorn);
- ///floating point inaccuracy can lead to w component > 1..., which breaks
+ ///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
-
+
angle = dorn.getAngle();
- axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ());
+ axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
axis[3] = b3Scalar(0.);
//check for axis length
b3Scalar len = axis.length2();
- if (len < B3_EPSILON*B3_EPSILON)
- axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
+ if (len < B3_EPSILON * B3_EPSILON)
+ axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
else
axis /= b3Sqrt(len);
}
-
};
-
-///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
+///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
-class b3ConvexSeparatingDistanceUtil
+class b3ConvexSeparatingDistanceUtil
{
- b3Quaternion m_ornA;
- b3Quaternion m_ornB;
- b3Vector3 m_posA;
- b3Vector3 m_posB;
-
- b3Vector3 m_separatingNormal;
+ b3Quaternion m_ornA;
+ b3Quaternion m_ornB;
+ b3Vector3 m_posA;
+ b3Vector3 m_posB;
- b3Scalar m_boundingRadiusA;
- b3Scalar m_boundingRadiusB;
- b3Scalar m_separatingDistance;
+ b3Vector3 m_separatingNormal;
-public:
+ b3Scalar m_boundingRadiusA;
+ b3Scalar m_boundingRadiusB;
+ b3Scalar m_separatingDistance;
- b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA,b3Scalar boundingRadiusB)
- :m_boundingRadiusA(boundingRadiusA),
- m_boundingRadiusB(boundingRadiusB),
- m_separatingDistance(0.f)
+public:
+ b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
+ : m_boundingRadiusA(boundingRadiusA),
+ m_boundingRadiusB(boundingRadiusB),
+ m_separatingDistance(0.f)
{
}
- b3Scalar getConservativeSeparatingDistance()
+ b3Scalar getConservativeSeparatingDistance()
{
return m_separatingDistance;
}
- void updateSeparatingDistance(const b3Transform& transA,const b3Transform& transB)
+ void updateSeparatingDistance(const b3Transform& transA, const b3Transform& transB)
{
const b3Vector3& toPosA = transA.getOrigin();
const b3Vector3& toPosB = transB.getOrigin();
b3Quaternion toOrnA = transA.getRotation();
b3Quaternion toOrnB = transB.getRotation();
- if (m_separatingDistance>0.f)
+ if (m_separatingDistance > 0.f)
{
-
-
- b3Vector3 linVelA,angVelA,linVelB,angVelB;
- b3TransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,b3Scalar(1.),linVelA,angVelA);
- b3TransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,b3Scalar(1.),linVelB,angVelB);
+ b3Vector3 linVelA, angVelA, linVelB, angVelB;
+ b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
+ b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
- b3Vector3 relLinVel = (linVelB-linVelA);
+ b3Vector3 relLinVel = (linVelB - linVelA);
b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
- if (relLinVelocLength<0.f)
+ if (relLinVelocLength < 0.f)
{
relLinVelocLength = 0.f;
}
-
- b3Scalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
+
+ b3Scalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
-
+
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
- void initSeparatingDistance(const b3Vector3& separatingVector,b3Scalar separatingDistance,const b3Transform& transA,const b3Transform& transB)
+ void initSeparatingDistance(const b3Vector3& separatingVector, b3Scalar separatingDistance, const b3Transform& transA, const b3Transform& transB)
{
m_separatingDistance = separatingDistance;
- if (m_separatingDistance>0.f)
+ if (m_separatingDistance > 0.f)
{
m_separatingNormal = separatingVector;
-
+
const b3Vector3& toPosA = transA.getOrigin();
const b3Vector3& toPosB = transB.getOrigin();
b3Quaternion toOrnA = transA.getRotation();
@@ -220,9 +205,6 @@ public:
m_ornB = toOrnB;
}
}
-
};
-
-#endif //B3_TRANSFORM_UTIL_H
-
+#endif //B3_TRANSFORM_UTIL_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-08 04:17:20 +0000