Example #1
0
 public Quaternion(double angle, Vector unit_axis)
 {
     double sa = Math.Sin(angle * 0.5);
     w = Math.Cos(angle * 0.5);
     x = unit_axis.x * sa;
     y = unit_axis.y * sa;
     z = unit_axis.z * sa;
 }
Example #2
0
 public TMatrix Translated(Vector v)
 {
     var x = new TMatrix( (double[])this.m.Clone() );
     x.m[12] += v.x;
     x.m[13] += v.y;
     x.m[14] += v.z;
     return x;
 }
Example #3
0
        public Quaternion GetRotation()
        {
            // Decompose the transformation to a rotation followed by translation, and return
            //   the rotation.  Throws InvalidOperationException if the matrix is not of that form.
            if (m[3] != 0 || m[7] != 0 || m[11] != 0 || m[15] != 1)
                throw new InvalidOperationException("Not a rotation+translation.");

            // Check for any scale, which we don't decompose (actually TMatrix is never supposed to
            // have a scale)
            for(int i=0; i<3; i++)
                if (Math.Abs(m[0+i]*m[0+i] + m[4+i]*m[4+i] + m[8+i]*m[8+i] - 1.0) > .0001)
                    throw new InvalidOperationException("Not a rotation+translation.");

            // Extract rotation matrix
            double[] r = new double[9];
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    r[i * 3 + j] = m[i * 4 + j];

            // Test that it is (approximately) special orthogonal.  If it isn't, the input matrix didn't meet the desired
            // form.
            double det = r[0] * (r[4] * r[8] - r[7] * r[5]) - r[1] * (r[3] * r[8] - r[6] * r[5]) + r[2] * (r[3] * r[7] - r[6] * r[4]);
            if (det < 0.99999 || det > 1.00001)
                throw new InvalidOperationException("Not a rotation+translation.");

            // Rotation -> quaternion
            var rotation = new Quaternion();

            var T = r[0] + r[3 + 1] + r[6 + 2];
            if (T > 0.0)
            {
                var S = 0.5 / Math.Sqrt(T + 1);
                rotation.w = 0.25 / S;
                rotation.x = (r[6 + 1] - r[3 + 2]) * S;
                rotation.y = (r[0 + 2] - r[6 + 0]) * S;
                rotation.z = (r[3 + 0] - r[0 + 1]) * S;
            }
            else if (r[0] > r[3 + 1] && r[0] > r[6 + 2])
            {
                var S = Math.Sqrt(1.0 + r[0] - r[3 + 1] - r[6 + 2]);
                rotation.x = S * 0.5;
                S = 0.5 / S;
                rotation.w = (r[6 + 1] - r[3 + 2]) * S;
                rotation.y = (r[0 + 1] + r[3 + 0]) * S;
                rotation.z = (r[0 + 2] + r[6 + 0]) * S;
            }
            else if (r[3 + 1] > r[6 + 2])
            {
                var S = Math.Sqrt(1.0 + r[3 + 1] - r[0 + 0] - r[6 + 2]);
                rotation.y = S * 0.5;
                S = 0.5 / S;
                rotation.w = (r[0 + 2] - r[6 + 0]) * S;
                rotation.x = (r[0 + 1] + r[3 + 0]) * S;
                rotation.z = (r[3 + 2] + r[6 + 1]) * S;
            }
            else
            {
                var S = Math.Sqrt(1.0 + r[6 + 2] - r[0 + 0] - r[3 + 1]);
                rotation.z = S * 0.5;
                S = 0.5 / S;
                rotation.w = (r[3 + 0] - r[0 + 1]) * S;
                rotation.x = (r[0 + 2] + r[6 + 0]) * S;
                rotation.y = (r[3 + 2] + r[6 + 1]) * S;
            }
            /*var tr = r[0] + r[4] + r[8];
            if (tr >= 0.0)
            {
                var s = Math.Sqrt(tr + 1.0);
                rotation.w = s * 0.5;
                s = 0.5 / s;
                rotation.x = (r[7] - r[5]) * s;
                rotation.y = (r[2] - r[6]) * s;
                rotation.z = (r[3] - r[1]) * s;
            } else {
                int h = 0;
                if (r[4] > */

            /*
            // Shoemake '85
            var rotation = new Quaternion();
            var w2 = 0.25 * (1 + r[0] + r[4] + r[8]);
            if (w2 > double.Epsilon)
            {
                rotation.w = Math.Sqrt(w2);
                rotation.x = (r[7] - r[5]) / (4 * rotation.w);
                rotation.y = (r[2] - r[6]) / (4 * rotation.w);
                rotation.z = (r[3] - r[1]) / (4 * rotation.w);
            }
            else
            {
                rotation.w = 0;
                var x2 = -0.5 * (r[4] + r[8]);
                if (x2 > double.Epsilon)
                {
                    rotation.x = Math.Sqrt(x2);
                    rotation.y = r[3] / (2 * rotation.x);
                    rotation.z = r[6] / (2 * rotation.x);
                }
                else
                {
                    rotation.x = 0;
                    var y2 = 0.5 * (1 - r[8]);
                    if (y2 > double.Epsilon)
                    {
                        rotation.y = Math.Sqrt(y2);
                        rotation.z = r[7] / (2 * rotation.y);
                    }
                    else
                    {
                        rotation.y = 0;
                        rotation.z = 1;
                    }
                }
            }
            rotation.Renormalize();*/

            var tv = new Vector(1,2,3);
            var tp = rotation.ToMatrix().Inverse() * this * tv;
            if ((tp-tv)*(tp-tv) > .0001)
                Console.WriteLine("GetRotation error.");

            rotation.Renormalize();
            return rotation;

            /*rotation.w = Math.Sqrt(Math.Max(0, 1 + r[0] + r[4] + r[8])) * 0.5;
            rotation.x = Math.Sqrt(Math.Max(0, 1 + r[0] - r[4] - r[8])) * 0.5;
            rotation.y = Math.Sqrt(Math.Max(0, 1 - r[0] + r[4] - r[8])) * 0.5;
            rotation.z = Math.Sqrt(Math.Max(0, 1 - r[0] - r[4] + r[8])) * 0.5;
            if (r[6 + 1] - r[3 + 2] < 0) rotation.x = -rotation.x;
            if (r[0 + 2] - r[6 + 0] < 0) rotation.y = -rotation.y;
            if (r[3 + 0] - r[0 + 1] < 0) rotation.z = -rotation.z;
            rotation.Renormalize();
            return rotation;*/

            /*var T = r[0] + r[3 + 1] + r[6 + 2] + 1;
            var rotation = new Quaternion();
            if (T > 0.00000001)
            {
                var S = 0.5 / Math.Sqrt(T);
                rotation.w = 0.25 / S;
                rotation.x = (r[6 + 1] - r[3 + 2]) * S;
                rotation.y = (r[0 + 2] - r[6 + 0]) * S;
                rotation.z = (r[3 + 0] - r[0 + 1]) * S;
            }
            else if (r[0] > r[3 + 1] && r[0] > r[6 + 2])
            {
                var S = 0.5 / Math.Sqrt(1.0 + r[0] - r[3 + 1] - r[6 + 2]);
                rotation.w = (r[6 + 1] - r[3 + 2]) * S;
                rotation.x = 0.25 / S;
                rotation.y = (r[0 + 1] + r[3 + 0]) * S;
                rotation.z = (r[0 + 2] + r[6 + 0]) * S;
            }
            else if (r[3 + 1] > r[6 + 2])
            {
                var S = 0.5 / Math.Sqrt(1.0 + r[3 + 1] - r[0 + 0] - r[6 + 2]);
                rotation.w = (r[0 + 2] - r[6 + 0]) * S;
                rotation.x = (r[0 + 1] + r[3 + 0]) * S;
                rotation.y = 0.25 * S;
                rotation.z = (r[3 + 2] + r[6 + 1]) * S;
            }
            else
            {
                var S = 0.5 / Math.Sqrt(1.0 + r[6 + 2] - r[0 + 0] - r[3 + 1]);
                rotation.w = (r[3 + 0] - r[0 + 1]) * S;
                rotation.x = (r[0 + 2] + r[6 + 0]) * S;
                rotation.y = (r[3 + 2] + r[6 + 1]) * S;
                rotation.z = 0.25 * S;
            }
            rotation.Renormalize();  // Rounding error can make w>1, yielding NAN for rotation.Angle
            return rotation;*/

            // translation = R^T * (-this.w_column)
            /*for (int i = 0; i < 3; i++)
                translation[i] = -(r[0 + i] * m[12] + r[3 + i] * m[13] + r[6 + i] * m[14]);*/
        }
