PARP Research Group University of Murcia, Spain


src/qvmath/qvquaternion.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007, 2008, 2009. 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 
00030 // Constructors
00031 QVQuaternion::QVQuaternion(): QVVector(4)
00032         {
00033         set(0);
00034         operator[](3) = 1;
00035         }
00036 
00037 QVQuaternion::QVQuaternion(QVVector a, float phi):QVVector(4)
00038         {
00039         QVVector a_normal = a * (sin(phi/2.0) / a.norm2());
00040 
00041         operator[](0) = a_normal[0];
00042         operator[](1) = a_normal[1];
00043         operator[](2) = a_normal[2];
00044         operator[](3) = cos(phi/2.0);
00045         }
00046 
00047 QVQuaternion::QVQuaternion(const QVMatrix matrix): QVVector(4)
00048         {
00049         Q_ASSERT(matrix.getCols() == 3);
00050         Q_ASSERT(matrix.getCols() == 3);
00051 
00052         operator[](3) = sqrt(1 + matrix(0,0) + matrix(1,1) + matrix(2,2)) / 2;
00053         operator[](0) = (matrix(2,1) - matrix(1,2)) / (4*operator[](3));
00054         operator[](1) = (matrix(0,2) - matrix(2,0)) / (4*operator[](3));
00055         operator[](2) = (matrix(1,0) - matrix(0,1)) / (4*operator[](3));
00056         }
00057 
00058 QVQuaternion::QVQuaternion(const double q1, const double q2, const double q3, const double q4): QVVector(4)
00059         {
00060         operator[](0) = q1;
00061         operator[](1) = q2;
00062         operator[](2) = q3;
00063         operator[](3) = q4;
00064         }
00065 
00067 
00068 #define TRACKBALLSIZE  (0.8)
00069 float pixelToSphere(const float r, const float x, const float y)
00070         {
00071         float d = sqrt(x*x + y*y);
00072         return (d < r * 0.70710678118654752440)? sqrt(r*r - d*d) : r*r / (2*d);
00073         }
00074 
00075 QVQuaternion QVQuaternion::trackball(const float p1x, const float p1y, const float p2x, const float p2y)
00076         {
00077         // Zero rotation
00078         if (p1x == p2x && p1y == p2y)
00079                 return QVQuaternion();
00080         
00081         // First, figure out z-coordinates for projection of P1 and P2 to deformed sphere
00082         QVVector p1(3), p2(3);
00083         p1[0] = p1x, p1[1] = p1y, p1[2] = pixelToSphere(TRACKBALLSIZE,p1x,p1y);
00084         p2[0] = p2x, p2[1] = p2y, p2[2] = pixelToSphere(TRACKBALLSIZE,p2x,p2y);
00085 
00086         // Figure out how much to rotate around that axis.
00087         float t = (p1 - p2).norm2() / (2.0*TRACKBALLSIZE);
00088         
00089         // Avoid problems with out-of-control values...
00090         if (t > 1.0) t = 1.0;
00091         if (t < -1.0) t = -1.0;
00092 
00093         return QVQuaternion(p2 ^ p1, 2.0 * asin(t));                    
00094         }
00095 
00096 QVQuaternion QVQuaternion::normalizeQuaternion() const
00097         {
00098         QVQuaternion q = *this;
00099 
00100         double size = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
00101         for (int i = 0; i < 4; i++)
00102                 q[i] /= size;
00103 
00104         return q;
00105         }
00106 
00107 QVQuaternion QVQuaternion::quaternionProduct(const QVQuaternion &quaternion) const
00108         {
00109         Q_ASSERT(size() == 4);
00110         Q_ASSERT(quaternion.size() == 4);
00111 
00112         QVQuaternion    q1Vector = *this, q2Vector = quaternion;
00113 
00114         QVVector        t1Vector = q1Vector.subVector(0,2),
00115                         t2Vector = q2Vector.subVector(0,2),
00116                         t3Vector = t2Vector ^ t1Vector,
00117                         tfVector = t1Vector * q2Vector[3] + t2Vector * q1Vector[3] + t3Vector;
00118 
00119         QVQuaternion destQuat;
00120 
00121         destQuat[0] = tfVector[0];
00122         destQuat[1] = tfVector[1];
00123         destQuat[2] = tfVector[2];
00124         destQuat[3] = q1Vector[3] * q2Vector[3] - t1Vector * t2Vector;
00125         
00126         return destQuat.normalizeQuaternion();
00127         }
00128 
00130 
00131 std::ostream& operator << ( std::ostream &os, const QVQuaternion &quaternion )
00132         {
00133         const int size = quaternion.size();
00134 
00135         os << "QVQuaternion [";
00136 
00137         for (int i = 0; i < size; i++)
00138                 os << qPrintable(QString("%1").arg(quaternion[i], -8, 'f', 6)) << " ";
00139 
00140         os << "]" << std::endl;
00141         return os;
00142         }
00143 



QVision framework. PARP research group, copyright 2007, 2008.