Skip to content

K
kicad-source-mirror
Commit cce6e270 authored by Maciej Suminski's avatar Maciej Suminski
Browse files
Options

Various fixes to geometry & math library.

parent d27ea789
Hide whitespace changes
Inline Side-by-side
Showing with 299 additions and 141 deletions
common/geometry/shape_collisions.cpp
View file @ cce6e270
......@@ -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,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 )
if( !aNeedMTV && inside )
return true;
for( int i = 0; i < 4; i++ )
{
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 )
{
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;
}
}
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<const SHAPE_RECT*>( aA ),
*static_cast<const SHAPE_CIRCLE*>( aB ), aClearance, aNeedMTV, aMTV );
return aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2 );
}
case SH_LINE_CHAIN:
return Collide( *static_cast<const SHAPE_RECT*>( aA ),
*static_cast<const SHAPE_LINE_CHAIN*>( 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<const SHAPE_RECT*>( aB ),
*static_cast<const SHAPE_CIRCLE*>( aA ), aClearance, aNeedMTV, aMTV );
case SH_CIRCLE:
return Collide( *static_cast<const SHAPE_CIRCLE*>( aA ),
*static_cast<const SHAPE_CIRCLE*>( 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<const SHAPE_CIRCLE*>( aA ),
*static_cast<const SHAPE_LINE_CHAIN *>( aB ), aClearance, aNeedMTV, aMTV );
return false;
}
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 );
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);
}
case SH_CIRCLE:
return Collide( *static_cast<const SHAPE_CIRCLE*>( aB ),
*static_cast<const SHAPE_LINE_CHAIN*>( 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<SHAPE_RECT, SHAPE_CIRCLE>( aA, aB, 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 );
case SH_LINE_CHAIN:
return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
default:
break;
}
break;
case SH_SEGMENT:
return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
default:
break;
}
default:
break;
}
assert( 0 ); // unsupported_collision
case SH_CIRCLE:
switch( aB->Type() )
{
case SH_RECT:
return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aB, aA, aClearance, aNeedMTV, aMTV );
return false;
case SH_CIRCLE:
return CollCase<SHAPE_CIRCLE, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );
case SH_LINE_CHAIN:
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;
}
case SH_LINE_CHAIN:
switch( aB->Type() )
{
case SH_RECT:
return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );
case SH_CIRCLE:
return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );
case SH_LINE_CHAIN:
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;
}
bool unsupported_collision = true;
assert( unsupported_collision == false );
return false;
}
......
common/geometry/shape_line_chain.cpp
View file @ cce6e270
......@@ -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 );
}
include/geometry/seg.h
View file @ cce6e270
......@@ -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
View file @ cce6e270
......@@ -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
View file @ cce6e270
......@@ -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;
......
include/geometry/shape_rect.h
View file @ cce6e270
......@@ -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++ )
{
......
include/math/math_util.h
View file @ cce6e270
......@@ -40,6 +40,11 @@ T rescale( T aNumerator, T aValue, T aDenominator )
return aNumerator * aValue / aDenominator;
}
template <typename T>
int sign( T val )
{
return ( T( 0 ) < val) - ( val < T( 0 ) );
}
// explicit specializations for integer types, taking care of overflow.
template <>
......
include/math/vector2d.h
View file @ cce6e270
......@@ -28,6 +28,7 @@
#ifndef VECTOR2D_H_
#define VECTOR2D_H_
#include <cmath>
#include <climits>
#include <iostream>
#include <sstream>
......@@ -376,7 +377,7 @@ VECTOR2<T> VECTOR2<T>::Resize( T aNewLength ) const
return VECTOR2<T> (
( 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 );
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment