00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056 #include "quaternion.h"
00057
00058 using namespace std;
00059
00060 #ifdef use_namespace
00061 namespace ROBOOP {
00062 using namespace NEWMAT;
00063 #endif
00064
00065
00066 static const char rcsid[] __UNUSED__ = "$Id: quaternion.cpp,v 1.6 2007/11/11 23:57:24 ejt Exp $";
00067
00068
00069 Quaternion::Quaternion()
00070
00071 {
00072 s_ = 1.0;
00073 v_ = ColumnVector(3);
00074 v_ = 0.0;
00075 }
00076
00077 Quaternion::Quaternion(const Quaternion & q)
00078
00079 {
00080 s_ = q.s_;
00081 v_ = q.v_;
00082 }
00083
00084 Quaternion::Quaternion(const Real angle, const ColumnVector & axis)
00085
00086 {
00087 if(axis.Nrows() != 3)
00088 {
00089 cerr << "Quaternion::Quaternion, size of axis != 3" << endl;
00090 exit(1);
00091 }
00092
00093
00094 Real norm_axis = sqrt(DotProduct(axis, axis));
00095
00096 if(norm_axis != 1)
00097 {
00098 cerr << "Quaternion::Quaternion(angle, axis), axis is not unit" << endl;
00099 cerr << "Make the axis unit." << endl;
00100 v_ = sin(angle/2) * axis/norm_axis;
00101 }
00102 else
00103 v_ = sin(angle/2) * axis;
00104
00105 s_ = cos(angle/2);
00106 }
00107
00108 Quaternion::Quaternion(const Real s_in, const Real v1, const Real v2,
00109 const Real v3)
00110
00111 {
00112 s_ = s_in;
00113 v_ = ColumnVector(3);
00114 v_(1) = v1;
00115 v_(2) = v2;
00116 v_(3) = v3;
00117 }
00118
00119 Quaternion::Quaternion(const Matrix & R)
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160 {
00161 if( (R.Nrows() == 3) && (R.Ncols() == 3) ||
00162 (R.Nrows() == 4) && (R.Ncols() == 4) )
00163 {
00164 Real tmp = fabs(R(1,1) + R(2,2) + R(3,3) + 1);
00165 s_ = 0.5*sqrt(tmp);
00166 if(v_.Nrows() != 3)
00167 v_ = ColumnVector(3);
00168
00169 if(s_ > EPSILON)
00170 {
00171 v_(1) = (R(3,2)-R(2,3))/(4*s_);
00172 v_(2) = (R(1,3)-R(3,1))/(4*s_);
00173 v_(3) = (R(2,1)-R(1,2))/(4*s_);
00174 }
00175 else
00176 {
00177
00178 static int s_iNext[3] = { 2, 3, 1 };
00179 int i = 1;
00180 if ( R(2,2) > R(1,1) )
00181 i = 2;
00182 if ( R(3,3) > R(2,2) )
00183 i = 3;
00184 int j = s_iNext[i-1];
00185 int k = s_iNext[j-1];
00186
00187 Real fRoot = sqrt(R(i,i)-R(j,j)-R(k,k) + 1.0);
00188
00189 Real *tmp[3] = { &v_(1), &v_(2), &v_(3) };
00190 *tmp[i-1] = 0.5*fRoot;
00191 fRoot = 0.5/fRoot;
00192 s_ = (R(k,j)-R(j,k))*fRoot;
00193 *tmp[j-1] = (R(j,i)+R(i,j))*fRoot;
00194 *tmp[k-1] = (R(k,i)+R(i,k))*fRoot;
00195 }
00196
00197 }
00198 else
00199 cerr << "Quaternion::Quaternion: matrix input is not 3x3 or 4x4" << endl;
00200 }
00201
00202 Quaternion & Quaternion::operator=(const Quaternion & q)
00203
00204 {
00205 s_ = q.s_;
00206 v_ = q.v_;
00207
00208 return *this;
00209 }
00210
00211 Quaternion Quaternion::operator+(const Quaternion & rhs)const
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223 {
00224 Quaternion q;
00225 q.s_ = s_ + rhs.s_;
00226 q.v_ = v_ + rhs.v_;
00227
00228 return q;
00229 }
00230
00231 Quaternion Quaternion::operator-(const Quaternion & rhs)const
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 {
00244 Quaternion q;
00245 q.s_ = s_ - rhs.s_;
00246 q.v_ = v_ - rhs.v_;
00247
00248 return q;
00249 }
00250
00251 Quaternion Quaternion::operator*(const Quaternion & rhs)const
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266 {
00267 Quaternion q;
00268 q.s_ = s_ * rhs.s_ - DotProduct(v_, rhs.v_);
00269 q.v_ = s_ * rhs.v_ + rhs.s_ * v_ + CrossProduct(v_, rhs.v_);
00270
00271 return q;
00272 }
00273
00274 Quaternion Quaternion::operator*(const Real c)const
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284 {
00285 Quaternion q;
00286 q.s_ = s_ * c;
00287 q.v_ = v_ * c;
00288
00289 return q;
00290 }
00291
00292 Quaternion Quaternion::operator/(const Quaternion & rhs)const
00293
00294 {
00295
00296
00297
00298
00299
00300
00301 return *this*rhs.i();
00302 }
00303
00304 Quaternion Quaternion::operator/(const Real c)const
00305
00306
00307
00308
00309
00310 {
00311 Quaternion q;
00312 q.s_ = s_ / c;
00313 q.v_ = v_ / c;
00314
00315 return q;
00316 }
00317
00318 void Quaternion::set_v(const ColumnVector & v)
00319
00320 {
00321 if(v.Nrows() == 3)
00322 v_ = v;
00323 else
00324 cerr << "Quaternion::set_v: input has a wrong size." << endl;
00325 }
00326
00327 Quaternion Quaternion::conjugate()const
00328
00329
00330
00331
00332
00333
00334 {
00335 Quaternion q;
00336 q.s_ = s_;
00337 q.v_ = -1*v_;
00338
00339 return q;
00340 }
00341
00342 Real Quaternion::norm()const
00343
00344
00345
00346
00347
00348
00349
00350
00351 {
00352 return( sqrt(s_*s_ + DotProduct(v_, v_)) );
00353 }
00354
00355 Quaternion & Quaternion::unit()
00356
00357 {
00358 Real tmp = norm();
00359 if(tmp > EPSILON)
00360 {
00361 s_ = s_/tmp;
00362 v_ = v_/tmp;
00363 }
00364 return *this;
00365 }
00366
00367 Quaternion Quaternion::i()const
00368
00369
00370
00371
00372
00373
00374
00375
00376 {
00377 return conjugate()/norm();
00378 }
00379
00380 Quaternion Quaternion::exp() const
00381
00382
00383
00384
00385
00386
00387
00388 {
00389 Quaternion q;
00390 Real theta = sqrt(DotProduct(v_,v_)),
00391 sin_theta = sin(theta);
00392
00393 q.s_ = cos(theta);
00394 if ( fabs(sin_theta) > EPSILON)
00395 q.v_ = v_*sin_theta/theta;
00396 else
00397 q.v_ = v_;
00398
00399 return q;
00400 }
00401
00402 Quaternion Quaternion::power(const Real t) const
00403 {
00404 Quaternion q = (Log()*t).exp();
00405
00406 return q;
00407 }
00408
00409 Quaternion Quaternion::Log()const
00410
00411
00412
00413
00414
00415
00416
00417
00418 {
00419 Quaternion q;
00420 q.s_ = 0;
00421 Real theta = acos(s_),
00422 sin_theta = sin(theta);
00423
00424 if ( fabs(sin_theta) > EPSILON)
00425 q.v_ = v_/sin_theta*theta;
00426 else
00427 q.v_ = v_;
00428
00429 return q;
00430 }
00431
00432 Quaternion Quaternion::dot(const ColumnVector & w, const short sign)const
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459 {
00460 Quaternion q;
00461 Matrix tmp;
00462
00463 tmp = -0.5*v_.t()*w;
00464 q.s_ = tmp(1,1);
00465 q.v_ = 0.5*E(sign)*w;
00466
00467 return q;
00468 }
00469
00470 ReturnMatrix Quaternion::E(const short sign)const
00471
00472
00473
00474
00475
00476 {
00477 Matrix E(3,3), I(3,3);
00478 I << threebythreeident;
00479
00480 if(sign == BODY_FRAME)
00481 E = s_*I + x_prod_matrix(v_);
00482 else
00483 E = s_*I - x_prod_matrix(v_);
00484
00485 E.Release();
00486 return E;
00487 }
00488
00489 Real Quaternion::dot_prod(const Quaternion & q)const
00490
00491
00492
00493
00494
00495
00496
00497
00498 {
00499 return (s_*q.s_ + v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
00500 }
00501
00502 ReturnMatrix Quaternion::R()const
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534 {
00535 Matrix R(3,3);
00536 R << threebythreeident;
00537 R = (1 - 2*DotProduct(v_, v_))*R + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00538
00539 R.Release();
00540 return R;
00541 }
00542
00543 ReturnMatrix Quaternion::T()const
00544
00545
00546
00547
00548
00549 {
00550 Matrix T(4,4);
00551 T << fourbyfourident;
00552 T.SubMatrix(1,3,1,3) = (1 - 2*DotProduct(v_, v_))*T.SubMatrix(1,3,1,3)
00553 + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00554 T.Release();
00555 return T;
00556 }
00557
00558
00559
00560 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot)
00561
00562
00563
00564
00565
00566 {
00567 Matrix A, B, M;
00568 UpperTriangularMatrix U;
00569 ColumnVector w(3);
00570 A = 0.5*q.E(BASE_FRAME);
00571 B = q_dot.v();
00572 if(A.Determinant())
00573 {
00574 QRZ(A,U);
00575 QRZ(A,B,M);
00576 w = U.i()*M;
00577 }
00578 else
00579 w = 0;
00580
00581 w.Release();
00582 return w;
00583 }
00584
00585 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
00586 Quaternion & quat, const Real dt)
00587
00588 {
00589 if (dt < 0)
00590 {
00591 cerr << "Integ_Trap(quat1, quat2, dt): dt < 0. dt is set to 0." << endl;
00592 return -1;
00593 }
00594
00595
00596
00597
00598 dquat_present.set_s(dquat_present.s() );
00599 dquat_present.set_v(dquat_present.v() );
00600
00601 quat.set_s(quat.s() + Integ_Trap_quat_s(dquat_present, dquat_past, dt));
00602 quat.set_v(quat.v() + Integ_Trap_quat_v(dquat_present, dquat_past, dt));
00603
00604 dquat_past.set_s(dquat_present.s());
00605 dquat_past.set_v(dquat_present.v());
00606
00607 quat.unit();
00608
00609 return 0;
00610 }
00611
00612 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
00613 const Real dt)
00614
00615 {
00616 Real integ = 0.5*(present.s()+past.s())*dt;
00617 past.set_s(present.s());
00618 return integ;
00619 }
00620
00621 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
00622 const Real dt)
00623
00624 {
00625 ColumnVector integ = 0.5*(present.v()+past.v())*dt;
00626 past.set_v(present.v());
00627 integ.Release();
00628 return integ;
00629 }
00630
00631 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t)
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682 {
00683 if( (t < 0) || (t > 1) )
00684 cerr << "Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
00685
00686 if(q0.dot_prod(q1) >= 0)
00687 return q0*((q0.i()*q1).power(t));
00688 else
00689 return q0*((q0.i()*-1*q1).power(t));
00690 }
00691
00692 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1,
00693 const Real t)
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715 {
00716 if( (t < 0) || (t > 1) )
00717 cerr << "Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
00718
00719 if(q0.dot_prod(q1) >= 0)
00720 return Slerp(q0, q1, t)*(q0.i()*q1).Log();
00721 else
00722 return Slerp(q0, q1, t)*(q0.i()*-1*q1).Log();
00723 }
00724
00725 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00726 const Quaternion & q, const Real t)
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744 {
00745 if( (t < 0) || (t > 1) )
00746 cerr << "Squad(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
00747
00748 return Slerp(Slerp(p,q,t),Slerp(a,b,t),2*t*(1-t));
00749 }
00750
00751 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00752 const Quaternion & q, const Real t)
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793 {
00794 if( (t < 0) || (t > 1) )
00795 cerr << "Squad_prime(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
00796
00797 Quaternion q_squad,
00798 U = Slerp(p, q, t),
00799 V = Slerp(a, b, t),
00800 W = U.i()*V,
00801 U_prime = U*(p.i()*q).Log(),
00802 V_prime = V*(a.i()*b).Log(),
00803 W_prime = U.i()*V_prime - U.power(-2)*U_prime*V;
00804
00805 q_squad = U*( W.power(2*t*(1-t))*W.Log()*(2-4*t) + W.power(2*t*(1-t)-1)*W_prime*2*t*(1-t) )
00806 + U_prime*( W.power(2*t*(1-t)) );
00807
00808 return q_squad;
00809 }
00810
00811 #ifdef use_namespace
00812 }
00813 #endif
00814
00815
00816
00817
00818
00819