bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
// cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
// -sy cy*sx cy*cx
btScalar fi = btGetMatrixElem(mat,6);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
xyz[1] = btAsin(-btGetMatrixElem(mat,6));
xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
return true;
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = SIMD_HALF_PI;
xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
return false;
}
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = -SIMD_HALF_PI;
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
}
return false;
}
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
btScalar fi = btGetMatrixElem(mat,2);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = btAsin(btGetMatrixElem(mat,2));
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = -SIMD_HALF_PI;
xyz[2] = btScalar(0.0);
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = SIMD_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
// cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
// -cx*sy sx cx*cy
btScalar fi = btGetMatrixElem(mat,7);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAsin(btGetMatrixElem(mat,7));
xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
return true;
}
else
{
xyz[0] = -SIMD_HALF_PI;
xyz[1] = btScalar(0.0);
xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
return false;
}
}
else
{
xyz[0] = SIMD_HALF_PI;
xyz[1] = btScalar(0.0);
xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
btScalar fi = btGetMatrixElem(mat,3);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
xyz[2] = btAsin(btGetMatrixElem(mat,3));
return true;
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
xyz[2] = -SIMD_HALF_PI;
return false;
}
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
xyz[2] = SIMD_HALF_PI;
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
// cx*sz cx*cz -sx
// cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
btScalar fi = btGetMatrixElem(mat,5);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAsin(-btGetMatrixElem(mat,5));
xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
return true;
}
else
{
xyz[0] = SIMD_HALF_PI;
xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
xyz[2] = btScalar(0.0);
return false;
}
}
else
{
xyz[0] = -SIMD_HALF_PI;
xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
xyz[2] = 0.0;
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cy*cz -sz sy*cz
// cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
// cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
btScalar fi = btGetMatrixElem(mat,1);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
xyz[2] = btAsin(-btGetMatrixElem(mat,1));
return true;
}
else
{
xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
xyz[1] = btScalar(0.0);
xyz[2] = SIMD_HALF_PI;
return false;
}
}
else
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
xyz[1] = 0.0;
xyz[2] = -SIMD_HALF_PI;
}
return false;
}
bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz)
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
if (btGetMatrixElem(mat,2) < real_t(1.0))
{
if (btGetMatrixElem(mat,2) > real_t(-1.0))
{
xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = Math::asin(btGetMatrixElem(mat,2));
xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = -Math_PI*0.5;
xyz[2] = real_t(0.0);
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = Math_PI*0.5;
xyz[2] = 0.0;
}
return false;
}
