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

Linux Cross Reference
Tina5/tina-libs/tina/math/mathGeom_geom3.c

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

  1 /**********
  2  *
  3  * Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
  4  * University of Manchester, UK.  All rights reserved.
  5  * 
  6  * Redistribution and use in source and binary forms, with or without modification, 
  7  * are permitted provided that the following conditions are met:
  8  * 
  9  *   . Redistributions of source code must retain the above copyright notice, 
 10  *     this list of conditions and the following disclaimer.
 11  *    
 12  *   . Redistributions in binary form must reproduce the above copyright notice,
 13  *     this list of conditions and the following disclaimer in the documentation 
 14  *     and/or other materials provided with the distribution.
 15  * 
 16  *   . Neither the name of the University of Manchester nor the names of its
 17  *     contributors may be used to endorse or promote products derived from this 
 18  *     software without specific prior written permission.
 19  * 
 20  * 
 21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 24  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  * POSSIBILITY OF SUCH DAMAGE.
 32  *
 33  **********
 34  *
 35  * Program :    TINA
 36  * File    :  $Source: /home/tina/cvs/tina-libs/tina/math/mathGeom_geom3.c,v $
 37  * Date    :  $Date: 2003/09/23 11:32:20 $
 38  * Version :  $Revision: 1.4 $
 39  * CVS Id  :  $Id: mathGeom_geom3.c,v 1.4 2003/09/23 11:32:20 matts Exp $
 40  *
 41  * Author  :  Legacy TINA
 42  *
 43  * Notes :@(#)3D geometry
 44  *
 45  * Notation:
 46  * q - point position
 47  * l - point on line,  v - unit direction of line
 48  * p - point on plane, n - unit normal to plane
 49  *
 50  *********
 51 */
 52 /** 
 53  *  @file
 54  *  @brief  3D Geometry functions.      
 55  *
 56  * 
 57  * Notation: Points and vectors are given as Vec3's where;
 58  * 
 59  *  - q is point position.
 60  *  - l is point on line.
 61  *  - v is unit direction of line.
 62  *  - p is point on plane. 
 63  *  - n is unit normal to plane.
 64  * 
 65  * Standard 3D line and plane geometry functions such as finding line midpoints, line/plane 
 66  * intersections etc.
 67 */
 68 
 69 
 70 
 71 #include "mathGeom_geom3.h"
 72 
 73 #if HAVE_CONFIG_H
 74 #include <config.h>
 75 #endif
 76 
 77 #include <stdio.h>
 78 #include <math.h>
 79 #include <tina/sys/sysDef.h>
 80 #include <tina/math/math_GeomDef.h>
 81 #include <tina/math/mathGeom_vec3.h>
 82 
 83 
 84 Vec3            vec3_midpoint(Vec3 q1, Vec3 q2)
 85 {
 86         Vec3            midpoint = {Vec3_id};
 87 
 88         vec3_x(midpoint) = (float) 0.5 *(vec3_x(q1) + vec3_x(q2));
 89         vec3_y(midpoint) = (float) 0.5 *(vec3_y(q1) + vec3_y(q2));
 90         vec3_z(midpoint) = (float) 0.5 *(vec3_z(q1) + vec3_z(q2));
 91         return (midpoint);
 92 }
 93 
 94 Vec3            vec3_projperp(Vec3 u, Vec3 v)   /**part of  u  perpendicular to unit  v**/
 95 
 96 {
 97         return (vec3_diff(u, vec3_times(vec3_dot(u, v), v)));
 98 }
 99 
