tarith3.c - plan9port - [fork] Plan 9 from user space
HTML git clone git://src.adamsgaard.dk/plan9port
DIR Log
DIR Files
DIR Refs
DIR README
DIR LICENSE
---
tarith3.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 }