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