It being a template isn't a horrible idea. You can swap between float and double, or even fixed point.
Building up your rotation matrix from components is a good idea. You only have to compute it once per frame/time slice, which is basically never at modern computer speeds.
Next, note that your code is doing a full matrix multiplication anyhow. So just write a matrix class.
- Code: Select all
// does work for us:
template<int n, typename T>
struct Vector_base
{
T values[n];
T& operator[](int i){
Assert(i >= 0);
Assert(i < n);
return values[i];
}
T const& operator[](int i) const{
Assert(i >= 0);
Assert(i < n);
return values[i];
}
Vector_base() {
for(int i = 0; i < n; ++i) {
values[i] = T();
}
}
};
template<int n=3, typename T=double>
struct Vector: public Vector_base<n, T>
{};
template<typename T>
struct Vector<3, T>: public Vector_base<n, T>
{
T& x() { return (*this)[0]; }
T const& x() const { return (*this)[0]; }
T& y() { return (*this)[1]; }
T const& y() const { return (*this)[1]; }
T& z() { return (*this)[2]; }
T const& z() const { return (*this)[2]; }
};
A really stripped down vector class. I'd also implement +=, and maybe use the CRTP (google it) to have the Vector_base return Vector types from operators like +=.
But I'm a bit of a template geek. In practice, Vector3 is what you usually want.
Then we toss together Matrix:
- Code: Select all
template<int n=3, typename T=double>
struct Matrix
{
struct slice {
int i;
Matrix* m;
slice(int i_, Matrix<n, T>* m_):i(i_), m(m_) {}
T& operator[]( int j ) const { Assert(j >=0); Assert(j<n); return (*m).value[i][j]; }
};
struct const_slice {
int i;
Matrix const* m;
const_slice(int i_, Matrix<n, T> const* m_):i(i_), m(m_) {}
T const& operator[]( int j ) const { Assert(j >=0); Assert(j<n); return (*m).value[i][j]; }
};
T values[n][n];
static void zero( T& t ) { t = T(); }
void ForEachElement( std::tr1::function< void(T&) > func ) {
for(int i = 0; i < n; ++i){
for(int j=0; j<n;++j){
func(value[i][j]);
}
}
}
Matrix()
{
this->ForEachElement(Matrix::zero);
}
slice operator[](int i){ Assert(i>=0); Assert(i<n); return slice(i, this); }
const_slice operator[](int i) const { Assert(i>=0); Assert(i<n); return const_slice(i, this); }
};
which, for the hell of it, has bounds-checking (in debug) operator[][] implemented.
Then we write:
- Code: Select all
template<int n, typename T>
Vector<n,T> operator*( Matrix<n, T> const& left, Vector<n, T> const& right )
{
Vector<n, T> retval;
for( int i = 0; i < n; ++i )
{
for( int j = 0; j < n; ++j )
{
retval[i] += left[i][j]*right[j];
}
}
return retval;
}
which gives you Matrix times Vector multiplication, and
- Code: Select all
template<int n, typename T>
Matrix<n, T> operator*( Matrix<n, T> const& left, Matrix<n, T> const& right )
{
Matrix<n, T> retval;
for( int i = 0; i < n; ++i )
{
for( int j = 0; j < n; ++j )
{
for( int k = 0; k < n; ++k )
{
retval[i][j] += left[i][k]*right[k][j];
}
}
}
return retval;
}
Of course, you could have more fun and write Matrixes as Vectors of Vectors, and teach operator* to deal with non-uniform Vector multiplication in such a way that matrix multiplication "falls out". But that is getting silly (and requires a strange default orientation for Matrixes, so probably isn't worth it). (note that any decent compiler will take the above loops and flatten them).
Anyhow, once you have the above, you can just write a few Matrix primitive generators.
Have one "rotate clockwise around z axis". Then a "rotate z axis to y axis" and "rotate z axis to x axis".
From those primitives, you can write "rotate clockwise around x axis" and "rotate clockwise around y axis" using the above matrix operator*.
Now you can generate the arbitrary rotation matrix by simply multiplying the above matrices together.
And why do it this way? Because you can test the pieces without testing the output. You can examine the "rotate around z" matrix, and manually test if it does what you want. The same for the pieces.
Then you can manually test the operator* (which may or may not be right -- the one I wrote above was off the top of my head, it may not even compile).
Once each piece is tested, you can run them all together and have confidence that the result is right.
If you just toss them all together at once, about the only thing you can do is hope that wherever you copied it from was accurate.