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

Linux Cross Reference
Tina4/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 ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.