mirror of
https://github.com/love2d/love.git
synced 2026-08-19 12:14:20 +02:00
Starting box2d update
This commit is contained in:
Garrett Brown
@@ -0,0 +1,524 @@
|
||||
// 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;
|
||||
}
|
||||
Reference in New Issue
Block a user