diff --git a/common/geometry/shape_collisions.cpp b/common/geometry/shape_collisions.cpp index 604335b43e..52db4ecb81 100644 --- a/common/geometry/shape_collisions.cpp +++ b/common/geometry/shape_collisions.cpp @@ -23,11 +23,13 @@ */ #include +#include #include #include #include #include +#include typedef VECTOR2I::extended_type ecoord; @@ -45,25 +47,21 @@ static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_CIRCLE& aB, int return false; if( aNeedMTV ) - aMTV = delta.Resize( sqrt( abs( min_dist_sq - dist_sq ) ) + 1 ); + aMTV = delta.Resize( min_dist - sqrt( dist_sq ) + 3 ); // fixme: apparent rounding error return true; } -static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance, - bool aNeedMTV, VECTOR2I& aMTV ) +static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance, + bool aNeedMTV, VECTOR2I& aMTV ) { const VECTOR2I c = aB.GetCenter(); const VECTOR2I p0 = aA.GetPosition(); const VECTOR2I size = aA.GetSize(); - const ecoord r = aB.GetRadius(); - const ecoord min_dist = aClearance + r; - const ecoord min_dist_sq = min_dist * min_dist; - - if( aA.BBox( 0 ).Contains( c ) ) - return true; - + const int r = aB.GetRadius(); + const int min_dist = aClearance + r; + const VECTOR2I vts[] = { VECTOR2I( p0.x, p0.y ), @@ -73,33 +71,35 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int a VECTOR2I( p0.x, p0.y ) }; - ecoord nearest_seg_dist_sq = VECTOR2I::ECOORD_MAX; + int nearest_seg_dist = INT_MAX; VECTOR2I nearest; bool inside = c.x >= p0.x && c.x <= ( p0.x + size.x ) && c.y >= p0.y && c.y <= ( p0.y + size.y ); - if( !inside ) - { - for( int i = 0; i < 4; i++ ) - { - const SEG seg( vts[i], vts[i + 1] ); - ecoord dist_sq = seg.SquaredDistance( c ); - if( dist_sq < min_dist_sq ) - { - if( !aNeedMTV ) - return true; - else - { - nearest = seg.NearestPoint( c ); - nearest_seg_dist_sq = dist_sq; - } - } + if( !aNeedMTV && inside ) + return true; + + for( int i = 0; i < 4; i++ ) + { + const SEG seg( vts[i], vts[i + 1] ); + + VECTOR2I pn = seg.NearestPoint( c ); + + int d = ( pn - c ).EuclideanNorm(); + + if( ( d < min_dist ) && !aNeedMTV ) + return true; + + if( d < nearest_seg_dist ) + { + nearest = pn; + nearest_seg_dist = d; } } - if( nearest_seg_dist_sq >= min_dist_sq && !inside ) + if( nearest_seg_dist >= min_dist && !inside ) return false; VECTOR2I delta = c - nearest; @@ -107,25 +107,77 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int a if( !aNeedMTV ) return true; + if( inside ) - aMTV = -delta.Resize( sqrt( abs( r * r + nearest_seg_dist_sq ) + 1 ) ); + aMTV = -delta.Resize( abs( min_dist + 1 + nearest_seg_dist ) + 1 ); else - aMTV = delta.Resize( sqrt( abs( r * r - nearest_seg_dist_sq ) + 1 ) ); + aMTV = delta.Resize( abs( min_dist + 1 - nearest_seg_dist ) + 1 ); + return true; } +static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance ) +{ + VECTOR2I nearest = aB.NearestPoint( aA.GetCenter() ); + VECTOR2I f (0, 0); + + int dist = ( nearest - aA.GetCenter() ).EuclideanNorm(); + int min_dist = aClearance + aA.GetRadius(); + + if( dist < min_dist ) + f = ( aA.GetCenter() - nearest ).Resize ( min_dist - dist + 10 ); + + return f; +} + + static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_LINE_CHAIN& aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV ) { + bool found = false; + VECTOR2I::extended_type clSq = (VECTOR2I::extended_type) aClearance * aClearance; + + for( int s = 0; s < aB.SegmentCount(); s++ ) { + if( aA.Collide( aB.CSegment( s ), aClearance ) ) - return true; + { + found = true; + break; + } } - return false; + if( !aNeedMTV || !found ) + return found; + + SHAPE_CIRCLE cmoved( aA ); + VECTOR2I f_total( 0, 0 ); + + for( int s = 0; s < aB.SegmentCount(); s++ ) + { + VECTOR2I f = pushoutForce( cmoved, aB.CSegment( s ), aClearance ); + cmoved.SetCenter( cmoved.GetCenter() + f ); + f_total += f; + } + + aMTV = f_total; + return found; +} + + +static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_SEGMENT& aSeg, int aClearance, + bool aNeedMTV, VECTOR2I& aMTV ) +{ + bool col = aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2); + + if( col && aNeedMTV ) + { + aMTV = pushoutForce( aA, aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2); + } + return col; } @@ -155,74 +207,123 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_LINE_CHAIN& aB, in } -bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, - bool aNeedMTV, VECTOR2I& aMTV ) +static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_SEGMENT& aSeg, int aClearance, + bool aNeedMTV, VECTOR2I& aMTV ) { - switch( aA->Type() ) - { - case SH_RECT: - switch( aB->Type() ) - { - case SH_CIRCLE: - return Collide( *static_cast( aA ), - *static_cast( aB ), aClearance, aNeedMTV, aMTV ); + return aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2 ); +} - case SH_LINE_CHAIN: - return Collide( *static_cast( aA ), - *static_cast( aB ), aClearance, aNeedMTV, aMTV ); - default: - break; - } - break; +static inline bool Collide( const SHAPE_SEGMENT& aA, const SHAPE_SEGMENT& aB, int aClearance, + bool aNeedMTV, VECTOR2I& aMTV ) +{ + return aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2 ); +} - case SH_CIRCLE: - switch( aB->Type() ) - { - case SH_RECT: - return Collide( *static_cast( aB ), - *static_cast( aA ), aClearance, aNeedMTV, aMTV ); - case SH_CIRCLE: - return Collide( *static_cast( aA ), - *static_cast( aB ), aClearance, aNeedMTV, aMTV ); +static inline bool Collide( const SHAPE_LINE_CHAIN& aA, const SHAPE_SEGMENT& aB, int aClearance, + bool aNeedMTV, VECTOR2I& aMTV ) +{ + if( aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2 ) ) + return true; - case SH_LINE_CHAIN: - return Collide( *static_cast( aA ), - *static_cast( aB ), aClearance, aNeedMTV, aMTV ); + return false; +} - default: - break; - } - break; - case SH_LINE_CHAIN: - switch( aB->Type() ) - { - case SH_RECT: - return Collide( *static_cast( aB ), - *static_cast( aA ), aClearance, aNeedMTV, aMTV ); +template bool +CollCase( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV ) +{ + return Collide (*static_cast( aA ), + *static_cast( aB ), + aClearance, aNeedMTV, aMTV); +} - case SH_CIRCLE: - return Collide( *static_cast( aB ), - *static_cast( aA ), aClearance, aNeedMTV, aMTV ); +bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV ) +{ + switch( aA->Type() ) + { + case SH_RECT: + switch( aB->Type() ) + { + case SH_CIRCLE: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); - case SH_LINE_CHAIN: - return Collide( *static_cast( aA ), - *static_cast( aB ), aClearance, aNeedMTV, aMTV ); + case SH_LINE_CHAIN: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); - default: - break; - } - break; + case SH_SEGMENT: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); - default: - break; - } + default: + break; + } - assert( 0 ); // unsupported_collision + case SH_CIRCLE: + switch( aB->Type() ) + { + case SH_RECT: + return CollCase( aB, aA, aClearance, aNeedMTV, aMTV ); - return false; + case SH_CIRCLE: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); + + case SH_LINE_CHAIN: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); + + case SH_SEGMENT: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); + default: + break; + } + + case SH_LINE_CHAIN: + switch( aB->Type() ) + { + case SH_RECT: + return CollCase( aB, aA, aClearance, aNeedMTV, aMTV ); + + case SH_CIRCLE: + return CollCase( aB, aA, aClearance, aNeedMTV, aMTV ); + + case SH_LINE_CHAIN: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); + + case SH_SEGMENT: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); + + default: + break; + } + + case SH_SEGMENT: + switch( aB->Type() ) + { + case SH_RECT: + return CollCase( aB, aA, aClearance, aNeedMTV, aMTV ); + + case SH_CIRCLE: + return CollCase( aB, aA, aClearance, aNeedMTV, aMTV ); + + case SH_LINE_CHAIN: + return CollCase( aB, aA, aClearance, aNeedMTV, aMTV ); + + case SH_SEGMENT: + return CollCase( aA, aB, aClearance, aNeedMTV, aMTV ); + + default: + break; + } + + default: + break; + } + + bool unsupported_collision = true; + + assert( unsupported_collision == false ); + + return false; } diff --git a/common/geometry/shape_line_chain.cpp b/common/geometry/shape_line_chain.cpp index 7ac8c10eae..e442fcd503 100644 --- a/common/geometry/shape_line_chain.cpp +++ b/common/geometry/shape_line_chain.cpp @@ -135,6 +135,9 @@ int SHAPE_LINE_CHAIN::Distance( const VECTOR2I& aP ) const { int d = INT_MAX; + if( IsClosed() && PointInside( aP ) ) + return 0; + for( int s = 0; s < SegmentCount(); s++ ) d = std::min( d, CSegment( s ).Distance( aP ) ); @@ -179,7 +182,7 @@ int SHAPE_LINE_CHAIN::Split( const VECTOR2I& aP ) int SHAPE_LINE_CHAIN::Find( const VECTOR2I& aP ) const { - for( int s = 0; s< PointCount(); s++ ) + for( int s = 0; s < PointCount(); s++ ) if( CPoint( s ) == aP ) return s; @@ -187,6 +190,16 @@ int SHAPE_LINE_CHAIN::Find( const VECTOR2I& aP ) const } +int SHAPE_LINE_CHAIN::FindSegment( const VECTOR2I& aP ) const +{ + for( int s = 0; s < SegmentCount(); s++ ) + if( CSegment( s ).Distance( aP ) <= 1 ) + return s; + + return -1; +} + + const SHAPE_LINE_CHAIN SHAPE_LINE_CHAIN::Slice( int aStartIndex, int aEndIndex ) const { SHAPE_LINE_CHAIN rv; @@ -261,6 +274,9 @@ int SHAPE_LINE_CHAIN::Intersect( const SHAPE_LINE_CHAIN& aChain, INTERSECTIONS& if( a.Collinear( b ) ) { + is.our = a; + is.their = b; + if( a.Contains( b.A ) ) { is.p = b.A; aIp.push_back( is ); } if( a.Contains( b.B ) ) { is.p = b.B; aIp.push_back( is ); } if( b.Contains( a.A ) ) { is.p = a.A; aIp.push_back( is ); } @@ -282,44 +298,6 @@ int SHAPE_LINE_CHAIN::Intersect( const SHAPE_LINE_CHAIN& aChain, INTERSECTIONS& } return aIp.size(); - - for( int s1 = 0; s1 < SegmentCount(); s1++ ) - { - for( int s2 = 0; s2 < aChain.SegmentCount(); s2++ ) - { - const SEG& a = CSegment( s1 ); - const SEG& b = aChain.CSegment( s2 ); - OPT_VECTOR2I p = a.Intersect( b ); - INTERSECTION is; - - if( p ) - { - is.p = *p; - is.our = a; - is.their = b; - aIp.push_back( is ); - } - else if( a.Collinear( b ) ) - { - if( a.A != b.A && a.A != b.B && b.Contains( a.A ) ) - { - is.p = a.A; - is.our = a; - is.their = b; - aIp.push_back( is ); - } - else if( a.B != b.A && a.B != b.B && b.Contains( a.B ) ) - { - is.p = a.B; - is.our = a; - is.their = b; - aIp.push_back( is ); - } - } - } - } - - return aIp.size(); } @@ -372,10 +350,13 @@ bool SHAPE_LINE_CHAIN::PointInside( const VECTOR2I& aP ) const bool SHAPE_LINE_CHAIN::PointOnEdge( const VECTOR2I& aP ) const { - if( SegmentCount() < 1 ) + if( !PointCount() ) + return false; + + else if( PointCount() == 1 ) return m_points[0] == aP; - for( int i = 1; i < SegmentCount(); i++ ) + for( int i = 0; i < SegmentCount(); i++ ) { const SEG s = CSegment( i ); @@ -534,3 +515,30 @@ const std::string SHAPE_LINE_CHAIN::Format() const return ss.str(); } + + +bool SHAPE_LINE_CHAIN::CompareGeometry ( const SHAPE_LINE_CHAIN & aOther ) const +{ + SHAPE_LINE_CHAIN a(*this), b(aOther); + a.Simplify(); + b.Simplify(); + + if(a.m_points.size() != b.m_points.size()) + return false; + + for(int i = 0; i < a.PointCount(); i++) + if(a.CPoint(i) != b.CPoint(i)) + return false; + return true; +} + +bool SHAPE_LINE_CHAIN::Intersects( const SHAPE_LINE_CHAIN& aChain ) const +{ + INTERSECTIONS dummy; + return Intersect(aChain, dummy) != 0; +} + +SHAPE* SHAPE_LINE_CHAIN::Clone() const +{ + return new SHAPE_LINE_CHAIN( *this ); +} diff --git a/include/geometry/seg.h b/include/geometry/seg.h index cf5b8d50c9..7e789f9d21 100644 --- a/include/geometry/seg.h +++ b/include/geometry/seg.h @@ -244,14 +244,14 @@ public: */ bool Collinear( const SEG& aSeg ) const { - ecoord qa = A.y - B.y; - ecoord qb = B.x - A.x; - ecoord qc = -qa * A.x - qb * A.y; + ecoord qa1 = A.y - B.y; + ecoord qb1 = B.x - A.x; + ecoord qc1 = -qa1 * A.x - qb1 * A.y; + ecoord qa2 = aSeg.A.y - aSeg.B.y; + ecoord qb2 = aSeg.B.x - aSeg.A.x; + ecoord qc2 = -qa2 * aSeg.A.x - qb2 * aSeg.A.y; - ecoord d1 = std::abs( aSeg.A.x * qa + aSeg.A.y * qb + qc ); - ecoord d2 = std::abs( aSeg.B.x * qa + aSeg.B.y * qb + qc ); - - return ( d1 <= 1 && d2 <= 1 ); + return ( qa1 == qa2 ) && ( qb1 == qb2 ) && ( qc1 == qc2 ); } /** diff --git a/include/geometry/shape_circle.h b/include/geometry/shape_circle.h index a11cd4da8f..e4b668b68c 100644 --- a/include/geometry/shape_circle.h +++ b/include/geometry/shape_circle.h @@ -38,9 +38,20 @@ public: SHAPE( SH_CIRCLE ), m_radius( aRadius ), m_center( aCenter ) {} + SHAPE_CIRCLE ( const SHAPE_CIRCLE& aOther ) : + SHAPE( SH_CIRCLE ), + m_radius( aOther.m_radius ), + m_center( aOther.m_center ) + {}; + ~SHAPE_CIRCLE() {} + SHAPE* Clone() const + { + return new SHAPE_CIRCLE( *this ); + } + const BOX2I BBox( int aClearance = 0 ) const { const VECTOR2I rc( m_radius + aClearance, m_radius + aClearance ); diff --git a/include/geometry/shape_line_chain.h b/include/geometry/shape_line_chain.h index da6494e91a..b91e6a566c 100644 --- a/include/geometry/shape_line_chain.h +++ b/include/geometry/shape_line_chain.h @@ -116,6 +116,8 @@ public: ~SHAPE_LINE_CHAIN() {} + SHAPE *Clone() const; + /** * Function Clear() * Removes all points from the line chain. @@ -366,6 +368,11 @@ public: } } + void Insert( int aVertex, const VECTOR2I& aP ) + { + m_points.insert( m_points.begin() + aVertex, aP ); + } + /** * Function Replace() * @@ -417,6 +424,15 @@ public: */ int Find( const VECTOR2I& aP ) const; + /** + * Function FindSegment() + * + * Searches for segment containing point aP. + * @param aP the point to be looked for + * @return index of the correspoinding segment in the line chain or negative when not found. + */ + int FindSegment( const VECTOR2I& aP ) const; + /** * Function Slice() * @@ -441,6 +457,8 @@ public: VECTOR2I m_origin; }; + bool Intersects( const SHAPE_LINE_CHAIN& aChain ) const; + /** * Function Intersect() * @@ -533,6 +551,8 @@ public: return false; } + bool CompareGeometry( const SHAPE_LINE_CHAIN & aOther ) const; + private: /// array of vertices std::vector m_points; diff --git a/include/geometry/shape_rect.h b/include/geometry/shape_rect.h index d2c5f22a33..bc3ba44d17 100644 --- a/include/geometry/shape_rect.h +++ b/include/geometry/shape_rect.h @@ -53,10 +53,22 @@ public: * Constructor * Creates a rectangle defined by top-left corner aP0, width aW and height aH. */ - SHAPE_RECT( const VECTOR2I& aP0, int aW, int aH ) : + SHAPE_RECT( const VECTOR2I& aP0, int aW, int aH ) : SHAPE( SH_RECT ), m_p0( aP0 ), m_w( aW ), m_h( aH ) {} + SHAPE_RECT ( const SHAPE_RECT& aOther ) : + SHAPE( SH_RECT ), + m_p0( aOther.m_p0 ), + m_w( aOther.m_w ), + m_h( aOther.m_h ) + {}; + + SHAPE* Clone() const + { + return new SHAPE_RECT( *this ); + } + /// @copydoc SHAPE::BBox() const BOX2I BBox( int aClearance = 0 ) const { @@ -90,11 +102,11 @@ public: if( BBox( 0 ).Contains( aSeg.A ) || BBox( 0 ).Contains( aSeg.B ) ) return true; - VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ), - VECTOR2I( m_p0.x, m_p0.y + m_h ), - VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ), - VECTOR2I( m_p0.x + m_w, m_p0.y ), - VECTOR2I( m_p0.x, m_p0.y ) }; + VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ), + VECTOR2I( m_p0.x, m_p0.y + m_h ), + VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ), + VECTOR2I( m_p0.x + m_w, m_p0.y ), + VECTOR2I( m_p0.x, m_p0.y ) }; for( int i = 0; i < 4; i++ ) { diff --git a/include/math/math_util.h b/include/math/math_util.h index 812b52fba8..cb3ce50460 100644 --- a/include/math/math_util.h +++ b/include/math/math_util.h @@ -40,6 +40,11 @@ T rescale( T aNumerator, T aValue, T aDenominator ) return aNumerator * aValue / aDenominator; } +template +int sign( T val ) +{ + return ( T( 0 ) < val) - ( val < T( 0 ) ); +} // explicit specializations for integer types, taking care of overflow. template <> diff --git a/include/math/vector2d.h b/include/math/vector2d.h index feee5ff060..5bb43e5872 100644 --- a/include/math/vector2d.h +++ b/include/math/vector2d.h @@ -28,6 +28,7 @@ #ifndef VECTOR2D_H_ #define VECTOR2D_H_ +#include #include #include #include @@ -376,7 +377,7 @@ VECTOR2 VECTOR2::Resize( T aNewLength ) const return VECTOR2 ( ( x < 0 ? -1 : 1 ) * sqrt( rescale( l_sq_new, (extended_type) x * x, l_sq_current ) ), - ( y < 0 ? -1 : 1 ) * sqrt( rescale( l_sq_new, (extended_type) y * y, l_sq_current ) ) ); + ( y < 0 ? -1 : 1 ) * sqrt( rescale( l_sq_new, (extended_type) y * y, l_sq_current ) ) ) * sign( aNewLength ); }