00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #include <QString>
00026 #include <QVMatrix>
00027 #include <QVQuaternion>
00028
00030
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
00078 if (p1x == p2x && p1y == p2y)
00079 return QVQuaternion();
00080
00081
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
00087 float t = (p1 - p2).norm2() / (2.0*TRACKBALLSIZE);
00088
00089
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