TriDist.cpp
Go to the documentation of this file.
00001 /*************************************************************************\
00002 
00003   Copyright 1999 The University of North Carolina at Chapel Hill.
00004   All Rights Reserved.
00005 
00006   Permission to use, copy, modify and distribute this software and its
00007   documentation for educational, research and non-profit purposes, without
00008   fee, and without a written agreement is hereby granted, provided that the
00009   above copyright notice and the following three paragraphs appear in all
00010   copies.
00011 
00012   IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
00013   LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
00014   CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
00015   USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
00016   OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
00017   DAMAGES.
00018 
00019   THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
00020   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00021   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
00022   PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
00023   NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
00024   UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00025 
00026   The authors may be contacted via:
00027 
00028   US Mail:             E. Larsen
00029                        Department of Computer Science
00030                        Sitterson Hall, CB #3175
00031                        University of N. Carolina
00032                        Chapel Hill, NC 27599-3175
00033 
00034   Phone:               (919)962-1749
00035 
00036   EMail:               geom@cs.unc.edu
00037 
00038 
00039 \**************************************************************************/
00040 
00041 //--------------------------------------------------------------------------
00042 // File:   TriDist.cpp
00043 // Author: Eric Larsen
00044 // Description:
00045 // contains SegPoints() for finding closest points on a pair of line
00046 // segments and TriDist() for finding closest points on a pair of triangles
00047 //--------------------------------------------------------------------------
00048 
00049 #include "MatVec.h"
00050 #ifdef _WIN32
00051 #include <float.h>
00052 #define isnan _isnan
00053 #endif
00054 
00055 //--------------------------------------------------------------------------
00056 // SegPoints() 
00057 //
00058 // Returns closest points between an segment pair.
00059 // Implemented from an algorithm described in
00060 //
00061 // Vladimir J. Lumelsky,
00062 // On fast computation of distance between line segments.
00063 // In Information Processing Letters, no. 21, pages 55-61, 1985.   
00064 //--------------------------------------------------------------------------
00065 
00066 void
00067 SegPoints(PQP_REAL VEC[3], 
00068           PQP_REAL X[3], PQP_REAL Y[3],             // closest points
00069           const PQP_REAL P[3], const PQP_REAL A[3], // seg 1 origin, vector
00070           const PQP_REAL Q[3], const PQP_REAL B[3]) // seg 2 origin, vector
00071 {
00072   PQP_REAL T[3], A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
00073   PQP_REAL TMP[3];
00074 
00075   VmV(T,Q,P);
00076   A_dot_A = VdotV(A,A);
00077   B_dot_B = VdotV(B,B);
00078   A_dot_B = VdotV(A,B);
00079   A_dot_T = VdotV(A,T);
00080   B_dot_T = VdotV(B,T);
00081 
00082   // t parameterizes ray P,A 
00083   // u parameterizes ray Q,B 
00084 
00085   PQP_REAL t,u;
00086 
00087   // compute t for the closest point on ray P,A to
00088   // ray Q,B
00089 
00090   PQP_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
00091 
00092   t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
00093 
00094   // clamp result so t is on the segment P,A
00095 
00096   if ((t < 0) || isnan(t)) t = 0; else if (t > 1) t = 1;
00097 
00098   // find u for point on ray Q,B closest to point at t
00099 
00100   u = (t*A_dot_B - B_dot_T) / B_dot_B;
00101 
00102   // if u is on segment Q,B, t and u correspond to 
00103   // closest points, otherwise, clamp u, recompute and
00104   // clamp t 
00105 
00106   if ((u <= 0) || isnan(u)) {
00107 
00108     VcV(Y, Q);
00109 
00110     t = A_dot_T / A_dot_A;
00111 
00112     if ((t <= 0) || isnan(t)) {
00113       VcV(X, P);
00114       VmV(VEC, Q, P);
00115     }
00116     else if (t >= 1) {
00117       VpV(X, P, A);
00118       VmV(VEC, Q, X);
00119     }
00120     else {
00121       VpVxS(X, P, A, t);
00122       VcrossV(TMP, T, A);
00123       VcrossV(VEC, A, TMP);
00124     }
00125   }
00126   else if (u >= 1) {
00127 
00128     VpV(Y, Q, B);
00129 
00130     t = (A_dot_B + A_dot_T) / A_dot_A;
00131 
00132     if ((t <= 0) || isnan(t)) {
00133       VcV(X, P);
00134       VmV(VEC, Y, P);
00135     }
00136     else if (t >= 1) {
00137       VpV(X, P, A);
00138       VmV(VEC, Y, X);
00139     }
00140     else {
00141       VpVxS(X, P, A, t);
00142       VmV(T, Y, P);
00143       VcrossV(TMP, T, A);
00144       VcrossV(VEC, A, TMP);
00145     }
00146   }
00147   else {
00148 
00149     VpVxS(Y, Q, B, u);
00150 
00151     if ((t <= 0) || isnan(t)) {
00152       VcV(X, P);
00153       VcrossV(TMP, T, B);
00154       VcrossV(VEC, B, TMP);
00155     }
00156     else if (t >= 1) {
00157       VpV(X, P, A);
00158       VmV(T, Q, X);
00159       VcrossV(TMP, T, B);
00160       VcrossV(VEC, B, TMP);
00161     }
00162     else {
00163       VpVxS(X, P, A, t);
00164       VcrossV(VEC, A, B);
00165       if (VdotV(VEC, T) < 0) {
00166         VxS(VEC, VEC, -1);
00167       }
00168     }
00169   }
00170 }
00171 
00172 //--------------------------------------------------------------------------
00173 // TriDist() 
00174 //
00175 // Computes the closest points on two triangles, and returns the 
00176 // distance between them.
00177 // 
00178 // S and T are the triangles, stored tri[point][dimension].
00179 //
00180 // If the triangles are disjoint, P and Q give the closest points of 
00181 // S and T respectively. However, if the triangles overlap, P and Q 
00182 // are basically a random pair of points from the triangles, not 
00183 // coincident points on the intersection of the triangles, as might 
00184 // be expected.
00185 //--------------------------------------------------------------------------
00186 
00187 PQP_REAL 
00188 TriDist(PQP_REAL P[3], PQP_REAL Q[3],
00189         const PQP_REAL S[3][3], const PQP_REAL T[3][3])  
00190 {
00191   // Compute vectors along the 6 sides
00192 
00193   PQP_REAL Sv[3][3], Tv[3][3];
00194   PQP_REAL VEC[3];
00195 
00196   VmV(Sv[0],S[1],S[0]);
00197   VmV(Sv[1],S[2],S[1]);
00198   VmV(Sv[2],S[0],S[2]);
00199 
00200   VmV(Tv[0],T[1],T[0]);
00201   VmV(Tv[1],T[2],T[1]);
00202   VmV(Tv[2],T[0],T[2]);
00203 
00204   // For each edge pair, the vector connecting the closest points 
00205   // of the edges defines a slab (parallel planes at head and tail
00206   // enclose the slab). If we can show that the off-edge vertex of 
00207   // each triangle is outside of the slab, then the closest points
00208   // of the edges are the closest points for the triangles.
00209   // Even if these tests fail, it may be helpful to know the closest
00210   // points found, and whether the triangles were shown disjoint
00211 
00212   PQP_REAL V[3];
00213   PQP_REAL Z[3];
00214   PQP_REAL minP[3], minQ[3], mindd;
00215   int shown_disjoint = 0;
00216 
00217   mindd = VdistV2(S[0],T[0]) + 1;  // Set first minimum safely high
00218 
00219   for (int i = 0; i < 3; i++)
00220   {
00221     for (int j = 0; j < 3; j++)
00222     {
00223       // Find closest points on edges i & j, plus the 
00224       // vector (and distance squared) between these points
00225 
00226       SegPoints(VEC,P,Q,S[i],Sv[i],T[j],Tv[j]);
00227       
00228       VmV(V,Q,P);
00229       PQP_REAL dd = VdotV(V,V);
00230 
00231       // Verify this closest point pair only if the distance 
00232       // squared is less than the minimum found thus far.
00233 
00234       if (dd <= mindd)
00235       {
00236         VcV(minP,P);
00237         VcV(minQ,Q);
00238         mindd = dd;
00239 
00240         VmV(Z,S[(i+2)%3],P);
00241         PQP_REAL a = VdotV(Z,VEC);
00242         VmV(Z,T[(j+2)%3],Q);
00243         PQP_REAL b = VdotV(Z,VEC);
00244 
00245         if ((a <= 0) && (b >= 0)) return sqrt(dd);
00246 
00247         PQP_REAL p = VdotV(V, VEC);
00248 
00249         if (a < 0) a = 0;
00250         if (b > 0) b = 0;
00251         if ((p - a + b) > 0) shown_disjoint = 1;        
00252       }
00253     }
00254   }
00255 
00256   // No edge pairs contained the closest points.  
00257   // either:
00258   // 1. one of the closest points is a vertex, and the
00259   //    other point is interior to a face.
00260   // 2. the triangles are overlapping.
00261   // 3. an edge of one triangle is parallel to the other's face. If
00262   //    cases 1 and 2 are not true, then the closest points from the 9
00263   //    edge pairs checks above can be taken as closest points for the
00264   //    triangles.
00265   // 4. possibly, the triangles were degenerate.  When the 
00266   //    triangle points are nearly colinear or coincident, one 
00267   //    of above tests might fail even though the edges tested
00268   //    contain the closest points.
00269 
00270   // First check for case 1
00271 
00272   PQP_REAL Sn[3], Snl;       
00273   VcrossV(Sn,Sv[0],Sv[1]); // Compute normal to S triangle
00274   Snl = VdotV(Sn,Sn);      // Compute square of length of normal
00275   
00276   // If cross product is long enough,
00277 
00278   if (Snl > 1e-15)  
00279   {
00280     // Get projection lengths of T points
00281 
00282     PQP_REAL Tp[3]; 
00283 
00284     VmV(V,S[0],T[0]);
00285     Tp[0] = VdotV(V,Sn);
00286 
00287     VmV(V,S[0],T[1]);
00288     Tp[1] = VdotV(V,Sn);
00289 
00290     VmV(V,S[0],T[2]);
00291     Tp[2] = VdotV(V,Sn);
00292 
00293     // If Sn is a separating direction,
00294     // find point with smallest projection
00295 
00296     int point = -1;
00297     if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
00298     {
00299       if (Tp[0] < Tp[1]) point = 0; else point = 1;
00300       if (Tp[2] < Tp[point]) point = 2;
00301     }
00302     else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
00303     {
00304       if (Tp[0] > Tp[1]) point = 0; else point = 1;
00305       if (Tp[2] > Tp[point]) point = 2;
00306     }
00307 
00308     // If Sn is a separating direction, 
00309 
00310     if (point >= 0) 
00311     {
00312       shown_disjoint = 1;
00313 
00314       // Test whether the point found, when projected onto the 
00315       // other triangle, lies within the face.
00316     
00317       VmV(V,T[point],S[0]);
00318       VcrossV(Z,Sn,Sv[0]);
00319       if (VdotV(V,Z) > 0)
00320       {
00321         VmV(V,T[point],S[1]);
00322         VcrossV(Z,Sn,Sv[1]);
00323         if (VdotV(V,Z) > 0)
00324         {
00325           VmV(V,T[point],S[2]);
00326           VcrossV(Z,Sn,Sv[2]);
00327           if (VdotV(V,Z) > 0)
00328           {
00329             // T[point] passed the test - it's a closest point for 
00330             // the T triangle; the other point is on the face of S
00331 
00332             VpVxS(P,T[point],Sn,Tp[point]/Snl);
00333             VcV(Q,T[point]);
00334             return sqrt(VdistV2(P,Q));
00335           }
00336         }
00337       }
00338     }
00339   }
00340 
00341   PQP_REAL Tn[3], Tnl;       
00342   VcrossV(Tn,Tv[0],Tv[1]); 
00343   Tnl = VdotV(Tn,Tn);      
00344   
00345   if (Tnl > 1e-15)  
00346   {
00347     PQP_REAL Sp[3]; 
00348 
00349     VmV(V,T[0],S[0]);
00350     Sp[0] = VdotV(V,Tn);
00351 
00352     VmV(V,T[0],S[1]);
00353     Sp[1] = VdotV(V,Tn);
00354 
00355     VmV(V,T[0],S[2]);
00356     Sp[2] = VdotV(V,Tn);
00357 
00358     int point = -1;
00359     if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
00360     {
00361       if (Sp[0] < Sp[1]) point = 0; else point = 1;
00362       if (Sp[2] < Sp[point]) point = 2;
00363     }
00364     else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
00365     {
00366       if (Sp[0] > Sp[1]) point = 0; else point = 1;
00367       if (Sp[2] > Sp[point]) point = 2;
00368     }
00369 
00370     if (point >= 0) 
00371     { 
00372       shown_disjoint = 1;
00373 
00374       VmV(V,S[point],T[0]);
00375       VcrossV(Z,Tn,Tv[0]);
00376       if (VdotV(V,Z) > 0)
00377       {
00378         VmV(V,S[point],T[1]);
00379         VcrossV(Z,Tn,Tv[1]);
00380         if (VdotV(V,Z) > 0)
00381         {
00382           VmV(V,S[point],T[2]);
00383           VcrossV(Z,Tn,Tv[2]);
00384           if (VdotV(V,Z) > 0)
00385           {
00386             VcV(P,S[point]);
00387             VpVxS(Q,S[point],Tn,Sp[point]/Tnl);
00388             return sqrt(VdistV2(P,Q));
00389           }
00390         }
00391       }
00392     }
00393   }
00394 
00395   // Case 1 can't be shown.
00396   // If one of these tests showed the triangles disjoint,
00397   // we assume case 3 or 4, otherwise we conclude case 2, 
00398   // that the triangles overlap.
00399   
00400   if (shown_disjoint)
00401   {
00402     VcV(P,minP);
00403     VcV(Q,minQ);
00404     return sqrt(mindd);
00405   }
00406   else return 0;
00407 }


jskeus
Author(s): JSK Alumnis
autogenerated on Tue Mar 7 2017 04:04:34