public override void ComputeAABB(out AABB aabb, XForm transform)
{
aabb = default(AABB);
Vec2 vec = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
aabb.LowerBound.Set(vec.X - this._radius, vec.Y - this._radius);
aabb.UpperBound.Set(vec.X + this._radius, vec.Y + this._radius);
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1._vertexCount;
Vec2[] vertices1 = poly1._vertices;
Vec2[] normals1 = poly1._normals;
int count2 = poly2._vertexCount;
Vec2[] vertices2 = poly2._vertices;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vec2 normal1World = Common.Math.Mul(xf1.R, normals1[edge1]);
Vec2 normal1 = Common.Math.MulT(xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Common.Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vec2 v1 = Common.Math.Mul(xf1, vertices1[edge1]);
Vec2 v2 = Common.Math.Mul(xf2, vertices2[index]);
float separation = Vec2.Dot(v2 - v1, normal1World);
return separation;
}
public float T0; //time interval = [T0,1], where T0 is in [0,1]
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="alpha">Alpha is a factor in [0,1], where 0 indicates t0.</param>
public void GetTransform(out XForm xf, float alpha)
{
xf = new XForm();
xf.Position = (1.0f - alpha) * C0 + alpha * C;
float angle = (1.0f - alpha) * A0 + alpha * A;
xf.R.Set(angle);
// Shift to origin
xf.Position -= Common.Math.Mul(xf.R, LocalCenter);
}
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
{
aabb = default(AABB);
Vec2 a = transform1.Position + Box2DX.Common.Math.Mul(transform1.R, this._localPosition);
Vec2 b = transform2.Position + Box2DX.Common.Math.Mul(transform2.R, this._localPosition);
Vec2 vec = Box2DX.Common.Math.Min(a, b);
Vec2 vec2 = Box2DX.Common.Math.Max(a, b);
aabb.LowerBound.Set(vec.X - this._radius, vec.Y - this._radius);
aabb.UpperBound.Set(vec2.X + this._radius, vec2.Y + this._radius);
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="hx">The half-width</param>
/// <param name="hy">The half-height.</param>
/// <param name="center">The center of the box in local coordinates.</param>
/// <param name="angle">The rotation of the box in local coordinates.</param>
public void SetAsBox(float hx, float hy, Vec2 center, float angle)
{
SetAsBox(hx, hy);
XForm xf = new XForm();
xf.Position = center;
xf.R.Set(angle);
for (int i = 0; i < VertexCount; ++i) {
Vertices[i] = Common.Math.Mul(xf, Vertices[i]);
}
}
public void GetXForm(out XForm xf, float t)
{
xf = default(XForm);
if (1f - this.T0 > Math.FLOAT32_EPSILON)
{
float num = (t - this.T0) / (1f - this.T0);
xf.Position = (1f - num) * this.C0 + num * this.C;
float angle = (1f - num) * this.A0 + num * this.A;
xf.R.Set(angle);
}
else
{
xf.Position = this.C;
xf.R.Set(this.A);
}
xf.Position -= Math.Mul(xf.R, this.LocalCenter);
}
public static void CollideCircles(ref Manifold manifold,
CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition());
Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition());
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float r1 = circle1.GetRadius();
float r2 = circle2.GetRadius();
float radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float separation;
if (distSqr < Common.Settings.FLT_EPSILON)
{
separation = -radiusSum;
manifold.Normal.Set(0.0f, 1.0f);
}
else
{
float dist = Common.Math.Sqrt(distSqr);
separation = dist - radiusSum;
float a = 1.0f / dist;
manifold.Normal.X = a * d.X;
manifold.Normal.Y = a * d.Y;
}
manifold.PointCount = 1;
manifold.Points[0].ID.Key = 0;
manifold.Points[0].Separation = separation;
p1 += r1 * manifold.Normal;
p2 -= r2 * manifold.Normal;
Vec2 p = 0.5f * (p1 + p2);
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition);
Vec2 s = segment.P1 - position;
float b = Vec2.Dot(s, s) - _radius * _radius;
// Does the segment start inside the circle?
if (b < 0.0f)
{
lambda = 0f;
return SegmentCollide.StartInsideCollide;
}
// Solve quadratic equation.
Vec2 r = segment.P2 - segment.P1;
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON)
{
return SegmentCollide.MissCollide;
}
// Find the point of intersection of the line with the circle.
float a = -(c + Common.Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= maxLambda * rr)
{
a /= rr;
lambda = a;
normal = s + a * r;
normal.Normalize();
return SegmentCollide.HitCollide;
}
return SegmentCollide.MissCollide;
}
public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
Vec2 v = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
Vec2 vec = segment.P1 - v;
float num = Vec2.Dot(vec, vec) - this._radius * this._radius;
SegmentCollide result;
if (num < 0f)
{
lambda = 0f;
result = SegmentCollide.StartInsideCollide;
}
else
{
Vec2 vec2 = segment.P2 - segment.P1;
float num2 = Vec2.Dot(vec, vec2);
float num3 = Vec2.Dot(vec2, vec2);
float num4 = num2 * num2 - num3 * num;
if (num4 < 0f || num3 < Settings.FLT_EPSILON)
{
result = SegmentCollide.MissCollide;
}
else
{
float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4));
if (0f <= num5 && num5 <= maxLambda * num3)
{
num5 /= num3;
lambda = num5;
normal = vec + num5 * vec2;
normal.Normalize();
result = SegmentCollide.HitCollide;
}
else
{
result = SegmentCollide.MissCollide;
}
}
}
return result;
}
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="t">The normalized time in [0,1].</param>
public void GetXForm(out XForm xf, float t)
{
xf = new XForm();
// center = p + R * LocalCenter
if (1.0f - T0 > Math.FLOAT32_EPSILON)
{
float alpha = (t - T0) / (1.0f - T0);
xf.Position = (1.0f - alpha) * C0 + alpha * C;
float angle = (1.0f - alpha) * A0 + alpha * A;
xf.R.Set(angle);
}
else
{
xf.Position = C;
xf.R.Set(A);
}
// Shift to origin
xf.Position -= Math.Mul(xf.R, LocalCenter);
}
public static void CollideCircles(ref Manifold manifold,
CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vec2 p1 = Common.Math.Mul(xf1, circle1._position);
Vec2 p2 = Common.Math.Mul(xf2, circle2._position);
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circle1._position;
manifold.LocalPlaneNormal.SetZero();
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._position;
manifold.Points[0].ID.Key = 0;
}
internal void RefilterProxy(BroadPhase broadPhase, XForm transform)
{
if (this._proxyId != PairManager.NullProxy)
{
broadPhase.DestroyProxy((int)this._proxyId);
AABB aabb;
this.ComputeAABB(out aabb, transform);
bool flag = broadPhase.InRange(aabb);
if (flag)
{
this._proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
this._proxyId = PairManager.NullProxy;
}
}
}
internal void CreateProxy(BroadPhase broadPhase, XForm transform)
{
Box2DXDebug.Assert(this._proxyId == PairManager.NullProxy);
AABB aabb;
this.ComputeAABB(out aabb, transform);
bool flag = broadPhase.InRange(aabb);
Box2DXDebug.Assert(flag);
if (flag)
{
this._proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
this._proxyId = PairManager.NullProxy;
}
}
public override void ComputeAABB(out AABB aabb, XForm xf)
{
Mat22 R = Common.Math.Mul(xf.R, _obb.R);
Mat22 absR = Common.Math.Abs(R);
Vec2 h = Common.Math.Mul(absR, _obb.Extents);
Vec2 position = xf.Position + Common.Math.Mul(xf.R, _obb.Center);
aabb.LowerBound = position - h;
aabb.UpperBound = position + h;
}
public override bool TestPoint(XForm xf, Vec2 p)
{
Vec2 pLocal = Common.Math.MulT(xf.R, p - xf.Position);
for (int i = 0; i < _vertexCount; ++i)
{
float dot = Vec2.Dot(_normals[i], pLocal - _vertices[i]);
if (dot > 0.0f)
{
return false;
}
}
return true;
}
/// <summary>
/// Get the centroid and apply the supplied transform.
/// </summary>
public Vec2 Centroid(XForm xf)
{
return Common.Math.Mul(xf, _centroid);
}
private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
{
Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2);
}
internal bool Synchronize(BroadPhase broadPhase, XForm transform1, XForm transform2)
{
bool result;
if (this._proxyId == PairManager.NullProxy)
{
result = false;
}
else
{
AABB aabb;
this.ComputeSweptAABB(out aabb, transform1, transform2);
if (broadPhase.InRange(aabb))
{
broadPhase.MoveProxy((int)this._proxyId, aabb);
result = true;
}
else
{
result = false;
}
}
return result;
}
internal void ComputeXForm(ref XForm xf, Vec2 center, Vec2 localCenter, float angle)
{
xf.R.Set(angle);
xf.Position = center - Box2DX.Common.Math.Mul(xf.R, localCenter);
}
public override void ComputeAABB(out AABB aabb, XForm transform)
{
aabb = new AABB();
Vec2 p = transform.Position + Common.Math.Mul(transform.R, _localPosition);
aabb.LowerBound.Set(p.X - _radius, p.Y - _radius);
aabb.UpperBound.Set(p.X + _radius, p.Y + _radius);
}
/// <summary>
/// Get the first vertex and apply the supplied transform.
/// </summary>
public Vec2 GetFirstVertex(XForm xf)
{
return Common.Math.Mul(xf, _coreVertices[0]);
}
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the shapes
/// that generated the manifold.
public void Initialize(Manifold manifold, XForm xfA, float radiusA, XForm xfB, float radiusB)
{
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint);
Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint);
Vec2 normal = new Vec2(1.0f, 0.0f);
if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED)
{
normal = pointB - pointA;
normal.Normalize();
}
Normal = normal;
Vec2 cA = pointA + radiusA * normal;
Vec2 cB = pointB - radiusB * normal;
Points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
Vec2 normal = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Vec2 cB = clipPoint - radiusB * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
Vec2 normal = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = -normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint - radiusA * normal;
Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
/// <summary>
/// Get the support point in the given world direction.
/// Use the supplied transform.
/// </summary>
public Vec2 Support(XForm xf, Vec2 d)
{
Vec2 dLocal = Common.Math.MulT(xf.R, d);
int bestIndex = 0;
float bestValue = Vec2.Dot(_coreVertices[0], dLocal);
for (int i = 1; i < _vertexCount; ++i)
{
float value = Vec2.Dot(_coreVertices[i], dLocal);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return Common.Math.Mul(xf, _coreVertices[bestIndex]);
}
public abstract void ComputeAABB(out AABB aabb, XForm xf);
public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
float lower = 0.0f, upper = maxLambda;
Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position);
Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
for (int i = 0; i < _vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vec2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vec2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return SegmentCollide.MissCollide;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return SegmentCollide.MissCollide;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
lambda = lower;
normal = Common.Math.Mul(xf.R, _normals[index]);
return SegmentCollide.HitCollide;
}
lambda = 0f;
return SegmentCollide.StartInsideCollide;
}
public abstract void ComputeSweptAABB(out AABB aabb, XForm xf1, XForm xf2);
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
{
AABB aabb1, aabb2;
ComputeAABB(out aabb1, transform1);
ComputeAABB(out aabb2, transform2);
aabb.LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound);
aabb.UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound);
}
public abstract bool TestPoint(XForm xf, Vec2 p);
/// <summary> /// Draw a transform. Choose your own length scale. /// </summary> /// <param name="xf">A transform.</param> public abstract void DrawXForm(XForm xf);
public abstract SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda);
