1 #include <u.h> 2 #include <libc.h> 3 #include <libg.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 return (Point3){a.x+b.x, a.y+b.y, a.z+b.z, 1.}; 12 } 13 Point3 sub3(Point3 a, Point3 b){ 14 return (Point3){a.x-b.x, a.y-b.y, a.z-b.z, 1.}; 15 } 16 Point3 neg3(Point3 a){ 17 return (Point3){-a.x, -a.y, -a.z, 1.}; 18 } 19 Point3 div3(Point3 a, double b){ 20 return (Point3){a.x/b, a.y/b, a.z/b, 1.}; 21 } 22 Point3 mul3(Point3 a, double b){ 23 return (Point3){a.x*b, a.y*b, a.z*b, 1.}; 24 } 25 int eqpt3(Point3 p, Point3 q){ 26 return p.x==q.x && p.y==q.y && p.z==q.z; 27 } 28 /* 29 * Are these points closer than eps, in a relative sense 30 */ 31 int closept3(Point3 p, Point3 q, double eps){ 32 return 2.*dist3(p, q)<eps*(len3(p)+len3(q)); 33 } 34 double dot3(Point3 p, Point3 q){ 35 return p.x*q.x+p.y*q.y+p.z*q.z; 36 } 37 Point3 cross3(Point3 p, Point3 q){ 38 return (Point3){p.y*q.z-p.z*q.y, p.z*q.x-p.x*q.z, p.x*q.y-p.y*q.x, 1.}; 39 } 40 double len3(Point3 p){ 41 return sqrt(p.x*p.x+p.y*p.y+p.z*p.z); 42 } 43 double dist3(Point3 p, Point3 q){ 44 p.x-=q.x; 45 p.y-=q.y; 46 p.z-=q.z; 47 return sqrt(p.x*p.x+p.y*p.y+p.z*p.z); 48 } 49 Point3 unit3(Point3 p){ 50 double len=sqrt(p.x*p.x+p.y*p.y+p.z*p.z); 51 return (Point3){p.x/len, p.y/len, p.z/len, 1.}; 52 } 53 Point3 midpt3(Point3 p, Point3 q){ 54 return (Point3){.5*(p.x+q.x), .5*(p.y+q.y), .5*(p.z+q.z), 1.}; 55 } 56 Point3 lerp3(Point3 p, Point3 q, double alpha){ 57 return (Point3){p.x+(q.x-p.x)*alpha, p.y+(q.y-p.y)*alpha, p.z+(q.z-p.z)*alpha, 1.}; 58 } 59 /* 60 * Reflect point p in the line joining p0 and p1 61 */ 62 Point3 reflect3(Point3 p, Point3 p0, Point3 p1){ 63 Point3 a, b; 64 a=sub3(p, p0); 65 b=sub3(p1, p0); 66 return add3(a, mul3(b, 2*dot3(a, b)/dot3(b, b))); 67 } 68 /* 69 * Return the nearest point on segment [p0,p1] to point testp 70 */ 71 Point3 nearseg3(Point3 p0, Point3 p1, Point3 testp){ 72 double num, den; 73 Point3 q, r; 74 q=sub3(p1, p0); 75 r=sub3(testp, p0); 76 num=dot3(q, r);; 77 if(num<=0) return p0; 78 den=dot3(q, q); 79 if(num>=den) return p1; 80 return add3(p0, mul3(q, num/den)); 81 } 82 /* 83 * distance from point p to segment [p0,p1] 84 */ 85 #define SMALL 1e-8 /* what should this value be? */ 86 double pldist3(Point3 p, Point3 p0, Point3 p1){ 87 Point3 d, e; 88 double dd, de, dsq; 89 d=sub3(p1, p0); 90 e=sub3(p, p0); 91 dd=dot3(d, d); 92 de=dot3(d, e); 93 if(dd<SMALL*SMALL) return len3(e); 94 dsq=dot3(e, e)-de*de/dd; 95 if(dsq<SMALL*SMALL) return 0; 96 return sqrt(dsq); 97 } 98 /* 99 * vdiv3(a, b) is the magnitude of the projection of a onto b 100 * measured in units of the length of b. 101 * vrem3(a, b) is the component of a perpendicular to b. 102 */ 103 double vdiv3(Point3 a, Point3 b){ 104 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); 105 } 106 Point3 vrem3(Point3 a, Point3 b){ 107 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); 108 return (Point3){a.x-b.x*quo, a.y-b.y*quo, a.z-b.z*quo, 1.}; 109 } 110 /* 111 * Compute face (plane) with given normal, containing a given point 112 */ 113 Point3 pn2f3(Point3 p, Point3 n){ 114 return (Point3){n.x, n.y, n.z, -dot3(p, n)}; 115 } 116 /* 117 * Compute face containing three points 118 */ 119 Point3 ppp2f3(Point3 p0, Point3 p1, Point3 p2){ 120 Point3 p01, p02; 121 p01=sub3(p1, p0); 122 p02=sub3(p2, p0); 123 return pn2f3(p0, cross3(p01, p02)); 124 } 125 /* 126 * Compute point common to three faces. 127 * Cramer's rule, yuk. 128 */ 129 Point3 fff2p3(Point3 f0, Point3 f1, Point3 f2){ 130 double det, x, y, z; 131 det=dot3(f0, cross3(f1, f2)); 132 if(fabs(det)<SMALL) return (Point3){0,0,0,0}; /* parallel planes, bogus answer */ 133 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); 134 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); 135 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); 136 return (Point3){x/det, y/det, z/det, 1.}; 137 } 138 /* 139 * pdiv4 does perspective division to convert a projective point to affine coordinates. 140 */ 141 Point3 pdiv4(Point3 a){ 142 if(a.w==0) return a; 143 return (Point3){a.x/a.w, a.y/a.w, a.z/a.w, 1.}; 144 } 145 Point3 add4(Point3 a, Point3 b){ 146 return (Point3){a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w}; 147 } 148 Point3 sub4(Point3 a, Point3 b){ 149 return (Point3){a.x-b.x, a.y-b.y, a.z-b.z, a.w-b.w}; 150 } 151