void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R)
{
  FCL_REAL c1 = cos(a);
  FCL_REAL c2 = cos(b);
  FCL_REAL c3 = cos(c);
  FCL_REAL s1 = sin(a);
  FCL_REAL s2 = sin(b);
  FCL_REAL s3 = sin(c);

  R.setValue(c1 * c2, - c2 * s1, s2,
             c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3,
             s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
}
示例#2
0
文件: transform.cpp 项目: orthez/fcl
void Quaternion3f::toRotation(Matrix3f& R) const
{
  assertNormalized();
  FCL_REAL twoX  = 2.0*data[1];
  FCL_REAL twoY  = 2.0*data[2];
  FCL_REAL twoZ  = 2.0*data[3];
  FCL_REAL twoWX = twoX*data[0];
  FCL_REAL twoWY = twoY*data[0];
  FCL_REAL twoWZ = twoZ*data[0];
  FCL_REAL twoXX = twoX*data[1];
  FCL_REAL twoXY = twoY*data[1];
  FCL_REAL twoXZ = twoZ*data[1];
  FCL_REAL twoYY = twoY*data[2];
  FCL_REAL twoYZ = twoZ*data[2];
  FCL_REAL twoZZ = twoZ*data[3];

  R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
             twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
             twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
}