/// Compute the collision manifold between two circles.
public static void CollideCircles(out Manifold manifold,
CircleShape circleA, Transform xfA,
CircleShape circleB, Transform xfB) {
throw new NotImplementedException();
//manifold.pointCount = 0;
//Vec2 pA = Utilities.Mul(xfA, circleA.m_p);
//Vec2 pB = Utilities.Mul(xfB, circleB.m_p);
//Vec2 d = pB - pA;
//float distSqr = Utilities.Dot(d, d);
//float rA = circleA.m_radius, rB = circleB.m_radius;
//float radius = rA + rB;
//if (distSqr > radius * radius)
//{
// return;
//}
//manifold.type = Manifold::e_circles;
//manifold.localPoint = circleA.m_p;
//manifold.localNormal.SetZero();
//manifold.pointCount = 1;
//manifold.points[0].localPoint = circleB.m_p;
//manifold.points[0].id.key = 0;
}
public override void Evaluate(out Manifold manifold, Transform xfA, Transform xfB){
ChainShape chain = (ChainShape)m_fixtureA.GetShape();
EdgeShape edge;
chain.GetChildEdge(out edge, m_indexA);
Collision.CollideEdgeAndCircle(out manifold, edge, xfA,
(CircleShape)m_fixtureB.GetShape(), xfB);
}
/// Implement Shape.
// 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 bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex){
throw new NotImplementedException();
//Vec2 position = transform.p + Utilities.Mul(transform.q, m_p);
//Vec2 s = input.p1 - position;
//float b = Utilities.Dot(s, s) - m_radius * m_radius;
//// Solve quadratic equation.
//Vec2 r = input.p2 - input.p1;
//float c = Utilities.Dot(s, r);
//float rr = Utilities.Dot(r, r);
//float sigma = c * c - rr * b;
//// Check for negative discriminant and short segment.
//if (sigma < 0.0f || rr < Single.Epsilon)
//{
// return false;
//}
//// Find the point of intersection of the line with the circle.
//float a = -(c + (float)Math.Sqrt(sigma));
//// Is the intersection point on the segment?
//if (0.0f <= a && a <= input.maxFraction * rr)
//{
// a /= rr;
// output.fraction = a;
// output.normal = s + a * r;
// output.normal.Normalize();
// return true;
//}
//return false;
}
/// Implement Shape.
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex){
throw new NotImplementedException();
//// Put the ray into the edge's frame of reference.
//Vec2 p1 = Utilities.MulT(xf.q, input.p1 - xf.p);
//Vec2 p2 = Utilities.MulT(xf.q, input.p2 - xf.p);
//Vec2 d = p2 - p1;
//Vec2 v1 = m_vertex1;
//Vec2 v2 = m_vertex2;
//Vec2 e = v2 - v1;
//Vec2 normal(e.Y, -e.X);
//normal.Normalize();
//// q = p1 + t * d
//// dot(normal, q - v1) = 0
//// dot(normal, p1 - v1) + t * dot(normal, d) = 0
//float numerator = Utilities.Dot(normal, v1 - p1);
//float denominator = Utilities.Dot(normal, d);
//if (denominator == 0.0f)
//{
// return false;
//}
//float t = numerator / denominator;
//if (t < 0.0f || input.maxFraction < t)
//{
// return false;
//}
//Vec2 q = p1 + t * d;
//// q = v1 + s * r
//// s = dot(q - v1, r) / dot(r, r)
//Vec2 r = v2 - v1;
//float rr = Utilities.Dot(r, r);
//if (rr == 0.0f)
//{
// return false;
//}
//float s = Utilities.Dot(q - v1, r) / rr;
//if (s < 0.0f || 1.0f < s)
//{
// return false;
//}
//output.fraction = t;
//if (numerator > 0.0f)
//{
// output.normal = -normal;
//}
//else
//{
// output.normal = normal;
//}
//return true;
}
/// Get the interpolated transform at a specific time.
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
public void GetTransform(out Transform xf, float beta) {
xf = new Transform();
xf.p = (1.0f - beta) * c0 + beta * c;
float angle = (1.0f - beta) * a0 + beta * a;
xf.q.Set(angle);
// Shift to origin
xf.p -= Utilities.Mul(xf.q, localCenter);
}
void DrawShape(Fixture fixture, Box2D.Transform xf, Color color)
{
switch (fixture.ShapeType)
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.GetShape();
Vector2 center = MathUtils.Multiply(ref xf, circle._p);
float radius = circle._radius;
Vector2 axis = xf.R.col1;
drawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.GetShape();
int vertexCount = poly._vertexCount;
FixedArray8 <Vector2> vertices = new FixedArray8 <Vector2>();
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref xf, poly._vertices[i]);
}
drawSolidPolygon(ref vertices, vertexCount, color, true);
}
break;
case ShapeType.Edge:
{
EdgeShape edge = (EdgeShape)fixture.GetShape();
Vector2 v1 = MathUtils.Multiply(ref xf, edge._vertex1);
Vector2 v2 = MathUtils.Multiply(ref xf, edge._vertex2);
drawSegment(v1, v2, color);
}
break;
case ShapeType.Loop:
{
LoopShape loop = (LoopShape)fixture.GetShape();
int count = loop._count;
Vector2 v1 = MathUtils.Multiply(ref xf, loop._vertices[count - 1]);
for (int i = 0; i < count; ++i)
{
Vector2 v2 = MathUtils.Multiply(ref xf, loop._vertices[i]);
drawSegment(v1, v2, color);
v1 = v2;
}
}
break;
}
}
public override void DrawTransform(ref Box2D.Transform xf)
{
float axisScale = 0.4f;
Vector2 p1 = xf.Position;
Vector2 p2 = p1 + axisScale * xf.R.col1;
DrawSegment(p1, p2, Color.red);
p2 = p1 + axisScale * xf.R.col2;
DrawSegment(p1, p2, Color.green);
}
public void ReadCache(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
Utilities.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
SimplexVertex[] vertices = this.verticies;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.GetVertex(v.indexA);
Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = Utilities.Mul(transformA, wALocal);
v.wB = Utilities.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Single.Epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = Utilities.Mul(transformA, wALocal);
v.wB = Utilities.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
public void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index)
{
Utilities.Assert(pc.pointCount > 0);
switch (pc.type)
{
case Manifold.ManifoldType.e_circles:
{
Vec2 pointA = Utilities.Mul(xfA, pc.localPoint);
Vec2 pointB = Utilities.Mul(xfB, pc.localPoints[0]);
normal = pointB - pointA;
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = Utilities.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
break;
case Manifold.ManifoldType.e_faceA:
{
normal = Utilities.Mul(xfA.q, pc.localNormal);
Vec2 planePoint = Utilities.Mul(xfA, pc.localPoint);
Vec2 clipPoint = Utilities.Mul(xfB, pc.localPoints[index]);
separation = Utilities.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
break;
case Manifold.ManifoldType.e_faceB:
{
normal = Utilities.Mul(xfB.q, pc.localNormal);
Vec2 planePoint = Utilities.Mul(xfB, pc.localPoint);
Vec2 clipPoint = Utilities.Mul(xfA, pc.localPoints[index]);
separation = Utilities.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
}
}
static void FindIncidentEdge(ClipVertex[/*2*/] c,
PolygonShape poly1, Transform xf1, int edge1,
PolygonShape poly2, Transform xf2)
{
Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_count;
Vec2[] vertices2 = poly2.m_vertices;
Vec2[] normals2 = poly2.m_normals;
Utilities.Assert(0 <= edge1 && edge1 < poly1.m_count);
// Get the normal of the reference edge in poly2's frame.
Vec2 normal1 = Utilities.MulT(xf2.q, Utilities.Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Single.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Utilities.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = Utilities.Mul(xf2, vertices2[i1]);
c[0].id.cf.indexA = (byte)edge1;
c[0].id.cf.indexB = (byte)i1;
c[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
c[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
c[1].v = Utilities.Mul(xf2, vertices2[i2]);
c[1].id.cf.indexA = (byte)edge1;
c[1].id.cf.indexB = (byte)i2;
c[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
c[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, RayCastInput input,
Transform transform, int childIndex){
throw new NotImplementedException();
//Utilities.Assert(childIndex < m_count);
//EdgeShape edgeShape;
//int i1 = childIndex;
//int i2 = childIndex + 1;
//if (i2 == m_count)
//{
// i2 = 0;
//}
//edgeShape.m_vertex1 = m_vertices[i1];
//edgeShape.m_vertex2 = m_vertices[i2];
//return edgeShape.RayCast(output, input, xf, 0);
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
/// Compute the collision manifold between two polygons.
public static void CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB) {
manifold = new Manifold();
float totalRadius = polyA.m_radius + polyB.m_radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = Manifold.ManifoldType.e_faceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = Manifold.ManifoldType.e_faceA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_count;
Vec2[] vertices1 = poly1.m_vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vec2 v11 = vertices1[iv1];
Vec2 v12 = vertices1[iv2];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Utilities.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Utilities.Mul(xf1.q, localTangent);
Vec2 normal = Utilities.Cross(tangent, 1.0f);
v11 = Utilities.Mul(xf1, v11);
v12 = Utilities.Mul(xf1, v12);
// Face offset.
float frontOffset = Utilities.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Utilities.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Utilities.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
ClipVertex[] clipPoints2 = new ClipVertex[2];
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
manifold.points.Clear();
for (int i = 0; i < Settings._maxManifoldPoints; ++i)
{
float separation = Utilities.Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = new ManifoldPoint();
cp.localPoint = Utilities.MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip)
{
// Swap features
ContactFeature cf = cp.id.cf;
cp.id.cf.indexA = cf.indexB;
cp.id.cf.indexB = cf.indexA;
cp.id.cf.typeA = cf.typeB;
cp.id.cf.typeB = cf.typeA;
}
manifold.points.Add(cp);
}
}
}
public ApplyForce() {
m_world.SetGravity(new Vec2(0.0f, 0.0f));
const float k_restitution = 0.4f;
Body ground;
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 20.0f);
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
FixtureDef sd = new FixtureDef();
sd.shape = shape;
sd.Density = 0.0f;
sd.restitution = k_restitution;
// Left vertical
shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(-20.0f, 20.0f));
ground.CreateFixture(sd);
// Right vertical
shape.Set(new Vec2(20.0f, -20.0f), new Vec2(20.0f, 20.0f));
ground.CreateFixture(sd);
// Top horizontal
shape.Set(new Vec2(-20.0f, 20.0f), new Vec2(20.0f, 20.0f));
ground.CreateFixture(sd);
// Bottom horizontal
shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(20.0f, -20.0f));
ground.CreateFixture(sd);
}
{
Transform xf1 = new Transform();
xf1.q.Set(0.3524f * (float)Math.PI);
xf1.p = xf1.q.GetXAxis();
Vec2[] vertices = new Vec2[3];
vertices[0] = Utilities.Mul(xf1, new Vec2(-1.0f, 0.0f));
vertices[1] = Utilities.Mul(xf1, new Vec2(1.0f, 0.0f));
vertices[2] = Utilities.Mul(xf1, new Vec2(0.0f, 0.5f));
PolygonShape poly1 = new PolygonShape();
poly1.Set(vertices, 3);
FixtureDef sd1 = new FixtureDef();
sd1.shape = poly1;
sd1.Density = 4.0f;
Transform xf2 = new Transform();
xf2.q.Set(-0.3524f * (float)Math.PI);
xf2.p = -xf2.q.GetXAxis();
vertices[0] = Utilities.Mul(xf2, new Vec2(-1.0f, 0.0f));
vertices[1] = Utilities.Mul(xf2, new Vec2(1.0f, 0.0f));
vertices[2] = Utilities.Mul(xf2, new Vec2(0.0f, 0.5f));
PolygonShape poly2 = new PolygonShape();
poly2.Set(vertices, 3);
FixtureDef sd2 = new FixtureDef();
sd2.shape = poly2;
sd2.Density = 2.0f;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.angularDamping = 5.0f;
bd.linearDamping = 0.1f;
bd.Position.Set(0.0f, 2.0f);
bd.angle = (float)Math.PI;
bd.allowSleep = false;
m_body = m_world.CreateBody(bd);
m_body.CreateFixture(sd1);
m_body.CreateFixture(sd2);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
fd.friction = 0.3f;
for (int i = 0; i < 10; ++i) {
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 5.0f + 1.54f * i);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
float gravity = 10.0f;
float I = body.GetInertia();
float mass = body.GetMass();
// For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
float radius = (float)Math.Sqrt(2.0f * I / mass);
FrictionJointDef jd = new FrictionJointDef();
jd.localAnchorA.SetZero();
jd.localAnchorB.SetZero();
jd.bodyA = ground;
jd.bodyB = body;
jd.collideConnected = true;
jd.maxForce = mass * gravity;
jd.maxTorque = mass * radius * gravity;
m_world.CreateJoint(jd);
}
}
}
/// Determine if two generic shapes overlap.
public static bool TestOverlap(Shape shapeA, int indexA,
Shape shapeB, int indexB,
Transform xfA, Transform xfB){
DistanceInput input = new DistanceInput();
input.proxyA.Set(shapeA, indexA);
input.proxyB.Set(shapeB, indexB);
input.transformA = xfA;
input.transformB = xfB;
input.useRadii = true;
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceOutput output;
Utilities.Distance(out output, cache, input);
return output.distance < 10.0f * Single.Epsilon;
}
/// Compute the collision manifold between a polygon and a circle.
public static void CollidePolygonAndCircle(out Manifold manifold,
PolygonShape polygonA, Transform xfA,
CircleShape circleB, Transform xfB) {
manifold = new Manifold();
manifold.points.Clear();
// Compute circle position in the frame of the polygon.
Vec2 c = Utilities.Mul(xfB, circleB.m_p);
Vec2 cLocal = Utilities.MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Single.MaxValue;
float radius = polygonA.m_radius + circleB.m_radius;
int vertexCount = polygonA.m_count;
List<Vec2> vertices = new List<Vec2>(polygonA.m_vertices);
List<Vec2> normals = new List<Vec2>(polygonA.m_normals);
for (int i = 0; i < vertexCount; ++i)
{
float s = Utilities.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Single.Epsilon)
{
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Utilities.Dot(cLocal - v1, v2 - v1);
float u2 = Utilities.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Utilities.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (Utilities.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.points = new List<ManifoldPoint>();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
}
else
{
Vec2 faceCenter = 0.5f * (v1 + v2);
float separation2 = Utilities.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation2 > radius)
{
return;
}
manifold.points = new List<ManifoldPoint>();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
}
}
/// Compute the collision manifold between an edge and a circle.
public static void CollideEdgeAndCircle(out Manifold manifold,
EdgeShape edgeA, Transform xfA,
CircleShape circleB, Transform xfB){
manifold = new Manifold();
// Compute circle in frame of edge
Vec2 Q = Utilities.MulT(xfA, Utilities.Mul(xfB, circleB.m_p));
Vec2 A = edgeA.m_vertex1, B = edgeA.m_vertex2;
Vec2 e = B - A;
// Barycentric coordinates
float u = Utilities.Dot(e, B - Q);
float v = Utilities.Dot(e, Q - A);
float radius = edgeA.m_radius + circleB.m_radius;
ContactFeature cf;
cf.indexB = 0;
cf.typeB = ContactFeature.FeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
Vec2 P = A;
Vec2 d = Q - P;
float dd = Utilities.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.m_hasVertex0)
{
Vec2 A1 = edgeA.m_vertex0;
Vec2 B1 = A;
Vec2 e1 = B1 - A1;
float u1 = Utilities.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = ContactFeature.FeatureType.e_vertex;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
return;
}
// Region B
if (u <= 0.0f)
{
Vec2 P = B;
Vec2 d = Q - P;
float dd = Utilities.Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.m_hasVertex3)
{
Vec2 B2 = edgeA.m_vertex3;
Vec2 A2 = B;
Vec2 e2 = B2 - A2;
float v2 = Utilities.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = ContactFeature.FeatureType.e_vertex;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
return;
}
// Region AB
float den = Utilities.Dot(e, e);
Utilities.Assert(den > 0.0f);
Vec2 Pb = (1.0f / den) * (u * A + v * B);
Vec2 db = Q - Pb;
float ddb = Utilities.Dot(db, db);
if (ddb > radius * radius)
{
return;
}
Vec2 n = new Vec2(-e.Y, e.X);
if (Utilities.Dot(n, Q - A) < 0.0f)
{
n.Set(-n.X, -n.Y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = ContactFeature.FeatureType.e_face;
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.cf = cf;
manifold.points[0].localPoint = circleB.m_p;
}
/// @see Shape::TestPoint
public override bool TestPoint(Transform transform, Vec2 p) {
return false;
}
/// Build vertices to represent an oriented box.
/// @param hx the half-width.
/// @param hy the half-height.
/// @param center the center of the box in local coordinates.
/// @param angle the rotation of the box in local coordinates.
public void SetAsBox(float hx, float hy, Vec2 center, float angle){
m_count = 4;
m_vertices[0].Set(-hx, -hy);
m_vertices[1].Set(hx, -hy);
m_vertices[2].Set(hx, hy);
m_vertices[3].Set(-hx, hy);
m_normals[0].Set(0.0f, -1.0f);
m_normals[1].Set(1.0f, 0.0f);
m_normals[2].Set(0.0f, 1.0f);
m_normals[3].Set(-1.0f, 0.0f);
m_centroid = center;
Transform xf = new Transform();
xf.p = center;
xf.q.Set(angle);
// Transform vertices and normals.
for (int i = 0; i < m_count; ++i) {
m_vertices[i] = Utilities.Mul(xf, m_vertices[i]);
m_normals[i] = Utilities.Mul(xf.q, m_normals[i]);
}
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, RayCastInput input, Transform transform, int childIndex) {
throw new NotImplementedException();
//// Put the ray into the polygon's frame of reference.
//Vec2 p1 = Utilities.MulT(xf.q, input.p1 - xf.p);
//Vec2 p2 = Utilities.MulT(xf.q, input.p2 - xf.p);
//Vec2 d = p2 - p1;
//float lower = 0.0f, upper = input.maxFraction;
//int index = -1;
//for (int i = 0; i < m_count; ++i)
//{
// // p = p1 + a * d
// // dot(normal, p - v) = 0
// // dot(normal, p1 - v) + a * dot(normal, d) = 0
// float numerator = Utilities.Dot(m_normals[i], m_vertices[i] - p1);
// float denominator = Utilities.Dot(m_normals[i], d);
// if (denominator == 0.0f)
// {
// if (numerator < 0.0f)
// {
// return false;
// }
// }
// 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;
// }
// }
// // The use of epsilon here causes the assert on lower to trip
// // in some cases. Apparently the use of epsilon was to make edge
// // shapes work, but now those are handled separately.
// //if (upper < lower - Single.Epsilon)
// if (upper < lower)
// {
// return false;
// }
//}
//Utilities.Assert(0.0f <= lower && lower <= input.maxFraction);
//if (index >= 0)
//{
// output.fraction = lower;
// output.normal = Utilities.Mul(xf.q, m_normals[index]);
// return true;
//}
//return false;
}
public bool SolvePositionConstraints(){
float minSeparation = 0.0f;
for (int i = 0; i < m_contacts.Count(); ++i) {
ContactPositionConstraint pc = m_positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
Vec2 localCenterA = pc.localCenterA;
float mA = pc.invMassA;
float iA = pc.invIA;
Vec2 localCenterB = pc.localCenterB;
float mB = pc.invMassB;
float iB = pc.invIB;
int pointCount = pc.pointCount;
Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j) {
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(pc, xfA, xfB, j);
Vec2 normal = psm.normal;
Vec2 point = psm.point;
float separation = psm.separation;
Vec2 rA = point - cA;
Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Utilities.Clamp(Settings._baumgarte * (separation + Settings._linearSlop), -Settings._maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = Utilities.Cross(rA, normal);
float rnB = Utilities.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * Utilities.Cross(rA, P);
cB += mB * P;
aB += iB * Utilities.Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return minSeparation >= -3.0f * Settings._linearSlop;
}
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public void Collide(out Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB){
manifold = new Manifold();
m_xf = Utilities.MulT(xfA, xfB);
m_centroidB = Utilities.Mul(m_xf, polygonB.m_centroid);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
bool hasVertex0 = edgeA.m_hasVertex0;
bool hasVertex3 = edgeA.m_hasVertex3;
Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.Y, -edge1.X);
float offset1 = Utilities.Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.Y, -edge0.X);
convex1 = Utilities.Cross(edge0, edge1) >= 0.0f;
offset0 = Utilities.Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.Y, -edge2.X);
convex2 = Utilities.Cross(edge1, edge2) > 0.0f;
offset2 = Utilities.Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
m_polygonB.vertices[i] = Utilities.Mul(m_xf, polygonB.m_vertices[i]);
m_polygonB.normals[i] = Utilities.Mul(m_xf.q, polygonB.m_normals[i]);
}
m_radius = 2.0f * Settings._polygonRadius;
manifold.points.Clear();
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.type == EPAxisType.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
ClipVertex[] ie = new ClipVertex[2];
ReferenceFace rf = new ReferenceFace();
if (primaryAxis.type == EPAxisType.e_edgeA)
{
manifold.type = Manifold.ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Utilities.Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = Utilities.Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)i1;
ie[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
ie[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)i2;
ie[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
ie[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = Manifold.ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)primaryAxis.index;
ie[0].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
ie[0].id.cf.typeB = ContactFeature.FeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)primaryAxis.index;
ie[1].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
ie[1].id.cf.typeB = ContactFeature.FeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.Y, -rf.normal.X);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Utilities.Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Utilities.Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
ClipVertex[] clipPoints2 = new ClipVertex[2];
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings._maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings._maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == EPAxisType.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.m_normals[rf.i1];
manifold.localPoint = polygonB.m_vertices[rf.i1];
}
manifold.points.Clear();
for (int i = 0; i < Settings._maxManifoldPoints; ++i)
{
float separation;
separation = Utilities.Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
ManifoldPoint cp = new ManifoldPoint();
if (primaryAxis.type == EPAxisType.e_edgeA)
{
cp.localPoint = Utilities.MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
manifold.points.Add(cp);
}
}
}
/// 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,
Transform xfA, float radiusA,
Transform xfB, float radiusB){
if (manifold.points.Count() == 0)
{
return;
}
switch (manifold.type)
{
case Manifold.ManifoldType.e_circles:
{
normal.Set(1.0f, 0.0f);
Vec2 pointA = Utilities.Mul(xfA, manifold.localPoint);
Vec2 pointB = Utilities.Mul(xfB, manifold.points[0].localPoint);
if (Utilities.DistanceSquared(pointA, pointB) > Single.Epsilon * Single.Epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
Vec2 cA = pointA + radiusA * normal;
Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case Manifold.ManifoldType.e_faceA:
{
normal = Utilities.Mul(xfA.q, manifold.localNormal);
Vec2 planePoint = Utilities.Mul(xfA, manifold.localPoint);
points.Clear();
for (int i = 0; i < manifold.points.Count(); ++i)
{
Vec2 clipPoint = Utilities.Mul(xfB, manifold.points[i].localPoint);
Vec2 cA = clipPoint + (radiusA - Utilities.Dot(clipPoint - planePoint, normal)) * normal;
Vec2 cB = clipPoint - radiusB * normal;
points.Add(0.5f * (cA + cB));
}
}
break;
case Manifold.ManifoldType.e_faceB:
{
normal = Utilities.Mul(xfB.q, manifold.localNormal);
Vec2 planePoint = Utilities.Mul(xfB, manifold.localPoint);
points.Clear();
for (int i = 0; i < manifold.points.Count(); ++i)
{
Vec2 clipPoint = Utilities.Mul(xfA, manifold.points[i].localPoint);
Vec2 cB = clipPoint + (radiusB - Utilities.Dot(clipPoint - planePoint, normal)) * normal;
Vec2 cA = clipPoint - radiusA * normal;
points.Add(0.5f * (cA + cB));
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
/// @see Shape::ComputeAABB
public override void ComputeAABB(out AABB aabb, Transform transform, int childIndex) {
throw new NotImplementedException();
//Utilities.Assert(childIndex < m_count);
//int i1 = childIndex;
//int i2 = childIndex + 1;
//if (i2 == m_count)
//{
// i2 = 0;
//}
//Vec2 v1 = Utilities.Mul(xf, m_vertices[i1]);
//Vec2 v2 = Utilities.Mul(xf, m_vertices[i2]);
//aabb.lowerBound = Math.Min(v1, v2);
//aabb.upperBound = Math.Max(v1, v2);
}
/// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. /// @param childIndex the child shape public abstract void ComputeAABB(out AABB aabb, Transform xf, int childIndex);
/// @see Shape::ComputeAABB
public override void ComputeAABB(out AABB aabb, Transform xf, int childIndex) {
Vec2 v1 = Utilities.Mul(xf, m_vertex1);
Vec2 v2 = Utilities.Mul(xf, m_vertex2);
Vec2 lower = Utilities.Min(v1, v2);
Vec2 upper = Utilities.Max(v1, v2);
Vec2 r = new Vec2(m_radius, m_radius);
aabb.lowerBound = lower - r;
aabb.upperBound = upper + r;
}
internal void SynchronizeFixtures(){
Transform xf1 = new Transform();
xf1.q.Set(m_sweep.a0);
xf1.p = m_sweep.c0 - Utilities.Mul(xf1.q, m_sweep.localCenter);
BroadPhase broadPhase = m_world.m_contactManager.m_broadPhase;
foreach (Fixture f in m_fixtureList){
f.Synchronize(broadPhase, xf1, m_xf);
}
}
public void InitializeVelocityConstraints() {
for (int i = 0; i < m_contacts.Count(); ++i) {
ContactVelocityConstraint vc = m_velocityConstraints[i];
ContactPositionConstraint pc = m_positionConstraints[i];
float radiusA = pc.radiusA;
float radiusB = pc.radiusB;
Manifold manifold = m_contacts[vc.contactIndex].GetManifold();
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
float mB = vc.invMassB;
float iA = vc.invIA;
float iB = vc.invIB;
Vec2 localCenterA = pc.localCenterA;
Vec2 localCenterB = pc.localCenterB;
Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
Vec2 vA = m_velocities[indexA].v;
float wA = m_velocities[indexA].w;
Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
Vec2 vB = m_velocities[indexB].v;
float wB = m_velocities[indexB].w;
Utilities.Assert(manifold.points.Count() > 0);
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.normal = worldManifold.normal;
int pointCount = vc.points.Count;
for (int j = 0; j < pointCount; ++j) {
VelocityConstraintPoint vcp = vc.points[j];
vcp.rA = worldManifold.points[j] - cA;
vcp.rB = worldManifold.points[j] - cB;
float rnA = Utilities.Cross(vcp.rA, vc.normal);
float rnB = Utilities.Cross(vcp.rB, vc.normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
Vec2 tangent = Utilities.Cross(vc.normal, 1.0f);
float rtA = Utilities.Cross(vcp.rA, tangent);
float rtB = Utilities.Cross(vcp.rB, tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp.velocityBias = 0.0f;
float vRel = Utilities.Dot(vc.normal, vB + Utilities.Cross(wB, vcp.rB) - vA - Utilities.Cross(wA, vcp.rA));
if (vRel < -Settings._velocityThreshold) {
vcp.velocityBias = -vc.restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.points.Count() == 2) {
VelocityConstraintPoint vcp1 = vc.points[0];
VelocityConstraintPoint vcp2 = vc.points[1];
float rn1A = Utilities.Cross(vcp1.rA, vc.normal);
float rn1B = Utilities.Cross(vcp1.rB, vc.normal);
float rn2A = Utilities.Cross(vcp2.rA, vc.normal);
float rn2B = Utilities.Cross(vcp2.rB, vc.normal);
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 1000.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
// K is safe to invert.
vc.K.ex.Set(k11, k12);
vc.K.ey.Set(k12, k22);
vc.normalMass = vc.K.GetInverse();
} else {
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.points.Clear();
vc.points.Add(new VelocityConstraintPoint());
}
}
}
}
/// Compute the collision manifold between an edge and a circle.
public static void CollideEdgeAndPolygon(out Manifold manifold,
EdgeShape edgeA, Transform xfA,
PolygonShape polygonB, Transform xfB) {
EPCollider collider = new EPCollider();
collider.Collide(out manifold, edgeA, xfA, polygonB, xfB);
}
/// @see Shape::TestPoint
public override bool TestPoint(Transform transform, Vec2 p) {
throw new NotImplementedException();
//Vec2 pLocal = Utilities.MulT(xf.q, p - xf.p);
//for (int i = 0; i < m_count; ++i)
//{
// float dot = Utilities.Dot(m_normals[i], pLocal - m_vertices[i]);
// if (dot > 0.0f)
// {
// return false;
// }
//}
//return true;
}
/// Test a point for containment in this shape. This only works for convex shapes. /// @param xf the shape world transform. /// @param p a point in world coordinates. public abstract bool TestPoint(Transform xf, Vec2 p);
/// @see Shape::ComputeAABB
public override void ComputeAABB(out AABB aabb, Transform xf, int childIndex) {
Vec2 lower = Utilities.Mul(xf, m_vertices[0]);
Vec2 upper = lower;
for (int i = 1; i < m_count; ++i)
{
Vec2 v = Utilities.Mul(xf, m_vertices[i]);
lower = Utilities.Min(lower, v);
upper = Utilities.Max(upper, v);
}
Vec2 r = new Vec2(m_radius, m_radius);
aabb.lowerBound = lower - r;
aabb.upperBound = upper + r;
}
/// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. /// @param childIndex the child shape index public abstract bool RayCast(out RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
