// ### must be optimized... consider using the 3x3 version
Matrix4x4& Matrix4x4::Rot(float angle, Point& p1, Point& p2)
{
Point Axis = (p2 - p1).Normalize();
Matrix4x4 T, InvT;
T.Identity();
T.SetTrans(-p1.x, -p1.y, -p1.z);
InvT = T;
InvT.Invert();
Matrix4x4 Rx, InvRx;
Rx.Identity();
float d = _sqrt(Axis.y*Axis.y + Axis.z*Axis.z);
if(d!=0.0f)
{
float CosAngle = Axis.z / d;
float SinAngle = Axis.y / d;
Rx.SetRow(1, Point(0.0f, CosAngle, SinAngle));
Rx.SetRow(2, Point(0.0f, -SinAngle, CosAngle));
}
InvRx = Rx;
InvRx.Invert();
Matrix4x4 Ry, InvRy;
Ry.Identity();
Ry.SetRow(0, Point(d, 0.0f, Axis.x));
Ry.SetRow(2, Point(-Axis.x, 0.0f, d));
InvRy = Ry;
InvRy.Invert();
Matrix4x4 Rz;
Rz.RotZ(angle);
Matrix4x4 Combo = T * Rx * Ry * Rz * InvRy * InvRx * InvT;
*this = Combo;
return *this;
}
