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