src/GUI/trackball.c

   1 /*
   2  * Trackball code:
   3  *
   4  * Implementation of a virtual trackball.
   5  * Implemented by Gavin Bell, lots of ideas from Thant Tessman and
   6  *   the August '88 issue of Siggraph's "Computer Graphics," pp. 121-129.
   7  *
   8  * Vector manip code:
   9  *
  10  * Original code from:
  11  * David M. Ciemiewicz, Mark Grossman, Henry Moreton, and Paul Haeberli
  12  *
  13  * Much mucking with by:
  14  * Gavin Bell
  15  */
  16
  17 #if defined(_WIN32) && !defined( __CYGWIN32__ )
  18 #pragma warning (disable:4244)          /* disable bogus conversion warnings */
  19 #endif
  20 #include <math.h>
  21 #include <stdio.h>
  22 #include "trackball.h"
  23
  24 /*
  25  * This size should really be based on the distance from the center of
  26  * rotation to the point on the object underneath the mouse.  That
  27  * point would then track the mouse as closely as possible.  This is a
  28  * simple example, though, so that is left as an Exercise for the
  29  * Programmer.
  30  */
  31 #define TRACKBALLSIZE  (0.8f)
  32 #define SQRT(x) sqrt(x)
  33
  34 /*
  35  * Local function prototypes (not defined in trackball.h)
  36  */
  37 static float tb_project_to_sphere(float, float, float);
  38 static void normalize_quat(float [4]);
  39
  40 static void
  41    vzero(float *v)
  42 {
  43         v[0] = 0.0;
  44         v[1] = 0.0;
  45         v[2] = 0.0;
  46 }
  47
  48 static void
  49    vset(float *v, float x, float y, float z)
  50 {
  51         v[0] = x;
  52         v[1] = y;
  53         v[2] = z;
  54 }
  55
  56 static void
  57    vsub(const float *src1, const float *src2, float *dst)
  58 {
  59         dst[0] = src1[0] - src2[0];
  60         dst[1] = src1[1] - src2[1];
  61         dst[2] = src1[2] - src2[2];
  62 }
  63
  64 static void
  65    vcopy(const float *v1, float *v2)
  66 {
  67         register int i;
  68         for (i = 0 ; i < 3 ; i++)
  69                 v2[i] = v1[i];
  70 }
  71
  72 static void
  73    vcross(const float *v1, const float *v2, float *cross)
  74 {
  75         float temp[3];
  76
  77         temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
  78         temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
  79         temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
  80         vcopy(temp, cross);
  81 }
  82
  83 static float
  84    vlength(const float *v)
  85 {
  86         float tmp = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
  87         return SQRT(tmp);
  88 }
  89
  90 static void
  91    vscale(float *v, float div)
  92 {
  93         v[0] *= div;
  94         v[1] *= div;
  95         v[2] *= div;
  96 }
  97
  98 static void
  99    vnormal(float *v)
 100 {
 101         vscale(v,1.0/vlength(v));
 102 }
 103
 104 static float
 105    vdot(const float *v1, const float *v2)
 106 {
 107         return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
 108 }
 109
 110 static void
 111    vadd(const float *src1, const float *src2, float *dst)
 112 {
 113         dst[0] = src1[0] + src2[0];
 114         dst[1] = src1[1] + src2[1];
 115         dst[2] = src1[2] + src2[2];
 116 }
 117
 118 /*
 119  *  Given an axis and angle, compute quaternion.
 120  */
 121 void
 122    axis_to_quat(float a[3], float phi, float q[4])
 123 {
 124         double sinphi2, cosphi2;
 125         double phi2 = phi/2.0;
 126         sinphi2 = sin(phi2);
 127         cosphi2 = cos(phi2);
 128         vnormal(a);
 129         vcopy(a,q);
 130         vscale(q,sinphi2);
 131         q[3] = cosphi2;
 132 }
 133
 134 /*
 135  * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
 136  * if we are away from the center of the sphere.
 137  */
 138 static float
 139    tb_project_to_sphere(float r, float x, float y)
 140 {
 141         float d, t, z, tmp;
 142
 143         tmp = x*x + y*y;
 144         d = SQRT(tmp);
 145         if (d < r * 0.70710678118654752440) {    /* Inside sphere */
 146                 tmp = r*r - d*d;
 147                 z = SQRT(tmp);
 148         } else {           /* On hyperbola */
 149                 t = r / 1.41421356237309504880;
 150                 z = t*t / d;
 151         }
 152         return z;
 153 }
 154
 155 /*
 156  * Quaternions always obey:  a^2 + b^2 + c^2 + d^2 = 1.0
 157  * If they don't add up to 1.0, dividing by their magnitued will
 158  * renormalize them.
 159  *
 160  * Note: See the following for more information on quaternions:
 161  *
 162  * - Shoemake, K., Animating rotation with quaternion curves, Computer
 163  *   Graphics 19, No 3 (Proc. SIGGRAPH'85), 245-254, 1985.
 164  * - Pletinckx, D., Quaternion calculus as a basic tool in computer
 165  *   graphics, The Visual Computer 5, 2-13, 1989.
 166  */
 167 static void
 168    normalize_quat(float q[4])
 169 {
 170         int i;
 171         float mag, tmp;
 172
 173         tmp = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
 174         mag = 1.0 / SQRT(tmp);
 175         for (i = 0; i < 4; i++)
 176                 q[i] *= mag;
 177 }
 178
 179 /*
 180  * Ok, simulate a track-ball.  Project the points onto the virtual
 181  * trackball, then figure out the axis of rotation, which is the cross
 182  * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 183  * Note:  This is a deformed trackball-- is a trackball in the center,
 184  * but is deformed into a hyperbolic sheet of rotation away from the
 185  * center.  This particular function was chosen after trying out
 186  * several variations.
 187  *
 188  * It is assumed that the arguments to this routine are in the range
 189  * (-1.0 ... 1.0)
 190  */
 191 void
 192    trackball(float q[4], float p1x, float p1y, float p2x, float p2y)
 193 {
 194         float a[3]; /* Axis of rotation */
 195         float phi;  /* how much to rotate about axis */
 196         float p1[3], p2[3], d[3];
 197         float t;
 198
 199         if (p1x == p2x && p1y == p2y) {
 200                 /* Zero rotation */
 201                 vzero(q);
 202                 q[3] = 1.0;
 203                 return;
 204         }
 205
 206         /*
 207          * First, figure out z-coordinates for projection of P1 and P2 to
 208          * deformed sphere
 209          */
 210         vset(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
 211         vset(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
 212
 213         /*
 214          *  Now, we want the cross product of P1 and P2
 215          */
 216         vcross(p2,p1,a);
 217
 218         /*
 219          *  Figure out how much to rotate around that axis.
 220          */
 221         vsub(p1,p2,d);
 222         t = vlength(d) / (2.0*TRACKBALLSIZE);
 223
 224         /*
 225          * Avoid problems with out-of-control values...
 226          */
 227         if (t > 1.0) t = 1.0;
 228         if (t < -1.0) t = -1.0;
 229         phi = 2.0 * asin(t);
 230
 231         axis_to_quat(a,phi,q);
 232 }
 233
 234 /*
 235  * Given two rotations, e1 and e2, expressed as quaternion rotations,
 236  * figure out the equivalent single rotation and stuff it into dest.
 237  *
 238  * This routine also normalizes the result every RENORMCOUNT times it is
 239  * called, to keep error from creeping in.
 240  *
 241  * NOTE: This routine is written so that q1 or q2 may be the same
 242  * as dest (or each other).
 243  */
 244
 245 #define RENORMCOUNT 97
 246
 247 void
 248    add_quats(float q1[4], float q2[4], float dest[4])
 249 {
 250         static int count=0;
 251         float t1[4], t2[4], t3[4];
 252         float tf[4];
 253
 254 #if 0
 255         printf("q1 = %f %f %f %f\n", q1[0], q1[1], q1[2], q1[3]);
 256         printf("q2 = %f %f %f %f\n", q2[0], q2[1], q2[2], q2[3]);
 257 #endif
 258
 259         vcopy(q1,t1);
 260         vscale(t1,q2[3]);
 261
 262         vcopy(q2,t2);
 263         vscale(t2,q1[3]);
 264
 265         vcross(q2,q1,t3);
 266         vadd(t1,t2,tf);
 267         vadd(t3,tf,tf);
 268         tf[3] = q1[3] * q2[3] - vdot(q1,q2);
 269
 270 #if 0
 271         printf("tf = %f %f %f %f\n", tf[0], tf[1], tf[2], tf[3]);
 272 #endif
 273
 274         dest[0] = tf[0];
 275         dest[1] = tf[1];
 276         dest[2] = tf[2];
 277         dest[3] = tf[3];
 278
 279         if (++count > RENORMCOUNT) {
 280                 count = 0;
 281                 normalize_quat(dest);
 282         }
 283 }
 284
 285 /*
 286  * Build a rotation matrix, given a quaternion rotation.
 287  *
 288  */
 289 void  build_rotmatrix(float m[4][4], float q[4])
 290 {
 291         m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
 292         m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
 293         m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
 294         m[0][3] = 0.0;
 295
 296         m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
 297         m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
 298         m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
 299         m[1][3] = 0.0;
 300
 301         m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
 302         m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
 303         m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
 304
 305         m[2][3] = 0.0;
 306         m[3][0] = 0.0;
 307         m[3][1] = 0.0;
 308         m[3][2] = 0.0;
 309         m[3][3] = 1.0;
 310 }
 311
 312 void  build_transposed_rotmatrix(float m[4][4], float q[4])
 313 {
 314         m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
 315         m[0][1] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
 316         m[0][2] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
 317         m[0][3] = 0.0;
 318
 319         m[1][0] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
 320         m[1][1] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
 321         m[1][2] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
 322         m[1][3] = 0.0;
 323
 324         m[2][0] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
 325         m[2][1] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
 326         m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
 327         m[2][3] = 0.0;
 328
 329         m[3][0] = 0.0;
 330         m[3][1] = 0.0;
 331         m[3][2] = 0.0;
 332         m[3][3] = 1.0;
 333 }
 334
 335