public static void muFromSO3(Matrix3x3d so3, Vector3d result)
{
double cosAngle = (so3.get(0, 0) + so3.get(1, 1) + so3.get(2, 2) - 1.0D) * 0.5D;
result.set((so3.get(2, 1) - so3.get(1, 2)) / 2.0D, (so3.get(0, 2) - so3.get(2, 0)) / 2.0D, (so3.get(1, 0) - so3.get(0, 1)) / 2.0D);
double sinAngleAbs = result.length();
if (cosAngle > M_SQRT1_2)
{
if (sinAngleAbs > 0.0D)
{
result.scale(Math.Asin(sinAngleAbs) / sinAngleAbs);
}
}
else if (cosAngle > -M_SQRT1_2)
{
double angle = Math.Acos(cosAngle);
result.scale(angle / sinAngleAbs);
}
else
{
double angle = 3.141592653589793D - Math.Asin(sinAngleAbs);
double d0 = so3.get(0, 0) - cosAngle;
double d1 = so3.get(1, 1) - cosAngle;
double d2 = so3.get(2, 2) - cosAngle;
Vector3d r2 = muFromSO3R2;
if ((d0 * d0 > d1 * d1) && (d0 * d0 > d2 * d2))
{
r2.set(d0, (so3.get(1, 0) + so3.get(0, 1)) / 2.0D, (so3.get(0, 2) + so3.get(2, 0)) / 2.0D);
}
else if (d1 * d1 > d2 * d2)
{
r2.set((so3.get(1, 0) + so3.get(0, 1)) / 2.0D, d1, (so3.get(2, 1) + so3.get(1, 2)) / 2.0D);
}
else
{
r2.set((so3.get(0, 2) + so3.get(2, 0)) / 2.0D, (so3.get(2, 1) + so3.get(1, 2)) / 2.0D, d2);
}
if (Vector3d.dot(r2, result) < 0.0D)
{
r2.scale(-1.0D);
}
r2.normalize();
r2.scale(angle);
result.set(r2);
}
}
