Back to index

nux  3.0.0
Quaternion.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 Inalogic® Inc.
00003  *
00004  * This program is free software: you can redistribute it and/or modify it
00005  * under the terms of the GNU Lesser General Public License, as
00006  * published by the  Free Software Foundation; either version 2.1 or 3.0
00007  * of the License.
00008  *
00009  * This program is distributed in the hope that it will be useful, but
00010  * WITHOUT ANY WARRANTY; without even the implied warranties of
00011  * MERCHANTABILITY, SATISFACTORY QUALITY or FITNESS FOR A PARTICULAR
00012  * PURPOSE.  See the applicable version of the GNU Lesser General Public
00013  * License for more details.
00014  *
00015  * You should have received a copy of both the GNU Lesser General Public
00016  * License along with this program. If not, see <http://www.gnu.org/licenses/>
00017  *
00018  * Authored by: Jay Taoko <jaytaoko@inalogic.com>
00019  *
00020  */
00021 
00022 
00023 #include "../NuxCore.h"
00024 #include "Matrix4.h"
00025 #include "Vector3.h"
00026 #include "Quaternion.h"
00027 
00028 // When writing to a matrix at row r and column c use m[r][c].
00029 // When reading from a matrix at row r and column c use m[c][r].
00030 
00031 namespace nux
00032 {
00033 
00034   Quaternion::Quaternion()
00035   {
00036     x = y = z = w = 0.0f;
00037   }
00038 
00039   Quaternion::Quaternion (const Quaternion &quat)
00040   {
00041     x = quat.x;
00042     y = quat.y;
00043     z = quat.z;
00044     w = quat.w;
00045   }
00046 
00047   Quaternion::Quaternion (const Vector3 &vec, float angle)
00048   {
00049     FromAngleAxis (vec.x, vec.y, vec.z, angle);
00050   }
00051 
00052   Quaternion::Quaternion (const Vector4 &vec)
00053   {
00054     FromAngleAxis (vec.w, vec.x, vec.y, vec.z);
00055   }
00056 
00057   Quaternion::Quaternion (float axis_x, float axis_y, float axis_z, float angle_radian)
00058   {
00059     FromAngleAxis (axis_x, axis_y, axis_z, angle_radian);
00060   }
00061 
00062   Quaternion::Quaternion (float euler_x, float euler_y, float euler_z)
00063   {
00064     FromEulerZXY (euler_x, euler_y, euler_z);
00065   }
00066 
00067   Quaternion::~Quaternion()
00068   {
00069   }
00070 
00071   Quaternion &Quaternion::operator = (const Quaternion &quat)
00072   {
00073     x = quat.x;
00074     y = quat.y;
00075     z = quat.z;
00076     w = quat.w;
00077 
00078     return (*this);
00079   }
00080 
00081   Quaternion Quaternion::operator + (const Quaternion &quat) const
00082   {
00083     Quaternion qt;
00084     qt.x = x + quat.x;
00085     qt.y = y + quat.y;
00086     qt.z = z + quat.z;
00087     qt.w = w + quat.w;
00088     return qt;
00089   }
00090 
00091   Quaternion Quaternion::operator - (const Quaternion &quat) const
00092   {
00093     Quaternion qt;
00094     qt.x = x - quat.x;
00095     qt.y = y - quat.y;
00096     qt.z = z - quat.z;
00097     qt.w = w - quat.w;
00098     return qt;
00099   }
00100 
00101   Quaternion Quaternion::operator * (const Quaternion &quat) const
00102   {
00103     Quaternion qt;
00104     float x1, x2, y1, y2, z1, z2, w1, w2;
00105 
00106     x1 = x;
00107     y1 = y;
00108     z1 = z;
00109     w1 = w;
00110     x2 = quat.x;
00111     y2 = quat.y;
00112     z2 = quat.z;
00113     w2 = quat.w;
00114 
00115     qt.x = w1 * x2 + x1 * w2 - z1 * y2 + y1 * z2;
00116     qt.y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2;
00117     qt.z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2;
00118     qt.w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
00119 
00120     return qt;
00121   }
00122 
00123   Quaternion Quaternion::operator * (const float &f) const
00124   {
00125     Quaternion qt;
00126     qt.x = x * f;
00127     qt.y = y * f;
00128     qt.z = z * f;
00129     qt.w = w * f;
00130     return qt;
00131   }
00132 
00133   Quaternion Quaternion::operator / (const float &f) const
00134   {
00135     Quaternion qt;
00136     qt.x = x / f;
00137     qt.y = y / f;
00138     qt.z = z / f;
00139     qt.w = w / f;
00140     return qt;
00141   }
00142 
00144   Quaternion &Quaternion::operator += (const Quaternion &quat)
00145   {
00146     x = x + quat.x;
00147     y = y + quat.y;
00148     z = z + quat.z;
00149     w = w + quat.w;
00150     return *this;
00151   }
00152 
00153   Quaternion &Quaternion::operator -= (const Quaternion &quat)
00154   {
00155     x = x - quat.x;
00156     y = y - quat.y;
00157     z = z - quat.z;
00158     w = w - quat.w;
00159     return *this;
00160   }
00161 
00162   Quaternion &Quaternion::operator *= (const Quaternion &quat)
00163   {
00164     Quaternion qt;
00165     float x1, x2, y1, y2, z1, z2, w1, w2;
00166 
00167     x1 = x;
00168     y1 = y;
00169     z1 = z;
00170     w1 = w;
00171     x2 = quat.x;
00172     y2 = quat.y;
00173     z2 = quat.z;
00174     w2 = quat.w;
00175 
00176     qt.x = w1 * x2 + x1 * w2 - z1 * y2 + y1 * z2;
00177     qt.y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2;
00178     qt.z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2;
00179     qt.w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
00180 
00181     x = qt.x;
00182     y = qt.y;
00183     z = qt.z;
00184     w = qt.w;
00185     return *this;
00186   }
00187 
00188   Quaternion &Quaternion::operator *= (const float &f)
00189   {
00190     x = x * f;
00191     y = y * f;
00192     z = z * f;
00193     w = w * f;
00194     return *this;
00195   }
00196 
00197   Quaternion &Quaternion::operator /= (const float &f)
00198   {
00199     x = x / f;
00200     y = y / f;
00201     z = z / f;
00202     w = w / f;
00203     return *this;
00204   }
00205 
00206 
00207   Quaternion Quaternion::operator + () const
00208   {
00209     Quaternion qt;
00210     qt.x = x;
00211     qt.y = y;
00212     qt.z = z;
00213     qt.w = w;
00214     return qt;
00215   }
00216 
00217   Quaternion Quaternion::operator - () const
00218   {
00219     Quaternion qt;
00220     qt.x = -x;
00221     qt.y = -y;
00222     qt.z = -z;
00223     qt.w = -w;
00224     return qt;
00225   }
00226 
00227   bool Quaternion::operator == (const Quaternion &quat) const
00228   {
00229     if ( (x == quat.x) &&
00230          (y == quat.y) &&
00231          (z == quat.z) &&
00232          (w == quat.w) )
00233     {
00234       return true;
00235     }
00236 
00237     return true;
00238   }
00239 
00240   bool Quaternion::operator != ( const Quaternion &quat) const
00241   {
00242     return ! ( (*this) == quat);
00243   }
00244 
00245   void Quaternion::Conjugate()
00246   {
00247     x = -x;
00248     y = -y;
00249     z = -z;
00250   }
00251 
00252   void Quaternion::Inverse()
00253   {
00254     float len;
00255     len = (float) std::sqrt (x * x + y * y + z * z + w * w);
00256     Conjugate();
00257     x = x / len;
00258     y = y / len;
00259     z = z / len;
00260     w = w / len;
00261   }
00262 
00263   void Quaternion::Normalize()
00264   {
00265     float len;
00266     len = (float) std::sqrt (x * x + y * y + z * z + w * w);
00267     x = x / len;
00268     y = y / len;
00269     z = z / len;
00270     w = w / len;
00271 
00272   }
00273 
00274   /***************************************************************************************\
00275   Function:       Quaternion::dot_product
00276 
00277   Description:    Compute the dot product of *this and the input quaternion vector
00278 
00279   Parameters:     - quat
00280 
00281   Return Value:   float.
00282 
00283   Comments:       None.
00284   \***************************************************************************************/
00285   float Quaternion::DotProduct (const Quaternion &quat) const
00286   {
00287     float d_p;
00288     d_p = x * quat.x + y * quat.y + z * quat.z + w * quat.w;
00289     return d_p;
00290   }
00291 
00292   /***************************************************************************************\
00293   Function:       Quaternion::length
00294 
00295   Description:    Compute the length of a quaternion vector
00296 
00297   Parameters:     None.
00298 
00299   Return Value:   float
00300 
00301   Comments:       None.
00302   \***************************************************************************************/
00303   float Quaternion::Length() const
00304   {
00305     float len;
00306     len = (float) std::sqrt (x * x + y * y + z * z + w * w);
00307     return len;
00308   }
00309 
00310   /***************************************************************************************\
00311   Function:       Quaternion::FromAngleAxis
00312 
00313   Description:    Set the quaternion to define a rotation of angle_randian around the
00314                   vector define by (axis_x, axis_y, axis_z)
00315 
00316   Parameters:     - angle_radian: angle in radian
00317                   - axis_x
00318                   - axis_y
00319                   - axis_z
00320 
00321   Return Value:   None.
00322 
00323   Comments:       None.
00324   \***************************************************************************************/
00325   void Quaternion::FromAngleAxis (float axis_x, float axis_y, float axis_z, float angle_radian)
00326   {
00327     float ax = axis_x;
00328     float ay = axis_y;
00329     float az = axis_z;
00330 
00331     // required: Normalize the axis
00332 
00333     float len = 1.0f / (float) std::sqrt ( ax * ax + ay * ay + az * az );
00334 
00335     ax = ax * len;
00336     ay = ay * len;
00337     az = az * len;
00338 
00339     float sin_theta_over_two = (float) std::sin ( angle_radian / 2.0);
00340     x = ax * sin_theta_over_two;
00341     y = ay * sin_theta_over_two;
00342     z = az * sin_theta_over_two;
00343     w = (float) std::cos (angle_radian / 2.0);
00344   }
00345 
00346   /***************************************************************************************\
00347   Function:       Quaternion::FromEulerZXY
00348 
00349   Description:    Set the quaternion to define a Euler transform
00350 
00351   Parameters:     - euler_x: rotation angle around X axis (in radian)
00352                   - euler_y: rotation angle around Y axis (in radian)
00353                   - euler_z: rotation angle around Z axis (in radian)
00354 
00355   Return Value:   None.
00356 
00357   Comments:       The rotation are applied in the following order:
00358                       rotation around Y axis
00359                       rotation around X axis
00360                       rotation around Z axis
00361   \***************************************************************************************/
00362   void Quaternion::FromEulerZXY (float euler_x, float euler_y, float euler_z)
00363   {
00364     float roll_axis[3]  = {0.0f, 0.0f, 1.0f};
00365     float pitch_axis[3] = {1.0f, 0.0f, 0.0f};
00366     float yaw_axis[3]   = {0.0f, 1.0f, 0.0f};
00367 
00368     Quaternion roll (euler_z, roll_axis[0], roll_axis[1], roll_axis[2]);
00369     Quaternion pitch (euler_x, pitch_axis[0], pitch_axis[1], pitch_axis[2]);
00370     Quaternion yaw (euler_y, yaw_axis[0], yaw_axis[1], yaw_axis[2]);
00371 
00372     (*this) = roll * pitch * yaw;
00373   }
00374 
00375   void Quaternion::GetAngleAxis (Vector3 &axis, float &angle_radian) const
00376   {
00377     Quaternion qt;
00378 
00379     qt.x = x;
00380     qt.y = y;
00381     qt.z = z;
00382     qt.w = w;
00383 
00384     // make a unit quaternion
00385     qt.Normalize();                                 // qt = sin(angle/2)*Uq + cos(angle/2)
00386     angle_radian = 2.0f * (float) std::acos (qt.w);
00387     float one_over_sin = 1.0f / (float) std::sqrt (qt.x * qt.x + qt.y * qt.y + qt.z * qt.z);
00388     //float one_over_sin = 1.0f / (float) sin(angle_radian / 2.0f);
00389 
00390     axis.x = qt.x * one_over_sin;
00391     axis.y = qt.y * one_over_sin;
00392     axis.z = qt.z * one_over_sin;
00393   }
00394 
00395   /***************************************************************************************\
00396   Function:       Quaternion::get_matrix
00397 
00398   Description:    Return a Matrix4 object containing the rotation defined by the
00399                   quaternion.
00400 
00401   Parameters:     None.
00402 
00403   Return Value:   Matrix4.
00404 
00405   Comments:       None.
00406   \***************************************************************************************/
00407   Matrix4 Quaternion::GetMatrix() const
00408   {
00409     Matrix4 mat4;
00410     /*float x = x;
00411     float y = y;
00412     float z = z;
00413     float w = w;*/
00414     float s;
00415 
00416     s = 2.0f / ( (float) std::sqrt (x * x + y * y + z * z + w * w) );
00417 
00418     mat4.m[0][0]  = 1.0f - s * (y * y + z * z);
00419     mat4.m[0][1]  = s * (x * y - w * z);
00420     mat4.m[0][2]  = s * (x * z + w * y);
00421     mat4.m[0][3]  = 0.0f;
00422 
00423     mat4.m[1][0]  = s * (x * y + w * z);
00424     mat4.m[1][1]  = 1.0f - s * (x * x + z * z);
00425     mat4.m[1][2]  = s * (y * z - w * x);
00426     mat4.m[1][3]  = 0.0f;
00427 
00428     mat4.m[2][0]  = s * (x * z - w * y);
00429     mat4.m[2][1]  = s * (y * z + w * x);
00430     mat4.m[2][2] = 1.0f - s * (x * x + y * y);
00431     mat4.m[2][3] = 0.0f;
00432 
00433     mat4.m[3][0] = 0.0f;
00434     mat4.m[3][1] = 0.0f;
00435     mat4.m[3][2] = 0.0f;
00436     mat4.m[3][3] = 1.0f;
00437 
00438     return mat4;
00439   }
00440 
00441   /*const Quaternion operator * (const Quaternion& quat1, const Quaternion& quat2)
00442   {
00443       Quaternion qt;
00444       float x1, x2, y1, y2, z1, z2, w1, w2;
00445 
00446       x1 = quat1.x;
00447       y1 = quat1.y;
00448       z1 = quat1.z;
00449       w1 = quat1.w;
00450 
00451       x2 = quat2.x;
00452       y2 = quat2.y;
00453       z2 = quat2.z;
00454       w2 = quat2.w;
00455 
00456       qt.x = w1*x2 + x1*w2 - z1*y2 + y1*z2;
00457       qt.y = w1*y2 + y1*w2 + z1*x2 - x1*z2;
00458       qt.z = w1*z2 + z1*w2 + x1*y2 - y1*x2;
00459       qt.w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
00460 
00461       return qt;
00462   }*/
00463 
00464   Quaternion operator * (float f, const Quaternion &quat)
00465   {
00466     Quaternion qt;
00467     qt.x = quat.x * f;
00468     qt.y = quat.y * f;
00469     qt.z = quat.z * f;
00470     qt.w = quat.w * f;
00471     return qt;
00472   }
00473 
00474   Quaternion Slerp (const float t, const Quaternion &lhs, const Quaternion &rhs)
00475   {
00476     // the slerp of a pair of unit quaterions is the weighted
00477     // interpolation between them, where the interpolation weight is
00478     // given by t = [0, 1.0]
00479     //
00480     // the trick to slerping is that we find the angle between the two
00481     // quats by treating them as a pair of four vectors and getting the
00482     // cosine [as the dot product].
00483     //
00484     // then the slerp between two quaternions A and B is:
00485     //
00486     //       A * (upper_weight) + B * (lower_weight)
00487     //
00488     // where the weights are the sines of the t-weighted angle
00489     // divided by the sine of the angle.
00490     //
00491     // the resulting quaternion is also a unit quaternion.
00492 
00493 
00494     // find the angle between the two quats by treating
00495     // them as 4-length vectors -- V1.V2 = cos(theta)
00496 
00497 
00498     float cosine_angle, angle_over_two;
00499     float coef1, coef2;
00500     Quaternion qt;
00501     Quaternion lhs_n, rhs_n;
00502 
00503     lhs_n = lhs;
00504     rhs_n = rhs;
00505     lhs_n.Normalize();
00506     rhs_n.Normalize();
00507 
00508     cosine_angle = lhs_n.DotProduct (rhs_n);  // = cos(angle_over_two)
00509 
00510     // adjust signs (if necessary)
00511     if ( cosine_angle < 0.0 )
00512     {
00513       cosine_angle = -cosine_angle;
00514       rhs_n = - rhs_n;
00515     }
00516 
00517     angle_over_two = (float) std::acos (cosine_angle);
00518 
00519     if ( (1 - cosine_angle) > 0.000001)
00520     {
00521       coef1 = (float) std::sin (angle_over_two * (1 - t) ) / (float) std::sin (angle_over_two);
00522       coef2 = (float) std::sin (angle_over_two * t)      / (float) std::sin (angle_over_two);
00523     }
00524     else
00525     {
00526       // lhs and rhs are very close ... so we can do a linear interpolation
00527       coef1 = 1 - t;
00528       coef2 = t;
00529     }
00530 
00531     qt = coef1 * lhs_n + coef2 * rhs_n;
00532     return qt;
00533   }
00534 
00535 }