Collision * PolygonVsPolygon(Poly * A , Poly * B)
{
int edgesA = A->size; // Pobieram iloœæ wierzcho³ków
int edgesB = B->size;
float minPenetration = INT_MAX; // Zmienna na minimaln¹ penetracje
float Penetration; // Zmienna na aktualn¹ penetracje
Vec2 normal ; // Poszukiwana normalna
Vec2 axis; // Potencjalna oœ separacji
float minA = 0; float maxA = 0; // Zmienne na rzuty obu wielok¹tów
float maxB = 0; float minB = 0;
for(int i = 0; i < edgesA + edgesB; i++) // Iteruje po wszystkich krawêdziach
{
if (i < edgesA)
{
axis = A->GlobNorm[i]; // Pobieramy normaln¹
}
else
{
axis = B->GlobNorm[i - edgesA];
}
ProjectPolygon(axis, A, minA, maxA); // Rzutujemy oba wielok¹ty na oœ
ProjectPolygon(axis, B, minB, maxB);
Penetration = ProjectionDistance(minA, maxA, minB, maxB); // Liczy odleg³oœæ projekcji
if ( Penetration > 0) // Jeœli odleg³oœæ wiêksza od zera nie ma kolizji
return NULL; // Wystarczy jedna oœ separacji
Penetration = -Penetration; // Zmieniam znak penetracji
// Teraz sprawdzam czy jest to najmniejsza mo¿liwa penetracja
if (Penetration < minPenetration)
{
minPenetration = Penetration;
normal = axis;
if ( Scalar(Vector( B->center , A->center ),normal) > 0) // Sprawdzam czy normalna ma dobry zwrot
normal = normal * -1;
}
}
// Skoro znalaz³em ju¿ wszystkie dane uzpe³niam dane o kolizji
Collision * c = new Collision;
c->contacts_num = 0;
// Szukam kolizji wierzcho³ków
//Kolizje wierzcho³ek B wielok¹t A
for(int b=0; b < B->size;b++)
{
if( A->IsPointInside(B->GlobVert[b] ) )
{
if( c->contacts_num != 2 )
{
c->contacts_num++;
c->contacts[c->contacts_num-1] = B->GlobVert[b];
}
else
break;
}
}
//Kolizje wierzcho³ek A wielok¹t B
if( c->contacts_num != 2 )
for(int a=0; a < A->size;a++)
{
if( B->IsPointInside(A->GlobVert[a] ) )
{
if( c->contacts_num != 2 )
{
c->contacts_num++;
c->contacts[c->contacts_num-1] = A->GlobVert[a];
}
else
break;
}
}
c->A = A->body;
c->B = B->body;
c->normal = normal;
c->penetration = minPenetration;
return c;
}
Collision * PolygonVsCircle(Poly * A , Circle * B )
{
int edges = A->size; // Pobieramy iloœæ wierzcho³ków
float minPenetration = INT_MAX; // Zmienna na minimaln¹ penetracje
float Penetration; // Zmienna na aktualn¹ penetracje
Vec2 normal ; // Poszukiwana normalna
Vec2 axis; // Potencjalna oœ separacji
float minA = 0; float maxA = 0; // Zmienna na rzuty obu wielok¹tów
float maxB = 0; float minB = 0;
// Poszukujemy osi sepracji wzglêdem wszystkich boków
for(int i=0 ; i<edges ; i++)
{
ProjectPolygon(A->GlobNorm[i] , A , minA, maxA); // Rzutuje figury
ProjectCircle (A->GlobNorm[i] , B , minB, maxB);
Penetration = ProjectionDistance(minA, maxA, minB, maxB); // Pobieram odleg³oœæ projekcji
if (Penetration > 0) // Jeœli odleg³oœæ jest wiêksza od zera cia³a nie koliduj¹
return NULL;
Penetration = -Penetration; // Zmieniam znak
if (Penetration < minPenetration) // Sprawdzam czy jest to minimalna penetracja
{
minPenetration = Penetration; // Zapamiêtuje dane
normal = A->GlobNorm[i];
if ( Scalar( Vector(B->center , A->center ) , normal ) > 0) // Sprawdzam zwrot normalnej
normal = normal * -1;
}
}
// Szukamy najbli¿szego wierzcho³ka
int ClosestVert;
float MinDist = FLT_MAX;
for(int i=0; i <A->size ; i++)
{
if( (B->center - A->GlobVert[i]).square_value() < MinDist )
{
ClosestVert = i;
MinDist = (B->center - A->GlobVert[i]).square_value();
}
}
// Sprawdzamy czy przez ten wierzcho³ek przechodzi oœ separacji
axis = B->center - A->GlobVert[ClosestVert];
axis.normalize();
ProjectPolygon(axis, A, minA, maxA); // Rzutuje figury
ProjectCircle(axis, B, minB, maxB);
Penetration = ProjectionDistance(minA, maxA, minB, maxB); // Pobieram odleg³oœæ projekcji
if (Penetration > 0) // Jeœli odleg³oœæ jest wiêksza od zera cia³a nie koliduj¹
return NULL;
Penetration = -Penetration; // Zmieniam znak
if (Penetration < minPenetration)
{
minPenetration = Penetration;
normal = axis;
if ( Scalar( Vector(B->center , A->center) , normal ) > 0) // Sprawdzam zwrot normalnej
normal = normal * -1;
}
// Uzupe³niamy dane o kolizji
Collision * c = new Collision;
c->A = A->body ;
c->B = B->body;
c->normal = normal;
c->penetration = minPenetration;
c->contacts[0] = B->center - normal * B->radius ;
c->contacts_num = 1;
return c;
}
bool Collisions::IsPolygonIntersectsPolygon(Polygon2 & poly1, Polygon2 & poly2)
{
//#define DEBUG_DRAW_INTERSECTIONS
Vector2 * points1 = poly1.GetPoints();
Vector2 * points2 = poly2.GetPoints();
separationAxes.clear();
for (int32 index1 = 0; index1 < poly1.pointCount; ++index1)
{
int32 index2 = (index1 + 1 != poly1.pointCount) ? (index1 + 1) : (0);
Vector2 line = points1[index2] - points1[index1];
Vector2 normal = Vector2(line.y, -line.x);
normal.Normalize();
AddSeparationAxis(normal);
#if defined(DEBUG_DRAW_INTERSECTIONS)
RenderManager::Instance()->SetColor(0.0f, 0.0f, 1.0f, 1.0f);
RenderHelper::DrawLine(points1[index1] + (line / 2), points1[index1] + (line / 2) + normal * 10);
#endif
}
for (int32 index1 = 0; index1 < poly2.pointCount; ++index1)
{
int32 index2 = (index1 + 1 != poly2.pointCount) ? (index1 + 1) : (0);
Vector2 line = points2[index2] - points2[index1];
Vector2 normal = Vector2(line.y, -line.x);
normal.Normalize();
AddSeparationAxis(normal);
#if defined(DEBUG_DRAW_INTERSECTIONS)
RenderManager::Instance()->SetColor(0.0f, 1.0f, 0.0f, 1.0f);
RenderHelper::DrawLine(points2[index1] + (line / 3), points2[index1] + (line / 3) + normal * 10);
#endif
}
size_t size = separationAxes.size();
#if defined(DEBUG_DRAW_INTERSECTIONS)
for (size_t index = 0; index < size; ++index)
{
Vector2 axis = separationAxes[index];
RenderManager::Instance()->SetColor(1.0f, 0.0f, 0.0f, 1.0f);
RenderHelper::DrawLine(Vector2(50.0f, 50.0f), Vector2(50.0f, 50.0f) + axis * 1000);
}
#endif
for (size_t index = 0; index < size; ++index)
{
Vector2 axis = separationAxes[index];
float32 p1Min, p1Max;
ProjectPolygon(axis, poly1, p1Min, p1Max);
float32 p2Min, p2Max;
ProjectPolygon(axis, poly2, p2Min, p2Max);
#if defined(DEBUG_DRAW_INTERSECTIONS)
RenderManager::Instance()->SetColor(0.0f, 1.0f, 1.0f, 1.0f);
Vector2 norm = Vector2(axis.y, -axis.x);
RenderHelper::DrawLine(Vector2(50.0f, 50.0f) + axis * p1Min + norm * 2.0f, Vector2(50.0f, 50.0f) + axis * p1Max + norm * 2.0f);
RenderManager::Instance()->SetColor(1.0f, 1.0f, 0.0f, 1.0f);
RenderHelper::DrawLine(Vector2(50.0f, 50.0f) + axis * p2Min - norm * 2.0f, Vector2(50.0f, 50.0f) + axis * p2Max - norm * 2.0f);
#endif
if (IntervalDistance(p1Min, p1Max, p2Min, p2Max) > 0)
return false;
}
return true;
}
