dali-physics/third-party/bullet3/src/LinearMath/btTransformUtil.h

   1 /*
   2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  https://bulletphysics.org
   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 #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
  28 /// Utils related to temporal transforms
  29 class btTransformUtil
  30 {
  31 public:
  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
 130                 ///floating point inaccuracy can lead to w component > 1..., which breaks
 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
 145 ///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
 146 ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
 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
 160 public:
 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