Ejemplo n.º 1
0
        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);
            }
        }