diff --git a/util/Transform.cc b/util/Transform.cc index 115a3e2..ec1f8ff 100644 --- a/util/Transform.cc +++ b/util/Transform.cc @@ -1,5 +1,6 @@ #include "Transform.h" +#include Transform::Transform() { @@ -15,9 +16,33 @@ void Transform::translate(double x, double y, double z) m_matrix *= t; } -void Transform::rotate(double amt, double xv, double yv, double zv) +void Transform::rotate(double angle, double xv, double yv, double zv) { - /* TODO: fill in */ + /* formula from http://en.wikipedia.org/wiki/Rotation_matrix */ + Vector l(xv, yv, zv); + l.normalize(); + + double c = cos(angle); + double s = sin(angle); + + double lx2 = l[0] * l[0]; + double ly2 = l[1] * l[1]; + double lz2 = l[2] * l[2]; + + Matrix t = Matrix::identity(); + t[0][0] = lx2 + (1 - lx2) * c; + t[0][1] = l[0] * l[1] * (1 - c) - l[2] * s; + t[0][2] = l[0] * l[2] * (1 - c) + l[1] * s; + + t[1][0] = l[0] * l[1] * (1 - c) + l[2] * s; + t[1][1] = ly2 + (1 - ly2) * c; + t[1][2] = l[1] * l[2] * (1 - c) - l[0] * s; + + t[2][0] = l[0] * l[2] * (1 - c) - l[1] * s; + t[2][1] = l[1] * l[2] * (1 - c) + l[0] * s; + t[2][2] = lz2 + (1 - lz2) * c; + + m_matrix *= t; } void Transform::scale(double xs, double ys, double zs) diff --git a/util/Transform.h b/util/Transform.h index ca3d75f..38a6242 100644 --- a/util/Transform.h +++ b/util/Transform.h @@ -10,7 +10,7 @@ class Transform public: Transform(); void translate(double x, double y, double z); - void rotate(double amt, double xv, double yv, double zv); + void rotate(double angle, double xv, double yv, double zv); void scale(double xs, double ys, double zs); Matrix & getMatrix() { return m_matrix; } diff --git a/util/Vector.cc b/util/Vector.cc index b239769..2208d11 100644 --- a/util/Vector.cc +++ b/util/Vector.cc @@ -10,6 +10,13 @@ Vector::Vector() m_array[2] = 0.0; } +Vector::Vector(double x, double y, double z) +{ + m_array[0] = x; + m_array[1] = y; + m_array[2] = z; +} + void Vector::normalize() { double length = sqrt(m_array[0] * m_array[0] diff --git a/util/Vector.h b/util/Vector.h index 084a1a7..0704d93 100644 --- a/util/Vector.h +++ b/util/Vector.h @@ -8,6 +8,7 @@ class Vector { public: Vector(); + Vector(double x, double y, double z); double & operator[](int idx) { return m_array[idx]; } double operator[](int idx) const { return m_array[idx]; } void normalize();