mirror of
https://github.com/BelfrySCAD/BOSL2.git
synced 2024-12-29 16:29:40 +00:00
Merge pull request #591 from RonaldoCMP/master
Better codes for _closest_s
This commit is contained in:
Revar Desmera
GitHub
commit
85a7085ea9
1 changed files with 50 additions and 54 deletions
104
geometry.scad
104
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])),
|
||||
"The input list should be a consistent non empty list of points of same dimension.")
|
||||
assert(is_matrix(points1) && is_matrix(points2,undef,len(points1[0])),
|
||||
"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
|
||||
// and may produce a non-stopping loop
|
||||
close || [for(nv=norm(v), s=simplex) if(norm(s-v)<=eps*nv) 1]!=[] ? d :
|
||||
close ? 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])),
|
||||
"The input list should be a consistent non empty list of points of same dimension.")
|
||||
assert(is_matrix(points1) && is_matrix(points2,undef,len(points1[0])),
|
||||
"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) )
|
||||
v*d > eps ? false : // no collision
|
||||
let( v = _support_diff(points1,points2,-d) )
|
||||
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)
|
||||
: _closest_s3(s,eps);
|
||||
len(s)==3 ? _closest_s2(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]),
|
||||
a = dim==3 ? s[0]: [ each s[0], 0] ,
|
||||
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)
|
||||
area = cross(s[2]-s[0], s[1]-s[0]),
|
||||
area2 = area*area // tri area squared
|
||||
)
|
||||
norm(nr) <= eps*max(ab,bc,ca) // degenerate case
|
||||
? let( i = max_index([ab, bc, ca]) )
|
||||
_closest_s1([s[i],s[(i+1)%3]],eps)
|
||||
// considering that s[2] was the last inserted vertex in s,
|
||||
// the only possible outcomes are :
|
||||
// s, [s[0],s[2]] and [s[1],s[2]]
|
||||
area2<=eps*max([for(si=s) pow(si*si,2)]) // degenerate tri
|
||||
? norm(s[2]-s[0]) < norm(s[2]-s[1])
|
||||
? _closest_s1([s[1],s[2]])
|
||||
: _closest_s1([s[0],s[2]])
|
||||
: let(
|
||||
class = (cross(nr,a-b)*a<0 ? 1 : 0 )
|
||||
+ (cross(nr,c-a)*a<0 ? 2 : 0 )
|
||||
+ (cross(nr,b-c)*b<0 ? 4 : 0 )
|
||||
crx1 = cross(s[0], s[2])*area,
|
||||
crx2 = cross(s[1], s[0])*area,
|
||||
crx0 = cross(s[2], s[1])*area
|
||||
)
|
||||
assert( class!=1, "Internal error" )
|
||||
class==0 ? [ nr*(nr*a)/(nr*nr), s] : // origin projects (or is) on the tri
|
||||
// class==1 ? _closest_s1([s[0],s[1]]) :
|
||||
class==2 ? _closest_s1([s[0],s[2]],eps) :
|
||||
class==4 ? _closest_s1([s[1],s[2]],eps) :
|
||||
// class==3 ? a*(a-b)> 0 ? _closest_s1([s[0],s[1]]) : _closest_s1([s[0],s[2]]) :
|
||||
class==3 ? _closest_s1([s[0],s[2]],eps) :
|
||||
// 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) :
|
||||
c*(c-a)>0 ? _closest_s1([s[0],s[2]],eps) : _closest_s1([s[1],s[2]],eps);
|
||||
|
||||
// all have the same signal -> origin projects inside the tri
|
||||
max(crx1, crx0, crx2) < 0 || min(crx1, crx0, crx2) > 0
|
||||
? // baricentric coords of projection
|
||||
[ [abs(crx0),abs(crx1),abs(crx2)]*s/area2, s ]
|
||||
: let(
|
||||
cl12 = _closest_s1([s[1],s[2]]),
|
||||
cl02 = _closest_s1([s[0],s[2]])
|
||||
)
|
||||
norm(cl12[0])<norm(cl02[0]) ? cl12 : cl02;
|
||||
|
||||
|
||||
// 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]) ] )
|
||||
norm(nr)<eps*max(sz)
|
||||
? let( i = max_index(sz) )
|
||||
sz = [ norm(s[0]-s[1]), norm(s[1]-s[2]), norm(s[2]-s[0]) ] )
|
||||
norm(nr)<=eps*pow(max(sz),2)
|
||||
? 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]
|
||||
: let(
|
||||
// s or some of the 3 faces of s containing s[3]
|
||||
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
|
||||
Loading…
Reference in a new issue