DistFuncs.cpp
Go to the documentation of this file.
00001 #include "DistFuncs.h"
00002 #if 1
00003 std::ostream &operator<<(std::ostream &ost, const Point& p)
00004 {
00005     ost << "(" << p.x << ", " << p.y << ", " << p.z << ")";
00006     return ost;
00007 } 
00008 
00009 #endif
00010 
00024 inline float SegSegDist(const Point& u0, const Point& u,
00025                         const Point& v0, const Point& v,
00026                         Point& cp0, Point& cp1)
00027 {
00028     Point    w = u0 - v0;
00029     float    a = u|u;        // always >= 0
00030     float    b = u|v;
00031     float    c = v|v;        // always >= 0
00032     float    d = u|w;
00033     float    e = v|w;
00034     float    D = a*c - b*b;       // always >= 0
00035     float    sc, sN, sD = D;      // sc = sN / sD, default sD = D >= 0
00036     float    tc, tN, tD = D;      // tc = tN / tD, default tD = D >= 0
00037 
00038     // compute the line parameters of the two closest points
00039 #define EPS 1e-8
00040     if (D < EPS) { // the lines are almost parallel
00041         sN = 0.0;        // force using point P0 on segment S1
00042         sD = 1.0;        // to prevent possible division by 0.0 later
00043         tN = e;
00044         tD = c;
00045     }
00046     else {                // get the closest points on the infinite lines
00047         sN = (b*e - c*d);
00048         tN = (a*e - b*d);
00049         if (sN < 0.0) {       // sc < 0 => the s=0 edge is visible
00050             sN = 0.0;
00051             tN = e;
00052             tD = c;
00053         }
00054         else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
00055             sN = sD;
00056             tN = e + b;
00057             tD = c;
00058         }
00059     }
00060 
00061     if (tN < 0.0) {           // tc < 0 => the t=0 edge is visible
00062         tN = 0.0;
00063         // recompute sc for this edge
00064         if (-d < 0.0)
00065             sN = 0.0;
00066         else if (-d > a)
00067             sN = sD;
00068         else {
00069             sN = -d;
00070             sD = a;
00071         }
00072     }
00073     else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
00074         tN = tD;
00075         // recompute sc for this edge
00076         if ((-d + b) < 0.0)
00077             sN = 0;
00078         else if ((-d + b) > a)
00079             sN = sD;
00080         else {
00081             sN = (-d + b);
00082             sD = a;
00083         }
00084     }
00085     // finally do the division to get sc and tc
00086     sc = (fabsf(sN) < EPS ? 0.0f : sN / sD);
00087     tc = (fabsf(tN) < EPS ? 0.0f : tN / tD);
00088 
00089     cp0 = u0 + sc * u;
00090     cp1 = v0 + tc * v;
00091 
00092     // get the difference of the two closest points
00093     Point dP = cp0 - cp1; 
00094 
00095     return dP.Magnitude();   // return the closest distance
00096 }
00097 
00105 inline float PointPlaneDist(const Point& P, const Point& pointOnPlane, const Point& n,
00106                      Point& cp)
00107 {
00108     Point v = P - pointOnPlane;
00109     float l = v|n;
00110     cp = P-l*n;
00111     return l;
00112 }
00113 
00114 // see DistFuncs.h
00115 float PointSegDist(const Point& P, const Point& u0, const Point& u1)
00116 {
00117     Point v = P - u0;
00118     Point dir = u1 - u0;
00119     float l = dir.Magnitude();
00120     dir /= l;
00121     
00122     float x = v|dir;
00123     if (x < 0){
00124         return v.Magnitude();
00125     }else if (x > l){
00126         Point dv = P - u1;
00127         return dv.Magnitude();
00128     }else{
00129         return sqrtf(v.SquareMagnitude() - x*x);
00130     }
00131 }
00132 
00141 inline bool PointFaceAppTest(const Point& P, const Point** vertices,
00142                              const Point* edges, const Point& n)
00143 {
00144     Point v, outer;
00145     for (unsigned int i=0; i<3; i++){
00146         v = P - *vertices[i];
00147         outer = edges[i]^n;
00148         if ((v|outer)>0) return false;
00149     }
00150     return true;
00151 }
00152 
00153 // see DistFuncs.h
00154 float TriTriDist(const Point& U0, const Point& U1, const Point& U2,
00155                  const Point& V0, const Point& V1, const Point& V2,
00156                  Point& cp0, Point& cp1)
00157 {
00158     const Point* uvertices[] = {&U0, &U1, &U2};
00159     const Point* vvertices[] = {&V0, &V1, &V2};
00160     float min_d, d;
00161     Point p0, p1, n, vec;
00162     bool overlap = true;
00163 
00164     // edges
00165     Point uedges[3], vedges[3];
00166     for (unsigned int i=0; i<3; i++){
00167         uedges[i] = *uvertices[(i+1)%3] - *uvertices[i];
00168         vedges[i] = *vvertices[(i+1)%3] - *vvertices[i];
00169     }
00170 
00171     // set initial values
00172     cp0 = U0;
00173     cp1 = V0;
00174     min_d = (cp0-V0).Magnitude();
00175 
00176     // vertex-vertex, vertex-edge, edge-edge
00177     for (unsigned int i=0; i<3; i++){
00178         for (unsigned int j=0; j<3; j++){
00179             d = SegSegDist(*uvertices[i], uedges[i],
00180                            *vvertices[j], vedges[j],
00181                            p0, p1);
00182             n = p0 - p1;
00183             if (d <= min_d){
00184                 min_d = d;
00185                 cp0 = p0;
00186                 cp1 = p1;
00187                 // check overlap
00188                 vec = *uvertices[(i+2)%3] - cp0;
00189                 float u = (vec|n)/min_d;
00190                 vec = *vvertices[(j+2)%3] - cp1;
00191                 float v = (vec|n)/min_d;
00192                 // n directs from v -> u
00193                 if (u>=0 && v<=0) return min_d;
00194 
00195                 if (u > 0) u = 0;
00196                 if (v < 0) v = 0;
00197                 if ((n.Magnitude() + u - v) > 0){
00198                     overlap = false;
00199                 }
00200             }
00201         }
00202     }
00203 
00204     Point un = uedges[0]^uedges[1];
00205     un.Normalize();
00206     Point vn = vedges[0]^vedges[1];
00207     vn.Normalize();
00208 
00209     // vertex-face, edge-face, face-face
00210     // plane-triangle overlap test
00211     float ds[3];
00212     int rank;
00213     //   u and plane including v
00214     rank = -1;
00215     for (int i=0; i<3; i++){
00216         vec = *uvertices[i] - *vvertices[0];
00217         ds[i] = vec|vn; 
00218     }
00219     if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){
00220         // find rank of the nearest vertex to the plane
00221         rank = ds[0] > ds[1] ? 1 : 0;
00222         rank = ds[rank] > ds[2] ? 2 : rank;
00223     }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){
00224         // find rank of the nearest vertex to the plane
00225         rank = ds[0] > ds[1] ? 0 : 1;
00226         rank = ds[rank] > ds[2] ? rank : 2;
00227     }
00228     if (rank >=0){
00229         overlap = false;
00230         if (PointFaceAppTest(*uvertices[rank], vvertices, vedges, vn)){
00231             min_d = fabsf(PointPlaneDist(*uvertices[rank], *vvertices[0], vn, p1));
00232             cp0 = *uvertices[rank];
00233             cp1 = p1;
00234             return min_d;
00235         }
00236     }
00237 
00238     //   v and plane including u
00239     rank = -1;
00240     for (int i=0; i<3; i++){
00241         vec = *vvertices[i] - *uvertices[0];
00242         ds[i] = vec|un; 
00243     }
00244     if (ds[0] > 0 && ds[1] > 0 && ds[2] > 0){
00245         // find rank of the nearest vertex to the plane
00246         rank = ds[0] > ds[1] ? 1 : 0;
00247         rank = ds[rank] > ds[2] ? 2 : rank;
00248     }else if(ds[0] < 0 && ds[1] < 0 && ds[2] < 0){
00249         // find rank of the nearest vertex to the plane
00250         rank = ds[0] > ds[1] ? 0 : 1;
00251         rank = ds[rank] > ds[2] ? rank : 2;
00252     }
00253     if (rank >= 0){
00254         overlap = false;
00255         if (PointFaceAppTest(*vvertices[rank], uvertices, uedges, un)){
00256             min_d = fabsf(PointPlaneDist(*vvertices[rank], *uvertices[0], un, p0));
00257             cp0 = p0;
00258             cp1 = *vvertices[rank];
00259             return min_d;
00260         }
00261     }
00262 
00263     if (overlap){
00264         return 0;
00265     }else{
00266         return min_d;
00267     }
00268 }
00269 
00270 // see DistFuncs.h
00271 float SegSegDist(const Point& u0, const Point& u1,
00272                  const Point& v0, const Point& v1)
00273 {
00274     Point cp0, cp1;
00275     return SegSegDist(u0, u1-u0, v0, v1-v0, cp0, cp1);
00276 }
00277 
00278 #if 0
00279 
00280 int main()
00281 {
00282     Point p0(0,0,0), p1(2,0,0), p2(1, 1, -1), p3(1, 1, 1);
00283     Point cp1, cp2, n;
00284     float d;
00285 
00286     d= SegSegDist(p0, p1, p2, p3, cp1, cp2);
00287     std::cout << "test1 : d = " << d << ", cp1 = " << cp1 
00288               << ", cp2 = " << cp2 << std::endl;
00289 
00290     Point p4(0,1,0), p5(2,1,0);
00291     d= SegSegDist(p0, p1, p4, p5, cp1, cp2);
00292     std::cout << "test2 : d = " << d << ", cp1 = " << cp1 
00293               << ", cp2 = " << cp2 << std::endl;
00294 
00295     d= SegSegDist(p0, p1, p0, p1, cp1, cp2);
00296     std::cout << "test3 : d = " << d << ", cp1 = " << cp1 
00297               << ", cp2 = " << cp2 << std::endl;
00298 
00299     Point p6(0, 2, 0), p7(-2, 1, 0);
00300     d = TriTriDist(p0, p1, p5, p4, p6, p7, cp1, cp2);
00301     std::cout << "test4 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 
00302               << std::endl;
00303 
00304     Point p8(3, -1, 1), p9(3, 2, 1), p10(-3, -1, 1);
00305     d = TriTriDist(p0, p1, p5, p8, p9, p10, cp1, cp2);
00306     std::cout << "test5 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 
00307               << std::endl;
00308 
00309     d = TriTriDist(p0, p1, p2, p8, p9, p10, cp1, cp2);
00310     std::cout << "test6 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 
00311               << std::endl;
00312     std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)" 
00313               << std::endl; 
00314 
00315     d = TriTriDist(p2, p0, p1, p8, p9, p10, cp1, cp2);
00316     std::cout << "test7 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 
00317               << std::endl;
00318     std::cout << "answer: d = 1, cp1 = (0, 0, 0), cp2 = (0, 0, 1)" 
00319               << std::endl; 
00320 
00321     d = TriTriDist(p1, p2, p0, p8, p9, p10, cp1, cp2);
00322     std::cout << "test8 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " << cp2 
00323               << std::endl;
00324     std::cout << "answer: d = 1, cp1 = (2, 0, 0), cp2 = (2, 0, 1)" 
00325               << std::endl; 
00326 
00327     {
00328         Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0),
00329             pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1);
00330         d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2);
00331         std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " 
00332                   << cp2 << std::endl;
00333         std::cout << "answer: d = 0.0" << std::endl;
00334     }
00335 
00336     {
00337         Point pp0(-2, 2, 0), pp1(-2, -2, 0), pp2(5, -2, 0),
00338             pp3(-0.1, 0.4, -0.1), pp4(-0.1, -0.4, -0.1), pp5(-0.1, -0.4, 0.1);
00339         d = TriTriDist(pp0, pp1, pp2, pp3, pp4, pp5, cp1, cp2);
00340         std::cout << "test9 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " 
00341                   << cp2 << std::endl;
00342         std::cout << "answer: d = 0.0" << std::endl;
00343     }
00344 
00345     {
00346         Point p0(0.1, 0.4, 0), p1(-0.1, 0.4, 0), p2(0.1, -0.4, 0);
00347         Point p3(0.05, 0.45, -0.15), p4(0.05, 0.45, 0.05), p5(0.05, -0.35, -0.15);
00348         d = TriTriDist(p0, p1, p2, p3, p4, p5, cp1, cp2);
00349         std::cout << "test10 : d = " << d << ", cp1 = " << cp1 << ", cp2 = " 
00350                   << cp2 << std::endl;
00351         std::cout << "answer: d = 0.0" << std::endl;
00352     }
00353     return 0;
00354 }
00355 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53