mirror of
https://github.com/love2d/love.git
synced 2026-08-19 04:06:17 +02:00
Garrett Brown
525 lines
12 KiB
C++
525 lines
12 KiB
C++
// MIT License
|
|
|
|
// Copyright (c) 2019 Erin Catto
|
|
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
// of this software and associated documentation files (the "Software"), to deal
|
|
// in the Software without restriction, including without limitation the rights
|
|
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
// copies of the Software, and to permit persons to whom the Software is
|
|
// furnished to do so, subject to the following conditions:
|
|
|
|
// The above copyright notice and this permission notice shall be included in all
|
|
// copies or substantial portions of the Software.
|
|
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
// SOFTWARE.
|
|
|
|
#include "box2d/b2_collision.h"
|
|
#include "box2d/b2_circle_shape.h"
|
|
#include "box2d/b2_edge_shape.h"
|
|
#include "box2d/b2_polygon_shape.h"
|
|
|
|
|
|
// Compute contact points for edge versus circle.
|
|
// This accounts for edge connectivity.
|
|
void b2CollideEdgeAndCircle(b2Manifold* manifold,
|
|
const b2EdgeShape* edgeA, const b2Transform& xfA,
|
|
const b2CircleShape* circleB, const b2Transform& xfB)
|
|
{
|
|
manifold->pointCount = 0;
|
|
|
|
// Compute circle in frame of edge
|
|
b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
|
|
|
|
b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
|
|
b2Vec2 e = B - A;
|
|
|
|
// Normal points to the right for a CCW winding
|
|
b2Vec2 n(e.y, -e.x);
|
|
float offset = b2Dot(n, Q - A);
|
|
|
|
bool oneSided = edgeA->m_oneSided;
|
|
if (oneSided && offset < 0.0f)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Barycentric coordinates
|
|
float u = b2Dot(e, B - Q);
|
|
float v = b2Dot(e, Q - A);
|
|
|
|
float radius = edgeA->m_radius + circleB->m_radius;
|
|
|
|
b2ContactFeature cf;
|
|
cf.indexB = 0;
|
|
cf.typeB = b2ContactFeature::e_vertex;
|
|
|
|
// Region A
|
|
if (v <= 0.0f)
|
|
{
|
|
b2Vec2 P = A;
|
|
b2Vec2 d = Q - P;
|
|
float dd = b2Dot(d, d);
|
|
if (dd > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Is there an edge connected to A?
|
|
if (edgeA->m_oneSided)
|
|
{
|
|
b2Vec2 A1 = edgeA->m_vertex0;
|
|
b2Vec2 B1 = A;
|
|
b2Vec2 e1 = B1 - A1;
|
|
float u1 = b2Dot(e1, B1 - Q);
|
|
|
|
// Is the circle in Region AB of the previous edge?
|
|
if (u1 > 0.0f)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
cf.indexA = 0;
|
|
cf.typeA = b2ContactFeature::e_vertex;
|
|
manifold->pointCount = 1;
|
|
manifold->type = b2Manifold::e_circles;
|
|
manifold->localNormal.SetZero();
|
|
manifold->localPoint = P;
|
|
manifold->points[0].id.key = 0;
|
|
manifold->points[0].id.cf = cf;
|
|
manifold->points[0].localPoint = circleB->m_p;
|
|
return;
|
|
}
|
|
|
|
// Region B
|
|
if (u <= 0.0f)
|
|
{
|
|
b2Vec2 P = B;
|
|
b2Vec2 d = Q - P;
|
|
float dd = b2Dot(d, d);
|
|
if (dd > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Is there an edge connected to B?
|
|
if (edgeA->m_oneSided)
|
|
{
|
|
b2Vec2 B2 = edgeA->m_vertex3;
|
|
b2Vec2 A2 = B;
|
|
b2Vec2 e2 = B2 - A2;
|
|
float v2 = b2Dot(e2, Q - A2);
|
|
|
|
// Is the circle in Region AB of the next edge?
|
|
if (v2 > 0.0f)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
cf.indexA = 1;
|
|
cf.typeA = b2ContactFeature::e_vertex;
|
|
manifold->pointCount = 1;
|
|
manifold->type = b2Manifold::e_circles;
|
|
manifold->localNormal.SetZero();
|
|
manifold->localPoint = P;
|
|
manifold->points[0].id.key = 0;
|
|
manifold->points[0].id.cf = cf;
|
|
manifold->points[0].localPoint = circleB->m_p;
|
|
return;
|
|
}
|
|
|
|
// Region AB
|
|
float den = b2Dot(e, e);
|
|
b2Assert(den > 0.0f);
|
|
b2Vec2 P = (1.0f / den) * (u * A + v * B);
|
|
b2Vec2 d = Q - P;
|
|
float dd = b2Dot(d, d);
|
|
if (dd > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (offset < 0.0f)
|
|
{
|
|
n.Set(-n.x, -n.y);
|
|
}
|
|
n.Normalize();
|
|
|
|
cf.indexA = 0;
|
|
cf.typeA = b2ContactFeature::e_face;
|
|
manifold->pointCount = 1;
|
|
manifold->type = b2Manifold::e_faceA;
|
|
manifold->localNormal = n;
|
|
manifold->localPoint = A;
|
|
manifold->points[0].id.key = 0;
|
|
manifold->points[0].id.cf = cf;
|
|
manifold->points[0].localPoint = circleB->m_p;
|
|
}
|
|
|
|
// This structure is used to keep track of the best separating axis.
|
|
struct b2EPAxis
|
|
{
|
|
enum Type
|
|
{
|
|
e_unknown,
|
|
e_edgeA,
|
|
e_edgeB
|
|
};
|
|
|
|
b2Vec2 normal;
|
|
Type type;
|
|
int32 index;
|
|
float separation;
|
|
};
|
|
|
|
// This holds polygon B expressed in frame A.
|
|
struct b2TempPolygon
|
|
{
|
|
b2Vec2 vertices[b2_maxPolygonVertices];
|
|
b2Vec2 normals[b2_maxPolygonVertices];
|
|
int32 count;
|
|
};
|
|
|
|
// Reference face used for clipping
|
|
struct b2ReferenceFace
|
|
{
|
|
int32 i1, i2;
|
|
b2Vec2 v1, v2;
|
|
b2Vec2 normal;
|
|
|
|
b2Vec2 sideNormal1;
|
|
float sideOffset1;
|
|
|
|
b2Vec2 sideNormal2;
|
|
float sideOffset2;
|
|
};
|
|
|
|
static b2EPAxis b2ComputeEdgeSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& normal1)
|
|
{
|
|
b2EPAxis axis;
|
|
axis.type = b2EPAxis::e_edgeA;
|
|
axis.index = -1;
|
|
axis.separation = -FLT_MAX;
|
|
axis.normal.SetZero();
|
|
|
|
b2Vec2 axes[2] = { normal1, -normal1 };
|
|
|
|
// Find axis with least overlap (min-max problem)
|
|
for (int32 j = 0; j < 2; ++j)
|
|
{
|
|
float sj = FLT_MAX;
|
|
|
|
// Find deepest polygon vertex along axis j
|
|
for (int32 i = 0; i < polygonB.count; ++i)
|
|
{
|
|
float si = b2Dot(axes[j], polygonB.vertices[i] - v1);
|
|
if (si < sj)
|
|
{
|
|
sj = si;
|
|
}
|
|
}
|
|
|
|
if (sj > axis.separation)
|
|
{
|
|
axis.index = j;
|
|
axis.separation = sj;
|
|
axis.normal = axes[j];
|
|
}
|
|
}
|
|
|
|
return axis;
|
|
}
|
|
|
|
static b2EPAxis b2ComputePolygonSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& v2)
|
|
{
|
|
b2EPAxis axis;
|
|
axis.type = b2EPAxis::e_unknown;
|
|
axis.index = -1;
|
|
axis.separation = -FLT_MAX;
|
|
axis.normal.SetZero();
|
|
|
|
for (int32 i = 0; i < polygonB.count; ++i)
|
|
{
|
|
b2Vec2 n = -polygonB.normals[i];
|
|
|
|
float s1 = b2Dot(n, polygonB.vertices[i] - v1);
|
|
float s2 = b2Dot(n, polygonB.vertices[i] - v2);
|
|
float s = b2Min(s1, s2);
|
|
|
|
if (s > axis.separation)
|
|
{
|
|
axis.type = b2EPAxis::e_edgeB;
|
|
axis.index = i;
|
|
axis.separation = s;
|
|
axis.normal = n;
|
|
}
|
|
}
|
|
|
|
return axis;
|
|
}
|
|
|
|
void b2CollideEdgeAndPolygon(b2Manifold* manifold,
|
|
const b2EdgeShape* edgeA, const b2Transform& xfA,
|
|
const b2PolygonShape* polygonB, const b2Transform& xfB)
|
|
{
|
|
manifold->pointCount = 0;
|
|
|
|
b2Transform xf = b2MulT(xfA, xfB);
|
|
|
|
b2Vec2 centroidB = b2Mul(xf, polygonB->m_centroid);
|
|
|
|
b2Vec2 v1 = edgeA->m_vertex1;
|
|
b2Vec2 v2 = edgeA->m_vertex2;
|
|
|
|
b2Vec2 edge1 = v2 - v1;
|
|
edge1.Normalize();
|
|
|
|
// Normal points to the right for a CCW winding
|
|
b2Vec2 normal1(edge1.y, -edge1.x);
|
|
float offset1 = b2Dot(normal1, centroidB - v1);
|
|
|
|
bool oneSided = edgeA->m_oneSided;
|
|
if (oneSided && offset1 < 0.0f)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Get polygonB in frameA
|
|
b2TempPolygon tempPolygonB;
|
|
tempPolygonB.count = polygonB->m_count;
|
|
for (int32 i = 0; i < polygonB->m_count; ++i)
|
|
{
|
|
tempPolygonB.vertices[i] = b2Mul(xf, polygonB->m_vertices[i]);
|
|
tempPolygonB.normals[i] = b2Mul(xf.q, polygonB->m_normals[i]);
|
|
}
|
|
|
|
float radius = polygonB->m_radius + edgeA->m_radius;
|
|
|
|
b2EPAxis edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1);
|
|
if (edgeAxis.separation > radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
b2EPAxis polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2);
|
|
if (polygonAxis.separation > radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Use hysteresis for jitter reduction.
|
|
const float k_relativeTol = 0.98f;
|
|
const float k_absoluteTol = 0.001f;
|
|
|
|
b2EPAxis primaryAxis;
|
|
if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol)
|
|
{
|
|
primaryAxis = polygonAxis;
|
|
}
|
|
else
|
|
{
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
|
|
if (oneSided)
|
|
{
|
|
// Smooth collision
|
|
// See https://box2d.org/posts/2020/06/ghost-collisions/
|
|
|
|
b2Vec2 edge0 = v1 - edgeA->m_vertex0;
|
|
edge0.Normalize();
|
|
b2Vec2 normal0(edge0.y, -edge0.x);
|
|
bool convex1 = b2Cross(edge0, edge1) >= 0.0f;
|
|
|
|
b2Vec2 edge2 = edgeA->m_vertex3 - v2;
|
|
edge2.Normalize();
|
|
b2Vec2 normal2(edge2.y, -edge2.x);
|
|
bool convex2 = b2Cross(edge1, edge2) >= 0.0f;
|
|
|
|
const float sinTol = 0.1f;
|
|
bool side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0f;
|
|
|
|
// Check Gauss Map
|
|
if (side1)
|
|
{
|
|
if (convex1)
|
|
{
|
|
if (b2Cross(primaryAxis.normal, normal0) > sinTol)
|
|
{
|
|
// Skip region
|
|
return;
|
|
}
|
|
|
|
// Admit region
|
|
}
|
|
else
|
|
{
|
|
// Snap region
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (convex2)
|
|
{
|
|
if (b2Cross(normal2, primaryAxis.normal) > sinTol)
|
|
{
|
|
// Skip region
|
|
return;
|
|
}
|
|
|
|
// Admit region
|
|
}
|
|
else
|
|
{
|
|
// Snap region
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
}
|
|
}
|
|
|
|
b2ClipVertex clipPoints[2];
|
|
b2ReferenceFace ref;
|
|
if (primaryAxis.type == b2EPAxis::e_edgeA)
|
|
{
|
|
manifold->type = b2Manifold::e_faceA;
|
|
|
|
// Search for the polygon normal that is most anti-parallel to the edge normal.
|
|
int32 bestIndex = 0;
|
|
float bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0]);
|
|
for (int32 i = 1; i < tempPolygonB.count; ++i)
|
|
{
|
|
float value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i]);
|
|
if (value < bestValue)
|
|
{
|
|
bestValue = value;
|
|
bestIndex = i;
|
|
}
|
|
}
|
|
|
|
int32 i1 = bestIndex;
|
|
int32 i2 = i1 + 1 < tempPolygonB.count ? i1 + 1 : 0;
|
|
|
|
clipPoints[0].v = tempPolygonB.vertices[i1];
|
|
clipPoints[0].id.cf.indexA = 0;
|
|
clipPoints[0].id.cf.indexB = static_cast<uint8>(i1);
|
|
clipPoints[0].id.cf.typeA = b2ContactFeature::e_face;
|
|
clipPoints[0].id.cf.typeB = b2ContactFeature::e_vertex;
|
|
|
|
clipPoints[1].v = tempPolygonB.vertices[i2];
|
|
clipPoints[1].id.cf.indexA = 0;
|
|
clipPoints[1].id.cf.indexB = static_cast<uint8>(i2);
|
|
clipPoints[1].id.cf.typeA = b2ContactFeature::e_face;
|
|
clipPoints[1].id.cf.typeB = b2ContactFeature::e_vertex;
|
|
|
|
ref.i1 = 0;
|
|
ref.i2 = 1;
|
|
ref.v1 = v1;
|
|
ref.v2 = v2;
|
|
ref.normal = primaryAxis.normal;
|
|
ref.sideNormal1 = -edge1;
|
|
ref.sideNormal2 = edge1;
|
|
}
|
|
else
|
|
{
|
|
manifold->type = b2Manifold::e_faceB;
|
|
|
|
clipPoints[0].v = v2;
|
|
clipPoints[0].id.cf.indexA = 1;
|
|
clipPoints[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
|
|
clipPoints[0].id.cf.typeA = b2ContactFeature::e_vertex;
|
|
clipPoints[0].id.cf.typeB = b2ContactFeature::e_face;
|
|
|
|
clipPoints[1].v = v1;
|
|
clipPoints[1].id.cf.indexA = 0;
|
|
clipPoints[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
|
|
clipPoints[1].id.cf.typeA = b2ContactFeature::e_vertex;
|
|
clipPoints[1].id.cf.typeB = b2ContactFeature::e_face;
|
|
|
|
ref.i1 = primaryAxis.index;
|
|
ref.i2 = ref.i1 + 1 < tempPolygonB.count ? ref.i1 + 1 : 0;
|
|
ref.v1 = tempPolygonB.vertices[ref.i1];
|
|
ref.v2 = tempPolygonB.vertices[ref.i2];
|
|
ref.normal = tempPolygonB.normals[ref.i1];
|
|
|
|
// CCW winding
|
|
ref.sideNormal1.Set(ref.normal.y, -ref.normal.x);
|
|
ref.sideNormal2 = -ref.sideNormal1;
|
|
}
|
|
|
|
ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1);
|
|
ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2);
|
|
|
|
// Clip incident edge against reference face side planes
|
|
b2ClipVertex clipPoints1[2];
|
|
b2ClipVertex clipPoints2[2];
|
|
int32 np;
|
|
|
|
// Clip to side 1
|
|
np = b2ClipSegmentToLine(clipPoints1, clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1);
|
|
|
|
if (np < b2_maxManifoldPoints)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Clip to side 2
|
|
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2);
|
|
|
|
if (np < b2_maxManifoldPoints)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Now clipPoints2 contains the clipped points.
|
|
if (primaryAxis.type == b2EPAxis::e_edgeA)
|
|
{
|
|
manifold->localNormal = ref.normal;
|
|
manifold->localPoint = ref.v1;
|
|
}
|
|
else
|
|
{
|
|
manifold->localNormal = polygonB->m_normals[ref.i1];
|
|
manifold->localPoint = polygonB->m_vertices[ref.i1];
|
|
}
|
|
|
|
int32 pointCount = 0;
|
|
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
|
|
{
|
|
float separation;
|
|
|
|
separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1);
|
|
|
|
if (separation <= radius)
|
|
{
|
|
b2ManifoldPoint* cp = manifold->points + pointCount;
|
|
|
|
if (primaryAxis.type == b2EPAxis::e_edgeA)
|
|
{
|
|
cp->localPoint = b2MulT(xf, clipPoints2[i].v);
|
|
cp->id = clipPoints2[i].id;
|
|
}
|
|
else
|
|
{
|
|
cp->localPoint = clipPoints2[i].v;
|
|
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
|
|
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
|
|
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
|
|
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
|
|
}
|
|
|
|
++pointCount;
|
|
}
|
|
}
|
|
|
|
manifold->pointCount = pointCount;
|
|
}
|