internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Cdot = v + cross(w, r)
FVector2 Cdot = vB + MathUtils.Cross(wB, _rB);
FVector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
FVector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = data.step.dt * MaxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
vB += _invMassB * impulse;
wB += _invIB * MathUtils.Cross(_rB, impulse);
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/// <summary>
/// Copy vertices. This assumes the vertices define a convex polygon.
/// It is assumed that the exterior is the the right of each edge.
/// </summary>
/// @warning the points may be re-ordered, even if they form a convex polygon
/// @warning collinear points are handled but not removed. Collinear points
/// may lead to poor stacking behavior.
/// <param name="input">The vertices.</param>
public void Set(Vertices input)
{
Debug.Assert(input.Count >= 3 && input.Count <= Settings.MaxPolygonVertices);
//TODO: Uncomment and remove the other line
//Vertices = GiftWrap.GetConvexHull(input);
Vertices = new Vertices(input);
Normals = new Vertices(Vertices.Count);
// Compute normals. Ensure the edges have non-zero length.
for (int i = 0; i < Vertices.Count; ++i)
{
int i1 = i;
int i2 = i + 1 < Vertices.Count ? i + 1 : 0;
FVector2 edge = Vertices[i2] - Vertices[i1];
Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon);
FVector2 temp = new FVector2(edge.Y, -edge.X);
temp.Normalize();
Normals.Add(temp);
}
// Compute the polygon mass data
ComputeProperties();
}
public PolyNode GetRightestConnection(PolyNode incoming)
{
if (NConnected == 0)
{
Debug.Assert(false); // This means the connection graph is inconsistent
}
if (NConnected == 1)
{
//b2Assert(false);
// Because of the possibility of collapsing nearby points,
// we may end up with "spider legs" dangling off of a region.
// The correct behavior here is to turn around.
return(incoming);
}
FVector2 inDir = Position - incoming.Position;
float inLength = inDir.Length();
inDir.Normalize();
Debug.Assert(inLength > Settings.Epsilon);
PolyNode result = null;
for (int i = 0; i < NConnected; ++i)
{
if (Connected[i] == incoming)
{
continue;
}
FVector2 testDir = Connected[i].Position - Position;
float testLengthSqr = testDir.LengthSquared();
testDir.Normalize();
Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon);
float myCos = FVector2.Dot(inDir, testDir);
float mySin = MathUtils.Cross(inDir, testDir);
if (result != null)
{
FVector2 resultDir = result.Position - Position;
resultDir.Normalize();
float resCos = FVector2.Dot(inDir, resultDir);
float resSin = MathUtils.Cross(inDir, resultDir);
if (IsRighter(mySin, myCos, resSin, resCos))
{
result = Connected[i];
}
}
else
{
result = Connected[i];
}
}
Debug.Assert(result != null);
return(result);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
float h = data.step.dt;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = h * MaxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
FVector2 Cdot = vB + MathUtils.Cross(wB, m_rB) - vA - MathUtils.Cross(wA, m_rA);
FVector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot);
FVector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = h * MaxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
private static bool SanityCheck(Vertices vertices)
{
if (vertices.Count < 3)
{
return(false);
}
if (vertices.GetArea() < 0.00001f)
{
return(false);
}
for (int i = 0; i < vertices.Count; ++i)
{
int i1 = i;
int i2 = i + 1 < vertices.Count ? i + 1 : 0;
FVector2 edge = vertices[i2] - vertices[i1];
if (edge.LengthSquared() < FSSettings.Epsilon * FSSettings.Epsilon)
{
return(false);
}
}
for (int i = 0; i < vertices.Count; ++i)
{
int i1 = i;
int i2 = i + 1 < vertices.Count ? i + 1 : 0;
FVector2 edge = vertices[i2] - vertices[i1];
for (int j = 0; j < vertices.Count; ++j)
{
// Don't check vertices on the current edge.
if (j == i1 || j == i2)
{
continue;
}
FVector2 r = vertices[j] - vertices[i1];
// Your polygon is non-convex (it has an indentation) or
// has colinear edges.
float s = edge.X * r.Y - edge.Y * r.X;
if (s < 0.0f)
{
return(false);
}
}
}
return(true);
}
private void ExcuteTestExtremaPoint()
{
FVector2 r = inputPoints[curRefreToPointIdx] - inputPoints[convexHull[curExtremaPointIdx]];
FVector2 v = inputPoints[curTestPointIdx] - inputPoints[convexHull[curExtremaPointIdx]];
float c = Cross(r, v);
if (c < 0.0f)
{
this.curRefreToPointIdx = curTestPointIdx;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
this.curRefreToPointIdx = curTestPointIdx;
}
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Cdot = v + cross(w, r)
FVector2 Cdot = vB + MathUtils.Cross(wB, _rB);
FVector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
FVector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = data.step.dt * MaxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
vB += _invMassB * impulse;
wB += _invIB * MathUtils.Cross(_rB, impulse);
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
float h = data.step.dt;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = h * MaxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
FVector2 Cdot = vB + MathUtils.Cross(wB, m_rB) - vA - MathUtils.Cross(wA, m_rA);
FVector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot);
FVector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = h * MaxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
public override void Update(float dt)
{
FVector2 f = FVector2.Zero;
foreach (FSBody body1 in World.BodyList)
{
if (!IsActiveOn(body1))
{
continue;
}
foreach (FSBody body2 in Bodies)
{
if (body1 == body2 || (body1.IsStatic && body2.IsStatic) || !body2.Enabled)
{
continue;
}
FVector2 d = body2.WorldCenter - body1.WorldCenter;
float r2 = d.LengthSquared();
if (r2 < FSSettings.Epsilon)
{
continue;
}
float r = d.Length();
if (r >= MaxRadius || r <= MinRadius)
{
continue;
}
switch (GravityType)
{
case GravityType.DistanceSquared:
f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * body2.Mass * d;
break;
case GravityType.Linear:
f = Strength / r2 * body1.Mass * body2.Mass * d;
break;
}
body1.ApplyForce(ref f);
FVector2.Negate(ref f, out f);
body2.ApplyForce(ref f);
}
foreach (FVector2 point in Points)
{
FVector2 d = point - body1.Position;
float r2 = d.LengthSquared();
if (r2 < FSSettings.Epsilon)
{
continue;
}
float r = d.Length();
if (r >= MaxRadius || r <= MinRadius)
{
continue;
}
switch (GravityType)
{
case GravityType.DistanceSquared:
f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * d;
break;
case GravityType.Linear:
f = Strength / r2 * body1.Mass * d;
break;
}
body1.ApplyForce(ref f);
}
}
}
public static List <FVector2> GetConvexHull(List <FVector2> vertices)
{
// Find the right most point on the hull
int i0 = 0;
float x0 = vertices[0].X;
for (int i = 1; i < vertices.Count; ++i)
{
float x = vertices[i].X;
if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y))
{
i0 = i;
x0 = x;
}
}
int[] hull = new int[vertices.Count];
int m = 0;
int ih = i0;
for (;;)
{
hull[m] = ih;
int ie = 0;
for (int j = 1; j < vertices.Count; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
FVector2 r = vertices[ie] - vertices[hull[m]];
FVector2 v = vertices[j] - vertices[hull[m]];
float c = Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
List <FVector2> result = new List <FVector2>();
// Copy vertices.
for (int i = 0; i < m; ++i)
{
result.Add(vertices[hull[i]]);
}
return(result);
}
public static void ComputeDistance(out DistanceOutput output,
out SimplexCache cache,
DistanceInput input)
{
cache = new SimplexCache();
++GJKCalls;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
// Get simplex vertices as an array.
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3 <int> saveA = new FixedArray3 <int>();
FixedArray3 <int> saveB = new FixedArray3 <int>();
FVector2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop.
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex.V[i].IndexA;
saveB[i] = simplex.V[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
FVector2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
FVector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < FSSettings.Epsilon * FSSettings.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// 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.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++GJKIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
GJKMaxIters = Math.Max(GJKMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length();
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
if (output.Distance > rA + rB && output.Distance > FSSettings.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
FVector2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
FVector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </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)
{
FVector2 p1 = input.Point1;
FVector2 p2 = input.Point2;
FVector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
FVector2 absV = MathUtils.Abs(new FVector2(-r.Y, r.X));
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
FVector2 t = p1 + maxFraction * (p2 - p1);
FVector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
FVector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_stack.Clear();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
TreeNode <T> node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FVector2 c = node.AABB.Center;
FVector2 h = node.AABB.Extents;
float separation = Math.Abs(FVector2.Dot(new FVector2(-r.Y, r.X), p1 - c)) - FVector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
FVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = FVector2.Min(p1, t);
segmentAABB.UpperBound = FVector2.Max(p1, t);
}
}
else
{
_stack.Push(node.Child1);
_stack.Push(node.Child2);
}
}
}
