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
00057 static const char rcsid[] = "$Id: quaternion.cpp,v 1.4 2004/07/14 02:32:12 ejt Exp $";
00058
00059 #include "quaternion.h"
00060
00061 #ifdef use_namespace
00062 namespace ROBOOP {
00063 using namespace NEWMAT;
00064 #endif
00065
00066
00067 Quaternion::Quaternion()
00068
00069 {
00070 s_ = 1.0;
00071 v_ = ColumnVector(3);
00072 v_ = 0.0;
00073 }
00074
00075 Quaternion::Quaternion(const Quaternion & q)
00076
00077 {
00078 s_ = q.s_;
00079 v_ = q.v_;
00080 }
00081
00082 Quaternion::Quaternion(const Real angle, const ColumnVector & axis)
00083
00084 {
00085 if(axis.Nrows() != 3)
00086 {
00087 cerr << "Quaternion::Quaternion, size of axis != 3" << endl;
00088 exit(1);
00089 }
00090
00091
00092 Real norm_axis = sqrt(DotProduct(axis, axis));
00093
00094 if(norm_axis != 1)
00095 {
00096 cerr << "Quaternion::Quaternion(angle, axis), axis is not unit" << endl;
00097 cerr << "Make the axis unit." << endl;
00098 v_ = sin(angle/2) * axis/norm_axis;
00099 }
00100 else
00101 v_ = sin(angle/2) * axis;
00102
00103 s_ = cos(angle/2);
00104 }
00105
00106 Quaternion::Quaternion(const Real s_in, const Real v1, const Real v2,
00107 const Real v3)
00108
00109 {
00110 s_ = s_in;
00111 v_ = ColumnVector(3);
00112 v_(1) = v1;
00113 v_(2) = v2;
00114 v_(3) = v3;
00115 }
00116
00117 Quaternion::Quaternion(const Matrix & R)
00118
00119
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 if( (R.Nrows() == 3) && (R.Ncols() == 3) ||
00160 (R.Nrows() == 4) && (R.Ncols() == 4) )
00161 {
00162 Real tmp = fabs(R(1,1) + R(2,2) + R(3,3) + 1);
00163 s_ = 0.5*sqrt(tmp);
00164 if(v_.Nrows() != 3)
00165 v_ = ColumnVector(3);
00166
00167 if(s_ > EPSILON)
00168 {
00169 v_(1) = (R(3,2)-R(2,3))/(4*s_);
00170 v_(2) = (R(1,3)-R(3,1))/(4*s_);
00171 v_(3) = (R(2,1)-R(1,2))/(4*s_);
00172 }
00173 else
00174 {
00175
00176 static int s_iNext[3] = { 2, 3, 1 };
00177 int i = 1;
00178 if ( R(2,2) > R(1,1) )
00179 i = 2;
00180 if ( R(3,3) > R(2,2) )
00181 i = 3;
00182 int j = s_iNext[i-1];
00183 int k = s_iNext[j-1];
00184
00185 Real fRoot = sqrt(R(i,i)-R(j,j)-R(k,k) + 1.0);
00186
00187 Real *tmp[3] = { &v_(1), &v_(2), &v_(3) };
00188 *tmp[i-1] = 0.5*fRoot;
00189 fRoot = 0.5/fRoot;
00190 s_ = (R(k,j)-R(j,k))*fRoot;
00191 *tmp[j-1] = (R(j,i)+R(i,j))*fRoot;
00192 *tmp[k-1] = (R(k,i)+R(i,k))*fRoot;
00193 }
00194
00195 }
00196 else
00197 cerr << "Quaternion::Quaternion: matrix input is not 3x3 or 4x4" << endl;
00198 }
00199
00200 Quaternion & Quaternion::operator=(const Quaternion & q)
00201
00202 {
00203 s_ = q.s_;
00204 v_ = q.v_;
00205
00206 return *this;
00207 }
00208
00209 Quaternion Quaternion::operator+(const Quaternion & rhs)const
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 {
00222 Quaternion q;
00223 q.s_ = s_ + rhs.s_;
00224 q.v_ = v_ + rhs.v_;
00225
00226 return q;
00227 }
00228
00229 Quaternion Quaternion::operator-(const Quaternion & rhs)const
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241 {
00242 Quaternion q;
00243 q.s_ = s_ - rhs.s_;
00244 q.v_ = v_ - rhs.v_;
00245
00246 return q;
00247 }
00248
00249 Quaternion Quaternion::operator*(const Quaternion & rhs)const
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264 {
00265 Quaternion q;
00266 q.s_ = s_ * rhs.s_ - DotProduct(v_, rhs.v_);
00267 q.v_ = s_ * rhs.v_ + rhs.s_ * v_ + CrossProduct(v_, rhs.v_);
00268
00269 return q;
00270 }
00271
00272 Quaternion Quaternion::operator*(const Real c)const
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282 {
00283 Quaternion q;
00284 q.s_ = s_ * c;
00285 q.v_ = v_ * c;
00286
00287 return q;
00288 }
00289
00290 Quaternion Quaternion::operator/(const Quaternion & rhs)const
00291
00292 {
00293
00294
00295
00296
00297
00298
00299 return *this*rhs.i();
00300 }
00301
00302 Quaternion Quaternion::operator/(const Real c)const
00303
00304
00305
00306
00307
00308 {
00309 Quaternion q;
00310 q.s_ = s_ / c;
00311 q.v_ = v_ / c;
00312
00313 return q;
00314 }
00315
00316 void Quaternion::set_v(const ColumnVector & v)
00317
00318 {
00319 if(v.Nrows() == 3)
00320 v_ = v;
00321 else
00322 cerr << "Quaternion::set_v: input has a wrong size." << endl;
00323 }
00324
00325 Quaternion Quaternion::conjugate()const
00326
00327
00328
00329
00330
00331
00332 {
00333 Quaternion q;
00334 q.s_ = s_;
00335 q.v_ = -1*v_;
00336
00337 return q;
00338 }
00339
00340 Real Quaternion::norm()const
00341
00342
00343
00344
00345
00346
00347
00348
00349 {
00350 return( sqrt(s_*s_ + DotProduct(v_, v_)) );
00351 }
00352
00353 Quaternion & Quaternion::unit()
00354
00355 {
00356 Real tmp = norm();
00357 if(tmp > EPSILON)
00358 {
00359 s_ = s_/tmp;
00360 v_ = v_/tmp;
00361 }
00362 return *this;
00363 }
00364
00365 Quaternion Quaternion::i()const
00366
00367
00368
00369
00370
00371
00372
00373
00374 {
00375 return conjugate()/norm();
00376 }
00377
00378 Quaternion Quaternion::exp() const
00379
00380
00381
00382
00383
00384
00385
00386 {
00387 Quaternion q;
00388 Real theta = sqrt(DotProduct(v_,v_)),
00389 sin_theta = sin(theta);
00390
00391 q.s_ = cos(theta);
00392 if ( fabs(sin_theta) > EPSILON)
00393 q.v_ = v_*sin_theta/theta;
00394 else
00395 q.v_ = v_;
00396
00397 return q;
00398 }
00399
00400 Quaternion Quaternion::power(const Real t) const
00401 {
00402 Quaternion q = (Log()*t).exp();
00403
00404 return q;
00405 }
00406
00407 Quaternion Quaternion::Log()const
00408
00409
00410
00411
00412
00413
00414
00415
00416 {
00417 Quaternion q;
00418 q.s_ = 0;
00419 Real theta = acos(s_),
00420 sin_theta = sin(theta);
00421
00422 if ( fabs(sin_theta) > EPSILON)
00423 q.v_ = v_/sin_theta*theta;
00424 else
00425 q.v_ = v_;
00426
00427 return q;
00428 }
00429
00430 Quaternion Quaternion::dot(const ColumnVector & w, const short sign)const
00431
00432
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 Quaternion q;
00459 Matrix tmp;
00460
00461 tmp = -0.5*v_.t()*w;
00462 q.s_ = tmp(1,1);
00463 q.v_ = 0.5*E(sign)*w;
00464
00465 return q;
00466 }
00467
00468 ReturnMatrix Quaternion::E(const short sign)const
00469
00470
00471
00472
00473
00474 {
00475 Matrix E(3,3), I(3,3);
00476 I << threebythreeident;
00477
00478 if(sign == BODY_FRAME)
00479 E = s_*I + x_prod_matrix(v_);
00480 else
00481 E = s_*I - x_prod_matrix(v_);
00482
00483 E.Release();
00484 return E;
00485 }
00486
00487 Real Quaternion::dot_prod(const Quaternion & q)const
00488
00489
00490
00491
00492
00493
00494
00495
00496 {
00497 return (s_*q.s_ + v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
00498 }
00499
00500 ReturnMatrix Quaternion::R()const
00501
00502
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 Matrix R(3,3);
00534 R << threebythreeident;
00535 R = (1 - 2*DotProduct(v_, v_))*R + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00536
00537 R.Release();
00538 return R;
00539 }
00540
00541 ReturnMatrix Quaternion::T()const
00542
00543
00544
00545
00546
00547 {
00548 Matrix T(4,4);
00549 T << fourbyfourident;
00550 T.SubMatrix(1,3,1,3) = (1 - 2*DotProduct(v_, v_))*T.SubMatrix(1,3,1,3)
00551 + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00552 T.Release();
00553 return T;
00554 }
00555
00556
00557
00558 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot)
00559
00560
00561
00562
00563
00564 {
00565 Matrix A, B, M;
00566 UpperTriangularMatrix U;
00567 ColumnVector w(3);
00568 A = 0.5*q.E(BASE_FRAME);
00569 B = q_dot.v();
00570 if(A.Determinant())
00571 {
00572 QRZ(A,U);
00573 QRZ(A,B,M);
00574 w = U.i()*M;
00575 }
00576 else
00577 w = 0;
00578
00579 w.Release();
00580 return w;
00581 }
00582
00583 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
00584 Quaternion & quat, const Real dt)
00585
00586 {
00587 if (dt < 0)
00588 {
00589 cerr << "Integ_Trap(quat1, quat2, dt): dt < 0. dt is set to 0." << endl;
00590 return -1;
00591 }
00592
00593
00594
00595
00596 dquat_present.set_s(dquat_present.s() );
00597 dquat_present.set_v(dquat_present.v() );
00598
00599 quat.set_s(quat.s() + Integ_Trap_quat_s(dquat_present, dquat_past, dt));
00600 quat.set_v(quat.v() + Integ_Trap_quat_v(dquat_present, dquat_past, dt));
00601
00602 dquat_past.set_s(dquat_present.s());
00603 dquat_past.set_v(dquat_present.v());
00604
00605 quat.unit();
00606
00607 return 0;
00608 }
00609
00610 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
00611 const Real dt)
00612
00613 {
00614 Real integ = 0.5*(present.s()+past.s())*dt;
00615 past.set_s(present.s());
00616 return integ;
00617 }
00618
00619 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
00620 const Real dt)
00621
00622 {
00623 ColumnVector integ = 0.5*(present.v()+past.v())*dt;
00624 past.set_v(present.v());
00625 integ.Release();
00626 return integ;
00627 }
00628
00629 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t)
00630
00631
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 if( (t < 0) || (t > 1) )
00682 cerr << "Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
00683
00684 if(q0.dot_prod(q1) >= 0)
00685 return q0*((q0.i()*q1).power(t));
00686 else
00687 return q0*((q0.i()*-1*q1).power(t));
00688 }
00689
00690 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1,
00691 const Real t)
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713 {
00714 if( (t < 0) || (t > 1) )
00715 cerr << "Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
00716
00717 if(q0.dot_prod(q1) >= 0)
00718 return Slerp(q0, q1, t)*(q0.i()*q1).Log();
00719 else
00720 return Slerp(q0, q1, t)*(q0.i()*-1*q1).Log();
00721 }
00722
00723 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00724 const Quaternion & q, const Real t)
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742 {
00743 if( (t < 0) || (t > 1) )
00744 cerr << "Squad(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
00745
00746 return Slerp(Slerp(p,q,t),Slerp(a,b,t),2*t*(1-t));
00747 }
00748
00749 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00750 const Quaternion & q, const Real t)
00751
00752
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 if( (t < 0) || (t > 1) )
00793 cerr << "Squad_prime(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
00794
00795 Quaternion q_squad,
00796 U = Slerp(p, q, t),
00797 V = Slerp(a, b, t),
00798 W = U.i()*V,
00799 U_prime = U*(p.i()*q).Log(),
00800 V_prime = V*(a.i()*b).Log(),
00801 W_prime = U.i()*V_prime - U.power(-2)*U_prime*V;
00802
00803 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) )
00804 + U_prime*( W.power(2*t*(1-t)) );
00805
00806 return q_squad;
00807 }
00808
00809 #ifdef use_namespace
00810 }
00811 #endif
00812
00813
00814
00815
00816
00817