nux-1.16.0
|
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 }