140 lines
4.1 KiB
C++
Executable File
140 lines
4.1 KiB
C++
Executable File
|
|
#pragma warning ( disable: 4786 )
|
|
#include <vector>
|
|
#include <cmath>
|
|
|
|
#include "DTSQuaternion.h"
|
|
|
|
namespace DTS
|
|
{
|
|
Quaternion Quaternion::identity (0,0,0,1) ;
|
|
|
|
/*! Create a quaternion from three spherical rotation angles
|
|
(XY rotation angle, latitude and longitude) in radians */
|
|
|
|
Quaternion::Quaternion (float angle, float latitude, float longitude)
|
|
{
|
|
Quaternion x (Point( 1, 0, 0), angle) ;
|
|
Quaternion y (Point( 0, 1, 0), latitude) ;
|
|
Quaternion z (Point( 0, 0, 1), longitude) ;
|
|
|
|
// This order is a little wierd, but we flip the x & y
|
|
// axis when importing from MilkShape.
|
|
(*this) = x * z * y;
|
|
}
|
|
|
|
/*! Calculate the angle in radians between this and another quaternion */
|
|
|
|
float Quaternion::angleBetween (const Quaternion &b) const
|
|
{
|
|
return acos (x()*b.x() + y()*b.y() + z()*b.z() + w()*b.w()) ;
|
|
}
|
|
|
|
/*! Calculate the rotation matrix */
|
|
|
|
Matrix<4,4> Quaternion::toMatrix () const
|
|
{
|
|
float xx = x() * x() ;
|
|
float xy = x() * y(), yy = y() * y() ;
|
|
float xz = x() * z(), yz = y() * z(), zz = z() * z() ;
|
|
float xw = x() * w(), yw = y() * w(), zw = z() * w(), ww = w() * w() ;
|
|
|
|
Matrix<4,4>::Cell data[] = {
|
|
1 - 2 * (yy+zz), 2 * (xy-zw), 2 * (xz+yw), 0,
|
|
2 * (xy+zw), 1 - 2 * (xx+zz), 2 * (yz-xw), 0,
|
|
2 * (xz-yw), 2 * (yz+xw), 1 - 2 * (xx+yy), 0,
|
|
0, 0, 0, 1
|
|
} ;
|
|
return Matrix<4,4>(data) ;
|
|
}
|
|
|
|
/*! Create a quaternion from a rotation matrix */
|
|
|
|
Quaternion::Quaternion (const Matrix<> &m)
|
|
{
|
|
fromMatrix(m) ;
|
|
}
|
|
|
|
void Quaternion::fromAxis (Point axis, float angle)
|
|
{
|
|
axis.normalize() ;
|
|
float s = sin(angle / 2.0f) ;
|
|
x(axis.x() * s) ;
|
|
y(axis.y() * s) ;
|
|
z(axis.z() * s) ;
|
|
w(cos(angle / 2.0f)) ;
|
|
}
|
|
|
|
void Quaternion::fromMatrix (const Matrix<> &m)
|
|
{
|
|
float qw2 = (1+ m[0][0] + m[1][1] + m[2][2]) * 0.25f ;
|
|
float qx2 = qw2 - 0.5f * (m[1][1] + m[2][2]) ;
|
|
float qy2 = qw2 - 0.5f * (m[0][0] + m[2][2]) ;
|
|
float qz2 = qw2 - 0.5f * (m[0][0] + m[1][1]) ;
|
|
float s ;
|
|
|
|
if (qw2 > qx2 && qw2 > qy2 && qw2 > qz2)
|
|
{
|
|
w( sqrtf(qw2) ); s = 0.25f / w() ;
|
|
x( (m[2][1] - m[1][2]) * s );
|
|
y( (m[0][2] - m[2][0]) * s );
|
|
z( (m[1][0] - m[0][1]) * s );
|
|
}
|
|
else if (qx2 > qy2 && qx2 > qz2)
|
|
{
|
|
x( sqrtf(qx2) ); s = 0.25f / x() ;
|
|
w( (m[2][1] - m[1][2]) * s );
|
|
y( (m[0][1] - m[1][0]) * s );
|
|
z( (m[2][0] - m[0][2]) * s );
|
|
}
|
|
else if (qy2 > qz2)
|
|
{
|
|
y( sqrtf(qy2) ); s = 0.25f / y() ;
|
|
w( (m[0][2] - m[2][0]) * s );
|
|
x( (m[0][1] - m[1][0]) * s );
|
|
z( (m[1][2] - m[2][1]) * s );
|
|
}
|
|
else
|
|
{
|
|
z( sqrtf(qz2) ); s = 0.25f / z() ;
|
|
w( (m[1][0] - m[0][1]) * s );
|
|
x( (m[0][2] - m[2][0]) * s );
|
|
y( (m[1][2] - m[2][1]) * s );
|
|
}
|
|
|
|
//normalize() ;
|
|
}
|
|
|
|
/*! Return the inverse of a quaternion */
|
|
|
|
Quaternion Quaternion::inverse() const
|
|
{
|
|
float norm = x()*x() + y()*y() + z()*z() + w()*w() ;
|
|
|
|
return conjugate() * (1.0f / norm) ;
|
|
}
|
|
|
|
Quaternion operator * (const Quaternion &a, const Quaternion &b)
|
|
{
|
|
return Quaternion (
|
|
+a[0]*b[3] +a[1]*b[2] -a[2]*b[1] +a[3]*b[0],
|
|
-a[0]*b[2] +a[1]*b[3] +a[2]*b[0] +a[3]*b[1],
|
|
+a[0]*b[1] -a[1]*b[0] +a[2]*b[3] +a[3]*b[2],
|
|
-a[0]*b[0] -a[1]*b[1] -a[2]*b[2] +a[3]*b[3]
|
|
) ;
|
|
}
|
|
|
|
Point Quaternion::apply (const Point &v) const
|
|
{
|
|
// Torque uses column vectors, which means quaternions
|
|
// rotate backwards from what you might normally expect.
|
|
Quaternion q(v.x(), v.y(), v.z(), 0) ;
|
|
Quaternion r = conjugate() * q * (*this);
|
|
return Point(r.x(), r.y(), r.z()) ;
|
|
|
|
// The following should give the same result:
|
|
//Matrix<> m = toMatrix() ;
|
|
//return m * v ;
|
|
}
|
|
|
|
} |