//I got this method for rotating around an arbitary axis from the
//GLspec15.pdf. (page number 42 in the image, page 55 of the pdf file)
//IMPORTANT!!! The angle 'radians' is in the direction of the
//"finger curl of the right hand rule" around the vector {x,y,z}
//------------------------------------------------------------------------------------------
public void RotateAnyAxis(float radians, IVector3H1 axisUnitVector) //unitize Axis first!!!!!
{
//but remember the cross product of 2 unit vectors is only a unit vector if the original 2 vectors are @ 90degrees .....
///................... should I go ahead & always run the unit calculation here for safety?
///maybe not b/c in FlightSimulator I pretty much always unit-ize the vectors before haand
//double myAxis[3] = {Axis.vect[0], Axis.vect[1], Axis.vect[2]};
//int mySize = 3;
//UnitVOfArray(myAxis, mySize);
Matrix3X3 S = new Matrix3X3();
S.Data[0, 0] = 0;
S.Data[0, 1] = -axisUnitVector.Z;
S.Data[0, 2] = axisUnitVector.Y;
S.Data[1, 0] = axisUnitVector.Z;
S.Data[1, 1] = 0;
S.Data[1, 2] = -axisUnitVector.X;
S.Data[2, 0] = -axisUnitVector.Y;
S.Data[2, 1] = axisUnitVector.X;
S.Data[2, 2] = 0;
Matrix3X3 outter = (Matrix3X3)Vector3H1.OutterProduct(axisUnitVector, axisUnitVector);
Matrix3X3 Id = new Matrix3X3();
Id.SetIdentity();
Matrix3X3 Result = outter + ((float)Math.Cos(radians)) * (Id - outter) + ((float)Math.Sin(radians)) * S;
//Manually copy for maximum speed
this.Data[0, 0] = Result.Data[0, 0]; this.Data[0, 1] = Result.Data[0, 1]; this.Data[0, 2] = Result.Data[0, 2];
this.Data[1, 0] = Result.Data[1, 0]; this.Data[1, 1] = Result.Data[1, 1]; this.Data[1, 2] = Result.Data[1, 2];
this.Data[2, 0] = Result.Data[2, 0]; this.Data[2, 1] = Result.Data[2, 1]; this.Data[2, 2] = Result.Data[2, 2];
}
public void SetTranslate(IVector3H1 V)
{
Data[0, 0] = 1; Data[0, 1] = 0; Data[0, 2] = 0; Data[0, 3] = V.X;
Data[1, 0] = 0; Data[1, 1] = 1; Data[1, 2] = 0; Data[1, 3] = V.Y;
Data[2, 0] = 0; Data[2, 1] = 0; Data[2, 2] = 1; Data[2, 3] = V.Z;
Data[3, 0] = 0; Data[3, 1] = 0; Data[3, 2] = 0; Data[3, 3] = 1;
}
//"Spin" = rotate around a relative center of motion :o)
//----------------------------------------------------------------
public void Spin(float radians, IVector3H1 axisUnitVector, IVector3H1 RelCenter)
{
Matrix3H4 Trans1 = new Matrix3H4();
Trans1.SetTranslate(-(Vector3H1)RelCenter); //translate relative center to oragine
Matrix3H4 RotateMat = new Matrix3H4();
RotateMat.RotateAnyAxis(radians, axisUnitVector); //Do 3D rotation
Matrix3H4 Trans2 = new Matrix3H4();
Trans2.SetTranslate(RelCenter); // put it back
IMatrix3H4 Answer = Trans2 * RotateMat * Trans1; //multiply out answer
this.Copy(Answer);
}
public static IMatrix3D OutterProduct(IVector3H1 A, IVector3H1 B)
{
Matrix3X3 NewMatrix = new Matrix3X3();
NewMatrix.Data[0, 0] = A.X * B.X;
NewMatrix.Data[0, 1] = A.X * B.Y;
NewMatrix.Data[0, 2] = A.X * B.Z;
NewMatrix.Data[1, 0] = A.Y * B.X;
NewMatrix.Data[1, 1] = A.Y * B.Y;
NewMatrix.Data[1, 2] = A.Y * B.Z;
NewMatrix.Data[2, 0] = A.Z * B.X;
NewMatrix.Data[2, 1] = A.Z * B.Y;
NewMatrix.Data[2, 2] = A.Z * B.Z;
return(NewMatrix);
}
//"Spin" = rotate around a relative center of motion :o)
//----------------------------------------------------------------
public void Spin(float radians, IVector3H1 axisUnitVector, IVector3H1 RelCenter)
{
Matrix4X4 Trans1 = new Matrix4X4();
//translate relative center to oragine
Trans1.SetTranslate(-(Vector3H1)RelCenter); //have to cast to object not interface type to apply operator
Matrix4X4 RotateMat = new Matrix4X4();
RotateMat.RotateAnyAxis(radians, axisUnitVector); //Do 3D rotation
Matrix4X4 Trans2 = new Matrix4X4();
Trans2.SetTranslate(RelCenter); // put it back
IMatrix4D Answer = Trans2 * RotateMat * Trans1; //multiply out answer
this.Copy(Answer);
}
//I got this method for rotating around an arbitary axis from the
//GLspec15.pdf. (page number 42 in the image, page 55 of the pdf file)
//IMPORTANT!!! The angle 'radians' is in the direction of the
//"finger curl of the right hand rule" around the vector {x,y,z}
//------------------------------------------------------------------------------------------
public void RotateAnyAxis(float radians, IVector3H1 axisUnitVector) //unitize Axis first!!!!!
{
//but remember the cross product of 2 unit vectors is only a unit vector if the original 2 vectors are @ 90degrees .....
///................... should I go ahead & always run the unit calculation here for safety?
///maybe not b/c in FlightSimulator I pretty much always unit-ize the vectors before haand
//double myAxis[3] = {Axis.vect[0], Axis.vect[1], Axis.vect[2]};
//int mySize = 3;
//UnitVOfArray(myAxis, mySize);
Matrix3X3 S = new Matrix3X3();
S.Data[0, 0] = 0;
S.Data[0, 1] = -axisUnitVector.Z;
S.Data[0, 2] = axisUnitVector.Y;
S.Data[1, 0] = axisUnitVector.Z;
S.Data[1, 1] = 0;
S.Data[1, 2] = -axisUnitVector.X;
S.Data[2, 0] = -axisUnitVector.Y;
S.Data[2, 1] = axisUnitVector.X;
S.Data[2, 2] = 0;
Matrix3X3 outter = (Matrix3X3)Vector3H1.OutterProduct(axisUnitVector, axisUnitVector);
Matrix3X3 Id = new Matrix3X3();
Id.SetIdentity();
//I'm not sure what to do about this error - may be necessary to have different interfaces for Matrix3H4 & Matrix4X4.....
IMatrix3D Result = outter + ((float)Math.Cos(radians)) * (Id - outter) + ((float)Math.Sin(radians)) * S;
//Manually copy for maximum speed
this.Data[0, 0] = Result.Data[0, 0]; this.Data[0, 1] = Result.Data[0, 1]; this.Data[0, 2] = Result.Data[0, 2]; this.Data[0, 3] = 0;
this.Data[1, 0] = Result.Data[1, 0]; this.Data[1, 1] = Result.Data[1, 1]; this.Data[1, 2] = Result.Data[1, 2]; this.Data[1, 3] = 0;
this.Data[2, 0] = Result.Data[2, 0]; this.Data[2, 1] = Result.Data[2, 1]; this.Data[2, 2] = Result.Data[2, 2]; this.Data[2, 3] = 0;
//this.Copy(Result); //replaces current value of calling matrix //forces bottom row & last col to be all 0s
}
//"stretch" = scale around a relative (quasi-center) point
//basically you move to the origin, do a normal scale, then move the object back
public void Stretch(float SX, float SY, float SZ, IVector3H1 RelCenter) //probably cannot be done in 3x3... 3H4???
{
//translate relative center to oragine
Matrix3H4 Trans1 = new Matrix3H4();
Trans1.SetTranslate(-RelCenter.X, -RelCenter.Y, -RelCenter.Z);
//do the scaling
Matrix3H4 ScaleMat = new Matrix3H4();
ScaleMat.SetScale(SX, SY, SZ);
// put it back
Matrix3H4 Trans2 = new Matrix3H4();
Trans2.SetTranslate(RelCenter);
IMatrix3H4 Answer = Trans2 * ScaleMat * Trans1;
this.Copy(Answer);
}
/// <summary>
/// QuickSolve3D
/// Solves matrix equation [A] * x = N for x based on Cramer's Rule
/// where A is a Matrix, N is a vector (usually a normal)
/// -----------------------------------------------------------------
/// Marix A is typed as IMatrix so that Matrix3X3, Matrix3H4 or Matrix4X4 can be used
/// extra data is ignored, but Matrix2X2 would cause an error (but it doesn't exist yet)
/// </summary>
/// <param name="A">IMatrix</param>
/// <param name="N">IVector3H1</param>
/// <param name="bUnitize">bool</param>
/// <returns></returns>
public static Vector3H1 QuickSolve3D(IMatrix A, IVector3H1 N, bool bUnitize = true)
{
float determinate = Determinate3D(A);
//if the determinate comes out to zero, it's probably b/c this is actually a small
if (determinate == 0) //2X2 matrix in a larger object w/ the rest zeros, so just
{
determinate = Determinate2D(A); //go ahead & take the 2X2 determinate
A.Data[2, 2] = 1; //but you aldo have to set the Z-identity
} //this works, but it it good to change the matrix????????
Vector3H1 V = new Vector3H1();
V.X = ((A.Data[2, 2] * A.Data[1, 1] - A.Data[1, 2] * A.Data[2, 1]) * N.X
+ (A.Data[2, 1] * A.Data[0, 2] - A.Data[0, 1] * A.Data[2, 2]) * N.Y
+ (A.Data[0, 1] * A.Data[1, 2] - A.Data[1, 1] * A.Data[0, 2]) * N.Z)
/ determinate;
V.Y = ((A.Data[1, 2] * A.Data[2, 0] - A.Data[2, 2] * A.Data[1, 0]) * N.X
+ (A.Data[0, 0] * A.Data[2, 2] - A.Data[2, 0] * A.Data[0, 2]) * N.Y
+ (A.Data[1, 0] * A.Data[0, 2] - A.Data[0, 0] * A.Data[1, 2]) * N.Z)
/ determinate;
V.Z = ((A.Data[2, 1] * A.Data[1, 0] - A.Data[1, 1] * A.Data[2, 0]) * N.X
+ (A.Data[0, 1] * A.Data[2, 0] - A.Data[0, 0] * A.Data[2, 1]) * N.Y
+ (A.Data[0, 0] * A.Data[1, 1] - A.Data[0, 1] * A.Data[1, 0]) * N.Z)
/ determinate;
if (bUnitize)
{
V.SetUnitVector();
}
return(V);
}
