00001 #include <stdio.h>
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 #if defined(_WIN32)
00054 #pragma warning (disable:4244)
00055 #endif
00056 #include <math.h>
00057 #include "trackball.h"
00058
00059
00060
00061
00062
00063
00064
00065
00066 #define TRACKBALLSIZE (0.8f)
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
00186
00187
00188 vcross(p2,p1,a);
00189
00190
00191
00192
00193 vsub(p1,p2,d);
00194 t = vlength(d) / (2.0*TRACKBALLSIZE);
00195
00196
00197
00198
00199 if (t > 1.0) t = 1.0;
00200 if (t < -1.0) t = -1.0;
00201 phi = 2.0 * asin(t);
00202
00203 axis_to_quat(a,phi,q);
00204 }
00205
00206
00207
00208
00209 void
00210 axis_to_quat(float a[3], float phi, float q[4])
00211 {
00212 vnormal(a);
00213 vcopy(a,q);
00214 vscale(q,sin(phi/2.0));
00215 q[3] = cos(phi/2.0);
00216 }
00217
00218
00219
00220
00221
00222 static float
00223 tb_project_to_sphere(float r, float x, float y)
00224 {
00225 float d, t, z;
00226
00227 d = sqrt(x*x + y*y);
00228 if (d < r * 0.70710678118654752440) {
00229 z = sqrt(r*r - d*d);
00230 } else {
00231 t = r / 1.41421356237309504880;
00232 z = t*t / d;
00233 }
00234 return z;
00235 }
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248 #define RENORMCOUNT 97
00249
00250 void
00251 negate_quat(float q[4], float nq[4])
00252 {
00253 nq[0] = -q[0];
00254 nq[1] = -q[1];
00255 nq[2] = -q[2];
00256 nq[3] = q[3];
00257 }
00258
00259 void
00260 add_quats(float q1[4], float q2[4], float dest[4])
00261 {
00262 static int count=0;
00263 float t1[4], t2[4], t3[4];
00264 float tf[4];
00265
00266 #if 0
00267 printf("q1 = %f %f %f %f\n", q1[0], q1[1], q1[2], q1[3]);
00268 printf("q2 = %f %f %f %f\n", q2[0], q2[1], q2[2], q2[3]);
00269 #endif
00270
00271 vcopy(q1,t1);
00272 vscale(t1,q2[3]);
00273
00274 vcopy(q2,t2);
00275 vscale(t2,q1[3]);
00276
00277 vcross(q2,q1,t3);
00278 vadd(t1,t2,tf);
00279 vadd(t3,tf,tf);
00280 tf[3] = q1[3] * q2[3] - vdot(q1,q2);
00281
00282 #if 0
00283 printf("tf = %f %f %f %f\n", tf[0], tf[1], tf[2], tf[3]);
00284 #endif
00285
00286 dest[0] = tf[0];
00287 dest[1] = tf[1];
00288 dest[2] = tf[2];
00289 dest[3] = tf[3];
00290
00291 if (++count > RENORMCOUNT) {
00292 count = 0;
00293 normalize_quat(dest);
00294 }
00295 }
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 static void
00310 normalize_quat(float q[4])
00311 {
00312 int i;
00313 float mag;
00314
00315 mag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
00316 for (i = 0; i < 4; i++) q[i] /= mag;
00317 }
00318
00319
00320
00321
00322
00323 void
00324 build_rotmatrix(float m[4][4], float q[4])
00325 {
00326 m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
00327 m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
00328 m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
00329 m[0][3] = 0.0;
00330
00331 m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
00332 m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
00333 m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
00334 m[1][3] = 0.0;
00335
00336 m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
00337 m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
00338 m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
00339 m[2][3] = 0.0;
00340
00341 m[3][0] = 0.0;
00342 m[3][1] = 0.0;
00343 m[3][2] = 0.0;
00344 m[3][3] = 1.0;
00345 }
00346