~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

# Linux Cross ReferenceTina4/src/math/geom/geom3.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

```  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 ```

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~