From e03492329887b6dbab531f02b49b82fe5a56bc06 Mon Sep 17 00:00:00 2001 From: Henrik Tidefelt Date: Sun, 5 Jul 2009 02:45:01 +0200 Subject: [PATCH] Bugfix: 3D path-to-point approximation now reports a problem correctly. The fix will be done later, when this branch has been merged with ht/approximate branch. --- source/elementarypath3D.cc | 143 ++++++++++++++++++++++++--------------------- 1 file changed, 78 insertions(+), 65 deletions(-) diff --git a/source/elementarypath3D.cc b/source/elementarypath3D.cc index 47ffd71c..9b491b4c 100644 --- a/source/elementarypath3D.cc +++ b/source/elementarypath3D.cc @@ -1886,7 +1886,7 @@ namespace Shapes * Since (by construction) no point can have smaller y-coordinate than p, all angles * are in the range [ 0, pi ], and it suffices to reason in terms of the argument * to the monotonic function arccotan. - * The ordinary counter-clockwise-of by means of computing the projection of the + * The ordinary counter-clockwise-of by means of computing the projection of the * second on the clockwise normal of the other leads to the same equation. */ const Concrete::Length d1x = p1->x_ - p_.x_; @@ -1943,84 +1943,97 @@ Shapes::ApproximationSegmentSection3D::triangleDistance( std::vector< const Conc * Note: We will mess around with points, but it will be returned as passed. */ - /* Remember: - * points[0] is the point which is on the inside side of the triangle. - * points[1..3] are the corners of the triangle. - */ - const Concrete::UnitFloatTriple n = Concrete::crossDirection( *points[2] - *points[1], *points[3] - *points[1] ); - const Concrete::Length tn = Concrete::inner( n, p - *points[1] ); - if( ( tn > Concrete::ZERO_LENGTH ) == ( Concrete::inner( n, *points[0] - *points[1] ) > Concrete::ZERO_LENGTH ) ) + try { - /* When p is on the same side of the plane as points[0], we don't update *bestDist. - * If *bestDist is never updated, it means that p is inside all planes and the minimum distance - * is 0. See calling function. + /* Remember: + * points[0] is the point which is on the inside side of the triangle. + * points[1..3] are the corners of the triangle. */ - return; - } - - Concrete::Length res = HUGE_VAL; - const Concrete::Coords3D pProj = p - tn * n; + /* This is the call that may throw a NonLocalExit exception, which we should catch! */ + const Concrete::UnitFloatTriple n = Concrete::crossDirection( *points[2] - *points[1], *points[3] - *points[1] ); - std::vector< const Concrete::Coords3D * >::iterator i0 = points.begin( ); - ++i0; - std::vector< const Concrete::Coords3D * >::iterator begin = i0; - for( ; i0 != points.end( ); ++i0 ) - { - const Concrete::Coords3D * tmp = *begin; - *begin = *i0; - *i0 = tmp; - - std::vector< const Concrete::Coords3D * >::iterator j = begin; - const Concrete::Coords3D & pInside( **j ); - ++j; - const Concrete::Coords3D & p0( **j ); - ++j; - const Concrete::Coords3D & p1( **j ); - - /* To find an outward normal, we find the projection of pInside, and then take a vector - * from pInside to the projection. (We work with coordinates less p0.) - */ - const Concrete::Coords3D d1 = p1 - p0; - const Concrete::Coords3D dInside = pInside - p0; - const Concrete::Coords3D dp = p - p0; - double d1Norm2 = Concrete::innerScalar( d1, d1 ); - if( d1Norm2 == 0 ) + const Concrete::Length tn = Concrete::inner( n, p - *points[1] ); + if( ( tn > Concrete::ZERO_LENGTH ) == ( Concrete::inner( n, *points[0] - *points[1] ) > Concrete::ZERO_LENGTH ) ) { - res = min( res, dp.norm( ) ); - goto next; + /* When p is on the same side of the plane as points[0], we don't update *bestDist. + * If *bestDist is never updated, it means that p is inside all planes and the minimum distance + * is 0. See calling function. + */ + return; } - /* Introduce new scope to prevent compiler warnings about goto past initializations. - */ - { - double sInside = Concrete::innerScalar( dInside, d1 ) / d1Norm2; - const Concrete::Coords3D nOut = d1 * sInside - dInside; - const Concrete::Coords3D dpProj = pProj - p0; + Concrete::Length res = HUGE_VAL; + + const Concrete::Coords3D pProj = p - tn * n; - if( Concrete::innerScalar( nOut, nOut ) > 0 && - Concrete::innerScalar( nOut, dpProj ) < 0 ) + std::vector< const Concrete::Coords3D * >::iterator i0 = points.begin( ); + ++i0; + std::vector< const Concrete::Coords3D * >::iterator begin = i0; + for( ; i0 != points.end( ); ++i0 ) + { + const Concrete::Coords3D * tmp = *begin; + *begin = *i0; + *i0 = tmp; + + std::vector< const Concrete::Coords3D * >::iterator j = begin; + const Concrete::Coords3D & pInside( **j ); + ++j; + const Concrete::Coords3D & p0( **j ); + ++j; + const Concrete::Coords3D & p1( **j ); + + /* To find an outward normal, we find the projection of pInside, and then take a vector + * from pInside to the projection. (We work with coordinates less p0.) + */ + const Concrete::Coords3D d1 = p1 - p0; + const Concrete::Coords3D dInside = pInside - p0; + const Concrete::Coords3D dp = p - p0; + double d1Norm2 = Concrete::innerScalar( d1, d1 ); + if( d1Norm2 == 0 ) + { + res = min( res, dp.norm( ) ); + goto next; + } + + /* Introduce new scope to prevent compiler warnings about goto past initializations. + */ { - /* pProj is on the same side as pInside - */ - goto next; + double sInside = Concrete::innerScalar( dInside, d1 ) / d1Norm2; + const Concrete::Coords3D nOut = d1 * sInside - dInside; + const Concrete::Coords3D dpProj = pProj - p0; + + if( Concrete::innerScalar( nOut, nOut ) > 0 && + Concrete::innerScalar( nOut, dpProj ) < 0 ) + { + /* pProj is on the same side as pInside + */ + goto next; + } + + res = min( res, ( dp - d1 * ( Concrete::innerScalar( dpProj, d1 ) / d1Norm2 ) ).norm( ) ); } + next: + /* Swap back so that points remains unchanged + */ + *i0 = *begin; + *begin = tmp; + } - res = min( res, ( dp - d1 * ( Concrete::innerScalar( dpProj, d1 ) / d1Norm2 ) ).norm( ) ); - } - next: - /* Swap back so that points remains unchanged - */ - *i0 = *begin; - *begin = tmp; - } + if( res == HUGE_VAL ) + { + res = ( p - pProj ).norm( ); + } - if( res == HUGE_VAL ) + *bestDist = min( *bestDist, res ); + } + catch( const NonLocalExit::CrossDirectionOfParallel & ball ) { - res = ( p - pProj ).norm( ); + /* The triangle is collapsed to a line segment (or just a point). + * The point cannot be strictly inside the triangle in this case. + */ + throw Exceptions::InternalError( "Unprepared to handle collapsed triangle here. This algorithm should be re-implemented using the robust polytope distance computation." ); } - - *bestDist = min( *bestDist, res ); } -- 2.11.4.GIT