ResidualVM logo ResidualVM website - Forums - Contact us BuildBot - Doxygen - Wiki curved edge

quat.cpp

Go to the documentation of this file.
00001 /* ResidualVM - A 3D game interpreter
00002  *
00003  * ResidualVM is the legal property of its developers, whose names
00004  * are too numerous to list here. Please refer to the COPYRIGHT
00005  * file distributed with this source distribution.
00006  *
00007  * This program is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU General Public License
00009  * as published by the Free Software Foundation; either version 2
00010  * of the License, or (at your option) any later version.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program; if not, write to the Free Software
00019  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
00020  *
00021  */
00022 
00023 /*
00024  * Quaternion-math originally borrowed from plib http://plib.sourceforge.net/index.html
00025  * This code was originally made available under the LGPLv2 license (or later).
00026  *
00027  * Quaternion routines are Copyright (C) 1999
00028  * Kevin B. Thompson <kevinbthompson@yahoo.com>
00029  * Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
00030  * Largely rewritten by "Negative0" <negative0@earthlink.net>
00031  *
00032  * This code (and our modifications) is made available here under the GPLv2 (or later).
00033  *
00034  * Additional changes written based on the math presented in
00035  * http://www.swarthmore.edu/NatSci/mzucker1/e27/diebel2006attitude.pdf
00036  *
00037  */
00038 
00039 #include "common/streamdebug.h"
00040 
00041 #include "common/math.h"
00042 #include "math/quat.h"
00043 
00044 namespace Math {
00045 
00046 Quaternion::Quaternion(const Matrix3 &m) {
00047     fromMatrix(m);
00048     normalize();
00049 }
00050 
00051 Quaternion::Quaternion(const Matrix4 &m) {
00052     fromMatrix(m.getRotation());
00053 }
00054 
00055 Quaternion::Quaternion(const Vector3d &axis, const Angle &angle) {
00056     float s = (angle / 2).getSine();
00057     float c = (angle / 2).getCosine();
00058     set(axis.x() * s, axis.y() * s, axis.z() * s, c);
00059 }
00060 
00061 Quaternion Quaternion::xAxis(const Angle &angle) {
00062     Quaternion q(Vector3d(1.0f, 0.0f, 0.0f), angle);
00063     return q;
00064 }
00065 
00066 Quaternion Quaternion::yAxis(const Angle &angle) {
00067     Quaternion q(Vector3d(0.0f, 1.0f, 0.0f), angle);
00068     return q;
00069 }
00070 
00071 Quaternion Quaternion::zAxis(const Angle &angle) {
00072     Quaternion q(Vector3d(0.0f, 0.0f, 1.0f), angle);
00073     return q;
00074 }
00075 
00076 Quaternion Quaternion::slerpQuat(const Quaternion& to, const float t) const {
00077     Quaternion dst;
00078     float scale0, scale1;
00079     float flip = 1.0f;
00080     float angle = this->dotProduct(to);
00081 
00082     // Make sure the rotation is the short one
00083     if (angle < 0.0f) {
00084         angle = -angle;
00085         flip = -1.0f;
00086     }
00087 
00088     // Spherical Interpolation
00089     // Threshold of 1e-6
00090     if (angle < 1.0f - (float) 1E-6f) {
00091         float theta = acosf(angle);
00092         float invSineTheta = 1.0f / sinf(theta);
00093 
00094         scale0 = sinf((1.0f - t) * theta) * invSineTheta;
00095         scale1 = (sinf(t * theta) * invSineTheta) * flip;
00096     // Linear Interpolation
00097     } else {
00098         scale0 = 1.0f - t;
00099         scale1 = t * flip;
00100     }
00101 
00102     // Apply the interpolation
00103     dst = (*this * scale0) + (to * scale1);
00104     return dst;
00105 }
00106 
00107 Quaternion& Quaternion::normalize() {
00108     const float scale = sqrtf(square(x()) + square(y()) + square(z()) + square(w()));
00109 
00110     // Already normalized if the scale is 1.0
00111     if (scale != 1.0f && scale != 0.0f)
00112         set(x() / scale, y() / scale, z() / scale, w() / scale);
00113 
00114     return *this;
00115 }
00116 
00117 void Quaternion::transform(Vector3d &v) const {
00118     const Vector3d im = Vector3d(x(), y(), z());
00119     v += 2.0 * Vector3d::crossProduct(im, Vector3d::crossProduct(im, v) + w() * v);
00120 }
00121 
00122 void Quaternion::fromMatrix(const Matrix3 &m) {
00123     float qx, qy, qz, qw;
00124     float tr = m.getValue(0, 0) + m.getValue(1, 1) + m.getValue(2, 2);
00125     float s;
00126 
00127     if (tr > 0.0f) {
00128         s = sqrtf(tr + 1.0f);
00129         qw = s * 0.5f;
00130         s = 0.5f / s;
00131         qx = (m.getValue(2, 1) - m.getValue(1, 2)) * s;
00132         qy = (m.getValue(0, 2) - m.getValue(2, 0)) * s;
00133         qz = (m.getValue(1, 0) - m.getValue(0, 1)) * s;
00134     } else {
00135         int h = 0;
00136         if (m.getValue(1, 1) > m.getValue(0, 0))
00137             h = 1;
00138         if (m.getValue(2, 2) > m.getValue(h, h))
00139             h = 2;
00140 
00141         if (h == 0) {
00142             s = sqrt(m.getValue(0, 0) - (m.getValue(1,1) + m.getValue(2, 2)) + 1.0f);
00143             qx = s * 0.5f;
00144             s = 0.5f / s;
00145             qy = (m.getValue(0, 1) + m.getValue(1, 0)) * s;
00146             qz = (m.getValue(2, 0) + m.getValue(0, 2)) * s;
00147             qw = (m.getValue(2, 1) - m.getValue(1, 2)) * s;
00148         } else if (h == 1) {
00149             s = sqrt(m.getValue(1, 1) - (m.getValue(2,2) + m.getValue(0, 0)) + 1.0f);
00150             qy = s * 0.5f;
00151             s = 0.5f / s;
00152             qz = (m.getValue(1, 2) + m.getValue(2, 1)) * s;
00153             qx = (m.getValue(0, 1) + m.getValue(1, 0)) * s;
00154             qw = (m.getValue(0, 2) - m.getValue(2, 0)) * s;
00155         } else {
00156             s = sqrt(m.getValue(2, 2) - (m.getValue(0,0) + m.getValue(1, 1)) + 1.0f);
00157             qz = s * 0.5f;
00158             s = 0.5f / s;
00159             qx = (m.getValue(2, 0) + m.getValue(0, 2)) * s;
00160             qy = (m.getValue(1, 2) + m.getValue(2, 1)) * s;
00161             qw = (m.getValue(1, 0) - m.getValue(0, 1)) * s;
00162         }
00163     }
00164     set(qx, qy, qz, qw);
00165 }
00166 
00167 void Quaternion::toMatrix(Matrix4 &dst) const {
00168     float two_xx = x() * (x() + x());
00169     float two_xy = x() * (y() + y());
00170     float two_xz = x() * (z() + z());
00171 
00172     float two_wx = w() * (x() + x());
00173     float two_wy = w() * (y() + y());
00174     float two_wz = w() * (z() + z());
00175 
00176     float two_yy = y() * (y() + y());
00177     float two_yz = y() * (z() + z());
00178 
00179     float two_zz = z() * (z() + z());
00180 
00181     float newMat[16] = {
00182         1.0f - (two_yy + two_zz),   two_xy - two_wz,        two_xz + two_wy,      0.0f,
00183         two_xy + two_wz,        1.0f - (two_xx + two_zz),   two_yz - two_wx,      0.0f,
00184         two_xz - two_wy,        two_yz + two_wx,        1.0f - (two_xx + two_yy), 0.0f,
00185         0.0f,               0.0f,               0.0f,             1.0f
00186     };
00187     dst.setData(newMat);
00188 }
00189 
00190 Matrix4 Quaternion::toMatrix() const {
00191     Matrix4 dst;
00192     toMatrix(dst);
00193     return dst;
00194 }
00195 
00196 Quaternion Quaternion::inverse() const {
00197     Quaternion q = *this;
00198     q.normalize();
00199     q.x() = -q.x();
00200     q.y() = -q.y();
00201     q.z() = -q.z();
00202     return q;
00203 }
00204 
00205 Vector3d Quaternion::directionVector(const int col) const {
00206     Matrix4 dirMat = toMatrix();
00207     return Vector3d(dirMat.getValue(0, col), dirMat.getValue(1, col), dirMat.getValue(2, col));
00208 }
00209 
00210 Angle Quaternion::getAngleBetween(const Quaternion &to) {
00211     Quaternion q = this->inverse() * to;
00212     Angle diff(Common::rad2deg(2 * acos(q.w())));
00213     return diff;
00214 }
00215 
00216 Quaternion Quaternion::fromEuler(const Angle &first, const Angle &second, const Angle &third, EulerOrder order) {
00217     // First create a matrix with the rotation
00218     Matrix4 rot(first, second, third, order);
00219 
00220     // Convert this rotation matrix to a Quaternion
00221     return Quaternion(rot);
00222 }
00223 
00224 void Quaternion::getEuler(Angle *first, Angle *second, Angle *third, EulerOrder order) const {
00225     // Create a matrix from the Quaternion
00226     Matrix4 rot = toMatrix();
00227 
00228     // Convert the matrix to Euler Angles
00229     Angle f, s, t;
00230     rot.getEuler(&f, &s, &t, order);
00231 
00232     // Assign the Angles if we have a reference
00233     if (first != nullptr)
00234         *first = f;
00235     if (second != nullptr)
00236         *second = s;
00237     if (third != nullptr)
00238         *third = t;
00239 }
00240 
00241 Quaternion Quaternion::operator*(const Quaternion &o) const {
00242     return Quaternion(
00243         w() * o.x() + x() * o.w() + y() * o.z() - z() * o.y(),
00244         w() * o.y() - x() * o.z() + y() * o.w() + z() * o.x(),
00245         w() * o.z() + x() * o.y() - y() * o.x() + z() * o.w(),
00246         w() * o.w() - x() * o.x() - y() * o.y() - z() * o.z()
00247     );
00248 }
00249 
00250 Quaternion Quaternion::operator*(const float c) const {
00251     return Quaternion(x() * c, y() * c, z() * c, w() * c);
00252 }
00253 
00254 Quaternion& Quaternion::operator*=(const Quaternion &o) {
00255     *this = *this * o;
00256     return *this;
00257 }
00258 
00259 Quaternion Quaternion::operator+(const Quaternion &o) const {
00260     return Quaternion(x() + o.x(), y() + o.y(), z() + o.z(), w() + o.w());
00261 }
00262 
00263 Quaternion& Quaternion::operator+=(const Quaternion &o) {
00264     *this = *this + o;
00265     return *this;
00266 }
00267 
00268 bool Quaternion::operator==(const Quaternion &o) const {
00269     float dw = fabs(w() - o.w());
00270     float dx = fabs(x() - o.x());
00271     float dy = fabs(y() - o.y());
00272     float dz = fabs(z() - o.z());
00273     // Threshold of equality
00274     float th = 1E-5f;
00275 
00276     if ((dw < th) && (dx < th) && (dy < th) && (dz < th)) {
00277         return true;
00278     }
00279     return false;
00280 }
00281 
00282 bool Quaternion::operator!=(const Quaternion &o) const {
00283     return !(*this == o);
00284 }
00285 
00286 } // End namespace Math


Generated on Sat May 25 2019 05:00:51 for ResidualVM by doxygen 1.7.1
curved edge   curved edge