mirror of
https://github.com/libretro/scummvm.git
synced 2025-02-04 09:56:30 +00:00
Merge pull request #943 from JoseJX/FixQuaternions
MATH: Remove redundant Euler conversion.
This commit is contained in:
Joel Teichroeb
commit
b13c379594
@ -2178,9 +2178,9 @@ Math::Vector3d Actor::getWorldPos() const {
|
||||
|
||||
Math::Quaternion Actor::getRotationQuat() const {
|
||||
if (g_grim->getGameType() == GType_MONKEY4) {
|
||||
Math::Quaternion ret = Math::Quaternion::fromEuler(_yaw, _pitch, _roll);
|
||||
Math::Quaternion ret = Math::Quaternion::fromXYZ(_yaw, _pitch, _roll, Math::EO_ZXY);
|
||||
if (_inOverworld)
|
||||
ret = Math::Quaternion::fromEuler(-_yaw, -_pitch, -_roll);
|
||||
ret = ret.inverse();
|
||||
|
||||
if (isAttached()) {
|
||||
Actor *attachedActor = Actor::getPool().getObject(_attachedActor);
|
||||
@ -2201,7 +2201,7 @@ Math::Quaternion Actor::getRotationQuat() const {
|
||||
}
|
||||
return ret;
|
||||
} else {
|
||||
return Math::Quaternion::fromEuler(-_roll, -_pitch, -_yaw);
|
||||
return Math::Quaternion::fromXYZ(_yaw, _pitch, _roll, Math::EO_ZXY).inverse();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -197,11 +197,11 @@ int KeyframeAnim::getMarker(float startTime, float stopTime) const {
|
||||
void KeyframeAnim::KeyframeEntry::loadBinary(const char *data) {
|
||||
_frame = get_float(data);
|
||||
_flags = READ_LE_UINT32(data + 4);
|
||||
_pos = Math::Vector3d::get_vector3d(data + 8);
|
||||
_pos = Math::Vector3d::getVector3d(data + 8);
|
||||
_pitch = get_float(data + 20);
|
||||
_yaw = get_float(data + 24);
|
||||
_roll = get_float(data + 28);
|
||||
_dpos = Math::Vector3d::get_vector3d(data + 32);
|
||||
_dpos = Math::Vector3d::getVector3d(data + 32);
|
||||
_dpitch = get_float(data + 44);
|
||||
_dyaw = get_float(data + 48);
|
||||
_droll = get_float(data + 52);
|
||||
|
||||
@ -129,7 +129,7 @@ void Model::loadBinary(Common::SeekableReadStream *data) {
|
||||
_radius = get_float(f);
|
||||
data->seek(36, SEEK_CUR);
|
||||
data->read(v3, 3 * 4);
|
||||
_insertOffset = Math::Vector3d::get_vector3d(v3);
|
||||
_insertOffset = Math::Vector3d::getVector3d(v3);
|
||||
}
|
||||
|
||||
void Model::loadText(TextSplitter *ts) {
|
||||
@ -332,7 +332,7 @@ int MeshFace::loadBinary(Common::SeekableReadStream *data, Material *materials[]
|
||||
_extraLight = get_float(f);
|
||||
data->seek(12, SEEK_CUR);
|
||||
data->read(v3, 4 * 3);
|
||||
_normal = Math::Vector3d::get_vector3d(v3);
|
||||
_normal = Math::Vector3d::getVector3d(v3);
|
||||
|
||||
_vertices = new int[_numVertices];
|
||||
for (int i = 0; i < _numVertices; i++) {
|
||||
@ -620,9 +620,9 @@ void ModelNode::loadBinary(Common::SeekableReadStream *data, ModelNode *hierNode
|
||||
int childPtr = data->readUint32LE();
|
||||
int siblingPtr = data->readUint32LE();
|
||||
data->read(v3, 4 * 3);
|
||||
_pivot = Math::Vector3d::get_vector3d(v3);
|
||||
_pivot = Math::Vector3d::getVector3d(v3);
|
||||
data->read(v3, 4 * 3);
|
||||
_pos = Math::Vector3d::get_vector3d(v3);
|
||||
_pos = Math::Vector3d::getVector3d(v3);
|
||||
data->read(f, 4);
|
||||
_pitch = get_float(f);
|
||||
data->read(f, 4);
|
||||
|
||||
@ -179,7 +179,7 @@ void Sector::loadBinary(Common::SeekableReadStream *data) {
|
||||
for (int i = 0; i < _numVertices; i++) {
|
||||
char v3[4 * 3];
|
||||
data->read(v3, 4 * 3);
|
||||
_vertices[i] = Math::Vector3d::get_vector3d(v3);
|
||||
_vertices[i] = Math::Vector3d::getVector3d(v3);
|
||||
}
|
||||
|
||||
// Repeat the last vertex for convenience
|
||||
|
||||
@ -354,10 +354,10 @@ void Set::Setup::loadBinary(Common::SeekableReadStream *data) {
|
||||
|
||||
char v[4 * 4];
|
||||
data->read(v, 4 * 3);
|
||||
_pos = Math::Vector3d::get_vector3d(v);
|
||||
_pos = Math::Vector3d::getVector3d(v);
|
||||
|
||||
data->read(v, 4 * 3);
|
||||
_interest = Math::Vector3d::get_vector3d(v);
|
||||
_interest = Math::Vector3d::getVector3d(v);
|
||||
|
||||
data->read(v, 4 * 4);
|
||||
_roll = get_float(v);
|
||||
|
||||
244
math/quat.cpp
244
math/quat.cpp
@ -20,7 +20,7 @@
|
||||
*
|
||||
*/
|
||||
|
||||
// Quaternion-math borrowed from plib http://plib.sourceforge.net/index.html
|
||||
// Quaternion-math originally borrowed from plib http://plib.sourceforge.net/index.html
|
||||
// Which is covered by LGPL2
|
||||
// And has this additional copyright note:
|
||||
/*
|
||||
@ -29,50 +29,116 @@
|
||||
Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
|
||||
Largely rewritten by "Negative0" <negative0@earthlink.net>
|
||||
*/
|
||||
// Additional changes written based on the math presented in
|
||||
// http://www.swarthmore.edu/NatSci/mzucker1/e27/diebel2006attitude.pdf
|
||||
|
||||
#include "common/streamdebug.h"
|
||||
|
||||
#include "common/math.h"
|
||||
#include "math/quat.h"
|
||||
|
||||
namespace Math {
|
||||
|
||||
Quaternion::Quaternion(const Matrix3 &m) {
|
||||
fromMatrix(m);
|
||||
normalize();
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const Matrix4 &m) {
|
||||
fromMatrix(m.getRotation());
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const Vector3d &axis, const Angle &angle) {
|
||||
float s = (angle / 2).getSine();
|
||||
float c = (angle / 2).getCosine();
|
||||
set(axis.x() * s, axis.y() * s, axis.z() * s, c);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::xAxis(const Angle &angle) {
|
||||
Quaternion q(Vector3d(1.0f, 0.0f, 0.0f), angle);
|
||||
return q;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::yAxis(const Angle &angle) {
|
||||
Quaternion q(Vector3d(0.0f, 1.0f, 0.0f), angle);
|
||||
return q;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::zAxis(const Angle &angle) {
|
||||
Quaternion q(Vector3d(0.0f, 0.0f, 1.0f), angle);
|
||||
return q;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::slerpQuat(const Quaternion& to, const float t) {
|
||||
Quaternion dst;
|
||||
float co, scale0, scale1;
|
||||
bool flip = false ;
|
||||
float scale0, scale1;
|
||||
float flip = 1.0f;
|
||||
float angle = this->dotProduct(to);
|
||||
|
||||
/* SWC - Interpolate between to quaternions */
|
||||
|
||||
co = this->dotProduct(to);
|
||||
|
||||
if (co < 0.0f) {
|
||||
co = -co;
|
||||
flip = true;
|
||||
// Make sure the rotation is the short one
|
||||
if (angle < 0.0f) {
|
||||
angle = -angle;
|
||||
flip = -1.0f;
|
||||
}
|
||||
|
||||
if ( co < 1.0f - (float) 1e-6 ) {
|
||||
float o = (float) acos ( co );
|
||||
float so = 1.0f / (float) sin ( o );
|
||||
// Spherical Interpolation
|
||||
// Threshold of 1e-6
|
||||
if (angle < 1.0f - (float) 1E-6f) {
|
||||
float theta = acosf(angle);
|
||||
float invSineTheta = 1.0f / sinf(theta);
|
||||
|
||||
scale0 = (float) sin ( (1.0f - t) * o ) * so;
|
||||
scale1 = (float) sin ( t * o ) * so;
|
||||
scale0 = sinf((1.0f - t) * theta) * invSineTheta;
|
||||
scale1 = (sinf(t * theta) * invSineTheta) * flip;
|
||||
// Linear Interpolation
|
||||
} else {
|
||||
scale0 = 1.0f - t;
|
||||
scale1 = t;
|
||||
scale1 = t * flip;
|
||||
}
|
||||
|
||||
if (flip) {
|
||||
scale1 = -scale1 ;
|
||||
}
|
||||
|
||||
dst.x() = scale0 * this->x() + scale1 * to.x() ;
|
||||
dst.y() = scale0 * this->y() + scale1 * to.y() ;
|
||||
dst.z() = scale0 * this->z() + scale1 * to.z() ;
|
||||
dst.w() = scale0 * this->w() + scale1 * to.w() ;
|
||||
|
||||
// Apply the interpolation
|
||||
dst = (*this * scale0) + (to * scale1);
|
||||
return dst;
|
||||
}
|
||||
|
||||
Quaternion& Quaternion::normalize() {
|
||||
const float scale = sqrtf(square(x()) + square(y()) + square(z()) + square(w()));
|
||||
|
||||
// Already normalized if the scale is 1.0
|
||||
if (scale != 1.0f)
|
||||
set(x() / scale, y() / scale, z() / scale, w() / scale);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void Quaternion::fromMatrix(const Matrix3 &m) {
|
||||
float qx, qy, qz;
|
||||
float qw = 0.25f * (m.getValue(0, 0) + m.getValue(1, 1) + m.getValue(2, 2) + 1.0f);
|
||||
|
||||
if (qw != 0.0f) {
|
||||
qw = sqrtf(qw);
|
||||
qx = (m.getValue(2, 1) - m.getValue(1, 2)) / (4 * qw);
|
||||
qy = (m.getValue(0, 2) - m.getValue(2, 0)) / (4 * qw);
|
||||
qz = (m.getValue(1, 0) - m.getValue(0, 1)) / (4 * qw);
|
||||
} else {
|
||||
float sqx = -0.5f * (m.getValue(1, 1) + m.getValue(2, 2));
|
||||
qx = sqrt(sqx);
|
||||
if (sqx > 0.0f) {
|
||||
qy = m.getValue(0, 1) / (2.0f * qx);
|
||||
qz = m.getValue(0, 2) / (2.0f * qx);
|
||||
} else {
|
||||
float sqy = 0.5f * (1.0f - m.getValue(2, 2));
|
||||
if (sqy > 0.0f) {
|
||||
qy = sqrtf(sqy);
|
||||
qz = m.getValue(1, 2) / (2.0f * qy);
|
||||
} else {
|
||||
qy = 0.0f;
|
||||
qz = 1.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
set(qx, qy, qz, qw);
|
||||
}
|
||||
|
||||
void Quaternion::toMatrix(Matrix4 &dst) const {
|
||||
float two_xx = x() * (x() + x());
|
||||
float two_xy = x() * (y() + y());
|
||||
@ -88,10 +154,10 @@ void Quaternion::toMatrix(Matrix4 &dst) const {
|
||||
float two_zz = z() * (z() + z());
|
||||
|
||||
float newMat[16] = {
|
||||
1.0f-(two_yy+two_zz), two_xy-two_wz, two_xz+two_wy, 0.0f,
|
||||
two_xy+two_wz, 1.0f-(two_xx+two_zz), two_yz-two_wx, 0.0f,
|
||||
two_xz-two_wy, two_yz+two_wx, 1.0f-(two_xx+two_yy), 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 1.0f
|
||||
1.0f - (two_yy + two_zz), two_xy - two_wz, two_xz + two_wy, 0.0f,
|
||||
two_xy + two_wz, 1.0f - (two_xx + two_zz), two_yz - two_wx, 0.0f,
|
||||
two_xz - two_wy, two_yz + two_wx, 1.0f - (two_xx + two_yy), 0.0f,
|
||||
0.0f, 0.0f, 0.0f, 1.0f
|
||||
};
|
||||
dst.setData(newMat);
|
||||
}
|
||||
@ -102,42 +168,90 @@ Matrix4 Quaternion::toMatrix() const {
|
||||
return dst;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::inverse() const {
|
||||
float norm = x() * x() + y() * y() + z() * z() + w() * w();
|
||||
if (norm > 0.f) {
|
||||
float invNorm = 1.0f / norm;
|
||||
return Quaternion(-x() * invNorm, -y() * invNorm, -z() * invNorm, w() * invNorm);
|
||||
} else {
|
||||
return Quaternion();
|
||||
}
|
||||
Quaternion Quaternion::inverse() {
|
||||
normalize();
|
||||
return Quaternion(-x(), -y(), -z(), w());
|
||||
}
|
||||
|
||||
Quaternion Quaternion::fromEuler(const Angle &yaw, const Angle &pitch, const Angle &roll) {
|
||||
float cr, cp, cy, sr, sp, sy, cpcy, spsy;
|
||||
|
||||
cy = (yaw / 2).getCosine();
|
||||
cp = (pitch / 2).getCosine();
|
||||
cr = (roll / 2).getCosine();
|
||||
|
||||
sy = (yaw / 2).getSine();
|
||||
sp = (pitch / 2).getSine();
|
||||
sr = (roll / 2).getSine();
|
||||
|
||||
cpcy = cp * cy;
|
||||
spsy = sp * sy;
|
||||
|
||||
return Quaternion(
|
||||
cr * sp * cy + sr * cp * sy,
|
||||
cr * cp * sy - sr * sp * cy,
|
||||
sr * cpcy - cr * spsy,
|
||||
cr * cpcy + sr * spsy);
|
||||
}
|
||||
Quaternion Quaternion::operator*(const Quaternion &o) const {
|
||||
return Quaternion(
|
||||
w() * o.x() + x() * o.w() + y() * o.z() - z() * o.y(),
|
||||
w() * o.y() - x() * o.z() + y() * o.w() + z() * o.x(),
|
||||
w() * o.z() + x() * o.y() - y() * o.x() + z() * o.w(),
|
||||
w() * o.w() - x() * o.x() - y() * o.y() - z() * o.z()
|
||||
);
|
||||
}
|
||||
Vector3d Quaternion::directionVector(const int col) const {
|
||||
Matrix4 dirMat = toMatrix();
|
||||
return Vector3d(dirMat.getValue(0, col), dirMat.getValue(1, col), dirMat.getValue(2, col));
|
||||
}
|
||||
|
||||
Angle Quaternion::getAngleBetween(const Quaternion &to) {
|
||||
Quaternion q = this->inverse() * to;
|
||||
Angle diff(Common::rad2deg(2 * acos(q.w())));
|
||||
return diff;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::fromXYZ(const Angle &rotX, const Angle &rotY, const Angle &rotZ, EulerOrder order) {
|
||||
// First create a matrix with the rotation
|
||||
Matrix4 rot(rotX, rotY, rotZ, order);
|
||||
|
||||
// Convert this rotation matrix to a Quaternion
|
||||
return Quaternion(rot);
|
||||
}
|
||||
|
||||
void Quaternion::getXYZ(Angle *rotX, Angle *rotY, Angle *rotZ, EulerOrder order) {
|
||||
// Create a matrix from the Quaternion
|
||||
Matrix4 rot = toMatrix();
|
||||
|
||||
// Convert the matrix to Euler Angles
|
||||
Angle ex, ey, ez;
|
||||
rot.getXYZ(&ex, &ey, &ez, order);
|
||||
|
||||
// Assign the Angles if we have a reference
|
||||
if (rotX != nullptr)
|
||||
*rotX = ex;
|
||||
if (rotY != nullptr)
|
||||
*rotY = ey;
|
||||
if (rotZ != nullptr)
|
||||
*rotZ = ez;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const Quaternion &o) const {
|
||||
return Quaternion(
|
||||
w() * o.x() + x() * o.w() + y() * o.z() - z() * o.y(),
|
||||
w() * o.y() - x() * o.z() + y() * o.w() + z() * o.x(),
|
||||
w() * o.z() + x() * o.y() - y() * o.x() + z() * o.w(),
|
||||
w() * o.w() - x() * o.x() - y() * o.y() - z() * o.z()
|
||||
);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const float c) const {
|
||||
return Quaternion(x() * c, y() * c, z() * c, w() * c);
|
||||
}
|
||||
|
||||
Quaternion& Quaternion::operator*=(const Quaternion &o) {
|
||||
*this = *this * o;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator+(const Quaternion &o) const {
|
||||
return Quaternion(x() + o.x(), y() + o.y(), z() + o.z(), w() + o.w());
|
||||
}
|
||||
|
||||
Quaternion& Quaternion::operator+=(const Quaternion &o) {
|
||||
*this = *this + o;
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Quaternion::operator==(const Quaternion &o) const {
|
||||
float dw = fabs(w() - o.w());
|
||||
float dx = fabs(x() - o.x());
|
||||
float dy = fabs(y() - o.y());
|
||||
float dz = fabs(z() - o.z());
|
||||
// Threshold of equality
|
||||
float th = 1E-5f;
|
||||
|
||||
if ((dw < th) && (dx < th) && (dy < th) && (dz < th)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Quaternion::operator!=(const Quaternion &o) const {
|
||||
return !(*this == o);
|
||||
}
|
||||
|
||||
} // End namespace Math
|
||||
|
||||
188
math/quat.h
188
math/quat.h
@ -36,38 +36,206 @@
|
||||
#include "common/scummsys.h"
|
||||
#include "common/endian.h"
|
||||
|
||||
#include "math/rotation3d.h"
|
||||
#include "math/vector.h"
|
||||
#include "math/angle.h"
|
||||
#include "math/vector4d.h"
|
||||
#include "matrix4.h"
|
||||
#include "math/matrix4.h"
|
||||
|
||||
namespace Math {
|
||||
|
||||
class Quaternion : public Vector4d {
|
||||
public:
|
||||
Quaternion() : Vector4d(0, 0, 0, 0) {}
|
||||
/**
|
||||
* Default Constructor, creates an identity Quaternion with no rotation.
|
||||
*/
|
||||
Quaternion() : Vector4d(0, 0, 0, 1.0) {}
|
||||
|
||||
/**
|
||||
* Constructor from four floats in the order X,Y,Z,W
|
||||
* The initial values should be normalized, otherwise call normalize() after creation
|
||||
* @param lx The X value of the Quaternion
|
||||
* @param ly The Y value of the Quaternion
|
||||
* @param lz The Z value of the Quaternion
|
||||
* @param lw The W value of the Quaternion
|
||||
*/
|
||||
Quaternion(float lx, float ly, float lz, float lw) : Vector4d(lx, ly, lz, lw) {}
|
||||
|
||||
/**
|
||||
* Constructor from an existing Quaternion
|
||||
* @param q The existing quaternion
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
Quaternion(const Quaternion &q) : Vector4d(q.x(), q.y(), q.z(), q.w()) {}
|
||||
|
||||
/**
|
||||
* Constructor from a vector of four floats in the order X,Y,Z,W
|
||||
* The initial values should be normalized, otherwise call normalize() after creation
|
||||
* @param vec The vector of floats comprising the quaternion
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
Quaternion(const Vector4d &vec) : Vector4d(vec.x(), vec.y(), vec.z(), vec.w()) {}
|
||||
|
||||
|
||||
/**
|
||||
* Build the saved Quaternion from the array of floats
|
||||
* @param data The array holding the four floats that comprise the Quaternion
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
inline static Quaternion getQuaternion(const char *data) {
|
||||
return Quaternion(get_float(data), get_float(data + 4), get_float(data + 8), get_float(data + 12));
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from a rotation matrix
|
||||
* @param m The rotation matrix
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
Quaternion(const Matrix3 &m);
|
||||
|
||||
/**
|
||||
* Constructor from a rotation matrix
|
||||
* @param m The rotation matrix
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
Quaternion(const Matrix4 &m);
|
||||
|
||||
/** Set the Quaternion from a rotation matrix
|
||||
* @param m The matrix used to set the Quaternion
|
||||
*/
|
||||
void fromMatrix(const Matrix3 &m);
|
||||
|
||||
/**
|
||||
* Constructor from an axis vector and the angle to rotate on that axis
|
||||
* @param axis The axis to perform the rotation around
|
||||
* @param angle The angle amount to rotate
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
Quaternion(const Vector3d &axis, const Angle &angle);
|
||||
|
||||
/**
|
||||
* Constructs a Quaternion from Euler Coordinates
|
||||
* @param x The Euler Angle for the X Axis
|
||||
* @param y The Euler Angle for the Y Axis
|
||||
* @param z The Euler Angle for the Z Axis
|
||||
* @param order The Euler Order, specified in Rotation3D
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
static Quaternion fromXYZ(const Angle &rotX, const Angle &rotY, const Angle &rotZ, EulerOrder order);
|
||||
|
||||
/**
|
||||
* Returns Euler Angles based on the Euler Order
|
||||
* @param x The Euler Angle for the X Axis
|
||||
* @param y The Euler Angle for the Y Axis
|
||||
* @param z The Euler Angle for the Z Axis
|
||||
* @param order The Euler Order, specified in Rotation3D
|
||||
* @return The new Quaternion
|
||||
*/
|
||||
void getXYZ(Angle *rotX, Angle *rotY, Angle *rotZ, EulerOrder order);
|
||||
|
||||
/**
|
||||
* Create a Quaternion from a rotation around the X Axis
|
||||
* @param angle The Euler Angle for rotation
|
||||
* @return The resulting Quaternion
|
||||
*/
|
||||
static Quaternion xAxis(const Angle &angle);
|
||||
|
||||
/**
|
||||
* Create a Quaternion from a rotation around the Y Axis
|
||||
* @param angle The Euler Angle for rotation
|
||||
* @return The resulting Quaternion
|
||||
*/
|
||||
static Quaternion yAxis(const Angle &angle);
|
||||
|
||||
/**
|
||||
* Create a Quaternion from a rotation around the Z Axis
|
||||
* @param angle The Euler Angle for rotation
|
||||
* @return The resulting Quaternion
|
||||
*/
|
||||
static Quaternion zAxis(const Angle &angle);
|
||||
|
||||
/**
|
||||
* Normalize the Quaternion
|
||||
* @return A reference to this quaternion
|
||||
*/
|
||||
Quaternion &normalize();
|
||||
|
||||
/**
|
||||
* Converts from this Quaternion to a Matrix4 representation
|
||||
* @return The resulting matrix
|
||||
*/
|
||||
Matrix4 toMatrix() const;
|
||||
|
||||
/**
|
||||
* Converts from this Quaternion to a Matrix4 representation
|
||||
* @param dst The resulting matrix
|
||||
*/
|
||||
void toMatrix(Matrix4 &dst) const;
|
||||
Quaternion inverse() const;
|
||||
|
||||
/**
|
||||
* Make a new Quaternion that's the inverse of this Quaternion
|
||||
* @return The resulting Quaternion
|
||||
*/
|
||||
Quaternion inverse();
|
||||
|
||||
/**
|
||||
* Slerps between this quaternion and to by factor t
|
||||
* @param to the quaternion to slerp between
|
||||
* @param to the quaternion to slerp between
|
||||
* @param t factor to slerp by.
|
||||
* @return the resulting quaternion.
|
||||
*/
|
||||
Quaternion slerpQuat(const Quaternion& to, const float t);
|
||||
static Quaternion fromEuler(const Angle &yaw, const Angle &pitch, const Angle &roll);
|
||||
|
||||
inline static Quaternion get_quaternion(const char *data) {
|
||||
return Quaternion(get_float(data), get_float(data + 4), get_float(data + 8), get_float(data + 12));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the direction vector specified by col
|
||||
* @param col Column in the rotation matrix to get the direction vector from
|
||||
* @return The resulting Vector3d
|
||||
*/
|
||||
Vector3d directionVector(const int col) const;
|
||||
|
||||
/**
|
||||
* Get the angle between two quaternions
|
||||
* @param to The quaternion we're comparing against
|
||||
* @return The angle between the two
|
||||
*/
|
||||
Angle getAngleBetween(const Quaternion &to);
|
||||
|
||||
/**
|
||||
* Assignment operator for assigning a vector of values (X,Y,Z,W) to a Quaternion
|
||||
* @param vec The source vector
|
||||
* @return A reference to this Quaternion
|
||||
*/
|
||||
Quaternion& operator=(Vector4d &vec);
|
||||
|
||||
/**
|
||||
* Multiply two Quaternions
|
||||
* @param quat The Quaternion multiplicand
|
||||
* @return The result of the multiplication
|
||||
*/
|
||||
Quaternion operator*(const Quaternion &quat) const;
|
||||
Quaternion& operator*=(const Quaternion &quat);
|
||||
|
||||
/**
|
||||
* Multiply this Quaternion by a constant
|
||||
* @param quat The Quaternion multiplicand
|
||||
* @return The result of the multiplication
|
||||
*/
|
||||
Quaternion operator*(const float c) const;
|
||||
|
||||
/**
|
||||
* Sum two quaternions
|
||||
* @param quat The Quaternion to be added
|
||||
* @return The result of the addition
|
||||
*/
|
||||
Quaternion operator+(const Quaternion &o) const;
|
||||
Quaternion& operator+=(const Quaternion &o);
|
||||
|
||||
/**
|
||||
* Compare quaternions
|
||||
* @param quat The Quaternion to be compared
|
||||
* @return The result of the comparison
|
||||
*/
|
||||
bool operator==(const Quaternion &o) const;
|
||||
bool operator!=(const Quaternion &o) const;
|
||||
};
|
||||
|
||||
} // end of namespace Math
|
||||
|
||||
@ -48,23 +48,49 @@ public:
|
||||
Matrix(const MatrixBase<3, 1> &m);
|
||||
Matrix(const float *data);
|
||||
|
||||
/**
|
||||
* Set the value of the vector using three floats
|
||||
* @param lx X Value
|
||||
* @param ly Y Value
|
||||
* @param lz Z Value
|
||||
*/
|
||||
void set(float lx, float ly, float lz);
|
||||
|
||||
// Get the angle a vector is around the unit circle
|
||||
// (ignores z-component)
|
||||
/**
|
||||
* Get the angle of this vector around the unit circle
|
||||
* This operation ignores the z-component
|
||||
* @return The computed angle
|
||||
*/
|
||||
Angle unitCircleAngle() const;
|
||||
|
||||
/**
|
||||
* Find the cross product between two vectors
|
||||
* @param v1 The first vector
|
||||
* @param v2 The second vector
|
||||
* @return The resulting cross product
|
||||
*/
|
||||
inline static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2) {
|
||||
return Vector3d(v1.y() * v2.z() - v1.z() * v2.y(),
|
||||
v1.z() * v2.x() - v1.x() * v2.z(),
|
||||
v1.x() * v2.y() - v1.y() * v2.x());
|
||||
v1.z() * v2.x() - v1.x() * v2.z(),
|
||||
v1.x() * v2.y() - v1.y() * v2.x());
|
||||
}
|
||||
|
||||
/**
|
||||
* Find the angle between two vectors
|
||||
* @param v1 The first vector
|
||||
* @param v2 The second vector
|
||||
* @return The computed angle
|
||||
*/
|
||||
inline static Angle angle(const Vector3d& v1, const Vector3d& v2) {
|
||||
return Angle::arcCosine(dotProduct(v1, v2) / (v1.getMagnitude() * v2.getMagnitude()));
|
||||
}
|
||||
|
||||
inline static Vector3d get_vector3d(const char *data) {
|
||||
/**
|
||||
* Retrieve a Vector3d from the saved data
|
||||
* @param data The data stream
|
||||
* @return The retrieved Vector3d
|
||||
*/
|
||||
inline static Vector3d getVector3d(const char *data) {
|
||||
return Vector3d(get_float(data), get_float(data + 4), get_float(data + 8));
|
||||
}
|
||||
|
||||
|
||||
104
test/math/quat.h
Normal file
104
test/math/quat.h
Normal file
@ -0,0 +1,104 @@
|
||||
#include <cxxtest/TestSuite.h>
|
||||
|
||||
#include "math/quat.h"
|
||||
|
||||
class QuatTestSuite : public CxxTest::TestSuite {
|
||||
public:
|
||||
// Test Constructors
|
||||
void test_Quaternion() {
|
||||
// Create an empty unit quaternion
|
||||
Math::Quaternion q;
|
||||
|
||||
TS_ASSERT(q.x() == 0.0f);
|
||||
TS_ASSERT(q.y() == 0.0f);
|
||||
TS_ASSERT(q.z() == 0.0f);
|
||||
TS_ASSERT(q.w() == 1.0f);
|
||||
}
|
||||
|
||||
// Test Normalization
|
||||
void test_Normalize() {
|
||||
Math::Quaternion q(1.0f, 2.0f, 3.0f, 4.0f);
|
||||
q.normalize();
|
||||
|
||||
// Check the values against the known result
|
||||
TS_ASSERT(fabs(q.x() - 0.182574f) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.y() - 0.365148f) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.z() - 0.547723f) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.w() - 0.730297f) < 0.0001f);
|
||||
|
||||
// Check that the RMS is 1.0
|
||||
float r = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z() + q.w() * q.w());
|
||||
TS_ASSERT(fabs(r - 1.0f) < 0.0001f);
|
||||
}
|
||||
|
||||
// Test going to a matrix and back
|
||||
void test_quatMatrix() {
|
||||
Math::Angle xAngle(20);
|
||||
Math::Angle yAngle(30);
|
||||
Math::Angle zAngle(40);
|
||||
|
||||
Math::Quaternion q = Math::Quaternion::fromXYZ(xAngle, yAngle, zAngle, Math::EO_XYZ);
|
||||
Math::Matrix4 m = q.toMatrix();
|
||||
|
||||
Math::Quaternion r(m);
|
||||
|
||||
// Compare
|
||||
TS_ASSERT(fabs(q.x() - r.x()) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.y() - r.y()) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.z() - r.z()) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.w() - r.w()) < 0.0001f);
|
||||
}
|
||||
|
||||
// Test rotations
|
||||
void test_quatRotationXYZ() {
|
||||
Math::Angle xAngle(20);
|
||||
Math::Angle yAngle(30);
|
||||
Math::Angle zAngle(40);
|
||||
|
||||
// First way
|
||||
Math::Quaternion q;
|
||||
q = Math::Quaternion::xAxis(xAngle);
|
||||
q *= Math::Quaternion::yAxis(yAngle);
|
||||
q *= Math::Quaternion::zAxis(zAngle);
|
||||
|
||||
// Second way
|
||||
Math::Quaternion r = Math::Quaternion::fromXYZ(xAngle, yAngle, zAngle, Math::EO_XYZ);
|
||||
|
||||
// Compare to the known correct result
|
||||
TS_ASSERT(fabs(q.x() - 0.244792f) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.y() - 0.182148f) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.z() - 0.36758f) < 0.0001f);
|
||||
TS_ASSERT(fabs(q.w() - 0.878512f) < 0.0001f);
|
||||
|
||||
// Compare to the known correct result
|
||||
TS_ASSERT(fabs(r.x() - 0.244792f) < 0.0001f);
|
||||
TS_ASSERT(fabs(r.y() - 0.182148f) < 0.0001f);
|
||||
TS_ASSERT(fabs(r.z() - 0.36758f) < 0.0001f);
|
||||
TS_ASSERT(fabs(r.w() - 0.878512f) < 0.0001f);
|
||||
}
|
||||
|
||||
void test_quatAngleBetween() {
|
||||
Math::Angle a1(10);
|
||||
Math::Angle a2(20);
|
||||
Math::Quaternion q = Math::Quaternion::fromXYZ(a1,0,0, Math::EO_XYZ);
|
||||
Math::Quaternion r = Math::Quaternion::fromXYZ(a2,0,0, Math::EO_XYZ);
|
||||
|
||||
Math::Angle a = q.getAngleBetween(r);
|
||||
|
||||
TS_ASSERT(fabs(a.getDegrees() - 10) < 0.0001f);
|
||||
}
|
||||
|
||||
void test_invert() {
|
||||
Math::Angle a1(50);
|
||||
Math::Angle a2(30);
|
||||
Math::Angle a3(10);
|
||||
|
||||
Math::Quaternion q = Math::Quaternion::fromXYZ(a1, a2, a3, Math::EO_XYZ);
|
||||
Math::Quaternion r = q.inverse();
|
||||
|
||||
TS_ASSERT(r.x() == -q.x());
|
||||
TS_ASSERT(r.y() == -q.y());
|
||||
TS_ASSERT(r.z() == -q.z());
|
||||
TS_ASSERT(r.w() == q.w());
|
||||
}
|
||||
};
|
||||
Loading…
x
Reference in New Issue
Block a user