git.maemo.org Git - opencv/blob - otherlibs/_graphics/include/OpenEXR/ImathQuat.h

   1 ///////////////////////////////////////////////////////////////////////////
   2 //
   3 // Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
   4 // Digital Ltd. LLC
   5 //
   6 // All rights reserved.
   7 //
   8 // Redistribution and use in source and binary forms, with or without
   9 // modification, are permitted provided that the following conditions are
  10 // met:
  11 // *       Redistributions of source code must retain the above copyright
  12 // notice, this list of conditions and the following disclaimer.
  13 // *       Redistributions in binary form must reproduce the above
  14 // copyright notice, this list of conditions and the following disclaimer
  15 // in the documentation and/or other materials provided with the
  16 // distribution.
  17 // *       Neither the name of Industrial Light & Magic nor the names of
  18 // its contributors may be used to endorse or promote products derived
  19 // from this software without specific prior written permission.
  20 //
  21 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  24 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  25 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  26 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  27 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  28 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  29 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  30 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  31 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  32 //
  33 ///////////////////////////////////////////////////////////////////////////
  34
  35
  36
  37 #ifndef INCLUDED_IMATHQUAT_H
  38 #define INCLUDED_IMATHQUAT_H
  39
  40 //----------------------------------------------------------------------
  41 //
  42 //      template class Quat<T>
  43 //
  44 //      "Quaternions came from Hamilton ... and have been an unmixed
  45 //      evil to those who have touched them in any way. Vector is a
  46 //      useless survival ... and has never been of the slightest use
  47 //      to any creature."
  48 //
  49 //          - Lord Kelvin
  50 //
  51 //      This class implements the quaternion numerical type -- you
  52 //      will probably want to use this class to represent orientations
  53 //      in R3 and to convert between various euler angle reps. You
  54 //      should probably use Imath::Euler<> for that.
  55 //
  56 //----------------------------------------------------------------------
  57
  58 #include "ImathExc.h"
  59 #include "ImathMatrix.h"
  60
  61 #include <iostream>
  62
  63 namespace Imath {
  64
  65 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
  66 // Disable MS VC++ warnings about conversion from double to float
  67 #pragma warning(disable:4244)
  68 #endif
  69
  70 template <class T>
  71 class Quat;
  72
  73 template<class T>
  74 Quat<T> slerp (const Quat<T> &q1,const Quat<T> &q2, T t);
  75
  76 template<class T>
  77 Quat<T> squad (const Quat<T> &q1,const Quat<T> &q2,
  78                const Quat<T> &qa,const Quat<T> &qb, T t);
  79
  80 template<class T>
  81 void intermediate (const Quat<T> &q0, const Quat<T> &q1,
  82                    const Quat<T> &q2, const Quat<T> &q3,
  83                    Quat<T> &qa, Quat<T> &qb);
  84
  85 template <class T>
  86 class Quat
  87 {
  88   public:
  89
  90     T                       r;      // real part
  91     Vec3<T>                 v;      // imaginary vector
  92
  93     //-----------------------------------------------------
  94     //  Constructors - default constructor is identity quat
  95     //-----------------------------------------------------
  96
  97     Quat()                          : r(1), v(0,0,0) {}
  98
  99     template <class S>
 100     Quat( const Quat<S>& q)         : r(q.r), v(q.v) {}
 101
 102     Quat( T s, T i, T j, T k )      : r(s), v(i,j,k) {}
 103
 104     Quat( T s, Vec3<T> d )          : r(s), v(d) {}
 105
 106     static Quat<T> identity()   { return Quat<T>(); }
 107
 108     //------------------------------------------------
 109     //  Basic Algebra - Operators and Methods
 110     //  The operator return values are *NOT* normalized
 111     //
 112     //  operator^ is 4D dot product
 113     //  operator/ uses the inverse() quaternion
 114     //  operator~ is conjugate -- if (S+V) is quat then
 115     //            the conjugate (S+V)* == (S-V)
 116     //
 117     //  some operators (*,/,*=,/=) treat the quat as
 118     //  a 4D vector when one of the operands is scalar
 119     //------------------------------------------------
 120
 121     const Quat<T>&          operator=   (const Quat<T>&);
 122     const Quat<T>&          operator*=  (const Quat<T>&);
 123     const Quat<T>&          operator*=  (T);
 124     const Quat<T>&          operator/=  (const Quat<T>&);
 125     const Quat<T>&          operator/=  (T);
 126     const Quat<T>&          operator+=  (const Quat<T>&);
 127     const Quat<T>&          operator-=  (const Quat<T>&);
 128     T&                      operator[]  (int index);    // as 4D vector
 129     T                       operator[]  (int index) const;
 130
 131     template <class S> bool operator == (const Quat<S> &q) const;
 132     template <class S> bool operator != (const Quat<S> &q) const;
 133
 134     Quat<T>&                invert();               // this -> 1 / this
 135     Quat<T>                 inverse() const;
 136     Quat<T>&                normalize();            // returns this
 137     Quat<T>                 normalized() const;
 138     T                       length() const;         // in R4
 139
 140     //-----------------------
 141     //  Rotation conversion
 142     //-----------------------
 143
 144     Quat<T>&                setAxisAngle(const Vec3<T>& axis, T radians);
 145     Quat<T>&                setRotation(const Vec3<T>& fromDirection,
 146                                         const Vec3<T>& toDirection);
 147
 148     T                       angle() const;
 149     Vec3<T>                 axis() const;
 150
 151     Matrix33<T>             toMatrix33() const;
 152     Matrix44<T>             toMatrix44() const;
 153
 154     Quat<T>                 log() const;
 155     Quat<T>                 exp() const;
 156 };
 157
 158
 159 //--------------------
 160 // Convenient typedefs
 161 //--------------------
 162
 163 typedef Quat<float>     Quatf;
 164 typedef Quat<double>    Quatd;
 165
 166
 167 //---------------
 168 // Implementation
 169 //---------------
 170
 171 template<class T>
 172 inline const Quat<T>& Quat<T>::operator= (const Quat<T>& q)
 173 {
 174     r = q.r;
 175     v = q.v;
 176     return *this;
 177 }
 178
 179 template<class T>
 180 inline const Quat<T>& Quat<T>::operator*= (const Quat<T>& q)
 181 {
 182     T rtmp = r * q.r - (v ^ q.v);
 183     v = r * q.v + v * q.r + v % q.v;
 184     r = rtmp;
 185     return *this;
 186 }
 187
 188 template<class T>
 189 inline const Quat<T>& Quat<T>::operator*= (T t)
 190 {
 191     r *= t;
 192     v *= t;
 193     return *this;
 194 }
 195
 196 template<class T>
 197 inline const Quat<T>& Quat<T>::operator/= (const Quat<T>& q)
 198 {
 199     *this = *this * q.inverse();
 200     return *this;
 201 }
 202
 203 template<class T>
 204 inline const Quat<T>& Quat<T>::operator/= (T t)
 205 {
 206     r /= t;
 207     v /= t;
 208     return *this;
 209 }
 210
 211 template<class T>
 212 inline const Quat<T>& Quat<T>::operator+= (const Quat<T>& q)
 213 {
 214     r += q.r;
 215     v += q.v;
 216     return *this;
 217 }
 218
 219 template<class T>
 220 inline const Quat<T>& Quat<T>::operator-= (const Quat<T>& q)
 221 {
 222     r -= q.r;
 223     v -= q.v;
 224     return *this;
 225 }
 226 template<class T>
 227 inline T& Quat<T>::operator[] (int index)
 228 {
 229     return index ? v[index-1] : r;
 230 }
 231
 232 template<class T>
 233 inline T Quat<T>::operator[] (int index) const
 234 {
 235     return index ? v[index-1] : r;
 236 }
 237
 238 template <class T>
 239 template <class S>
 240 inline bool
 241 Quat<T>::operator == (const Quat<S> &q) const
 242 {
 243     return r == q.r && v == q.v;
 244 }
 245
 246 template <class T>
 247 template <class S>
 248 inline bool
 249 Quat<T>::operator != (const Quat<S> &q) const
 250 {
 251     return r != q.r || v != q.v;
 252 }
 253
 254 template<class T>
 255 inline T operator^ (const Quat<T>& q1,const Quat<T>& q2)
 256 {
 257     return q1.r * q2.r + (q1.v ^ q2.v);
 258 }
 259
 260 template <class T>
 261 inline T Quat<T>::length() const
 262 {
 263     return Math<T>::sqrt( r * r + (v ^ v) );
 264 }
 265
 266 template <class T>
 267 inline Quat<T>& Quat<T>::normalize()
 268 {
 269     if ( T l = length() ) { r /= l; v /= l; }
 270     else { r = 1; v = Vec3<T>(0); }
 271     return *this;
 272 }
 273
 274 template <class T>
 275 inline Quat<T> Quat<T>::normalized() const
 276 {
 277     if ( T l = length() ) return Quat( r / l, v / l );
 278     return Quat();
 279 }
 280
 281 template<class T>
 282 inline Quat<T> Quat<T>::inverse() const
 283 {
 284     // 1    Q*
 285     // - = ----   where Q* is conjugate (operator~)
 286     // Q   Q* Q   and (Q* Q) == Q ^ Q (4D dot)
 287
 288     T qdot = *this ^ *this;
 289     return Quat( r / qdot, -v / qdot );
 290 }
 291
 292 template<class T>
 293 inline Quat<T>& Quat<T>::invert()
 294 {
 295     T qdot = (*this) ^ (*this);
 296     r /= qdot;
 297     v = -v / qdot;
 298     return *this;
 299 }
 300
 301 template<class T>
 302 Quat<T>
 303 slerp(const Quat<T> &q1,const Quat<T> &q2, T t)
 304 {
 305     //
 306     // Spherical linear interpolation.
 307     //
 308     // NOTE: Assumes q1 and q2 are normalized and that 0 <= t <= 1.
 309     //
 310     // This method does *not* interpolate along the shortest arc
 311     // between q1 and q2. If you desire interpolation along the
 312     // shortest arc, then consider flipping the second quaternion
 313     // explicitly before calling slerp. The implementation of squad()
 314     // depends on a slerp() that interpolates as is, without the
 315     // automatic flipping.
 316     //
 317
 318     T cosomega = q1 ^ q2;
 319     if (cosomega >= (T) 1.0)
 320     {
 321         //
 322         // Special case: q1 and q2 are the same, so just return one of them.
 323         // This also catches the case where cosomega is very slightly > 1.0
 324         //
 325
 326         return q1;
 327     }
 328
 329     T sinomega = Math<T>::sqrt (1 - cosomega * cosomega);
 330
 331     Quat<T> result;
 332
 333     if (sinomega * limits<T>::max() > 1)
 334     {
 335         T omega = Math<T>::acos (cosomega);
 336         T s1 = Math<T>::sin ((1.0 - t) * omega) / sinomega;
 337         T s2 = Math<T>::sin (t * omega) / sinomega;
 338
 339         result  = s1 * q1 + s2 * q2;
 340     }
 341     else if (cosomega > 0)
 342     {
 343         //
 344         // omega == 0
 345         //
 346
 347         T s1 = 1.0 - t;
 348         T s2 = t;
 349
 350         result = s1 * q1 + s2 * q2;
 351     }
 352     else
 353     {
 354         //
 355         // omega == -pi
 356         //
 357
 358         result.v.x  = - q1.v.y;
 359         result.v.y  =   q1.v.x;
 360         result.v.z  = - q1.r;
 361         result.r    =   q1.v.z;
 362
 363         T s1 = Math<T>::sin ((0.5 - t) * M_PI);
 364         T s2 = Math<T>::sin (t * M_PI);
 365
 366         result  = s1 * q1 + s2 * result;
 367     }
 368
 369     return result;
 370 }
 371
 372 template<class T>
 373 Quat<T> spline(const Quat<T> &q0, const Quat<T> &q1,
 374                const Quat<T> &q2, const Quat<T> &q3,
 375                T t)
 376 {
 377     // Spherical Cubic Spline Interpolation -
 378     // from Advanced Animation and Rendering
 379     // Techniques by Watt and Watt, Page 366:
 380     // A spherical curve is constructed using three
 381     // spherical linear interpolations of a quadrangle
 382     // of unit quaternions: q1, qa, qb, q2.
 383     // Given a set of quaternion keys: q0, q1, q2, q3,
 384     // this routine does the interpolation between
 385     // q1 and q2 by constructing two intermediate
 386     // quaternions: qa and qb. The qa and qb are
 387     // computed by the intermediate function to
 388     // guarantee the continuity of tangents across
 389     // adjacent cubic segments. The qa represents in-tangent
 390     // for q1 and the qb represents the out-tangent for q2.
 391     //
 392     // The q1 q2 is the cubic segment being interpolated.
 393     // The q0 is from the previous adjacent segment and q3 is
 394     // from the next adjacent segment. The q0 and q3 are used
 395     // in computing qa and qb.
 396     //
 397
 398     Quat<T> qa = intermediate (q0, q1, q2);
 399     Quat<T> qb = intermediate (q1, q2, q3);
 400     Quat<T> result = squad(q1, qa, qb, q2, t);
 401
 402     return result;
 403 }
 404
 405 template<class T>
 406 Quat<T> squad(const Quat<T> &q1, const Quat<T> &qa,
 407               const Quat<T> &qb, const Quat<T> &q2,
 408               T t)
 409 {
 410     // Spherical Quadrangle Interpolation -
 411     // from Advanced Animation and Rendering
 412     // Techniques by Watt and Watt, Page 366:
 413     // It constructs a spherical cubic interpolation as
 414     // a series of three spherical linear interpolations
 415     // of a quadrangle of unit quaternions.
 416     //
 417
 418     Quat<T> r1 = slerp(q1, q2, t);
 419     Quat<T> r2 = slerp(qa, qb, t);
 420     Quat<T> result = slerp(r1, r2, 2*t*(1-t));
 421
 422     return result;
 423 }
 424
 425 template<class T>
 426 Quat<T> intermediate(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
 427 {
 428     // From advanced Animation and Rendering
 429     // Techniques by Watt and Watt, Page 366:
 430     // computing the inner quadrangle
 431     // points (qa and qb) to guarantee tangent
 432     // continuity.
 433     //
 434     Quat<T> q1inv = q1.inverse();
 435     Quat<T> c1 = q1inv*q2;
 436     Quat<T> c2 = q1inv*q0;
 437     Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
 438     Quat<T> qa = q1 * c3.exp();
 439     qa.normalize();
 440     return qa;
 441 }
 442
 443 template <class T>
 444 inline Quat<T> Quat<T>::log() const
 445 {
 446     //
 447     // For unit quaternion, from Advanced Animation and
 448     // Rendering Techniques by Watt and Watt, Page 366:
 449     //
 450
 451     T theta = Math<T>::acos (std::min (r, (T) 1.0));
 452     if (theta == 0)
 453         return Quat<T> (0, v);
 454
 455     T sintheta = Math<T>::sin (theta);
 456
 457     T k;
 458     if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
 459         k = 0;
 460     else
 461         k = theta / sintheta;
 462
 463     return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
 464 }
 465
 466 template <class T>
 467 inline Quat<T> Quat<T>::exp() const
 468 {
 469     //
 470     // For pure quaternion (zero scalar part):
 471     // from Advanced Animation and Rendering
 472     // Techniques by Watt and Watt, Page 366:
 473     //
 474
 475     T theta = v.length();
 476     T sintheta = Math<T>::sin (theta);
 477
 478     T k;
 479     if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
 480         k = 0;
 481     else
 482         k = sintheta / theta;
 483
 484     T costheta = Math<T>::cos (theta);
 485
 486     return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
 487 }
 488
 489 template <class T>
 490 inline T Quat<T>::angle() const
 491 {
 492     return 2.0*Math<T>::acos(r);
 493 }
 494
 495 template <class T>
 496 inline Vec3<T> Quat<T>::axis() const
 497 {
 498     return v.normalized();
 499 }
 500
 501 template <class T>
 502 inline Quat<T>& Quat<T>::setAxisAngle(const Vec3<T>& axis, T radians)
 503 {
 504     r = Math<T>::cos(radians/2);
 505     v = axis.normalized() * Math<T>::sin(radians/2);
 506     return *this;
 507 }
 508
 509
 510 template <class T>
 511 Quat<T>&
 512 Quat<T>::setRotation(const Vec3<T>& from, const Vec3<T>& to)
 513 {
 514     //
 515     // Ported from SbRotation
 516     //
 517
 518     T cost = from.dot(to) / Math<T>::sqrt(from.dot(from) * to.dot(to));
 519
 520     // check for degeneracies
 521     if (cost > 0.99999)
 522     {
 523         //
 524         // Vectors are parallel.
 525         //
 526
 527         r = 1.0;
 528         v = Vec3<T>(0);
 529     }
 530     else if (cost < -0.99999)
 531     {
 532         //
 533         // Vectors are opposite. Find an axis to rotate around,
 534         // which should be perpendicular to the original axis.
 535         //
 536
 537         Vec3<T> frm = from.normalized();
 538         v = frm.cross(Vec3<T>(1, 0, 0));
 539         if (v.length() < 0.00001)
 540             v   = frm.cross(Vec3<T>(0, 1, 0));
 541         r = 0;
 542         v.normalize();
 543     }
 544     else
 545     {
 546         //
 547         // Use half-angle formulae:
 548         // cos^2 t = ( 1 + cos (2t) ) / 2
 549         // w part is cosine of half the rotation angle
 550         //
 551
 552         r = Math<T>::sqrt(0.5 * (1.0 + cost));
 553
 554         //
 555         // sin^2 t = ( 1 - cos (2t) ) / 2
 556         // Do the normalization of the axis vector at the same time so
 557         // we only call sqrt once.
 558         //
 559
 560         v = from.cross(to);
 561         v *= Math<T>::sqrt((0.5 * (1.0 - cost))/(v.dot(v)));
 562     }
 563
 564     return *this;
 565 }
 566
 567 template<class T>
 568 Matrix33<T> Quat<T>::toMatrix33() const
 569 {
 570     return Matrix33<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
 571                             2.0 * (v.x * v.y + v.z * r),
 572                             2.0 * (v.z * v.x - v.y * r),
 573
 574                             2.0 * (v.x * v.y - v.z * r),
 575                        1. - 2.0 * (v.z * v.z + v.x * v.x),
 576                             2.0 * (v.y * v.z + v.x * r),
 577
 578                             2.0 * (v.z * v.x + v.y * r),
 579                             2.0 * (v.y * v.z - v.x * r),
 580                        1. - 2.0 * (v.y * v.y + v.x * v.x));
 581 }
 582
 583 template<class T>
 584 Matrix44<T> Quat<T>::toMatrix44() const
 585 {
 586     return Matrix44<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
 587                             2.0 * (v.x * v.y + v.z * r),
 588                             2.0 * (v.z * v.x - v.y * r),
 589                             0.,
 590                             2.0 * (v.x * v.y - v.z * r),
 591                        1. - 2.0 * (v.z * v.z + v.x * v.x),
 592                             2.0 * (v.y * v.z + v.x * r),
 593                             0.,
 594                             2.0 * (v.z * v.x + v.y * r),
 595                             2.0 * (v.y * v.z - v.x * r),
 596                        1. - 2.0 * (v.y * v.y + v.x * v.x),
 597                             0.,
 598                             0.,
 599                             0.,
 600                             0.,
 601                             1.0 );
 602 }
 603
 604
 605 template<class T>
 606 inline Matrix33<T> operator* (const Matrix33<T> &M, const Quat<T> &q)
 607 {
 608     return M * q.toMatrix33();
 609 }
 610
 611 template<class T>
 612 inline Matrix33<T> operator* (const Quat<T> &q, const Matrix33<T> &M)
 613 {
 614     return q.toMatrix33() * M;
 615 }
 616
 617 template<class T>
 618 std::ostream& operator<< (std::ostream &o, const Quat<T> &q)
 619 {
 620     return o << "(" << q.r
 621              << " " << q.v.x
 622              << " " << q.v.y
 623              << " " << q.v.z
 624              << ")";
 625
 626 }
 627
 628 template<class T>
 629 inline Quat<T> operator* (const Quat<T>& q1, const Quat<T>& q2)
 630 {
 631     // (S1+V1) (S2+V2) = S1 S2 - V1.V2 + S1 V2 + V1 S2 + V1 x V2
 632     return Quat<T>( q1.r * q2.r - (q1.v ^ q2.v),
 633                     q1.r * q2.v + q1.v * q2.r + q1.v % q2.v );
 634 }
 635
 636 template<class T>
 637 inline Quat<T> operator/ (const Quat<T>& q1, const Quat<T>& q2)
 638 {
 639     return q1 * q2.inverse();
 640 }
 641
 642 template<class T>
 643 inline Quat<T> operator/ (const Quat<T>& q,T t)
 644 {
 645     return Quat<T>(q.r/t,q.v/t);
 646 }
 647
 648 template<class T>
 649 inline Quat<T> operator* (const Quat<T>& q,T t)
 650 {
 651     return Quat<T>(q.r*t,q.v*t);
 652 }
 653
 654 template<class T>
 655 inline Quat<T> operator* (T t, const Quat<T>& q)
 656 {
 657     return Quat<T>(q.r*t,q.v*t);
 658 }
 659
 660 template<class T>
 661 inline Quat<T> operator+ (const Quat<T>& q1, const Quat<T>& q2)
 662 {
 663     return Quat<T>( q1.r + q2.r, q1.v + q2.v );
 664 }
 665
 666 template<class T>
 667 inline Quat<T> operator- (const Quat<T>& q1, const Quat<T>& q2)
 668 {
 669     return Quat<T>( q1.r - q2.r, q1.v - q2.v );
 670 }
 671
 672 template<class T>
 673 inline Quat<T> operator~ (const Quat<T>& q)
 674 {
 675     return Quat<T>( q.r, -q.v );        // conjugate: (S+V)* = S-V
 676 }
 677
 678 template<class T>
 679 inline Quat<T> operator- (const Quat<T>& q)
 680 {
 681     return Quat<T>( -q.r, -q.v );
 682 }
 683
 684 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
 685 #pragma warning(default:4244)
 686 #endif
 687
 688 } // namespace Imath
 689
 690 #endif