///////////////////////////////////////////////////////////////////////////////
// Ditto above, but for doubles
void m3dRotationMatrix33(M3DMatrix33d m, double angle, double x, double y, double z)
{
double mag, s, c;
double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
s = sin(angle);
c = cos(angle);
mag = sqrt( x*x + y*y + z*z );
// Identity matrix
if (mag == 0.0) {
m3dLoadIdentity33(m);
return;
}
// Rotation matrix is normalized
x /= mag;
y /= mag;
z /= mag;
#define M(row,col) m[col*3+row]
xx = x * x;
yy = y * y;
zz = z * z;
xy = x * y;
yz = y * z;
zx = z * x;
xs = x * s;
ys = y * s;
zs = z * s;
one_c = 1.0 - c;
M(0,0) = (one_c * xx) + c;
M(0,1) = (one_c * xy) - zs;
M(0,2) = (one_c * zx) + ys;
M(1,0) = (one_c * xy) + zs;
M(1,1) = (one_c * yy) + c;
M(1,2) = (one_c * yz) - xs;
M(2,0) = (one_c * zx) - ys;
M(2,1) = (one_c * yz) + xs;
M(2,2) = (one_c * zz) + c;
#undef M
}
///////////////////////////////////////////////////////////////////////////////
// Creates a 3x3 rotation matrix, takes radians NOT degrees
void m3dRotationMatrix33(M3DMatrix33f m, float angle, float x, float y, float z)
{
float mag, s, c;
float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
s = float(sin(angle));
c = float(cos(angle));
mag = float(sqrt( x*x + y*y + z*z ));
// Identity matrix
if (mag == 0.0f) {
m3dLoadIdentity33(m);
return;
}
// Rotation matrix is normalized
x /= mag;
y /= mag;
z /= mag;
xx = x * x;
yy = y * y;
zz = z * z;
xy = x * y;
yz = y * z;
zx = z * x;
xs = x * s;
ys = y * s;
zs = z * s;
one_c = 1.0f - c;
M33(0,0) = (one_c * xx) + c;
M33(0,1) = (one_c * xy) - zs;
M33(0,2) = (one_c * zx) + ys;
M33(1,0) = (one_c * xy) + zs;
M33(1,1) = (one_c * yy) + c;
M33(1,2) = (one_c * yz) - xs;
M33(2,0) = (one_c * zx) - ys;
M33(2,1) = (one_c * yz) + xs;
M33(2,2) = (one_c * zz) + c;
}
