// Rotate the angle is radian
public override Matrix44Adaptor SetRotate(double AngleThita, UnitVectorAdaptor axis, PointAdaptor basePoint)
{
Debug.Assert(axis != null && basePoint != null);
if (!(axis != null && basePoint != null)) return this;
UnitizeSubmatrixA();
// Translation the origin to base point
NativeMatrix44Adaptor TA = NativeMatrix44Adaptor.Identity;
NativePointAdaptor origin = new NativePointAdaptor(0, 0, 0);
TA.SetTranslation(origin - basePoint);
// Rotate the axis by X'-axis to X'-Z' plane
double CosAlpha = 0;
double SinAlpha = 0;
double v = System.Math.Sqrt(axis.Y * axis.Y
+ axis.Z * axis.Z);
if (MathUtil.IsTwoDoubleEqual(v, 0))
{
// The axis is parallel to X-axis
CosAlpha = 1;
SinAlpha = 0;
}
else
{
CosAlpha = axis.Z / v;
SinAlpha = axis.Y / v;
}
NativeMatrix44Adaptor Rx = NativeMatrix44Adaptor.Identity;
Rx[1, 1] = CosAlpha;
Rx[1, 2] = -SinAlpha;
Rx[2, 1] = SinAlpha;
Rx[2, 2] = CosAlpha;
// Rotate the axis by Y'-axis to Z'-axis
double u = axis.Length();
double CosBeta = v / u;
double SinBeta = -axis.X / u;
NativeMatrix44Adaptor Ry = NativeMatrix44Adaptor.Identity;
Ry[0, 0] = CosBeta;
Ry[0, 2] = SinBeta;
Ry[2, 0] = -SinBeta;
Ry[2, 2] = CosBeta;
// Rotate by Z'
double CosThita = System.Math.Cos(AngleThita);
double SinThita = System.Math.Sin(AngleThita);
NativeMatrix44Adaptor Rz = NativeMatrix44Adaptor.Identity;
Rz[0, 0] = CosThita;
Rz[0, 1] = -SinThita;
Rz[1, 0] = SinThita;
Rz[1, 1] = CosThita;
// Get the inverse matrix of the above step
NativeMatrix44Adaptor Ry_1 = Ry.Inverse as NativeMatrix44Adaptor;
NativeMatrix44Adaptor Rx_1 = Rx.Inverse as NativeMatrix44Adaptor;
NativeMatrix44Adaptor TA_1 = TA.Inverse as NativeMatrix44Adaptor;
// The transform result
Matrix44Adaptor R = TA_1 * Rx_1 * Ry_1 * Rz * Ry * Rx * TA;
// Save the result
for (int row = 0; row < 3; row++)
for (int column = 0; column < 3; column++)
m_Matrix2d[row, column] = R[row, column];
return this;
}