00001 #include "SUMA_suma.h"
00002 
00003 extern SUMA_CommonFields *SUMAg_CF; 
00004 
00005 /* Copyright (c) Mark J. Kilgard, 1996. */
00006 
00007 /* This program is freely distributable without licensing fees 
00008    and is provided without guarantee or warrantee expressed or 
00009    implied. This program is -not- in the public domain. */
00010 
00011 /* This size should really be based on the distance from the
00012    center of rotation to the point on the object underneath the
00013    mouse.  That point would then track the mouse as closely as
00014    possible.  This is a simple example, though, so that is left
00015    as an Exercise for the Programmer. */
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];           /* Axis of rotation. */
00102   float phi;            /* How much to rotate about axis. */
00103   float p1[3], p2[3], d[3];
00104   float t;
00105 
00106   if (p1x == p2x && p1y == p2y) {
00107     /* Zero rotation */
00108     vzero(q);
00109     q[3] = 1.0;
00110     return;
00111   }
00112   /* First, figure out z-coordinates for projection of P1 and
00113      P2 to deformed sphere. */
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   /* Now, we want the cross product of P1 and P2. */
00118   vcross(p2, p1, a);
00119   /* Figure out how much to rotate around that axis. */
00120   vsub(p1, p2, d);
00121   t = vlength(d) / (2.0 * TRACKBALLSIZE);
00122 
00123   /* Avoid problems with out-of-control values. */
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    A modification/hack of trackball function to control the rotation angle directement
00134 */
00135 void
00136 trackball_Phi(float q[4], float p1x, float p1y, float p2x, float p2y, float phi)
00137 {
00138   float a[3];           /* Axis of rotation. */
00139   float p1[3], p2[3], d[3];
00140   float t;
00141 
00142   if (p1x == p2x && p1y == p2y) {
00143     /* Zero rotation */
00144     vzero(q);
00145     q[3] = 1.0;
00146     return;
00147   }
00148   /* First, figure out z-coordinates for projection of P1 and
00149      P2 to deformed sphere. */
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   /* Now, we want the cross product of P1 and P2. */
00154   vcross(p2, p1, a);
00155   /* Figure out how much to rotate around that axis. */
00156   vsub(p1, p2, d);
00157   t = vlength(d) / (2.0 * TRACKBALLSIZE);
00158 
00159   /* Avoid problems with out-of-control values. */
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 /* Given an axis and angle, compute quaternion. */
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 /* Project an x,y pair onto a sphere of radius r OR a
00183    hyperbolic sheet if we are away from the center of the
00184    sphere. */
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) {  /* Inside sphere. */
00192     z = sqrt(r * r - d * d);
00193   } else {              /* On hyperbola. */
00194     t = r / 1.41421356237309504880;
00195     z = t * t / d;
00196   }
00197   return z;
00198 }
00199 
00200 /* Given two rotations, e1 and e2, expressed as quaternion
00201    rotations, figure out the equivalent single rotation and
00202    stuff it into dest.  This routine also normalizes the result
00203    every RENORMCOUNT times it is called, to keep error from
00204    creeping in.  NOTE: This routine is written so that q1 or q2
00205    may be the same as dest (or each other). */
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 /* Quaternions always obey:  a^2 + b^2 + c^2 + d^2 = 1.0 If
00239    they don't add up to 1.0, dividing by their magnitude will
00240    renormalize them. */
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 /* Build a rotation matrix, given a quaternion rotation. */
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 }