internal Fixture()
{
_userData = null;
_body = null;
_next = null;
_proxyId = BroadPhase.NullProxy;
_shape = null;
}
/// The ructor sets the default fixture definition values.
public FixtureDef()
{
shape = null;
userData = null;
friction = 0.2f;
restitution = 0.0f;
density = 0.0f;
filter.categoryBits = 0x0001;
filter.maskBits = 0xFFFF;
filter.groupIndex = 0;
isSensor = false;
}
// We need separation create/destroy functions from the ructor/destructor because
// the destructor cannot access the allocator or broad-phase (no destructor arguments allowed by C++).
internal void Create(BroadPhase broadPhase, Body body, ref XForm xf, FixtureDef def)
{
_userData = def.userData;
_friction = def.friction;
_restitution = def.restitution;
_density = def.density;
_body = body;
_next = null;
_filter = def.filter;
_isSensor = def.isSensor;
_shape = def.shape.Clone();
// Create proxy in the broad-phase.
AABB aabb;
_shape.ComputeAABB(out aabb, ref xf);
_proxyId = broadPhase.CreateProxy(ref aabb, this);
}
internal void Destroy(BroadPhase broadPhase)
{
// Remove proxy from the broad-phase.
if (_proxyId != BroadPhase.NullProxy)
{
broadPhase.DestroyProxy(_proxyId);
_proxyId = BroadPhase.NullProxy;
}
_shape = null;
}
/// Creates a fixture from a shape and attach it to this body.
/// This is a convenience function. Use FixtureDef if you need to set parameters
/// like friction, restitution, user data, or filtering.
/// @param shape the shape to be cloned.
/// @param density the shape density (set to zero for static bodies).
/// @warning This function is locked during callbacks.
public Fixture CreateFixture(Shape shape, float density)
{
Debug.Assert(_world.IsLocked == false);
if (_world.IsLocked == true)
{
return null;
}
BroadPhase broadPhase = _world._contactManager._broadPhase;
FixtureDef def = new FixtureDef();
def.shape = shape;
def.density = density;
Fixture fixture = new Fixture();
fixture.Create(broadPhase, this, ref _xf, def);
fixture._next = _fixtureList;
_fixtureList = fixture;
++_fixtureCount;
fixture._body = this;
// Let the world know we have a new fixture.
_world._flags |= WorldFlags.NewFixture;
return fixture;
}
public SeparationFunction(ref SimplexCache cache,
Shape shapeA, ref XForm transformA,
Shape shapeB, ref XForm transformB)
{
_localPoint = Vector2.Zero;
_shapeA = shapeA;
_shapeB = shapeB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _shapeA.GetVertex(cache.indexA[0]);
Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.indexB[0] == cache.indexB[1])
{
// Two points on A and one on B
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]);
_localPoint = 0.5f * (localPointA1 + localPointA2);
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref transformA, _localPoint);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointA = shapeA.GetVertex(cache.indexA[0]);
Vector2 localPointB1 = shapeB.GetVertex(cache.indexB[0]);
Vector2 localPointB2 = shapeB.GetVertex(cache.indexB[1]);
_localPoint = 0.5f * (localPointB1 + localPointB2);
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
Vector2 pointB = MathUtils.Multiply(ref transformB, _localPoint);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
Vector2 localPointB1 = _shapeB.GetVertex(cache.indexB[0]);
Vector2 localPointB2 = _shapeB.GetVertex(cache.indexB[1]);
Vector2 pA = MathUtils.Multiply(ref transformA, localPointA1);
Vector2 dA = MathUtils.Multiply(ref transformA.R, localPointA2 - localPointA1);
Vector2 pB = MathUtils.Multiply(ref transformB, localPointB1);
Vector2 dB = MathUtils.Multiply(ref transformB.R, localPointB2 - localPointB1);
float a = Vector2.Dot(dA, dA);
float e = Vector2.Dot(dB, dB);
Vector2 r = pA - pB;
float c = Vector2.Dot(dA, r);
float f = Vector2.Dot(dB, r);
float b = Vector2.Dot(dA, dB);
float denom = a * e - b * b;
float s = 0.0f;
if (denom != 0.0f)
{
s = MathUtils.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
}
float t = (b * s + f) / e;
if (t < 0.0f)
{
t = 0.0f;
s = MathUtils.Clamp(-c / a, 0.0f, 1.0f);
}
else if (t > 1.0f)
{
t = 1.0f;
s = MathUtils.Clamp((b - c) / a, 0.0f, 1.0f);
}
Vector2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
Vector2 localPointB = localPointB1 + t * (localPointB2 - localPointB1);
if (s == 0.0f || s == 1.0f)
{
_type = SeparationFunctionType.FaceB;
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
_localPoint = localPointB;
Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
float sgn = Vector2.Dot(pointA - pointB, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
else
{
_type = SeparationFunctionType.FaceA;
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
_localPoint = localPointA;
Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
float sgn = Vector2.Dot(pointB - pointA, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
}
}
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// @warning the sweeps must have the same time interval.
/// @return the fraction between [0,1] in which the shapes first touch.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
public static float CalculateTimeOfImpact(ref TOIInput input, Shape shapeA, Shape shapeB)
{
++b2_toiCalls;
Sweep sweepA = input.sweepA;
Sweep sweepB = input.sweepB;
Debug.Assert(sweepA.t0 == sweepB.t0);
Debug.Assert(1.0f - sweepA.t0 > Settings.b2_FLT_EPSILON);
float radius = shapeA._radius + shapeB._radius;
float tolerance = input.tolerance;
float alpha = 0.0f;
int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache;
DistanceInput distanceInput;
distanceInput.useRadii = false;
for(;;)
{
XForm xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
DistanceOutput distanceOutput;
Distance.ComputeDistance(out distanceOutput, out cache, ref distanceInput, shapeA, shapeB);
if (distanceOutput.distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction(ref cache, shapeA, ref xfA, shapeB, ref xfB);
float separation = fcn.Evaluate(ref xfA, ref xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if false
// Dump the curve seen by the root finder
{
int N = 100;
float dx = 1.0f / N;
float xs[N+1];
public static void ComputeDistance(out DistanceOutput output,
out SimplexCache cache,
ref DistanceInput input,
Shape shapeA,
Shape shapeB)
{
cache = new SimplexCache();
++b2_gjkCalls;
XForm transformA = input.transformA;
XForm transformB = input.transformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, shapeA, ref transformA, shapeB, ref transformB);
// Get simplex vertices as an array.
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>();
int saveCount = 0;
Vector2 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.
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.
Vector2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_FLT_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 = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d));
vertex.wA = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA));
//Vector2 wBLocal;
vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d));
vertex.wB = MathUtils.Multiply(ref transformB, shapeB.GetVertex(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;
++b2_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;
}
b2_gjkMaxIters = Math.Max(b2_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 = shapeA._radius;
float rB = shapeB._radius;
if (output.distance > rA + rB && output.distance > Settings.b2_FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
Vector2 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.
Vector2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
internal void ReadCache( ref SimplexCache cache,
Shape shapeA, ref XForm transformA,
Shape shapeB, ref XForm transformB)
{
Debug.Assert(0 <= cache.count && cache.count <= 3);
// Copy data from cache.
_count = cache.count;
for (int i = 0; i < _count; ++i)
{
SimplexVertex v = _v[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vector2 wALocal = shapeA.GetVertex(v.indexA);
Vector2 wBLocal = shapeB.GetVertex(v.indexB);
v.wA = MathUtils.Multiply(ref transformA, wALocal);
v.wB = MathUtils.Multiply(ref transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
_v[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.b2_FLT_EPSILON)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex v = _v[0];
v.indexA = 0;
v.indexB = 0;
Vector2 wALocal = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(0);
v.wA = MathUtils.Multiply(ref transformA, wALocal);
v.wB = MathUtils.Multiply(ref transformB, wBLocal);
v.w = v.wB - v.wA;
_v[0] = v;
_count = 1;
}
}
