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); }