100 Vec3            vec3_projpar(Vec3 u, Vec3 v)    /**part of  u  parallel to unit  v**/
101 
102 {
103         return (vec3_times(vec3_dot(u, v), v));
104 }
105 
106 Vec3            vec3_proj_on_line(Vec3 q, Vec3 l, Vec3 v)
107 {
108         Vec3            lq = {Vec3_id};
109 
110         lq = vec3_diff(q, l);
111         return (vec3_sum(l, vec3_times(vec3_dot(lq, v), v)));
112 }
113 
114 Vec3            vec3_proj_on_plane(Vec3 q, Vec3 p, Vec3 n)
115 {
116         Vec3            qp = {Vec3_id};
117 
118         qp = vec3_diff(p, q);
119         return (vec3_sum(q, vec3_times(vec3_dot(qp, n), n)));
120 }
121 /*
122 Vec3    vec3_proj_line_on_plane(Vec3 l1, Vec3 v1, Vec3 p, Vec3 n, Vec3 * l2, Vec3 * v2)
123 {
124     *l2 = vec3_proj_on_plane(l1, p, n);
125     *v2 = vec3_unit(vec3_projperp(v1, n));
126 }
127 */
128 /* BUG: function incomplete */
129 
130 Vec3            vec3_closest_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2)
131 {
132         double          d12 = vec3_dot(v1, v2);
133         double          t1;
134 
135         if (d12 == 1.0)
136                 return (l1);
137         t1 = vec3_dot(vec3_diff(l2, l1),
138                     vec3_diff(v1, vec3_times(d12, v2))) / (1.0 - d12 * d12);
139         return (vec3_sum(l1, vec3_times(t1, v1)));
140 }
141 
142 Vec3            vec3_inter_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2)
143 {
144         return (vec3_midpoint(vec3_closest_lines(l1, v1, l2, v2),
145                               vec3_closest_lines(l2, v2, l1, v1)));
146 }
147 
148 Vec3            vec3_inter_line_plane(Vec3 l, Vec3 v, Vec3 p, Vec3 n)
149 {
150         double          vn = vec3_dot(v, n);
151         double          t;
152 
153         if (vn == 0.0)
154                 return (vec3_proj_on_plane(l, p, n));
155         t = vec3_dot(vec3_diff(p, l), n) / vn;
156         return (vec3_sum(l, vec3_times(t, v)));
157 }
158 
159 void            vec3_inter_planes(Vec3 p1, Vec3 n1, Vec3 p2, Vec3 n2, Vec3 * l, Vec3 * v)
160 {
161         Vec3            n12 = {Vec3_id};
162         Vec3            n21 = {Vec3_id};
163         Vec3            p = {Vec3_id};
164         Vec3            c = {Vec3_id};
165         double          cc, s1, s2;
166 
167         c = vec3_cross(n1, n2);
168         cc = vec3_modunit(c, v);
169         cc *= cc;
170         if (cc == 0.0)
171         {
172                 *l = vec3_times(0.5, vec3_sum(p1, p2));
173                 *v = vec3_perp(n1);
174                 return;
175         }
176         p = vec3_midpoint(p1, p2);
177         n12 = vec3_projperp(n1, n2);
178         s1 = vec3_dot(vec3_diff(p1, p), n1) / cc;
179         n21 = vec3_projperp(n2, n1);
180         s2 = vec3_dot(vec3_diff(p2, p), n2) / cc;
181         *l = vec3_sum3(p, vec3_times(s1, n12), vec3_times(s2, n21));
182         *v = vec3_unit(c);
183 }
184 
185 void            vec3_join_2_points(Vec3 q1, Vec3 q2, Vec3 * l, Vec3 * v)
186 {
187         *l = vec3_times(0.5, vec3_sum(q1, q2));
188         *v = vec3_unit(vec3_diff(q2, q1));
189 }
190 
191 void            vec3_join_3_points(Vec3 q1, Vec3 q2, Vec3 q3, Vec3 * p, Vec3 * n)
192 {
193         *p = vec3_times(1.0 / 3.0, vec3_sum3(q1, q2, q3));
194         *n = vec3_unit(vec3_cross(vec3_diff(q3, q2), vec3_diff(q1, q2)));
195 }
196 
197 void            vec3_join_point_line(Vec3 q, Vec3 l, Vec3 v, Vec3 * p, Vec3 * n)
198 {
199         *p = vec3_midpoint(q, l);
200         *n = vec3_unitcross(vec3_diff(q, l), v);
201 }
202 
203 void            vec3_join_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2, Vec3 * p, Vec3 * n)
204 {
205         double          a = vec3_dot(v1, v2);
206 
207         if (a < 0.7071)
208         {
209                 *p = vec3_sum3(l1, l2, vec3_minus(vec3_inter_lines(l1, v1, l2, v2)));
210                 *n = vec3_unitcross(v1, v2);
211         } else
212         {
213                 *p = vec3_times(0.5, vec3_sum(l1, l2));
214                 *n = vec3_unitcross(v1, vec3_diff(l2, l1));
215         }
216 }
217 
218 double          vec3_dist_point_plane(Vec3 q, Vec3 p, Vec3 n)
219 {
220         Vec3            dq = {Vec3_id};
221 
222         dq = vec3_projpar(vec3_diff(q, p), n);
223         return (vec3_mod(dq));
224 }
225 
226 double          vec3_dist_point_line(Vec3 q, Vec3 l, Vec3 v)
227 {
228         Vec3            dq = {Vec3_id};
229 
230         dq = vec3_projperp(vec3_diff(q, l), v);
231         return (vec3_mod(dq));
232 }
233 
234 double          vec3_dist_lines(Vec3 l1, Vec3 v1, Vec3 l2, Vec3 v2)
235 {
236         Vec3            n = {Vec3_id};
237 
238         if (vec3_modunit(vec3_cross(v1, v2), &n) == 0.0)
239                 return (vec3_mod(vec3_projperp(vec3_diff(l2, l1), v1)));
240         else
241                 return (fabs(vec3_dot(n, vec3_diff(l2, l1))));
242 }
243 
244 void            vec3_circ_3_points(Vec3 p1, Vec3 p2, Vec3 p3, Vec3 * centre, Vec3 * normal, double *radius)
245 {
246         Vec3            a = {Vec3_id};
247         Vec3            b = {Vec3_id};
248         double          a2, b2, ab, u, v, w;
249 
250         a = vec3_diff(p2, p1);
251         b = vec3_diff(p3, p1);
252         *normal = vec3_unitcross(a, b);
253 
254         a2 = vec3_sqrmod(a);
255         b2 = vec3_sqrmod(b);
256         ab = vec3_dot(a, b);
257 
258         u = b2 * (a2 - ab);
259         v = a2 * (b2 - ab);
260         w = 2.0 * (a2 * b2 - ab * ab);
261 
262         a = vec3_times(u, a);
263         b = vec3_times(v, b);
264 
265         *centre = vec3_sum(p1, vec3_times(1 / w, vec3_sum(a, b)));
266         *normal = vec3_unitcross(a, b);
267         *radius = sqrt((a2 * b2 * (a2 + b2 - 2.0 * ab)) / (2.0 * w));
268 }
269 
270 /**
271 some geometry using thresholds
272 **/
273 
274 Bool            vec3_collinear(Vec3 p1, Vec3 p2, Vec3 q1, Vec3 q2, double dotth1, double dotth2)
275 /* defines a line */
276 
277 
278 {
279         float           sqrmod[4];
280         float           maxmod;
281         Vec3            vec[4] = {{Vec3_id}, {Vec3_id}, {Vec3_id}, {Vec3_id}};
282         Vec3            maxv = {Vec3_id};
283         Vec3            v1 = {Vec3_id};
284         Vec3            v2 = {Vec3_id};
285         int             maxi, i;
286 
287         v1 = vec3_unit(vec3_diff(p2, p1));
288         v2 = vec3_unit(vec3_diff(q2, q1));
289 
290         vec[0] = vec3_diff(p1, q1);
291         sqrmod[0] = vec3_sqrmod(vec[0]);
292         vec[1] = vec3_diff(p1, q2);
293         sqrmod[1] = vec3_sqrmod(vec[1]);
294         vec[2] = vec3_diff(p2, q1);
295         sqrmod[2] = vec3_sqrmod(vec[2]);
296         vec[3] = vec3_diff(p2, q2);
297         sqrmod[3] = vec3_sqrmod(vec[3]);
298 
299         maxmod = sqrmod[0];
300         maxi = 0;
301         for (i = 1; i < 4; ++i)
302                 if (sqrmod[i] > maxmod)
303                 {
304                         maxmod = sqrmod[i];
305                         maxi = i;
306                 }
307         maxv = vec3_times(1 / sqrt(maxmod), vec[maxi]);
308         return (Bool) (fabs(vec3_dot(v1, maxv)) > dotth1 && fabs(vec3_dot(v2, maxv)) > dotth2);
309 }
310 
311 double          vec3_closest_app(Vec3 p1, Vec3 v1, Vec3 p2, Vec3 v2, Vec3 * c1, Vec3 * c2)
312 
313 
314 /* closest points respectively */
315 {
316         Vec3            cross = {Vec3_id};
317         Vec3            diff = {Vec3_id};
318         float           dp, crossmagsq;
319         float           l1, l2;
320 
321         if (c1 == NULL || c2 == NULL)
322                 return (0.0);
323 
324         diff = vec3_diff(p1, p2);
325         cross = vec3_cross(v1, v2);
326 
327         dp = vec3_dot(diff, cross);
328 
329         crossmagsq = vec3_sqrmod(cross);
330         diff = vec3_diff(diff, vec3_times(dp / crossmagsq, cross));
331         l1 = -vec3_dot(cross, vec3_cross(diff, v2)) / crossmagsq;
332         l2 = vec3_dot(cross, vec3_cross(v1, diff)) / crossmagsq;
333 
334         *c1 = vec3_sum(p1, vec3_times(l1, v1));
335         *c2 = vec3_sum(p2, vec3_times(l2, v2));
336         return (dp / sqrt(crossmagsq));
337 }
338 
339 Bool            vec3_parallel(Vec3 v1, Vec3 v2, double dotthres)
340 {
341         return (Bool) (fabs(vec3_dot(v1, v2)) > dotthres);
342 }
343 

~ [ 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.