sys/src/libgeometry/qball.c

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