mirror of
https://github.com/liabru/matter-js.git
synced 2025-01-21 17:14:38 -05:00
optimised Resolver.solveVelocity
This commit is contained in:
liabru
parent
0b07a31944
commit
e4b35d3961
1 changed files with 79 additions and 54 deletions
|
|
@ -228,14 +228,17 @@ var Bounds = require('../geometry/Bounds');
|
|||
*/
|
||||
Resolver.solveVelocity = function(pairs, timeScale) {
|
||||
var timeScaleSquared = timeScale * timeScale,
|
||||
impulse = Vector._temp[0],
|
||||
tempA = Vector._temp[1],
|
||||
tempB = Vector._temp[2],
|
||||
tempC = Vector._temp[3],
|
||||
tempD = Vector._temp[4],
|
||||
tempE = Vector._temp[5];
|
||||
|
||||
for (var i = 0; i < pairs.length; i++) {
|
||||
restingThresh = Resolver._restingThresh * timeScaleSquared,
|
||||
frictionNormalMultiplier = Resolver._frictionNormalMultiplier,
|
||||
restingThreshTangent = Resolver._restingThreshTangent * timeScaleSquared,
|
||||
NumberMaxValue = Number.MAX_VALUE,
|
||||
pairsLength = pairs.length,
|
||||
tangentImpulse,
|
||||
maxFriction,
|
||||
i,
|
||||
j;
|
||||
|
||||
for (i = 0; i < pairsLength; i++) {
|
||||
var pair = pairs[i];
|
||||
|
||||
if (!pair.isActive || pair.isSensor)
|
||||
|
|
@ -244,97 +247,119 @@ var Bounds = require('../geometry/Bounds');
|
|||
var collision = pair.collision,
|
||||
bodyA = collision.parentA,
|
||||
bodyB = collision.parentB,
|
||||
normal = collision.normal,
|
||||
tangent = collision.tangent,
|
||||
bodyAVelocity = bodyA.velocity,
|
||||
bodyBVelocity = bodyB.velocity,
|
||||
normalX = collision.normal.x,
|
||||
normalY = collision.normal.y,
|
||||
tangentX = collision.tangent.x,
|
||||
tangentY = collision.tangent.y,
|
||||
contacts = pair.activeContacts,
|
||||
contactShare = 1 / contacts.length;
|
||||
contactsLength = contacts.length,
|
||||
contactShare = 1 / contactsLength,
|
||||
inverseMassTotal = bodyA.inverseMass + bodyB.inverseMass,
|
||||
friction = pair.friction * pair.frictionStatic * frictionNormalMultiplier * timeScaleSquared;
|
||||
|
||||
// update body velocities
|
||||
bodyA.velocity.x = bodyA.position.x - bodyA.positionPrev.x;
|
||||
bodyA.velocity.y = bodyA.position.y - bodyA.positionPrev.y;
|
||||
bodyB.velocity.x = bodyB.position.x - bodyB.positionPrev.x;
|
||||
bodyB.velocity.y = bodyB.position.y - bodyB.positionPrev.y;
|
||||
bodyAVelocity.x = bodyA.position.x - bodyA.positionPrev.x;
|
||||
bodyAVelocity.y = bodyA.position.y - bodyA.positionPrev.y;
|
||||
bodyBVelocity.x = bodyB.position.x - bodyB.positionPrev.x;
|
||||
bodyBVelocity.y = bodyB.position.y - bodyB.positionPrev.y;
|
||||
bodyA.angularVelocity = bodyA.angle - bodyA.anglePrev;
|
||||
bodyB.angularVelocity = bodyB.angle - bodyB.anglePrev;
|
||||
|
||||
// resolve each contact
|
||||
for (var j = 0; j < contacts.length; j++) {
|
||||
for (j = 0; j < contactsLength; j++) {
|
||||
var contact = contacts[j],
|
||||
contactVertex = contact.vertex,
|
||||
offsetA = Vector.sub(contactVertex, bodyA.position, tempA),
|
||||
offsetB = Vector.sub(contactVertex, bodyB.position, tempB),
|
||||
velocityPointA = Vector.add(bodyA.velocity, Vector.mult(Vector.perp(offsetA), bodyA.angularVelocity), tempC),
|
||||
velocityPointB = Vector.add(bodyB.velocity, Vector.mult(Vector.perp(offsetB), bodyB.angularVelocity), tempD),
|
||||
relativeVelocity = Vector.sub(velocityPointA, velocityPointB, tempE),
|
||||
normalVelocity = Vector.dot(normal, relativeVelocity);
|
||||
contactVertex = contact.vertex;
|
||||
|
||||
var tangentVelocity = Vector.dot(tangent, relativeVelocity),
|
||||
tangentSpeed = Math.abs(tangentVelocity),
|
||||
tangentVelocityDirection = Common.sign(tangentVelocity);
|
||||
var offsetAX = contactVertex.x - bodyA.position.x,
|
||||
offsetAY = contactVertex.y - bodyA.position.y,
|
||||
offsetBX = contactVertex.x - bodyB.position.x,
|
||||
offsetBY = contactVertex.y - bodyB.position.y;
|
||||
|
||||
// raw impulses
|
||||
var normalImpulse = (1 + pair.restitution) * normalVelocity,
|
||||
normalForce = Common.clamp(pair.separation + normalVelocity, 0, 1) * Resolver._frictionNormalMultiplier;
|
||||
var velocityPointAX = bodyAVelocity.x - offsetAY * bodyA.angularVelocity,
|
||||
velocityPointAY = bodyAVelocity.y + offsetAX * bodyA.angularVelocity,
|
||||
velocityPointBX = bodyBVelocity.x - offsetBY * bodyB.angularVelocity,
|
||||
velocityPointBY = bodyBVelocity.y + offsetBX * bodyB.angularVelocity;
|
||||
|
||||
var relativeVelocityX = velocityPointAX - velocityPointBX,
|
||||
relativeVelocityY = velocityPointAY - velocityPointBY;
|
||||
|
||||
var normalVelocity = normalX * relativeVelocityX + normalY * relativeVelocityY,
|
||||
tangentVelocity = tangentX * relativeVelocityX + tangentY * relativeVelocityY;
|
||||
|
||||
// coulomb friction
|
||||
var tangentImpulse = tangentVelocity,
|
||||
maxFriction = Infinity;
|
||||
var normalOverlap = pair.separation + normalVelocity;
|
||||
var normalForce = normalOverlap > 1 ? 1 : normalOverlap;
|
||||
normalForce = normalOverlap < 0 ? 0 : normalForce;
|
||||
|
||||
var frictionLimit = normalForce * friction;
|
||||
|
||||
if (tangentSpeed > pair.friction * pair.frictionStatic * normalForce * timeScaleSquared) {
|
||||
maxFriction = tangentSpeed;
|
||||
tangentImpulse = Common.clamp(
|
||||
pair.friction * tangentVelocityDirection * timeScaleSquared,
|
||||
-maxFriction, maxFriction
|
||||
);
|
||||
if (tangentVelocity > frictionLimit || -tangentVelocity > frictionLimit) {
|
||||
maxFriction = tangentVelocity > 0 ? tangentVelocity : -tangentVelocity;
|
||||
tangentImpulse = pair.friction * (tangentVelocity > 0 ? 1 : -1) * timeScaleSquared;
|
||||
|
||||
if (tangentImpulse < -maxFriction) {
|
||||
tangentImpulse = -maxFriction;
|
||||
} else if (tangentImpulse > maxFriction) {
|
||||
tangentImpulse = maxFriction;
|
||||
}
|
||||
} else {
|
||||
tangentImpulse = tangentVelocity;
|
||||
maxFriction = NumberMaxValue;
|
||||
}
|
||||
|
||||
// modify impulses accounting for mass, inertia and offset
|
||||
var oAcN = Vector.cross(offsetA, normal),
|
||||
oBcN = Vector.cross(offsetB, normal),
|
||||
share = contactShare / (bodyA.inverseMass + bodyB.inverseMass + bodyA.inverseInertia * oAcN * oAcN + bodyB.inverseInertia * oBcN * oBcN);
|
||||
// account for mass, inertia and contact offset
|
||||
var oAcN = offsetAX * normalY - offsetAY * normalX,
|
||||
oBcN = offsetBX * normalY - offsetBY * normalX,
|
||||
share = contactShare / (inverseMassTotal + bodyA.inverseInertia * oAcN * oAcN + bodyB.inverseInertia * oBcN * oBcN);
|
||||
|
||||
normalImpulse *= share;
|
||||
// raw impulses
|
||||
var normalImpulse = (1 + pair.restitution) * normalVelocity * share;
|
||||
tangentImpulse *= share;
|
||||
|
||||
// handle high velocity and resting collisions separately
|
||||
if (normalVelocity < 0 && normalVelocity * normalVelocity > Resolver._restingThresh * timeScaleSquared) {
|
||||
if (normalVelocity * normalVelocity > restingThresh && normalVelocity < 0) {
|
||||
// high normal velocity so clear cached contact normal impulse
|
||||
contact.normalImpulse = 0;
|
||||
} else {
|
||||
// solve resting collision constraints using Erin Catto's method (GDC08)
|
||||
// impulse constraint tends to 0
|
||||
var contactNormalImpulse = contact.normalImpulse;
|
||||
contact.normalImpulse = Math.min(contact.normalImpulse + normalImpulse, 0);
|
||||
contact.normalImpulse += normalImpulse;
|
||||
contact.normalImpulse = contact.normalImpulse < 0 ? contact.normalImpulse : 0;
|
||||
normalImpulse = contact.normalImpulse - contactNormalImpulse;
|
||||
}
|
||||
|
||||
// handle high velocity and resting collisions separately
|
||||
if (tangentVelocity * tangentVelocity > Resolver._restingThreshTangent * timeScaleSquared) {
|
||||
if (tangentVelocity * tangentVelocity > restingThreshTangent) {
|
||||
// high tangent velocity so clear cached contact tangent impulse
|
||||
contact.tangentImpulse = 0;
|
||||
} else {
|
||||
// solve resting collision constraints using Erin Catto's method (GDC08)
|
||||
// tangent impulse tends to -tangentSpeed or +tangentSpeed
|
||||
var contactTangentImpulse = contact.tangentImpulse;
|
||||
contact.tangentImpulse = Common.clamp(contact.tangentImpulse + tangentImpulse, -maxFriction, maxFriction);
|
||||
contact.tangentImpulse += tangentImpulse;
|
||||
if (contact.tangentImpulse < -maxFriction) contact.tangentImpulse = -maxFriction;
|
||||
if (contact.tangentImpulse > maxFriction) contact.tangentImpulse = maxFriction;
|
||||
tangentImpulse = contact.tangentImpulse - contactTangentImpulse;
|
||||
}
|
||||
|
||||
// total impulse from contact
|
||||
impulse.x = (normal.x * normalImpulse) + (tangent.x * tangentImpulse);
|
||||
impulse.y = (normal.y * normalImpulse) + (tangent.y * tangentImpulse);
|
||||
var impulseX = normalX * normalImpulse + tangentX * tangentImpulse,
|
||||
impulseY = normalY * normalImpulse + tangentY * tangentImpulse;
|
||||
|
||||
// apply impulse from contact
|
||||
if (!(bodyA.isStatic || bodyA.isSleeping)) {
|
||||
bodyA.positionPrev.x += impulse.x * bodyA.inverseMass;
|
||||
bodyA.positionPrev.y += impulse.y * bodyA.inverseMass;
|
||||
bodyA.anglePrev += Vector.cross(offsetA, impulse) * bodyA.inverseInertia;
|
||||
bodyA.positionPrev.x += impulseX * bodyA.inverseMass;
|
||||
bodyA.positionPrev.y += impulseY * bodyA.inverseMass;
|
||||
bodyA.anglePrev += (offsetAX * impulseY - offsetAY * impulseX) * bodyA.inverseInertia;
|
||||
}
|
||||
|
||||
if (!(bodyB.isStatic || bodyB.isSleeping)) {
|
||||
bodyB.positionPrev.x -= impulse.x * bodyB.inverseMass;
|
||||
bodyB.positionPrev.y -= impulse.y * bodyB.inverseMass;
|
||||
bodyB.anglePrev -= Vector.cross(offsetB, impulse) * bodyB.inverseInertia;
|
||||
bodyB.positionPrev.x -= impulseX * bodyB.inverseMass;
|
||||
bodyB.positionPrev.y -= impulseY * bodyB.inverseMass;
|
||||
bodyB.anglePrev -= (offsetBX * impulseY - offsetBY * impulseX) * bodyB.inverseInertia;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue