1 /**@(#)3D geometry
2 **/
3
4 #include <stdio.h>
5 #include <math.h>
6 #include <tina/sys.h>
7 #include <tina/math.h>
8 #include <tina/mathfuncs.h>
9
10 /**
11 Notation:
12 q - point position
13 l - point on line, v - unit direction of line
14 p - point on plane, n - unit normal to plane
15 */
16
17 Vec3 vec3_midpoint(Vec3 q1, Vec3 q2)
18 {
19 Vec3 midpoint = {Vec3_id};
20
21 vec3_x(midpoint) = (float)0.5 * (vec3_x(q1) + vec3_x(q2));
22 vec3_y(midpoint) = (float)0.5 * (vec3_y(q1) + vec3_y(q2));
23 vec3_z(midpoint) = (float)0.5 * (vec3_z(q1) + vec3_z(q2));
24 return (midpoint);
25 }
26
27 Vec3 vec3_projperp(Vec3 u, Vec3 v) /**part of u perpendicular to unit v**/
28
29 {
30 return (vec3_diff(u, vec3_times(vec3_dot(u, v), v)));
31 }
32
33 Vec3 vec3_projpar(Vec3 u, Vec3 v) /**part of u parallel to unit v**/
34
35 {
36 return (vec3_times(vec3_dot(u, v), v));
37 }
38
39 Vec3 vec3_proj_on_line(Vec3 q, Vec3 l, Vec3 v)
40 {
41 Vec3 lq = {Vec3_id};
42
43 lq = vec3_diff(q, l);
44 return (vec3_sum(l, vec3_times(vec3_dot(lq, v), v)));
45 }
46
47 Vec3 vec3_proj_on_plane(Vec3 q, Vec3 p, Vec3 n)
48 {
49 Vec3 qp = {Vec3_id};
50
51 qp = vec3_diff(p, q);
52 return (vec3_sum(q, vec3_times(vec3_dot(qp, n), n)));
53 }
54 /*
55 Vec3 vec3_proj_line_on_plane(Vec3 l1, Vec3 v1, Vec3 p, Vec3 n, Vec3 * l2, Vec3 * v2)
56 {
57 *l2 = vec3_proj_on_plane(l1, p, n);
58 *v2 = vec3_unit(vec3_projperp(v1, n));
59 }
60 */
61 /* BUG: function incomplete */
62
63 Vec3 vec3_closest_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2)
64 {
65 double d12 = vec3_dot(v1, v2);
66 double t1;
67
68 if (d12 == 1.0)
69 return (l1);
70 t1 = vec3_dot(vec3_diff(l2, l1),
71 vec3_diff(v1, vec3_times(d12, v2))) / (1.0 - d12 * d12);
72 return (vec3_sum(l1, vec3_times(t1, v1)));
73 }
74
75 Vec3 vec3_inter_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2)
76 {
77 return (vec3_midpoint(vec3_closest_lines(l1, v1, l2, v2),
78 vec3_closest_lines(l2, v2, l1, v1)));
79 }
80
81 Vec3 vec3_inter_line_plane(Vec3 l, Vec3 v, Vec3 p, Vec3 n)
82 {
83 double vn = vec3_dot(v, n);
84 double t;
85
86 if (vn == 0.0)
87 return (vec3_proj_on_plane(l, p, n));
88 t = vec3_dot(vec3_diff(p, l), n) / vn;
89 return (vec3_sum(l, vec3_times(t, v)));
90 }
91
92 void vec3_inter_planes(Vec3 p1, Vec3 n1, Vec3 p2, Vec3 n2, Vec3 * l, Vec3 * v)
93 {
94 Vec3 n12 = {Vec3_id};
95 Vec3 n21 = {Vec3_id};
96 Vec3 p = {Vec3_id};
97 Vec3 c = {Vec3_id};
98 double cc, s1, s2;
99
100 c = vec3_cross(n1, n2);
101 cc = vec3_modunit(c, v);
102 cc *= cc;
103 if (cc == 0.0)
104 {
105 *l = vec3_times(0.5, vec3_sum(p1, p2));
106 *v = vec3_perp(n1);
107 return;
108 }
109 p = vec3_midpoint(p1, p2);
110 n12 = vec3_projperp(n1, n2);
111 s1 = vec3_dot(vec3_diff(p1, p), n1) / cc;
112 n21 = vec3_projperp(n2, n1);
113 s2 = vec3_dot(vec3_diff(p2, p), n2) / cc;
114 *l = vec3_sum3(p, vec3_times(s1, n12), vec3_times(s2, n21));
115 *v = vec3_unit(c);
116 }
117
118 void vec3_join_2_points(Vec3 q1, Vec3 q2, Vec3 * l, Vec3 * v)
119 {
120 *l = vec3_times(0.5, vec3_sum(q1, q2));
121 *v = vec3_unit(vec3_diff(q2, q1));
122 }
123
124 void vec3_join_3_points(Vec3 q1, Vec3 q2, Vec3 q3, Vec3 * p, Vec3 * n)
125 {
126 *p = vec3_times(1.0 / 3.0, vec3_sum3(q1, q2, q3));
127 *n = vec3_unit(vec3_cross(vec3_diff(q3, q2), vec3_diff(q1, q2)));
128 }
129
130 void vec3_join_point_line(Vec3 q, Vec3 l, Vec3 v, Vec3 * p, Vec3 * n)
131 {
132 *p = vec3_midpoint(q, l);
133 *n = vec3_unitcross(vec3_diff(q, l), v);
134 }
135
136 void vec3_join_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2, Vec3 * p, Vec3 * n)
137 {
138 double a = vec3_dot(v1, v2);
139
140 if (a < 0.7071)
141 {
142 *p = vec3_sum3(l1, l2, vec3_minus(vec3_inter_lines(l1, v1, l2, v2)));
143 *n = vec3_unitcross(v1, v2);
144 } else
145 {
146 *p = vec3_times(0.5, vec3_sum(l1, l2));
147 *n = vec3_unitcross(v1, vec3_diff(l2, l1));
148 }
149 }
150
151 double vec3_dist_point_plane(Vec3 q, Vec3 p, Vec3 n)
152 {
153 Vec3 dq = {Vec3_id};
154
155 dq = vec3_projpar(vec3_diff(q, p), n);
156 return (vec3_mod(dq));
157 }
158
159 double vec3_dist_point_line(Vec3 q, Vec3 l, Vec3 v)
160 {
161 Vec3 dq = {Vec3_id};
162
163 dq = vec3_projperp(vec3_diff(q, l), v);
164 return (vec3_mod(dq));
165 }
166
167 double vec3_dist_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2)
168 {
169 Vec3 n = {Vec3_id};
170
171 if (vec3_modunit(vec3_cross(v1, v2), &n) == 0.0)
172 return (vec3_mod(vec3_projperp(vec3_diff(l2, l1), v1)));
173 else
174 return (fabs(vec3_dot(n, vec3_diff(l2, l1))));
175 }
176
177 void vec3_circ_3_points(Vec3 p1, Vec3 p2, Vec3 p3, Vec3 * centre, Vec3 * normal, double *radius)
178 {
179 Vec3 a = {Vec3_id};
180 Vec3 b = {Vec3_id};
181 double a2, b2, ab, u, v, w;
182
183 a = vec3_diff(p2, p1);
184 b = vec3_diff(p3, p1);
185 *normal = vec3_unitcross(a, b);
186
187 a2 = vec3_sqrmod(a);
188 b2 = vec3_sqrmod(b);
189 ab = vec3_dot(a, b);
190
191 u = b2 * (a2 - ab);
192 v = a2 * (b2 - ab);
193 w = 2.0 * (a2 * b2 - ab * ab);
194
195 a = vec3_times(u, a);
196 b = vec3_times(v, b);
197
198 *centre = vec3_sum(p1, vec3_times(1 / w, vec3_sum(a, b)));
199 *normal = vec3_unitcross(a, b);
200 *radius = sqrt((a2 * b2 * (a2 + b2 - 2.0 * ab)) / (2.0 * w));
201 }
202
203 /**
204 some geometry using thresholds
205 **/
206
207 Bool vec3_collinear(Vec3 p1, Vec3 p2, Vec3 q1, Vec3 q2, double dotth1, double dotth2)
208 /* defines a line */
209
210
211 {
212 float sqrmod[4];
213 float maxmod;
214 Vec3 vec[4] = {{Vec3_id}, {Vec3_id}, {Vec3_id}, {Vec3_id}};
215 Vec3 maxv = {Vec3_id};
216 Vec3 v1 = {Vec3_id};
217 Vec3 v2 = {Vec3_id};
218 int maxi, i;
219
220 v1 = vec3_unit(vec3_diff(p2, p1));
221 v2 = vec3_unit(vec3_diff(q2, q1));
222
223 vec[0] = vec3_diff(p1, q1);
224 sqrmod[0] = vec3_sqrmod(vec[0]);
225 vec[1] = vec3_diff(p1, q2);
226 sqrmod[1] = vec3_sqrmod(vec[1]);
227 vec[2] = vec3_diff(p2, q1);
228 sqrmod[2] = vec3_sqrmod(vec[2]);
229 vec[3] = vec3_diff(p2, q2);
230 sqrmod[3] = vec3_sqrmod(vec[3]);
231
232 maxmod = sqrmod[0];
233 maxi = 0;
234 for (i = 1; i < 4; ++i)
235 if (sqrmod[i] > maxmod)
236 {
237 maxmod = sqrmod[i];
238 maxi = i;
239 }
240 maxv = vec3_times(1 / sqrt(maxmod), vec[maxi]);
241 return (Bool) (fabs(vec3_dot(v1, maxv)) > dotth1 && fabs(vec3_dot(v2, maxv)) > dotth2);
242 }
243
244 double vec3_closest_app(Vec3 p1, Vec3 v1, Vec3 p2, Vec3 v2, Vec3 * c1, Vec3 * c2)
245
246
247 /* closest points respectively */
248 {
249 Vec3 cross = {Vec3_id};
250 Vec3 diff = {Vec3_id};
251 float dp, crossmagsq;
252 float l1, l2;
253
254 if (c1 == NULL || c2 == NULL)
255 return (0.0);
256
257 diff = vec3_diff(p1, p2);
258 cross = vec3_cross(v1, v2);
259
260 dp = vec3_dot(diff, cross);
261
262 crossmagsq = vec3_sqrmod(cross);
263 diff = vec3_diff(diff, vec3_times(dp / crossmagsq, cross));
264 l1 = -vec3_dot(cross, vec3_cross(diff, v2)) / crossmagsq;
265 l2 = vec3_dot(cross, vec3_cross(v1, diff)) / crossmagsq;
266
267 *c1 = vec3_sum(p1, vec3_times(l1, v1));
268 *c2 = vec3_sum(p2, vec3_times(l2, v2));
269 return (dp / sqrt(crossmagsq));
270 }
271
272 Bool vec3_parallel(Vec3 v1, Vec3 v2, double dotthres)
273 {
274 return (Bool) (fabs(vec3_dot(v1, v2)) > dotthres);
275 }
276
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.