00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 #include <math.h>
00054 #include "trackball.h"
00055 #include <stdio.h>
00056 #include <stdlib.h>
00057 #include <iostream>
00058
00059
00060
00061
00062
00063
00064
00065
00066 #define TRACKBALLSIZE (0.8)
00067
00068
00069
00070
00071 static float tb_project_to_sphere(float, float, float);
00072 static void normalize_quat(float [4]);
00073
00074 void
00075 vzero(float *v)
00076 {
00077 v[0] = 0.0;
00078 v[1] = 0.0;
00079 v[2] = 0.0;
00080 }
00081
00082 void
00083 vset(float *v, float x, float y, float z)
00084 {
00085 v[0] = x;
00086 v[1] = y;
00087 v[2] = z;
00088 }
00089
00090 void
00091 vsub(const float *src1, const float *src2, float *dst)
00092 {
00093 dst[0] = src1[0] - src2[0];
00094 dst[1] = src1[1] - src2[1];
00095 dst[2] = src1[2] - src2[2];
00096 }
00097
00098 void
00099 vcopy(const float *v1, float *v2)
00100 {
00101 register int i;
00102 for (i = 0 ; i < 3 ; i++)
00103 v2[i] = v1[i];
00104 }
00105
00106 void
00107 vcross(const float *v1, const float *v2, float *cross)
00108 {
00109 float temp[3];
00110
00111 temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
00112 temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
00113 temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
00114 vcopy(temp, cross);
00115 }
00116
00117 float
00118 vlength(const float *v)
00119 {
00120 return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00121 }
00122
00123 void
00124 vscale(float *v, float div)
00125 {
00126 v[0] *= div;
00127 v[1] *= div;
00128 v[2] *= div;
00129 }
00130
00131 void
00132 vnormal(float *v)
00133 {
00134 vscale(v,1.0/vlength(v));
00135 }
00136
00137 float
00138 vdot(const float *v1, const float *v2)
00139 {
00140 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
00141 }
00142
00143 void
00144 vadd(const float *src1, const float *src2, float *dst)
00145 {
00146 dst[0] = src1[0] + src2[0];
00147 dst[1] = src1[1] + src2[1];
00148 dst[2] = src1[2] + src2[2];
00149 }
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163 void
00164 trackball(float q[4], float p1x, float p1y, float p2x, float p2y)
00165 {
00166 float a[3];
00167 float phi;
00168 float p1[3], p2[3], d[3];
00169 float t;
00170
00171 if (p1x == p2x && p1y == p2y) {
00172
00173 vzero(q);
00174 q[3] = 1.0;
00175 return;
00176 }
00177
00178
00179
00180
00181
00182 vset(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
00183 vset(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
00184
00185 std::cout << "p1 "; printVect(p1);
00186 std::cout << "p2 "; printVect(p2);
00187
00188
00189
00190 vcross(p2,p1,a);
00191
00192
00193
00194
00195 vsub(p1,p2,d);
00196 t = vlength(d) / (2.0*TRACKBALLSIZE);
00197
00198
00199
00200
00201 if (t > 1.0) t = 1.0;
00202 if (t < -1.0) t = -1.0;
00203 phi = 2.0 * asin(t);
00204
00205 axis_to_quat(a,phi,q);
00206 }
00207
00208
00209
00210
00211 void
00212 axis_to_quat(float a[3], float phi, float q[4])
00213 {
00214 vnormal(a);
00215 vcopy(a,q);
00216 vscale(q,sin(phi/2.0));
00217 q[3] = cos(phi/2.0);
00218 }
00219
00220
00221
00222
00223
00224 static float
00225 tb_project_to_sphere(float r, float x, float y)
00226 {
00227 float d, t, z;
00228
00229 d = sqrt(x*x + y*y);
00230 if (d < r * 0.70710678118654752440) {
00231 z = sqrt(r*r - d*d);
00232 } else {
00233 t = r / 1.41421356237309504880;
00234 z = t*t / d;
00235 }
00236 return z;
00237 }
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250 #define RENORMCOUNT 97
00251
00252 void
00253 add_quats(float q1[4], float q2[4], float dest[4])
00254 {
00255 static int count=0;
00256 float t1[4], t2[4], t3[4];
00257 float tf[4];
00258
00259 vcopy(q1,t1);
00260 vscale(t1,q2[3]);
00261
00262 vcopy(q2,t2);
00263 vscale(t2,q1[3]);
00264
00265 vcross(q2,q1,t3);
00266 vadd(t1,t2,tf);
00267 vadd(t3,tf,tf);
00268 tf[3] = q1[3] * q2[3] - vdot(q1,q2);
00269
00270 dest[0] = tf[0];
00271 dest[1] = tf[1];
00272 dest[2] = tf[2];
00273 dest[3] = tf[3];
00274
00275 if (++count > RENORMCOUNT) {
00276 count = 0;
00277 normalize_quat(dest);
00278 }
00279 }
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293 static void
00294 normalize_quat(float q[4])
00295 {
00296 int i;
00297 float mag;
00298
00299 mag = (q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
00300 for (i = 0; i < 4; i++) q[i] /= mag;
00301 }
00302
00303
00304
00305
00306
00307 void
00308 build_rotmatrix(float m[4][4], float q[4])
00309 {
00310 m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
00311 m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
00312 m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
00313 m[0][3] = 0.0;
00314
00315 m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
00316 m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
00317 m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
00318 m[1][3] = 0.0;
00319
00320 m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
00321 m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
00322 m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
00323 m[2][3] = 0.0;
00324
00325 m[3][0] = 0.0;
00326 m[3][1] = 0.0;
00327 m[3][2] = 0.0;
00328 m[3][3] = 1.0;
00329 }
00330
00331
00332 void printQuat(const float quat[4])
00333 {
00334 printf("Quat:\t [ ");
00335
00336 for (int i = 0; i < 4; i++)
00337 std::cout << qPrintable(QString("%1").arg(quat[i], -8, 'f', 6)) << " ";
00338
00339 std::cout << "]" << std::endl;
00340 }
00341
00342 void printVect(const float vect[3])
00343 {
00344 std::cout << "Vect:\t [ ";
00345
00346 for (int i = 0; i < 3; i++)
00347 std::cout << qPrintable(QString("%1").arg(vect[i], -8, 'f', 6)) << " ";
00348
00349 std::cout << "]" << std::endl;
00350 }
00351
00352 void printMat(const float mat[4][4])
00353 {
00354 std::cout << "Mat:" << std::endl << "[" << std::endl;
00355
00356 for (int i = 0; i < 4; i++)
00357 {
00358 std::cout << "[ ";
00359 for (int j = 0; j < 4; j++)
00360 std::cout << qPrintable(QString("%1").arg(mat[i][j], -8, 'f', 6)) << " ";
00361 std::cout << "]" << std::endl;
00362 }
00363 std::cout << "]" << std::endl;
00364 }