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