四元数指数映射和应用

四元数指数映射和应用

背景

主要参考论文Practical Parameterization of Rotations Using the Exponential Map 和中文翻译 。

Bullet 中源码

  static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
  {
    predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
//  #define QUATERNION_DERIVATIVE
  #ifdef QUATERNION_DERIVATIVE
    btQuaternion predictedOrn = curTrans.getRotation();
    predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
    predictedOrn.safeNormalize();
  #else
    //Exponential map
    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

    btVector3 axis;
    btScalar  fAngle2 = angvel.length2();
          btScalar    fAngle = 0;
          if (fAngle2>SIMD_EPSILON)
          {
                fAngle = btSqrt(fAngle2);
          }

    //limit the angular motion
    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
    {
      fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
    }

    if ( fAngle < btScalar(0.001) )
    {
      // use Taylor's expansions of sync function
      axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
    }
    else
    {
      // sync(fAngle) = sin(c*fAngle)/t
      axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
    }
    btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
    btQuaternion orn0 = curTrans.getRotation();

    btQuaternion predictedOrn = dorn * orn0;
    predictedOrn.safeNormalize();
  #endif
    if (predictedOrn.length2()>SIMD_EPSILON)
    {
      predictedTransform.setRotation(predictedOrn);
    }
    else
    {
      predictedTransform.setBasis(curTrans.getBasis());
    }
  }

这里 intergrateform 函数实现了给定当前变换 curTrans、线速度 linvel、角速度 angvel 和步长 timeStep,求下一步的变换矩阵 predictedTransform.
如果定义了 QUATERNION_DERIVATIVE，就会按照四元数积分的方法进行积分,积分公式如下:

q'=q+0.5⋅ δ t ω q

q^'=q+0.5⋅ δ t ω q

Date: [2018-11-06 Tue 21:30]

Author: fhln

Created: 2018-11-06 Tue 23:24

Validate