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