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
00054
00060 #if defined(_WIN32)
00061 #pragma warning (disable:4244)
00062 #endif
00063 #include <math.h>
00064 #include "trackball.h"
00065
00066
00067
00068
00069
00070
00071
00072
00073 #define TRACKBALLSIZE (0.8f)
00074
00075
00076
00077
00078 static float tb_project_to_sphere(float, float, float);
00079 static void normalize_quat(float [4]);
00080
00081 void
00082 vzero(float *v)
00083 {
00084 v[0] = 0.0;
00085 v[1] = 0.0;
00086 v[2] = 0.0;
00087 }
00088
00089 void
00090 vset(float *v, float x, float y, float z)
00091 {
00092 v[0] = x;
00093 v[1] = y;
00094 v[2] = z;
00095 }
00096
00097 void
00098 vsub(const float *src1, const float *src2, float *dst)
00099 {
00100 dst[0] = src1[0] - src2[0];
00101 dst[1] = src1[1] - src2[1];
00102 dst[2] = src1[2] - src2[2];
00103 }
00104
00105 void
00106 vcopy(const float *v1, float *v2)
00107 {
00108 register int i;
00109 for (i = 0 ; i < 3 ; i++)
00110 v2[i] = v1[i];
00111 }
00112
00113 void
00114 vcross(const float *v1, const float *v2, float *cross)
00115 {
00116 float temp[3];
00117
00118 temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
00119 temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
00120 temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
00121 vcopy(temp, cross);
00122 }
00123
00124 float
00125 vlength(const float *v)
00126 {
00127 return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00128 }
00129
00130 void
00131 vscale(float *v, float div)
00132 {
00133 v[0] *= div;
00134 v[1] *= div;
00135 v[2] *= div;
00136 }
00137
00138 void
00139 vnormal(float *v)
00140 {
00141 vscale(v,1.0/vlength(v));
00142 }
00143
00144 float
00145 vdot(const float *v1, const float *v2)
00146 {
00147 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
00148 }
00149
00150 void
00151 vadd(const float *src1, const float *src2, float *dst)
00152 {
00153 dst[0] = src1[0] + src2[0];
00154 dst[1] = src1[1] + src2[1];
00155 dst[2] = src1[2] + src2[2];
00156 }
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170 void
00171 trackball(float q[4], float p1x, float p1y, float p2x, float p2y)
00172 {
00173 float a[3];
00174 float phi;
00175 float p1[3], p2[3], d[3];
00176 float t;
00177
00178 if (p1x == p2x && p1y == p2y) {
00179
00180 vzero(q);
00181 q[3] = 1.0;
00182 return;
00183 }
00184
00185
00186
00187
00188
00189 vset(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
00190 vset(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
00191
00192
00193
00194
00195 vcross(p2,p1,a);
00196
00197
00198
00199
00200 vsub(p1,p2,d);
00201 t = vlength(d) / (2.0*TRACKBALLSIZE);
00202
00203
00204
00205
00206 if (t > 1.0) t = 1.0;
00207 if (t < -1.0) t = -1.0;
00208 phi = 2.0 * asin(t);
00209
00210 axis_to_quat(a,phi,q);
00211 }
00212
00213
00214
00215
00216 void
00217 axis_to_quat(float a[3], float phi, float q[4])
00218 {
00219 vnormal(a);
00220 vcopy(a,q);
00221 vscale(q,sin(phi/2.0));
00222 q[3] = cos(phi/2.0);
00223 }
00224
00225
00226
00227
00228
00229 static float
00230 tb_project_to_sphere(float r, float x, float y)
00231 {
00232 float d, t, z;
00233
00234 d = sqrt(x*x + y*y);
00235 if (d < r * 0.70710678118654752440) {
00236 z = sqrt(r*r - d*d);
00237 } else {
00238 t = r / 1.41421356237309504880;
00239 z = t*t / d;
00240 }
00241 return z;
00242 }
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255 #define RENORMCOUNT 97
00256
00257 void
00258 negate_quat(float q[4], float nq[4])
00259 {
00260 nq[0] = -q[0];
00261 nq[1] = -q[1];
00262 nq[2] = -q[2];
00263 nq[3] = q[3];
00264 }
00265
00266 void
00267 add_quats(float q1[4], float q2[4], float dest[4])
00268 {
00269 static int count=0;
00270 float t1[4], t2[4], t3[4];
00271 float tf[4];
00272
00273 #if 0
00274 printf("q1 = %f %f %f %f\n", q1[0], q1[1], q1[2], q1[3]);
00275 printf("q2 = %f %f %f %f\n", q2[0], q2[1], q2[2], q2[3]);
00276 #endif
00277
00278 vcopy(q1,t1);
00279 vscale(t1,q2[3]);
00280
00281 vcopy(q2,t2);
00282 vscale(t2,q1[3]);
00283
00284 vcross(q2,q1,t3);
00285 vadd(t1,t2,tf);
00286 vadd(t3,tf,tf);
00287 tf[3] = q1[3] * q2[3] - vdot(q1,q2);
00288
00289 #if 0
00290 printf("tf = %f %f %f %f\n", tf[0], tf[1], tf[2], tf[3]);
00291 #endif
00292
00293 dest[0] = tf[0];
00294 dest[1] = tf[1];
00295 dest[2] = tf[2];
00296 dest[3] = tf[3];
00297
00298 if (++count > RENORMCOUNT) {
00299 count = 0;
00300 normalize_quat(dest);
00301 }
00302 }
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316 static void
00317 normalize_quat(float q[4])
00318 {
00319 int i;
00320 float mag;
00321
00322 mag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
00323 for (i = 0; i < 4; i++) q[i] /= mag;
00324 }
00325
00326
00327
00328
00329
00330 void
00331 build_rotmatrix(float m[4][4], float q[4])
00332 {
00333 m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
00334 m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
00335 m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
00336 m[0][3] = 0.0;
00337
00338 m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
00339 m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
00340 m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
00341 m[1][3] = 0.0;
00342
00343 m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
00344 m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
00345 m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
00346 m[2][3] = 0.0;
00347
00348 m[3][0] = 0.0;
00349 m[3][1] = 0.0;
00350 m[3][2] = 0.0;
00351 m[3][3] = 1.0;
00352 }
00353