dali-physics/third-party/bullet3/src/Bullet3Common/b3TransformUtil.h

   1 /*
   2 Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://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 B3_TRANSFORM_UTIL_H
  16 #define B3_TRANSFORM_UTIL_H
  17
  18 #include "b3Transform.h"
  19 #define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
  20
  21 B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
  22 {
  23         return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
  24                                                  supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
  25                                                  supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
  26 }
  27
  28 /// Utils related to temporal transforms
  29 class b3TransformUtil
  30 {
  31 public:
  32         static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
  33         {
  34                 predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
  35                 //      #define QUATERNION_DERIVATIVE
  36 #ifdef QUATERNION_DERIVATIVE
  37                 b3Quaternion predictedOrn = curTrans.getRotation();
  38                 predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
  39                 predictedOrn.normalize();
  40 #else
  41                 //Exponential map
  42                 //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
  43
  44                 b3Vector3 axis;
  45                 b3Scalar fAngle = angvel.length();
  46                 //limit the angular motion
  47                 if (fAngle * timeStep > B3_ANGULAR_MOTION_THRESHOLD)
  48                 {
  49                         fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep;
  50                 }
  51
  52                 if (fAngle < b3Scalar(0.001))
  53                 {
  54                         // use Taylor's expansions of sync function
  55                         axis = angvel * (b3Scalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (b3Scalar(0.020833333333)) * fAngle * fAngle);
  56                 }
  57                 else
  58                 {
  59                         // sync(fAngle) = sin(c*fAngle)/t
  60                         axis = angvel * (b3Sin(b3Scalar(0.5) * fAngle * timeStep) / fAngle);
  61                 }
  62                 b3Quaternion dorn(axis.getX(), axis.getY(), axis.getZ(), b3Cos(fAngle * timeStep * b3Scalar(0.5)));
  63                 b3Quaternion orn0 = curTrans.getRotation();
  64
  65                 b3Quaternion predictedOrn = dorn * orn0;
  66                 predictedOrn.normalize();
  67 #endif
  68                 predictedTransform.setRotation(predictedOrn);
  69         }
  70
  71         static void calculateVelocityQuaternion(const b3Vector3& pos0, const b3Vector3& pos1, const b3Quaternion& orn0, const b3Quaternion& orn1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
  72         {
  73                 linVel = (pos1 - pos0) / timeStep;
  74                 b3Vector3 axis;
  75                 b3Scalar angle;
  76                 if (orn0 != orn1)
  77                 {
  78                         calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
  79                         angVel = axis * angle / timeStep;
  80                 }
  81                 else
  82                 {
  83                         angVel.setValue(0, 0, 0);
  84                 }
  85         }
  86
  87         static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0, const b3Quaternion& orn1a, b3Vector3& axis, b3Scalar& angle)
  88         {
  89                 b3Quaternion orn1 = orn0.nearest(orn1a);
  90                 b3Quaternion dorn = orn1 * orn0.inverse();
  91                 angle = dorn.getAngle();
  92                 axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
  93                 axis[3] = b3Scalar(0.);
  94                 //check for axis length
  95                 b3Scalar len = axis.length2();
  96                 if (len < B3_EPSILON * B3_EPSILON)
  97                         axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
  98                 else
  99                         axis /= b3Sqrt(len);
 100         }
 101
 102         static void calculateVelocity(const b3Transform& transform0, const b3Transform& transform1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
 103         {
 104                 linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
 105                 b3Vector3 axis;
 106                 b3Scalar angle;
 107                 calculateDiffAxisAngle(transform0, transform1, axis, angle);
 108                 angVel = axis * angle / timeStep;
 109         }
 110
 111         static void calculateDiffAxisAngle(const b3Transform& transform0, const b3Transform& transform1, b3Vector3& axis, b3Scalar& angle)
 112         {
 113                 b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
 114                 b3Quaternion dorn;
 115                 dmat.getRotation(dorn);
 116
 117                 ///floating point inaccuracy can lead to w component > 1..., which breaks
 118                 dorn.normalize();
 119
 120                 angle = dorn.getAngle();
 121                 axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
 122                 axis[3] = b3Scalar(0.);
 123                 //check for axis length
 124                 b3Scalar len = axis.length2();
 125                 if (len < B3_EPSILON * B3_EPSILON)
 126                         axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
 127                 else
 128                         axis /= b3Sqrt(len);
 129         }
 130 };
 131
 132 ///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
 133 ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
 134 class b3ConvexSeparatingDistanceUtil
 135 {
 136         b3Quaternion m_ornA;
 137         b3Quaternion m_ornB;
 138         b3Vector3 m_posA;
 139         b3Vector3 m_posB;
 140
 141         b3Vector3 m_separatingNormal;
 142
 143         b3Scalar m_boundingRadiusA;
 144         b3Scalar m_boundingRadiusB;
 145         b3Scalar m_separatingDistance;
 146
 147 public:
 148         b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
 149                 : m_boundingRadiusA(boundingRadiusA),
 150                   m_boundingRadiusB(boundingRadiusB),
 151                   m_separatingDistance(0.f)
 152         {
 153         }
 154
 155         b3Scalar getConservativeSeparatingDistance()
 156         {
 157                 return m_separatingDistance;
 158         }
 159
 160         void updateSeparatingDistance(const b3Transform& transA, const b3Transform& transB)
 161         {
 162                 const b3Vector3& toPosA = transA.getOrigin();
 163                 const b3Vector3& toPosB = transB.getOrigin();
 164                 b3Quaternion toOrnA = transA.getRotation();
 165                 b3Quaternion toOrnB = transB.getRotation();
 166
 167                 if (m_separatingDistance > 0.f)
 168                 {
 169                         b3Vector3 linVelA, angVelA, linVelB, angVelB;
 170                         b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
 171                         b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
 172                         b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
 173                         b3Vector3 relLinVel = (linVelB - linVelA);
 174                         b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
 175                         if (relLinVelocLength < 0.f)
 176                         {
 177                                 relLinVelocLength = 0.f;
 178                         }
 179
 180                         b3Scalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
 181                         m_separatingDistance -= projectedMotion;
 182                 }
 183
 184                 m_posA = toPosA;
 185                 m_posB = toPosB;
 186                 m_ornA = toOrnA;
 187                 m_ornB = toOrnB;
 188         }
 189
 190         void initSeparatingDistance(const b3Vector3& separatingVector, b3Scalar separatingDistance, const b3Transform& transA, const b3Transform& transB)
 191         {
 192                 m_separatingDistance = separatingDistance;
 193
 194                 if (m_separatingDistance > 0.f)
 195                 {
 196                         m_separatingNormal = separatingVector;
 197
 198                         const b3Vector3& toPosA = transA.getOrigin();
 199                         const b3Vector3& toPosB = transB.getOrigin();
 200                         b3Quaternion toOrnA = transA.getRotation();
 201                         b3Quaternion toOrnB = transB.getRotation();
 202                         m_posA = toPosA;
 203                         m_posB = toPosB;
 204                         m_ornA = toOrnA;
 205                         m_ornB = toOrnB;
 206                 }
 207         }
 208 };
 209
 210 #endif  //B3_TRANSFORM_UTIL_H