public static bool CircletoCircle(Manifold m, Collider a, Collider b)
{
Circle ca = (Circle)a.shape;
Circle cb = (Circle)b.shape;
Vector2R normal = b.pos - a.pos;
float distSquared = normal.LengthSquared();
double radius = a.radius + b.radius;
if (distSquared >= (float)(radius * radius))
{
m.contact_count = 0;
return(false);
}
double distance = Math.Sqrt(distSquared);
m.contact_count = 1;
if (distance == 0)
{
m.penetration = ca.radius;
m.normal = new Vector2R(1, 0);
m.contacts[0] = a.pos;
}
else
{
m.penetration = radius - distance;
m.normal = VMath.MultVectDouble(normal, 1.0 / distance); //normal / distance;
m.contacts[0] = VMath.MultVectDouble(m.normal, ca.radius) + a.pos; //m.normal * ca.radius + a.body.position;
}
return(true);
}
public void PositionalCorrection()
{
double k_slop = 0.05;
double percent = 0.4;
Vector2R correction = VMath.MultVectDouble(normal,
Math.Max(penetration - k_slop, 0.0) / (a.invmass + b.invmass) * percent);
a.pos -= VMath.MultVectDouble(correction, a.invmass);
b.pos += VMath.MultVectDouble(correction, b.invmass);
}
private void IntegrateForces()
{
if (!active)
{
return;
}
if (parent.body.invmass == 0)
{
return;
}
Body b = parent.body;
b.velocity += VMath.MultVectDouble(b.force, b.invmass); //* dt / 2.0;
b.angularVelocity += b.torque * b.invinertia; // * dt / 2.0;
}
public static int Clip(Vector2R n, double c, ref Vector2R[] face)
{
int sp = 0;
Vector2R[] outV = new Vector2R[2] {
face[0],
face[1],
};
// Retrieve distances from each endpoint to the line
// d = ax + by - c
double d1 = Vector2R.Dot(n, face[0]) - c;
double d2 = Vector2R.Dot(n, face[1]) - c;
// If negative (behind plane) clip
if (d1 <= 0.0f)
{
outV[sp++] = face[0];
}
if (d2 <= 0.0f)
{
outV[sp++] = face[1];
}
// If the points are on different sides of the plane
if (d1 * d2 < 0.0f) // less than to ignore -0.0f
{
// Push interesection point
double alpha = d1 / (d1 - d2);
outV[sp] = face[0] + VMath.MultVectDouble((face[1] - face[0]), alpha);
++sp;
}
// Assign our new converted values
face[0] = outV[0];
face[1] = outV[1];
//assert( sp != 3 );
//System.Diagnostics.Debug.Assert(sp != 3);
return(sp);
}
public override void ComputeMass(float density)
{
//calculate centroid and moment of inertia
Vector2R c = new Vector2R(0, 0); // centroid
double area = 0;
double I = 0;
double k_inv3 = 1.0 / 3.0;
for (int i1 = 0; i1 < vertexCount; i1++)
{
Vector2R p1 = vertices[i1];
int i2 = i1 + 1 < vertexCount ? i1 + 1 : 0;
Vector2R p2 = vertices[i2];
double D = VMath.Cross(p1, p2);
double triangleArea = 0.5 * D;
area += triangleArea;
//use area to weight the centroid average, not just the vertex position
c += VMath.MultVectDouble(p1 + p2, triangleArea * k_inv3); // triangleArea * k_inv3 * (p1 + p2);
double intx2 = p1.X * p1.X + p2.X * p1.X + p2.X * p2.X;
double inty2 = p1.Y * p1.Y + p2.Y * p1.Y + p2.Y * p2.Y;
I += (0.25 * k_inv3 * D) * (intx2 + inty2);
}
c = VMath.MultVectDouble(c, 1.0 / area);
//translate verticies to centroid (make centroid (0,0)
//for the polygon in model space)
//Not really necessary but I like doing this anyway
for (int i = 0; i < vertexCount; i++)
{
vertices[i] -= c;
}
body.mass = density * (float)area;
body.inertia = (float)I * density;
}
public void ApplyImpulse()
{
if (GMath.Equal(a.invmass + b.invmass, 0))
{
InfinitMassCorrection();
return;
}
for (int i = 0; i < contact_count; i++)
{
//calcuate radii from COM to contact
Vector2R ra = contacts[i] - a.pos;
Vector2R rb = contacts[i] - b.pos;
//relative velocity
Vector2R rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
a.velocity - VMath.Cross(a.angularVelocity, ra);
//relative velocity along the normal
double contactVel = Vector2R.Dot(rv, normal);
//do not resolve if velocities are seperating
if (contactVel > 0)
{
return;
}
double raCrossN = VMath.Cross(ra, normal);
double rbCrossN = VMath.Cross(rb, normal);
double invMassSum = a.invmass + b.invmass + (raCrossN * raCrossN) * a.invinertia + (rbCrossN * rbCrossN) * b.invinertia;
//calculate impulse scalar
double j = -(1.0 + e) * contactVel;
j /= invMassSum;
j /= (double)contact_count;
//apply impulse
Vector2R impulse = VMath.MultVectDouble(normal, j); // normal * j;
a.ApplyImpulse(-impulse, ra);
b.ApplyImpulse(impulse, rb);
//friction impulse
rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
a.velocity - VMath.Cross(a.angularVelocity, ra);
Vector2R t = rv - (normal * Vector2R.Dot(rv, normal));
//t.Normalize();
VMath.NormalizeSafe(ref t);
//j tangent magnitude
double jt = -Vector2R.Dot(rv, t);
jt /= invMassSum;
jt /= (double)contact_count;
//don't apply tiny friction impulses
if (GMath.Equal(jt, 0.0))
{
return;
}
//coulumbs law
Vector2R tangentImpulse;
if (Math.Abs(jt) < j * sf)
{
tangentImpulse = VMath.MultVectDouble(t, df); // t * df;
}
else
{
tangentImpulse = VMath.MultVectDouble(t, -j * df); // t * -j * df
}
//apply friction impulse
a.ApplyImpulse(-tangentImpulse, ra);
b.ApplyImpulse(tangentImpulse, rb);
}
}
public static bool CircletoPolygon(Manifold m, Collider a, Collider b)
{
Circle A = (Circle)a.shape;
Polygon B = (Polygon)b.shape;
m.contact_count = 0;
// Transform circle center to Polygon model space
Vector2R center = a.pos;
center = B.u.Transpose() * (center - b.pos);
// Find edge with minimum penetration
// Exact concept as using support points in Polygon vs Polygon
double separation = -float.MaxValue;
int faceNormal = 0;
for (int i = 0; i < B.vertexCount; ++i)
{
double s = Vector2R.Dot(B.normals[i], center - B.vertices[i]);
if (s > A.radius)
{
return(false);
}
if (s > separation)
{
separation = s;
faceNormal = i;
}
}
// Grab face's vertices
Vector2R v1 = B.vertices[faceNormal];
int i2 = faceNormal + 1 < B.vertexCount ? faceNormal + 1 : 0;
Vector2R v2 = B.vertices[i2];
// Check to see if center is within polygon
if (separation < GMath.EPSILON)
{
m.contact_count = 1;
m.normal = -(B.u * B.normals[faceNormal]);
m.contacts[0] = VMath.MultVectDouble(m.normal, A.radius) + a.pos;
m.penetration = A.radius;
return(true);
}
// Determine which voronoi region of the edge center of circle lies within
double dot1 = Vector2R.Dot(center - v1, v2 - v1);
double dot2 = Vector2R.Dot(center - v2, v1 - v2);
m.penetration = A.radius - separation;
// Closest to v1
if (dot1 <= 0.0f)
{
if (Vector2R.DistanceSquared(center, v1) > A.radius * A.radius)
{
return(false);
}
m.contact_count = 1;
Vector2R n = v1 - center;
n = B.u * n;
VMath.NormalizeSafe(ref n);
m.normal = n;
v1 = B.u * v1 + b.pos;
m.contacts[0] = v1;
}
// Closest to v2
else if (dot2 <= 0.0f)
{
if (Vector2R.DistanceSquared(center, v2) > A.radius * A.radius)
{
return(false);
}
m.contact_count = 1;
Vector2R n = v2 - center;
v2 = B.u * v2 + b.pos;
m.contacts[0] = v2;
n = B.u * n;
VMath.NormalizeSafe(ref n);
m.normal = n;
}
// Closest to face
else
{
Vector2R n = B.normals[faceNormal];
if (Vector2R.Dot(center - v1, n) > A.radius)
{
return(false);
}
n = B.u * n;
m.normal = -n;
m.contacts[0] = VMath.MultVectDouble(m.normal, A.radius) + a.pos;
m.contact_count = 1;
}
return(true);
}
