00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <math.h>
00018
00019 #include "trackball.h"
00020 #include <GL/gl.h>
00021 #include <GL/glu.h>
00022
00023
00024
00025
00026
00027
00028
00029
00030 #define TRACKBALLSIZE (0.5)
00031
00032
00033 int spinning = 1, moving = 0;
00034 int beginx, beginy;
00035 float curquat[4];
00036 float lastquat[4];
00037 int scaling;
00038 float scalefactor = 1.0;
00039
00040
00041
00042
00043 static float tb_project_to_sphere(float, float, float);
00044 static void normalize_quat(float [4]);
00045
00046 void
00047 vzero(float *v)
00048 {
00049 v[0] = 0.0;
00050 v[1] = 0.0;
00051 v[2] = 0.0;
00052 }
00053
00054 void
00055 vset(float *v, float x, float y, float z)
00056 {
00057 v[0] = x;
00058 v[1] = y;
00059 v[2] = z;
00060 }
00061
00062 void
00063 vsub(const float *src1, const float *src2, float *dst)
00064 {
00065 dst[0] = src1[0] - src2[0];
00066 dst[1] = src1[1] - src2[1];
00067 dst[2] = src1[2] - src2[2];
00068 }
00069
00070 void
00071 vcopy(const float *v1, float *v2)
00072 {
00073 register int i;
00074 for (i = 0 ; i < 3 ; i++)
00075 v2[i] = v1[i];
00076 }
00077
00078 void
00079 vcross(const float *v1, const float *v2, float *cross)
00080 {
00081 float temp[3];
00082
00083 temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
00084 temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
00085 temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
00086 vcopy(temp, cross);
00087 }
00088
00089 float
00090 vlength(const float *v)
00091 {
00092 return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00093 }
00094
00095 void
00096 vscale(float *v, float div)
00097 {
00098 v[0] *= div;
00099 v[1] *= div;
00100 v[2] *= div;
00101 }
00102
00103 void
00104 vnormal(float *v)
00105 {
00106 vscale(v,1.0/vlength(v));
00107 }
00108
00109 float
00110 vdot(const float *v1, const float *v2)
00111 {
00112 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
00113 }
00114
00115 void
00116 vadd(const float *src1, const float *src2, float *dst)
00117 {
00118 dst[0] = src1[0] + src2[0];
00119 dst[1] = src1[1] + src2[1];
00120 dst[2] = src1[2] + src2[2];
00121 }
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 void trackball(float q[4], float p1x, float p1y, float p2x, float p2y)
00136 {
00137 float a[3];
00138 float phi;
00139 float p1[3], p2[3], d[3];
00140 float t;
00141
00142 if (p1x == p2x && p1y == p2y) {
00143
00144 vzero(q);
00145 q[3] = 1.0;
00146 return;
00147 }
00148
00149
00150
00151
00152
00153 vset(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
00154 vset(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
00155
00156
00157
00158
00159 vcross(p2,p1,a);
00160
00161
00162
00163
00164 vsub(p1,p2,d);
00165 t = vlength(d) / (2.0*TRACKBALLSIZE);
00166
00167
00168
00169
00170 if (t > 1.0) t = 1.0;
00171 if (t < -1.0) t = -1.0;
00172 phi = 2.0 * asin(t);
00173
00174 axis_to_quat(a,phi,q);
00175 }
00176
00177
00178
00179
00180 void axis_to_quat(float a[3], float phi, float q[4])
00181 {
00182 vnormal(a);
00183 vcopy(a,q);
00184 vscale(q,sin(phi/2.0));
00185 q[3] = cos(phi/2.0);
00186 }
00187
00188
00189
00190
00191
00192 static float tb_project_to_sphere(float r, float x, float y)
00193 {
00194 float d, t, z;
00195
00196 d = sqrt(x*x + y*y);
00197 if (d < r * 0.70710678118654752440) {
00198 z = sqrt(r*r - d*d);
00199 } else {
00200 t = r / 1.41421356237309504880;
00201 z = t*t / d;
00202 }
00203 return z;
00204 }
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217 #define RENORMCOUNT 97
00218
00219 void add_quats(float q1[4], float q2[4], float dest[4])
00220 {
00221 static int count=0;
00222 float t1[4], t2[4], t3[4];
00223 float tf[4];
00224
00225 vcopy(q1,t1);
00226 vscale(t1,q2[3]);
00227
00228 vcopy(q2,t2);
00229 vscale(t2,q1[3]);
00230
00231 vcross(q2,q1,t3);
00232 vadd(t1,t2,tf);
00233 vadd(t3,tf,tf);
00234 tf[3] = q1[3] * q2[3] - vdot(q1,q2);
00235
00236 dest[0] = tf[0];
00237 dest[1] = tf[1];
00238 dest[2] = tf[2];
00239 dest[3] = tf[3];
00240
00241 if (++count > RENORMCOUNT) {
00242 count = 0;
00243 normalize_quat(dest);
00244 }
00245 }
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259 static void normalize_quat(float q[4])
00260 {
00261 int i;
00262 float mag;
00263
00264 mag = (q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
00265 for (i = 0; i < 4; i++) q[i] /= mag;
00266 }
00267
00268
00269
00270
00271
00272 void build_rotmatrix(float m[4][4], float q[4])
00273 {
00274 m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
00275 m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
00276 m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
00277 m[0][3] = 0.0;
00278
00279 m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
00280 m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
00281 m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
00282 m[1][3] = 0.0;
00283
00284 m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
00285 m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
00286 m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
00287 m[2][3] = 0.0;
00288
00289 m[3][0] = 0.0;
00290 m[3][1] = 0.0;
00291 m[3][2] = 0.0;
00292 m[3][3] = 1.0;
00293 }
00294