Merge pull request #591 from RonaldoCMP/master

Better codes for _closest_s

geometry.scad

@@ -1139,7 +1139,7 @@ function plane_offset(plane) =
 //   }
 function projection_on_plane(plane, points) =
     assert( _valid_plane(plane), "Invalid plane." )
-    assert( is_path(points), "Invalid list of points or dimension." )
+    assert( is_matrix(points,undef,3), "Invalid list of points or dimension." )
     let(
         p  = len(points[0])==2
              ? [for(pi=points) point3d(pi) ]
@@ -1778,6 +1778,7 @@ function circle_line_intersection(c,r,d,line,bounded=false,eps=EPSILON) =
 function noncollinear_triple(points,error=true,eps=EPSILON) =
     assert( is_path(points), "Invalid input points." )
     assert( is_finite(eps) && (eps>=0), "The tolerance should be a non-negative value." )
+    len(points)<3 ? [] :
     let(
         pa = points[0],
         b  = furthest_point(pa, points),
@@ -2105,6 +2106,7 @@ function point_in_polygon(point, poly, nonzero=true, eps=EPSILON) =
 // Usage:
 //   test = polygon_is_clockwise(poly);
 // Topics: Geometry, Polygons, Clockwise
+// See Also: clockwise_polygon(), ccw_polygon(), reverse_polygon()
 // Description:
 //   Return true if the given 2D simple polygon is in clockwise order, false otherwise.
 //   Results for complex (self-intersecting) polygon are indeterminate.
@@ -2119,6 +2121,7 @@ function polygon_is_clockwise(poly) =
 // Usage:
 //   newpoly = clockwise_polygon(poly);
 // Topics: Geometry, Polygons, Clockwise
+// See Also: polygon_is_clockwise(), ccw_polygon(), reverse_polygon()
 // Description:
 //   Given a 2D polygon path, returns the clockwise winding version of that path.
 // Arguments:
@@ -2131,6 +2134,7 @@ function clockwise_polygon(poly) =
 // Function: ccw_polygon()
 // Usage:
 //   newpoly = ccw_polygon(poly);
+// See Also: polygon_is_clockwise(), clockwise_polygon(), reverse_polygon()
 // Topics: Geometry, Polygons, Clockwise
 // Description:
 //   Given a 2D polygon poly, returns the counter-clockwise winding version of that poly.
@@ -2145,6 +2149,7 @@ function ccw_polygon(poly) =
 // Usage:
 //   newpoly = reverse_polygon(poly)
 // Topics: Geometry, Polygons, Clockwise
+// See Also: polygon_is_clockwise(), ccw_polygon(), clockwise_polygon()
 // Description:
 //   Reverses a polygon's winding direction, while still using the same start point.
 // Arguments:
@@ -2397,8 +2402,8 @@ function is_convex_polygon(poly,eps=EPSILON) =
 //    echo(convex_distance(sphr1[0], sphr2[0])); // Returns: 0
 //    echo(convex_distance(sphr1[0], sphr3[0])); // Returns: 0.5
 function convex_distance(points1, points2, eps=EPSILON) =
-    assert(is_matrix(points1) && is_matrix(points2,undef,len(points1[0])),
+    assert(is_matrix(points1) && is_matrix(points2,undef,len(points1[0])), 
-           "The input list should be a consistent non empty list of points of same dimension.")
+           "The input lists should be compatible consistent non empty lists of points.")
     assert(len(points1[0])==2 || len(points1[0])==3 ,
            "The input points should be 2d or 3d points.")
     let( d = points1[0]-points2[0] )
@@ -2418,9 +2423,7 @@ function _GJK_distance(points1, points2, eps=EPSILON, lbd, d, simplex=[]) =
         lbd   = max(lbd, d*v/nrd), // distance lower bound
         close = (nrd-lbd <= eps*nrd)
     )
-    // v already in the simplex is a degenerence due to numerical errors
+    close ? d :
-    // and may produce a non-stopping loop
-    close || [for(nv=norm(v), s=simplex) if(norm(s-v)<=eps*nv) 1]!=[] ? d :
     let( newsplx = _closest_simplex(concat(simplex,[v]),eps) )
     _GJK_distance(points1, points2, eps, lbd, newsplx[0], newsplx[1]);
 
@@ -2459,8 +2462,8 @@ function _GJK_distance(points1, points2, eps=EPSILON, lbd, d, simplex=[]) =
 //    echo(convex_collision(sphr1[0], sphr3[0])); // Returns: false
 //
 function convex_collision(points1, points2, eps=EPSILON) =
-    assert(is_matrix(points1) && is_matrix(points2,undef,len(points1[0])),
+    assert(is_matrix(points1) && is_matrix(points2,undef,len(points1[0])), 
-           "The input list should be a consistent non empty list of points of same dimension.")
+           "The input lists should be compatible consistent non empty lists of points.")
     assert(len(points1[0])==2 || len(points1[0])==3 ,
            "The input points should be 2d or 3d points.")
     let( d = points1[0]-points2[0] )
@@ -2475,9 +2478,10 @@ function convex_collision(points1, points2, eps=EPSILON) =
 // http://www.dtecta.com/papers/jgt98convex.pdf
 function _GJK_collide(points1, points2, d, simplex, eps=EPSILON) =
     norm(d) < eps ? true :          // does collide
-    let( v = _support_diff(points1,points2,-d) )
+    let( v = _support_diff(points1,points2,-d) ) 
-    v*d > eps ? false : // no collision
+    v*d > eps*eps ? false : // no collision
     let( newsplx = _closest_simplex(concat(simplex,[v]),eps) )
+    norm(v-newsplx[0])<eps ? norm(v)<eps :
     _GJK_collide(points1, points2, newsplx[0], newsplx[1], eps);
 
 
@@ -2485,16 +2489,15 @@ function _GJK_collide(points1, points2, d, simplex, eps=EPSILON) =
 //  - the point of the s closest to the origin
 //  - the smallest sub-simplex of s that contains that point
 function _closest_simplex(s,eps=EPSILON) =
-    assert(len(s)>=2 && len(s)<=4, "Internal error.")
     len(s)==2 ? _closest_s1(s,eps) :
-    len(s)==3 ? _closest_s2(s,eps)
+    len(s)==3 ? _closest_s2(s,eps) :
-              : _closest_s3(s,eps);
+    len(s)==4 ? _closest_s3(s,eps) :
+    assert(false, "Internal error.");
 
 
 // find the point of a 1-simplex closest to the origin
-// Based on: http://uu.diva-portal.org/smash/get/diva2/FFULLTEXT01.pdf
 function _closest_s1(s,eps=EPSILON) =
-    norm(s[1]-s[0])<eps*(norm(s[0])+norm(s[1]))/2 ? [ s[0], [s[0]] ] :
+    norm(s[1]-s[0])<=eps*(norm(s[0])+norm(s[1]))/2 ? [ s[0], [s[0]] ] :
     let(
         c = s[1]-s[0],
         t = -s[0]*c/(c*c)
@@ -2505,54 +2508,47 @@ function _closest_s1(s,eps=EPSILON) =
 
 
 // find the point of a 2-simplex closest to the origin
-// Based on: http://uu.diva-portal.org/smash/get/diva2/FFULLTEXT01.pdf
+function _closest_s2(s, eps=EPSILON) =
-function _closest_s2(s,eps=EPSILON) =
+    // considering that s[2] was the last inserted vertex in s by GJK, 
+    // the plane orthogonal to the triangle [ origin, s[0], s[1] ] that 
+    // contains [s[0],s[1]] have the origin and s[2] on the same side;
+    // that reduces the cases to test and the only possible simplex
+    // outcomes are s, [s[0],s[2]] and [s[1],s[2]] 
     let(
-        dim = len(s[0]),
+        area  = cross(s[2]-s[0], s[1]-s[0]), 
-        a  = dim==3 ? s[0]: [ each s[0], 0] ,
+        area2 = area*area                     // tri area squared
-        b  = dim==3 ? s[1]: [ each s[1], 0] ,
-        c  = dim==3 ? s[2]: [ each s[2], 0] ,
-        ab = norm(a-b),
-        bc = norm(b-c),
-        ca = norm(c-a),
-        nr = cross(b-a,c-a)
     )
-    norm(nr) <= eps*max(ab,bc,ca) // degenerate case
+    area2<=eps*max([for(si=s) pow(si*si,2)]) // degenerate tri
-    ?   let( i = max_index([ab, bc, ca]) )
+    ?   norm(s[2]-s[0]) < norm(s[2]-s[1]) 
-        _closest_s1([s[i],s[(i+1)%3]],eps)
+        ? _closest_s1([s[1],s[2]])
-// considering that s[2] was the last inserted vertex in s,
+        : _closest_s1([s[0],s[2]])
-// the only possible outcomes are :
-//    s, [s[0],s[2]] and [s[1],s[2]]
     :   let(
-            class =   (cross(nr,a-b)*a<0 ? 1 : 0 )
+            crx1  = cross(s[0], s[2])*area,
-                    + (cross(nr,c-a)*a<0 ? 2 : 0 )
+            crx2  = cross(s[1], s[0])*area,
-                    + (cross(nr,b-c)*b<0 ? 4 : 0 )
+            crx0  = cross(s[2], s[1])*area
         )
-        assert( class!=1, "Internal error" )
+        // all have the same signal -> origin projects inside the tri 
-        class==0 ? [ nr*(nr*a)/(nr*nr), s] : // origin projects (or is) on the tri
+        max(crx1, crx0, crx2) < 0  || min(crx1, crx0, crx2) > 0
-//        class==1 ? _closest_s1([s[0],s[1]]) :
+        ?   // baricentric coords of projection   
-        class==2 ? _closest_s1([s[0],s[2]],eps) :
+            [ [abs(crx0),abs(crx1),abs(crx2)]*s/area2, s ] 
-        class==4 ? _closest_s1([s[1],s[2]],eps) :
+       :   let( 
-//        class==3 ? a*(a-b)> 0 ? _closest_s1([s[0],s[1]]) : _closest_s1([s[0],s[2]]) :
+               cl12 = _closest_s1([s[1],s[2]]),
-        class==3 ? _closest_s1([s[0],s[2]],eps) :
+               cl02 = _closest_s1([s[0],s[2]])
-//        class==5 ? b*(b-c)<=0 ? _closest_s1([s[0],s[1]]) : _closest_s1([s[1],s[2]]) :
+            )
-        class==5 ? _closest_s1([s[1],s[2]],eps) :
+            norm(cl12[0])<norm(cl02[0]) ? cl12 : cl02;
-        c*(c-a)>0 ? _closest_s1([s[0],s[2]],eps) : _closest_s1([s[1],s[2]],eps);
+        
-
 
 // find the point of a 3-simplex closest to the origin
-// it seems that degenerate 3-simplices are correctly manage without extra code
 function _closest_s3(s,eps=EPSILON) =
-    assert( len(s[0])==3 && len(s)==4, "Internal error." )
     let( nr = cross(s[1]-s[0],s[2]-s[0]),
-         sz = [ norm(s[1]-s[0]), norm(s[1]-s[2]), norm(s[2]-s[0]) ] )
+         sz = [ norm(s[0]-s[1]), norm(s[1]-s[2]), norm(s[2]-s[0]) ] )
-    norm(nr)<eps*max(sz)
+    norm(nr)<=eps*pow(max(sz),2)
-      ? let( i = max_index(sz) )
+    ?   let( i = max_index(sz) )
         _closest_s2([ s[i], s[(i+1)%3], s[3] ], eps) // degenerate case
-        // considering that s[3] was the last inserted vertex in s,
+    :   // considering that s[3] was the last inserted vertex in s by GJK,
         // the only possible outcomes will be:
-        //    s or some of the 3 triangles of s containing s[3]
+        //    s or some of the 3 faces of s containing s[3]
-      : let(
+        let(
             tris = [ [s[0], s[1], s[3]],
                      [s[1], s[2], s[3]],
                      [s[2], s[0], s[3]] ],
@@ -2581,4 +2577,4 @@ function _support_diff(p1,p2,d) =
 
 
 
-// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap
+// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap