p2s
Version:
A JavaScript 2D physics engine.
1,721 lines (1,445 loc) • 69.4 kB
JavaScript
var vec2 = require('../math/vec2')
, sub = vec2.subtract
, add = vec2.add
, dot = vec2.dot
, rotate = vec2.rotate
, normalize = vec2.normalize
, copy = vec2.copy
, scale = vec2.scale
, squaredLength = vec2.squaredLength
, createVec2 = vec2.create
, ContactEquationPool = require('../utils/ContactEquationPool')
, FrictionEquationPool = require('../utils/FrictionEquationPool')
, TupleDictionary = require('../utils/TupleDictionary')
, Circle = require('../shapes/Circle')
, Convex = require('../shapes/Convex')
, Shape = require('../shapes/Shape')
, Box = require('../shapes/Box');
module.exports = Narrowphase;
// Temp things
var yAxis = vec2.fromValues(0,1);
var tmp1 = createVec2()
, tmp2 = createVec2()
, tmp3 = createVec2()
, tmp4 = createVec2()
, tmp5 = createVec2()
, tmp6 = createVec2()
, tmp7 = createVec2()
, tmp8 = createVec2()
, tmp9 = createVec2()
, tmp10 = createVec2()
, tmp11 = createVec2()
, tmp12 = createVec2()
, tmp13 = createVec2()
, tmp14 = createVec2()
, tmp15 = createVec2()
, tmpArray = [];
/**
* Narrowphase. Creates contacts and friction given shapes and transforms.
* @class Narrowphase
* @constructor
*/
function Narrowphase(){
/**
* @property contactEquations
* @type {Array}
*/
this.contactEquations = [];
/**
* @property frictionEquations
* @type {Array}
*/
this.frictionEquations = [];
/**
* Whether to make friction equations in the upcoming contacts.
* @property enableFriction
* @type {Boolean}
*/
this.enableFriction = true;
/**
* Whether to make equations enabled in upcoming contacts.
* @property enabledEquations
* @type {Boolean}
*/
this.enabledEquations = true;
/**
* The friction slip force to use when creating friction equations.
* @property slipForce
* @type {Number}
*/
this.slipForce = 10.0;
/**
* Keeps track of the allocated ContactEquations.
* @property {ContactEquationPool} contactEquationPool
*
* @example
*
* // Allocate a few equations before starting the simulation.
* // This way, no contact objects need to be created on the fly in the game loop.
* world.narrowphase.contactEquationPool.resize(1024);
* world.narrowphase.frictionEquationPool.resize(1024);
*/
this.contactEquationPool = new ContactEquationPool({ size: 32 });
/**
* Keeps track of the allocated ContactEquations.
* @property {FrictionEquationPool} frictionEquationPool
*/
this.frictionEquationPool = new FrictionEquationPool({ size: 64 });
/**
* Enable reduction of friction equations. If disabled, a box on a plane will generate 2 contact equations and 2 friction equations. If enabled, there will be only one friction equation. Same kind of simplifications are made for all collision types.
* @property enableFrictionReduction
* @type {Boolean}
* @deprecated This flag will be removed when the feature is stable enough.
* @default true
*/
this.enableFrictionReduction = true;
/**
* Keeps track of the colliding bodies last step.
* @private
* @property collidingBodiesLastStep
* @type {TupleDictionary}
*/
this.collidingBodiesLastStep = new TupleDictionary();
/**
* @property currentContactMaterial
* @type {ContactMaterial}
*/
this.currentContactMaterial = null;
}
var bodiesOverlap_shapePositionA = createVec2();
var bodiesOverlap_shapePositionB = createVec2();
/**
* @method bodiesOverlap
* @param {Body} bodyA
* @param {Body} bodyB
* @param {boolean} [checkCollisionMasks=false]
* @return {Boolean}
*/
Narrowphase.prototype.bodiesOverlap = function(bodyA, bodyB, checkCollisionMasks){
var shapePositionA = bodiesOverlap_shapePositionA;
var shapePositionB = bodiesOverlap_shapePositionB;
// Loop over all shapes of bodyA
for(var k=0, Nshapesi=bodyA.shapes.length; k!==Nshapesi; k++){
var shapeA = bodyA.shapes[k];
// All shapes of body j
for(var l=0, Nshapesj=bodyB.shapes.length; l!==Nshapesj; l++){
var shapeB = bodyB.shapes[l];
// Check collision groups and masks
if(checkCollisionMasks && !((shapeA.collisionGroup & shapeB.collisionMask) !== 0 && (shapeB.collisionGroup & shapeA.collisionMask) !== 0)){
return;
}
bodyA.toWorldFrame(shapePositionA, shapeA.position);
bodyB.toWorldFrame(shapePositionB, shapeB.position);
if(shapeA.type <= shapeB.type)
{
if(this[shapeA.type | shapeB.type](
bodyA,
shapeA,
shapePositionA,
shapeA.angle + bodyA.angle,
bodyB,
shapeB,
shapePositionB,
shapeB.angle + bodyB.angle,
true
)){
return true;
}
}
else
{
if(this[shapeA.type | shapeB.type](
bodyB,
shapeB,
shapePositionB,
shapeB.angle + bodyB.angle,
bodyA,
shapeA,
shapePositionA,
shapeA.angle + bodyA.angle,
true
)){
return true;
}
}
}
}
return false;
};
/**
* Check if the bodies were in contact since the last reset().
* @method collidedLastStep
* @param {Body} bodyA
* @param {Body} bodyB
* @return {Boolean}
*/
Narrowphase.prototype.collidedLastStep = function(bodyA, bodyB){
var id1 = bodyA.id|0,
id2 = bodyB.id|0;
return !!this.collidingBodiesLastStep.get(id1, id2);
};
/**
* Throws away the old equations and gets ready to create new
* @method reset
*/
Narrowphase.prototype.reset = function(){
this.collidingBodiesLastStep.reset();
var eqs = this.contactEquations;
var l = eqs.length;
while(l--){
var eq = eqs[l],
id1 = eq.bodyA.id,
id2 = eq.bodyB.id;
this.collidingBodiesLastStep.set(id1, id2, true);
}
var ce = this.contactEquations,
fe = this.frictionEquations;
for(var i=0; i<ce.length; i++){
this.contactEquationPool.release(ce[i]);
}
for(var i=0; i<fe.length; i++){
this.frictionEquationPool.release(fe[i]);
}
// Reset
this.contactEquations.length = this.frictionEquations.length = 0;
};
/**
* Creates a ContactEquation, either by reusing an existing object or creating a new one.
* @method createContactEquation
* @param {Body} bodyA
* @param {Body} bodyB
* @return {ContactEquation}
*/
Narrowphase.prototype.createContactEquation = function(bodyA, bodyB, shapeA, shapeB){
var c = this.contactEquationPool.get();
var currentContactMaterial = this.currentContactMaterial;
c.bodyA = bodyA;
c.bodyB = bodyB;
c.shapeA = shapeA;
c.shapeB = shapeB;
c.enabled = this.enabledEquations;
c.firstImpact = !this.collidedLastStep(bodyA,bodyB);
c.restitution = currentContactMaterial.restitution;
c.stiffness = currentContactMaterial.stiffness;
c.relaxation = currentContactMaterial.relaxation;
c.offset = currentContactMaterial.contactSkinSize;
c.needsUpdate = true;
return c;
};
/**
* Creates a FrictionEquation, either by reusing an existing object or creating a new one.
* @method createFrictionEquation
* @param {Body} bodyA
* @param {Body} bodyB
* @return {FrictionEquation}
*/
Narrowphase.prototype.createFrictionEquation = function(bodyA, bodyB, shapeA, shapeB){
var c = this.frictionEquationPool.get();
var currentContactMaterial = this.currentContactMaterial;
c.bodyA = bodyA;
c.bodyB = bodyB;
c.shapeA = shapeA;
c.shapeB = shapeB;
c.setSlipForce(this.slipForce);
c.enabled = this.enabledEquations;
c.frictionCoefficient = currentContactMaterial.friction;
c.relativeVelocity = currentContactMaterial.surfaceVelocity;
c.stiffness = currentContactMaterial.frictionStiffness;
c.relaxation = currentContactMaterial.frictionRelaxation;
c.needsUpdate = true;
c.contactEquations.length = 0;
return c;
};
/**
* Creates a FrictionEquation given the data in the ContactEquation. Uses same offset vectors ri and rj, but the tangent vector will be constructed from the collision normal.
* @method createFrictionFromContact
* @param {ContactEquation} contactEquation
* @return {FrictionEquation}
*/
Narrowphase.prototype.createFrictionFromContact = function(c){
var eq = this.createFrictionEquation(c.bodyA, c.bodyB, c.shapeA, c.shapeB);
copy(eq.contactPointA, c.contactPointA);
copy(eq.contactPointB, c.contactPointB);
vec2.rotate90cw(eq.t, c.normalA);
eq.contactEquations.push(c);
return eq;
};
// Take the average N latest contact point on the plane.
Narrowphase.prototype.createFrictionFromAverage = function(numContacts){
var c = this.contactEquations[this.contactEquations.length - 1];
var eq = this.createFrictionEquation(c.bodyA, c.bodyB, c.shapeA, c.shapeB);
var bodyA = c.bodyA;
vec2.set(eq.contactPointA, 0, 0);
vec2.set(eq.contactPointB, 0, 0);
vec2.set(eq.t, 0, 0);
for(var i=0; i!==numContacts; i++){
c = this.contactEquations[this.contactEquations.length - 1 - i];
if(c.bodyA === bodyA){
add(eq.t, eq.t, c.normalA);
add(eq.contactPointA, eq.contactPointA, c.contactPointA);
add(eq.contactPointB, eq.contactPointB, c.contactPointB);
} else {
sub(eq.t, eq.t, c.normalA);
add(eq.contactPointA, eq.contactPointA, c.contactPointB);
add(eq.contactPointB, eq.contactPointB, c.contactPointA);
}
eq.contactEquations.push(c);
}
var invNumContacts = 1/numContacts;
scale(eq.contactPointA, eq.contactPointA, invNumContacts);
scale(eq.contactPointB, eq.contactPointB, invNumContacts);
normalize(eq.t, eq.t);
vec2.rotate90cw(eq.t, eq.t);
return eq;
};
/**
* Convex/line narrowphase
* @method convexLine
* @param {Body} convexBody
* @param {Convex} convexShape
* @param {Array} convexOffset
* @param {Number} convexAngle
* @param {Body} lineBody
* @param {Line} lineShape
* @param {Array} lineOffset
* @param {Number} lineAngle
* @param {boolean} justTest
* @return {number}
* @todo Implement me!
*/
Narrowphase.prototype[Shape.CONVEX | Shape.LINE] =
Narrowphase.prototype.convexLine = function(
/*
convexBody,
convexShape,
convexOffset,
convexAngle,
lineBody,
lineShape,
lineOffset,
lineAngle,
justTest
*/
){
// TODO
return 0;
};
/**
* Line/box narrowphase
* @method lineBox
* @param {Body} lineBody
* @param {Line} lineShape
* @param {Array} lineOffset
* @param {Number} lineAngle
* @param {Body} boxBody
* @param {Box} boxShape
* @param {Array} boxOffset
* @param {Number} boxAngle
* @param {Boolean} justTest
* @return {number}
* @todo Implement me!
*/
Narrowphase.prototype[Shape.LINE | Shape.BOX] =
Narrowphase.prototype.lineBox = function(
/*
lineBody,
lineShape,
lineOffset,
lineAngle,
boxBody,
boxShape,
boxOffset,
boxAngle,
justTest
*/
){
// TODO
return 0;
};
function setConvexToCapsuleShapeMiddle(convexShape, capsuleShape){
var capsuleRadius = capsuleShape.radius;
var halfCapsuleLength = capsuleShape.length * 0.5;
var verts = convexShape.vertices;
vec2.set(verts[0], -halfCapsuleLength, -capsuleRadius);
vec2.set(verts[1], halfCapsuleLength, -capsuleRadius);
vec2.set(verts[2], halfCapsuleLength, capsuleRadius);
vec2.set(verts[3], -halfCapsuleLength, capsuleRadius);
}
var convexCapsule_tempRect = new Box({ width: 1, height: 1 }),
convexCapsule_tempVec = createVec2();
/**
* Convex/capsule narrowphase
* @method convexCapsule
* @param {Body} convexBody
* @param {Convex} convexShape
* @param {Array} convexPosition
* @param {Number} convexAngle
* @param {Body} capsuleBody
* @param {Capsule} capsuleShape
* @param {Array} capsulePosition
* @param {Number} capsuleAngle
* @return {number}
*/
Narrowphase.prototype[Shape.CONVEX | Shape.CAPSULE] =
Narrowphase.prototype[Shape.BOX | Shape.CAPSULE] =
Narrowphase.prototype.convexCapsule = function(
convexBody,
convexShape,
convexPosition,
convexAngle,
capsuleBody,
capsuleShape,
capsulePosition,
capsuleAngle,
justTest
){
// Check the circles
// Add offsets!
var circlePos = convexCapsule_tempVec;
var halfLength = capsuleShape.length / 2;
vec2.set(circlePos, halfLength, 0);
vec2.toGlobalFrame(circlePos, circlePos, capsulePosition, capsuleAngle);
var result1 = this.circleConvex(capsuleBody,capsuleShape,circlePos,capsuleAngle, convexBody,convexShape,convexPosition,convexAngle, justTest, capsuleShape.radius);
vec2.set(circlePos,-halfLength, 0);
vec2.toGlobalFrame(circlePos, circlePos, capsulePosition, capsuleAngle);
var result2 = this.circleConvex(capsuleBody,capsuleShape,circlePos,capsuleAngle, convexBody,convexShape,convexPosition,convexAngle, justTest, capsuleShape.radius);
if(justTest && (result1 + result2) !== 0){
return 1;
}
// Check center rect
var r = convexCapsule_tempRect;
setConvexToCapsuleShapeMiddle(r,capsuleShape);
var result = this.convexConvex(convexBody,convexShape,convexPosition,convexAngle, capsuleBody,r,capsulePosition,capsuleAngle, justTest);
return result + result1 + result2;
};
/**
* Capsule/line narrowphase
* @method lineCapsule
* @param {Body} lineBody
* @param {Line} lineShape
* @param {Array} linePosition
* @param {Number} lineAngle
* @param {Body} capsuleBody
* @param {Capsule} capsuleShape
* @param {Array} capsulePosition
* @param {Number} capsuleAngle
* @return {number}
* @todo Implement me!
*/
Narrowphase.prototype[Shape.LINE | Shape.CAPSULE] =
Narrowphase.prototype.lineCapsule = function(
/*
lineBody,
lineShape,
linePosition,
lineAngle,
capsuleBody,
capsuleShape,
capsulePosition,
capsuleAngle,
justTest
*/
){
// TODO
return 0;
};
var capsuleCapsule_tempVec1 = createVec2();
var capsuleCapsule_tempVec2 = createVec2();
var capsuleCapsule_tempRect1 = new Box({ width: 1, height: 1 });
/**
* Capsule/capsule narrowphase
* @method capsuleCapsule
* @param {Body} bi
* @param {Capsule} si
* @param {Array} xi
* @param {Number} ai
* @param {Body} bj
* @param {Capsule} sj
* @param {Array} xj
* @param {Number} aj
*/
Narrowphase.prototype[Shape.CAPSULE] =
Narrowphase.prototype.capsuleCapsule = function(bi,si,xi,ai, bj,sj,xj,aj, justTest){
var enableFrictionBefore;
// Check the circles
// Add offsets!
var circlePosi = capsuleCapsule_tempVec1,
circlePosj = capsuleCapsule_tempVec2;
var numContacts = 0;
// Need 4 circle checks, between all
for(var i=0; i<2; i++){
vec2.set(circlePosi,(i===0?-1:1)*si.length/2,0);
vec2.toGlobalFrame(circlePosi, circlePosi, xi, ai);
for(var j=0; j<2; j++){
vec2.set(circlePosj,(j===0?-1:1)*sj.length/2, 0);
vec2.toGlobalFrame(circlePosj, circlePosj, xj, aj);
// Temporarily turn off friction
if(this.enableFrictionReduction){
enableFrictionBefore = this.enableFriction;
this.enableFriction = false;
}
var result = this.circleCircle(bi,si,circlePosi,ai, bj,sj,circlePosj,aj, justTest, si.radius, sj.radius);
if(this.enableFrictionReduction){
this.enableFriction = enableFrictionBefore;
}
if(justTest && result !== 0){
return 1;
}
numContacts += result;
}
}
if(this.enableFrictionReduction){
// Temporarily turn off friction
enableFrictionBefore = this.enableFriction;
this.enableFriction = false;
}
// Check circles against the center boxs
var rect = capsuleCapsule_tempRect1;
setConvexToCapsuleShapeMiddle(rect,si);
var result1 = this.convexCapsule(bi,rect,xi,ai, bj,sj,xj,aj, justTest);
if(this.enableFrictionReduction){
this.enableFriction = enableFrictionBefore;
}
if(justTest && result1 !== 0){
return 1;
}
numContacts += result1;
if(this.enableFrictionReduction){
// Temporarily turn off friction
var enableFrictionBefore = this.enableFriction;
this.enableFriction = false;
}
setConvexToCapsuleShapeMiddle(rect,sj);
var result2 = this.convexCapsule(bj,rect,xj,aj, bi,si,xi,ai, justTest);
if(this.enableFrictionReduction){
this.enableFriction = enableFrictionBefore;
}
if(justTest && result2 !== 0){
return 1;
}
numContacts += result2;
if(this.enableFrictionReduction){
if(numContacts && this.enableFriction){
this.frictionEquations.push(this.createFrictionFromAverage(numContacts));
}
}
return numContacts;
};
/**
* Line/line narrowphase
* @method lineLine
* @param {Body} bodyA
* @param {Line} shapeA
* @param {Array} positionA
* @param {Number} angleA
* @param {Body} bodyB
* @param {Line} shapeB
* @param {Array} positionB
* @param {Number} angleB
* @return {number}
* @todo Implement me!
*/
Narrowphase.prototype[Shape.LINE] =
Narrowphase.prototype.lineLine = function(
/* bodyA,
shapeA,
positionA,
angleA,
bodyB,
shapeB,
positionB,
angleB,
justTest*/
){
// TODO
return 0;
};
/**
* Plane/line Narrowphase
* @method planeLine
* @param {Body} planeBody
* @param {Plane} planeShape
* @param {Array} planeOffset
* @param {Number} planeAngle
* @param {Body} lineBody
* @param {Line} lineShape
* @param {Array} lineOffset
* @param {Number} lineAngle
*/
Narrowphase.prototype[Shape.PLANE | Shape.LINE] =
Narrowphase.prototype.planeLine = function(planeBody, planeShape, planeOffset, planeAngle,
lineBody, lineShape, lineOffset, lineAngle, justTest){
var worldVertex0 = tmp1,
worldVertex1 = tmp2,
worldVertex01 = tmp3,
worldVertex11 = tmp4,
worldEdge = tmp5,
worldEdgeUnit = tmp6,
dist = tmp7,
worldNormal = tmp8,
worldTangent = tmp9,
verts = tmpArray,
numContacts = 0;
// Get start and end points
vec2.set(worldVertex0, -lineShape.length/2, 0);
vec2.set(worldVertex1, lineShape.length/2, 0);
// Not sure why we have to use worldVertex*1 here, but it won't work otherwise. Tired.
vec2.toGlobalFrame(worldVertex01, worldVertex0, lineOffset, lineAngle);
vec2.toGlobalFrame(worldVertex11, worldVertex1, lineOffset, lineAngle);
copy(worldVertex0,worldVertex01);
copy(worldVertex1,worldVertex11);
// Get vector along the line
sub(worldEdge, worldVertex1, worldVertex0);
normalize(worldEdgeUnit, worldEdge);
// Get tangent to the edge.
vec2.rotate90cw(worldTangent, worldEdgeUnit);
rotate(worldNormal, yAxis, planeAngle);
// Check line ends
verts[0] = worldVertex0;
verts[1] = worldVertex1;
for(var i=0; i<verts.length; i++){
var v = verts[i];
sub(dist, v, planeOffset);
var d = dot(dist,worldNormal);
if(d < 0){
if(justTest){
return 1;
}
var c = this.createContactEquation(planeBody,lineBody,planeShape,lineShape);
numContacts++;
copy(c.normalA, worldNormal);
normalize(c.normalA,c.normalA);
// distance vector along plane normal
scale(dist, worldNormal, d);
// Vector from plane center to contact
sub(c.contactPointA, v, dist);
sub(c.contactPointA, c.contactPointA, planeBody.position);
// From line center to contact
sub(c.contactPointB, v, lineOffset);
add(c.contactPointB, c.contactPointB, lineOffset);
sub(c.contactPointB, c.contactPointB, lineBody.position);
this.contactEquations.push(c);
if(!this.enableFrictionReduction){
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
}
}
}
if(justTest){
return 0;
}
if(!this.enableFrictionReduction){
if(numContacts && this.enableFriction){
this.frictionEquations.push(this.createFrictionFromAverage(numContacts));
}
}
return numContacts;
};
Narrowphase.prototype[Shape.PARTICLE | Shape.CAPSULE] =
Narrowphase.prototype.particleCapsule = function(
particleBody,
particleShape,
particlePosition,
particleAngle,
capsuleBody,
capsuleShape,
capsulePosition,
capsuleAngle,
justTest
){
return this.circleLine(particleBody,particleShape,particlePosition,particleAngle, capsuleBody,capsuleShape,capsulePosition,capsuleAngle, justTest, capsuleShape.radius, 0);
};
/**
* Circle/line Narrowphase
* @method circleLine
* @param {Body} circleBody
* @param {Circle} circleShape
* @param {Array} circleOffset
* @param {Number} circleAngle
* @param {Body} lineBody
* @param {Line} lineShape
* @param {Array} lineOffset
* @param {Number} lineAngle
* @param {Boolean} justTest If set to true, this function will return the result (intersection or not) without adding equations.
* @param {Number} lineRadius Radius to add to the line. Can be used to test Capsules.
* @param {Number} circleRadius If set, this value overrides the circle shape radius.
* @return {number}
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.LINE] =
Narrowphase.prototype.circleLine = function(
circleBody,
circleShape,
circleOffset,
circleAngle,
lineBody,
lineShape,
lineOffset,
lineAngle,
justTest,
lineRadius,
circleRadius
){
var lineRadius = lineRadius || 0,
circleRadius = circleRadius !== undefined ? circleRadius : circleShape.radius,
orthoDist = tmp1,
lineToCircleOrthoUnit = tmp2,
projectedPoint = tmp3,
centerDist = tmp4,
worldTangent = tmp5,
worldEdge = tmp6,
worldEdgeUnit = tmp7,
worldVertex0 = tmp8,
worldVertex1 = tmp9,
worldVertex01 = tmp10,
worldVertex11 = tmp11,
dist = tmp12,
lineToCircle = tmp13,
lineEndToLineRadius = tmp14,
verts = tmpArray;
var halfLineLength = lineShape.length / 2;
// Get start and end points
vec2.set(worldVertex0, -halfLineLength, 0);
vec2.set(worldVertex1, halfLineLength, 0);
// Not sure why we have to use worldVertex*1 here, but it won't work otherwise. Tired.
vec2.toGlobalFrame(worldVertex01, worldVertex0, lineOffset, lineAngle);
vec2.toGlobalFrame(worldVertex11, worldVertex1, lineOffset, lineAngle);
copy(worldVertex0,worldVertex01);
copy(worldVertex1,worldVertex11);
// Get vector along the line
sub(worldEdge, worldVertex1, worldVertex0);
normalize(worldEdgeUnit, worldEdge);
// Get tangent to the edge.
vec2.rotate90cw(worldTangent, worldEdgeUnit);
// Check distance from the plane spanned by the edge vs the circle
sub(dist, circleOffset, worldVertex0);
var d = dot(dist, worldTangent); // Distance from center of line to circle center
sub(centerDist, worldVertex0, lineOffset);
sub(lineToCircle, circleOffset, lineOffset);
var radiusSum = circleRadius + lineRadius;
if(Math.abs(d) < radiusSum){
// Now project the circle onto the edge
scale(orthoDist, worldTangent, d);
sub(projectedPoint, circleOffset, orthoDist);
// Add the missing line radius
scale(lineToCircleOrthoUnit, worldTangent, dot(worldTangent, lineToCircle));
normalize(lineToCircleOrthoUnit,lineToCircleOrthoUnit);
scale(lineToCircleOrthoUnit, lineToCircleOrthoUnit, lineRadius);
add(projectedPoint,projectedPoint,lineToCircleOrthoUnit);
// Check if the point is within the edge span
var pos = dot(worldEdgeUnit, projectedPoint);
var pos0 = dot(worldEdgeUnit, worldVertex0);
var pos1 = dot(worldEdgeUnit, worldVertex1);
if(pos > pos0 && pos < pos1){
// We got contact!
if(justTest){
return 1;
}
var c = this.createContactEquation(circleBody,lineBody,circleShape,lineShape);
scale(c.normalA, orthoDist, -1);
normalize(c.normalA, c.normalA);
scale( c.contactPointA, c.normalA, circleRadius);
add(c.contactPointA, c.contactPointA, circleOffset);
sub(c.contactPointA, c.contactPointA, circleBody.position);
sub(c.contactPointB, projectedPoint, lineOffset);
add(c.contactPointB, c.contactPointB, lineOffset);
sub(c.contactPointB, c.contactPointB, lineBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 1;
}
}
// Add corner
verts[0] = worldVertex0;
verts[1] = worldVertex1;
for(var i=0; i<verts.length; i++){
var v = verts[i];
sub(dist, v, circleOffset);
if(squaredLength(dist) < Math.pow(radiusSum, 2)){
if(justTest){
return 1;
}
var c = this.createContactEquation(circleBody,lineBody,circleShape,lineShape);
copy(c.normalA, dist);
normalize(c.normalA,c.normalA);
// Vector from circle to contact point is the normal times the circle radius
scale(c.contactPointA, c.normalA, circleRadius);
add(c.contactPointA, c.contactPointA, circleOffset);
sub(c.contactPointA, c.contactPointA, circleBody.position);
sub(c.contactPointB, v, lineOffset);
scale(lineEndToLineRadius, c.normalA, -lineRadius);
add(c.contactPointB, c.contactPointB, lineEndToLineRadius);
add(c.contactPointB, c.contactPointB, lineOffset);
sub(c.contactPointB, c.contactPointB, lineBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 1;
}
}
return 0;
};
/**
* Circle/capsule Narrowphase
* @method circleCapsule
* @param {Body} bi
* @param {Circle} si
* @param {Array} xi
* @param {Number} ai
* @param {Body} bj
* @param {Line} sj
* @param {Array} xj
* @param {Number} aj
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.CAPSULE] =
Narrowphase.prototype.circleCapsule = function(bi,si,xi,ai, bj,sj,xj,aj, justTest){
return this.circleLine(bi,si,xi,ai, bj,sj,xj,aj, justTest, sj.radius);
};
/**
* Circle/convex Narrowphase.
* @method circleConvex
* @param {Body} circleBody
* @param {Circle} circleShape
* @param {Array} circleOffset
* @param {Number} circleAngle
* @param {Body} convexBody
* @param {Convex} convexShape
* @param {Array} convexOffset
* @param {Number} convexAngle
* @param {Boolean} justTest
* @param {Number} circleRadius
* @return {number}
* @todo Should probably do a separating axis test like https://github.com/erincatto/Box2D/blob/master/Box2D/Box2D/Collision/b2CollideCircle.cpp#L62
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.CONVEX] =
Narrowphase.prototype[Shape.CIRCLE | Shape.BOX] =
Narrowphase.prototype.circleConvex = function(
circleBody,
circleShape,
circleOffset,
circleAngle,
convexBody,
convexShape,
convexOffset,
convexAngle,
justTest,
circleRadius
){
var circleRadius = circleRadius !== undefined ? circleRadius : circleShape.radius;
var worldVertex0 = tmp1,
worldVertex1 = tmp2,
edge = tmp3,
edgeUnit = tmp4,
normal = tmp5,
zero = tmp6,
localCirclePosition = tmp7,
r = tmp8,
dist = tmp10,
worldVertex = tmp11,
closestEdgeProjectedPoint = tmp13,
candidate = tmp14,
candidateDist = tmp15,
found = -1,
minCandidateDistance = Number.MAX_VALUE;
vec2.set(zero, 0, 0);
// New algorithm:
// 1. Check so center of circle is not inside the polygon. If it is, this wont work...
// 2. For each edge
// 2. 1. Get point on circle that is closest to the edge (scale normal with -radius)
// 2. 2. Check if point is inside.
vec2.toLocalFrame(localCirclePosition, circleOffset, convexOffset, convexAngle);
var vertices = convexShape.vertices;
var normals = convexShape.normals;
var numVertices = vertices.length;
var normalIndex = -1;
// Find the min separating edge.
var separation = -Number.MAX_VALUE;
var radius = convexShape.boundingRadius + circleRadius;
for (var i = 0; i < numVertices; i++){
sub(r, localCirclePosition, vertices[i]);
var s = dot(normals[i], r);
if (s > radius){
// Early out.
return 0;
}
if (s > separation){
separation = s;
normalIndex = i;
}
}
// Check edges first
for(var i=normalIndex + numVertices - 1; i < normalIndex + numVertices + 2; i++){
var v0 = vertices[i % numVertices],
n = normals[i % numVertices];
// Get point on circle, closest to the convex
scale(candidate, n, -circleRadius);
add(candidate,candidate,localCirclePosition);
if(pointInConvexLocal(candidate,convexShape)){
sub(candidateDist,v0,candidate);
var candidateDistance = Math.abs(dot(candidateDist, n));
if(candidateDistance < minCandidateDistance){
minCandidateDistance = candidateDistance;
found = i;
}
}
}
if(found !== -1){
if(justTest){
return 1;
}
var v0 = vertices[found % numVertices],
v1 = vertices[(found+1) % numVertices];
vec2.toGlobalFrame(worldVertex0, v0, convexOffset, convexAngle);
vec2.toGlobalFrame(worldVertex1, v1, convexOffset, convexAngle);
sub(edge, worldVertex1, worldVertex0);
normalize(edgeUnit, edge);
// Get tangent to the edge. Points out of the Convex
vec2.rotate90cw(normal, edgeUnit);
// Get point on circle, closest to the convex
scale(candidate, normal, -circleRadius);
add(candidate,candidate,circleOffset);
scale(closestEdgeProjectedPoint, normal, minCandidateDistance);
add(closestEdgeProjectedPoint,closestEdgeProjectedPoint,candidate);
var c = this.createContactEquation(circleBody,convexBody,circleShape,convexShape);
sub(c.normalA, candidate, circleOffset);
normalize(c.normalA, c.normalA);
scale(c.contactPointA, c.normalA, circleRadius);
add(c.contactPointA, c.contactPointA, circleOffset);
sub(c.contactPointA, c.contactPointA, circleBody.position);
sub(c.contactPointB, closestEdgeProjectedPoint, convexOffset);
add(c.contactPointB, c.contactPointB, convexOffset);
sub(c.contactPointB, c.contactPointB, convexBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push( this.createFrictionFromContact(c) );
}
return 1;
}
// Check closest vertices
if(circleRadius > 0 && normalIndex !== -1){
for(var i=normalIndex + numVertices; i < normalIndex + numVertices + 2; i++){
var localVertex = vertices[i % numVertices];
sub(dist, localVertex, localCirclePosition);
if(squaredLength(dist) < circleRadius * circleRadius){
if(justTest){
return 1;
}
vec2.toGlobalFrame(worldVertex, localVertex, convexOffset, convexAngle);
sub(dist, worldVertex, circleOffset);
var c = this.createContactEquation(circleBody,convexBody,circleShape,convexShape);
copy(c.normalA, dist);
normalize(c.normalA,c.normalA);
// Vector from circle to contact point is the normal times the circle radius
scale(c.contactPointA, c.normalA, circleRadius);
add(c.contactPointA, c.contactPointA, circleOffset);
sub(c.contactPointA, c.contactPointA, circleBody.position);
sub(c.contactPointB, worldVertex, convexOffset);
add(c.contactPointB, c.contactPointB, convexOffset);
sub(c.contactPointB, c.contactPointB, convexBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 1;
}
}
}
return 0;
};
var pic_localPoint = createVec2(),
pic_r0 = createVec2(),
pic_r1 = createVec2();
/*
* Check if a point is in a polygon
*/
function pointInConvex(worldPoint,convexShape,convexOffset,convexAngle){
var localPoint = pic_localPoint,
r0 = pic_r0,
r1 = pic_r1,
verts = convexShape.vertices,
lastCross = null;
vec2.toLocalFrame(localPoint, worldPoint, convexOffset, convexAngle);
for(var i=0, numVerts=verts.length; i!==numVerts+1; i++){
var v0 = verts[i % numVerts],
v1 = verts[(i+1) % numVerts];
sub(r0, v0, localPoint);
sub(r1, v1, localPoint);
var cross = vec2.crossLength(r0,r1);
if(lastCross === null){
lastCross = cross;
}
// If we got a different sign of the distance vector, the point is out of the polygon
if(cross*lastCross < 0){
return false;
}
lastCross = cross;
}
return true;
}
/*
* Check if a point is in a polygon
*/
function pointInConvexLocal(localPoint,convexShape){
var r0 = pic_r0,
r1 = pic_r1,
verts = convexShape.vertices,
lastCross = null,
numVerts = verts.length;
for(var i=0; i < numVerts + 1; i++){
var v0 = verts[i % numVerts],
v1 = verts[(i + 1) % numVerts];
sub(r0, v0, localPoint);
sub(r1, v1, localPoint);
var cross = vec2.crossLength(r0,r1);
if(lastCross === null){
lastCross = cross;
}
// If we got a different sign of the distance vector, the point is out of the polygon
if(cross*lastCross < 0){
return false;
}
lastCross = cross;
}
return true;
}
/**
* Particle/convex Narrowphase
* @method particleConvex
* @param {Body} particleBody
* @param {Particle} particleShape
* @param {Array} particleOffset
* @param {Number} particleAngle
* @param {Body} convexBody
* @param {Convex} convexShape
* @param {Array} convexOffset
* @param {Number} convexAngle
* @param {Boolean} justTest
* @return {number}
* @todo use pointInConvex and code more similar to circleConvex
* @todo don't transform each vertex, but transform the particle position to convex-local instead
*/
Narrowphase.prototype[Shape.PARTICLE | Shape.CONVEX] =
Narrowphase.prototype[Shape.PARTICLE | Shape.BOX] =
Narrowphase.prototype.particleConvex = function(
particleBody,
particleShape,
particleOffset,
particleAngle,
convexBody,
convexShape,
convexOffset,
convexAngle,
justTest
){
var worldVertex0 = tmp1,
worldVertex1 = tmp2,
worldEdge = tmp3,
worldEdgeUnit = tmp4,
worldTangent = tmp5,
centerDist = tmp6,
convexToparticle = tmp7,
closestEdgeProjectedPoint = tmp13,
candidateDist = tmp14,
minEdgeNormal = tmp15,
minCandidateDistance = Number.MAX_VALUE,
found = false,
verts = convexShape.vertices;
// Check if the particle is in the polygon at all
if(!pointInConvex(particleOffset,convexShape,convexOffset,convexAngle)){
return 0;
}
if(justTest){
return 1;
}
// Check edges first
for(var i=0, numVerts=verts.length; i!==numVerts+1; i++){
var v0 = verts[i%numVerts],
v1 = verts[(i+1)%numVerts];
// Transform vertices to world
// @todo transform point to local space instead
rotate(worldVertex0, v0, convexAngle);
rotate(worldVertex1, v1, convexAngle);
add(worldVertex0, worldVertex0, convexOffset);
add(worldVertex1, worldVertex1, convexOffset);
// Get world edge
sub(worldEdge, worldVertex1, worldVertex0);
normalize(worldEdgeUnit, worldEdge);
// Get tangent to the edge. Points out of the Convex
vec2.rotate90cw(worldTangent, worldEdgeUnit);
// Check distance from the infinite line (spanned by the edge) to the particle
//sub(dist, particleOffset, worldVertex0);
//var d = dot(dist, worldTangent);
sub(centerDist, worldVertex0, convexOffset);
sub(convexToparticle, particleOffset, convexOffset);
sub(candidateDist,worldVertex0,particleOffset);
var candidateDistance = Math.abs(dot(candidateDist,worldTangent));
if(candidateDistance < minCandidateDistance){
minCandidateDistance = candidateDistance;
scale(closestEdgeProjectedPoint,worldTangent,candidateDistance);
add(closestEdgeProjectedPoint,closestEdgeProjectedPoint,particleOffset);
copy(minEdgeNormal,worldTangent);
found = true;
}
}
if(found){
var c = this.createContactEquation(particleBody,convexBody,particleShape,convexShape);
scale(c.normalA, minEdgeNormal, -1);
normalize(c.normalA, c.normalA);
// Particle has no extent to the contact point
vec2.set(c.contactPointA, 0, 0);
add(c.contactPointA, c.contactPointA, particleOffset);
sub(c.contactPointA, c.contactPointA, particleBody.position);
// From convex center to point
sub(c.contactPointB, closestEdgeProjectedPoint, convexOffset);
add(c.contactPointB, c.contactPointB, convexOffset);
sub(c.contactPointB, c.contactPointB, convexBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push( this.createFrictionFromContact(c) );
}
return 1;
}
return 0;
};
/**
* Circle/circle Narrowphase
* @method circleCircle
* @param {Body} bodyA
* @param {Circle} shapeA
* @param {Array} offsetA
* @param {Number} angleA
* @param {Body} bodyB
* @param {Circle} shapeB
* @param {Array} offsetB
* @param {Number} angleB
* @param {Boolean} justTest
* @param {Number} [radiusA] Optional radius to use for shapeA
* @param {Number} [radiusB] Optional radius to use for shapeB
* @return {number}
*/
Narrowphase.prototype[Shape.CIRCLE] =
Narrowphase.prototype.circleCircle = function(
bodyA,
shapeA,
offsetA,
angleA,
bodyB,
shapeB,
offsetB,
angleB,
justTest,
radiusA,
radiusB
){
var dist = tmp1,
radiusA = radiusA || shapeA.radius,
radiusB = radiusB || shapeB.radius;
sub(dist,offsetA,offsetB);
var r = radiusA + radiusB;
if(squaredLength(dist) > r*r){
return 0;
}
if(justTest){
return 1;
}
var c = this.createContactEquation(bodyA,bodyB,shapeA,shapeB);
var cpA = c.contactPointA;
var cpB = c.contactPointB;
var normalA = c.normalA;
sub(normalA, offsetB, offsetA);
normalize(normalA,normalA);
scale( cpA, normalA, radiusA);
scale( cpB, normalA, -radiusB);
addSub(cpA, cpA, offsetA, bodyA.position);
addSub(cpB, cpB, offsetB, bodyB.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 1;
};
function addSub(out, a, b, c){
out[0] = a[0] + b[0] - c[0];
out[1] = a[1] + b[1] - c[1];
}
/**
* Plane/Convex Narrowphase
* @method planeConvex
* @param {Body} planeBody
* @param {Plane} planeShape
* @param {Array} planeOffset
* @param {Number} planeAngle
* @param {Body} convexBody
* @param {Convex} convexShape
* @param {Array} convexOffset
* @param {Number} convexAngle
* @param {Boolean} justTest
* @return {number}
* @todo only use the deepest contact point + the contact point furthest away from it
*/
Narrowphase.prototype[Shape.PLANE | Shape.CONVEX] =
Narrowphase.prototype[Shape.PLANE | Shape.BOX] =
Narrowphase.prototype.planeConvex = function(
planeBody,
planeShape,
planeOffset,
planeAngle,
convexBody,
convexShape,
convexOffset,
convexAngle,
justTest
){
var worldVertex = tmp1,
worldNormal = tmp2,
dist = tmp3,
localPlaneOffset = tmp4,
localPlaneNormal = tmp5,
localDist = tmp6;
var numReported = 0;
rotate(worldNormal, yAxis, planeAngle);
// Get convex-local plane offset and normal
vec2.vectorToLocalFrame(localPlaneNormal, worldNormal, convexAngle);
vec2.toLocalFrame(localPlaneOffset, planeOffset, convexOffset, convexAngle);
var vertices = convexShape.vertices;
for(var i=0, numVerts=vertices.length; i!==numVerts; i++){
var v = vertices[i];
sub(localDist, v, localPlaneOffset);
if(dot(localDist,localPlaneNormal) <= 0){
if(justTest){
return 1;
}
vec2.toGlobalFrame(worldVertex, v, convexOffset, convexAngle);
sub(dist, worldVertex, planeOffset);
// Found vertex
numReported++;
var c = this.createContactEquation(planeBody,convexBody,planeShape,convexShape);
sub(dist, worldVertex, planeOffset);
copy(c.normalA, worldNormal);
var d = dot(dist, c.normalA);
scale(dist, c.normalA, d);
// rj is from convex center to contact
sub(c.contactPointB, worldVertex, convexBody.position);
// ri is from plane center to contact
sub( c.contactPointA, worldVertex, dist);
sub( c.contactPointA, c.contactPointA, planeBody.position);
this.contactEquations.push(c);
if(!this.enableFrictionReduction){
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
}
}
}
if(this.enableFrictionReduction){
if(this.enableFriction && numReported){
this.frictionEquations.push(this.createFrictionFromAverage(numReported));
}
}
return numReported;
};
/**
* Narrowphase for particle vs plane
* @method particlePlane
* @param {Body} particleBody
* @param {Particle} particleShape
* @param {Array} particleOffset
* @param {Number} particleAngle
* @param {Body} planeBody
* @param {Plane} planeShape
* @param {Array} planeOffset
* @param {Number} planeAngle
* @param {Boolean} justTest
* @return {number}
*/
Narrowphase.prototype[Shape.PARTICLE | Shape.PLANE] =
Narrowphase.prototype.particlePlane = function(
particleBody,
particleShape,
particleOffset,
particleAngle,
planeBody,
planeShape,
planeOffset,
planeAngle,
justTest
){
var dist = tmp1,
worldNormal = tmp2;
planeAngle = planeAngle || 0;
sub(dist, particleOffset, planeOffset);
rotate(worldNormal, yAxis, planeAngle);
var d = dot(dist, worldNormal);
if(d > 0){
return 0;
}
if(justTest){
return 1;
}
var c = this.createContactEquation(planeBody,particleBody,planeShape,particleShape);
copy(c.normalA, worldNormal);
scale( dist, c.normalA, d );
// dist is now the distance vector in the normal direction
// ri is the particle position projected down onto the plane, from the plane center
sub( c.contactPointA, particleOffset, dist);
sub( c.contactPointA, c.contactPointA, planeBody.position);
// rj is from the body center to the particle center
sub( c.contactPointB, particleOffset, particleBody.position );
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 1;
};
/**
* Circle/Particle Narrowphase
* @method circleParticle
* @param {Body} circleBody
* @param {Circle} circleShape
* @param {Array} circleOffset
* @param {Number} circleAngle
* @param {Body} particleBody
* @param {Particle} particleShape
* @param {Array} particleOffset
* @param {Number} particleAngle
* @param {Boolean} justTest
* @return {number}
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.PARTICLE] =
Narrowphase.prototype.circleParticle = function(
circleBody,
circleShape,
circleOffset,
circleAngle,
particleBody,
particleShape,
particleOffset,
particleAngle,
justTest
){
var dist = tmp1;
var circleRadius = circleShape.radius;
sub(dist, particleOffset, circleOffset);
if(squaredLength(dist) > circleRadius*circleRadius){
return 0;
}
if(justTest){
return 1;
}
var c = this.createContactEquation(circleBody,particleBody,circleShape,particleShape);
var normalA = c.normalA;
var contactPointA = c.contactPointA;
var contactPointB = c.contactPointB;
copy(normalA, dist);
normalize(normalA, normalA);
// Vector from circle to contact point is the normal times the circle radius
scale(contactPointA, normalA, circleRadius);
add(contactPointA, contactPointA, circleOffset);
sub(contactPointA, contactPointA, circleBody.position);
// Vector from particle center to contact point is zero
sub(contactPointB, particleOffset, particleBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 1;
};
var planeCapsule_tmpCircle = new Circle({ radius: 1 }),
planeCapsule_tmp1 = createVec2(),
planeCapsule_tmp2 = createVec2();
/**
* @method planeCapsule
* @param {Body} planeBody
* @param {Circle} planeShape
* @param {Array} planeOffset
* @param {Number} planeAngle
* @param {Body} capsuleBody
* @param {Particle} capsuleShape
* @param {Array} capsuleOffset
* @param {Number} capsuleAngle
* @param {Boolean} justTest
* @return {number}
*/
Narrowphase.prototype[Shape.PLANE | Shape.CAPSULE] =
Narrowphase.prototype.planeCapsule = function(
planeBody,
planeShape,
planeOffset,
planeAngle,
capsuleBody,
capsuleShape,
capsuleOffset,
capsuleAngle,
justTest
){
var end1 = planeCapsule_tmp1,
end2 = planeCapsule_tmp2,
circle = planeCapsule_tmpCircle,
halfLength = capsuleShape.length / 2;
// Compute world end positions
vec2.set(end1, -halfLength, 0);
vec2.set(end2, halfLength, 0);
vec2.toGlobalFrame(end1, end1, capsuleOffset, capsuleAngle);
vec2.toGlobalFrame(end2, end2, capsuleOffset, capsuleAngle);
circle.radius = capsuleShape.radius;
var enableFrictionBefore;
// Temporarily turn off friction
if(this.enableFrictionReduction){
enableFrictionBefore = this.enableFriction;
this.enableFriction = false;
}
// Do Narrowphase as two circles
var numContacts1 = this.circlePlane(capsuleBody,circle,end1,0, planeBody,planeShape,planeOffset,planeAngle, justTest),
numContacts2 = this.circlePlane(capsuleBody,circle,end2,0, planeBody,planeShape,planeOffset,planeAngle, justTest);
// Restore friction
if(this.enableFrictionReduction){
this.enableFriction = enableFrictionBefore;
}
if(justTest){
return numContacts1 + numContacts2;
} else {
var numTotal = numContacts1 + numContacts2;
if(this.enableFrictionReduction){
if(numTotal){
this.frictionEquations.push(this.createFrictionFromAverage(numTotal));
}
}
return numTotal;
}
};
/**
* @method circlePlane
* @param {Body} circleBody
* @param {Circle} circleShape
* @param {Array} circleOffset
* @param {Number} circleAngle
* @param {Body} planeBody
* @param {Plane} planeShape
* @param {Array} planeOffset
* @param {Number} planeAngle
* @param {Boolean} justTest
* @return {number}
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.PLANE] =
Narrowphase.prototype.circlePlane = function(
circleBody,
circleShape,
circleOffset,
circleAngle,
planeBody,
planeShape,
planeOffset,
planeAngle,
justTest
){
var circleRadius = circleShape.radius;
// Vector from plane to circle
var planeToCircle = tmp1,
worldNormal = tmp2,
temp = tmp3;
sub(planeToCircle, circleOffset, planeOffset);
// World plane normal
rotate(worldNormal, yA