quaternion.cpp

00001 #include <config.h>
00002 #include "quaternion.h"
00003 #include <cmath>
00004 
00005 namespace geometry
00006 {
00007   Quaternion::Quaternion(const Point3d& from, const Point3d& to)
00008     : Point4d(0,0,0,1)
00009   {
00010     const double fromSqNorm = util::normsq(from);
00011     const double toSqNorm = util::normsq(to);
00012     // Identity Quaternion when one vector is null
00013     if ((fromSqNorm < VVE_EPSILON) || (toSqNorm < VVE_EPSILON))
00014     {
00015       return;
00016     }
00017     else
00018     {
00019       Point3d axis = from ^ to;
00020       const double axisSqNorm = util::normsq(axis);
00021 
00022       // Aligned vectors, pick any axis, not aligned with from or to
00023       if (axisSqNorm < VVE_EPSILON)
00024         axis = util::orthogonal(from);
00025 
00026       double angle = std::asin(std::sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
00027 
00028       if (from*to < 0.0)
00029         angle = M_PI-angle;
00030 
00031       setAxisAngle(axis, angle);
00032     }
00033   }
00034 
00035   Quaternion& Quaternion::operator=(const Quaternion& other)
00036   {
00037     for(size_t i = 0 ; i < 4 ; ++i) elems[i] = other.elems[i];
00038     return *this;
00039   }
00040 
00041   Quaternion Quaternion::operator*(const Quaternion& other) const
00042   {
00043     double w_ = w()*other.w() - x()*other.x() -
00044       y()*other.y() - z()*other.z();
00045     double x_ = w()*other.x() + other.w()*x() + y()*other.z() - z()*other.y();
00046     double y_ = w()*other.y() + other.w()*y() + z()*other.x() - x()*other.z();
00047     double z_ = w()*other.z() + other.w()*z() + x()*other.y() - y()*other.x();
00048     return Quaternion(x_,y_,z_,w_);
00049   }
00050 
00051   Quaternion& Quaternion::operator*=(const Quaternion& other)
00052   {
00053     Quaternion res = *this * other;
00054     *this = res;
00055     return *this;
00056   }
00057 
00058   Quaternion::Quaternion(const Point3d& axis, double angle)
00059     : Point4d()
00060     {
00061       setAxisAngle(axis, angle);
00062     }
00063 
00064   void Quaternion::setAxisAngle(const Point3d& axis, double angle)
00065     {
00066       const double norm = util::norm(axis);
00067       if(norm < VVE_EPSILON)
00068       {
00069         x() = 0;
00070         y() = 0;
00071         z() = 0;
00072         w() = 1;
00073       }
00074       else
00075       {
00076         const double sin_half_angle = std::sin(angle/2.0);
00077         x() = sin_half_angle*axis.x()/norm;
00078         y() = sin_half_angle*axis.y()/norm;
00079         z() = sin_half_angle*axis.z()/norm;
00080         w() = std::cos(angle/2.0);
00081       }
00082     }
00083 
00084   Quaternion::Quaternion(const Matrix3d& m)
00085   {
00086     // Compute one plus the trace of the matrix
00087     const double onePlusTrace = 1.0 + m.trace();
00088     if(onePlusTrace > VVE_EPSILON)
00089     {
00090       const double s = std::sqrt(onePlusTrace)*2.0;
00091       x() = (m(2,1) - m(1,2))/s;
00092       y() = (m(0,2) - m(2,0))/s;
00093       z() = (m(1,0) - m(0,1))/s;
00094       w() = 0.25*s;
00095     }
00096     else
00097     {
00098       // Computation depends on major diagonal term
00099       if(m(0,0) > m(1,1) and m(0,0) > m(2,2))
00100       {
00101         const double s = std::sqrt(1+m(0,0)-m(1,1)-m(2,2))*2.0;
00102         x() = 0.25*s;
00103         y() = (m(1,0)+m(0,1))/s;
00104         z() = (m(2,0)+m(0,2))/s;
00105         w() = (m(1,2)-m(2,1))/s;
00106       }
00107       else if(m(1,1) > m(2,2))
00108       {
00109         const double s = std::sqrt(1+m(1,1)-m(0,0)-m(2,2))*2.0;
00110         x() = (m(0,1)+m(1,0))/s;
00111         y() = 0.25*s;
00112         z() = (m(1,2)+m(2,1))/s;
00113         w() = (m(0,2)-m(2,0))/s;
00114       }
00115       else
00116       {
00117         const double s = std::sqrt(1+m(2,2)-m(0,0)-m(1,1))*2.0;
00118         x() = (m(0,2)+m(0,0))/s;
00119         y() = (m(1,2)+m(2,1))/s;
00120         z() = 0.25*s;
00121         w() = (m(0,1)-m(1,0))/s;
00122       }
00123     }
00124     normalize();
00125   }
00126 
00127   template <class matrix>
00128     static void setMatrixFromQuaternion(matrix& m, const Quaternion& q)
00129     {
00130       const double xx = q.x()*q.x();
00131       const double xy = q.x()*q.y();
00132       const double xz = q.x()*q.z();
00133       const double yy = q.y()*q.y();
00134       const double yz = q.y()*q.z();
00135       const double zz = q.z()*q.z();
00136 
00137       const double wx = q.w()*q.x();
00138       const double wy = q.w()*q.y();
00139       const double wz = q.w()*q.z();
00140 
00141       m(0,0) = 1-2*(yy+zz);
00142       m(0,1) =   2*(xy-wz);
00143       m(0,2) =   2*(wy+xz);
00144       m(1,0) =   2*(xy+wz);
00145       m(1,1) = 1-2*(xx+zz);
00146       m(1,2) =   2*(yz-wx);
00147       m(2,0) =   2*(xz-wy);
00148       m(2,1) =   2*(yz+wx);
00149       m(2,2) = 1-2*(xx+yy);
00150     }
00151 
00152   void Quaternion::setMatrix(Matrix3d& m) const
00153   {
00154     setMatrixFromQuaternion(m, *this);
00155   }
00156 
00157   void Quaternion::setMatrix(Matrix4d& m) const
00158   {
00159     setMatrixFromQuaternion(m, *this);
00160     for(size_t i = 0 ; i < 4 ; ++i)
00161     {
00162       m(i,3) = 0.0;
00163       m(3,i) = 0.0;
00164     }
00165     m(3,3) = 1.0;
00166   }
00167 
00168   Point3d Quaternion::axis() const
00169   {
00170     Point3d res(x(), y(), z());
00171     const double sin = res.norm();
00172     if(sin > VVE_EPSILON)
00173       res /= sin;
00174     return (std::acos(w()) < M_PI/2.0)?res:-res;
00175   }
00176 
00177   double Quaternion::angle() const
00178   {
00179     const double a = 2.0*std::acos(w());
00180     return (a <= M_PI) ? a : 2.0*M_PI-a;
00181   }
00182 
00183   Point3d Quaternion::rotate(const Point3d& v) const
00184   {
00185     /*
00186     Quaternion q(v.x(), v.y(), v.z(), 0);
00187     Quaternion result = *this * q * conjugate();
00188     return Point3d(result.x(), result.y(), result.z());
00189     */
00190 
00191     const double xx = 2.0l*x()*x();
00192     const double yy = 2.0l*y()*y();
00193     const double zz = 2.0l*z()*z();
00194     const double xy = 2.0l*x()*y();
00195     const double xz = 2.0l*x()*z();
00196     const double yz = 2.0l*y()*z();
00197 
00198     const double wx = 2.0l*w()*x();
00199     const double wy = 2.0l*w()*y();
00200     const double wz = 2.0l*w()*z();
00201 
00202     return Point3d((1.0-yy-zz)*v.x() + (    xy-wz)*v.y() + (    xz+wy)*v.z(),
00203                    (    xy+wz)*v.x() + (1.0-zz-xx)*v.y() + (    yz-wx)*v.z(),
00204                    (    xz-wy)*v.x() + (    yz+wx)*v.y() + (1.0-xx-yy)*v.z());
00205   }
00206 
00207   Point3d Quaternion::inverseRotate(const Point3d& v) const
00208   {
00209     return inverse().rotate(v);
00210   }
00211 
00212   Quaternion slerp(const Quaternion& q1, const Quaternion& q2, double t)
00213   {
00214     double ca = static_cast<const Point4d&>(q1) * static_cast<const Point4d&>(q2);
00215     double a = std::acos(ca);
00216     if(a > M_PI/2)
00217       a -= M_PI;
00218     return (q1*std::sin(a*(1-t)) + q2*std::sin(a*t))/std::sin(a);
00219   }
00220 }
00221 
00222 namespace std
00223 {
00224   using geometry::Quaternion;
00225 
00226   Quaternion pow(const Quaternion& q, double p)
00227   {
00228     double a = q.angle();
00229     geometry::Point3d v = q.axis();
00230     return Quaternion(v,a*p);
00231   }
00232 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
Generated on Fri May 31 15:37:50 2013 for VVE by  doxygen 1.6.3