Matrix Rotation Around an Arbitrary Axis Bug - graphics

Question

I have been attempting to get matrix rotation around an arbitrary axis working, I think I'm close but I have a bug. I am relatively new to 3D rotations and have a basic understanding of what is going on.
public static Matrix4D Rotate(Vector3D u, Vector3D v)
{
double angle = Acos(u.Dot(v));
Vector3D axis = u.Cross(v);
double c = Cos(angle);
double s = Sin(angle);
double t = 1 - c;
return new Matrix4D(new double [,]
{
{ c + Pow(axis.X, 2) * t, axis.X * axis.Y * t -axis.Z * s, axis.X * axis.Z * t + axis.Y * s, 0 },
{ axis.Y * axis.X * t + axis.Z * s, c + Pow(axis.Y, 2) * t, axis.Y * axis.Z * t - axis.X * s, 0 },
{ axis.Z * axis.X * t - axis.Y * s, axis.Z * axis.Y * t + axis.X * s, c + Pow(axis.Z, 2) * t, 0 },
{ 0, 0, 0, 1 }
});
}
The above code is the algorithm for the matrix rotation. When I test the algorithm with unit vectors I get the following:
Matrix4D rotationMatrix = Matrix4D.Rotate(new Vector3D(1, 0, 0), new Vector3D(0, 0, 1));
Vector4D vectorToRotate = new Vector4D(1,0,0,0);
Vector4D result = rotationMatrix * vectorToRotate;
//Result
X = 0.0000000000000000612;
Y = 0;
Z = 1;
Length = 1;
With a 90 degree rotation I find it works almost perfectly. Now lets look at a 45 degree rotation:
Matrix4D rotationMatrix = Matrix4D.Rotate(new Vector3D(1, 0, 0), new Vector3D(1, 0, 1).Normalize());
Vector4D vectorToRotate = new Vector4D(1,0,0,0);
Vector4D result = rotationMatrix * vectorToRotate;
//Result
X = .70710678118654746;
Y = 0;
Z = .5;
Length = 0.8660254037844386;
When we take the atan(.5/.707) we find that we have a 35.28 degree rotation instead of a 45 degree rotation. Also the length of the vector is changing from 1 to .866. Does anyone have any tips on what I am doing wrong?

Answer 1

Your matrix code looks correct, but you need to normalize the axis too (just because u and v are normalized doesn't mean u cross v also is).
(I would also recommend using a simple multiply instead of Pow, for performance and accuracy reasons; but this is minor and not the source of your problem)

Answer 2

Just for posterity, this is the code that I use for this exact thing. I alway recommend using Atan2(dy,dx) instead of Acos(dx) because it is more stable numerically at angles near 90°.
public static class Rotations
{
public static Matrix4x4 FromTwoVectors(Vector3 u, Vector3 v)
{
Vector3 n = Vector3.Cross(u, v);
double sin = n.Length(); // use |u×v| = |u||v| SIN(θ)
double cos = Vector3.Dot(u, v); // use u·v = |u||v| COS(θ)
// what if u×v=0, or u·v=0. The full quadrant arctan below is fine with it.
double angle = Atan2(sin, cos);
n = Vector3.Normalize(n);
return FromAxisAngle(n, angle);
}
public static Matrix4x4 FromAxisAngle(Vector3 n, double angle)
{
// Asssume `n` is normalized
double x = n.X, y = n.Y, z = n.Z;
double sin = Sin(angle);
double vcos = 1.0-Cos(angle);
return new Matrix4x4(
1.0-vcos*(y*y+z*z), vcos*x*y-sin*z, vcos*x*z+sin*y, 0,
vcos*x*y+sin*z, 1.0-vcos*(x*x+z*z), vcos*y*z-sin*x, 0,
vcos*x*z-sin*y, vcos*y*z+sin*x, 1.0-vcos*(x*x+y*y), 0,
0, 0, 0, 1.0);
}
}
Code is C#