Closed RandyGaul closed 5 years ago
I ended up writing one here. I just double checked your memory layout -- I think it's transposed relative to my ordering.
0 1 2 - x axis
3 4 5 - y axis
6 7 8 - z axis
Where yours is like so.
0 3 6
1 4 7
2 5 8
x y z
| | |
a a a
x x x
i i i
s s s
void CUTE_MATH_CALL axis_angle_from_m3(m3 m, v3* axis, float* angle_radians)
{
const float k_tol = 1.0e-8f;
float c = 0.5f * (trace(m) - 1.0f);
float angle = acosf(c);
*angle_radians = angle;
bool angle_near_zero = fabsf(angle) < k_tol;
bool angle_not_near_pi = angle < CUTE_MATH_PI - k_tol;
if (angle_near_zero) {
// When angle is zero the axis can be anything. X axis is good.
*axis = v3(1, 0, 0);
} else if (angle_not_near_pi) {
// Standard case with no singularity.
v3 n = v3(m[1][2] - m[2][1], m[2][0] - m[0][2], m[0][1] - m[1][0]);
*axis = norm(n);
} else {
// Angle is near 180-degrees.
int i = 0;
if (m[1][1] > m[0][0]) i = 1;
if (m[2][2] > m[i][i]) i = 2;
int j = (i + 1) % 3;
int k = (j + 1) % 3;
float s = sqrtf(m[i][i] - m[j][j] - m[k][k] + 1.0f);
float inv_s = s != 0 ? 1.0f / s : 0;
float v[3];
v[i] = 0.5f * s;
v[j] = m[j][i] * inv_s;
v[k] = m[i][k] * inv_s;
*axis = v3(v[0], v[1], v[2]);
}
}
You can use this as a reference if you ever wanted to get this function in your library. Most of the function involves looking at diagonal elements. So the only pieces that would need transposition are the double bracket indices, where the indices can be flipped.
I learned this from Essential Math by Van Verth. His matrix storage ordering matches yours, so you can just reference his book or source code directly. It might be a better reference.
Does this function exist, to get the axis/angle out of a rotation matrix? I couldn't find it after scanning your header.