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