Various fixes to geometry & math library. (cce6e270) · Commits · Elphel / kicad-source-mirror

common/geometry/shape_collisions.cpp

+186 −85

Original line number	Diff line number	Diff line
		@@ -23,11 +23,13 @@
		*/

		#include <math/vector2d.h>
		#include <math.h>

		#include <geometry/shape.h>
		#include <geometry/shape_line_chain.h>
		#include <geometry/shape_circle.h>
		#include <geometry/shape_rect.h>
		#include <geometry/shape_segment.h>

		typedef VECTOR2I::extended_type ecoord;

		@@ -45,7 +47,7 @@ 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;
		}
		@@ -57,12 +59,8 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int a
		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[] =
		{
		@@ -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 )
		{

		if( !aNeedMTV && inside )
		return true;

		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 )
		VECTOR2I pn = seg.NearestPoint( c );

		int d = ( pn - c ).EuclideanNorm();

		if( ( d < min_dist ) && !aNeedMTV )
		return true;
		else

		if( d < nearest_seg_dist )
		{
		nearest = seg.NearestPoint( c );
		nearest_seg_dist_sq = dist_sq;
		}
		}
		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,8 +207,39 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_LINE_CHAIN& aB, in
		}


		bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance,
		static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_SEGMENT& aSeg, int aClearance,
		bool aNeedMTV, VECTOR2I& aMTV )
		{
		return aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2 );
		}


		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 );
		}


		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;

		return false;
		}


		template<class ShapeAType, class ShapeBType> bool
		CollCase( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
		{
		return Collide (static_cast<const ShapeAType>( aA ),
		static_cast<const ShapeBType>( aB ),
		aClearance, aNeedMTV, aMTV);
		}

		bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
		{
		switch( aA->Type() )
		{
		@@ -164,63 +247,81 @@ bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance,
		switch( aB->Type() )
		{
		case SH_CIRCLE:
		return Collide( static_cast<const SHAPE_RECT>( aA ),
		static_cast<const SHAPE_CIRCLE>( aB ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );

		case SH_LINE_CHAIN:
		return Collide( static_cast<const SHAPE_RECT>( aA ),
		static_cast<const SHAPE_LINE_CHAIN>( aB ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );

		case SH_SEGMENT:
		return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );

		default:
		break;
		}
		break;

		case SH_CIRCLE:
		switch( aB->Type() )
		{
		case SH_RECT:
		return Collide( static_cast<const SHAPE_RECT>( aB ),
		static_cast<const SHAPE_CIRCLE>( aA ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aB, aA, aClearance, aNeedMTV, aMTV );

		case SH_CIRCLE:
		return Collide( static_cast<const SHAPE_CIRCLE>( aA ),
		static_cast<const SHAPE_CIRCLE>( aB ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_CIRCLE, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );

		case SH_LINE_CHAIN:
		return Collide( static_cast<const SHAPE_CIRCLE>( aA ),
		static_cast<const SHAPE_LINE_CHAIN >( aB ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );

		case SH_SEGMENT:
		return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
		default:
		break;
		}
		break;

		case SH_LINE_CHAIN:
		switch( aB->Type() )
		{
		case SH_RECT:
		return Collide( static_cast<const SHAPE_RECT>( aB ),
		static_cast<const SHAPE_LINE_CHAIN>( aA ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );

		case SH_CIRCLE:
		return Collide( static_cast<const SHAPE_CIRCLE>( aB ),
		static_cast<const SHAPE_LINE_CHAIN>( aA ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );

		case SH_LINE_CHAIN:
		return Collide( static_cast<const SHAPE_LINE_CHAIN>( aA ),
		static_cast<const SHAPE_LINE_CHAIN>( aB ), aClearance, aNeedMTV, aMTV );
		return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );

		case SH_SEGMENT:
		return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );

		default:
		break;
		}

		case SH_SEGMENT:
		switch( aB->Type() )
		{
		case SH_RECT:
		return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );

		case SH_CIRCLE:
		return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );

		case SH_LINE_CHAIN:
		return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );

		case SH_SEGMENT:
		return CollCase<SHAPE_SEGMENT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );

		default:
		break;
		}

		default:
		break;
		}

		assert( 0 ); // unsupported_collision
		bool unsupported_collision = true;

		assert( unsupported_collision == false );

		return false;
		}

common/geometry/shape_line_chain.cpp

+49 −41

Original line number	Diff line number	Diff line
		@@ -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 ) );

		@@ -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 );
		}

include/geometry/seg.h

+8 −8

Original line number	Diff line number	Diff line
		@@ -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 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 );
		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;

		return ( qa1 == qa2 ) && ( qb1 == qb2 ) && ( qc1 == qc2 );
		}

		/**

include/geometry/shape_circle.h

+11 −0

Original line number	Diff line number	Diff line
		@@ -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 );

include/geometry/shape_line_chain.h

+20 −0

Original line number	Diff line number	Diff line
		@@ -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<VECTOR2I> m_points;

Admin message