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);
}
//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;
}
private void resetGraph()
{
lock (p)
{
Matrix3x3 rotMatrix = new Matrix3x3();
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;
p.rotMatrix = rotMatrix;
p.originalVertices.Clear();
p.vertexBuffer.Clear();
p.pointColors.Clear();
//add some initial points
p.addPoint(new Vector3(0, 0, 0), Brushes.Black);
p.addPoint(new Vector3(1, 0, 0), Brushes.Black);
p.addPoint(new Vector3(0, 1, 0), Brushes.Black);
p.addPoint(new Vector3(0, 0, -1), Brushes.Black);
p.addPoint(new Vector3(0, 0, 0), Brushes.Black);
p.addPoint(new Vector3(0, 0, 0), Brushes.Black);
}
}
private void Form1_Load(object sender, EventArgs e)
{
p = new Pipeline();
//add some initial points
p.addPoint(new Vector3(0, 0, 0), Brushes.Black);
p.addPoint(new Vector3(1, 0, 0), Brushes.Black);
p.addPoint(new Vector3(0, 1, 0), Brushes.Black);
p.addPoint(new Vector3(0, 0, -1), Brushes.Black);
p.addPoint(new Vector3(0, 0, 0), Brushes.Black);
p.addPoint(new Vector3(0, 0, 0), Brushes.Black);
p.indexBuffer.AddRange(new int[] { 0, 1 });
p.indexBuffer.AddRange(new int[] { 0, 2 });
p.indexBuffer.AddRange(new int[] { 0, 3 });
p.indexBuffer.AddRange(new int[] { 0, 4 });
p.indexBuffer.AddRange(new int[] { 0, 5 });
p.lineColors.AddRange(new Pen[] { Pens.Black, Pens.Black, Pens.Black, Pens.Red, Pens.Green });
//rotation speed
rots = new Vector3(0, 0.01, 0);
Vector3 initialRots = new Vector3(0, 0, 0);
Matrix3x3 rotMatrix = new Matrix3x3();
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;
rotMatrix = Math3D.nextRotMatrix2(rotMatrix, initialRots);
p.rotMatrix = rotMatrix;
origin = new Point(pictureBox1.Width / 2, pictureBox1.Height / 2);
Math3D.RotatePoints(p.rotMatrix, p.vertexBuffer, p.originalVertices);
pictureBox1.Image = p.Draw(origin, zoom);
trackBar4.Value = zoom;
spatial = new Spatial();
spatial.Attach += new Phidgets.Events.AttachEventHandler(spatial_Attach);
spatial.Detach += new Phidgets.Events.DetachEventHandler(spatial_Detach);
spatial.SpatialData += new Phidgets.Events.SpatialDataEventHandler(spatial_SpatialData);
spatial.open();
}
//rotates points from one frame of reference to another using a rotation matrix
public static void RotatePoints(Matrix3x3 rotMatrix, List<Vector3> bodyFramePoints, List<Vector3> referenceFramePoints)
{
for (int i = 0; i < referenceFramePoints.Count; i++)
bodyFramePoints[i] = Matrix3x3.Multiply(referenceFramePoints[i], rotMatrix);
}
