Show:

File: src/collision/Resolver.js

                                /**
                                * The `Matter.Resolver` module contains methods for resolving collision pairs.
                                *
                                * @class Resolver
                                */
                                
                                var Resolver = {};
                                
                                module.exports = Resolver;
                                
                                var Vertices = require('../geometry/Vertices');
                                var Common = require('../core/Common');
                                var Bounds = require('../geometry/Bounds');
                                
                                (function() {
                                
                                    Resolver._restingThresh = 2;
                                    Resolver._restingThreshTangent = Math.sqrt(6);
                                    Resolver._positionDampen = 0.9;
                                    Resolver._positionWarming = 0.8;
                                    Resolver._frictionNormalMultiplier = 5;
                                    Resolver._frictionMaxStatic = Number.MAX_VALUE;
                                
                                    /**
                                     * Prepare pairs for position solving.
                                     * @method preSolvePosition
                                     * @param {pair[]} pairs
                                     */
                                    Resolver.preSolvePosition = function(pairs) {
                                        var i,
                                            pair,
                                            activeCount,
                                            pairsLength = pairs.length;
                                
                                        // find total contacts on each body
                                        for (i = 0; i < pairsLength; i++) {
                                            pair = pairs[i];
                                            
                                            if (!pair.isActive)
                                                continue;
                                            
                                            activeCount = pair.activeContacts.length;
                                            pair.collision.parentA.totalContacts += activeCount;
                                            pair.collision.parentB.totalContacts += activeCount;
                                        }
                                    };
                                
                                    /**
                                     * Find a solution for pair positions.
                                     * @method solvePosition
                                     * @param {pair[]} pairs
                                     * @param {number} delta
                                     * @param {number} [damping=1]
                                     */
                                    Resolver.solvePosition = function(pairs, delta, damping) {
                                        var i,
                                            pair,
                                            collision,
                                            bodyA,
                                            bodyB,
                                            normal,
                                            contactShare,
                                            positionImpulse,
                                            positionDampen = Resolver._positionDampen * (damping || 1),
                                            slopDampen = Common.clamp(delta / Common._baseDelta, 0, 1),
                                            pairsLength = pairs.length;
                                
                                        // find impulses required to resolve penetration
                                        for (i = 0; i < pairsLength; i++) {
                                            pair = pairs[i];
                                            
                                            if (!pair.isActive || pair.isSensor)
                                                continue;
                                
                                            collision = pair.collision;
                                            bodyA = collision.parentA;
                                            bodyB = collision.parentB;
                                            normal = collision.normal;
                                
                                            // get current separation between body edges involved in collision
                                            pair.separation = 
                                                normal.x * (bodyB.positionImpulse.x + collision.penetration.x - bodyA.positionImpulse.x)
                                                + normal.y * (bodyB.positionImpulse.y + collision.penetration.y - bodyA.positionImpulse.y);
                                        }
                                        
                                        for (i = 0; i < pairsLength; i++) {
                                            pair = pairs[i];
                                
                                            if (!pair.isActive || pair.isSensor)
                                                continue;
                                            
                                            collision = pair.collision;
                                            bodyA = collision.parentA;
                                            bodyB = collision.parentB;
                                            normal = collision.normal;
                                            positionImpulse = pair.separation - pair.slop * slopDampen;
                                
                                            if (bodyA.isStatic || bodyB.isStatic)
                                                positionImpulse *= 2;
                                            
                                            if (!(bodyA.isStatic || bodyA.isSleeping)) {
                                                contactShare = positionDampen / bodyA.totalContacts;
                                                bodyA.positionImpulse.x += normal.x * positionImpulse * contactShare;
                                                bodyA.positionImpulse.y += normal.y * positionImpulse * contactShare;
                                            }
                                
                                            if (!(bodyB.isStatic || bodyB.isSleeping)) {
                                                contactShare = positionDampen / bodyB.totalContacts;
                                                bodyB.positionImpulse.x -= normal.x * positionImpulse * contactShare;
                                                bodyB.positionImpulse.y -= normal.y * positionImpulse * contactShare;
                                            }
                                        }
                                    };
                                
                                    /**
                                     * Apply position resolution.
                                     * @method postSolvePosition
                                     * @param {body[]} bodies
                                     */
                                    Resolver.postSolvePosition = function(bodies) {
                                        var positionWarming = Resolver._positionWarming,
                                            bodiesLength = bodies.length,
                                            verticesTranslate = Vertices.translate,
                                            boundsUpdate = Bounds.update;
                                
                                        for (var i = 0; i < bodiesLength; i++) {
                                            var body = bodies[i],
                                                positionImpulse = body.positionImpulse,
                                                positionImpulseX = positionImpulse.x,
                                                positionImpulseY = positionImpulse.y,
                                                velocity = body.velocity;
                                
                                            // reset contact count
                                            body.totalContacts = 0;
                                
                                            if (positionImpulseX !== 0 || positionImpulseY !== 0) {
                                                // update body geometry
                                                for (var j = 0; j < body.parts.length; j++) {
                                                    var part = body.parts[j];
                                                    verticesTranslate(part.vertices, positionImpulse);
                                                    boundsUpdate(part.bounds, part.vertices, velocity);
                                                    part.position.x += positionImpulseX;
                                                    part.position.y += positionImpulseY;
                                                }
                                
                                                // move the body without changing velocity
                                                body.positionPrev.x += positionImpulseX;
                                                body.positionPrev.y += positionImpulseY;
                                
                                                if (positionImpulseX * velocity.x + positionImpulseY * velocity.y < 0) {
                                                    // reset cached impulse if the body has velocity along it
                                                    positionImpulse.x = 0;
                                                    positionImpulse.y = 0;
                                                } else {
                                                    // warm the next iteration
                                                    positionImpulse.x *= positionWarming;
                                                    positionImpulse.y *= positionWarming;
                                                }
                                            }
                                        }
                                    };
                                
                                    /**
                                     * Prepare pairs for velocity solving.
                                     * @method preSolveVelocity
                                     * @param {pair[]} pairs
                                     */
                                    Resolver.preSolveVelocity = function(pairs) {
                                        var pairsLength = pairs.length,
                                            i,
                                            j;
                                        
                                        for (i = 0; i < pairsLength; i++) {
                                            var pair = pairs[i];
                                            
                                            if (!pair.isActive || pair.isSensor)
                                                continue;
                                            
                                            var contacts = pair.activeContacts,
                                                contactsLength = contacts.length,
                                                collision = pair.collision,
                                                bodyA = collision.parentA,
                                                bodyB = collision.parentB,
                                                normal = collision.normal,
                                                tangent = collision.tangent;
                                    
                                            // resolve each contact
                                            for (j = 0; j < contactsLength; j++) {
                                                var contact = contacts[j],
                                                    contactVertex = contact.vertex,
                                                    normalImpulse = contact.normalImpulse,
                                                    tangentImpulse = contact.tangentImpulse;
                                    
                                                if (normalImpulse !== 0 || tangentImpulse !== 0) {
                                                    // total impulse from contact
                                                    var impulseX = normal.x * normalImpulse + tangent.x * tangentImpulse,
                                                        impulseY = normal.y * normalImpulse + tangent.y * tangentImpulse;
                                                    
                                                    // apply impulse from contact
                                                    if (!(bodyA.isStatic || bodyA.isSleeping)) {
                                                        bodyA.positionPrev.x += impulseX * bodyA.inverseMass;
                                                        bodyA.positionPrev.y += impulseY * bodyA.inverseMass;
                                                        bodyA.anglePrev += bodyA.inverseInertia * (
                                                            (contactVertex.x - bodyA.position.x) * impulseY
                                                            - (contactVertex.y - bodyA.position.y) * impulseX
                                                        );
                                                    }
                                    
                                                    if (!(bodyB.isStatic || bodyB.isSleeping)) {
                                                        bodyB.positionPrev.x -= impulseX * bodyB.inverseMass;
                                                        bodyB.positionPrev.y -= impulseY * bodyB.inverseMass;
                                                        bodyB.anglePrev -= bodyB.inverseInertia * (
                                                            (contactVertex.x - bodyB.position.x) * impulseY 
                                                            - (contactVertex.y - bodyB.position.y) * impulseX
                                                        );
                                                    }
                                                }
                                            }
                                        }
                                    };
                                
                                    /**
                                     * Find a solution for pair velocities.
                                     * @method solveVelocity
                                     * @param {pair[]} pairs
                                     * @param {number} delta
                                     */
                                    Resolver.solveVelocity = function(pairs, delta) {
                                        var timeScale = delta / Common._baseDelta,
                                            timeScaleSquared = timeScale * timeScale,
                                            timeScaleCubed = timeScaleSquared * timeScale,
                                            restingThresh = -Resolver._restingThresh * timeScale,
                                            restingThreshTangent = Resolver._restingThreshTangent,
                                            frictionNormalMultiplier = Resolver._frictionNormalMultiplier * timeScale,
                                            frictionMaxStatic = Resolver._frictionMaxStatic,
                                            pairsLength = pairs.length,
                                            tangentImpulse,
                                            maxFriction,
                                            i,
                                            j;
                                
                                        for (i = 0; i < pairsLength; i++) {
                                            var pair = pairs[i];
                                            
                                            if (!pair.isActive || pair.isSensor)
                                                continue;
                                            
                                            var collision = pair.collision,
                                                bodyA = collision.parentA,
                                                bodyB = collision.parentB,
                                                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,
                                                contactsLength = contacts.length,
                                                contactShare = 1 / contactsLength,
                                                inverseMassTotal = bodyA.inverseMass + bodyB.inverseMass,
                                                friction = pair.friction * pair.frictionStatic * frictionNormalMultiplier;
                                
                                            // update body velocities
                                            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 (j = 0; j < contactsLength; j++) {
                                                var contact = contacts[j],
                                                    contactVertex = contact.vertex;
                                
                                                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;
                                 
                                                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 normalOverlap = pair.separation + normalVelocity;
                                                var normalForce = Math.min(normalOverlap, 1);
                                                normalForce = normalOverlap < 0 ? 0 : normalForce;
                                
                                                var frictionLimit = normalForce * friction;
                                
                                                if (tangentVelocity < -frictionLimit || tangentVelocity > frictionLimit) {
                                                    maxFriction = (tangentVelocity > 0 ? tangentVelocity : -tangentVelocity);
                                                    tangentImpulse = pair.friction * (tangentVelocity > 0 ? 1 : -1) * timeScaleCubed;
                                                    
                                                    if (tangentImpulse < -maxFriction) {
                                                        tangentImpulse = -maxFriction;
                                                    } else if (tangentImpulse > maxFriction) {
                                                        tangentImpulse = maxFriction;
                                                    }
                                                } else {
                                                    tangentImpulse = tangentVelocity;
                                                    maxFriction = frictionMaxStatic;
                                                }
                                
                                                // 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);
                                
                                                // raw impulses
                                                var normalImpulse = (1 + pair.restitution) * normalVelocity * share;
                                                tangentImpulse *= share;
                                
                                                // handle high velocity and resting collisions separately
                                                if (normalVelocity < restingThresh) {
                                                    // 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 += normalImpulse;
                                                    if (contact.normalImpulse > 0) contact.normalImpulse = 0;
                                                    normalImpulse = contact.normalImpulse - contactNormalImpulse;
                                                }
                                
                                                // handle high velocity and resting collisions separately
                                                if (tangentVelocity < -restingThreshTangent || 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 += tangentImpulse;
                                                    if (contact.tangentImpulse < -maxFriction) contact.tangentImpulse = -maxFriction;
                                                    if (contact.tangentImpulse > maxFriction) contact.tangentImpulse = maxFriction;
                                                    tangentImpulse = contact.tangentImpulse - contactTangentImpulse;
                                                }
                                
                                                // total impulse from contact
                                                var impulseX = normalX * normalImpulse + tangentX * tangentImpulse,
                                                    impulseY = normalY * normalImpulse + tangentY * tangentImpulse;
                                                
                                                // apply impulse from contact
                                                if (!(bodyA.isStatic || bodyA.isSleeping)) {
                                                    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 -= impulseX * bodyB.inverseMass;
                                                    bodyB.positionPrev.y -= impulseY * bodyB.inverseMass;
                                                    bodyB.anglePrev -= (offsetBX * impulseY - offsetBY * impulseX) * bodyB.inverseInertia;
                                                }
                                            }
                                        }
                                    };
                                
                                })();