Example #4
0
 public static TMatrix Translation(Vector v)
 {
     return new TMatrix( new double[] { 1,0,0,0, 0,1,0,0, 0,0,1,0, v.x,v.y,v.z,1 } );
 }
Example #5
0
        static void TestMath()
        {
            var rand = new Random();

            for (int i = 0; i < 1000; i++)
            {
                var u = new Vector( rand.NextDouble()-0.5, rand.NextDouble()-0.5, rand.NextDouble()-0.5 ).Normalized();
                var t1 = new Vector( rand.NextDouble()-0.5, rand.NextDouble()-0.5, rand.NextDouble()-0.5 ) * 10;
                var t2 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;
                var p1 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;

                var q = new Quaternion(rand.NextDouble() * Math.PI, u);
                var m = q.ToMatrix();
                var mt = TMatrix.Translation(t1) * m * TMatrix.Translation(t2);
                var q2 = mt.GetRotation();
                var m2 = q2.ToMatrix();

                if (((m * p1) - (m2 * p1)) * (m * p1 - m2 * p1) > .0001)
                    Console.WriteLine("math error.");
            }

            var badq = new Quaternion(3.1414934204542249,
                new Vector(-0.920567453, -0.390583634, -0));
            badq = badq.ToMatrix().GetRotation();
            Console.WriteLine(badq.Angle);
            Console.WriteLine(badq.Axis);
        }