1 d1e9002f 2005-01-04 devnull /*
 2 d1e9002f 2005-01-04 devnull  * Ken Shoemake's Quaternion rotation controller
 3 d1e9002f 2005-01-04 devnull  */
 4 d1e9002f 2005-01-04 devnull #include <u.h>
 5 d1e9002f 2005-01-04 devnull #include <libc.h>
 6 d1e9002f 2005-01-04 devnull #include <draw.h>
 7 d1e9002f 2005-01-04 devnull #include <stdio.h>
 8 d1e9002f 2005-01-04 devnull #include <event.h>
 9 d1e9002f 2005-01-04 devnull #include <geometry.h>
10 d1e9002f 2005-01-04 devnull #define	BORDER	4
11 d1e9002f 2005-01-04 devnull static Point ctlcen;		/* center of qball */
12 d1e9002f 2005-01-04 devnull static int ctlrad;		/* radius of qball */
13 d1e9002f 2005-01-04 devnull static Quaternion *axis;	/* constraint plane orientation, 0 if none */
14 d1e9002f 2005-01-04 devnull /*
15 d1e9002f 2005-01-04 devnull  * Convert a mouse point into a unit quaternion, flattening if
16 d1e9002f 2005-01-04 devnull  * constrained to a particular plane.
17 d1e9002f 2005-01-04 devnull  */
18 d1e9002f 2005-01-04 devnull static Quaternion mouseq(Point p){
19 d1e9002f 2005-01-04 devnull 	double qx=(double)(p.x-ctlcen.x)/ctlrad;
20 d1e9002f 2005-01-04 devnull 	double qy=(double)(p.y-ctlcen.y)/ctlrad;
21 d1e9002f 2005-01-04 devnull 	double rsq=qx*qx+qy*qy;
22 d1e9002f 2005-01-04 devnull 	double l;
23 d1e9002f 2005-01-04 devnull 	Quaternion q;
24 d1e9002f 2005-01-04 devnull 	if(rsq>1){
25 d1e9002f 2005-01-04 devnull 		rsq=sqrt(rsq);
26 d1e9002f 2005-01-04 devnull 		q.r=0.;
27 d1e9002f 2005-01-04 devnull 		q.i=qx/rsq;
28 d1e9002f 2005-01-04 devnull 		q.j=qy/rsq;
29 d1e9002f 2005-01-04 devnull 		q.k=0.;
30 d1e9002f 2005-01-04 devnull 	}
31 d1e9002f 2005-01-04 devnull 	else{
32 d1e9002f 2005-01-04 devnull 		q.r=0.;
33 d1e9002f 2005-01-04 devnull 		q.i=qx;
34 d1e9002f 2005-01-04 devnull 		q.j=qy;
35 d1e9002f 2005-01-04 devnull 		q.k=sqrt(1.-rsq);
36 d1e9002f 2005-01-04 devnull 	}
37 d1e9002f 2005-01-04 devnull 	if(axis){
38 d1e9002f 2005-01-04 devnull 		l=q.i*axis->i+q.j*axis->j+q.k*axis->k;
39 d1e9002f 2005-01-04 devnull 		q.i-=l*axis->i;
40 d1e9002f 2005-01-04 devnull 		q.j-=l*axis->j;
41 d1e9002f 2005-01-04 devnull 		q.k-=l*axis->k;
42 d1e9002f 2005-01-04 devnull 		l=sqrt(q.i*q.i+q.j*q.j+q.k*q.k);
43 d1e9002f 2005-01-04 devnull 		if(l!=0.){
44 d1e9002f 2005-01-04 devnull 			q.i/=l;
45 d1e9002f 2005-01-04 devnull 			q.j/=l;
46 d1e9002f 2005-01-04 devnull 			q.k/=l;
47 d1e9002f 2005-01-04 devnull 		}
48 d1e9002f 2005-01-04 devnull 	}
49 d1e9002f 2005-01-04 devnull 	return q;
50 d1e9002f 2005-01-04 devnull }
51 d1e9002f 2005-01-04 devnull void qball(Rectangle r, Mouse *m, Quaternion *result, void (*redraw)(void), Quaternion *ap){
52 d1e9002f 2005-01-04 devnull 	Quaternion q, down;
53 d1e9002f 2005-01-04 devnull 	Point rad;
54 d1e9002f 2005-01-04 devnull 	axis=ap;
55 d1e9002f 2005-01-04 devnull 	ctlcen=divpt(addpt(r.min, r.max), 2);
56 d1e9002f 2005-01-04 devnull 	rad=divpt(subpt(r.max, r.min), 2);
57 d1e9002f 2005-01-04 devnull 	ctlrad=(rad.x<rad.y?rad.x:rad.y)-BORDER;
58 d1e9002f 2005-01-04 devnull 	down=qinv(mouseq(m->xy));
59 d1e9002f 2005-01-04 devnull 	q=*result;
60 d1e9002f 2005-01-04 devnull 	for(;;){
61 d1e9002f 2005-01-04 devnull 		*m=emouse();
62 d1e9002f 2005-01-04 devnull 		if(!m->buttons) break;
63 d1e9002f 2005-01-04 devnull 		*result=qmul(q, qmul(down, mouseq(m->xy)));
64 d1e9002f 2005-01-04 devnull 		(*redraw)();
65 d1e9002f 2005-01-04 devnull 	}
66 d1e9002f 2005-01-04 devnull }