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
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
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
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
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
00187
00188
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 }