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 #include <qvmath.h>
00029
00031
00032 QVQuaternion::QVQuaternion(): QVVector(4)
00033 {
00034 set(0);
00035 operator[](3) = 1;
00036 }
00037
00038 QVQuaternion::QVQuaternion(const QVVector a, const double phi):QVVector(4)
00039 {
00040 QVVector a_normal = a * (sin(phi/2.0) / a.norm2());
00041
00042 operator[](0) = a_normal[0];
00043 operator[](1) = a_normal[1];
00044 operator[](2) = a_normal[2];
00045 operator[](3) = cos(phi/2.0);
00046 }
00047
00048 QVQuaternion::QVQuaternion(const QVMatrix &R): QVVector(4)
00049 {
00050 int u, v, w;
00051
00052
00053 if (R(0,0) > R(1,1))
00054 if (R(0,0) > R(2,2))
00055 if (R(1,1) > R(2,2))
00056 { u = 0; v = 1; w = 2; }
00057 else
00058 { u = 0; v = 2; w = 1; }
00059 else
00060 if (R(0,0) > R(2,2))
00061 { u = 1; v = 0; w = 2; }
00062 else
00063 { u = 1; v = 2; w = 0; }
00064 else
00065 if (R(1,1) > R(2,2))
00066 if (R(0,0) > R(2,2))
00067 { u = 1; v = 0; w = 2; }
00068 else
00069 { u = 1; v = 2; w = 0; }
00070 else
00071 if (R(0,0) > R(1,1))
00072 { u = 2; v = 0; w = 1; }
00073 else
00074 { u = 2; v = 1; w = 0; }
00075
00076 const double r = sqrt(1 + R(u,u) - R(v,v) - R(w,w));
00077
00078
00079 if (ABS(r) < 1e-100)
00080 {
00081 operator[](u) = 0.0;
00082 operator[](v) = 0.0;
00083 operator[](w) = 0.0;
00084 operator[](3) = 1.0;
00085 }
00086 else {
00087 operator[](u) = r / 2.0;
00088 operator[](v) = (R(u,v) + R(v,u)) / (2.0*r);
00089 operator[](w) = (R(u,w) + R(w,u)) / (2.0*r);
00090 operator[](3) = (R(w,v) - R(v,w)) / (2.0*r);
00091 }
00092
00093
00094
00095
00096
00097
00098
00099 switch(u)
00100 {
00101
00102 case 0: if ( (R(2,1) - R(1,2))*operator[](0)*operator[](3) < 0)
00103 operator[](3) = -operator[](3);
00104 break;
00105
00106
00107 case 1: if ( (R(0,2) - R(2,0))*operator[](1)*operator[](3) < 0)
00108 operator[](3) = -operator[](3);
00109 break;
00110
00111
00112 case 2: if ( (R(1,0) - R(0,1))*operator[](2)*operator[](3) < 0)
00113 operator[](3) = -operator[](3);
00114 break;
00115
00116 default:
00117 break;
00118 }
00119 }
00120
00121 QVQuaternion::QVQuaternion(const double i, const double j, const double k, const double r): QVVector(4)
00122 {
00123 operator[](0) = i;
00124 operator[](1) = j;
00125 operator[](2) = k;
00126 operator[](3) = r;
00127 }
00128
00129 QVQuaternion::QVQuaternion(const double xAngle, const double yAngle, const double zAngle): QVVector(4)
00130 {
00131 const double sinX = sinf(0.5*xAngle),
00132 sinY = sinf(0.5*yAngle),
00133 sinZ = sinf(0.5*zAngle),
00134 cosX = cosf(0.5*xAngle),
00135 cosY = cosf(0.5*yAngle),
00136 cosZ = cosf(0.5*zAngle);
00137
00138
00139 operator[](0) = cosZ*cosY*sinX - sinZ*sinY*cosX;
00140 operator[](1) = cosZ*sinY*cosX + sinZ*cosY*sinX;
00141 operator[](2) = sinZ*cosY*cosX - cosZ*sinY*sinX;
00142 operator[](3) = cosZ*cosY*cosX + sinZ*sinY*sinX;
00143 }
00144
00146
00147 #define TRACKBALLSIZE (0.8)
00148 float pixelToSphere(const float r, const float x, const float y)
00149 {
00150 float d = sqrt(x*x + y*y);
00151 return (d < r * 0.70710678118654752440)? sqrt(r*r - d*d) : r*r / (2*d);
00152 }
00153
00154 QVQuaternion QVQuaternion::trackball(const float p1x, const float p1y, const float p2x, const float p2y)
00155 {
00156
00157 if (p1x == p2x && p1y == p2y)
00158 return QVQuaternion();
00159
00160
00161 QVVector p1(3), p2(3);
00162 p1[0] = p1x, p1[1] = p1y, p1[2] = pixelToSphere(TRACKBALLSIZE,p1x,p1y);
00163 p2[0] = p2x, p2[1] = p2y, p2[2] = pixelToSphere(TRACKBALLSIZE,p2x,p2y);
00164
00165
00166 float t = (p1 - p2).norm2() / (2.0*TRACKBALLSIZE);
00167
00168
00169 if (t > 1.0) t = 1.0;
00170 if (t < -1.0) t = -1.0;
00171
00172 return QVQuaternion(p2 ^ p1, 2.0 * asin(t));
00173 }
00174
00175 QVQuaternion QVQuaternion::normalizeQuaternion() const
00176 {
00177 QVQuaternion q = *this;
00178
00179 double size = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
00180 for (int i = 0; i < 4; i++)
00181 q[i] /= size;
00182
00183 return q;
00184 }
00185
00186 QVQuaternion QVQuaternion::quaternionProduct(const QVQuaternion &other) const
00187 {
00188 const double x = operator[](0), y = operator[](1), z = operator[](2), w = operator[](3),
00189 other_x = other[0], other_y = other[1], other_z = other[2], other_w = other[3];
00190
00191 return QVQuaternion( w * other_x + x * other_w + y * other_z - z * other_y,
00192 w * other_y + y * other_w + z * other_x - x * other_z,
00193 w * other_z + z * other_w + x * other_y - y * other_x,
00194 w * other_w - x * other_x - y * other_y - z * other_z);
00195 }
00196
00197 void QVQuaternion::toEulerAngles(double &xAngle, double &yAngle, double &zAngle) const
00198 {
00199 const double s = operator[](3),
00200 x = operator[](0),
00201 y = operator[](1),
00202 z = operator[](2),
00203 sqw = s*s,
00204 sqx = x*x,
00205 sqy = y*y,
00206 sqz = z*z;
00207
00208 xAngle = atan2f(2.f * (x*y + z*s), sqx - sqy - sqz + sqw);
00209 yAngle = asinf(-2.f * (x*z - y*s));
00210 zAngle = atan2f(2.f * (y*z + x*s), -sqx - sqy + sqz + sqw);
00211 }
00212
00213 QVMatrix QVQuaternion::toRotationMatrix() const
00214 {
00215 const double norm = norm2(),
00216 x = operator[](0) / norm,
00217 y = operator[](1) / norm,
00218 z = operator[](2) / norm,
00219 w = operator[](3) / norm;
00220
00221 QVMatrix result(3,3);
00222 result(0,0) = 1.0 - 2.0 * (y*y + z*z);
00223 result(0,1) = 2.0 * (x*y - z*w);
00224 result(0,2) = 2.0 * (z*x + y*w);
00225
00226 result(1,0) = 2.0 * (x*y + z*w);
00227 result(1,1) = 1.0 - 2.0 * (z*z + x*x);
00228 result(1,2) = 2.0 * (y*z - x*w);
00229
00230 result(2,0) = 2.0 * (z*x - y*w);
00231 result(2,1) = 2.0 * (y*z + x*w);
00232 result(2,2) = 1.0 - 2.0 * (y*y + x*x);
00233
00234 return result;
00235 }
00236
00237 QVQuaternion QVQuaternion::conjugate() const
00238 {
00239 return QVQuaternion(-operator[](0), -operator[](1), -operator[](2), operator[](3));
00240 }
00241
00242 QVQuaternion QVQuaternion::inverse() const
00243 {
00244 const QVVector q = conjugate();
00245 return q / q.norm2();
00246 }
00247
00248 double QVQuaternion::norm2() const
00249 {
00250 return QVVector::norm2();
00251 }
00252
00253 QVVector QVQuaternion::rotate(const QVVector &v) const
00254 {
00255 const double norm = norm2(),
00256 x = operator[](0) / norm,
00257 y = operator[](1) / norm,
00258 z = operator[](2) / norm,
00259 w = operator[](3) / norm;
00260
00261 const QVQuaternion q(x, y, z, w), p(v[0], v[1], v[2], 0), product = q * p * q.conjugate();
00262
00263 QVVector result(3);
00264 result[0] = product[0];
00265 result[1] = product[1];
00266 result[2] = product[2];
00267
00268 return result;
00269 }
00270
00272
00273 std::ostream& operator << ( std::ostream &os, const QVQuaternion &quaternion )
00274 {
00275 const int size = quaternion.size();
00276
00277 os << "QVQuaternion [";
00278
00279 for (int i = 0; i < size; i++)
00280 os << qPrintable(QString("%1").arg(quaternion[i], -8, 'f', 6)) << " ";
00281
00282 os << "]" << std::endl;
00283 return os;
00284 }
00285