1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
|
#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;
}
|