| commit | author | age | ||
| 69b66f | 1 | /************************************************************************ |
| JS | 2 | |
| 3 | POLHEMUS PROPRIETARY | |
| 4 | ||
| 5 | Polhemus | |
| 6 | P.O. Box 560 | |
| 7 | Colchester, Vermont 05446 | |
| 8 | (802) 655-3159 | |
| 9 | ||
| 10 | ||
| 11 | ||
| 12 | ||
| 13 | Copyright © 2005 by Polhemus | |
| 14 | All Rights Reserved. | |
| 15 | ||
| 16 | ||
| 17 | *************************************************************************/ | |
| 18 | ||
| 19 | // Quaternion.cpp: implementation of the CQuaternion class. | |
| 20 | // | |
| 21 | ////////////////////////////////////////////////////////////////////// | |
| 22 | ||
| 23 | #include "Quaternion.h" | |
| 24 | #include <math.h> | |
| 25 | ||
| 26 | ||
| 27 | ////////////////////////////////////////////////////////////////////// | |
| 28 | // Construction/Destruction | |
| 29 | ////////////////////////////////////////////////////////////////////// | |
| 30 | ||
| 31 | const float PI=3.14159265f; | |
| 32 | const float DEG2RADS=PI/180.0f; | |
| 33 | ||
| 34 | ||
| 35 | ||
| 36 | // Function name : CQuaternion::CQuaternion | |
| 37 | // Description : Default Constructor -- Orientaton 0,0,0 | |
| 38 | // Return type : | |
| 39 | CQuaternion::CQuaternion() | |
| 40 | { | |
| 41 | q0=1.0f; | |
| 42 | q1=q2=q3=0.0f; | |
| 43 | } | |
| 44 | ||
| 45 | CQuaternion::~CQuaternion() | |
| 46 | { | |
| 47 | ||
| 48 | } | |
| 49 | ||
| 50 | ||
| 51 | // Function name : CQuaternion::CQuaternion | |
| 52 | // Description : Constructs class from array of four floats | |
| 53 | // Argument : float q[] | |
| 54 | CQuaternion::CQuaternion(const float q[]) | |
| 55 | { | |
| 56 | q0=q[0]; | |
| 57 | q1=q[1]; | |
| 58 | q2=q[2]; | |
| 59 | q3=q[3]; | |
| 60 | Normalize(); | |
| 61 | ||
| 62 | } | |
| 63 | ||
| 64 | ||
| 65 | // Function name : CQuaternion::CQuaternion | |
| 66 | // Description : Constructs class from 4 individual floats | |
| 67 | // Argument : float quat0 | |
| 68 | // Argument : float quat1 | |
| 69 | // Argument : float quat2 | |
| 70 | // Argument : float quat3 | |
| 71 | CQuaternion::CQuaternion(float quat0, float quat1, float quat2, float quat3) | |
| 72 | { | |
| 73 | q0=quat0; | |
| 74 | q1=quat1; | |
| 75 | q2=quat2; | |
| 76 | q3=quat3; | |
| 77 | Normalize(); | |
| 78 | } | |
| 79 | ||
| 80 | ||
| 81 | // Function name : CQuaternion::CQuaternion | |
| 82 | // Description : Constructs class from az,el,roll in either deg or radians | |
| 83 | // Argument : float az | |
| 84 | // Argument : float el | |
| 85 | // Argument : float rl | |
| 86 | // Argument : bool deg - bool to indicate that angles are in degrees. Default is true | |
| 87 | CQuaternion::CQuaternion(float az, float el, float rl,bool deg) | |
| 88 | { | |
| 89 | float eul[3]={az,el,rl}; | |
| 90 | SetFromEulers(eul,deg); | |
| 91 | ||
| 92 | } | |
| 93 | ||
| 94 | ||
| 95 | // Function name : CQuaternion::CQuaternion | |
| 96 | // Description : Constructs class from an axis and an angle of rotation about that axis | |
| 97 | // Argument : float vect[] - 3 values representing the vector which is the axis of rotation | |
| 98 | // Argument : float angle - The angle to rotate about vect. Degrees or Radians | |
| 99 | // Argument : bool deg - bool to indicate that angles are in degrees. Default is true | |
| 100 | CQuaternion::CQuaternion(const float vect[], float angle, bool deg) | |
| 101 | { | |
| 102 | if (deg) | |
| 103 | angle*=DEG2RADS; | |
| 104 | ||
| 105 | float halfAng=angle/2.0f; | |
| 106 | ||
| 107 | q0=(float)cos(halfAng); | |
| 108 | q1=vect[0]*(float)sin(halfAng); | |
| 109 | q2=vect[1]*(float)sin(halfAng); | |
| 110 | q3=vect[2]*(float)sin(halfAng); | |
| 111 | ||
| 112 | Normalize(); | |
| 113 | } | |
| 114 | ||
| 115 | ||
| 116 | ||
| 117 | // Function name : CQuaternion::GetEuler | |
| 118 | // Description : Returns the Euler angles associated with this quaternion in deg or rads | |
| 119 | // Return type : void | |
| 120 | // Argument : float& az - azimuth returned here. | |
| 121 | // Argument : float& el - elevation returned here. | |
| 122 | // Argument : float& rl - roll returned here. | |
| 123 | // Argument : bool deg - If true returned angles are in degrees, otherwise radians. Default is true | |
| 124 | void CQuaternion::GetEuler(float& az, float& el, float& rl,bool deg) const | |
| 125 | { | |
| 126 | float mat[3][3]; | |
| 127 | float sinAz,sinEl,sinRl,cosAz,cosEl,cosRl; | |
| 128 | ||
| 129 | // create orthogonal Attitude matrix | |
| 130 | GetAttMat(mat); | |
| 131 | ||
| 132 | sinEl=-mat[2][0]; | |
| 133 | cosEl=(float)sqrt(1.0f-(sinEl*sinEl)); | |
| 134 | ||
| 135 | if (fabs(cosEl)<0.001f){ | |
| 136 | sinAz=0.0f; | |
| 137 | cosAz=1.0f; | |
| 138 | } | |
| 139 | ||
| 140 | else { | |
| 141 | sinAz=mat[1][0]/cosEl; | |
| 142 | cosAz=mat[0][0]/cosEl; | |
| 143 | } | |
| 144 | ||
| 145 | sinRl=sinAz*mat[0][2]-cosAz*mat[1][2]; | |
| 146 | cosRl=-sinAz*mat[0][1]+cosAz*mat[1][1]; | |
| 147 | ||
| 148 | az=(float)atan2((double)sinAz,(double)cosAz); | |
| 149 | el=(float)atan2((double)sinEl,(double)cosEl); | |
| 150 | rl=(float)atan2((double)sinRl,(double)cosRl); | |
| 151 | ||
| 152 | if (deg){ // convert to degrees | |
| 153 | az/=DEG2RADS; | |
| 154 | el/=DEG2RADS; | |
| 155 | rl/=DEG2RADS; | |
| 156 | } | |
| 157 | ||
| 158 | } | |
| 159 | ||
| 160 | ||
| 161 | // Function name : CQuaternion::GetEuler | |
| 162 | // Description : Returns the Euler angles associated with this quaternion in deg or rads | |
| 163 | // Return type : void | |
| 164 | // Argument : float aer[] - array of 3 floats where az,el,roll are returned. | |
| 165 | // Argument : bool deg - If true returned angles are in degrees, otherwise radians. Default is true | |
| 166 | void CQuaternion::GetEuler(float aer[],bool deg) const | |
| 167 | { | |
| 168 | GetEuler(aer[0],aer[1],aer[2],deg); | |
| 169 | } | |
| 170 | ||
| 171 | ||
| 172 | // Function name : CQuaternion::Normalize | |
| 173 | // Description : normalizes the quat to a unit quaternion | |
| 174 | // Return type : void | |
| 175 | void CQuaternion::Normalize() | |
| 176 | { | |
| 177 | float mag=(float)sqrt(q0*q0+q1*q1+q2*q2+q3*q3); | |
| 178 | ||
| 179 | if (q0<0.0f) | |
| 180 | mag*=-1.0f; // make first entry pos | |
| 181 | ||
| 182 | q0/=mag; | |
| 183 | q1/=mag; | |
| 184 | q2/=mag; | |
| 185 | q3/=mag; | |
| 186 | ||
| 187 | } | |
| 188 | ||
| 189 | ||
| 190 | // Function name : CQuaternion::GetAttMat | |
| 191 | // Description : Returns the attitude matrix associated with this quaternion | |
| 192 | // Return type : void | |
| 193 | // Argument : float mat[][3] - 3x3 float array where att matrix is returned. | |
| 194 | void CQuaternion::GetAttMat(float mat[][3]) const | |
| 195 | { | |
| 196 | // create orthogonal Attitude matrix | |
| 197 | mat[0][0]=q0*q0+q1*q1-q2*q2-q3*q3; | |
| 198 | mat[0][1]=2*(q1*q2-q0*q3); | |
| 199 | mat[0][2]=2*(q1*q3+q0*q2); | |
| 200 | mat[1][0]=2*(q0*q3+q1*q2); | |
| 201 | mat[1][1]=q0*q0-q1*q1+q2*q2-q3*q3; | |
| 202 | mat[1][2]=2*(q2*q3-q0*q1); | |
| 203 | mat[2][0]=2*(q1*q3-q0*q2); | |
| 204 | mat[2][1]=2*(q0*q1+q2*q3); | |
| 205 | mat[2][2]=q0*q0-q1*q1-q2*q2+q3*q3; | |
| 206 | ||
| 207 | } | |
| 208 | ||
| 209 | ||
| 210 | ||
| 211 | ||
| 212 | ||
| 213 | ||
| 214 | // Function name : * | |
| 215 | // Description : multiplies two quaternion. | |
| 216 | // Return type : CQuaternion - The product of the multiplication. | |
| 217 | // Argument : const CQuaternion& quat2 - The quat to mult this quat by. | |
| 218 | CQuaternion CQuaternion::operator *(const CQuaternion& quat2) const | |
| 219 | { | |
| 220 | ||
| 221 | CQuaternion prod; | |
| 222 | ||
| 223 | prod.q0 = q0*quat2.q0 - q1*quat2.q1 - q2*quat2.q2 - q3*quat2.q3; | |
| 224 | prod.q1 = q0*quat2.q1 + q1*quat2.q0 + q2*quat2.q3 - q3*quat2.q2; | |
| 225 | prod.q2 = q0*quat2.q2 - q1*quat2.q3 + q2*quat2.q0 + q3*quat2.q1; | |
| 226 | prod.q3 = q0*quat2.q3 + q1*quat2.q2 - q2*quat2.q1 + q3*quat2.q0; | |
| 227 | ||
| 228 | prod.Normalize(); | |
| 229 | return prod; | |
| 230 | } | |
| 231 | ||
| 232 | // Function name : * | |
| 233 | // Description : Multiplies quaternion by a scaler | |
| 234 | // Return type : CQuaternion -- the product | |
| 235 | // Argument : const float scaler | |
| 236 | CQuaternion CQuaternion::operator *(const float scaler) const | |
| 237 | { | |
| 238 | CQuaternion prod; | |
| 239 | prod.q0=q0*scaler; | |
| 240 | prod.q1=q1*scaler; | |
| 241 | prod.q2=q2*scaler; | |
| 242 | prod.q3=q3*scaler; | |
| 243 | ||
| 244 | return prod; | |
| 245 | } | |
| 246 | ||
| 247 | ||
| 248 | ||
| 249 | // Function name : *= | |
| 250 | // Description : Multiply and assign eg. thisQuat*=thatQuat--->thisQuat=thisQuat*thatQuat | |
| 251 | // Return type : void | |
| 252 | // Argument : const CQuaternion &quat - The quat to mult this quat by. | |
| 253 | void CQuaternion::operator *=(const CQuaternion &quat) | |
| 254 | { | |
| 255 | CQuaternion prod=*this * quat; | |
| 256 | *this=prod; | |
| 257 | } | |
| 258 | ||
| 259 | ||
| 260 | // Function name : CQuaternion::GetAxisAngle | |
| 261 | // Description : Returns the axis and angle of rotation that is associated with this quaternion | |
| 262 | // Return type : void | |
| 263 | // Argument : float vect[] - array of 3 floats to receive vector representing the axis | |
| 264 | // Argument : float &angle - reference to received the angle. | |
| 265 | // Argument : bool deg - if true angle will be in degrees, otherwise radians. Default is true. | |
| 266 | void CQuaternion::GetAxisAngle(float vect[], float &angle, bool deg) const | |
| 267 | { | |
| 268 | angle=2.0f*(float)acos(q0); | |
| 269 | if (deg) | |
| 270 | angle/=DEG2RADS; | |
| 271 | ||
| 272 | float scale=(float)sqrt(q1*q1+q2*q2+q3*q3); | |
| 273 | ||
| 274 | if (fabs(scale)<0.001f){ // infinite axis, set to no rotation | |
| 275 | angle=0.0f; | |
| 276 | vect[0]=1.0f; | |
| 277 | vect[1]=vect[2]=0.0f; | |
| 278 | return; | |
| 279 | } | |
| 280 | ||
| 281 | vect[0]=q1/scale; | |
| 282 | vect[1]=q2/scale; | |
| 283 | vect[2]=q3/scale; | |
| 284 | ||
| 285 | scale=(float)sqrt(vect[0]*vect[0]+vect[1]*vect[1]+vect[2]*vect[2]); | |
| 286 | for (int i=0;i<3;i++) | |
| 287 | vect[i]/=scale; | |
| 288 | } | |
| 289 | ||
| 290 | ||
| 291 | // Function name : - | |
| 292 | // Description : Returns inverse of quaternion. eg -thisQuat --> InvthisQuat | |
| 293 | // Return type : CQuaternion - The inverse of this quaternion | |
| 294 | CQuaternion CQuaternion::operator -() const | |
| 295 | { | |
| 296 | CQuaternion inv=*this; | |
| 297 | inv.q1*=-1; | |
| 298 | inv.q2*=-1; | |
| 299 | inv.q3*=-1; | |
| 300 | return inv; | |
| 301 | } | |
| 302 | ||
| 303 | ||
| 304 | ||
| 305 | // Function name : CQuaternion::GetDeltaQuat | |
| 306 | // Description : Returns the quaternion that represents the difference between two quaternions | |
| 307 | // Return type : CQuaternion - the delta quaternion | |
| 308 | // Argument : const CQuaternion &quat - reference to a quaternion to measure the difference between | |
| 309 | CQuaternion CQuaternion::GetDeltaQuat(const CQuaternion &quat) const | |
| 310 | { | |
| 311 | /* CQuaternion inv=-(*this); | |
| 312 | return inv*quat; | |
| 313 | */ | |
| 314 | CQuaternion inv=-quat; | |
| 315 | return *this * inv; | |
| 316 | ||
| 317 | } | |
| 318 | ||
| 319 | ||
| 320 | // Function name : - | |
| 321 | // Description : Also retrieves the delta quat. eg. thisQuat-thatQuat --> GetDeltaQuat(thatQuat) | |
| 322 | // Yes this is really a multiplicaton process, but it's kind of intuitive. | |
| 323 | // Return type : CQuaternion CQuaternion::operator | |
| 324 | // Argument : const CQuaternion &quat | |
| 325 | CQuaternion CQuaternion::operator -(const CQuaternion &quat) const | |
| 326 | { | |
| 327 | return GetDeltaQuat(quat); | |
| 328 | } | |
| 329 | ||
| 330 | ||
| 331 | // Function name : CQuaternion::GetQuatVal | |
| 332 | // Description : returns the four values of the quaternion | |
| 333 | // Return type : void | |
| 334 | // Argument : float vals[] - array of 4 floats to recieve the quat values | |
| 335 | void CQuaternion::GetQuatVal(float vals[]) const | |
| 336 | { | |
| 337 | vals[0]=q0; | |
| 338 | vals[1]=q1; | |
| 339 | vals[2]=q2; | |
| 340 | vals[3]=q3; | |
| 341 | ||
| 342 | } | |
| 343 | ||
| 344 | ||
| 345 | // Function name : CQuaternion::GetQuatVal | |
| 346 | // Description : Returns an individual value of the quaternion | |
| 347 | // Return type : float - the requested value of the quaternion | |
| 348 | // Argument : int ind - the index (0-3) of the value to return. Invalid values return q0. | |
| 349 | float CQuaternion::GetQuatVal(int ind) const | |
| 350 | { | |
| 351 | float retVal; | |
| 352 | switch (ind) { | |
| 353 | case 0: | |
| 354 | default: | |
| 355 | retVal=q0; | |
| 356 | break; | |
| 357 | case 1: | |
| 358 | retVal=q1; | |
| 359 | break; | |
| 360 | case 2: | |
| 361 | retVal=q2; | |
| 362 | break; | |
| 363 | case 3: | |
| 364 | retVal=q3; | |
| 365 | } | |
| 366 | ||
| 367 | return retVal; | |
| 368 | } | |
| 369 | ||
| 370 | ||
| 371 | // Function name : CQuaternion::SetQuatVals | |
| 372 | // Description : Sets the values of the quaternion as indicated. | |
| 373 | // Return type : void | |
| 374 | // Argument : float val[] - Array of 4 floats that indicate the values to set the quaternion members to. | |
| 375 | void CQuaternion::SetQuatVals(const float val[]) | |
| 376 | { | |
| 377 | SetQuatVals(val[0],val[1],val[2],val[3]); | |
| 378 | } | |
| 379 | ||
| 380 | ||
| 381 | // Function name : CQuaternion::SetQuatVals | |
| 382 | // Description : Sets the values of the quaternion as indicated by the individual parameters | |
| 383 | // Return type : void | |
| 384 | // Argument : float w - set first quaternion value to this | |
| 385 | // Argument : float x - set second quaternion value to this | |
| 386 | // Argument : float y - set third quaternion value to this | |
| 387 | // Argument : float z - set fourth quaternion value to this | |
| 388 | void CQuaternion::SetQuatVals(const float w, const float x, const float y, const float z) | |
| 389 | { | |
| 390 | q0=w; | |
| 391 | q1=x; | |
| 392 | q2=y; | |
| 393 | q3=z; | |
| 394 | Normalize(); | |
| 395 | ||
| 396 | } | |
| 397 | ||
| 398 | bool CQuaternion::IsIdentity() | |
| 399 | { | |
| 400 | return ((q0==1.0f) && (q1==0.0f) && (q2==0.0f) && (q3==0.0f)); | |
| 401 | } | |
| 402 | ||
| 403 | void CQuaternion::Eul2Quat(float *quat, float *eul, bool isDeg) | |
| 404 | { | |
| 405 | CQuaternion q(eul[0],eul[1],eul[2],isDeg); | |
| 406 | q.MakeLargestElementPos(); | |
| 407 | q.GetQuatVal(quat); | |
| 408 | } | |
| 409 | ||
| 410 | void CQuaternion::Quats2Eul(float *eul, float *quats, bool isDeg) | |
| 411 | { | |
| 412 | CQuaternion q(quats); | |
| 413 | q.GetEuler(eul,isDeg); | |
| 414 | } | |
| 415 | ||
| 416 | void CQuaternion::MakeLargestElementPos() | |
| 417 | { | |
| 418 | float max=(float)fabs(q0); | |
| 419 | bool bChgSign=q0<0; | |
| 420 | ||
| 421 | ||
| 422 | if ((float)fabs(q1)>max){ | |
| 423 | max=(float)fabs(q1); | |
| 424 | bChgSign=q1<0; | |
| 425 | } | |
| 426 | ||
| 427 | if ((float)fabs(q2)>max){ | |
| 428 | max=(float)fabs(q2); | |
| 429 | bChgSign=q2<0; | |
| 430 | } | |
| 431 | ||
| 432 | if ((float)fabs(q3)>max){ | |
| 433 | max=(float)fabs(q3); | |
| 434 | bChgSign=q3<0; | |
| 435 | } | |
| 436 | ||
| 437 | if (bChgSign){ | |
| 438 | q0*=-1; | |
| 439 | q1*=-1; | |
| 440 | q2*=-1; | |
| 441 | q3*=-1; | |
| 442 | } | |
| 443 | } | |
| 444 | ||
| 445 | void CQuaternion::SetFromEulers(float *eul, bool deg/*=true*/) | |
| 446 | { | |
| 447 | float azHalf=eul[0]/2.0f; | |
| 448 | float elHalf=eul[1]/2.0f; | |
| 449 | float rollHalf=eul[2]/2.0f; | |
| 450 | ||
| 451 | if (deg){ | |
| 452 | azHalf*=DEG2RADS; | |
| 453 | elHalf*=DEG2RADS; | |
| 454 | rollHalf*=DEG2RADS; | |
| 455 | } | |
| 456 | ||
| 457 | q0=(float)(cos(azHalf)*cos(elHalf)*cos(rollHalf)+sin(azHalf)*sin(elHalf)*sin(rollHalf)); | |
| 458 | q1=(float)(cos(azHalf)*cos(elHalf)*sin(rollHalf)-sin(azHalf)*sin(elHalf)*cos(rollHalf)); | |
| 459 | q2=(float)(cos(azHalf)*sin(elHalf)*cos(rollHalf)+sin(azHalf)*cos(elHalf)*sin(rollHalf)); | |
| 460 | q3=(float)(sin(azHalf)*cos(elHalf)*cos(rollHalf)-cos(azHalf)*sin(elHalf)*sin(rollHalf)); | |
| 461 | ||
| 462 | ||
| 463 | Normalize(); | |
| 464 | ||
| 465 | } | |
| 466 | ||
| 467 | // Function name : CQuaternion::Slerp | |
| 468 | // Description : Spherical interpolation of quaternions. Interpolates between | |
| 469 | // this quaternion and the quaternion passed in as a parameter | |
| 470 | // Return type : CQuaternion -- The interpolated quaternion | |
| 471 | // Argument : const CQuaternion &q | |
| 472 | // Argument : float t -- value between 0 and 1 which indicates the distance between | |
| 473 | // quaternions to interpolate | |
| 474 | CQuaternion CQuaternion::Slerp(const CQuaternion &q, float t) const | |
| 475 | { | |
| 476 | ||
| 477 | // qa*sin((1-t)theta)+qb*sin(t*theta) | |
| 478 | // q= ---------------------------------- | |
| 479 | // sin(theta) | |
| 480 | ||
| 481 | // 2*theta = qa.q0*qb.q0+qa.q1*qb.q1+qa.q2*qb.q2+qa.q3*qb.q3 | |
| 482 | ||
| 483 | ||
| 484 | ||
| 485 | ||
| 486 | float angle; | |
| 487 | ||
| 488 | if (t>=1.0f) | |
| 489 | return CQuaternion(q); | |
| 490 | ||
| 491 | if (t<=0.0f) | |
| 492 | return CQuaternion(*this); | |
| 493 | ||
| 494 | ||
| 495 | float dot=q0*q.q0+q1*q.q1+q2*q.q2+q3*q.q3; | |
| 496 | angle=(float)(acos(dot))/2.0f; | |
| 497 | if (angle<0.0f) | |
| 498 | angle*=-1; | |
| 499 | ||
| 500 | float coeff1,coeff2; | |
| 501 | ||
| 502 | if (angle==0.0f){ | |
| 503 | coeff1=1-t; | |
| 504 | coeff2=t; | |
| 505 | } | |
| 506 | else { | |
| 507 | ||
| 508 | coeff1=(float)sin((1.0f-t)*angle)/(float)sin(angle); | |
| 509 | coeff2=(float)sin(t*angle)/(float)sin(angle); | |
| 510 | } | |
| 511 | ||
| 512 | CQuaternion res=*this*coeff1+q*coeff2; | |
| 513 | res.Normalize(); | |
| 514 | ||
| 515 | return res; | |
| 516 | } | |
| 517 | ||
| 518 | // Function name : + | |
| 519 | // Description : Adds to quaternions | |
| 520 | // Return type : CQuaternion -- the sum | |
| 521 | // Argument : const CQuaternion &q1 | |
| 522 | // Argument : const CQuaternion &q2 | |
| 523 | CQuaternion CQuaternion::operator +(const CQuaternion &q) const | |
| 524 | { | |
| 525 | CQuaternion sum; | |
| 526 | sum.q0=q0+q.q0; | |
| 527 | sum.q1=q1+q.q1; | |
| 528 | sum.q2=q2+q.q2; | |
| 529 | sum.q3=q3+q.q3; | |
| 530 | ||
| 531 | return sum; | |
| 532 | ||
| 533 | } | |
| 534 | ||
| 535 | ||
| 536 | ||
| 537 | ||
| 538 | void CQuaternion::GetAngle(float &a,bool deg) | |
| 539 | { | |
| 540 | a=2.0f*(float)acos(q0); | |
| 541 | if (deg) | |
| 542 | a/=DEG2RADS; | |
| 543 | ||
| 544 | } | |
