home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / general / modelers / geomview / source.lha / Geomview / src / lib / geometry / point3 / segments.c < prev   
Encoding:
C/C++ Source or Header  |  1992-02-26  |  7.7 KB  |  279 lines
  1. /* Copyright (c) 1992 The Geometry Center; University of Minnesota
  2.    1300 South Second Street;  Minneapolis, MN  55454, USA;
  3.    
  4. This file is part of geomview/OOGL. geomview/OOGL is free software;
  5. you can redistribute it and/or modify it only under the terms given in
  6. the file COPYING, which you should have received along with this file.
  7. This and other related software may be obtained via anonymous ftp from
  8. geom.umn.edu; email: software@geom.umn.edu. */
  9.  
  10. /* Authors: Charlie Gunn, Pat Hanrahan, Stuart Levy, Tamara Munzner, Mark Phillips */
  11.  
  12. #include <stdio.h>
  13. #include <math.h>
  14. #include "3d.h"
  15. #include "ooglutil.h"
  16.  
  17. static void PtNormalPlane(Point3 *base, Point3 *normal, HPoint3 *ans);
  18. static void Proj(Point3 *a, Point3 *b, Point3 *ans);
  19. static void Orth(Point3 *a, Point3 *b, Point3 *ans);
  20. static float ParSgSgDistance(Point3 *a1, Point3 *a2, Point3 *adir,
  21.                  Point3 *b1, Point3 *b2);
  22. static float SgPtDistance(Point3 *p, Point3 *a1, Point3 *a2, Point3 *dir);
  23. static void SgPlMinPoint(HPoint3 *pl, Point3 *a, Point3 *b,
  24.              Point3 *dir, Point3 *ans);
  25. static void TComb(Point3 *a, float t, Point3 *dir, Point3 *b);
  26. static int LnPlIntersect(HPoint3 *pl, Point3 *a, Point3 *dir, float *t);
  27.  
  28. /*-----------------------------------------------------------------------
  29.  * Function:    SgSgDistance
  30.  * Description:    distance between two line segments
  31.  * Args:    a1,a2: first segment
  32.  *        b1,b2: second segment
  33.  * Returns:    distance between segment [a1,a2] and segment [b1,b2]
  34.  * Author:    mbp
  35.  * Date:    Mon Dec 23 15:17:30 1991
  36.  * Notes:    This procedure is totally general; either or both
  37.  *        segments may in fact be points.
  38.  */
  39. float SgSgDistance(Point3 *a1, Point3 *a2,
  40.            Point3 *b1, Point3 *b2)
  41. {
  42.   Point3 adir, bdir, amin, bmin, na, nb;
  43.   HPoint3 aplane, bplane;
  44.   float alen, blen, d, cosang;
  45.  
  46.   Pt3Sub(a2,a1, &adir);
  47.   alen = Pt3Length( &adir );
  48.   Pt3Sub(b2,b1, &bdir);
  49.   blen = Pt3Length( &bdir );
  50.  
  51.   switch ( ((alen<1.e-12)<<1) | (blen<1.e-12) ) {
  52.   case 1:
  53.     /* b is a point, a is not */
  54.     d = SgPtDistance(b1, a1,a2,&adir);
  55.     return d;
  56.   case 2:
  57.     /* a is a point, b is not */
  58.     d = SgPtDistance(a1, b1,b2,&bdir);
  59.     return d;
  60.   case 3:
  61.     /* both a and b are points */
  62.     d = Pt3Distance(a1,b1);
  63.     return d;
  64.   }
  65.  
  66.   /*
  67.    * We fall thru the above switch if neither segment is a point.
  68.    * Now check for parallelism.
  69.    */
  70.  
  71.   cosang = Pt3Dot(&adir,&bdir) / ( alen * blen );
  72.   if ( fabs((double)cosang)  > .99 ) {
  73.     /* segments are essentially parallel */
  74.     d = ParSgSgDistance(a1,a2,&adir,b1,b2);
  75.   }
  76.   else {
  77.     /* segments are skew */
  78.     Orth(&adir, &bdir, &na);
  79.     Orth(&bdir, &adir, &nb);
  80.     PtNormalPlane(a1, &na, &aplane);
  81.     PtNormalPlane(b1, &nb, &bplane);
  82.     SgPlMinPoint(&aplane, b1, b2, &bdir, &bmin);
  83.     SgPlMinPoint(&bplane, a1, b2, &adir, &amin);
  84.     d = Pt3Distance(&amin, &bmin);
  85.   }
  86.   return d;
  87. }
  88.  
  89. /*-----------------------------------------------------------------------
  90.  * Function:    PtNormalPlane
  91.  * Description:    compute homog coords of a plane
  92.  * Args:    *base: point on plane
  93.  *        *normal: normal vector to plane
  94.  *        *ans: homog coords of plane
  95.  * Returns:    nothing
  96.  * Author:    mbp
  97.  * Date:    Mon Dec 23 15:08:46 1991
  98.  */
  99. static void PtNormalPlane(Point3 *base, Point3 *normal, HPoint3 *ans)
  100. {
  101.   ans->x = normal->x;
  102.   ans->y = normal->y;
  103.   ans->z = normal->z;
  104.   ans->w = - Pt3Dot(base, normal);
  105. }
  106.  
  107.  
  108. /*-----------------------------------------------------------------------
  109.  * Function:    Proj
  110.  * Description:    projection of a onto b
  111.  * Args IN:    a,b
  112.  *    OUT:    ans
  113.  * Returns:    nothing
  114.  * Author:    mbp
  115.  * Date:    Mon Dec 23 15:09:29 1991
  116.  */
  117. static void Proj(Point3 *a, Point3 *b, Point3 *ans)
  118. {
  119.   Pt3Mul( Pt3Dot(a,b) / Pt3Dot(a,a), a, ans );
  120. }
  121.  
  122. /*-----------------------------------------------------------------------
  123.  * Function:    Orth
  124.  * Description:    orthogonalization of b wrt a
  125.  * Args IN:    a, b
  126.  *    OUT:    ans
  127.  * Returns:    nothing
  128.  * Author:    mbp
  129.  * Date:    Mon Dec 23 15:09:54 1991
  130.  * Notes:    answer is vector in plane spanned
  131.  *          by a & b, orthogonal to a.
  132.  */
  133. static void Orth(Point3 *a, Point3 *b, Point3 *ans)
  134. {
  135.   Point3 p;
  136.  
  137.   Proj(a,b,&p);
  138.   Pt3Sub(b, &p, ans);
  139. }
  140.  
  141. /*-----------------------------------------------------------------------
  142.  * Function:    ParSgSgDistance
  143.  * Description:    compute distance between two line segments on
  144.  *          parallel lines
  145.  * Args:    a1, a2: first segment
  146.  *        *adir: a2 - a1
  147.  *        b1,b2: second segment
  148.  * Returns:    distance between segments [a1,a2] and [b1,b2]
  149.  * Author:    mbp
  150.  * Date:    Mon Dec 23 15:11:38 1991
  151.  * Notes:    segments must lie on parallel lines in order for
  152.  *          this to work.
  153.  *        adir must be exactly a2 - a1 upon input.  It appears
  154.  *          as an argument to this procedure because we assume
  155.  *          the caller has already computed it, and we need it
  156.  *          here, so why recompute it?!!
  157.  */
  158. static float ParSgSgDistance(Point3 *a1, Point3 *a2, Point3 *adir,
  159.                  Point3 *b1, Point3 *b2)
  160. {
  161.   Point3 b1p,b2p;
  162.   HPoint3 b1plane, b2plane;
  163.   float d,t1,t2;
  164.  
  165.   Pt3Sub(a2,a1,adir);
  166.   PtNormalPlane(b1, adir, &b1plane);
  167.   LnPlIntersect(&b1plane, a1, adir, &t1);
  168.   TComb(a1, t1, adir, &b1p);
  169.   d = Pt3Distance(b1, &b1p);
  170.   if ( (t1>=0) && (t1<=1) )
  171.     return d;
  172.   PtNormalPlane(b2, adir, &b2plane);
  173.   LnPlIntersect(&b2plane, a1, adir, &t2);
  174.   TComb(a1, t2, adir, &b2p);
  175.   if ( (t2>=0) && (t2<=1) )
  176.     return d;
  177.   if ( t2 > t1 ) {
  178.     if ( t1 > 1 )
  179.       d = Pt3Distance(a2,b1);
  180.     else
  181.       d = Pt3Distance(a1,b2);
  182.   }
  183.   else {
  184.     if ( t2 > 1 )
  185.       d = Pt3Distance(a2,b2);
  186.     else
  187.       d = Pt3Distance(a1,b1);
  188.   }
  189.   return d;
  190. }
  191.  
  192. /*-----------------------------------------------------------------------
  193.  * Function:    SgPtDistance
  194.  * Description:    distance from a segment to a point
  195.  * Args:    p: the point
  196.  *        a1,a2: the segment
  197.  *        *dir: a2 - a1
  198.  * Returns:    the distance from p to segment [a1,a2]
  199.  * Author:    mbp
  200.  * Date:    Mon Dec 23 15:15:19 1991
  201.  * Notes:    dir must be a2 - a1 upon input
  202.  */
  203. static float SgPtDistance(Point3 *p, Point3 *a1, Point3 *a2, Point3 *dir)
  204. {
  205.   HPoint3 pl;
  206.   Point3 min;
  207.   float d,t;
  208.  
  209.   PtNormalPlane(p, dir, &pl);
  210.   SgPlMinPoint(&pl, a1, a2, dir, &min);
  211.   d = Pt3Distance(p, &min);
  212.   return d;
  213. }
  214.  
  215.  
  216. /*-----------------------------------------------------------------------
  217.  * Function:    SgPlMinPoint
  218.  * Description:    find the point of a segment closest to a plane
  219.  * Args:    pl: homog coords of the plane
  220.  *        a,b: the segment
  221.  *        dir: b - a
  222.  *        ans: the point of segment [a,b] closest to plane
  223.  * Returns:    nothing
  224.  * Author:    mbp
  225.  * Date:    Mon Dec 23 15:19:02 1991
  226.  * Notes:    dir must be b - a upon input
  227.  */
  228. static void SgPlMinPoint(HPoint3 *pl, Point3 *a, Point3 *b,
  229.              Point3 *dir, Point3 *ans)
  230. {
  231.   float t;
  232.  
  233.   LnPlIntersect(pl, a, dir, &t);
  234.   if (t <= 0)
  235.     *ans = *a;
  236.   else if (t >= 1)
  237.     *ans = *b;
  238.   else
  239.     TComb(a, t, dir, ans);
  240. }
  241.  
  242. /*-----------------------------------------------------------------------
  243.  * Function:    TComb
  244.  * Description:    form a t combination: b = a + t * dir
  245.  * Args    IN:    a,dir,t
  246.  *    OUT:    b: a + t * dir
  247.  * Returns:    nothing
  248.  * Author:    mbp
  249.  * Date:    Mon Dec 23 15:20:43 1991
  250.  */
  251. static void TComb(Point3 *a, float t, Point3 *dir, Point3 *b)
  252. {
  253.   b->x = a->x + t * dir->x;
  254.   b->y = a->y + t * dir->y;
  255.   b->z = a->z + t * dir->z;
  256. }
  257.  
  258. /*-----------------------------------------------------------------------
  259.  * Function:    LnPlIntersect
  260.  * Description:    intersect a plane with a line
  261.  * Arg    IN:    pl: the plane
  262.  *        a: base point of line
  263.  *        dir: direction vector of line
  264.  *    OUT:    t: t value such that a + t * dir lies on plane
  265.  * Returns:    1 if successful, 0 if not
  266.  * Author:    mbp
  267.  * Date:    Mon Dec 23 15:22:25 1991
  268.  * Notes:    return value of 0 means line is parallel to plane
  269.  */
  270. static int LnPlIntersect(HPoint3 *pl, Point3 *a, Point3 *dir, float *t)
  271. {
  272.   register float d;
  273.  
  274.   d = pl->x*dir->x + pl->y*dir->y + pl->z*dir->z;
  275.   if (d == 0) return 0;
  276.   *t = - ( a->x * pl->x + a->y * pl->y + a->z * pl->z + pl->w ) / d;
  277.   return 1;
  278. }
  279.