QVision: Qt's Image, Video and Computer Vision Library

00001 /*
00002  *      Copyright (C) 2007, 2008, 2009, 2010, 2011, 2012. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #include <QString>
00026 #include <QVMatrix>
00027 #include <QVQuaternion>
00028 #include <qvmath.h>
00029 
00031 // Constructors
00032 QVQuaternion::QVQuaternion(): QVVector(4)
00033         {
00034         set(0);
00035         operator[](3) = 1;
00036         Q_ASSERT(norm2() > 0);
00037         }
00038 
00039 QVQuaternion::QVQuaternion(const QVVector a, const double phi):QVVector(4)
00040         {
00041         QVVector a_normal = a * (sin(phi/2.0) / a.norm2());
00042 
00043         operator[](0) = a_normal[0];
00044         operator[](1) = a_normal[1];
00045         operator[](2) = a_normal[2];
00046         operator[](3) = cos(phi/2.0);
00047 
00048         Q_WARNING(not containsNaN());
00049         Q_ASSERT(norm2() > 0);
00050         }
00051 
00052 QVQuaternion::QVQuaternion(const QVMatrix &R): QVVector(4)
00053         {
00054         Q_ASSERT(not R.containsNaN());
00055         Q_ASSERT(R.norm2() > 0);
00056 
00057         int u, v, w;
00058 
00059         // Sort the elements of the trace of Q.
00060         if (R(0,0) > R(1,1))
00061                 if (R(0,0) > R(2,2))
00062                         if (R(1,1) > R(2,2))
00063                                 { u = 0; v = 1; w = 2; }
00064                         else
00065                                 { u = 0; v = 2; w = 1; }
00066                 else
00067                         if (R(0,0) > R(2,2))
00068                                 { u = 1; v = 0; w = 2; }
00069                         else
00070                                 { u = 1; v = 2; w = 0; }
00071         else
00072                 if (R(1,1) > R(2,2))
00073                         if (R(0,0) > R(2,2))
00074                                 { u = 1; v = 0; w = 2; }
00075                         else
00076                                 { u = 1; v = 2; w = 0; }
00077                 else
00078                         if (R(0,0) > R(1,1))
00079                                 { u = 2; v = 0; w = 1; }
00080                         else
00081                                 { u = 2; v = 1; w = 0; }
00082 
00083         Q_ASSERT(u != v);
00084         Q_ASSERT(v != w);
00085         Q_ASSERT(u != w);
00086 
00087         if (1.0 + R(u,u) - R(v,v) - R(w,w) < 0.0)
00088                 {
00089                 *this = QVQuaternion(-R);
00090                 return;
00091                 }
00092 
00093         Q_ASSERT(1.0 >= - R(u,u) + R(v,v) + R(w,w));
00094         const double r = sqrt(1.0 + R(u,u) - R(v,v) - R(w,w));
00095 
00096         // Estimate the values of the quaternion
00097         if (ABS(r) < 1e-100)
00098                 {
00099                 operator[](u) = 0.0; 
00100                 operator[](v) = 0.0;
00101                 operator[](w) = 0.0;
00102                 operator[](3) = 1.0;
00103                 }
00104         else    {
00105                 operator[](u) = r / 2.0; 
00106                 operator[](v) = (R(u,v) + R(v,u)) / (2.0*r);
00107                 operator[](w) = (R(u,w) + R(w,u)) / (2.0*r);
00108                 operator[](3) = (R(w,v) - R(v,w)) / (2.0*r);
00109                 }
00110 
00111         Q_WARNING(not isnan(real()));
00112         Q_WARNING(not isnan(i()));
00113         Q_WARNING(not isnan(j()));
00114         Q_WARNING(not isnan(k()));
00115 
00116         // Obtain correct sign for 'w'. Knowing:
00117         //      R(2,1) - R(1,2) == 4*x*w
00118         // We know that:
00119         //      SIGN(R(2,1) - R(1,2)) == SIGN(4*x*w)
00120         // thus
00121         //      (R(2,1) - R(1,2))*x*w > 0;
00122         switch(u)
00123                 {
00124                 // R(2,1) - R(1,2) == 4*x*w     <=>     (R(2,1) - R(1,2))*x*w > 0;
00125                 case 0: if ( (R(2,1) - R(1,2))*operator[](0)*operator[](3) < 0)
00126                                 operator[](3) = -operator[](3);
00127                         break;
00128 
00129                 // R(0,2) - R(2,0) == 4*y*w     <=>     (R(0,2) - R(2,0))*y*w > 0;
00130                 case 1: if ( (R(0,2) - R(2,0))*operator[](1)*operator[](3) < 0)
00131                                 operator[](3) = -operator[](3);
00132                         break;
00133 
00134                 // R(1,0) - R(0,1) == 4*z*w     <=>     (R(1,0) - R(0,1))*z*w > 0;
00135                 case 2: if ( (R(1,0) - R(0,1))*operator[](2)*operator[](3) < 0)
00136                                 operator[](3) = -operator[](3);
00137                         break;
00138 
00139                 default:
00140                         break;
00141                 }
00142 
00143         Q_ASSERT(not containsNaN());
00144         Q_ASSERT(norm2() > 0);
00145         }
00146 
00147 QVQuaternion::QVQuaternion(const double i, const double j, const double k, const double r): QVVector(4)
00148         {
00149         operator[](0) = i;
00150         operator[](1) = j;
00151         operator[](2) = k;
00152         operator[](3) = r;
00153 
00154         Q_WARNING(not containsNaN());
00155         Q_ASSERT(norm2() > 0);
00156         }
00157 
00158 QVQuaternion::QVQuaternion(const double xAngle, const double yAngle, const double zAngle): QVVector(4)
00159         {
00160         const double    sinX = sinf(0.5*xAngle),
00161                         sinY = sinf(0.5*yAngle),
00162                         sinZ = sinf(0.5*zAngle),
00163                         cosX = cosf(0.5*xAngle),
00164                         cosY = cosf(0.5*yAngle),
00165                         cosZ = cosf(0.5*zAngle);
00166 
00167         // and now compute quaternion
00168         operator[](0) = cosZ*cosY*sinX - sinZ*sinY*cosX;
00169         operator[](1) = cosZ*sinY*cosX + sinZ*cosY*sinX;
00170         operator[](2) = sinZ*cosY*cosX - cosZ*sinY*sinX;
00171         operator[](3) = cosZ*cosY*cosX + sinZ*sinY*sinX;
00172 
00173         Q_WARNING(not containsNaN());
00174         Q_ASSERT(norm2() > 0);
00175         }
00176 
00178 
00179 #define TRACKBALLSIZE  (0.8)
00180 float pixelToSphere(const float r, const float x, const float y)
00181         {
00182         float d = sqrt(x*x + y*y);
00183         return (d < r * 0.70710678118654752440)? sqrt(r*r - d*d) : r*r / (2*d);
00184         }
00185 
00186 QVQuaternion QVQuaternion::trackball(const float p1x, const float p1y, const float p2x, const float p2y)
00187         {
00188         // Zero rotation
00189         if (p1x == p2x && p1y == p2y)
00190                 return QVQuaternion();
00191         
00192         // First, figure out z-coordinates for projection of P1 and P2 to deformed sphere
00193         QVVector p1(3), p2(3);
00194         p1[0] = p1x, p1[1] = p1y, p1[2] = pixelToSphere(TRACKBALLSIZE,p1x,p1y);
00195         p2[0] = p2x, p2[1] = p2y, p2[2] = pixelToSphere(TRACKBALLSIZE,p2x,p2y);
00196 
00197         // Figure out how much to rotate around that axis.
00198         float t = (p1 - p2).norm2() / (2.0*TRACKBALLSIZE);
00199         
00200         // Avoid problems with out-of-control values...
00201         if (t > 1.0) t = 1.0;
00202         if (t < -1.0) t = -1.0;
00203 
00204         return QVQuaternion(p2 ^ p1, 2.0 * asin(t));                    
00205         }
00206 
00207 QVQuaternion QVQuaternion::normalizeQuaternion() const
00208         {
00209         QVQuaternion q = *this;
00210 
00211         double size = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
00212         for (int i = 0; i < 4; i++)
00213                 q[i] /= size;
00214 
00215         return q;
00216         }
00217 
00218 QVQuaternion QVQuaternion::quaternionProduct(const QVQuaternion &other) const
00219         {
00220         const double    x = operator[](0),      y = operator[](1),      z = operator[](2),      w = operator[](3),
00221                         other_x = other[0],     other_y = other[1],     other_z = other[2],     other_w = other[3];
00222 
00223         const QVQuaternion result(      w * other_x + x * other_w + y * other_z - z * other_y,
00224                                                                 w * other_y + y * other_w + z * other_x - x * other_z,
00225                                                                 w * other_z + z * other_w + x * other_y - y * other_x,
00226                                                                 w * other_w - x * other_x - y * other_y - z * other_z);
00227 
00228         Q_WARNING(not result.containsNaN());
00229         Q_ASSERT(result.norm2() > 0);
00230 
00231         return result;
00232         }
00233 
00234 void QVQuaternion::toEulerAngles(double &xAngle, double &yAngle, double &zAngle) const
00235         {
00236         Q_WARNING(not containsNaN());
00237         Q_ASSERT(norm2() > 0);
00238 
00239         const double    s = operator[](3),
00240                         x = operator[](0),
00241                         y = operator[](1),
00242                         z = operator[](2),
00243                         sqw = s*s,
00244                         sqx = x*x,
00245                         sqy = y*y,
00246                         sqz = z*z;
00247 
00248         xAngle = atan2f(2.f * (x*y + z*s), sqx - sqy - sqz + sqw);
00249         yAngle = asinf(-2.f * (x*z - y*s));
00250         zAngle = atan2f(2.f * (y*z + x*s), -sqx - sqy + sqz + sqw);
00251         }
00252 
00253 QVMatrix QVQuaternion::toRotationMatrix() const
00254         {
00255         Q_WARNING(not containsNaN());
00256         Q_ASSERT(norm2() > 0);
00257 
00258         const double    norm = norm2(),
00259                         x = operator[](0) / norm,
00260                         y = operator[](1) / norm,
00261                         z = operator[](2) / norm,
00262                         w = operator[](3) / norm;
00263 
00264         QVMatrix result(3,3);
00265         result(0,0) = 1.0 - 2.0 * (y*y + z*z);
00266         result(0,1) = 2.0 * (x*y - z*w);
00267         result(0,2) = 2.0 * (z*x + y*w);
00268         
00269         result(1,0) = 2.0 * (x*y + z*w);
00270         result(1,1) = 1.0 - 2.0 * (z*z + x*x);
00271         result(1,2) = 2.0 * (y*z - x*w);
00272         
00273         result(2,0) = 2.0 * (z*x - y*w);
00274         result(2,1) = 2.0 * (y*z + x*w);
00275         result(2,2) = 1.0 - 2.0 * (y*y + x*x);
00276 
00277         return result;
00278         }
00279 
00280 QVQuaternion QVQuaternion::conjugate() const
00281         {
00282         const QVQuaternion result(-operator[](0), -operator[](1), -operator[](2), operator[](3));
00283 
00284         Q_WARNING(not result.containsNaN());
00285         Q_ASSERT(result.norm2() > 0);
00286 
00287         return result;
00288         }
00289 
00290 QVQuaternion QVQuaternion::inverse() const
00291         {
00292         Q_ASSERT(norm2() > 0);
00293 
00294         const QVQuaternion result = conjugate();
00295 
00296         Q_WARNING(not result.containsNaN());
00297         Q_ASSERT(result.norm2() > 0);
00298 
00299         return result / result.norm2();
00300         }
00301 
00302 double QVQuaternion::norm2() const
00303         {
00304         return QVVector::norm2();
00305         }
00306 
00307 QV3DPointF QVQuaternion::rotate(const QV3DPointF &v) const
00308         {
00309         const double    norm = norm2(),
00310                         x = operator[](0) / norm,
00311                         y = operator[](1) / norm,
00312                         z = operator[](2) / norm,
00313                         w = operator[](3) / norm;
00314 
00315         if (v[0] == 0 and v[1] == 0 and v[2] == 0)
00316                 return v;
00317 
00318 //      std::cout << "q1" << std::endl;
00319         const QVQuaternion q(x, y, z, w);
00320 //      std::cout << "q2" << std::endl;
00321         const QVQuaternion p(v[0], v[1], v[2], 0);
00322 //      std::cout << "q3" << std::endl;
00323         const QVQuaternion product = q * p * q.conjugate();
00324 //      std::cout << "q4" << std::endl; 
00325 
00326         QVVector result(3);
00327         result[0] = product[0];
00328         result[1] = product[1];
00329         result[2] = product[2];
00330 
00331         Q_WARNING(not result.containsNaN());
00332 
00333         return result;
00334         }
00335 
00337 
00338 std::ostream& operator << ( std::ostream &os, const QVQuaternion &quaternion )
00339         {
00340         const int size = quaternion.size();
00341 
00342         os << "QVQuaternion [";
00343 
00344         for (int i = 0; i < size; i++)
00345                 os << qPrintable(QString("%1").arg(quaternion[i], -8, 'f', 6)) << " ";
00346 
00347         os << "]" << std::endl;
00348         return os;
00349         }
00350
src/qvmath/qvquaternion.cpp
