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 operator[](4) = sqrt(1 + matrix(0,0) + matrix(1,1) + matrix(2,2)) / 2;
00050 operator[](1) = (matrix(1,2) - matrix(2,1)) / (4*operator[](4));
00051 operator[](2) = (matrix(2,0) - matrix(0,2)) / (4*operator[](4));
00052 operator[](3) = (matrix(0,1) - matrix(1,0)) / (4*operator[](4));
00053 }
00054
00055 QVQuaternion::QVQuaternion(const double q1, const double q2, const double q3, const double q4): QVVector(4)
00056 {
00057 operator[](0) = q1;
00058 operator[](1) = q2;
00059 operator[](2) = q3;
00060 operator[](3) = q4;
00061 }
00062
00064
00065 #define TRACKBALLSIZE (0.8)
00066 float pixelToSphere(const float r, const float x, const float y)
00067 {
00068 float d = sqrt(x*x + y*y);
00069 return (d < r * 0.70710678118654752440)? sqrt(r*r - d*d) : r*r / (2*d);
00070 }
00071
00072 QVQuaternion QVQuaternion::trackball(const float p1x, const float p1y, const float p2x, const float p2y)
00073 {
00074
00075 if (p1x == p2x && p1y == p2y)
00076 return QVQuaternion();
00077
00078
00079 QVVector p1(3), p2(3);
00080 p1[0] = p1x, p1[1] = p1y, p1[2] = pixelToSphere(TRACKBALLSIZE,p1x,p1y);
00081 p2[0] = p2x, p2[1] = p2y, p2[2] = pixelToSphere(TRACKBALLSIZE,p2x,p2y);
00082
00083
00084 float t = (p1 - p2).norm2() / (2.0*TRACKBALLSIZE);
00085
00086
00087 if (t > 1.0) t = 1.0;
00088 if (t < -1.0) t = -1.0;
00089
00090 return QVQuaternion(p2 ^ p1, 2.0 * asin(t));
00091 }
00092
00093 QVQuaternion QVQuaternion::normalizeQuaternion() const
00094 {
00095 QVQuaternion q = *this;
00096
00097 double size = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
00098 for (int i = 0; i < 4; i++)
00099 q[i] /= size;
00100
00101 return q;
00102 }
00103
00104 QVQuaternion QVQuaternion::quaternionProduct(const QVQuaternion &quaternion) const
00105 {
00106 Q_ASSERT(size() == 4);
00107 Q_ASSERT(quaternion.size() == 4);
00108
00109 QVQuaternion q1Vector = *this, q2Vector = quaternion;
00110
00111 QVVector t1Vector = q1Vector.subVector(0,2),
00112 t2Vector = q2Vector.subVector(0,2),
00113 t3Vector = t2Vector ^ t1Vector,
00114 tfVector = t1Vector * q2Vector[3] + t2Vector * q1Vector[3] + t3Vector;
00115
00116 QVQuaternion destQuat;
00117
00118 destQuat[0] = tfVector[0];
00119 destQuat[1] = tfVector[1];
00120 destQuat[2] = tfVector[2];
00121 destQuat[3] = q1Vector[3] * q2Vector[3] - t1Vector * t2Vector;
00122
00123 return destQuat.normalizeQuaternion();
00124 }
00125
00127
00128 std::ostream& operator << ( std::ostream &os, const QVQuaternion &quaternion )
00129 {
00130 const int size = quaternion.size();
00131
00132 os << "QVQuaternion [";
00133
00134 for (int i = 0; i < size; i++)
00135 os << qPrintable(QString("%1").arg(quaternion[i], -8, 'f', 6)) << " ";
00136
00137 os << "]" << std::endl;
00138 return os;
00139 }
00140