foster/liabru-matter-js
0
0
Fork 0
mirror of https://github.com/liabru/matter-js.git synced 2025-02-01 18:24:54 -05:00
Code Issues Wiki Activity

optimised Resolver.solveVelocity

Browse source
This commit is contained in:
liabru 2021-05-01 14:16:39 +01:00
parent 0b07a31944
commit e4b35d3961
1 changed files with 79 additions and 54 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

133
src/collision/Resolver.js
View file

@ -228,14 +228,17 @@ var Bounds = require('../geometry/Bounds');
*/ */
Resolver.solveVelocity = function(pairs, timeScale) { Resolver.solveVelocity = function(pairs, timeScale) {
var timeScaleSquared = timeScale * timeScale, var timeScaleSquared = timeScale * timeScale,
impulse = Vector._temp[0], restingThresh = Resolver._restingThresh * timeScaleSquared,
tempA = Vector._temp[1], frictionNormalMultiplier = Resolver._frictionNormalMultiplier,
tempB = Vector._temp[2], restingThreshTangent = Resolver._restingThreshTangent * timeScaleSquared,
tempC = Vector._temp[3], NumberMaxValue = Number.MAX_VALUE,
tempD = Vector._temp[4], pairsLength = pairs.length,
tempE = Vector._temp[5]; tangentImpulse,
maxFriction,
for (var i = 0; i < pairs.length; i++) { i,
j;
for (i = 0; i < pairsLength; i++) {
var pair = pairs[i]; var pair = pairs[i];
if (!pair.isActive || pair.isSensor) if (!pair.isActive || pair.isSensor)
@ -244,97 +247,119 @@ var Bounds = require('../geometry/Bounds');
var collision = pair.collision, var collision = pair.collision,
bodyA = collision.parentA, bodyA = collision.parentA,
bodyB = collision.parentB, bodyB = collision.parentB,
normal = collision.normal, bodyAVelocity = bodyA.velocity,
tangent = collision.tangent, bodyBVelocity = bodyB.velocity,
normalX = collision.normal.x,
normalY = collision.normal.y,
tangentX = collision.tangent.x,
tangentY = collision.tangent.y,
contacts = pair.activeContacts, 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 // update body velocities
bodyA.velocity.x = bodyA.position.x - bodyA.positionPrev.x; bodyAVelocity.x = bodyA.position.x - bodyA.positionPrev.x;
bodyA.velocity.y = bodyA.position.y - bodyA.positionPrev.y; bodyAVelocity.y = bodyA.position.y - bodyA.positionPrev.y;
bodyB.velocity.x = bodyB.position.x - bodyB.positionPrev.x; bodyBVelocity.x = bodyB.position.x - bodyB.positionPrev.x;
bodyB.velocity.y = bodyB.position.y - bodyB.positionPrev.y; bodyBVelocity.y = bodyB.position.y - bodyB.positionPrev.y;
bodyA.angularVelocity = bodyA.angle - bodyA.anglePrev; bodyA.angularVelocity = bodyA.angle - bodyA.anglePrev;
bodyB.angularVelocity = bodyB.angle - bodyB.anglePrev; bodyB.angularVelocity = bodyB.angle - bodyB.anglePrev;
// resolve each contact // resolve each contact
for (var j = 0; j < contacts.length; j++) { for (j = 0; j < contactsLength; j++) {
var contact = contacts[j], var contact = contacts[j],
contactVertex = contact.vertex, 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);
var tangentVelocity = Vector.dot(tangent, relativeVelocity), var offsetAX = contactVertex.x - bodyA.position.x,
tangentSpeed = Math.abs(tangentVelocity), offsetAY = contactVertex.y - bodyA.position.y,
tangentVelocityDirection = Common.sign(tangentVelocity); offsetBX = contactVertex.x - bodyB.position.x,
offsetBY = contactVertex.y - bodyB.position.y;
// raw impulses var velocityPointAX = bodyAVelocity.x - offsetAY * bodyA.angularVelocity,
var normalImpulse = (1 + pair.restitution) * normalVelocity, velocityPointAY = bodyAVelocity.y + offsetAX * bodyA.angularVelocity,
normalForce = Common.clamp(pair.separation + normalVelocity, 0, 1) * Resolver._frictionNormalMultiplier; 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 // coulomb friction
var tangentImpulse = tangentVelocity, var normalOverlap = pair.separation + normalVelocity;
maxFriction = Infinity; var normalForce = normalOverlap > 1 ? 1 : normalOverlap;
normalForce = normalOverlap < 0 ? 0 : normalForce;
var frictionLimit = normalForce * friction;
if (tangentSpeed > pair.friction * pair.frictionStatic * normalForce * timeScaleSquared) { if (tangentVelocity > frictionLimit || -tangentVelocity > frictionLimit) {
maxFriction = tangentSpeed; maxFriction = tangentVelocity > 0 ? tangentVelocity : -tangentVelocity;
tangentImpulse = Common.clamp( tangentImpulse = pair.friction * (tangentVelocity > 0 ? 1 : -1) * timeScaleSquared;
pair.friction * tangentVelocityDirection * timeScaleSquared,
-maxFriction, maxFriction if (tangentImpulse < -maxFriction) {
); tangentImpulse = -maxFriction;
} else if (tangentImpulse > maxFriction) {
tangentImpulse = maxFriction;
}
} else {
tangentImpulse = tangentVelocity;
maxFriction = NumberMaxValue;
} }
// modify impulses accounting for mass, inertia and offset // account for mass, inertia and contact offset
var oAcN = Vector.cross(offsetA, normal), var oAcN = offsetAX * normalY - offsetAY * normalX,
oBcN = Vector.cross(offsetB, normal), oBcN = offsetBX * normalY - offsetBY * normalX,
share = contactShare / (bodyA.inverseMass + bodyB.inverseMass + bodyA.inverseInertia * oAcN * oAcN + bodyB.inverseInertia * oBcN * oBcN); share = contactShare / (inverseMassTotal + bodyA.inverseInertia * oAcN * oAcN + bodyB.inverseInertia * oBcN * oBcN);
normalImpulse *= share; // raw impulses
var normalImpulse = (1 + pair.restitution) * normalVelocity * share;
tangentImpulse *= share; tangentImpulse *= share;
// handle high velocity and resting collisions separately // 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 // high normal velocity so clear cached contact normal impulse
contact.normalImpulse = 0; contact.normalImpulse = 0;
} else { } else {
// solve resting collision constraints using Erin Catto's method (GDC08) // solve resting collision constraints using Erin Catto's method (GDC08)
// impulse constraint tends to 0 // impulse constraint tends to 0
var contactNormalImpulse = contact.normalImpulse; 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; normalImpulse = contact.normalImpulse - contactNormalImpulse;
} }
// handle high velocity and resting collisions separately // 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 // high tangent velocity so clear cached contact tangent impulse
contact.tangentImpulse = 0; contact.tangentImpulse = 0;
} else { } else {
// solve resting collision constraints using Erin Catto's method (GDC08) // solve resting collision constraints using Erin Catto's method (GDC08)
// tangent impulse tends to -tangentSpeed or +tangentSpeed // tangent impulse tends to -tangentSpeed or +tangentSpeed
var contactTangentImpulse = contact.tangentImpulse; 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; tangentImpulse = contact.tangentImpulse - contactTangentImpulse;
} }
// total impulse from contact // total impulse from contact
impulse.x = (normal.x * normalImpulse) + (tangent.x * tangentImpulse); var impulseX = normalX * normalImpulse + tangentX * tangentImpulse,
impulse.y = (normal.y * normalImpulse) + (tangent.y * tangentImpulse); impulseY = normalY * normalImpulse + tangentY * tangentImpulse;
// apply impulse from contact // apply impulse from contact
if (!(bodyA.isStatic || bodyA.isSleeping)) { if (!(bodyA.isStatic || bodyA.isSleeping)) {
bodyA.positionPrev.x += impulse.x * bodyA.inverseMass; bodyA.positionPrev.x += impulseX * bodyA.inverseMass;
bodyA.positionPrev.y += impulse.y * bodyA.inverseMass; bodyA.positionPrev.y += impulseY * bodyA.inverseMass;
bodyA.anglePrev += Vector.cross(offsetA, impulse) * bodyA.inverseInertia; bodyA.anglePrev += (offsetAX * impulseY - offsetAY * impulseX) * bodyA.inverseInertia;
} }
if (!(bodyB.isStatic || bodyB.isSleeping)) { if (!(bodyB.isStatic || bodyB.isSleeping)) {
bodyB.positionPrev.x -= impulse.x * bodyB.inverseMass; bodyB.positionPrev.x -= impulseX * bodyB.inverseMass;
bodyB.positionPrev.y -= impulse.y * bodyB.inverseMass; bodyB.positionPrev.y -= impulseY * bodyB.inverseMass;
bodyB.anglePrev -= Vector.cross(offsetB, impulse) * bodyB.inverseInertia; bodyB.anglePrev -= (offsetBX * impulseY - offsetBY * impulseX) * bodyB.inverseInertia;
} }
} }
} }