vlad.os/LuaCsForBarotraumaEP
0
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

(61d00a474) v0.9.7.1

Browse Source
This commit is contained in:
Regalis
2020-03-04 13:04:10 +01:00
parent 3c50efa5c9
commit 3c09ebe02f
5086 changed files with 786063 additions and 295871 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Libraries/Farseer Physics Engine 3.5/Collision/Collision.cs
+209 -198
View File
@@ -1,4 +1,11 @@
/*
// Copyright (c) 2017 Kastellanos Nikolaos
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -26,6 +33,7 @@ using System.Diagnostics;
using System.Runtime.InteropServices;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision
@@ -382,8 +390,8 @@ namespace FarseerPhysics.Collision
/// <param name="aabb">The aabb.</param>
public void Combine(ref AABB aabb)
{
LowerBound = Vector2.Min(LowerBound, aabb.LowerBound);
UpperBound = Vector2.Max(UpperBound, aabb.UpperBound);
Vector2.Min(ref LowerBound, ref aabb.LowerBound, out LowerBound);
Vector2.Max(ref UpperBound, ref aabb.UpperBound, out UpperBound);
}
/// <summary>
@@ -393,8 +401,8 @@ namespace FarseerPhysics.Collision
/// <param name="aabb2">The aabb2.</param>
public void Combine(ref AABB aabb1, ref AABB aabb2)
{
LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound);
UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound);
Vector2.Min(ref aabb1.LowerBound, ref aabb2.LowerBound, out LowerBound);
Vector2.Max(ref aabb1.UpperBound, ref aabb2.UpperBound, out UpperBound);
}
/// <summary>
@@ -434,15 +442,13 @@ namespace FarseerPhysics.Collision
/// <param name="a">The first AABB.</param>
/// <param name="b">The second AABB.</param>
/// <returns>True if they are overlapping.</returns>
public static bool TestOverlap(ref AABB a, ref AABB b)
{
if (b.LowerBound.X - a.UpperBound.X > 0.0f ||
b.LowerBound.Y - a.UpperBound.Y > 0.0f ||
a.LowerBound.X - b.UpperBound.X > 0.0f ||
a.LowerBound.Y - b.UpperBound.Y > 0.0f) return false;
return true;
return
b.LowerBound.X - a.UpperBound.X <= 0.0f &&
b.LowerBound.Y - a.UpperBound.Y <= 0.0f &&
a.LowerBound.X - b.UpperBound.X <= 0.0f &&
a.LowerBound.Y - b.UpperBound.Y <= 0.0f;
}
/// <summary>
@@ -537,16 +543,6 @@ namespace FarseerPhysics.Collision
}
}
/// <summary>
/// This holds polygon B expressed in frame A.
/// </summary>
public class TempPolygon
{
public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices];
public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices];
public int Count;
}
/// <summary>
/// This structure is used to keep track of the best separating axis.
/// </summary>
@@ -587,9 +583,6 @@ namespace FarseerPhysics.Collision
/// </summary>
public static class Collision
{
[ThreadStatic]
private static DistanceInput _input;
/// <summary>
/// Test overlap between the two shapes.
/// </summary>
@@ -602,9 +595,9 @@ namespace FarseerPhysics.Collision
/// <returns></returns>
public static bool TestOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, ref Transform xfA, ref Transform xfB)
{
_input = _input ?? new DistanceInput();
_input.ProxyA.Set(shapeA, indexA);
_input.ProxyB.Set(shapeB, indexB);
DistanceInput _input = new DistanceInput();
_input.ProxyA = new DistanceProxy(shapeA, indexA);
_input.ProxyB = new DistanceProxy(shapeB, indexB);
_input.TransformA = xfA;
_input.TransformB = xfB;
_input.UseRadii = true;
@@ -663,8 +656,8 @@ namespace FarseerPhysics.Collision
{
manifold.PointCount = 0;
Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position);
Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position);
Vector2 pA = Transform.Multiply(ref circleA._position, ref xfA);
Vector2 pB = Transform.Multiply(ref circleB._position, ref xfB);
Vector2 d = pB - pA;
float distSqr = Vector2.Dot(d, d);
@@ -700,8 +693,8 @@ namespace FarseerPhysics.Collision
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = MathUtils.Mul(ref xfB, circleB.Position);
Vector2 cLocal = MathUtils.MulT(ref xfA, c);
Vector2 c = Transform.Multiply(ref circleB._position, ref xfB);
Vector2 cLocal = Transform.Divide(ref c, ref xfA);
// Find the min separating edge.
int normalIndex = 0;
@@ -902,13 +895,13 @@ namespace FarseerPhysics.Collision
Vector2 localNormal = new Vector2(localTangent.Y, -localTangent.X);
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 tangent = MathUtils.Mul(xf1.q, localTangent);
Vector2 tangent = Complex.Multiply(ref localTangent, ref xf1.q);
float normalx = tangent.Y;
float normaly = -tangent.X;
v11 = MathUtils.Mul(ref xf1, v11);
v12 = MathUtils.Mul(ref xf1, v12);
v11 = Transform.Multiply(ref v11, ref xf1);
v12 = Transform.Multiply(ref v12, ref xf1);
// Face offset.
float frontOffset = normalx * v11.X + normaly * v11.Y;
@@ -948,7 +941,7 @@ namespace FarseerPhysics.Collision
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V);
Transform.Divide(clipPoints2[i].V, ref xf2, out cp.LocalPoint);
cp.Id = clipPoints2[i].ID;
if (flip)
@@ -984,7 +977,7 @@ namespace FarseerPhysics.Collision
manifold.PointCount = 0;
// Compute circle in frame of edge
Vector2 Q = MathUtils.MulT(ref transformA, MathUtils.Mul(ref transformB, ref circleB._position));
Vector2 Q = Transform.Divide(Transform.Multiply(ref circleB._position, ref transformB), ref transformA);
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
Vector2 e = B - A;
@@ -1126,24 +1119,29 @@ namespace FarseerPhysics.Collision
/// <param name="xfB">The xf B.</param>
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
EPCollider collider = new EPCollider();
collider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
EPCollider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
}
private class EPCollider
private static class EPCollider
{
private TempPolygon _polygonB = new TempPolygon();
/// <summary>
/// This holds polygon B expressed in frame A.
/// </summary>
internal struct TempPolygon
{
public Vector2[] Vertices;
public Vector2[] Normals;
public int Count;
Transform _xf;
Vector2 _centroidB;
Vector2 _v0, _v1, _v2, _v3;
Vector2 _normal0, _normal1, _normal2;
Vector2 _normal;
Vector2 _lowerLimit, _upperLimit;
float _radius;
bool _front;
internal TempPolygon(int maxPolygonVertices)
{
Vertices = new Vector2[maxPolygonVertices];
Normals = new Vector2[maxPolygonVertices];
Count = 0;
}
}
public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
public static void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
@@ -1155,43 +1153,54 @@ namespace FarseerPhysics.Collision
// 7. Return if _any_ axis indicates separation
// 8. Clip
_xf = MathUtils.MulT(xfA, xfB);
TempPolygon tempPolygonB = new TempPolygon(Settings.MaxPolygonVertices);
Transform xf;
Vector2 centroidB;
Vector2 normal0 = new Vector2();
Vector2 normal1;
Vector2 normal2 = new Vector2();
Vector2 normal;
Vector2 lowerLimit, upperLimit;
float radius;
bool front;
_centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid);
Transform.Divide(ref xfB, ref xfA, out xf);
_v0 = edgeA.Vertex0;
_v1 = edgeA._vertex1;
_v2 = edgeA._vertex2;
_v3 = edgeA.Vertex3;
centroidB = Transform.Multiply(polygonB.MassData.Centroid, ref xf);
Vector2 v0 = edgeA.Vertex0;
Vector2 v1 = edgeA._vertex1;
Vector2 v2 = edgeA._vertex2;
Vector2 v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = _v2 - _v1;
Vector2 edge1 = v2 - v1;
edge1.Normalize();
_normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(_normal1, _centroidB - _v1);
normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(normal1, centroidB - v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = _v1 - _v0;
Vector2 edge0 = v1 - v0;
edge0.Normalize();
_normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(_normal0, _centroidB - _v0);
normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(ref edge0, ref edge1) >= 0.0f;
offset0 = Vector2.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = _v3 - _v2;
Vector2 edge2 = v3 - v2;
edge2.Normalize();
_normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(_normal2, _centroidB - _v2);
normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(ref edge1, ref edge2) > 0.0f;
offset2 = Vector2.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
@@ -1199,66 +1208,66 @@ namespace FarseerPhysics.Collision
{
if (convex1 && convex2)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal2;
normal = normal1;
lowerLimit = normal0;
upperLimit = normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
}
else if (convex1)
{
_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (_front)
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal1;
normal = normal1;
lowerLimit = normal0;
upperLimit = normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal1;
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal1;
}
}
else if (convex2)
{
_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (_front)
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal2;
normal = normal1;
lowerLimit = normal1;
upperLimit = normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal0;
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal0;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
normal = normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal0;
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal0;
}
}
}
@@ -1266,34 +1275,34 @@ namespace FarseerPhysics.Collision
{
if (convex1)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (_front)
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = -_normal1;
normal = normal1;
lowerLimit = normal0;
upperLimit = -normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (_front)
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
normal = normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal0;
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal0;
}
}
}
@@ -1301,67 +1310,67 @@ namespace FarseerPhysics.Collision
{
if (convex2)
{
_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal2;
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
normal = -normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
}
else
{
_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = _normal1;
normal = -normal1;
lowerLimit = -normal2;
upperLimit = normal1;
}
}
}
else
{
_front = offset1 >= 0.0f;
if (_front)
front = offset1 >= 0.0f;
if (front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
normal = normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
normal = -normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
}
// Get polygonB in frameA
_polygonB.Count = polygonB.Vertices.Count;
tempPolygonB.Count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]);
_polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]);
tempPolygonB.Vertices[i] = Transform.Multiply(polygonB.Vertices[i], ref xf);
tempPolygonB.Normals[i] = Complex.Multiply(polygonB.Normals[i], ref xf.q);
}
_radius = 2.0f * Settings.PolygonRadius;
radius = 2.0f * Settings.PolygonRadius;
manifold.PointCount = 0;
EPAxis edgeAxis = ComputeEdgeSeparation();
EPAxis edgeAxis = ComputeEdgeSeparation(ref tempPolygonB, ref normal, ref v1, front);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxisType.Unknown)
@@ -1369,13 +1378,13 @@ namespace FarseerPhysics.Collision
return;
}
if (edgeAxis.Separation > _radius)
if (edgeAxis.Separation > radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
EPAxis polygonAxis = ComputePolygonSeparation(ref tempPolygonB, ref normal, ref v1, ref v2, ref lowerLimit, ref upperLimit, radius);
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > radius)
{
return;
}
@@ -1406,10 +1415,10 @@ namespace FarseerPhysics.Collision
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]);
for (int i = 1; i < _polygonB.Count; ++i)
float bestValue = Vector2.Dot(normal, tempPolygonB.Normals[0]);
for (int i = 1; i < tempPolygonB.Count; ++i)
{
float value = Vector2.Dot(_normal, _polygonB.Normals[i]);
float value = Vector2.Dot(normal, tempPolygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
@@ -1418,10 +1427,10 @@ namespace FarseerPhysics.Collision
}
int i1 = bestIndex;
int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0;
int i2 = i1 + 1 < tempPolygonB.Count ? i1 + 1 : 0;
ClipVertex c0 = ie[0];
c0.V = _polygonB.Vertices[i1];
c0.V = tempPolygonB.Vertices[i1];
c0.ID.Features.IndexA = 0;
c0.ID.Features.IndexB = (byte)i1;
c0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
@@ -1429,35 +1438,35 @@ namespace FarseerPhysics.Collision
ie[0] = c0;
ClipVertex c1 = ie[1];
c1.V = _polygonB.Vertices[i2];
c1.V = tempPolygonB.Vertices[i2];
c1.ID.Features.IndexA = 0;
c1.ID.Features.IndexB = (byte)i2;
c1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
c1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
ie[1] = c1;
if (_front)
if (front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = _v1;
rf.v2 = _v2;
rf.normal = _normal1;
rf.v1 = v1;
rf.v2 = v2;
rf.normal = normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = _v2;
rf.v2 = _v1;
rf.normal = -_normal1;
rf.v1 = v2;
rf.v2 = v1;
rf.normal = -normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ClipVertex c0 = ie[0];
c0.V = _v1;
c0.V = v1;
c0.ID.Features.IndexA = 0;
c0.ID.Features.IndexB = (byte)primaryAxis.Index;
c0.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
@@ -1465,7 +1474,7 @@ namespace FarseerPhysics.Collision
ie[0] = c0;
ClipVertex c1 = ie[1];
c1.V = _v2;
c1.V = v2;
c1.ID.Features.IndexA = 0;
c1.ID.Features.IndexB = (byte)primaryAxis.Index;
c1.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
@@ -1473,10 +1482,10 @@ namespace FarseerPhysics.Collision
ie[1] = c1;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0;
rf.v1 = _polygonB.Vertices[rf.i1];
rf.v2 = _polygonB.Vertices[rf.i2];
rf.normal = _polygonB.Normals[rf.i1];
rf.i2 = rf.i1 + 1 < tempPolygonB.Count ? rf.i1 + 1 : 0;
rf.v1 = tempPolygonB.Vertices[rf.i1];
rf.v2 = tempPolygonB.Vertices[rf.i2];
rf.normal = tempPolygonB.Normals[rf.i1];
}
rf.sideNormal1 = new Vector2(rf.normal.Y, -rf.normal.X);
@@ -1522,13 +1531,13 @@ namespace FarseerPhysics.Collision
{
float separation = Vector2.Dot(rf.normal, clipPoints2[i].V - rf.v1);
if (separation <= _radius)
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V);
Transform.Divide(clipPoints2[i].V, ref xf, out cp.LocalPoint);
cp.Id = clipPoints2[i].ID;
}
else
@@ -1548,16 +1557,16 @@ namespace FarseerPhysics.Collision
manifold.PointCount = pointCount;
}
private EPAxis ComputeEdgeSeparation()
private static EPAxis ComputeEdgeSeparation(ref TempPolygon polygonB, ref Vector2 normal, ref Vector2 v1, bool front)
{
EPAxis axis;
axis.Type = EPAxisType.EdgeA;
axis.Index = _front ? 0 : 1;
axis.Index = front ? 0 : 1;
axis.Separation = Settings.MaxFloat;
for (int i = 0; i < _polygonB.Count; ++i)
for (int i = 0; i < polygonB.Count; ++i)
{
float s = Vector2.Dot(_normal, _polygonB.Vertices[i] - _v1);
float s = Vector2.Dot(normal, polygonB.Vertices[i] - v1);
if (s < axis.Separation)
{
axis.Separation = s;
@@ -1567,24 +1576,24 @@ namespace FarseerPhysics.Collision
return axis;
}
private EPAxis ComputePolygonSeparation()
private static EPAxis ComputePolygonSeparation(ref TempPolygon polygonB, ref Vector2 normal, ref Vector2 v1, ref Vector2 v2, ref Vector2 lowerLimit, ref Vector2 upperLimit, float radius)
{
EPAxis axis;
axis.Type = EPAxisType.Unknown;
axis.Index = -1;
axis.Separation = -Settings.MaxFloat;
Vector2 perp = new Vector2(-_normal.Y, _normal.X);
Vector2 perp = new Vector2(-normal.Y, normal.X);
for (int i = 0; i < _polygonB.Count; ++i)
for (int i = 0; i < polygonB.Count; ++i)
{
Vector2 n = -_polygonB.Normals[i];
Vector2 n = -polygonB.Normals[i];
float s1 = Vector2.Dot(n, _polygonB.Vertices[i] - _v1);
float s2 = Vector2.Dot(n, _polygonB.Vertices[i] - _v2);
float s1 = Vector2.Dot(n, polygonB.Vertices[i] - v1);
float s2 = Vector2.Dot(n, polygonB.Vertices[i] - v2);
float s = Math.Min(s1, s2);
if (s > _radius)
if (s > radius)
{
// No collision
axis.Type = EPAxisType.EdgeB;
@@ -1596,14 +1605,14 @@ namespace FarseerPhysics.Collision
// Adjacency
if (Vector2.Dot(n, perp) >= 0.0f)
{
if (Vector2.Dot(n - _upperLimit, _normal) < -Settings.AngularSlop)
if (Vector2.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2.Dot(n - _lowerLimit, _normal) < -Settings.AngularSlop)
if (Vector2.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
@@ -1682,7 +1691,7 @@ namespace FarseerPhysics.Collision
/// <param name="poly2">The poly2.</param>
/// <param name="xf2">The XF2.</param>
/// <returns></returns>
private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2)
private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1To2, int edge1, PolygonShape poly2)
{
List<Vector2> vertices1 = poly1.Vertices;
List<Vector2> normals1 = poly1.Normals;
@@ -1693,8 +1702,7 @@ namespace FarseerPhysics.Collision
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = MathUtils.Mul(xf1.q, normals1[edge1]);
Vector2 normal1 = MathUtils.MulT(xf2.q, normal1World);
Vector2 normal1 = Complex.Multiply(normals1[edge1], ref xf1To2.q);
// Find support vertex on poly2 for -normal.
int index = 0;
@@ -1702,7 +1710,7 @@ namespace FarseerPhysics.Collision
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(vertices2[i], normal1);
float dot = MathUtils.Dot(vertices2[i], ref normal1);
if (dot < minDot)
{
minDot = dot;
@@ -1710,9 +1718,10 @@ namespace FarseerPhysics.Collision
}
}
Vector2 v1 = MathUtils.Mul(ref xf1, vertices1[edge1]);
Vector2 v2 = MathUtils.Mul(ref xf2, vertices2[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
Vector2 v1 = Transform.Multiply(vertices1[edge1], ref xf1To2);
Vector2 v2 = vertices2[index];
float separation = MathUtils.Dot(v2 - v1, ref normal1);
return separation;
}
@@ -1730,16 +1739,18 @@ namespace FarseerPhysics.Collision
int count1 = poly1.Vertices.Count;
List<Vector2> normals1 = poly1.Normals;
var xf1To2 = Transform.Divide(ref xf1, ref xf2);
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vector2 d = MathUtils.Mul(ref xf2, poly2.MassData.Centroid) - MathUtils.Mul(ref xf1, poly1.MassData.Centroid);
Vector2 dLocal1 = MathUtils.MulT(xf1.q, d);
Vector2 c2local = Transform.Divide(poly2.MassData.Centroid, ref xf1To2);
Vector2 dLocal1 = c2local - poly1.MassData.Centroid;
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Settings.MaxFloat;
for (int i = 0; i < count1; ++i)
{
float dot = Vector2.Dot(normals1[i], dLocal1);
float dot = MathUtils.Dot(normals1[i], ref dLocal1);
if (dot > maxDot)
{
maxDot = dot;
@@ -1748,15 +1759,15 @@ namespace FarseerPhysics.Collision
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
float s = EdgeSeparation(poly1, ref xf1To2, edge, poly2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);
float sPrev = EdgeSeparation(poly1, ref xf1To2, prevEdge, poly2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);
float sNext = EdgeSeparation(poly1, ref xf1To2, nextEdge, poly2);
// Find the best edge and the search direction.
int bestEdge;
@@ -1788,7 +1799,7 @@ namespace FarseerPhysics.Collision
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
s = EdgeSeparation(poly1, ref xf1To2, edge, poly2);
if (s > bestSeparation)
{
@@ -1817,7 +1828,7 @@ namespace FarseerPhysics.Collision
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = MathUtils.MulT(xf2.q, MathUtils.Mul(xf1.q, normals1[edge1]));
Vector2 normal1 = Complex.Divide(Complex.Multiply(normals1[edge1], ref xf1.q), ref xf2.q);
// Find the incident edge on poly2.
@@ -1839,7 +1850,7 @@ namespace FarseerPhysics.Collision
ClipVertex cv0 = c[0];
cv0.V = MathUtils.Mul(ref xf2, vertices2[i1]);
cv0.V = Transform.Multiply(vertices2[i1], ref xf2);
cv0.ID.Features.IndexA = (byte)edge1;
cv0.ID.Features.IndexB = (byte)i1;
cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
@@ -1848,7 +1859,7 @@ namespace FarseerPhysics.Collision
c[0] = cv0;
ClipVertex cv1 = c[1];
cv1.V = MathUtils.Mul(ref xf2, vertices2[i2]);
cv1.V = Transform.Multiply(vertices2[i2], ref xf2);
cv1.ID.Features.IndexA = (byte)edge1;
cv1.ID.Features.IndexB = (byte)i2;
cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
Libraries/Farseer Physics Engine 3.5/Collision/Distance.cs
+30 -21
View File
@@ -1,4 +1,9 @@
/*
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -24,6 +29,7 @@ using System;
using System.Diagnostics;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision
@@ -32,10 +38,10 @@ namespace FarseerPhysics.Collision
/// A distance proxy is used by the GJK algorithm.
/// It encapsulates any shape.
/// </summary>
public class DistanceProxy
public struct DistanceProxy
{
internal float Radius;
internal Vertices Vertices = new Vertices();
internal Vertices Vertices;
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
@@ -45,8 +51,10 @@ namespace FarseerPhysics.Collision
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="index">The index.</param>
public void Set(Shape shape, int index)
public DistanceProxy(Shape shape, int index)
{
Vertices = new Vertices();
switch (shape.ShapeType)
{
case ShapeType.Circle:
@@ -93,6 +101,7 @@ namespace FarseerPhysics.Collision
break;
default:
Radius = 0;
Debug.Assert(false);
break;
}
@@ -171,10 +180,10 @@ namespace FarseerPhysics.Collision
/// Input for Distance.ComputeDistance().
/// You have to option to use the shape radii in the computation.
/// </summary>
public class DistanceInput
public struct DistanceInput
{
public DistanceProxy ProxyA = new DistanceProxy();
public DistanceProxy ProxyB = new DistanceProxy();
public DistanceProxy ProxyA;
public DistanceProxy ProxyB;
public Transform TransformA;
public Transform TransformB;
public bool UseRadii;
@@ -241,7 +250,7 @@ namespace FarseerPhysics.Collision
internal int Count;
internal FixedArray3<SimplexVertex> V;
internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB)
internal void ReadCache(ref SimplexCache cache, ref DistanceProxy proxyA, ref Transform transformA, ref DistanceProxy proxyB, ref Transform transformB)
{
Debug.Assert(cache.Count <= 3);
@@ -254,8 +263,8 @@ namespace FarseerPhysics.Collision
v.IndexB = cache.IndexB[i];
Vector2 wALocal = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.WA = MathUtils.Mul(ref transformA, wALocal);
v.WB = MathUtils.Mul(ref transformB, wBLocal);
v.WA = Transform.Multiply(ref wALocal, ref transformA);
v.WB = Transform.Multiply(ref wBLocal, ref transformB);
v.W = v.WB - v.WA;
v.A = 0.0f;
V[i] = v;
@@ -282,8 +291,8 @@ namespace FarseerPhysics.Collision
v.IndexB = 0;
Vector2 wALocal = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.WA = MathUtils.Mul(ref transformA, wALocal);
v.WB = MathUtils.Mul(ref transformB, wBLocal);
v.WA = Transform.Multiply(ref wALocal, ref transformA);
v.WB = Transform.Multiply(ref wBLocal, ref transformB);
v.W = v.WB - v.WA;
v.A = 1.0f;
V[0] = v;
@@ -514,11 +523,11 @@ namespace FarseerPhysics.Collision
float d23_2 = -w2e23;
// Triangle123
float n123 = MathUtils.Cross(e12, e13);
float n123 = MathUtils.Cross(ref e12, ref e13);
float d123_1 = n123 * MathUtils.Cross(w2, w3);
float d123_2 = n123 * MathUtils.Cross(w3, w1);
float d123_3 = n123 * MathUtils.Cross(w1, w2);
float d123_1 = n123 * MathUtils.Cross(ref w2, ref w3);
float d123_2 = n123 * MathUtils.Cross(ref w3, ref w1);
float d123_3 = n123 * MathUtils.Cross(ref w1, ref w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
@@ -646,7 +655,7 @@ namespace FarseerPhysics.Collision
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
simplex.ReadCache(ref cache, ref input.ProxyA, ref input.TransformA, ref input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
@@ -717,11 +726,11 @@ namespace FarseerPhysics.Collision
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d));
vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexA = input.ProxyA.GetSupport(-Complex.Divide(ref d, ref input.TransformA.q));
vertex.WA = Transform.Multiply(input.ProxyA.Vertices[vertex.IndexA], ref input.TransformA);
vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.IndexB = input.ProxyB.GetSupport( Complex.Divide(ref d, ref input.TransformB.q));
vertex.WB = Transform.Multiply(input.ProxyB.Vertices[vertex.IndexB], ref input.TransformB);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
Libraries/Farseer Physics Engine 3.5/Collision/DynamicTree.cs
+215 -125
View File
@@ -1,4 +1,11 @@
/*
// Copyright (c) 2018 Kastellanos Nikolaos
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -24,6 +31,7 @@ using System;
using System.Collections.Generic;
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision
@@ -31,7 +39,7 @@ namespace FarseerPhysics.Collision
/// <summary>
/// A node in the dynamic tree. The client does not interact with this directly.
/// </summary>
internal class TreeNode<T>
internal struct TreeNode<T>
{
/// <summary>
/// Enlarged AABB
@@ -41,8 +49,12 @@ namespace FarseerPhysics.Collision
internal int Child1;
internal int Child2;
// leaf = 0, free node = -1
internal int Height;
internal int ParentOrNext;
public object Body;
internal T UserData;
internal bool IsLeaf()
@@ -85,13 +97,11 @@ namespace FarseerPhysics.Collision
// Build a linked list for the free list.
for (int i = 0; i < _nodeCapacity - 1; ++i)
{
_nodes[i] = new TreeNode<T>();
_nodes[i].ParentOrNext = i + 1;
_nodes[i].Height = 1;
_nodes[i].Height = -1;
}
_nodes[_nodeCapacity - 1] = new TreeNode<T>();
_nodes[_nodeCapacity - 1].ParentOrNext = NullNode;
_nodes[_nodeCapacity - 1].Height = 1;
_nodes[_nodeCapacity - 1].Height = -1;
_freeList = 0;
}
@@ -123,20 +133,20 @@ namespace FarseerPhysics.Collision
return 0.0f;
}
TreeNode<T> root = _nodes[_root];
float rootArea = root.AABB.Perimeter;
//TreeNode<T>* root = &_nodes[_root];
float rootArea = _nodes[_root].AABB.Perimeter;
float totalArea = 0.0f;
for (int i = 0; i < _nodeCapacity; ++i)
{
TreeNode<T> node = _nodes[i];
if (node.Height < 0)
//TreeNode<T>* node = &_nodes[i];
if (_nodes[i].Height < 0)
{
// Free node in pool
continue;
}
totalArea += node.AABB.Perimeter;
totalArea += _nodes[i].AABB.Perimeter;
}
return totalArea / rootArea;
@@ -154,16 +164,16 @@ namespace FarseerPhysics.Collision
int maxBalance = 0;
for (int i = 0; i < _nodeCapacity; ++i)
{
TreeNode<T> node = _nodes[i];
if (node.Height <= 1)
//TreeNode<T>* node = &_nodes[i];
if (_nodes[i].Height <= 1)
{
continue;
}
Debug.Assert(node.IsLeaf() == false);
Debug.Assert(_nodes[i].IsLeaf() == false);
int child1 = node.Child1;
int child2 = node.Child2;
int child1 = _nodes[i].Child1;
int child2 = _nodes[i].Child2;
int balance = Math.Abs(_nodes[child2].Height - _nodes[child1].Height);
maxBalance = Math.Max(maxBalance, balance);
}
@@ -180,7 +190,7 @@ namespace FarseerPhysics.Collision
/// <param name="aabb">The aabb.</param>
/// <param name="userData">The user data.</param>
/// <returns>Index of the created proxy</returns>
public int AddProxy(ref AABB aabb, T userData)
public int AddProxy(ref AABB aabb)
{
int proxyId = AllocateNode();
@@ -188,7 +198,6 @@ namespace FarseerPhysics.Collision
Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
_nodes[proxyId].AABB.LowerBound = aabb.LowerBound - r;
_nodes[proxyId].AABB.UpperBound = aabb.UpperBound + r;
_nodes[proxyId].UserData = userData;
_nodes[proxyId].Height = 0;
InsertLeaf(proxyId);
@@ -264,6 +273,18 @@ namespace FarseerPhysics.Collision
return true;
}
/// <summary>
/// Set proxy user data.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="proxyId">The proxy id.</param>
/// <param name="userData">The proxy user data.</param>
public void SetUserData(int proxyId, T userData, Body body)
{
_nodes[proxyId].UserData = userData;
_nodes[proxyId].Body = body;
}
/// <summary>
/// Get proxy user data.
/// </summary>
@@ -287,12 +308,75 @@ namespace FarseerPhysics.Collision
fatAABB = _nodes[proxyId].AABB;
}
/// <summary>
/// Get the fat AABB for a proxy.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <returns>The fat AABB.</returns>
public AABB GetFatAABB(int proxyId)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
return _nodes[proxyId].AABB;
}
public object GetBody(int proxyId)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
return _nodes[proxyId].Body;
}
/// <summary>
/// Test overlap of fat AABBs.
/// </summary>
/// <param name="proxyIdA">The proxy id A.</param>
/// <param name="proxyIdB">The proxy id B.</param>
public bool TestFatAABBOverlap(int proxyIdA, int proxyIdB)
{
Debug.Assert(0 <= proxyIdA && proxyIdA < _nodeCapacity);
Debug.Assert(0 <= proxyIdB && proxyIdB < _nodeCapacity);
return AABB.TestOverlap(ref _nodes[proxyIdA].AABB, ref _nodes[proxyIdB].AABB);
}
/// <summary>
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The aabb.</param>
public void Query(Func<int, bool> callback, ref AABB aabb, ref object body)
{
_queryStack.Clear();
_queryStack.Push(_root);
while (_queryStack.Count > 0)
{
int nodeId = _queryStack.Pop();
if (nodeId == NullNode)
{
continue;
}
//TreeNode<T>* node = &_nodes[nodeId];
if (!ReferenceEquals(_nodes[nodeId].Body, body) && AABB.TestOverlap(ref _nodes[nodeId].AABB, ref aabb))
{
if (_nodes[nodeId].IsLeaf())
{
bool proceed = callback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
_queryStack.Push(_nodes[nodeId].Child1);
_queryStack.Push(_nodes[nodeId].Child2);
}
}
}
}
public void Query(Func<int, bool> callback, ref AABB aabb)
{
_queryStack.Clear();
@@ -306,11 +390,10 @@ namespace FarseerPhysics.Collision
continue;
}
TreeNode<T> node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref aabb))
//TreeNode<T>* node = &_nodes[nodeId];
if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref aabb))
{
if (node.IsLeaf())
if (_nodes[nodeId].IsLeaf())
{
bool proceed = callback(nodeId);
if (proceed == false)
@@ -320,8 +403,8 @@ namespace FarseerPhysics.Collision
}
else
{
_queryStack.Push(node.Child1);
_queryStack.Push(node.Child2);
_queryStack.Push(_nodes[nodeId].Child1);
_queryStack.Push(_nodes[nodeId].Child2);
}
}
}
@@ -336,7 +419,8 @@ namespace FarseerPhysics.Collision
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input)
/// <param name="collisionCategory">The collision categories of the fixtures to raycast against.</param>
public void RayCast(IBroadPhase broadPhase, Func<RayCastInput, FixtureProxy, float> callback, ref RayCastInput input, Category collisionCategory = Category.All)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
@@ -371,31 +455,39 @@ namespace FarseerPhysics.Collision
continue;
}
TreeNode<T> node = _nodes[nodeId];
//TreeNode<T>* node = &_nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.AABB.Center;
Vector2 h = node.AABB.Extents;
Vector2 c = _nodes[nodeId].AABB.Center;
Vector2 h = _nodes[nodeId].AABB.Extents;
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
if (_nodes[nodeId].IsLeaf())
{
FixtureProxy proxy = broadPhase.GetProxy(nodeId);
if (collisionCategory != Category.All &&
//!collisionCategory.HasFlag(proxy.Fixture.CollisionCategories)
(collisionCategory & proxy.Fixture.CollisionCategories) == 0)
{
continue;
}
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, nodeId);
float value = callback(subInput, proxy);
if (value == 0.0f)
{
@@ -408,14 +500,14 @@ namespace FarseerPhysics.Collision
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Vector2.Min(p1, t);
segmentAABB.UpperBound = Vector2.Max(p1, t);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
}
else
{
_raycastStack.Push(node.Child1);
_raycastStack.Push(node.Child2);
_raycastStack.Push(_nodes[nodeId].Child1);
_raycastStack.Push(_nodes[nodeId].Child2);
}
}
}
@@ -437,11 +529,9 @@ namespace FarseerPhysics.Collision
// pointer becomes the "next" pointer.
for (int i = _nodeCount; i < _nodeCapacity - 1; ++i)
{
_nodes[i] = new TreeNode<T>();
_nodes[i].ParentOrNext = i + 1;
_nodes[i].Height = -1;
}
_nodes[_nodeCapacity - 1] = new TreeNode<T>();
_nodes[_nodeCapacity - 1].ParentOrNext = NullNode;
_nodes[_nodeCapacity - 1].Height = -1;
_freeList = _nodeCount;
@@ -675,48 +765,48 @@ namespace FarseerPhysics.Collision
{
Debug.Assert(iA != NullNode);
TreeNode<T> A = _nodes[iA];
if (A.IsLeaf() || A.Height < 2)
//TreeNode<T>* A = &_nodes[iA];
if (_nodes[iA].IsLeaf() || _nodes[iA].Height < 2)
{
return iA;
}
int iB = A.Child1;
int iC = A.Child2;
int iB = _nodes[iA].Child1;
int iC = _nodes[iA].Child2;
Debug.Assert(0 <= iB && iB < _nodeCapacity);
Debug.Assert(0 <= iC && iC < _nodeCapacity);
TreeNode<T> B = _nodes[iB];
TreeNode<T> C = _nodes[iC];
//TreeNode<T>* B = &_nodes[iB];
//TreeNode<T>* C = &_nodes[iC];
int balance = C.Height - B.Height;
int balance = _nodes[iC].Height - _nodes[iB].Height;
// Rotate C up
if (balance > 1)
{
int iF = C.Child1;
int iG = C.Child2;
TreeNode<T> F = _nodes[iF];
TreeNode<T> G = _nodes[iG];
int iF = _nodes[iC].Child1;
int iG = _nodes[iC].Child2;
//TreeNode<T>* F = &_nodes[iF];
//TreeNode<T>* G = &_nodes[iG];
Debug.Assert(0 <= iF && iF < _nodeCapacity);
Debug.Assert(0 <= iG && iG < _nodeCapacity);
// Swap A and C
C.Child1 = iA;
C.ParentOrNext = A.ParentOrNext;
A.ParentOrNext = iC;
_nodes[iC].Child1 = iA;
_nodes[iC].ParentOrNext = _nodes[iA].ParentOrNext;
_nodes[iA].ParentOrNext = iC;
// A's old parent should point to C
if (C.ParentOrNext != NullNode)
if (_nodes[iC].ParentOrNext != NullNode)
{
if (_nodes[C.ParentOrNext].Child1 == iA)
if (_nodes[_nodes[iC].ParentOrNext].Child1 == iA)
{
_nodes[C.ParentOrNext].Child1 = iC;
_nodes[_nodes[iC].ParentOrNext].Child1 = iC;
}
else
{
Debug.Assert(_nodes[C.ParentOrNext].Child2 == iA);
_nodes[C.ParentOrNext].Child2 = iC;
Debug.Assert(_nodes[_nodes[iC].ParentOrNext].Child2 == iA);
_nodes[_nodes[iC].ParentOrNext].Child2 = iC;
}
}
else
@@ -725,27 +815,27 @@ namespace FarseerPhysics.Collision
}
// Rotate
if (F.Height > G.Height)
if (_nodes[iF].Height > _nodes[iG].Height)
{
C.Child2 = iF;
A.Child2 = iG;
G.ParentOrNext = iA;
A.AABB.Combine(ref B.AABB, ref G.AABB);
C.AABB.Combine(ref A.AABB, ref F.AABB);
_nodes[iC].Child2 = iF;
_nodes[iA].Child2 = iG;
_nodes[iG].ParentOrNext = iA;
_nodes[iA].AABB.Combine(ref _nodes[iB].AABB, ref _nodes[iG].AABB);
_nodes[iC].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iF].AABB);
A.Height = 1 + Math.Max(B.Height, G.Height);
C.Height = 1 + Math.Max(A.Height, F.Height);
_nodes[iA].Height = 1 + Math.Max(_nodes[iB].Height, _nodes[iG].Height);
_nodes[iC].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iF].Height);
}
else
{
C.Child2 = iG;
A.Child2 = iF;
F.ParentOrNext = iA;
A.AABB.Combine(ref B.AABB, ref F.AABB);
C.AABB.Combine(ref A.AABB, ref G.AABB);
_nodes[iC].Child2 = iG;
_nodes[iA].Child2 = iF;
_nodes[iF].ParentOrNext = iA;
_nodes[iA].AABB.Combine(ref _nodes[iB].AABB, ref _nodes[iF].AABB);
_nodes[iC].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iG].AABB);
A.Height = 1 + Math.Max(B.Height, F.Height);
C.Height = 1 + Math.Max(A.Height, G.Height);
_nodes[iA].Height = 1 + Math.Max(_nodes[iB].Height, _nodes[iF].Height);
_nodes[iC].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iG].Height);
}
return iC;
@@ -754,29 +844,29 @@ namespace FarseerPhysics.Collision
// Rotate B up
if (balance < -1)
{
int iD = B.Child1;
int iE = B.Child2;
TreeNode<T> D = _nodes[iD];
TreeNode<T> E = _nodes[iE];
int iD = _nodes[iB].Child1;
int iE = _nodes[iB].Child2;
//TreeNode<T>* D = &_nodes[iD];
//TreeNode<T>* E = &_nodes[iE];
Debug.Assert(0 <= iD && iD < _nodeCapacity);
Debug.Assert(0 <= iE && iE < _nodeCapacity);
// Swap A and B
B.Child1 = iA;
B.ParentOrNext = A.ParentOrNext;
A.ParentOrNext = iB;
_nodes[iB].Child1 = iA;
_nodes[iB].ParentOrNext = _nodes[iA].ParentOrNext;
_nodes[iA].ParentOrNext = iB;
// A's old parent should point to B
if (B.ParentOrNext != NullNode)
if (_nodes[iB].ParentOrNext != NullNode)
{
if (_nodes[B.ParentOrNext].Child1 == iA)
if (_nodes[_nodes[iB].ParentOrNext].Child1 == iA)
{
_nodes[B.ParentOrNext].Child1 = iB;
_nodes[_nodes[iB].ParentOrNext].Child1 = iB;
}
else
{
Debug.Assert(_nodes[B.ParentOrNext].Child2 == iA);
_nodes[B.ParentOrNext].Child2 = iB;
Debug.Assert(_nodes[_nodes[iB].ParentOrNext].Child2 == iA);
_nodes[_nodes[iB].ParentOrNext].Child2 = iB;
}
}
else
@@ -785,27 +875,27 @@ namespace FarseerPhysics.Collision
}
// Rotate
if (D.Height > E.Height)
if (_nodes[iD].Height > _nodes[iE].Height)
{
B.Child2 = iD;
A.Child1 = iE;
E.ParentOrNext = iA;
A.AABB.Combine(ref C.AABB, ref E.AABB);
B.AABB.Combine(ref A.AABB, ref D.AABB);
_nodes[iB].Child2 = iD;
_nodes[iA].Child1 = iE;
_nodes[iE].ParentOrNext = iA;
_nodes[iA].AABB.Combine(ref _nodes[iC].AABB, ref _nodes[iE].AABB);
_nodes[iB].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iD].AABB);
A.Height = 1 + Math.Max(C.Height, E.Height);
B.Height = 1 + Math.Max(A.Height, D.Height);
_nodes[iA].Height = 1 + Math.Max(_nodes[iC].Height, _nodes[iE].Height);
_nodes[iB].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iD].Height);
}
else
{
B.Child2 = iE;
A.Child1 = iD;
D.ParentOrNext = iA;
A.AABB.Combine(ref C.AABB, ref D.AABB);
B.AABB.Combine(ref A.AABB, ref E.AABB);
_nodes[iB].Child2 = iE;
_nodes[iA].Child1 = iD;
_nodes[iD].ParentOrNext = iA;
_nodes[iA].AABB.Combine(ref _nodes[iC].AABB, ref _nodes[iD].AABB);
_nodes[iB].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iE].AABB);
A.Height = 1 + Math.Max(C.Height, D.Height);
B.Height = 1 + Math.Max(A.Height, E.Height);
_nodes[iA].Height = 1 + Math.Max(_nodes[iC].Height, _nodes[iD].Height);
_nodes[iB].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iE].Height);
}
return iB;
@@ -822,15 +912,15 @@ namespace FarseerPhysics.Collision
public int ComputeHeight(int nodeId)
{
Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
TreeNode<T> node = _nodes[nodeId];
//TreeNode<T>* node = &_nodes[nodeId];
if (node.IsLeaf())
if (_nodes[nodeId].IsLeaf())
{
return 0;
}
int height1 = ComputeHeight(node.Child1);
int height2 = ComputeHeight(node.Child2);
int height1 = ComputeHeight(_nodes[nodeId].Child1);
int height2 = ComputeHeight(_nodes[nodeId].Child2);
return 1 + Math.Max(height1, height2);
}
@@ -856,16 +946,16 @@ namespace FarseerPhysics.Collision
Debug.Assert(_nodes[index].ParentOrNext == NullNode);
}
TreeNode<T> node = _nodes[index];
//TreeNode<T>* node = &_nodes[index];
int child1 = node.Child1;
int child2 = node.Child2;
int child1 = _nodes[index].Child1;
int child2 = _nodes[index].Child2;
if (node.IsLeaf())
if (_nodes[index].IsLeaf())
{
Debug.Assert(child1 == NullNode);
Debug.Assert(child2 == NullNode);
Debug.Assert(node.Height == 0);
Debug.Assert(_nodes[index].Height == 0);
return;
}
@@ -886,16 +976,16 @@ namespace FarseerPhysics.Collision
return;
}
TreeNode<T> node = _nodes[index];
//TreeNode<T>* node = &_nodes[index];
int child1 = node.Child1;
int child2 = node.Child2;
int child1 = _nodes[index].Child1;
int child2 = _nodes[index].Child2;
if (node.IsLeaf())
if (_nodes[index].IsLeaf())
{
Debug.Assert(child1 == NullNode);
Debug.Assert(child2 == NullNode);
Debug.Assert(node.Height == 0);
Debug.Assert(_nodes[index].Height == 0);
return;
}
@@ -905,13 +995,13 @@ namespace FarseerPhysics.Collision
int height1 = _nodes[child1].Height;
int height2 = _nodes[child2].Height;
int height = 1 + Math.Max(height1, height2);
Debug.Assert(node.Height == height);
Debug.Assert(_nodes[index].Height == height);
AABB AABB = new AABB();
AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB);
Debug.Assert(AABB.LowerBound == node.AABB.LowerBound);
Debug.Assert(AABB.UpperBound == node.AABB.UpperBound);
Debug.Assert(AABB.LowerBound == _nodes[index].AABB.LowerBound);
Debug.Assert(AABB.UpperBound == _nodes[index].AABB.UpperBound);
ValidateMetrics(child1);
ValidateMetrics(child2);
@@ -993,19 +1083,19 @@ namespace FarseerPhysics.Collision
int index1 = nodes[iMin];
int index2 = nodes[jMin];
TreeNode<T> child1 = _nodes[index1];
TreeNode<T> child2 = _nodes[index2];
//TreeNode<T>* child1 = &_nodes[index1];
//TreeNode<T>* child2 = &_nodes[index2];
int parentIndex = AllocateNode();
TreeNode<T> parent = _nodes[parentIndex];
parent.Child1 = index1;
parent.Child2 = index2;
parent.Height = 1 + Math.Max(child1.Height, child2.Height);
parent.AABB.Combine(ref child1.AABB, ref child2.AABB);
parent.ParentOrNext = NullNode;
//TreeNode<T>* parent = &_nodes[parentIndex];
_nodes[parentIndex].Child1 = index1;
_nodes[parentIndex].Child2 = index2;
_nodes[parentIndex].Height = 1 + Math.Max(_nodes[index1].Height, _nodes[index2].Height);
_nodes[parentIndex].AABB.Combine(ref _nodes[index1].AABB, ref _nodes[index2].AABB);
_nodes[parentIndex].ParentOrNext = NullNode;
child1.ParentOrNext = parentIndex;
child2.ParentOrNext = parentIndex;
_nodes[index1].ParentOrNext = parentIndex;
_nodes[index2].ParentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
Libraries/Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs
+39 -22
View File
@@ -1,4 +1,9 @@
/*
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -21,6 +26,7 @@
*/
using System;
using System.Collections.Generic;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
@@ -35,17 +41,17 @@ namespace FarseerPhysics.Collision
public int CompareTo(Pair other)
{
if (ProxyIdA < other.ProxyIdA)
if (ProxyIdB < other.ProxyIdB)
{
return -1;
}
if (ProxyIdA == other.ProxyIdA)
if (ProxyIdB == other.ProxyIdB)
{
if (ProxyIdB < other.ProxyIdB)
if (ProxyIdA < other.ProxyIdA)
{
return -1;
}
if (ProxyIdB == other.ProxyIdB)
if (ProxyIdA == other.ProxyIdA)
{
return 0;
}
@@ -109,11 +115,12 @@ namespace FarseerPhysics.Collision
/// </summary>
/// <param name="proxy">The user data.</param>
/// <returns></returns>
public int AddProxy(ref FixtureProxy proxy)
public int AddProxy(ref AABB aabb)
{
int proxyId = _tree.AddProxy(ref proxy.AABB, proxy);
int proxyId = _tree.AddProxy(ref aabb);
++_proxyCount;
BufferMove(proxyId);
return proxyId;
}
@@ -206,6 +213,11 @@ namespace FarseerPhysics.Collision
_tree.GetFatAABB(proxyId, out aabb);
}
public void SetProxy(int proxyId, ref FixtureProxy proxy)
{
_tree.SetUserData(proxyId, proxy, proxy.Body);
}
/// <summary>
/// Get user data from a proxy. Returns null if the id is invalid.
/// </summary>
@@ -224,12 +236,11 @@ namespace FarseerPhysics.Collision
/// <returns></returns>
public bool TestOverlap(int proxyIdA, int proxyIdB)
{
AABB aabbA, aabbB;
_tree.GetFatAABB(proxyIdA, out aabbA);
_tree.GetFatAABB(proxyIdB, out aabbB);
return AABB.TestOverlap(ref aabbA, ref aabbB);
return _tree.TestFatAABBOverlap(proxyIdA, proxyIdB);
}
private readonly HashSet<int> processedPairs = new HashSet<int>();
/// <summary>
/// Update the pairs. This results in pair callbacks. This can only add pairs.
/// </summary>
@@ -250,32 +261,37 @@ namespace FarseerPhysics.Collision
// We have to query the tree with the fat AABB so that
// we don't fail to create a pair that may touch later.
AABB fatAABB;
_tree.GetFatAABB(_queryProxyId, out fatAABB);
AABB fatAABB = _tree.GetFatAABB(_queryProxyId);
object body = _tree.GetBody(_queryProxyId);
// Query tree, create pairs and add them pair buffer.
_tree.Query(_queryCallback, ref fatAABB);
_tree.Query(_queryCallback, ref fatAABB, ref body);
}
// Reset move buffer
_moveCount = 0;
// Sort the pair buffer to expose duplicates.
Array.Sort(_pairBuffer, 0, _pairCount);
//Array.Sort(_pairBuffer, 0, _pairCount);
processedPairs.Clear();
// Send the pairs back to the client.
int i = 0;
while (i < _pairCount)
{
Pair primaryPair = _pairBuffer[i];
FixtureProxy userDataA = _tree.GetUserData(primaryPair.ProxyIdA);
FixtureProxy userDataB = _tree.GetUserData(primaryPair.ProxyIdB);
int pairID = primaryPair.ProxyIdA + (primaryPair.ProxyIdB << 16);
if (!processedPairs.Contains(pairID))
{
callback(primaryPair.ProxyIdA, primaryPair.ProxyIdB);
processedPairs.Add(pairID);
}
callback(ref userDataA, ref userDataB);
++i;
// Skip any duplicate pairs.
while (i < _pairCount)
/*while (i < _pairCount)
{
Pair pair = _pairBuffer[i];
if (pair.ProxyIdA != primaryPair.ProxyIdA || pair.ProxyIdB != primaryPair.ProxyIdB)
@@ -283,7 +299,7 @@ namespace FarseerPhysics.Collision
break;
}
++i;
}
}*/
}
// Try to keep the tree balanced.
@@ -310,9 +326,10 @@ namespace FarseerPhysics.Collision
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input)
/// <param name="collisionCategory">The collision categories of the fixtures to raycast against.</param>
public void RayCast(Func<RayCastInput, FixtureProxy, float> callback, ref RayCastInput input, Category collisionCategory = Category.All)
{
_tree.RayCast(callback, ref input);
_tree.RayCast(this, callback, ref input, collisionCategory);
}
public void ShiftOrigin(Vector2 newOrigin)
Libraries/Farseer Physics Engine 3.5/Collision/IBroadPhase.cs
+10 -3
View File
@@ -1,4 +1,9 @@
using System;
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
using System;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
@@ -11,12 +16,14 @@ namespace FarseerPhysics.Collision
bool TestOverlap(int proxyIdA, int proxyIdB);
int AddProxy(ref FixtureProxy proxy);
int AddProxy(ref AABB aabb);
void RemoveProxy(int proxyId);
void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement);
void SetProxy(int proxyId, ref FixtureProxy proxy);
FixtureProxy GetProxy(int proxyId);
void TouchProxy(int proxyId);
@@ -25,7 +32,7 @@ namespace FarseerPhysics.Collision
void Query(Func<int, bool> callback, ref AABB aabb);
void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input);
void RayCast(Func<RayCastInput, FixtureProxy, float> callback, ref RayCastInput input, Category collisionCategory = Category.All);
void ShiftOrigin(Vector2 newOrigin);
}
Libraries/Farseer Physics Engine 3.5/Collision/QuadTree.cs
+276
View File
@@ -0,0 +1,276 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
using System;
using System.Collections.Generic;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision
{
public class Element<T>
{
public QuadTree<T> Parent;
public AABB Span;
public T Value;
public Element(AABB span)
{
Span = span;
Value = default(T);
Parent = null;
}
}
public class QuadTree<T>
{
public int MaxBucket;
public int MaxDepth;
public List<Element<T>> Nodes;
public AABB Span;
public QuadTree<T>[] SubTrees;
public QuadTree(AABB span, int maxbucket, int maxdepth)
{
Span = span;
Nodes = new List<Element<T>>();
MaxBucket = maxbucket;
MaxDepth = maxdepth;
}
public bool IsPartitioned
{
get { return SubTrees != null; }
}
/// <summary>
/// returns the quadrant of span that entirely contains test. if none, return 0.
/// </summary>
/// <param name="span"></param>
/// <param name="test"></param>
/// <returns></returns>
private int Partition(AABB span, AABB test)
{
if (span.Q1.Contains(ref test)) return 1;
if (span.Q2.Contains(ref test)) return 2;
if (span.Q3.Contains(ref test)) return 3;
if (span.Q4.Contains(ref test)) return 4;
return 0;
}
public void AddNode(Element<T> node)
{
if (!IsPartitioned)
{
if (Nodes.Count >= MaxBucket && MaxDepth > 0) //bin is full and can still subdivide
{
//
//partition into quadrants and sort existing nodes amonst quads.
//
Nodes.Add(node); //treat new node just like other nodes for partitioning
SubTrees = new QuadTree<T>[4];
SubTrees[0] = new QuadTree<T>(Span.Q1, MaxBucket, MaxDepth - 1);
SubTrees[1] = new QuadTree<T>(Span.Q2, MaxBucket, MaxDepth - 1);
SubTrees[2] = new QuadTree<T>(Span.Q3, MaxBucket, MaxDepth - 1);
SubTrees[3] = new QuadTree<T>(Span.Q4, MaxBucket, MaxDepth - 1);
List<Element<T>> remNodes = new List<Element<T>>();
//nodes that are not fully contained by any quadrant
foreach (Element<T> n in Nodes)
{
switch (Partition(Span, n.Span))
{
case 1: //quadrant 1
SubTrees[0].AddNode(n);
break;
case 2:
SubTrees[1].AddNode(n);
break;
case 3:
SubTrees[2].AddNode(n);
break;
case 4:
SubTrees[3].AddNode(n);
break;
default:
n.Parent = this;
remNodes.Add(n);
break;
}
}
Nodes = remNodes;
}
else
{
node.Parent = this;
Nodes.Add(node);
//if bin is not yet full or max depth has been reached, just add the node without subdividing
}
}
else //we already have children nodes
{
//
//add node to specific sub-tree
//
switch (Partition(Span, node.Span))
{
case 1: //quadrant 1
SubTrees[0].AddNode(node);
break;
case 2:
SubTrees[1].AddNode(node);
break;
case 3:
SubTrees[2].AddNode(node);
break;
case 4:
SubTrees[3].AddNode(node);
break;
default:
node.Parent = this;
Nodes.Add(node);
break;
}
}
}
/// <summary>
/// tests if ray intersects AABB
/// </summary>
/// <param name="aabb"></param>
/// <returns></returns>
public static bool RayCastAABB(AABB aabb, Vector2 p1, Vector2 p2)
{
AABB segmentAABB = new AABB();
{
Vector2.Min(ref p1, ref p2, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref p2, out segmentAABB.UpperBound);
}
if (!AABB.TestOverlap(ref aabb, ref segmentAABB)) return false;
Vector2 rayDir = p2 - p1;
Vector2 rayPos = p1;
Vector2 norm = new Vector2(-rayDir.Y, rayDir.X); //normal to ray
if (norm.Length() == 0.0f)
return true; //if ray is just a point, return true (iff point is within aabb, as tested earlier)
norm.Normalize();
float dPos = Vector2.Dot(rayPos, norm);
var verts = aabb.Vertices;
float d0 = Vector2.Dot(verts[0], norm) - dPos;
for (int i = 1; i < 4; i++)
{
float d = Vector2.Dot(verts[i], norm) - dPos;
if (Math.Sign(d) != Math.Sign(d0))
//return true if the ray splits the vertices (ie: sign of dot products with normal are not all same)
return true;
}
return false;
}
public void QueryAABB(Func<Element<T>, bool> callback, ref AABB searchR)
{
Stack<QuadTree<T>> stack = new Stack<QuadTree<T>>();
stack.Push(this);
while (stack.Count > 0)
{
QuadTree<T> qt = stack.Pop();
if (!AABB.TestOverlap(ref searchR, ref qt.Span))
continue;
foreach (Element<T> n in qt.Nodes)
if (AABB.TestOverlap(ref searchR, ref n.Span))
{
if (!callback(n)) return;
}
if (qt.IsPartitioned)
foreach (QuadTree<T> st in qt.SubTrees)
stack.Push(st);
}
}
public void RayCast(Func<RayCastInput, Element<T>, float> callback, ref RayCastInput input, Category collisionCategory = Category.All)
{
Stack<QuadTree<T>> stack = new Stack<QuadTree<T>>();
stack.Push(this);
float maxFraction = input.MaxFraction;
Vector2 p1 = input.Point1;
Vector2 p2 = p1 + (input.Point2 - input.Point1) * maxFraction;
while (stack.Count > 0)
{
QuadTree<T> qt = stack.Pop();
if (!RayCastAABB(qt.Span, p1, p2))
continue;
foreach (Element<T> n in qt.Nodes)
{
if (!RayCastAABB(n.Span, p1, p2))
continue;
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, n);
if (value == 0.0f)
return; // the client has terminated the raycast.
if (value <= 0.0f)
continue;
maxFraction = value;
p2 = p1 + (input.Point2 - input.Point1) * maxFraction; //update segment endpoint
}
if (qt.IsPartitioned)
foreach (QuadTree<T> st in qt.SubTrees)
stack.Push(st);
}
}
public void GetAllNodesR(ref List<Element<T>> nodes)
{
nodes.AddRange(Nodes);
if (IsPartitioned)
foreach (QuadTree<T> st in SubTrees) st.GetAllNodesR(ref nodes);
}
public void RemoveNode(Element<T> node)
{
node.Parent.Nodes.Remove(node);
}
public void Reconstruct()
{
List<Element<T>> allNodes = new List<Element<T>>();
GetAllNodesR(ref allNodes);
Clear();
foreach (var node in allNodes)
AddNode(node);
}
public void Clear()
{
Nodes.Clear();
SubTrees = null;
}
}
}
Libraries/Farseer Physics Engine 3.5/Collision/QuadTreeBroadPhase.cs
+261
View File
@@ -0,0 +1,261 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using FarseerPhysics.Dynamics;
namespace FarseerPhysics.Collision
{
public class QuadTreeBroadPhase : IBroadPhase
{
private const int TreeUpdateThresh = 10000;
private int _currId;
private Dictionary<int, Element<FixtureProxy>> _idRegister;
private List<Element<FixtureProxy>> _moveBuffer;
private List<Pair> _pairBuffer;
private QuadTree<FixtureProxy> _quadTree;
private int _treeMoveNum;
/// <summary>
/// Creates a new quad tree broadphase with the specified span.
/// </summary>
/// <param name="span">the maximum span of the tree (world size)</param>
public QuadTreeBroadPhase(AABB span)
{
_quadTree = new QuadTree<FixtureProxy>(span, 5, 10);
_idRegister = new Dictionary<int, Element<FixtureProxy>>();
_moveBuffer = new List<Element<FixtureProxy>>();
_pairBuffer = new List<Pair>();
}
#region IBroadPhase Members
///<summary>
/// The number of proxies
///</summary>
public int ProxyCount
{
get { return _idRegister.Count; }
}
public void GetFatAABB(int proxyID, out AABB aabb)
{
if (_idRegister.ContainsKey(proxyID))
aabb = _idRegister[proxyID].Span;
else
throw new KeyNotFoundException("proxyID not found in register");
}
public void UpdatePairs(BroadphaseDelegate callback)
{
_pairBuffer.Clear();
foreach (Element<FixtureProxy> qtnode in _moveBuffer)
{
// Query tree, create pairs and add them pair buffer.
Query(proxyID => PairBufferQueryCallback(proxyID, qtnode.Value.ProxyId), ref qtnode.Span);
}
_moveBuffer.Clear();
// Sort the pair buffer to expose duplicates.
_pairBuffer.Sort();
// Send the pairs back to the client.
int i = 0;
while (i < _pairBuffer.Count)
{
Pair primaryPair = _pairBuffer[i];
callback(primaryPair.ProxyIdA, primaryPair.ProxyIdB);
++i;
// Skip any duplicate pairs.
while (i < _pairBuffer.Count && _pairBuffer[i].ProxyIdA == primaryPair.ProxyIdA &&
_pairBuffer[i].ProxyIdB == primaryPair.ProxyIdB)
++i;
}
}
/// <summary>
/// Test overlap of fat AABBs.
/// </summary>
/// <param name="proxyIdA">The proxy id A.</param>
/// <param name="proxyIdB">The proxy id B.</param>
/// <returns></returns>
public bool TestOverlap(int proxyIdA, int proxyIdB)
{
AABB aabb1;
AABB aabb2;
GetFatAABB(proxyIdA, out aabb1);
GetFatAABB(proxyIdB, out aabb2);
return AABB.TestOverlap(ref aabb1, ref aabb2);
}
public int AddProxy(ref AABB uaabb)
{
int proxyId = _currId++;
AABB aabb = Fatten(ref uaabb);
Element<FixtureProxy> qtnode = new Element<FixtureProxy>(aabb);
_idRegister.Add(proxyId, qtnode);
_quadTree.AddNode(qtnode);
return proxyId;
}
public void RemoveProxy(int proxyId)
{
if (_idRegister.ContainsKey(proxyId))
{
Element<FixtureProxy> qtnode = _idRegister[proxyId];
UnbufferMove(qtnode);
_idRegister.Remove(proxyId);
_quadTree.RemoveNode(qtnode);
}
else
throw new KeyNotFoundException("proxyID not found in register");
}
public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement)
{
AABB fatAABB;
GetFatAABB(proxyId, out fatAABB);
//exit if movement is within fat aabb
if (fatAABB.Contains(ref aabb))
return;
// Extend AABB.
AABB b = aabb;
Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
b.LowerBound = b.LowerBound - r;
b.UpperBound = b.UpperBound + r;
// Predict AABB displacement.
Vector2 d = Settings.AABBMultiplier * displacement;
if (d.X < 0.0f)
b.LowerBound.X += d.X;
else
b.UpperBound.X += d.X;
if (d.Y < 0.0f)
b.LowerBound.Y += d.Y;
else
b.UpperBound.Y += d.Y;
Element<FixtureProxy> qtnode = _idRegister[proxyId];
qtnode.Value.AABB = b; //not neccesary for QTree, but might be accessed externally
qtnode.Span = b;
ReinsertNode(qtnode);
BufferMove(qtnode);
}
public void SetProxy(int proxyId, ref FixtureProxy proxy)
{
_idRegister[proxyId].Value = proxy;
}
public FixtureProxy GetProxy(int proxyId)
{
if (_idRegister.ContainsKey(proxyId))
return _idRegister[proxyId].Value;
throw new KeyNotFoundException("proxyID not found in register");
}
public void TouchProxy(int proxyId)
{
if (_idRegister.ContainsKey(proxyId))
BufferMove(_idRegister[proxyId]);
else
throw new KeyNotFoundException("proxyID not found in register");
}
public void Query(Func<int, bool> callback, ref AABB query)
{
_quadTree.QueryAABB(TransformPredicate(callback), ref query);
}
public void RayCast(Func<RayCastInput, FixtureProxy, float> callback, ref RayCastInput input, Category collisionCategory = Category.All)
{
_quadTree.RayCast(TransformRayCallback(callback), ref input, collisionCategory);
}
public void ShiftOrigin(Vector2 newOrigin)
{
//TODO
}
#endregion
private AABB Fatten(ref AABB aabb)
{
Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
return new AABB(aabb.LowerBound - r, aabb.UpperBound + r);
}
private Func<Element<FixtureProxy>, bool> TransformPredicate(Func<int, bool> idPredicate)
{
Func<Element<FixtureProxy>, bool> qtPred = qtnode => idPredicate(qtnode.Value.ProxyId);
return qtPred;
}
private Func<RayCastInput, Element<FixtureProxy>, float> TransformRayCallback(Func<RayCastInput, FixtureProxy, float> callback)
{
Func<RayCastInput, Element<FixtureProxy>, float> newCallback =
(input, qtnode) => callback(input, qtnode.Value);
return newCallback;
}
private bool PairBufferQueryCallback(int proxyId, int baseId)
{
// A proxy cannot form a pair with itself.
if (proxyId == baseId)
return true;
Pair p = new Pair();
p.ProxyIdA = Math.Min(proxyId, baseId);
p.ProxyIdB = Math.Max(proxyId, baseId);
_pairBuffer.Add(p);
return true;
}
private void ReconstructTree()
{
//this is faster than _quadTree.Reconstruct(), since the quadtree method runs a recusive query to find all nodes.
_quadTree.Clear();
foreach (Element<FixtureProxy> elem in _idRegister.Values)
_quadTree.AddNode(elem);
}
private void ReinsertNode(Element<FixtureProxy> qtnode)
{
_quadTree.RemoveNode(qtnode);
_quadTree.AddNode(qtnode);
if (++_treeMoveNum > TreeUpdateThresh)
{
ReconstructTree();
_treeMoveNum = 0;
}
}
private void BufferMove(Element<FixtureProxy> proxy)
{
_moveBuffer.Add(proxy);
}
private void UnbufferMove(Element<FixtureProxy> proxy)
{
_moveBuffer.Remove(proxy);
}
}
}
Libraries/Farseer Physics Engine 3.5/Collision/Shapes/ChainShape.cs
+11 -7
View File
@@ -1,3 +1,8 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
@@ -63,8 +68,8 @@ namespace FarseerPhysics.Collision.Shapes
{
ShapeType = ShapeType.Chain;
_radius = Settings.PolygonRadius;
Debug.Assert(vertices != null && vertices.Count >= 2);
Debug.Assert(vertices != null && vertices.Count >= 3);
Debug.Assert(vertices[0] != vertices[vertices.Count - 1]); // FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363
for (int i = 1; i < vertices.Count; ++i)
@@ -73,7 +78,6 @@ namespace FarseerPhysics.Collision.Shapes
Vector2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
Debug.Assert(Vector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop);
}
@@ -208,11 +212,11 @@ namespace FarseerPhysics.Collision.Shapes
i2 = 0;
}
Vector2 v1 = MathUtils.Mul(ref transform, Vertices[i1]);
Vector2 v2 = MathUtils.Mul(ref transform, Vertices[i2]);
Vector2 v1 = Transform.Multiply(Vertices[i1], ref transform);
Vector2 v2 = Transform.Multiply(Vertices[i2], ref transform);
aabb.LowerBound = Vector2.Min(v1, v2);
aabb.UpperBound = Vector2.Max(v1, v2);
Vector2.Min(ref v1, ref v2, out aabb.LowerBound);
Vector2.Max(ref v1, ref v2, out aabb.UpperBound);
}
protected override void ComputeProperties()
Libraries/Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs
+23 -10
View File
@@ -1,4 +1,9 @@
/*
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -23,6 +28,7 @@
using System;
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision.Shapes
@@ -78,7 +84,7 @@ namespace FarseerPhysics.Collision.Shapes
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
Vector2 center = transform.p + MathUtils.Mul(transform.q, Position);
Vector2 center = transform.p + Complex.Multiply(ref _position, ref transform.q);
Vector2 d = point - center;
return Vector2.Dot(d, d) <= _2radius;
}
@@ -92,7 +98,7 @@ namespace FarseerPhysics.Collision.Shapes
output = new RayCastOutput();
Vector2 position = transform.p + MathUtils.Mul(transform.q, Position);
Vector2 position = transform.p + Complex.Multiply(ref _position, ref transform.q);
Vector2 s = input.Point1 - position;
float b = Vector2.Dot(s, s) - _2radius;
@@ -128,14 +134,21 @@ namespace FarseerPhysics.Collision.Shapes
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 p = transform.p + MathUtils.Mul(transform.q, Position);
aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius);
aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius);
// OPT: Vector2 p = transform.p + Complex.Multiply(ref _position, ref transform.q);
var pX = (_position.X * transform.q.Real - _position.Y * transform.q.Imaginary) + transform.p.X;
var pY = (_position.Y * transform.q.Real + _position.X * transform.q.Imaginary) + transform.p.Y;
// OPT: aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius);
// OPT: aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius);
aabb.LowerBound.X = pX - Radius;
aabb.LowerBound.Y = pY - Radius;
aabb.UpperBound.X = pX + Radius;
aabb.UpperBound.Y = pY + Radius;
}
protected override sealed void ComputeProperties()
{
float area = Settings.Pi * _2radius;
float area = MathHelper.Pi * _2radius;
MassData.Area = area;
MassData.Mass = Density * area;
MassData.Centroid = Position;
@@ -148,7 +161,7 @@ namespace FarseerPhysics.Collision.Shapes
{
sc = Vector2.Zero;
Vector2 p = MathUtils.Mul(ref xf, Position);
Vector2 p = Transform.Multiply(ref _position, ref xf);
float l = -(Vector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
@@ -159,12 +172,12 @@ namespace FarseerPhysics.Collision.Shapes
{
//Completely wet
sc = p;
return Settings.Pi * _2radius;
return MathHelper.Pi * _2radius;
}
//Magic
float l2 = l * l;
float area = _2radius * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(_2radius - l2));
float area = _2radius * (float)((Math.Asin(l / Radius) + MathHelper.Pi / 2) + l * Math.Sqrt(_2radius - l2));
float com = -2.0f / 3.0f * (float)Math.Pow(_2radius - l2, 1.5f) / area;
sc.X = p.X + normal.X * com;
Libraries/Farseer Physics Engine 3.5/Collision/Shapes/EdgeShape.cs
+43 -9
View File
@@ -1,3 +1,8 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
@@ -21,6 +26,7 @@
*/
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision.Shapes
@@ -143,8 +149,8 @@ namespace FarseerPhysics.Collision.Shapes
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
Vector2 p1 = Complex.Divide(input.Point1 - transform.p, ref transform.q);
Vector2 p2 = Complex.Divide(input.Point2 - transform.p, ref transform.q);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
@@ -201,15 +207,43 @@ namespace FarseerPhysics.Collision.Shapes
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 v1 = MathUtils.Mul(ref transform, _vertex1);
Vector2 v2 = MathUtils.Mul(ref transform, _vertex2);
// OPT: Vector2 v1 = Transform.Multiply(ref _vertex1, ref transform);
float v1X = (_vertex1.X * transform.q.Real - _vertex1.Y * transform.q.Imaginary) + transform.p.X;
float v1Y = (_vertex1.Y * transform.q.Real + _vertex1.X * transform.q.Imaginary) + transform.p.Y;
// OPT: Vector2 v2 = Transform.Multiply(ref _vertex2, ref transform);
float v2X = (_vertex2.X * transform.q.Real - _vertex2.Y * transform.q.Imaginary) + transform.p.X;
float v2Y = (_vertex2.Y * transform.q.Real + _vertex2.X * transform.q.Imaginary) + transform.p.Y;
Vector2 lower = Vector2.Min(v1, v2);
Vector2 upper = Vector2.Max(v1, v2);
// OPT: aabb.LowerBound = Vector2.Min(v1, v2);
// OPT: aabb.UpperBound = Vector2.Max(v1, v2);
if (v1X < v2X)
{
aabb.LowerBound.X = v1X;
aabb.UpperBound.X = v2X;
}
else
{
aabb.LowerBound.X = v2X;
aabb.UpperBound.X = v1X;
}
if (v1Y < v2Y)
{
aabb.LowerBound.Y = v1Y;
aabb.UpperBound.Y = v2Y;
}
else
{
aabb.LowerBound.Y = v2Y;
aabb.UpperBound.Y = v1Y;
}
Vector2 r = new Vector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
// OPT: Vector2 r = new Vector2(Radius, Radius);
// OPT: aabb.LowerBound = aabb.LowerBound - r;
// OPT: aabb.UpperBound = aabb.LowerBound + r;
aabb.LowerBound.X -= Radius;
aabb.LowerBound.Y -= Radius;
aabb.UpperBound.X += Radius;
aabb.UpperBound.Y += Radius;
}
protected override void ComputeProperties()
Libraries/Farseer Physics Engine 3.5/Collision/Shapes/PolygonShape.cs
+41 -18
View File
@@ -1,3 +1,8 @@
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
@@ -23,6 +28,7 @@
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.ConvexHull;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision.Shapes
@@ -87,7 +93,7 @@ namespace FarseerPhysics.Collision.Shapes
{
_vertices = new Vertices(value);
//Debug.Assert(_vertices.Count >= 3 && _vertices.Count <= Settings.MaxPolygonVertices);
Debug.Assert(_vertices.Count >= 3 && _vertices.Count <= Settings.MaxPolygonVertices);
if (Settings.UseConvexHullPolygons)
{
@@ -179,7 +185,7 @@ namespace FarseerPhysics.Collision.Shapes
Vector2 e1 = Vertices[i] - s;
Vector2 e2 = i + 1 < Vertices.Count ? Vertices[i + 1] - s : Vertices[0] - s;
float D = MathUtils.Cross(e1, e2);
float D = MathUtils.Cross(ref e1, ref e2);
float triangleArea = 0.5f * D;
area += triangleArea;
@@ -218,7 +224,7 @@ namespace FarseerPhysics.Collision.Shapes
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
Vector2 pLocal = MathUtils.MulT(transform.q, point - transform.p);
Vector2 pLocal = Complex.Divide(point - transform.p, ref transform.q);
for (int i = 0; i < Vertices.Count; ++i)
{
@@ -237,8 +243,8 @@ namespace FarseerPhysics.Collision.Shapes
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
Vector2 p1 = Complex.Divide(input.Point1 - transform.p, ref transform.q);
Vector2 p2 = Complex.Divide(input.Point2 - transform.p, ref transform.q);
Vector2 d = p2 - p1;
float lower = 0.0f, upper = input.MaxFraction;
@@ -296,7 +302,7 @@ namespace FarseerPhysics.Collision.Shapes
if (index >= 0)
{
output.Fraction = lower;
output.Normal = MathUtils.Mul(transform.q, Normals[index]);
output.Normal = Complex.Multiply(Normals[index], ref transform.q);
return true;
}
@@ -311,19 +317,36 @@ namespace FarseerPhysics.Collision.Shapes
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 lower = MathUtils.Mul(ref transform, Vertices[0]);
Vector2 upper = lower;
// OPT: aabb.LowerBound = Transform.Multiply(Vertices[0], ref transform);
var vert = Vertices[0];
aabb.LowerBound.X = (vert.X * transform.q.Real - vert.Y * transform.q.Imaginary) + transform.p.X;
aabb.LowerBound.Y = (vert.Y * transform.q.Real + vert.X * transform.q.Imaginary) + transform.p.Y;
aabb.UpperBound = aabb.LowerBound;
for (int i = 1; i < Vertices.Count; ++i)
{
Vector2 v = MathUtils.Mul(ref transform, Vertices[i]);
lower = Vector2.Min(lower, v);
upper = Vector2.Max(upper, v);
// OPT: Vector2 v = Transform.Multiply(Vertices[i], ref transform);
vert = Vertices[i];
float vX = (vert.X * transform.q.Real - vert.Y * transform.q.Imaginary) + transform.p.X;
float vY = (vert.Y * transform.q.Real + vert.X * transform.q.Imaginary) + transform.p.Y;
// OPT: Vector2.Min(ref aabb.LowerBound, ref v, out aabb.LowerBound);
// OPT: Vector2.Max(ref aabb.UpperBound, ref v, out aabb.UpperBound);
Debug.Assert(aabb.LowerBound.X <= aabb.UpperBound.X);
if (vX < aabb.LowerBound.X) aabb.LowerBound.X = vX;
else if (vX > aabb.UpperBound.X) aabb.UpperBound.X = vX;
Debug.Assert(aabb.LowerBound.Y <= aabb.UpperBound.Y);
if (vY < aabb.LowerBound.Y) aabb.LowerBound.Y = vY;
else if (vY > aabb.UpperBound.Y) aabb.UpperBound.Y = vY;
}
Vector2 r = new Vector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
// OPT: Vector2 r = new Vector2(Radius, Radius);
// OPT: aabb.LowerBound = aabb.LowerBound - r;
// OPT: aabb.UpperBound = aabb.UpperBound + r;
aabb.LowerBound.X -= Radius;
aabb.LowerBound.Y -= Radius;
aabb.UpperBound.X += Radius;
aabb.UpperBound.Y += Radius;
}
public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc)
@@ -331,7 +354,7 @@ namespace FarseerPhysics.Collision.Shapes
sc = Vector2.Zero;
//Transform plane into shape co-ordinates
Vector2 normalL = MathUtils.MulT(xf.q, normal);
Vector2 normalL = Complex.Divide(ref normal, ref xf.q);
float offsetL = offset - Vector2.Dot(normal, xf.p);
float[] depths = new float[Settings.MaxPolygonVertices];
@@ -372,7 +395,7 @@ namespace FarseerPhysics.Collision.Shapes
if (lastSubmerged)
{
//Completely submerged
sc = MathUtils.Mul(ref xf, MassData.Centroid);
sc = Transform.Multiply(MassData.Centroid, ref xf);
return MassData.Mass / Density;
}
@@ -421,7 +444,7 @@ namespace FarseerPhysics.Collision.Shapes
Vector2 e1 = p2 - intoVec;
Vector2 e2 = p3 - intoVec;
float D = MathUtils.Cross(e1, e2);
float D = MathUtils.Cross(ref e1, ref e2);
float triangleArea = 0.5f * D;
@@ -437,7 +460,7 @@ namespace FarseerPhysics.Collision.Shapes
//Normalize and transform centroid
center *= 1.0f / area;
sc = MathUtils.Mul(ref xf, center);
sc = Transform.Multiply(ref center, ref xf);
return area;
}
Libraries/Farseer Physics Engine 3.5/Collision/Shapes/Shape.cs
+6 -23
View File
@@ -1,4 +1,9 @@
/*
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -225,28 +230,6 @@ namespace FarseerPhysics.Collision.Shapes
/// </summary>
protected abstract void ComputeProperties();
/// <summary>
/// Compare this shape to another shape based on type and properties.
/// </summary>
/// <param name="shape">The other shape</param>
/// <returns>True if the two shapes are the same.</returns>
public bool CompareTo(Shape shape)
{
if (shape is PolygonShape && this is PolygonShape)
return ((PolygonShape)this).CompareTo((PolygonShape)shape);
if (shape is CircleShape && this is CircleShape)
return ((CircleShape)this).CompareTo((CircleShape)shape);
if (shape is EdgeShape && this is EdgeShape)
return ((EdgeShape)this).CompareTo((EdgeShape)shape);
if (shape is ChainShape && this is ChainShape)
return ((ChainShape)this).CompareTo((ChainShape)shape);
return false;
}
/// <summary>
/// Used for the buoyancy controller
/// </summary>
Libraries/Farseer Physics Engine 3.5/Collision/TimeOfImpact.cs
+47 -47
View File
@@ -1,4 +1,9 @@
/*
/* Original source Farseer Physics Engine:
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
* Microsoft Permissive License (Ms-PL) v1.1
*/
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
@@ -23,6 +28,7 @@
using System;
using System.Diagnostics;
using FarseerPhysics.Common;
using FarseerPhysics.Common.Maths;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Collision
@@ -32,8 +38,8 @@ namespace FarseerPhysics.Collision
/// </summary>
public class TOIInput
{
public DistanceProxy ProxyA = new DistanceProxy();
public DistanceProxy ProxyB = new DistanceProxy();
public DistanceProxy ProxyA;
public DistanceProxy ProxyB;
public Sweep SweepA;
public Sweep SweepB;
public float TMax; // defines sweep interval [0, tMax]
@@ -76,7 +82,7 @@ namespace FarseerPhysics.Collision
[ThreadStatic]
private static SeparationFunctionType _type;
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1)
public static void Set(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
@@ -96,8 +102,8 @@ namespace FarseerPhysics.Collision
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
_axis = pointB - pointA;
_axis.Normalize();
}
@@ -111,13 +117,13 @@ namespace FarseerPhysics.Collision
Vector2 a = localPointB2 - localPointB1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = MathUtils.Mul(ref xfB.q, _axis);
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
@@ -135,13 +141,13 @@ namespace FarseerPhysics.Collision
Vector2 a = localPointA2 - localPointA1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = MathUtils.Mul(ref xfA.q, _axis);
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
@@ -149,8 +155,6 @@ namespace FarseerPhysics.Collision
_axis = -_axis;
}
}
//FPE note: the returned value that used to be here has been removed, as it was not used.
}
public static float FindMinSeparation(out int indexA, out int indexB, float t)
@@ -163,8 +167,8 @@ namespace FarseerPhysics.Collision
{
case SeparationFunctionType.Points:
{
Vector2 axisA = MathUtils.MulT(ref xfA.q, _axis);
Vector2 axisB = MathUtils.MulT(ref xfB.q, -_axis);
Vector2 axisA = Complex.Divide(ref _axis, ref xfA.q);
Vector2 axisB = -Complex.Divide(ref _axis, ref xfB.q);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
@@ -172,8 +176,8 @@ namespace FarseerPhysics.Collision
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return separation;
@@ -181,16 +185,16 @@ namespace FarseerPhysics.Collision
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Mul(ref xfA.q, _axis);
Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal);
Vector2 axisB = -Complex.Divide(ref normal, ref xfB.q);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, normal);
return separation;
@@ -198,16 +202,16 @@ namespace FarseerPhysics.Collision
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Mul(ref xfB.q, _axis);
Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal);
Vector2 axisA = -Complex.Divide(ref normal, ref xfA.q);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float separation = Vector2.Dot(pointA - pointB, normal);
return separation;
@@ -223,8 +227,6 @@ namespace FarseerPhysics.Collision
public static float Evaluate(int indexA, int indexB, float t)
{
if (float.IsNaN(t)) return 0.0f;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
@@ -236,30 +238,30 @@ namespace FarseerPhysics.Collision
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, _axis);
return separation;
}
case SeparationFunctionType.FaceA:
{
Vector2 normal = MathUtils.Mul(ref xfA.q, _axis);
Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 localPointB = _proxyB.Vertices[indexB];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float separation = Vector2.Dot(pointB - pointA, normal);
return separation;
}
case SeparationFunctionType.FaceB:
{
Vector2 normal = MathUtils.Mul(ref xfB.q, _axis);
Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 localPointA = _proxyA.Vertices[indexA];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float separation = Vector2.Dot(pointA - pointB, normal);
return separation;
@@ -280,8 +282,6 @@ namespace FarseerPhysics.Collision
public static int TOICalls, TOIIters, TOIMaxIters;
[ThreadStatic]
public static int TOIRootIters, TOIMaxRootIters;
[ThreadStatic]
private static DistanceInput _distanceInput;
/// <summary>
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
@@ -292,7 +292,7 @@ namespace FarseerPhysics.Collision
/// </summary>
/// <param name="output">The output.</param>
/// <param name="input">The input.</param>
public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input)
{
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++TOICalls;
@@ -321,10 +321,10 @@ namespace FarseerPhysics.Collision
int iter = 0;
// Prepare input for distance query.
_distanceInput = _distanceInput ?? new DistanceInput();
_distanceInput.ProxyA = input.ProxyA;
_distanceInput.ProxyB = input.ProxyB;
_distanceInput.UseRadii = false;
DistanceInput distanceInput = new DistanceInput();
distanceInput.ProxyA = input.ProxyA;
distanceInput.ProxyB = input.ProxyB;
distanceInput.UseRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
@@ -336,11 +336,11 @@ namespace FarseerPhysics.Collision
// Get the distance between shapes. We can also use the results
// to get a separating axis.
_distanceInput.TransformA = xfA;
_distanceInput.TransformB = xfB;
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
SimplexCache cache;
Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput);
Distance.ComputeDistance(out distanceOutput, out cache, distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.Distance <= 0.0f)
@@ -359,7 +359,7 @@ namespace FarseerPhysics.Collision
break;
}
SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1);
SeparationFunction.Set(ref cache, ref input.ProxyA, ref sweepA, ref input.ProxyB, ref sweepB, t1);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.