1219b2ee8SDavid du Colombier /*
2219b2ee8SDavid du Colombier * Ken Shoemake's Quaternion rotation controller
3219b2ee8SDavid du Colombier */
4219b2ee8SDavid du Colombier #include <u.h>
5219b2ee8SDavid du Colombier #include <libc.h>
6*7dd7cddfSDavid du Colombier #include <draw.h>
7*7dd7cddfSDavid du Colombier #include <event.h>
8219b2ee8SDavid du Colombier #include <geometry.h>
9219b2ee8SDavid du Colombier #define BORDER 4
10219b2ee8SDavid du Colombier static Point ctlcen; /* center of qball */
11219b2ee8SDavid du Colombier static int ctlrad; /* radius of qball */
12219b2ee8SDavid du Colombier static Quaternion *axis; /* constraint plane orientation, 0 if none */
13219b2ee8SDavid du Colombier /*
14219b2ee8SDavid du Colombier * Convert a mouse point into a unit quaternion, flattening if
15219b2ee8SDavid du Colombier * constrained to a particular plane.
16219b2ee8SDavid du Colombier */
mouseq(Point p)17219b2ee8SDavid du Colombier static Quaternion mouseq(Point p){
18219b2ee8SDavid du Colombier double qx=(double)(p.x-ctlcen.x)/ctlrad;
19219b2ee8SDavid du Colombier double qy=(double)(p.y-ctlcen.y)/ctlrad;
20219b2ee8SDavid du Colombier double rsq=qx*qx+qy*qy;
21219b2ee8SDavid du Colombier double l;
22219b2ee8SDavid du Colombier Quaternion q;
23219b2ee8SDavid du Colombier if(rsq>1){
24219b2ee8SDavid du Colombier rsq=sqrt(rsq);
25*7dd7cddfSDavid du Colombier q.r=0.;
26*7dd7cddfSDavid du Colombier q.i=qx/rsq;
27*7dd7cddfSDavid du Colombier q.j=qy/rsq;
28*7dd7cddfSDavid du Colombier q.k=0.;
29219b2ee8SDavid du Colombier }
30*7dd7cddfSDavid du Colombier else{
31*7dd7cddfSDavid du Colombier q.r=0.;
32*7dd7cddfSDavid du Colombier q.i=qx;
33*7dd7cddfSDavid du Colombier q.j=qy;
34*7dd7cddfSDavid du Colombier q.k=sqrt(1.-rsq);
35*7dd7cddfSDavid du Colombier }
36219b2ee8SDavid du Colombier if(axis){
37219b2ee8SDavid du Colombier l=q.i*axis->i+q.j*axis->j+q.k*axis->k;
38219b2ee8SDavid du Colombier q.i-=l*axis->i;
39219b2ee8SDavid du Colombier q.j-=l*axis->j;
40219b2ee8SDavid du Colombier q.k-=l*axis->k;
41219b2ee8SDavid du Colombier l=sqrt(q.i*q.i+q.j*q.j+q.k*q.k);
42219b2ee8SDavid du Colombier if(l!=0.){
43219b2ee8SDavid du Colombier q.i/=l;
44219b2ee8SDavid du Colombier q.j/=l;
45219b2ee8SDavid du Colombier q.k/=l;
46219b2ee8SDavid du Colombier }
47219b2ee8SDavid du Colombier }
48219b2ee8SDavid du Colombier return q;
49219b2ee8SDavid du Colombier }
qball(Rectangle r,Mouse * m,Quaternion * result,void (* redraw)(void),Quaternion * ap)50219b2ee8SDavid du Colombier void qball(Rectangle r, Mouse *m, Quaternion *result, void (*redraw)(void), Quaternion *ap){
51219b2ee8SDavid du Colombier Quaternion q, down;
52219b2ee8SDavid du Colombier Point rad;
53219b2ee8SDavid du Colombier axis=ap;
54*7dd7cddfSDavid du Colombier ctlcen=divpt(addpt(r.min, r.max), 2);
55*7dd7cddfSDavid du Colombier rad=divpt(subpt(r.max, r.min), 2);
56219b2ee8SDavid du Colombier ctlrad=(rad.x<rad.y?rad.x:rad.y)-BORDER;
57219b2ee8SDavid du Colombier down=qinv(mouseq(m->xy));
58219b2ee8SDavid du Colombier q=*result;
59219b2ee8SDavid du Colombier for(;;){
60219b2ee8SDavid du Colombier *m=emouse();
61219b2ee8SDavid du Colombier if(!m->buttons) break;
62219b2ee8SDavid du Colombier *result=qmul(q, qmul(down, mouseq(m->xy)));
63219b2ee8SDavid du Colombier (*redraw)();
64219b2ee8SDavid du Colombier }
65219b2ee8SDavid du Colombier }
66