public Matrix3x3(Matrix3x3 otherMatrix)
 {
     matrix = new double[3, 3];
     for (int i = 0; i < 3; i++)
         for (int j = 0; j < 3; j++)
             matrix[i, j] = otherMatrix.matrix[i, j];
 }
 //Static Matrix Operations
 public static Vector3 Multiply(Vector3 A, Matrix3x3 B)
 {
     Vector3 ret = new Vector3(0, 0, 0);
     ret.X = B.matrix[0, 0] * A.X + B.matrix[1, 0] * A.Y + B.matrix[2, 0] * A.Z;
     ret.Y = B.matrix[0, 1] * A.X + B.matrix[1, 1] * A.Y + B.matrix[2, 1] * A.Z;
     ret.Z = B.matrix[0, 2] * A.X + B.matrix[1, 2] * A.Y + B.matrix[2, 2] * A.Z;
     return ret;
 }
 public static double Determinant(Matrix3x3 A)
 {
     double ret = 0;
     ret += A.matrix[0, 0] * A.matrix[1, 1] * A.matrix[2, 2];
     ret += A.matrix[1, 0] * A.matrix[2, 1] * A.matrix[0, 2];
     ret += A.matrix[2, 0] * A.matrix[0, 1] * A.matrix[1, 2];
     ret -= A.matrix[2, 0] * A.matrix[1, 1] * A.matrix[0, 2];
     ret -= A.matrix[1, 0] * A.matrix[0, 1] * A.matrix[2, 2];
     ret -= A.matrix[0, 0] * A.matrix[2, 1] * A.matrix[1, 2];
     return ret;
 }
 public static Matrix3x3 Multiply(double A, Matrix3x3 B)
 {
     Matrix3x3 ret = new Matrix3x3();
     for (int i = 0; i < 3; i++)
     {
         for (int j = 0; j < 3; j++)
         {
             ret.matrix[i, j] = A * B.matrix[i,j];
         }
     }
     return ret;
 }
 public static Matrix3x3 Multiply(Matrix3x3 A, Matrix3x3 B)
 {
     Matrix3x3 ret = new Matrix3x3();
     for (int i = 0; i < 3; i++)
     {
         for (int j = 0; j < 3; j++)
         {
             ret.matrix[i, j] = B.matrix[i, 0] * A.matrix[0, j] + B.matrix[i, 1] * A.matrix[1, j] + B.matrix[i, 2] * A.matrix[2, j];
         }
     }
     return ret;
 }
 public static Matrix3x3 Add(Matrix3x3 A, Matrix3x3 B)
 {
     Matrix3x3 ret = new Matrix3x3();
     for (int i = 0; i < 3; i++)
     {
         for (int j = 0; j < 3; j++)
         {
             ret.matrix[i, j] = A.matrix[j, i] + B.matrix[i, j];
         }
     }
     return ret;
 }
        //computes a rotation matrix based on a previous rotation matrix and a series of angle rotations
        public static Matrix3x3 nextRotMatrix(Matrix3x3 rotMatrix, Vector3 rotations)
        {
            //assuming C(t2) = C(t1)A(t1) where A(t1) is the rotation matrix relating the body frame between time t1 and t2 (I + B)
            //A(t1) = [  1  y  z ]  for small angles (<180 degrees). x, y and z are rotations about the axes
            //        [ -y  1  x ]
            //        [ -z -x  1 ]

            Matrix3x3 A = new Matrix3x3();
            A.matrix[0, 0] = 1;             A.matrix[1, 0] = rotations.Y;   A.matrix[2, 0] = rotations.Z;
            A.matrix[0, 1] = -rotations.Y;  A.matrix[1, 1] = 1;             A.matrix[2, 1] = rotations.X;
            A.matrix[0, 2] = -rotations.Z;  A.matrix[1, 2] = -rotations.X;  A.matrix[2, 2] = 1;

            //Normalized to keep the vectors unit length
            Matrix3x3 newRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(rotMatrix, A));

            return newRotMatrix;
        }
        //computes a rotation matrix based on a previous rotation matrix and a series of angle rotations
        //better algorithm then nextRotMatrix - still need to keep rotation < 180 degrees
        //This uses the rectangular rule
        public static Matrix3x3 nextRotMatrix2(Matrix3x3 rotMatrix, Vector3 rotations)
        {
            //This uses C2 = C1( I + (sin(w)/w)B + ((1 - cos(w))/w)B^2 )
            //where w is the total rotation, I is the identity matrix and B is the scew symmetric form of the rotation vector

            Matrix3x3 I = new Matrix3x3();
            I.matrix[0, 0] = 1; I.matrix[1, 0] = 0; I.matrix[2, 0] = 0;
            I.matrix[0, 1] = 0; I.matrix[1, 1] = 1; I.matrix[2, 1] = 0;
            I.matrix[0, 2] = 0; I.matrix[1, 2] = 0; I.matrix[2, 2] = 1;

            Matrix3x3 B = new Matrix3x3();
            B.matrix[0, 0] = 0;             B.matrix[1, 0] = -rotations.Z;  B.matrix[2, 0] = rotations.Y;
            B.matrix[0, 1] = rotations.Z;   B.matrix[1, 1] = 0;             B.matrix[2, 1] = -rotations.X;
            B.matrix[0, 2] = -rotations.Y;  B.matrix[1, 2] = rotations.X;   B.matrix[2, 2] = 0;

            double totalRotation = Vector3.Length(rotations);

            Matrix3x3 smallRot;
            //Don't divide by 0
            if (totalRotation > 0)
            {
                smallRot = Matrix3x3.Add(Matrix3x3.Add(
                    I,
                    Matrix3x3.Multiply(Math.Sin(totalRotation) / totalRotation, B)),
                    Matrix3x3.Multiply((1 - Math.Cos(totalRotation)) / (totalRotation * totalRotation), Matrix3x3.Multiply(B, B))
                );
            }
            else
                smallRot = I;

            Matrix3x3 newRotMatrix = Matrix3x3.Multiply(rotMatrix, smallRot);

            //If these are off, it's because of slight errors - these are no longer Rotation matrices, strictly speaking
            //The determinant should be 1
            //double det = Matrix3x3.Determinant(newRotMatrix)
            //This should give an Identity matrix
            //Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(newRotMatrix), newRotMatrix);

            //Normalize to the the vectors Unit length
            //return newRotMatrix;
            return Matrix3x3.Normalize(newRotMatrix);

            //TODO: We should really be doing an orthonormalization
        }
        public static Matrix3x3 getRotationMatrix(Vector3 from1, Vector3 to1)
        {
            Vector3 from = Vector3.Normalize(from1);
            Vector3 to = Vector3.Normalize(to1);

            Vector3 vs = Vector3.CrossProduct(from, to); // axis multiplied by sin

            Vector3 v = Vector3.Normalize(vs); // axis of rotation
            double c = Vector3.DotProduct(from, to); // cos angle

            Vector3 vc = Vector3.Multiply(1.0 - c, v); //axis multiplied by (1-cos angle)

            Vector3 vp = new Vector3(vc.X *= v.Y, vc.Z *= v.X, vc.Y *= v.Z); //some cross multiplies

            Matrix3x3 rotM = new Matrix3x3();
            //----------------------------------------------------------------------------------------------------------
            rotM.matrix[0, 0] = vc.X * v.X + c; rotM.matrix[1, 0] = vp.X - vs.Z;    rotM.matrix[2, 0] = vp.Y + vs.Y;
            rotM.matrix[0, 1] = vp.X + vs.Z;    rotM.matrix[1, 1] = vc.Y * v.Y + c; rotM.matrix[2, 1] = vp.Z - vs.X;
            rotM.matrix[0, 2] = vp.Y - vs.Y;    rotM.matrix[1, 2] = vp.Z + vs.X;    rotM.matrix[2, 2] = vc.Z * v.Z + c;
            //----------------------------------------------------------------------------------------------------------

            //return rotM;
            return Matrix3x3.Normalize(rotM);
        }
