haskal
/
kicad
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

kimath: fix MTV and actual clearance calculation for SHAPE_COMPOUNDs

This commit is contained in:
Tomasz Wlostowski 2020-07-22 22:48:50 +02:00
parent 8d9d3ff795
commit 747f2eb948
2 changed files with 32 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libs/kimath/src/geometry/shape_collisions.cpp
View File

@ -448,7 +448,7 @@ inline bool CollCaseReversed ( const SHAPE* aA, const SHAPE* aB, int aClearance,
static bool collideSingleShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual, VECTOR2I* aMTV ) static bool collideSingleShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual, VECTOR2I* aMTV )
{ {
switch( aA->Type() ) switch( aA->Type() )
{ {
@ -623,27 +623,30 @@ static bool collideSingleShapes( const SHAPE* aA, const SHAPE* aB, int aClearanc
static bool collideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual, VECTOR2I* aMTV ) static bool collideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual, VECTOR2I* aMTV )
{ {
int currentActual = std::numeric_limits<int>::max(); int currentActual = std::numeric_limits<int>::max();
VECTOR2I currentMTV; VECTOR2I currentMTV(0, 0);
bool colliding = false; bool colliding = false;
bool exitOnFirstCollision = aActual != nullptr || aMTV != nullptr; bool exitOnFirstCollision = aActual == nullptr && aMTV == nullptr;
auto collideCompoundSubshapes = [&] ( const SHAPE* elemA, const SHAPE* elemB, int clearance, int* actual, VECTOR2I* mtv ) -> bool auto collideCompoundSubshapes = [&] ( const SHAPE* elemA, const SHAPE* elemB, int clearance ) -> bool
{ {
int actual;
VECTOR2I mtv;
bool c = collideSingleShapes( elemA, elemB, bool c = collideSingleShapes( elemA, elemB,
clearance, clearance,
actual ? &currentActual : nullptr, aActual ? &actual : nullptr,
mtv ? &currentMTV : nullptr ); aMTV ? &mtv : nullptr );
if(c) if(c)
{ {
if (actual) if (aActual)
{ {
*actual = std::min( *actual, currentActual ); currentActual = std::min( actual, currentActual );
} }
if( mtv ) if( aMTV )
{ {
if( currentMTV.SquaredEuclideanNorm() > mtv->SquaredEuclideanNorm() ) if( mtv.SquaredEuclideanNorm() > currentMTV.SquaredEuclideanNorm() )
*mtv = currentMTV; currentMTV = mtv;
} }
} }
@ -654,57 +657,62 @@ static bool collideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int
{ {
auto cmpA = static_cast<const SHAPE_COMPOUND*>( aA ); auto cmpA = static_cast<const SHAPE_COMPOUND*>( aA );
auto cmpB = static_cast<const SHAPE_COMPOUND*>( aB ); auto cmpB = static_cast<const SHAPE_COMPOUND*>( aB );
for( auto elemA : cmpA->Shapes() ) for( auto elemA : cmpA->Shapes() )
{ {
for( auto elemB : cmpB->Shapes() ) for( auto elemB : cmpB->Shapes() )
{ {
if( collideCompoundSubshapes( elemA, elemB, aClearance, aActual, aMTV ) ) if( collideCompoundSubshapes( elemA, elemB, aClearance ) )
{ {
colliding = true; colliding = true;
if ( exitOnFirstCollision ) if ( exitOnFirstCollision )
return true; break;
} }
} }
if( colliding && exitOnFirstCollision )
break;
} }
return colliding;
} }
else if ( aA->Type() == SH_COMPOUND ) else if ( aA->Type() == SH_COMPOUND )
{ {
auto cmpA = static_cast<const SHAPE_COMPOUND*>( aA ); auto cmpA = static_cast<const SHAPE_COMPOUND*>( aA );
for( auto elemA : cmpA->Shapes() ) for( auto elemA : cmpA->Shapes() )
{ {
if( collideCompoundSubshapes( elemA, aB, aClearance, aActual, aMTV ) ) if( collideCompoundSubshapes( elemA, aB, aClearance ) )
{ {
colliding = true; colliding = true;
if ( exitOnFirstCollision ) if ( exitOnFirstCollision )
return true; break;
} }
} }
return colliding;
} }
else if ( aB->Type() == SH_COMPOUND ) else if ( aB->Type() == SH_COMPOUND )
{ {
auto cmpB = static_cast<const SHAPE_COMPOUND*>( aB ); auto cmpB = static_cast<const SHAPE_COMPOUND*>( aB );
for( auto elemB : cmpB->Shapes() ) for( auto elemB : cmpB->Shapes() )
{ {
if( collideCompoundSubshapes( aA, elemB, aClearance, aActual, aMTV ) ) if( collideCompoundSubshapes( aA, elemB, aClearance ) )
{ {
colliding = true; colliding = true;
if ( exitOnFirstCollision ) if ( exitOnFirstCollision )
return true; break;
} }
} }
return colliding;
} }
else else
{ {
return collideSingleShapes( aA, aB, aClearance, aActual, aMTV ); return collideSingleShapes( aA, aB, aClearance, aActual, aMTV );
} }
if( colliding )
{
if( aActual )
*aActual = currentActual;
if( aMTV )
*aMTV = currentMTV;
}
return colliding;
} }
bool SHAPE::Collide( const SHAPE* aShape, int aClearance, VECTOR2I* aMTV ) const bool SHAPE::Collide( const SHAPE* aShape, int aClearance, VECTOR2I* aMTV ) const

2
libs/kimath/src/geometry/shape_compound.cpp
View File

@ -97,7 +97,7 @@ bool SHAPE_COMPOUND::Collide( const SEG& aSeg, int aClearance, int* aActual ) co
{ {
for( auto& item : m_shapes ) for( auto& item : m_shapes )
{ {
if( item->Collide( aSeg, aClearance ) ) if( item->Collide( aSeg, aClearance, aActual ) )
return true; return true;
} }