public void SetPosAndAngle(float X, float Y, float Z, float A, float B, float C)
{
TMat4x4 TransMat = TMat4x4.ZeroMat();
TMat4x4 RotMatX = TMat4x4.ZeroMat();
TMat4x4 RotMatY = TMat4x4.ZeroMat();
TMat4x4 RotMatZ = TMat4x4.ZeroMat();
CurrentPos.Set(X, Y, Z);
CurrentRot.Set(A, B, C);
TransMat.SetTrans(X, Y, Z);
RotMatX.SetRotateX(A);
RotMatY.SetRotateY(B);
RotMatZ.SetRotateZ(C);
SetMatrix(RotMatX.MulMat(RotMatY).MulMat(RotMatZ).MulMat(TransMat));
}
// Set rotation matrix around arbitrary axis
public void SetArbitraryRot(TVertex P1, TVertex P2, float Angle)
{
TMat4x4 MTra = ZeroMat(), MRo_X = ZeroMat(), MRo_Y = ZeroMat(), MRo_Z = ZeroMat(), TempM = ZeroMat();
float D;
// Find the direction cosines of the arbitrary axis P1-->P2 .
// The direction cosines will be in C
TVertex C = Math3D.CalcNormalizedVec(P1, P2);
D = (float)Math.Sqrt(Math3D.Sqr(C.Y) + Math3D.Sqr(C.Z));
// Special case for the X axis
if (D == 0)
{
MTra.SetTrans(-P1.X, -P1.Y, -P1.Z);
MRo_X.SetRotateX(Angle);
TempM = MTra.MulMat(MRo_X);
MTra.SetTrans(P1.X, P1.Y, P1.Z);
this = TempM.MulMat(MTra);
}
else
{
MTra.SetTrans(-P1.X, -P1.Y, -P1.Z);
// Prepare matrix rotation about axis X with angle Alfa Cos(Alfa) = C.z / D Sin(Alfa) = C.y / D }
MRo_X.SetUnit();
MRo_X.m11 = C.Z / D;
MRo_X.m22 = MRo_X.m11;
MRo_X.m12 = C.Y / D;
MRo_X.m21 = -MRo_X.m12;
// prepare matrix rotation about axis Y with angle Beta Cos(Beta) = D Sin(Beta) = -C.x
MRo_Y.SetUnit();
MRo_Y.m00 = D;
MRo_Y.m22 = MRo_Y.m00;
MRo_Y.m02 = C.X;
MRo_Y.m20 = -MRo_Y.m02;
TMat4x4 M;
// M= Trans * Rot about axis X * Rot about axis Y
TempM = MTra.MulMat(MRo_X);
M = TempM.MulMat(MRo_Y);
// prepare matrix rotation about axis Z with angle Angle
MRo_Z.SetRotateZ(Angle);
// TempM= Trans * Rot axis X * Rot axis Y * Rot about axis Z by angle Angle
TempM = M.MulMat(MRo_Z);
// Find inverse Y matrix
MRo_Y.m00 = D;
MRo_Y.m22 = D;
MRo_Y.m02 = -C.X;
MRo_Y.m20 = C.X;
M = TempM.MulMat(MRo_Y);
// Find inverse x matrix
MRo_X.m11 = C.Z / D;
MRo_X.m22 = MRo_X.m11;
MRo_X.m21 = C.Y / D;
MRo_X.m12 = -MRo_X.m21;
TempM = M.MulMat(MRo_X);
// Find inverse translation matrix
MTra.SetTrans(P1.X, P1.Y, P1.Z);
this = TempM.MulMat(MTra);
}
}