TimeOfImpactSolver class
class TimeOfImpactSolver { List<TimeOfImpactConstraint> constraints; int count; Body toiBody; /** Pooling. */ final TimeOfImpactSolverManifold psm; final Vector rA; final Vector rB; final Vector P; final Vector temp; TimeOfImpactSolver() : count = 0, toiBody = null, constraints = new List<TimeOfImpactConstraint>(4), // Initialize pool variables. psm = new TimeOfImpactSolverManifold(), rA = new Vector(), rB = new Vector(), P = new Vector(), temp = new Vector() { for (int i = 0; i<constraints.length; i++){ constraints[i] = new TimeOfImpactConstraint(); } } void initialize(List<Contact> contacts, int argCount, Body argToiBody) { count = argCount; toiBody = argToiBody; if(count >= constraints.length){ List<TimeOfImpactConstraint> old = constraints; int newLen = Math.max(count, old.length*2); constraints = new List<TimeOfImpactConstraint>(newLen); constraints.setRange(0, old.length, old); for(int i=old.length; i<constraints.length; i++){ constraints[i] = new TimeOfImpactConstraint(); } } for (int i=0; i<count; i++) { Contact contact = contacts[i]; Fixture fixtureA = contact.fixtureA; Fixture fixtureB = contact.fixtureB; Shape shapeA = fixtureA.shape; Shape shapeB = fixtureB.shape; num radiusA = shapeA.radius; num radiusB = shapeB.radius; Body bodyA = fixtureA.body; Body bodyB = fixtureB.body; Manifold manifold = contact.manifold; assert(manifold.pointCount > 0); TimeOfImpactConstraint constraint = constraints[i]; constraint.bodyA = bodyA; constraint.bodyB = bodyB; constraint.localNormal.setFrom(manifold.localNormal); constraint.localPoint.setFrom(manifold.localPoint); constraint.type = manifold.type; constraint.pointCount = manifold.pointCount; constraint.radius = radiusA + radiusB; for (int j = 0; j < constraint.pointCount; ++j){ ManifoldPoint cp = manifold.points[j]; constraint.localPoints[j] = cp.localPoint; } } } /** * Perform one solver iteration. Returns true if converged. */ bool solve(num baumgarte){ num minSeparation = 0; for (int i = 0; i < count; ++i){ TimeOfImpactConstraint c = constraints[i]; Body bodyA = c.bodyA; Body bodyB = c.bodyB; num massA = bodyA.mass; num massB = bodyB.mass; // Only the TimeOfImpact body should move. if (bodyA == toiBody){ massB = 0.0; } else{ massA = 0.0; } num invMassA = massA * bodyA.invMass; num invIA = massA * bodyA.invInertia; num invMassB = massB * bodyB.invMass; num invIB = massB * bodyB.invInertia; // Solve normal constraints for (int j = 0; j < c.pointCount; ++j){ psm.initialize(c, j); Vector normal = psm.normal; Vector point = psm.point; num separation = psm.separation; rA.setFrom(point).subLocal(bodyA.sweep.center); rB.setFrom(point).subLocal(bodyB.sweep.center); // Track max constraint error. minSeparation = Math.min(minSeparation, separation); // Prevent large corrections and allow slop. num C = MathBox.clamp(baumgarte * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0); // Compute the effective mass. num rnA = Vector.crossVectors(rA, normal); num rnB = Vector.crossVectors(rB, normal); num K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse num impulse = K > 0.0 ? - C / K : 0.0; P.setFrom(normal).mulLocal(impulse); temp.setFrom(P).mulLocal(invMassA); bodyA.sweep.center.subLocal(temp); bodyA.sweep.angle -= invIA * Vector.crossVectors(rA, P); bodyA.synchronizeTransform(); temp.setFrom(P).mulLocal(invMassB); bodyB.sweep.center.addLocal(temp); bodyB.sweep.angle += invIB * Vector.crossVectors(rB, P); bodyB.synchronizeTransform(); } } // We can't expect minSpeparation >= -_LINEAR_SLOP because we don't // push the separation above -_LINEAR_SLOP. return minSeparation >= -1.5 * Settings.LINEAR_SLOP; } }
Constructors
new TimeOfImpactSolver() #
TimeOfImpactSolver() : count = 0, toiBody = null, constraints = new List<TimeOfImpactConstraint>(4), // Initialize pool variables. psm = new TimeOfImpactSolverManifold(), rA = new Vector(), rB = new Vector(), P = new Vector(), temp = new Vector() { for (int i = 0; i<constraints.length; i++){ constraints[i] = new TimeOfImpactConstraint(); } }
Properties
List<TimeOfImpactConstraint> constraints #
List<TimeOfImpactConstraint> constraints;
int count #
int count;
final TimeOfImpactSolverManifold psm #
Pooling.
final TimeOfImpactSolverManifold psm;
Methods
void initialize(List<Contact> contacts, int argCount, Body argToiBody) #
void initialize(List<Contact> contacts, int argCount, Body argToiBody) { count = argCount; toiBody = argToiBody; if(count >= constraints.length){ List<TimeOfImpactConstraint> old = constraints; int newLen = Math.max(count, old.length*2); constraints = new List<TimeOfImpactConstraint>(newLen); constraints.setRange(0, old.length, old); for(int i=old.length; i<constraints.length; i++){ constraints[i] = new TimeOfImpactConstraint(); } } for (int i=0; i<count; i++) { Contact contact = contacts[i]; Fixture fixtureA = contact.fixtureA; Fixture fixtureB = contact.fixtureB; Shape shapeA = fixtureA.shape; Shape shapeB = fixtureB.shape; num radiusA = shapeA.radius; num radiusB = shapeB.radius; Body bodyA = fixtureA.body; Body bodyB = fixtureB.body; Manifold manifold = contact.manifold; assert(manifold.pointCount > 0); TimeOfImpactConstraint constraint = constraints[i]; constraint.bodyA = bodyA; constraint.bodyB = bodyB; constraint.localNormal.setFrom(manifold.localNormal); constraint.localPoint.setFrom(manifold.localPoint); constraint.type = manifold.type; constraint.pointCount = manifold.pointCount; constraint.radius = radiusA + radiusB; for (int j = 0; j < constraint.pointCount; ++j){ ManifoldPoint cp = manifold.points[j]; constraint.localPoints[j] = cp.localPoint; } } }
bool solve(num baumgarte) #
Perform one solver iteration. Returns true if converged.
bool solve(num baumgarte){ num minSeparation = 0; for (int i = 0; i < count; ++i){ TimeOfImpactConstraint c = constraints[i]; Body bodyA = c.bodyA; Body bodyB = c.bodyB; num massA = bodyA.mass; num massB = bodyB.mass; // Only the TimeOfImpact body should move. if (bodyA == toiBody){ massB = 0.0; } else{ massA = 0.0; } num invMassA = massA * bodyA.invMass; num invIA = massA * bodyA.invInertia; num invMassB = massB * bodyB.invMass; num invIB = massB * bodyB.invInertia; // Solve normal constraints for (int j = 0; j < c.pointCount; ++j){ psm.initialize(c, j); Vector normal = psm.normal; Vector point = psm.point; num separation = psm.separation; rA.setFrom(point).subLocal(bodyA.sweep.center); rB.setFrom(point).subLocal(bodyB.sweep.center); // Track max constraint error. minSeparation = Math.min(minSeparation, separation); // Prevent large corrections and allow slop. num C = MathBox.clamp(baumgarte * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0); // Compute the effective mass. num rnA = Vector.crossVectors(rA, normal); num rnB = Vector.crossVectors(rB, normal); num K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse num impulse = K > 0.0 ? - C / K : 0.0; P.setFrom(normal).mulLocal(impulse); temp.setFrom(P).mulLocal(invMassA); bodyA.sweep.center.subLocal(temp); bodyA.sweep.angle -= invIA * Vector.crossVectors(rA, P); bodyA.synchronizeTransform(); temp.setFrom(P).mulLocal(invMassB); bodyB.sweep.center.addLocal(temp); bodyB.sweep.angle += invIB * Vector.crossVectors(rB, P); bodyB.synchronizeTransform(); } } // We can't expect minSpeparation >= -_LINEAR_SLOP because we don't // push the separation above -_LINEAR_SLOP. return minSeparation >= -1.5 * Settings.LINEAR_SLOP; }