sys/src/libgeometry/arith3.c

   1 #include <u.h>
   2 #include <libc.h>
   3 #include <draw.h>
   4 #include <geometry.h>
   5 /*
   6  * Routines whose names end in 3 work on points in Affine 3-space.
   7  * They ignore w in all arguments and produce w=1 in all results.
   8  * Routines whose names end in 4 work on points in Projective 3-space.
   9  */
  10 Point3 add3(Point3 a, Point3 b){
  11         a.x+=b.x;
  12         a.y+=b.y;
  13         a.z+=b.z;
  14         a.w=1.;
  15         return a;
  16 }
  17 Point3 sub3(Point3 a, Point3 b){
  18         a.x-=b.x;
  19         a.y-=b.y;
  20         a.z-=b.z;
  21         a.w=1.;
  22         return a;
  23 }
  24 Point3 neg3(Point3 a){
  25         a.x=-a.x;
  26         a.y=-a.y;
  27         a.z=-a.z;
  28         a.w=1.;
  29         return a;
  30 }
  31 Point3 div3(Point3 a, double b){
  32         a.x/=b;
  33         a.y/=b;
  34         a.z/=b;
  35         a.w=1.;
  36         return a;
  37 }
  38 Point3 mul3(Point3 a, double b){
  39         a.x*=b;
  40         a.y*=b;
  41         a.z*=b;
  42         a.w=1.;
  43         return a;
  44 }
  45 int eqpt3(Point3 p, Point3 q){
  46         return p.x==q.x && p.y==q.y && p.z==q.z;
  47 }
  48 /*
  49  * Are these points closer than eps, in a relative sense
  50  */
  51 int closept3(Point3 p, Point3 q, double eps){
  52         return 2.*dist3(p, q)<eps*(len3(p)+len3(q));
  53 }
  54 double dot3(Point3 p, Point3 q){
  55         return p.x*q.x+p.y*q.y+p.z*q.z;
  56 }
  57 Point3 cross3(Point3 p, Point3 q){
  58         Point3 r;
  59         r.x=p.y*q.z-p.z*q.y;
  60         r.y=p.z*q.x-p.x*q.z;
  61         r.z=p.x*q.y-p.y*q.x;
  62         r.w=1.;
  63         return r;
  64 }
  65 double len3(Point3 p){
  66         return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
  67 }
  68 double dist3(Point3 p, Point3 q){
  69         p.x-=q.x;
  70         p.y-=q.y;
  71         p.z-=q.z;
  72         return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
  73 }
  74 Point3 unit3(Point3 p){
  75         double len=sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
  76         p.x/=len;
  77         p.y/=len;
  78         p.z/=len;
  79         p.w=1.;
  80         return p;
  81 }
  82 Point3 midpt3(Point3 p, Point3 q){
  83         p.x=.5*(p.x+q.x);
  84         p.y=.5*(p.y+q.y);
  85         p.z=.5*(p.z+q.z);
  86         p.w=1.;
  87         return p;
  88 }
  89 Point3 lerp3(Point3 p, Point3 q, double alpha){
  90         p.x+=(q.x-p.x)*alpha;
  91         p.y+=(q.y-p.y)*alpha;
  92         p.z+=(q.z-p.z)*alpha;
  93         p.w=1.;
  94         return p;
  95 }
  96 /*
  97  * Reflect point p in the line joining p0 and p1
  98  */
  99 Point3 reflect3(Point3 p, Point3 p0, Point3 p1){
 100         Point3 a, b;
 101         a=sub3(p, p0);
 102         b=sub3(p1, p0);
 103         return add3(a, mul3(b, 2*dot3(a, b)/dot3(b, b)));
 104 }
 105 /*
 106  * Return the nearest point on segment [p0,p1] to point testp
 107  */
 108 Point3 nearseg3(Point3 p0, Point3 p1, Point3 testp){
 109         double num, den;
 110         Point3 q, r;
 111         q=sub3(p1, p0);
 112         r=sub3(testp, p0);
 113         num=dot3(q, r);;
 114         if(num<=0) return p0;
 115         den=dot3(q, q);
 116         if(num>=den) return p1;
 117         return add3(p0, mul3(q, num/den));
 118 }
 119 /*
 120  * distance from point p to segment [p0,p1]
 121  */
 122 #define SMALL   1e-8    /* what should this value be? */
 123 double pldist3(Point3 p, Point3 p0, Point3 p1){
 124         Point3 d, e;
 125         double dd, de, dsq;
 126         d=sub3(p1, p0);
 127         e=sub3(p, p0);
 128         dd=dot3(d, d);
 129         de=dot3(d, e);
 130         if(dd<SMALL*SMALL) return len3(e);
 131         dsq=dot3(e, e)-de*de/dd;
 132         if(dsq<SMALL*SMALL) return 0;
 133         return sqrt(dsq);
 134 }
 135 /*
 136  * vdiv3(a, b) is the magnitude of the projection of a onto b
 137  * measured in units of the length of b.
 138  * vrem3(a, b) is the component of a perpendicular to b.
 139  */
 140 double vdiv3(Point3 a, Point3 b){
 141         return (a.x*b.x+a.y*b.y+a.z*b.z)/(b.x*b.x+b.y*b.y+b.z*b.z);
 142 }
 143 Point3 vrem3(Point3 a, Point3 b){
 144         double quo=(a.x*b.x+a.y*b.y+a.z*b.z)/(b.x*b.x+b.y*b.y+b.z*b.z);
 145         a.x-=b.x*quo;
 146         a.y-=b.y*quo;
 147         a.z-=b.z*quo;
 148         a.w=1.;
 149         return a;
 150 }
 151 /*
 152  * Compute face (plane) with given normal, containing a given point
 153  */
 154 Point3 pn2f3(Point3 p, Point3 n){
 155         n.w=-dot3(p, n);
 156         return n;
 157 }
 158 /*
 159  * Compute face containing three points
 160  */
 161 Point3 ppp2f3(Point3 p0, Point3 p1, Point3 p2){
 162         Point3 p01, p02;
 163         p01=sub3(p1, p0);
 164         p02=sub3(p2, p0);
 165         return pn2f3(p0, cross3(p01, p02));
 166 }
 167 /*
 168  * Compute point common to three faces.
 169  * Cramer's rule, yuk.
 170  */
 171 Point3 fff2p3(Point3 f0, Point3 f1, Point3 f2){
 172         double det;
 173         Point3 p;
 174         det=dot3(f0, cross3(f1, f2));
 175         if(fabs(det)<SMALL){    /* parallel planes, bogus answer */
 176                 p.x=0.;
 177                 p.y=0.;
 178                 p.z=0.;
 179                 p.w=0.;
 180                 return p;
 181         }
 182         p.x=(f0.w*(f2.y*f1.z-f1.y*f2.z)
 183                 +f1.w*(f0.y*f2.z-f2.y*f0.z)+f2.w*(f1.y*f0.z-f0.y*f1.z))/det;
 184         p.y=(f0.w*(f2.z*f1.x-f1.z*f2.x)
 185                 +f1.w*(f0.z*f2.x-f2.z*f0.x)+f2.w*(f1.z*f0.x-f0.z*f1.x))/det;
 186         p.z=(f0.w*(f2.x*f1.y-f1.x*f2.y)
 187                 +f1.w*(f0.x*f2.y-f2.x*f0.y)+f2.w*(f1.x*f0.y-f0.x*f1.y))/det;
 188         p.w=1.;
 189         return p;
 190 }
 191 /*
 192  * pdiv4 does perspective division to convert a projective point to affine coordinates.
 193  */
 194 Point3 pdiv4(Point3 a){
 195         if(a.w==0) return a;
 196         a.x/=a.w;
 197         a.y/=a.w;
 198         a.z/=a.w;
 199         a.w=1.;
 200         return a;
 201 }
 202 Point3 add4(Point3 a, Point3 b){
 203         a.x+=b.x;
 204         a.y+=b.y;
 205         a.z+=b.z;
 206         a.w+=b.w;
 207         return a;
 208 }
 209 Point3 sub4(Point3 a, Point3 b){
 210         a.x-=b.x;
 211         a.y-=b.y;
 212         a.z-=b.z;
 213         a.w-=b.w;
 214         return a;
 215 }