/* orxonox - the future of 3D-vertical-scrollers Copyright (C) 2004 orx This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2, or (at your option) any later version. ### File Specific: main-programmer: Christian Meyer co-programmer: Patrick Boenzli : Vector::scale() Vector::abs() Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake 2005-06-02: Benjamin Grauer: speed up, and new Functionality to Vector (mostly inline now) */ #define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH #include "quaternion.h" #ifdef DEBUG #include "debug.h" #else #include #define PRINT(x) printf #endif using namespace std; ///////////////// /* QUATERNIONS */ ///////////////// /** * @brief calculates a lookAt rotation * @param dir: the direction you want to look * @param up: specify what direction up should be * * Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point * the same way as dir. If you want to use this with cameras, you'll have to reverse the * dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You * can use this for meshes as well (then you do not have to reverse the vector), but keep * in mind that if you do that, the model's front has to point in +z direction, and left * and right should be -x or +x respectively or the mesh wont rotate correctly. * * @TODO !!! OPTIMIZE THIS !!! */ Quaternion::Quaternion (const Vector& dir, const Vector& up) { Vector z = dir.getNormalized(); Vector x = up.cross(z).getNormalized(); Vector y = z.cross(x); float m[4][4]; m[0][0] = x.x; m[0][1] = x.y; m[0][2] = x.z; m[0][3] = 0; m[1][0] = y.x; m[1][1] = y.y; m[1][2] = y.z; m[1][3] = 0; m[2][0] = z.x; m[2][1] = z.y; m[2][2] = z.z; m[2][3] = 0; m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; *this = Quaternion (m); } /** * @brief calculates a rotation from euler angles * @param roll: the roll in radians * @param pitch: the pitch in radians * @param yaw: the yaw in radians */ Quaternion::Quaternion (float roll, float pitch, float yaw) { float cr, cp, cy, sr, sp, sy, cpcy, spsy; // calculate trig identities cr = cos(roll/2); cp = cos(pitch/2); cy = cos(yaw/2); sr = sin(roll/2); sp = sin(pitch/2); sy = sin(yaw/2); cpcy = cp * cy; spsy = sp * sy; w = cr * cpcy + sr * spsy; v.x = sr * cpcy - cr * spsy; v.y = cr * sp * cy + sr * cp * sy; v.z = cr * cp * sy - sr * sp * cy; } /** * @brief convert the Quaternion to a 4x4 rotational glMatrix * @param m: a buffer to store the Matrix in */ void Quaternion::matrix (float m[4][4]) const { float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; // calculate coefficients x2 = v.x + v.x; y2 = v.y + v.y; z2 = v.z + v.z; xx = v.x * x2; xy = v.x * y2; xz = v.x * z2; yy = v.y * y2; yz = v.y * z2; zz = v.z * z2; wx = w * x2; wy = w * y2; wz = w * z2; m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz; m[2][0] = xz + wy; m[3][0] = 0.0; m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz); m[2][1] = yz - wx; m[3][1] = 0.0; m[0][2] = xz - wy; m[1][2] = yz + wx; m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0; m[0][3] = 0; m[1][3] = 0; m[2][3] = 0; m[3][3] = 1; } /** * @brief Slerps this QUaternion performs a smooth move. * @param toQuat to this Quaternion * @param t \% inth the the direction[0..1] */ void Quaternion::slerpTo(const Quaternion& toQuat, float t) { float tol[4]; double omega, cosom, sinom, scale0, scale1; // float DELTA = 0.2; cosom = this->v.x * toQuat.v.x + this->v.y * toQuat.v.y + this->v.z * toQuat.v.z + this->w * toQuat.w; if( cosom < 0.0 ) { cosom = -cosom; tol[0] = -toQuat.v.x; tol[1] = -toQuat.v.y; tol[2] = -toQuat.v.z; tol[3] = -toQuat.w; } else { tol[0] = toQuat.v.x; tol[1] = toQuat.v.y; tol[2] = toQuat.v.z; tol[3] = toQuat.w; } omega = acos(cosom); sinom = sin(omega); scale0 = sin((1.0 - t) * omega) / sinom; scale1 = sin(t * omega) / sinom; this->v = Vector(scale0 * this->v.x + scale1 * tol[0], scale0 * this->v.y + scale1 * tol[1], scale0 * this->v.z + scale1 * tol[2]); this->w = scale0 * this->w + scale1 * tol[3]; } /** * @brief performs a smooth move. * @param from where * @param to where * @param t the time this transformation should take value [0..1] * @returns the Result of the smooth move */ Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t) { float tol[4]; double omega, cosom, sinom, scale0, scale1; // float DELTA = 0.2; cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w; if( cosom < 0.0 ) { cosom = -cosom; tol[0] = -to.v.x; tol[1] = -to.v.y; tol[2] = -to.v.z; tol[3] = -to.w; } else { tol[0] = to.v.x; tol[1] = to.v.y; tol[2] = to.v.z; tol[3] = to.w; } omega = acos(cosom); sinom = sin(omega); scale0 = sin((1.0 - t) * omega) / sinom; scale1 = sin(t * omega) / sinom; return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0], scale0 * from.v.y + scale1 * tol[1], scale0 * from.v.z + scale1 * tol[2]), scale0 * from.w + scale1 * tol[3]); } /** * @returns the heading */ float Quaternion::getHeading() const { float pole = this->v.x*this->v.y + this->v.z*this->w; if (fabsf(pole) != 0.5) return atan2(2.0* (v.y*w - v.x*v.z), 1 - 2.0*(v.y*v.y - v.z*v.z)); else if (pole == .5) // North Pole return 2.0 * atan2(v.x, w); else // South Pole return -2.0 * atan2(v.x, w); } /** * @returns the Attitude */ float Quaternion::getAttitude() const { return asin(2.0 * (v.x*v.y + v.z*w)); } /** * @returns the Bank */ float Quaternion::getBank() const { if (fabsf(this->v.x*this->v.y + this->v.z*this->w) != 0.5) return atan2(2.0*(v.x*w-v.y*v.z) , 1 - 2.0*(v.x*v.x - v.z*v.z)); else return 0.0f; } /** * @brief convert a rotational 4x4 glMatrix into a Quaternion * @param m: a 4x4 matrix in glMatrix order */ Quaternion::Quaternion (float m[4][4]) { float tr, s, q[4]; int i, j, k; int nxt[3] = {1, 2, 0}; tr = m[0][0] + m[1][1] + m[2][2]; // check the diagonal if (tr > 0.0) { s = sqrt (tr + 1.0); w = s / 2.0; s = 0.5 / s; v.x = (m[1][2] - m[2][1]) * s; v.y = (m[2][0] - m[0][2]) * s; v.z = (m[0][1] - m[1][0]) * s; } else { // diagonal is negative i = 0; if (m[1][1] > m[0][0]) i = 1; if (m[2][2] > m[i][i]) i = 2; j = nxt[i]; k = nxt[j]; s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0); q[i] = s * 0.5; if (s != 0.0) s = 0.5 / s; q[3] = (m[j][k] - m[k][j]) * s; q[j] = (m[i][j] + m[j][i]) * s; q[k] = (m[i][k] + m[k][i]) * s; v.x = q[0]; v.y = q[1]; v.z = q[2]; w = q[3]; } } /** * Creates a quaternion from a 3x3 rotation matrix. * @param mat The 3x3 source rotation matrix. * @return The equivalent 4 float quaternion. */ Quaternion::Quaternion(float mat[3][3]) { int NXT[] = {1, 2, 0}; float q[4]; // check the diagonal float tr = mat[0][0] + mat[1][1] + mat[2][2]; if( tr > 0.0f) { float s = (float)sqrtf(tr + 1.0f); this->w = s * 0.5f; s = 0.5f / s; this->v.x = (mat[1][2] - mat[2][1]) * s; this->v.y = (mat[2][0] - mat[0][2]) * s; this->v.z = (mat[0][1] - mat[1][0]) * s; } else { // diagonal is negative // get biggest diagonal element int i = 0; if (mat[1][1] > mat[0][0]) i = 1; if (mat[2][2] > mat[i][i]) i = 2; //setup index sequence int j = NXT[i]; int k = NXT[j]; float s = (float)sqrtf((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0f); q[i] = s * 0.5f; if (s != 0.0f) s = 0.5f / s; q[j] = (mat[i][j] + mat[j][i]) * s; q[k] = (mat[i][k] + mat[k][i]) * s; q[3] = (mat[j][k] - mat[k][j]) * s; this->v.x = q[0]; this->v.y = q[1]; this->v.z = q[2]; this->w = q[3]; } } /** * @brief outputs some nice formated debug information about this quaternion */ void Quaternion::debug() const { PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z); } /** * @brief another better Quaternion Debug Function. */ void Quaternion::debug2() const { Vector axis = this->getSpacialAxis(); PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z ); }