space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-15 03:30:53 +01:00
Code Issues Packages Projects Releases Wiki Activity

Use Box2D FindMaxSeparation (#1786)

Browse Source
This commit is contained in:
metalgearsloth
2021-05-31 16:52:01 +10:00
committed by GitHub
parent b98b36a461
commit 196a6047f1
1 changed files with 55 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
Robust.Shared/Physics/Collision/Collision.cs
View File

@@ -1025,10 +1025,11 @@ namespace Robust.Shared.Physics.Collision
PolygonShape polyB, in Transform transformB)
{
manifold.PointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
var totalRadius = polyA.Radius + polyB.Radius;
var edgeA = 0;
var separationA = FindMaxSeparation(out edgeA, polyA, transformA, polyB, transformB);
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, transformA, polyB, transformB);
if (separationA > totalRadius)
return;
@@ -1265,84 +1266,64 @@ namespace Robust.Shared.Physics.Collision
private static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, in Transform xf1,
PolygonShape poly2, in Transform xf2)
{
int count1 = poly1.Vertices.Count;
List<Vector2> normals1 = poly1.Normals;
// MIT License
// Vector pointing from the centroid of poly1 to the centroid of poly2.
// TODO: Center of mass
// Vector2 d = Transform.Mul(xf2, poly2.MassData.Centroid) - Transform.Mul(xf1, poly1.MassData.Centroid);
Vector2 d = Transform.Mul(xf2, Vector2.Zero) - Transform.Mul(xf1, Vector2.Zero);
Vector2 dLocal1 = Transform.MulT(xf1.Quaternion2D, d);
// Copyright (c) 2019 Erin Catto
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = float.MinValue;
for (int i = 0; i < count1; ++i)
// 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.
var n1s = poly1.Normals;
var v1s = poly1.Vertices;
var v2s = poly2.Vertices;
var count1 = v1s.Count;
var count2 = v2s.Count;
var xf = Transform.MulT(xf2, xf1);
var bestIndex = 0;
var maxSeparation = float.MinValue;
for (var i = 0; i < count1; i++)
{
float dot = Vector2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
// Get poly1 normal in frame2.
var n = Transform.Mul(xf.Quaternion2D, n1s[i]);
var v1 = Transform.Mul(xf, v1s[i]);
// Find deepest point for normal i.
float si = float.MaxValue;
for (var j = 0; j < count2; ++j)
{
maxDot = dot;
edge = i;
float sij = Vector2.Dot(n, v2s[j] - v1);
if (sij < si)
{
si = sij;
}
}
if (si > maxSeparation)
{
maxSeparation = si;
bestIndex = i;
}
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for (;;)
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
edgeIndex = bestIndex;
return maxSeparation;
}
private static void FindIncidentEdge(Span<ClipVertex> c, PolygonShape poly1, in Transform xf1, int edge1,
@@ -1395,50 +1376,6 @@ namespace Robust.Shared.Physics.Collision
c[1] = cv1;
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
/// <param name="poly1">The poly1.</param>
/// <param name="xf1">The XF1.</param>
/// <param name="edge1">The edge1.</param>
/// <param name="poly2">The poly2.</param>
/// <param name="xf2">The XF2.</param>
/// <returns></returns>
private static float EdgeSeparation(PolygonShape poly1, in Transform xf1, int edge1, PolygonShape poly2,
in Transform xf2)
{
List<Vector2> vertices1 = poly1.Vertices;
List<Vector2> normals1 = poly1.Normals;
int count2 = poly2.Vertices.Count;
List<Vector2> vertices2 = poly2.Vertices;
DebugTools.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = Transform.Mul(xf1.Quaternion2D, normals1[edge1]);
Vector2 normal1 = Transform.MulT(xf2.Quaternion2D, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = float.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vector2 v1 = Transform.Mul(xf1, vertices1[edge1]);
Vector2 v2 = Transform.Mul(xf2, vertices2[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
return separation;
}
}
/// <summary>