#include <u.h>
#include <libc.h>
#include <draw.h>
#include <geometry.h>
/*
* Routines whose names end in 3 work on points in Affine 3-space.
* They ignore w in all arguments and produce w=1 in all results.
* Routines whose names end in 4 work on points in Projective 3-space.
*/
Point3 add3(Point3 a, Point3 b){
       a.x+=b.x;
       a.y+=b.y;
       a.z+=b.z;
       a.w=1.;
       return a;
}
Point3 sub3(Point3 a, Point3 b){
       a.x-=b.x;
       a.y-=b.y;
       a.z-=b.z;
       a.w=1.;
       return a;
}
Point3 neg3(Point3 a){
       a.x=-a.x;
       a.y=-a.y;
       a.z=-a.z;
       a.w=1.;
       return a;
}
Point3 div3(Point3 a, double b){
       a.x/=b;
       a.y/=b;
       a.z/=b;
       a.w=1.;
       return a;
}
Point3 mul3(Point3 a, double b){
       a.x*=b;
       a.y*=b;
       a.z*=b;
       a.w=1.;
       return a;
}
int eqpt3(Point3 p, Point3 q){
       return p.x==q.x && p.y==q.y && p.z==q.z;
}
/*
* Are these points closer than eps, in a relative sense
*/
int closept3(Point3 p, Point3 q, double eps){
       return 2.*dist3(p, q)<eps*(len3(p)+len3(q));
}
double dot3(Point3 p, Point3 q){
       return p.x*q.x+p.y*q.y+p.z*q.z;
}
Point3 cross3(Point3 p, Point3 q){
       Point3 r;
       r.x=p.y*q.z-p.z*q.y;
       r.y=p.z*q.x-p.x*q.z;
       r.z=p.x*q.y-p.y*q.x;
       r.w=1.;
       return r;
}
double len3(Point3 p){
       return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
}
double dist3(Point3 p, Point3 q){
       p.x-=q.x;
       p.y-=q.y;
       p.z-=q.z;
       return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
}
Point3 unit3(Point3 p){
       double len=sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
       p.x/=len;
       p.y/=len;
       p.z/=len;
       p.w=1.;
       return p;
}
Point3 midpt3(Point3 p, Point3 q){
       p.x=.5*(p.x+q.x);
       p.y=.5*(p.y+q.y);
       p.z=.5*(p.z+q.z);
       p.w=1.;
       return p;
}
Point3 lerp3(Point3 p, Point3 q, double alpha){
       p.x+=(q.x-p.x)*alpha;
       p.y+=(q.y-p.y)*alpha;
       p.z+=(q.z-p.z)*alpha;
       p.w=1.;
       return p;
}
/*
* Reflect point p in the line joining p0 and p1
*/
Point3 reflect3(Point3 p, Point3 p0, Point3 p1){
       Point3 a, b;
       a=sub3(p, p0);
       b=sub3(p1, p0);
       return add3(a, mul3(b, 2*dot3(a, b)/dot3(b, b)));
}
/*
* Return the nearest point on segment [p0,p1] to point testp
*/
Point3 nearseg3(Point3 p0, Point3 p1, Point3 testp){
       double num, den;
       Point3 q, r;
       q=sub3(p1, p0);
       r=sub3(testp, p0);
       num=dot3(q, r);;
       if(num<=0) return p0;
       den=dot3(q, q);
       if(num>=den) return p1;
       return add3(p0, mul3(q, num/den));
}
/*
* distance from point p to segment [p0,p1]
*/
#define SMALL   1e-8    /* what should this value be? */
double pldist3(Point3 p, Point3 p0, Point3 p1){
       Point3 d, e;
       double dd, de, dsq;
       d=sub3(p1, p0);
       e=sub3(p, p0);
       dd=dot3(d, d);
       de=dot3(d, e);
       if(dd<SMALL*SMALL) return len3(e);
       dsq=dot3(e, e)-de*de/dd;
       if(dsq<SMALL*SMALL) return 0;
       return sqrt(dsq);
}
/*
* vdiv3(a, b) is the magnitude of the projection of a onto b
* measured in units of the length of b.
* vrem3(a, b) is the component of a perpendicular to b.
*/
double vdiv3(Point3 a, Point3 b){
       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);
}
Point3 vrem3(Point3 a, Point3 b){
       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);
       a.x-=b.x*quo;
       a.y-=b.y*quo;
       a.z-=b.z*quo;
       a.w=1.;
       return a;
}
/*
* Compute face (plane) with given normal, containing a given point
*/
Point3 pn2f3(Point3 p, Point3 n){
       n.w=-dot3(p, n);
       return n;
}
/*
* Compute face containing three points
*/
Point3 ppp2f3(Point3 p0, Point3 p1, Point3 p2){
       Point3 p01, p02;
       p01=sub3(p1, p0);
       p02=sub3(p2, p0);
       return pn2f3(p0, cross3(p01, p02));
}
/*
* Compute point common to three faces.
* Cramer's rule, yuk.
*/
Point3 fff2p3(Point3 f0, Point3 f1, Point3 f2){
       double det;
       Point3 p;
       det=dot3(f0, cross3(f1, f2));
       if(fabs(det)<SMALL){    /* parallel planes, bogus answer */
               p.x=0.;
               p.y=0.;
               p.z=0.;
               p.w=0.;
               return p;
       }
       p.x=(f0.w*(f2.y*f1.z-f1.y*f2.z)
               +f1.w*(f0.y*f2.z-f2.y*f0.z)+f2.w*(f1.y*f0.z-f0.y*f1.z))/det;
       p.y=(f0.w*(f2.z*f1.x-f1.z*f2.x)
               +f1.w*(f0.z*f2.x-f2.z*f0.x)+f2.w*(f1.z*f0.x-f0.z*f1.x))/det;
       p.z=(f0.w*(f2.x*f1.y-f1.x*f2.y)
               +f1.w*(f0.x*f2.y-f2.x*f0.y)+f2.w*(f1.x*f0.y-f0.x*f1.y))/det;
       p.w=1.;
       return p;
}
/*
* pdiv4 does perspective division to convert a projective point to affine coordinates.
*/
Point3 pdiv4(Point3 a){
       if(a.w==0) return a;
       a.x/=a.w;
       a.y/=a.w;
       a.z/=a.w;
       a.w=1.;
       return a;
}
Point3 add4(Point3 a, Point3 b){
       a.x+=b.x;
       a.y+=b.y;
       a.z+=b.z;
       a.w+=b.w;
       return a;
}
Point3 sub4(Point3 a, Point3 b){
       a.x-=b.x;
       a.y-=b.y;
       a.z-=b.z;
       a.w-=b.w;
       return a;
}

You are viewing proxied material from sdf.org. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.