public Body(Shape shape, float x, float y) { this.shape = shape; position.Set(x, y); velocity.Set(0, 0); angularVelocity = 0; torque = 0; orient = ImpulseMath.Random(-ImpulseMath.PI, ImpulseMath.PI); force.Set(0, 0); staticFriction = 0.5f; dynamicFriction = 0.3f; restitution = 0.2f; shape.body = this; shape.Initialize(); }
public override void HandleCollision(Manifold m, Body a, Body b) { Polygon A = (Polygon)a.shape; Polygon B = (Polygon)b.shape; m.contactCount = 0; // Check for a separating axis with A's face planes int[] faceA = { 0 }; float penetrationA = FindAxisLeastPenetration(faceA, A, B); if (penetrationA >= 0.0f) { return; } // Check for a separating axis with B's face planes int[] faceB = { 0 }; float penetrationB = FindAxisLeastPenetration(faceB, B, A); if (penetrationB >= 0.0f) { return; } int referenceIndex; bool flip; // Always point from a to b Polygon RefPoly; // Reference Polygon IncPoly; // Incident // Determine which shape contains reference face if (ImpulseMath.Gt(penetrationA, penetrationB)) { RefPoly = A; IncPoly = B; referenceIndex = faceA[0]; flip = false; } else { RefPoly = B; IncPoly = A; referenceIndex = faceB[0]; flip = true; } // World space incident face Vec2[] incidentFace = Vec2.ArrayOf(2); FindIncidentFace(incidentFace, RefPoly, IncPoly, referenceIndex); // y // ^ .n ^ // +---c ------posPlane-- // x < | i |\ // +---+ c-----negPlane-- // \ v // r // // r : reference face // i : incident poly // c : clipped point // n : incident normal // Setup reference face vertices Vec2 v1 = RefPoly.vertices[referenceIndex]; referenceIndex = referenceIndex + 1 == RefPoly.vertexCount ? 0 : referenceIndex + 1; Vec2 v2 = RefPoly.vertices[referenceIndex]; // Transform vertices to world space // v1 = RefPoly->u * v1 + RefPoly->body->position; // v2 = RefPoly->u * v2 + RefPoly->body->position; v1 = RefPoly.u.Mul(v1).Addi(RefPoly.body.position); v2 = RefPoly.u.Mul(v2).Addi(RefPoly.body.position); // Calculate reference face side normal in world space // Vec2 sidePlaneNormal = (v2 - v1); // sidePlaneNormal.Normalize( ); Vec2 sidePlaneNormal = v2.Sub(v1); sidePlaneNormal.Normalize(); // Orthogonalize // Vec2 refFaceNormal( sidePlaneNormal.y, -sidePlaneNormal.x ); Vec2 refFaceNormal = new Vec2(sidePlaneNormal.y, -sidePlaneNormal.x); // ax + by = c // c is distance from origin // real refC = Dot( refFaceNormal, v1 ); // real negSide = -Dot( sidePlaneNormal, v1 ); // real posSide = Dot( sidePlaneNormal, v2 ); float refC = Vec2.Dot(refFaceNormal, v1); float negSide = -Vec2.Dot(sidePlaneNormal, v1); float posSide = Vec2.Dot(sidePlaneNormal, v2); // Clip incident face to reference face side planes // if(Clip( -sidePlaneNormal, negSide, incidentFace ) < 2) if (Clip(sidePlaneNormal.Neg(), negSide, incidentFace) < 2) { return; // Due to floating point error, possible to not have required // points } // if(Clip( sidePlaneNormal, posSide, incidentFace ) < 2) if (Clip(sidePlaneNormal, posSide, incidentFace) < 2) { return; // Due to floating point error, possible to not have required // points } // Flip m.normal.Set(refFaceNormal); if (flip) { m.normal.Negi(); } // Keep points behind reference face int cp = 0; // clipped points behind reference face float separation = Vec2.Dot(refFaceNormal, incidentFace[0]) - refC; if (separation <= 0.0f) { m.contacts[cp].Set(incidentFace[0]); m.penetration = -separation; ++cp; } else { m.penetration = 0; } separation = Vec2.Dot(refFaceNormal, incidentFace[1]) - refC; if (separation <= 0.0f) { m.contacts[cp].Set(incidentFace[1]); m.penetration += -separation; ++cp; // Average penetration m.penetration /= cp; } m.contactCount = cp; }
public void ApplyImpulse() { // Early out and positional correct if both objects have infinite mass // if(Equal( A->im + B->im, 0 )) if (ImpulseMath.Equal(A.invMass + B.invMass, 0)) { InfiniteMassCorrection(); return; } for (int i = 0; i < contactCount; ++i) { // Calculate radii from COM to contact // Vec2 ra = contacts[i] - A->position; // Vec2 rb = contacts[i] - B->position; Vec2 ra = contacts[i].Sub(A.position); Vec2 rb = contacts[i].Sub(B.position); // Relative velocity // Vec2 rv = B->velocity + Cross( B->angularVelocity, rb ) - // A->velocity - Cross( A->angularVelocity, ra ); Vec2 rv = B.velocity.Add(Vec2.Cross(B.angularVelocity, rb, new Vec2())).Subi(A.velocity).Subi(Vec2.Cross(A.angularVelocity, ra, new Vec2())); // Relative velocity along the normal // real contactVel = Dot( rv, normal ); float contactVel = Vec2.Dot(rv, normal); // Do not resolve if velocities are separating if (contactVel > 0) { return; } // real raCrossN = Cross( ra, normal ); // real rbCrossN = Cross( rb, normal ); // real invMassSum = A->im + B->im + Sqr( raCrossN ) * A->iI + Sqr( // rbCrossN ) * B->iI; float raCrossN = Vec2.Cross(ra, normal); float rbCrossN = Vec2.Cross(rb, normal); float invMassSum = A.invMass + B.invMass + (raCrossN * raCrossN) * A.invInertia + (rbCrossN * rbCrossN) * B.invInertia; // Calculate impulse scalar float j = -(1.0f + e) * contactVel; j /= invMassSum; j /= contactCount; // Apply impulse Vec2 impulse = normal.Mul(j); A.ApplyImpulse(impulse.Neg(), ra); B.ApplyImpulse(impulse, rb); // Friction impulse // rv = B->velocity + Cross( B->angularVelocity, rb ) - // A->velocity - Cross( A->angularVelocity, ra ); rv = B.velocity.Add(Vec2.Cross(B.angularVelocity, rb, new Vec2())).Subi(A.velocity).Subi(Vec2.Cross(A.angularVelocity, ra, new Vec2())); // Vec2 t = rv - (normal * Dot( rv, normal )); // t.Normalize( ); Vec2 t = new Vec2(rv); t.Addsi(normal, -Vec2.Dot(rv, normal)); t.Normalize(); // j tangent magnitude float jt = -Vec2.Dot(rv, t); jt /= invMassSum; jt /= contactCount; // Don't apply tiny friction impulses if (ImpulseMath.Equal(jt, 0.0f)) { return; } // Coulumb's law Vec2 tangentImpulse; // if(std::abs( jt ) < j * sf) if (Mathf.Abs(jt) < j * sf) { // tangentImpulse = t * jt; tangentImpulse = t.Mul(jt); } else { // tangentImpulse = t * -j * df; tangentImpulse = t.Mul(j).Muli(-df); } // Apply friction impulse // A->ApplyImpulse( -tangentImpulse, ra ); // B->ApplyImpulse( tangentImpulse, rb ); A.ApplyImpulse(tangentImpulse.Neg(), ra); B.ApplyImpulse(tangentImpulse, rb); } }
void InputImpulseEngine(CommonMonoBehaviour.GameInput input) { if (!playing) { return; } if (input.keyDown[0]) { playing = false; } if (input.keyDown[1]) { if (input.mouseUp[0] || input.mouseDown[0]) { float hw = ImpulseMath.Random(1.0f, 3.0f); float hh = ImpulseMath.Random(1.0f, 3.0f); Body b = impulse.Add(new Polygon(hw, hh), input.rayPosition.x, input.rayPosition.y); b.SetOrient(0.0f); } if (input.mouseUp[1] || input.mouseDown[1]) { float r = ImpulseMath.Random(1.0f, 5.0f); int vertCount = 3; Vec2[] verts = Vec2.ArrayOf(vertCount); for (int i = 0; i < vertCount; i++) { verts[i].Set(ImpulseMath.Random(-r, r), ImpulseMath.Random(-r, r)); } Body b = impulse.Add(new Polygon(verts), input.rayPosition.x, input.rayPosition.y); b.SetOrient(ImpulseMath.Random(-ImpulseMath.PI, ImpulseMath.PI)); b.restitution = 0.2f; b.dynamicFriction = 0.2f; b.staticFriction = 0.4f; } } else { if (input.mouseUp[0] || input.mouseDown[0]) { float r = ImpulseMath.Random(1.0f, 5.0f); int vertCount = ImpulseMath.Random(3, Polygon.MAX_POLY_VERTEX_COUNT); Vec2[] verts = Vec2.ArrayOf(vertCount); for (int i = 0; i < vertCount; i++) { verts[i].Set(ImpulseMath.Random(-r, r), ImpulseMath.Random(-r, r)); } Body b = impulse.Add(new Polygon(verts), input.rayPosition.x, input.rayPosition.y); b.SetOrient(ImpulseMath.Random(-ImpulseMath.PI, ImpulseMath.PI)); b.restitution = 0.2f; b.dynamicFriction = 0.2f; b.staticFriction = 0.4f; } if (input.mouseUp[1] || input.mouseDown[1]) { float r = ImpulseMath.Random(1.0f, 3.0f); impulse.Add(new Circle(r), input.rayPosition.x, input.rayPosition.y); } //Move your cube GameObject to the point where you clicked if (PointerTransform) { PointerTransform.position = input.rayPosition; } } }