1/* 2 * (c) Copyright 1993, 1994, Silicon Graphics, Inc. 3 * ALL RIGHTS RESERVED 4 * Permission to use, copy, modify, and distribute this software for 5 * any purpose and without fee is hereby granted, provided that the above 6 * copyright notice appear in all copies and that both the copyright notice 7 * and this permission notice appear in supporting documentation, and that 8 * the name of Silicon Graphics, Inc. not be used in advertising 9 * or publicity pertaining to distribution of the software without specific, 10 * written prior permission. 11 * 12 * THE MATERIAL EMBODIED ON THIS SOFTWARE IS PROVIDED TO YOU "AS-IS" 13 * AND WITHOUT WARRANTY OF ANY KIND, EXPRESS, IMPLIED OR OTHERWISE, 14 * INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY OR 15 * FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL SILICON 16 * GRAPHICS, INC. BE LIABLE TO YOU OR ANYONE ELSE FOR ANY DIRECT, 17 * SPECIAL, INCIDENTAL, INDIRECT OR CONSEQUENTIAL DAMAGES OF ANY 18 * KIND, OR ANY DAMAGES WHATSOEVER, INCLUDING WITHOUT LIMITATION, 19 * LOSS OF PROFIT, LOSS OF USE, SAVINGS OR REVENUE, OR THE CLAIMS OF 20 * THIRD PARTIES, WHETHER OR NOT SILICON GRAPHICS, INC. HAS BEEN 21 * ADVISED OF THE POSSIBILITY OF SUCH LOSS, HOWEVER CAUSED AND ON 22 * ANY THEORY OF LIABILITY, ARISING OUT OF OR IN CONNECTION WITH THE 23 * POSSESSION, USE OR PERFORMANCE OF THIS SOFTWARE. 24 * 25 * US Government Users Restricted Rights 26 * Use, duplication, or disclosure by the Government is subject to 27 * restrictions set forth in FAR 52.227.19(c)(2) or subparagraph 28 * (c)(1)(ii) of the Rights in Technical Data and Computer Software 29 * clause at DFARS 252.227-7013 and/or in similar or successor 30 * clauses in the FAR or the DOD or NASA FAR Supplement. 31 * Unpublished-- rights reserved under the copyright laws of the 32 * United States. Contractor/manufacturer is Silicon Graphics, 33 * Inc., 2011 N. Shoreline Blvd., Mountain View, CA 94039-7311. 34 * 35 * OpenGL(TM) is a trademark of Silicon Graphics, Inc. 36 */ 37/* 38 * Trackball code: 39 * 40 * Implementation of a virtual trackball. 41 * Implemented by Gavin Bell, lots of ideas from Thant Tessman and 42 * the August '88 issue of Siggraph's "Computer Graphics," pp. 121-129. 43 * 44 * Vector manip code: 45 * 46 * Original code from: 47 * David M. Ciemiewicz, Mark Grossman, Henry Moreton, and Paul Haeberli 48 * 49 * Much mucking with by: 50 * Gavin Bell 51 */ 52#include <math.h> 53#include "trackball.h" 54 55/* 56 * This size should really be based on the distance from the center of 57 * rotation to the point on the object underneath the mouse. That 58 * point would then track the mouse as closely as possible. This is a 59 * simple example, though, so that is left as an Exercise for the 60 * Programmer. 61 */ 62#define TRACKBALLSIZE (0.8f) 63 64/* 65 * Local function prototypes (not defined in trackball.h) 66 */ 67static float tb_project_to_sphere(float, float, float); 68static void normalize_quat(float [4]); 69 70void 71vzero(float *v) 72{ 73 v[0] = 0.0; 74 v[1] = 0.0; 75 v[2] = 0.0; 76} 77 78void 79vset(float *v, float x, float y, float z) 80{ 81 v[0] = x; 82 v[1] = y; 83 v[2] = z; 84} 85 86void 87vsub(const float *src1, const float *src2, float *dst) 88{ 89 dst[0] = src1[0] - src2[0]; 90 dst[1] = src1[1] - src2[1]; 91 dst[2] = src1[2] - src2[2]; 92} 93 94void 95vcopy(const float *v1, float *v2) 96{ 97 register int i; 98 for (i = 0 ; i < 3 ; i++) 99 v2[i] = v1[i]; 100} 101 102void 103vcross(const float *v1, const float *v2, float *cross) 104{ 105 float temp[3]; 106 107 temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]); 108 temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]); 109 temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]); 110 vcopy(temp, cross); 111} 112 113float 114vlength(const float *v) 115{ 116 return (float) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); 117} 118 119void 120vscale(float *v, float div) 121{ 122 v[0] *= div; 123 v[1] *= div; 124 v[2] *= div; 125} 126 127void 128vnormal(float *v) 129{ 130 vscale(v, 1.0f/vlength(v)); 131} 132 133float 134vdot(const float *v1, const float *v2) 135{ 136 return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; 137} 138 139void 140vadd(const float *src1, const float *src2, float *dst) 141{ 142 dst[0] = src1[0] + src2[0]; 143 dst[1] = src1[1] + src2[1]; 144 dst[2] = src1[2] + src2[2]; 145} 146 147/* 148 * Ok, simulate a track-ball. Project the points onto the virtual 149 * trackball, then figure out the axis of rotation, which is the cross 150 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0) 151 * Note: This is a deformed trackball-- is a trackball in the center, 152 * but is deformed into a hyperbolic sheet of rotation away from the 153 * center. This particular function was chosen after trying out 154 * several variations. 155 * 156 * It is assumed that the arguments to this routine are in the range 157 * (-1.0 ... 1.0) 158 */ 159void 160trackball(float q[4], float p1x, float p1y, float p2x, float p2y) 161{ 162 float a[3]; /* Axis of rotation */ 163 float phi; /* how much to rotate about axis */ 164 float p1[3], p2[3], d[3]; 165 float t; 166 167 if (p1x == p2x && p1y == p2y) { 168 /* Zero rotation */ 169 vzero(q); 170 q[3] = 1.0; 171 return; 172 } 173 174 /* 175 * First, figure out z-coordinates for projection of P1 and P2 to 176 * deformed sphere 177 */ 178 vset(p1, p1x, p1y, tb_project_to_sphere(TRACKBALLSIZE, p1x, p1y)); 179 vset(p2, p2x, p2y, tb_project_to_sphere(TRACKBALLSIZE, p2x, p2y)); 180 181 /* 182 * Now, we want the cross product of P1 and P2 183 */ 184 vcross(p2,p1,a); 185 186 /* 187 * Figure out how much to rotate around that axis. 188 */ 189 vsub(p1, p2, d); 190 t = vlength(d) / (2.0f*TRACKBALLSIZE); 191 192 /* 193 * Avoid problems with out-of-control values... 194 */ 195 if (t > 1.0) t = 1.0; 196 if (t < -1.0) t = -1.0; 197 phi = 2.0f * (float) asin(t); 198 199 axis_to_quat(a,phi,q); 200} 201 202/* 203 * Given an axis and angle, compute quaternion. 204 */ 205void 206axis_to_quat(float a[3], float phi, float q[4]) 207{ 208 vnormal(a); 209 vcopy(a, q); 210 vscale(q, (float) sin(phi/2.0)); 211 q[3] = (float) cos(phi/2.0); 212} 213 214/* 215 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet 216 * if we are away from the center of the sphere. 217 */ 218static float 219tb_project_to_sphere(float r, float x, float y) 220{ 221 float d, t, z; 222 223 d = (float) sqrt(x*x + y*y); 224 if (d < r * 0.70710678118654752440) { /* Inside sphere */ 225 z = (float) sqrt(r*r - d*d); 226 } else { /* On hyperbola */ 227 t = r / 1.41421356237309504880f; 228 z = t*t / d; 229 } 230 return z; 231} 232 233/* 234 * Given two rotations, e1 and e2, expressed as quaternion rotations, 235 * figure out the equivalent single rotation and stuff it into dest. 236 * 237 * This routine also normalizes the result every RENORMCOUNT times it is 238 * called, to keep error from creeping in. 239 * 240 * NOTE: This routine is written so that q1 or q2 may be the same 241 * as dest (or each other). 242 */ 243 244#define RENORMCOUNT 97 245 246void 247add_quats(float q1[4], float q2[4], float dest[4]) 248{ 249 static int count=0; 250 float t1[4], t2[4], t3[4]; 251 float tf[4]; 252 253 vcopy(q1,t1); 254 vscale(t1,q2[3]); 255 256 vcopy(q2,t2); 257 vscale(t2,q1[3]); 258 259 vcross(q2,q1,t3); 260 vadd(t1,t2,tf); 261 vadd(t3,tf,tf); 262 tf[3] = q1[3] * q2[3] - vdot(q1,q2); 263 264 dest[0] = tf[0]; 265 dest[1] = tf[1]; 266 dest[2] = tf[2]; 267 dest[3] = tf[3]; 268 269 if (++count > RENORMCOUNT) { 270 count = 0; 271 normalize_quat(dest); 272 } 273} 274 275/* 276 * Quaternions always obey: a^2 + b^2 + c^2 + d^2 = 1.0 277 * If they don't add up to 1.0, dividing by their magnitued will 278 * renormalize them. 279 * 280 * Note: See the following for more information on quaternions: 281 * 282 * - Shoemake, K., Animating rotation with quaternion curves, Computer 283 * Graphics 19, No 3 (Proc. SIGGRAPH'85), 245-254, 1985. 284 * - Pletinckx, D., Quaternion calculus as a basic tool in computer 285 * graphics, The Visual Computer 5, 2-13, 1989. 286 */ 287static void 288normalize_quat(float q[4]) 289{ 290 int i; 291 float mag; 292 293 mag = (q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); 294 for (i = 0; i < 4; i++) q[i] /= mag; 295} 296 297/* 298 * Build a rotation matrix, given a quaternion rotation. 299 * 300 */ 301void 302build_rotmatrix(float m[4][4], float q[4]) 303{ 304 m[0][0] = 1.0f - 2.0f * (q[1] * q[1] + q[2] * q[2]); 305 m[0][1] = 2.0f * (q[0] * q[1] - q[2] * q[3]); 306 m[0][2] = 2.0f * (q[2] * q[0] + q[1] * q[3]); 307 m[0][3] = 0.0f; 308 309 m[1][0] = 2.0f * (q[0] * q[1] + q[2] * q[3]); 310 m[1][1]= 1.0f - 2.0f * (q[2] * q[2] + q[0] * q[0]); 311 m[1][2] = 2.0f * (q[1] * q[2] - q[0] * q[3]); 312 m[1][3] = 0.0f; 313 314 m[2][0] = 2.0f * (q[2] * q[0] - q[1] * q[3]); 315 m[2][1] = 2.0f * (q[1] * q[2] + q[0] * q[3]); 316 m[2][2] = 1.0f - 2.0f * (q[1] * q[1] + q[0] * q[0]); 317 m[2][3] = 0.0f; 318 319 m[3][0] = 0.0f; 320 m[3][1] = 0.0f; 321 m[3][2] = 0.0f; 322 m[3][3] = 1.0f; 323} 324 325