arith3.c (4298B)
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 }