public static Matrix4D CreateFromAxisAngle(Vector3 axis, float angle)
{
axis.Normalise();
float axisX = axis.X;
float axisY = axis.Y;
float axisZ = axis.Z;
float cos = (float)Math.Cos(-angle);
float sin = (float)Math.Sin(-angle);
float t = 1.0f - cos;
float tXX = t * axisX * axisX;
float tXY = t * axisX * axisY;
float tXZ = t * axisX * axisZ;
float tYY = t * axisY * axisY;
float tYZ = t * axisY * axisZ;
float tZZ = t * axisZ * axisZ;
float sinX = sin * axisX;
float sinY = sin * axisY;
float sinZ = sin * axisZ;
return(new Matrix4D(
tXX + cos, tXY - sinZ, tXZ + sinY, 0,
tXY + sinZ, tYY + cos, tYZ - sinX, 0,
tXZ - sinY, tYZ + sinX, tZZ + cos, 0,
0, 0, 0, 1));
}
public static Quaternion FromAxisAngle(Vector3 axis, float angle)
{
if (axis.LengthSquared == 0.0f)
{
return(Identity);
}
Quaternion q = Identity;
angle *= 0.5f;
axis.Normalise();
axis *= (float)Math.Sin(angle);
q.X = axis.X;
q.Y = axis.Y;
q.Z = axis.Z;
q.W = (float)Math.Cos(angle);
q.Normalise();
return(q);
}