Example #10
0
        private void finishZeroing()
        {
            //align body rotation matrix with reference frame
            Matrix3x3 rotMatrix = new Matrix3x3();

            if (initialRotWithGravity.Checked)
            {
                //base the initial rotation matrix on the gravity measurement - keep the y axis (up-down) rotated so the cord is facing out
                //Calculate the angles and make sure they are -1 <= x <= 1

                //get a normalized version of the gravity vector to find angles
                gravityTemp = Vector3.Normalize(gravityRef);

                double xAngle = Math.Asin(-gravityTemp.X);
                double zAngle = Math.Asin(gravityTemp.Z);

                //The board is up-side down
                if (gravityRef.Y > 0)
                {
                    xAngle = -xAngle;
                    zAngle = -zAngle;
                }

                Matrix3x3 xRotMatrix = new Matrix3x3();
                xRotMatrix.matrix[0, 0] = Math.Cos(xAngle); xRotMatrix.matrix[1, 0] = -Math.Sin(xAngle); xRotMatrix.matrix[2, 0] = 0;
                xRotMatrix.matrix[0, 1] = Math.Sin(xAngle); xRotMatrix.matrix[1, 1] = Math.Cos(xAngle); xRotMatrix.matrix[2, 1] = 0;
                xRotMatrix.matrix[0, 2] = 0; xRotMatrix.matrix[1, 2] = 0; xRotMatrix.matrix[2, 2] = 1;

                //no rotation
                Matrix3x3 yRotMatrix = new Matrix3x3();
                yRotMatrix.matrix[0, 0] = 1; yRotMatrix.matrix[1, 0] = 0; yRotMatrix.matrix[2, 0] = 0;
                yRotMatrix.matrix[0, 1] = 0; yRotMatrix.matrix[1, 1] = 1; yRotMatrix.matrix[2, 1] = 0;
                yRotMatrix.matrix[0, 2] = 0; yRotMatrix.matrix[1, 2] = 0; yRotMatrix.matrix[2, 2] = 1;

                Matrix3x3 zRotMatrix = new Matrix3x3();
                zRotMatrix.matrix[0, 0] = 1; zRotMatrix.matrix[1, 0] = 0; zRotMatrix.matrix[2, 0] = 0;
                zRotMatrix.matrix[0, 1] = 0; zRotMatrix.matrix[1, 1] = Math.Cos(zAngle); zRotMatrix.matrix[2, 1] = -Math.Sin(zAngle);
                zRotMatrix.matrix[0, 2] = 0; zRotMatrix.matrix[1, 2] = Math.Sin(zAngle); zRotMatrix.matrix[2, 2] = Math.Cos(zAngle);

                rotMatrix = Matrix3x3.Multiply(Matrix3x3.Multiply(xRotMatrix, yRotMatrix), zRotMatrix);

                //The board is up-side down
                if (gravityRef.Y < 0)
                {
                    rotMatrix = Matrix3x3.Multiply(-1, rotMatrix);
                }

                //now rotate gravity into reference frame
                gravityRef = Matrix3x3.Multiply(gravityRef, rotMatrix);

                magRef = Matrix3x3.Multiply(magRef, rotMatrix);
            }
            //Assume initial rotation is flat
            else
            {
                rotMatrix.matrix[0, 0] = 1; rotMatrix.matrix[1, 0] = 0; rotMatrix.matrix[2, 0] = 0;
                rotMatrix.matrix[0, 1] = 0; rotMatrix.matrix[1, 1] = 1; rotMatrix.matrix[2, 1] = 0;
                rotMatrix.matrix[0, 2] = 0; rotMatrix.matrix[1, 2] = 0; rotMatrix.matrix[2, 2] = 1;
            }

            timer = DateTime.Now;
            milliseconds = 0;
            milliseconds2 = 0;

            p.rotMatrix = rotMatrix;

            xGravTxt.Text = gravityRef.X.ToString("F4");
            yGravTxt.Text = gravityRef.Y.ToString("F4");
            zGravTxt.Text = gravityRef.Z.ToString("F4");
            totGravTxt.Text = Vector3.Length(gravityRef).ToString("F4");

            Math3D.RotatePoints(p.rotMatrix, p.vertexBuffer, p.originalVertices);
            zeroStatusTxt.Text = "Done.";
        }
 //rotates points from one frame of reference to another using a rotation matrix
 public static void RotatePoints(Matrix3x3 rotMatrix, Vector3[] bodyFramePoints, Vector3[] referenceFramePoints)
 {
     for (int i = 0; i < referenceFramePoints.Length; i++)
         bodyFramePoints[i] = Matrix3x3.Multiply(referenceFramePoints[i], rotMatrix);
 }
 //this normalizes the column vectors in a rotation matrix to unit length
 public static Matrix3x3 Normalize(Matrix3x3 A)
 {
     Matrix3x3 ret = new Matrix3x3();
     for (int i = 0; i < 3; i++)
     {
         double length = Math.Sqrt(A.matrix[i, 0] * A.matrix[i, 0] + A.matrix[i, 1] * A.matrix[i, 1] + A.matrix[i, 2] * A.matrix[i, 2]);
         ret.matrix[i, 0] = A.matrix[i, 0] / length;
         ret.matrix[i, 1] = A.matrix[i, 1] / length;
         ret.matrix[i, 2] = A.matrix[i, 2] / length;
     }
     return ret;
 }
 public static Matrix3x3 Transpose(Matrix3x3 A)
 {
     Matrix3x3 ret = new Matrix3x3();
     for (int i = 0; i < 3; i++)
     {
         for (int j = 0; j < 3; j++)
         {
             ret.matrix[i, j] = A.matrix[j, i];
         }
     }
     return ret;
 }
        public void fillVertices(String objFile)
        {
            ObjLoader l = new ObjLoader();

            vertexBuffer = l.loadVertices(objFile);
            indexBuffer = l.loadIndices(objFile);

            originalVertices = (Vector3[]) vertexBuffer.Clone();

            lock (innerRotMatrixLock)
            {
                innerRotMatrix = new Matrix3x3();
                innerRotMatrix.matrix[0, 0] = 1; innerRotMatrix.matrix[1, 0] = 0; innerRotMatrix.matrix[2, 0] = 0;
                innerRotMatrix.matrix[0, 1] = 0; innerRotMatrix.matrix[1, 1] = 1; innerRotMatrix.matrix[2, 1] = 0;
                innerRotMatrix.matrix[0, 2] = 0; innerRotMatrix.matrix[1, 2] = 0; innerRotMatrix.matrix[2, 2] = 1;
            }
        }