+++ /dev/null
-///////////////////////////////////////////////////////////////////////////
-//
-// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
-// Digital Ltd. LLC
-//
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are
-// met:
-// * Redistributions of source code must retain the above copyright
-// notice, this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above
-// copyright notice, this list of conditions and the following disclaimer
-// in the documentation and/or other materials provided with the
-// distribution.
-// * Neither the name of Industrial Light & Magic nor the names of
-// its contributors may be used to endorse or promote products derived
-// from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
-// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
-// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
-// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
-// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
-// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-//
-///////////////////////////////////////////////////////////////////////////
-
-
-
-#ifndef INCLUDED_IMATHQUAT_H
-#define INCLUDED_IMATHQUAT_H
-
-//----------------------------------------------------------------------
-//
-// template class Quat<T>
-//
-// "Quaternions came from Hamilton ... and have been an unmixed
-// evil to those who have touched them in any way. Vector is a
-// useless survival ... and has never been of the slightest use
-// to any creature."
-//
-// - Lord Kelvin
-//
-// This class implements the quaternion numerical type -- you
-// will probably want to use this class to represent orientations
-// in R3 and to convert between various euler angle reps. You
-// should probably use Imath::Euler<> for that.
-//
-//----------------------------------------------------------------------
-
-#include "ImathExc.h"
-#include "ImathMatrix.h"
-
-#include <iostream>
-
-namespace Imath {
-
-#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
-// Disable MS VC++ warnings about conversion from double to float
-#pragma warning(disable:4244)
-#endif
-
-template <class T>
-class Quat;
-
-template<class T>
-Quat<T> slerp (const Quat<T> &q1,const Quat<T> &q2, T t);
-
-template<class T>
-Quat<T> squad (const Quat<T> &q1,const Quat<T> &q2,
- const Quat<T> &qa,const Quat<T> &qb, T t);
-
-template<class T>
-void intermediate (const Quat<T> &q0, const Quat<T> &q1,
- const Quat<T> &q2, const Quat<T> &q3,
- Quat<T> &qa, Quat<T> &qb);
-
-template <class T>
-class Quat
-{
- public:
-
- T r; // real part
- Vec3<T> v; // imaginary vector
-
- //-----------------------------------------------------
- // Constructors - default constructor is identity quat
- //-----------------------------------------------------
-
- Quat() : r(1), v(0,0,0) {}
-
- template <class S>
- Quat( const Quat<S>& q) : r(q.r), v(q.v) {}
-
- Quat( T s, T i, T j, T k ) : r(s), v(i,j,k) {}
-
- Quat( T s, Vec3<T> d ) : r(s), v(d) {}
-
- static Quat<T> identity() { return Quat<T>(); }
-
- //------------------------------------------------
- // Basic Algebra - Operators and Methods
- // The operator return values are *NOT* normalized
- //
- // operator^ is 4D dot product
- // operator/ uses the inverse() quaternion
- // operator~ is conjugate -- if (S+V) is quat then
- // the conjugate (S+V)* == (S-V)
- //
- // some operators (*,/,*=,/=) treat the quat as
- // a 4D vector when one of the operands is scalar
- //------------------------------------------------
-
- const Quat<T>& operator= (const Quat<T>&);
- const Quat<T>& operator*= (const Quat<T>&);
- const Quat<T>& operator*= (T);
- const Quat<T>& operator/= (const Quat<T>&);
- const Quat<T>& operator/= (T);
- const Quat<T>& operator+= (const Quat<T>&);
- const Quat<T>& operator-= (const Quat<T>&);
- T& operator[] (int index); // as 4D vector
- T operator[] (int index) const;
-
- template <class S> bool operator == (const Quat<S> &q) const;
- template <class S> bool operator != (const Quat<S> &q) const;
-
- Quat<T>& invert(); // this -> 1 / this
- Quat<T> inverse() const;
- Quat<T>& normalize(); // returns this
- Quat<T> normalized() const;
- T length() const; // in R4
-
- //-----------------------
- // Rotation conversion
- //-----------------------
-
- Quat<T>& setAxisAngle(const Vec3<T>& axis, T radians);
- Quat<T>& setRotation(const Vec3<T>& fromDirection,
- const Vec3<T>& toDirection);
-
- T angle() const;
- Vec3<T> axis() const;
-
- Matrix33<T> toMatrix33() const;
- Matrix44<T> toMatrix44() const;
-
- Quat<T> log() const;
- Quat<T> exp() const;
-};
-
-
-//--------------------
-// Convenient typedefs
-//--------------------
-
-typedef Quat<float> Quatf;
-typedef Quat<double> Quatd;
-
-
-//---------------
-// Implementation
-//---------------
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator= (const Quat<T>& q)
-{
- r = q.r;
- v = q.v;
- return *this;
-}
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator*= (const Quat<T>& q)
-{
- T rtmp = r * q.r - (v ^ q.v);
- v = r * q.v + v * q.r + v % q.v;
- r = rtmp;
- return *this;
-}
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator*= (T t)
-{
- r *= t;
- v *= t;
- return *this;
-}
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator/= (const Quat<T>& q)
-{
- *this = *this * q.inverse();
- return *this;
-}
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator/= (T t)
-{
- r /= t;
- v /= t;
- return *this;
-}
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator+= (const Quat<T>& q)
-{
- r += q.r;
- v += q.v;
- return *this;
-}
-
-template<class T>
-inline const Quat<T>& Quat<T>::operator-= (const Quat<T>& q)
-{
- r -= q.r;
- v -= q.v;
- return *this;
-}
-template<class T>
-inline T& Quat<T>::operator[] (int index)
-{
- return index ? v[index-1] : r;
-}
-
-template<class T>
-inline T Quat<T>::operator[] (int index) const
-{
- return index ? v[index-1] : r;
-}
-
-template <class T>
-template <class S>
-inline bool
-Quat<T>::operator == (const Quat<S> &q) const
-{
- return r == q.r && v == q.v;
-}
-
-template <class T>
-template <class S>
-inline bool
-Quat<T>::operator != (const Quat<S> &q) const
-{
- return r != q.r || v != q.v;
-}
-
-template<class T>
-inline T operator^ (const Quat<T>& q1,const Quat<T>& q2)
-{
- return q1.r * q2.r + (q1.v ^ q2.v);
-}
-
-template <class T>
-inline T Quat<T>::length() const
-{
- return Math<T>::sqrt( r * r + (v ^ v) );
-}
-
-template <class T>
-inline Quat<T>& Quat<T>::normalize()
-{
- if ( T l = length() ) { r /= l; v /= l; }
- else { r = 1; v = Vec3<T>(0); }
- return *this;
-}
-
-template <class T>
-inline Quat<T> Quat<T>::normalized() const
-{
- if ( T l = length() ) return Quat( r / l, v / l );
- return Quat();
-}
-
-template<class T>
-inline Quat<T> Quat<T>::inverse() const
-{
- // 1 Q*
- // - = ---- where Q* is conjugate (operator~)
- // Q Q* Q and (Q* Q) == Q ^ Q (4D dot)
-
- T qdot = *this ^ *this;
- return Quat( r / qdot, -v / qdot );
-}
-
-template<class T>
-inline Quat<T>& Quat<T>::invert()
-{
- T qdot = (*this) ^ (*this);
- r /= qdot;
- v = -v / qdot;
- return *this;
-}
-
-template<class T>
-Quat<T>
-slerp(const Quat<T> &q1,const Quat<T> &q2, T t)
-{
- //
- // Spherical linear interpolation.
- //
- // NOTE: Assumes q1 and q2 are normalized and that 0 <= t <= 1.
- //
- // This method does *not* interpolate along the shortest arc
- // between q1 and q2. If you desire interpolation along the
- // shortest arc, then consider flipping the second quaternion
- // explicitly before calling slerp. The implementation of squad()
- // depends on a slerp() that interpolates as is, without the
- // automatic flipping.
- //
-
- T cosomega = q1 ^ q2;
- if (cosomega >= (T) 1.0)
- {
- //
- // Special case: q1 and q2 are the same, so just return one of them.
- // This also catches the case where cosomega is very slightly > 1.0
- //
-
- return q1;
- }
-
- T sinomega = Math<T>::sqrt (1 - cosomega * cosomega);
-
- Quat<T> result;
-
- if (sinomega * limits<T>::max() > 1)
- {
- T omega = Math<T>::acos (cosomega);
- T s1 = Math<T>::sin ((1.0 - t) * omega) / sinomega;
- T s2 = Math<T>::sin (t * omega) / sinomega;
-
- result = s1 * q1 + s2 * q2;
- }
- else if (cosomega > 0)
- {
- //
- // omega == 0
- //
-
- T s1 = 1.0 - t;
- T s2 = t;
-
- result = s1 * q1 + s2 * q2;
- }
- else
- {
- //
- // omega == -pi
- //
-
- result.v.x = - q1.v.y;
- result.v.y = q1.v.x;
- result.v.z = - q1.r;
- result.r = q1.v.z;
-
- T s1 = Math<T>::sin ((0.5 - t) * M_PI);
- T s2 = Math<T>::sin (t * M_PI);
-
- result = s1 * q1 + s2 * result;
- }
-
- return result;
-}
-
-template<class T>
-Quat<T> spline(const Quat<T> &q0, const Quat<T> &q1,
- const Quat<T> &q2, const Quat<T> &q3,
- T t)
-{
- // Spherical Cubic Spline Interpolation -
- // from Advanced Animation and Rendering
- // Techniques by Watt and Watt, Page 366:
- // A spherical curve is constructed using three
- // spherical linear interpolations of a quadrangle
- // of unit quaternions: q1, qa, qb, q2.
- // Given a set of quaternion keys: q0, q1, q2, q3,
- // this routine does the interpolation between
- // q1 and q2 by constructing two intermediate
- // quaternions: qa and qb. The qa and qb are
- // computed by the intermediate function to
- // guarantee the continuity of tangents across
- // adjacent cubic segments. The qa represents in-tangent
- // for q1 and the qb represents the out-tangent for q2.
- //
- // The q1 q2 is the cubic segment being interpolated.
- // The q0 is from the previous adjacent segment and q3 is
- // from the next adjacent segment. The q0 and q3 are used
- // in computing qa and qb.
- //
-
- Quat<T> qa = intermediate (q0, q1, q2);
- Quat<T> qb = intermediate (q1, q2, q3);
- Quat<T> result = squad(q1, qa, qb, q2, t);
-
- return result;
-}
-
-template<class T>
-Quat<T> squad(const Quat<T> &q1, const Quat<T> &qa,
- const Quat<T> &qb, const Quat<T> &q2,
- T t)
-{
- // Spherical Quadrangle Interpolation -
- // from Advanced Animation and Rendering
- // Techniques by Watt and Watt, Page 366:
- // It constructs a spherical cubic interpolation as
- // a series of three spherical linear interpolations
- // of a quadrangle of unit quaternions.
- //
-
- Quat<T> r1 = slerp(q1, q2, t);
- Quat<T> r2 = slerp(qa, qb, t);
- Quat<T> result = slerp(r1, r2, 2*t*(1-t));
-
- return result;
-}
-
-template<class T>
-Quat<T> intermediate(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
-{
- // From advanced Animation and Rendering
- // Techniques by Watt and Watt, Page 366:
- // computing the inner quadrangle
- // points (qa and qb) to guarantee tangent
- // continuity.
- //
- Quat<T> q1inv = q1.inverse();
- Quat<T> c1 = q1inv*q2;
- Quat<T> c2 = q1inv*q0;
- Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
- Quat<T> qa = q1 * c3.exp();
- qa.normalize();
- return qa;
-}
-
-template <class T>
-inline Quat<T> Quat<T>::log() const
-{
- //
- // For unit quaternion, from Advanced Animation and
- // Rendering Techniques by Watt and Watt, Page 366:
- //
-
- T theta = Math<T>::acos (std::min (r, (T) 1.0));
- if (theta == 0)
- return Quat<T> (0, v);
-
- T sintheta = Math<T>::sin (theta);
-
- T k;
- if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
- k = 0;
- else
- k = theta / sintheta;
-
- return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
-}
-
-template <class T>
-inline Quat<T> Quat<T>::exp() const
-{
- //
- // For pure quaternion (zero scalar part):
- // from Advanced Animation and Rendering
- // Techniques by Watt and Watt, Page 366:
- //
-
- T theta = v.length();
- T sintheta = Math<T>::sin (theta);
-
- T k;
- if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
- k = 0;
- else
- k = sintheta / theta;
-
- T costheta = Math<T>::cos (theta);
-
- return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
-}
-
-template <class T>
-inline T Quat<T>::angle() const
-{
- return 2.0*Math<T>::acos(r);
-}
-
-template <class T>
-inline Vec3<T> Quat<T>::axis() const
-{
- return v.normalized();
-}
-
-template <class T>
-inline Quat<T>& Quat<T>::setAxisAngle(const Vec3<T>& axis, T radians)
-{
- r = Math<T>::cos(radians/2);
- v = axis.normalized() * Math<T>::sin(radians/2);
- return *this;
-}
-
-
-template <class T>
-Quat<T>&
-Quat<T>::setRotation(const Vec3<T>& from, const Vec3<T>& to)
-{
- //
- // Ported from SbRotation
- //
-
- T cost = from.dot(to) / Math<T>::sqrt(from.dot(from) * to.dot(to));
-
- // check for degeneracies
- if (cost > 0.99999)
- {
- //
- // Vectors are parallel.
- //
-
- r = 1.0;
- v = Vec3<T>(0);
- }
- else if (cost < -0.99999)
- {
- //
- // Vectors are opposite. Find an axis to rotate around,
- // which should be perpendicular to the original axis.
- //
-
- Vec3<T> frm = from.normalized();
- v = frm.cross(Vec3<T>(1, 0, 0));
- if (v.length() < 0.00001)
- v = frm.cross(Vec3<T>(0, 1, 0));
- r = 0;
- v.normalize();
- }
- else
- {
- //
- // Use half-angle formulae:
- // cos^2 t = ( 1 + cos (2t) ) / 2
- // w part is cosine of half the rotation angle
- //
-
- r = Math<T>::sqrt(0.5 * (1.0 + cost));
-
- //
- // sin^2 t = ( 1 - cos (2t) ) / 2
- // Do the normalization of the axis vector at the same time so
- // we only call sqrt once.
- //
-
- v = from.cross(to);
- v *= Math<T>::sqrt((0.5 * (1.0 - cost))/(v.dot(v)));
- }
-
- return *this;
-}
-
-template<class T>
-Matrix33<T> Quat<T>::toMatrix33() const
-{
- return Matrix33<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
- 2.0 * (v.x * v.y + v.z * r),
- 2.0 * (v.z * v.x - v.y * r),
-
- 2.0 * (v.x * v.y - v.z * r),
- 1. - 2.0 * (v.z * v.z + v.x * v.x),
- 2.0 * (v.y * v.z + v.x * r),
-
- 2.0 * (v.z * v.x + v.y * r),
- 2.0 * (v.y * v.z - v.x * r),
- 1. - 2.0 * (v.y * v.y + v.x * v.x));
-}
-
-template<class T>
-Matrix44<T> Quat<T>::toMatrix44() const
-{
- return Matrix44<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
- 2.0 * (v.x * v.y + v.z * r),
- 2.0 * (v.z * v.x - v.y * r),
- 0.,
- 2.0 * (v.x * v.y - v.z * r),
- 1. - 2.0 * (v.z * v.z + v.x * v.x),
- 2.0 * (v.y * v.z + v.x * r),
- 0.,
- 2.0 * (v.z * v.x + v.y * r),
- 2.0 * (v.y * v.z - v.x * r),
- 1. - 2.0 * (v.y * v.y + v.x * v.x),
- 0.,
- 0.,
- 0.,
- 0.,
- 1.0 );
-}
-
-
-template<class T>
-inline Matrix33<T> operator* (const Matrix33<T> &M, const Quat<T> &q)
-{
- return M * q.toMatrix33();
-}
-
-template<class T>
-inline Matrix33<T> operator* (const Quat<T> &q, const Matrix33<T> &M)
-{
- return q.toMatrix33() * M;
-}
-
-template<class T>
-std::ostream& operator<< (std::ostream &o, const Quat<T> &q)
-{
- return o << "(" << q.r
- << " " << q.v.x
- << " " << q.v.y
- << " " << q.v.z
- << ")";
-
-}
-
-template<class T>
-inline Quat<T> operator* (const Quat<T>& q1, const Quat<T>& q2)
-{
- // (S1+V1) (S2+V2) = S1 S2 - V1.V2 + S1 V2 + V1 S2 + V1 x V2
- return Quat<T>( q1.r * q2.r - (q1.v ^ q2.v),
- q1.r * q2.v + q1.v * q2.r + q1.v % q2.v );
-}
-
-template<class T>
-inline Quat<T> operator/ (const Quat<T>& q1, const Quat<T>& q2)
-{
- return q1 * q2.inverse();
-}
-
-template<class T>
-inline Quat<T> operator/ (const Quat<T>& q,T t)
-{
- return Quat<T>(q.r/t,q.v/t);
-}
-
-template<class T>
-inline Quat<T> operator* (const Quat<T>& q,T t)
-{
- return Quat<T>(q.r*t,q.v*t);
-}
-
-template<class T>
-inline Quat<T> operator* (T t, const Quat<T>& q)
-{
- return Quat<T>(q.r*t,q.v*t);
-}
-
-template<class T>
-inline Quat<T> operator+ (const Quat<T>& q1, const Quat<T>& q2)
-{
- return Quat<T>( q1.r + q2.r, q1.v + q2.v );
-}
-
-template<class T>
-inline Quat<T> operator- (const Quat<T>& q1, const Quat<T>& q2)
-{
- return Quat<T>( q1.r - q2.r, q1.v - q2.v );
-}
-
-template<class T>
-inline Quat<T> operator~ (const Quat<T>& q)
-{
- return Quat<T>( q.r, -q.v ); // conjugate: (S+V)* = S-V
-}
-
-template<class T>
-inline Quat<T> operator- (const Quat<T>& q)
-{
- return Quat<T>( -q.r, -q.v );
-}
-
-#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
-#pragma warning(default:4244)
-#endif
-
-} // namespace Imath
-
-#endif
projects / opencv / blobdiff