var vec2 = require('../math/vec2')
, sub = vec2.sub
, add = vec2.add
, dot = vec2.dot
, Utils = require('../utils/Utils')
, ContactEquationPool = require('../utils/ContactEquationPool')
, FrictionEquationPool = require('../utils/FrictionEquationPool')
, TupleDictionary = require('../utils/TupleDictionary')
, Equation = require('../equations/Equation')
, ContactEquation = require('../equations/ContactEquation')
, FrictionEquation = require('../equations/FrictionEquation')
, Circle = require('../shapes/Circle')
, Convex = require('../shapes/Convex')
, Shape = require('../shapes/Shape')
, Body = require('../objects/Body')
, Box = require('../shapes/Box');
module.exports = Narrowphase;
// Temp things
var yAxis = vec2.fromValues(0,1);
var tmp1 = vec2.fromValues(0,0)
, tmp2 = vec2.fromValues(0,0)
, tmp3 = vec2.fromValues(0,0)
, tmp4 = vec2.fromValues(0,0)
, tmp5 = vec2.fromValues(0,0)
, tmp6 = vec2.fromValues(0,0)
, tmp7 = vec2.fromValues(0,0)
, tmp8 = vec2.fromValues(0,0)
, tmp9 = vec2.fromValues(0,0)
, tmp10 = vec2.fromValues(0,0)
, tmp11 = vec2.fromValues(0,0)
, tmp12 = vec2.fromValues(0,0)
, tmp13 = vec2.fromValues(0,0)
, tmp14 = vec2.fromValues(0,0)
, tmp15 = vec2.fromValues(0,0)
, tmp16 = vec2.fromValues(0,0)
, tmp17 = vec2.fromValues(0,0)
, tmp18 = vec2.fromValues(0,0)
, 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;
/**
* The friction value to use in the upcoming friction equations.
* @property frictionCoefficient
* @type {Number}
*/
this.frictionCoefficient = 0.3;
/**
* Will be the .relativeVelocity in each produced FrictionEquation.
* @property {Number} surfaceVelocity
*/
this.surfaceVelocity = 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 });
/**
* The restitution value to use in the next contact equations.
* @property restitution
* @type {Number}
*/
this.restitution = 0;
/**
* The stiffness value to use in the next contact equations.
* @property {Number} stiffness
*/
this.stiffness = Equation.DEFAULT_STIFFNESS;
/**
* The stiffness value to use in the next contact equations.
* @property {Number} stiffness
*/
this.relaxation = Equation.DEFAULT_RELAXATION;
/**
* The stiffness value to use in the next friction equations.
* @property frictionStiffness
* @type {Number}
*/
this.frictionStiffness = Equation.DEFAULT_STIFFNESS;
/**
* The relaxation value to use in the next friction equations.
* @property frictionRelaxation
* @type {Number}
*/
this.frictionRelaxation = Equation.DEFAULT_RELAXATION;
/**
* 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();
/**
* Contact skin size value to use in the next contact equations.
* @property {Number} contactSkinSize
* @default 0.01
*/
this.contactSkinSize = 0.01;
}
var bodiesOverlap_shapePositionA = vec2.create();
var bodiesOverlap_shapePositionB = vec2.create();
/**
* @method bodiesOverlap
* @param {Body} bodyA
* @param {Body} bodyB
* @return {Boolean}
* @todo shape world transforms are wrong
*/
Narrowphase.prototype.bodiesOverlap = function(bodyA, bodyB){
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];
bodyA.toWorldFrame(shapePositionA, shapeA.position);
// All shapes of body j
for(var l=0, Nshapesj=bodyB.shapes.length; l!==Nshapesj; l++){
var shapeB = bodyB.shapes[l];
bodyB.toWorldFrame(shapePositionB, shapeB.position);
if(this[shapeA.type | shapeB.type](
bodyA,
shapeA,
shapePositionA,
shapeA.angle + bodyA.angle,
bodyB,
shapeB,
shapePositionB,
shapeB.angle + bodyB.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();
c.bodyA = bodyA;
c.bodyB = bodyB;
c.shapeA = shapeA;
c.shapeB = shapeB;
c.restitution = this.restitution;
c.firstImpact = !this.collidedLastStep(bodyA,bodyB);
c.stiffness = this.stiffness;
c.relaxation = this.relaxation;
c.needsUpdate = true;
c.enabled = this.enabledEquations;
c.offset = this.contactSkinSize;
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();
c.bodyA = bodyA;
c.bodyB = bodyB;
c.shapeA = shapeA;
c.shapeB = shapeB;
c.setSlipForce(this.slipForce);
c.frictionCoefficient = this.frictionCoefficient;
c.relativeVelocity = this.surfaceVelocity;
c.enabled = this.enabledEquations;
c.needsUpdate = true;
c.stiffness = this.frictionStiffness;
c.relaxation = this.frictionRelaxation;
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);
vec2.copy(eq.contactPointA, c.contactPointA);
vec2.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;
var bodyB = c.bodyB;
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){
vec2.add(eq.t, eq.t, c.normalA);
vec2.add(eq.contactPointA, eq.contactPointA, c.contactPointA);
vec2.add(eq.contactPointB, eq.contactPointB, c.contactPointB);
} else {
vec2.sub(eq.t, eq.t, c.normalA);
vec2.add(eq.contactPointA, eq.contactPointA, c.contactPointB);
vec2.add(eq.contactPointB, eq.contactPointB, c.contactPointA);
}
eq.contactEquations.push(c);
}
var invNumContacts = 1/numContacts;
vec2.scale(eq.contactPointA, eq.contactPointA, invNumContacts);
vec2.scale(eq.contactPointB, eq.contactPointB, invNumContacts);
vec2.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
* @todo Implement me!
*/
Narrowphase.prototype[Shape.LINE | Shape.CONVEX] =
Narrowphase.prototype.convexLine = function(
convexBody,
convexShape,
convexOffset,
convexAngle,
lineBody,
lineShape,
lineOffset,
lineAngle,
justTest
){
// TODO
if(justTest){
return false;
} else {
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
* @todo Implement me!
*/
Narrowphase.prototype[Shape.LINE | Shape.BOX] =
Narrowphase.prototype.lineBox = function(
lineBody,
lineShape,
lineOffset,
lineAngle,
boxBody,
boxShape,
boxOffset,
boxAngle,
justTest
){
// TODO
if(justTest){
return false;
} else {
return 0;
}
};
function setConvexToCapsuleShapeMiddle(convexShape, capsuleShape){
vec2.set(convexShape.vertices[0], -capsuleShape.length * 0.5, -capsuleShape.radius);
vec2.set(convexShape.vertices[1], capsuleShape.length * 0.5, -capsuleShape.radius);
vec2.set(convexShape.vertices[2], capsuleShape.length * 0.5, capsuleShape.radius);
vec2.set(convexShape.vertices[3], -capsuleShape.length * 0.5, capsuleShape.radius);
}
var convexCapsule_tempRect = new Box({ width: 1, height: 1 }),
convexCapsule_tempVec = vec2.create();
/**
* 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
*/
Narrowphase.prototype[Shape.CAPSULE | Shape.CONVEX] =
Narrowphase.prototype[Shape.CAPSULE | Shape.BOX] =
Narrowphase.prototype.convexCapsule = function(
convexBody,
convexShape,
convexPosition,
convexAngle,
capsuleBody,
capsuleShape,
capsulePosition,
capsuleAngle,
justTest
){
// Check the circles
// Add offsets!
var circlePos = convexCapsule_tempVec;
vec2.set(circlePos, capsuleShape.length/2,0);
vec2.rotate(circlePos,circlePos,capsuleAngle);
vec2.add(circlePos,circlePos,capsulePosition);
var result1 = this.circleConvex(capsuleBody,capsuleShape,circlePos,capsuleAngle, convexBody,convexShape,convexPosition,convexAngle, justTest, capsuleShape.radius);
vec2.set(circlePos,-capsuleShape.length/2, 0);
vec2.rotate(circlePos,circlePos,capsuleAngle);
vec2.add(circlePos,circlePos,capsulePosition);
var result2 = this.circleConvex(capsuleBody,capsuleShape,circlePos,capsuleAngle, convexBody,convexShape,convexPosition,convexAngle, justTest, capsuleShape.radius);
if(justTest && (result1 || result2)){
return true;
}
// 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
* @todo Implement me!
*/
Narrowphase.prototype[Shape.CAPSULE | Shape.LINE] =
Narrowphase.prototype.lineCapsule = function(
lineBody,
lineShape,
linePosition,
lineAngle,
capsuleBody,
capsuleShape,
capsulePosition,
capsuleAngle,
justTest
){
// TODO
if(justTest){
return false;
} else {
return 0;
}
};
var capsuleCapsule_tempVec1 = vec2.create();
var capsuleCapsule_tempVec2 = vec2.create();
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 | 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.rotate(circlePosi,circlePosi,ai);
vec2.add(circlePosi,circlePosi,xi);
for(var j=0; j<2; j++){
vec2.set(circlePosj,(j===0?-1:1)*sj.length/2, 0);
vec2.rotate(circlePosj,circlePosj,aj);
vec2.add(circlePosj,circlePosj,xj);
// 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){
return true;
}
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){
return true;
}
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){
return true;
}
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
* @todo Implement me!
*/
Narrowphase.prototype[Shape.LINE | Shape.LINE] =
Narrowphase.prototype.lineLine = function(
bodyA,
shapeA,
positionA,
angleA,
bodyB,
shapeB,
positionB,
angleB,
justTest
){
// TODO
if(justTest){
return false;
} else {
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.rotate(worldVertex01, worldVertex0, lineAngle);
vec2.rotate(worldVertex11, worldVertex1, lineAngle);
add(worldVertex01, worldVertex01, lineOffset);
add(worldVertex11, worldVertex11, lineOffset);
vec2.copy(worldVertex0,worldVertex01);
vec2.copy(worldVertex1,worldVertex11);
// Get vector along the line
sub(worldEdge, worldVertex1, worldVertex0);
vec2.normalize(worldEdgeUnit, worldEdge);
// Get tangent to the edge.
vec2.rotate90cw(worldTangent, worldEdgeUnit);
vec2.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 true;
}
var c = this.createContactEquation(planeBody,lineBody,planeShape,lineShape);
numContacts++;
vec2.copy(c.normalA, worldNormal);
vec2.normalize(c.normalA,c.normalA);
// distance vector along plane normal
vec2.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 false;
}
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.
*/
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 = typeof(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;
// 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.rotate(worldVertex01, worldVertex0, lineAngle);
vec2.rotate(worldVertex11, worldVertex1, lineAngle);
add(worldVertex01, worldVertex01, lineOffset);
add(worldVertex11, worldVertex11, lineOffset);
vec2.copy(worldVertex0,worldVertex01);
vec2.copy(worldVertex1,worldVertex11);
// Get vector along the line
sub(worldEdge, worldVertex1, worldVertex0);
vec2.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
vec2.scale(orthoDist, worldTangent, d);
sub(projectedPoint, circleOffset, orthoDist);
// Add the missing line radius
vec2.scale(lineToCircleOrthoUnit, worldTangent, dot(worldTangent, lineToCircle));
vec2.normalize(lineToCircleOrthoUnit,lineToCircleOrthoUnit);
vec2.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 true;
}
var c = this.createContactEquation(circleBody,lineBody,circleShape,lineShape);
vec2.scale(c.normalA, orthoDist, -1);
vec2.normalize(c.normalA, c.normalA);
vec2.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(vec2.squaredLength(dist) < Math.pow(radiusSum, 2)){
if(justTest){
return true;
}
var c = this.createContactEquation(circleBody,lineBody,circleShape,lineShape);
vec2.copy(c.normalA, dist);
vec2.normalize(c.normalA,c.normalA);
// Vector from circle to contact point is the normal times the circle radius
vec2.scale(c.contactPointA, c.normalA, circleRadius);
add(c.contactPointA, c.contactPointA, circleOffset);
sub(c.contactPointA, c.contactPointA, circleBody.position);
sub(c.contactPointB, v, lineOffset);
vec2.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
*/
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 = typeof(circleRadius)==="number" ? circleRadius : circleShape.radius;
var worldVertex0 = tmp1,
worldVertex1 = tmp2,
worldEdge = tmp3,
worldEdgeUnit = tmp4,
worldNormal = tmp5,
centerDist = tmp6,
convexToCircle = tmp7,
orthoDist = tmp8,
projectedPoint = tmp9,
dist = tmp10,
worldVertex = tmp11,
closestEdge = -1,
closestEdgeDistance = null,
closestEdgeOrthoDist = tmp12,
closestEdgeProjectedPoint = tmp13,
candidate = tmp14,
candidateDist = tmp15,
minCandidate = tmp16,
found = false,
minCandidateDistance = Number.MAX_VALUE;
var numReported = 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.
var verts = convexShape.vertices;
// Check all edges first
for(var i=0; i!==verts.length+1; i++){
var v0 = verts[i%verts.length],
v1 = verts[(i+1)%verts.length];
vec2.rotate(worldVertex0, v0, convexAngle);
vec2.rotate(worldVertex1, v1, convexAngle);
add(worldVertex0, worldVertex0, convexOffset);
add(worldVertex1, worldVertex1, convexOffset);
sub(worldEdge, worldVertex1, worldVertex0);
vec2.normalize(worldEdgeUnit, worldEdge);
// Get tangent to the edge. Points out of the Convex
vec2.rotate90cw(worldNormal, worldEdgeUnit);
// Get point on circle, closest to the polygon
vec2.scale(candidate,worldNormal,-circleShape.radius);
add(candidate,candidate,circleOffset);
if(pointInConvex(candidate,convexShape,convexOffset,convexAngle)){
vec2.sub(candidateDist,worldVertex0,candidate);
var candidateDistance = Math.abs(vec2.dot(candidateDist,worldNormal));
if(candidateDistance < minCandidateDistance){
vec2.copy(minCandidate,candidate);
minCandidateDistance = candidateDistance;
vec2.scale(closestEdgeProjectedPoint,worldNormal,candidateDistance);
vec2.add(closestEdgeProjectedPoint,closestEdgeProjectedPoint,candidate);
found = true;
}
}
}
if(found){
if(justTest){
return true;
}
var c = this.createContactEquation(circleBody,convexBody,circleShape,convexShape);
vec2.sub(c.normalA, minCandidate, circleOffset);
vec2.normalize(c.normalA, c.normalA);
vec2.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 all vertices
if(circleRadius > 0){
for(var i=0; i<verts.length; i++){
var localVertex = verts[i];
vec2.rotate(worldVertex, localVertex, convexAngle);
add(worldVertex, worldVertex, convexOffset);
sub(dist, worldVertex, circleOffset);
if(vec2.squaredLength(dist) < Math.pow(circleRadius, 2)){
if(justTest){
return true;
}
var c = this.createContactEquation(circleBody,convexBody,circleShape,convexShape);
vec2.copy(c.normalA, dist);
vec2.normalize(c.normalA,c.normalA);
// Vector from circle to contact point is the normal times the circle radius
vec2.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_worldVertex0 = vec2.create(),
pic_worldVertex1 = vec2.create(),
pic_r0 = vec2.create(),
pic_r1 = vec2.create();
/*
* Check if a point is in a polygon
*/
function pointInConvex(worldPoint,convexShape,convexOffset,convexAngle){
var worldVertex0 = pic_worldVertex0,
worldVertex1 = pic_worldVertex1,
r0 = pic_r0,
r1 = pic_r1,
point = worldPoint,
verts = convexShape.vertices,
lastCross = null;
for(var i=0; i!==verts.length+1; i++){
var v0 = verts[i%verts.length],
v1 = verts[(i+1)%verts.length];
// Transform vertices to world
// @todo The point should be transformed to local coordinates in the convex, no need to transform each vertex
vec2.rotate(worldVertex0, v0, convexAngle);
vec2.rotate(worldVertex1, v1, convexAngle);
add(worldVertex0, worldVertex0, convexOffset);
add(worldVertex1, worldVertex1, convexOffset);
sub(r0, worldVertex0, point);
sub(r1, worldVertex1, point);
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
* @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,
orthoDist = tmp8,
projectedPoint = tmp9,
dist = tmp10,
worldVertex = tmp11,
closestEdge = -1,
closestEdgeDistance = null,
closestEdgeOrthoDist = tmp12,
closestEdgeProjectedPoint = tmp13,
r0 = tmp14, // vector from particle to vertex0
r1 = tmp15,
localPoint = tmp16,
candidateDist = tmp17,
minEdgeNormal = tmp18,
minCandidateDistance = Number.MAX_VALUE;
var numReported = 0,
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 true;
}
// Check edges first
var lastCross = null;
for(var i=0; i!==verts.length+1; i++){
var v0 = verts[i%verts.length],
v1 = verts[(i+1)%verts.length];
// Transform vertices to world
vec2.rotate(worldVertex0, v0, convexAngle);
vec2.rotate(worldVertex1, v1, convexAngle);
add(worldVertex0, worldVertex0, convexOffset);
add(worldVertex1, worldVertex1, convexOffset);
// Get world edge
sub(worldEdge, worldVertex1, worldVertex0);
vec2.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);
vec2.sub(candidateDist,worldVertex0,particleOffset);
var candidateDistance = Math.abs(vec2.dot(candidateDist,worldTangent));
if(candidateDistance < minCandidateDistance){
minCandidateDistance = candidateDistance;
vec2.scale(closestEdgeProjectedPoint,worldTangent,candidateDistance);
vec2.add(closestEdgeProjectedPoint,closestEdgeProjectedPoint,particleOffset);
vec2.copy(minEdgeNormal,worldTangent);
found = true;
}
}
if(found){
var c = this.createContactEquation(particleBody,convexBody,particleShape,convexShape);
vec2.scale(c.normalA, minEdgeNormal, -1);
vec2.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
*/
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(vec2.squaredLength(dist) > Math.pow(r,2)){
return 0;
}
if(justTest){
return true;
}
var c = this.createContactEquation(bodyA,bodyB,shapeA,shapeB);
sub(c.normalA, offsetB, offsetA);
vec2.normalize(c.normalA,c.normalA);
vec2.scale( c.contactPointA, c.normalA, radiusA);
vec2.scale( c.contactPointB, c.normalA, -radiusB);
add(c.contactPointA, c.contactPointA, offsetA);
sub(c.contactPointA, c.contactPointA, bodyA.position);
add(c.contactPointB, c.contactPointB, offsetB);
sub(c.contactPointB, c.contactPointB, bodyB.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
return 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
*/
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;
var numReported = 0;
vec2.rotate(worldNormal, yAxis, planeAngle);
for(var i=0; i!==convexShape.vertices.length; i++){
var v = convexShape.vertices[i];
vec2.rotate(worldVertex, v, convexAngle);
add(worldVertex, worldVertex, convexOffset);
sub(dist, worldVertex, planeOffset);
if(dot(dist,worldNormal) <= 0){
if(justTest){
return true;
}
// Found vertex
numReported++;
var c = this.createContactEquation(planeBody,convexBody,planeShape,convexShape);
sub(dist, worldVertex, planeOffset);
vec2.copy(c.normalA, worldNormal);
var d = dot(dist, c.normalA);
vec2.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
*/
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);
vec2.rotate(worldNormal, yAxis, planeAngle);
var d = dot(dist, worldNormal);
if(d > 0){
return 0;
}
if(justTest){
return true;
}
var c = this.createContactEquation(planeBody,particleBody,planeShape,particleShape);
vec2.copy(c.normalA, worldNormal);
vec2.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
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.PARTICLE] =
Narrowphase.prototype.circleParticle = function(
circleBody,
circleShape,
circleOffset,
circleAngle,
particleBody,
particleShape,
particleOffset,
particleAngle,
justTest
){
var dist = tmp1;
sub(dist, particleOffset, circleOffset);
if(vec2.squaredLength(dist) > Math.pow(circleShape.radius, 2)){
return 0;
}
if(justTest){
return true;
}
var c = this.createContactEquation(circleBody,particleBody,circleShape,particleShape);
vec2.copy(c.normalA, dist);
vec2.normalize(c.normalA,c.normalA);
// Vector from circle to contact point is the normal times the circle radius
vec2.scale(c.contactPointA, c.normalA, circleShape.radius);
add(c.contactPointA, c.contactPointA, circleOffset);
sub(c.contactPointA, c.contactPointA, circleBody.position);
// Vector from particle center to contact point is zero
sub(c.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 = vec2.create(),
planeCapsule_tmp2 = vec2.create(),
planeCapsule_tmp3 = vec2.create();
/**
* @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
*/
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,
dst = planeCapsule_tmp3;
// Compute world end positions
vec2.set(end1, -capsuleShape.length/2, 0);
vec2.rotate(end1,end1,capsuleAngle);
add(end1,end1,capsuleOffset);
vec2.set(end2, capsuleShape.length/2, 0);
vec2.rotate(end2,end2,capsuleAngle);
add(end2,end2,capsuleOffset);
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;
}
};
/**
* Creates ContactEquations and FrictionEquations for a collision.
* @method circlePlane
* @param {Body} bi The first body that should be connected to the equations.
* @param {Circle} si The circle shape participating in the collision.
* @param {Array} xi Extra offset to take into account for the Shape, in addition to the one in circleBody.position. Will *not* be rotated by circleBody.angle (maybe it should, for sake of homogenity?). Set to null if none.
* @param {Body} bj The second body that should be connected to the equations.
* @param {Plane} sj The Plane shape that is participating
* @param {Array} xj Extra offset for the plane shape.
* @param {Number} aj Extra angle to apply to the plane
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.PLANE] =
Narrowphase.prototype.circlePlane = function( bi,si,xi,ai, bj,sj,xj,aj, justTest ){
var circleBody = bi,
circleShape = si,
circleOffset = xi, // Offset from body center, rotated!
planeBody = bj,
shapeB = sj,
planeOffset = xj,
planeAngle = aj;
planeAngle = planeAngle || 0;
// Vector from plane to circle
var planeToCircle = tmp1,
worldNormal = tmp2,
temp = tmp3;
sub(planeToCircle, circleOffset, planeOffset);
// World plane normal
vec2.rotate(worldNormal, yAxis, planeAngle);
// Normal direction distance
var d = dot(worldNormal, planeToCircle);
if(d > circleShape.radius){
return 0; // No overlap. Abort.
}
if(justTest){
return true;
}
// Create contact
var contact = this.createContactEquation(planeBody,circleBody,sj,si);
// ni is the plane world normal
vec2.copy(contact.normalA, worldNormal);
// rj is the vector from circle center to the contact point
vec2.scale(contact.contactPointB, contact.normalA, -circleShape.radius);
add(contact.contactPointB, contact.contactPointB, circleOffset);
sub(contact.contactPointB, contact.contactPointB, circleBody.position);
// ri is the distance from plane center to contact.
vec2.scale(temp, contact.normalA, d);
sub(contact.contactPointA, planeToCircle, temp ); // Subtract normal distance vector from the distance vector
add(contact.contactPointA, contact.contactPointA, planeOffset);
sub(contact.contactPointA, contact.contactPointA, planeBody.position);
this.contactEquations.push(contact);
if(this.enableFriction){
this.frictionEquations.push( this.createFrictionFromContact(contact) );
}
return 1;
};
/**
* Convex/convex Narrowphase.See <a href="http://www.altdevblogaday.com/2011/05/13/contact-generation-between-3d-convex-meshes/">this article</a> for more info.
* @method convexConvex
* @param {Body} bi
* @param {Convex} si
* @param {Array} xi
* @param {Number} ai
* @param {Body} bj
* @param {Convex} sj
* @param {Array} xj
* @param {Number} aj
*/
Narrowphase.prototype[Shape.CONVEX] =
Narrowphase.prototype[Shape.CONVEX | Shape.BOX] =
Narrowphase.prototype[Shape.BOX] =
Narrowphase.prototype.convexConvex = function( bi,si,xi,ai, bj,sj,xj,aj, justTest, precision ){
var sepAxis = tmp1,
worldPoint = tmp2,
worldPoint0 = tmp3,
worldPoint1 = tmp4,
worldEdge = tmp5,
projected = tmp6,
penetrationVec = tmp7,
dist = tmp8,
worldNormal = tmp9,
numContacts = 0,
precision = typeof(precision) === 'number' ? precision : 0;
var found = Narrowphase.findSeparatingAxis(si,xi,ai,sj,xj,aj,sepAxis);
if(!found){
return 0;
}
// Make sure the separating axis is directed from shape i to shape j
sub(dist,xj,xi);
if(dot(sepAxis,dist) > 0){
vec2.scale(sepAxis,sepAxis,-1);
}
// Find edges with normals closest to the separating axis
var closestEdge1 = Narrowphase.getClosestEdge(si,ai,sepAxis,true), // Flipped axis
closestEdge2 = Narrowphase.getClosestEdge(sj,aj,sepAxis);
if(closestEdge1 === -1 || closestEdge2 === -1){
return 0;
}
// Loop over the shapes
for(var k=0; k<2; k++){
var closestEdgeA = closestEdge1,
closestEdgeB = closestEdge2,
shapeA = si, shapeB = sj,
offsetA = xi, offsetB = xj,
angleA = ai, angleB = aj,
bodyA = bi, bodyB = bj;
if(k === 0){
// Swap!
var tmp;
tmp = closestEdgeA;
closestEdgeA = closestEdgeB;
closestEdgeB = tmp;
tmp = shapeA;
shapeA = shapeB;
shapeB = tmp;
tmp = offsetA;
offsetA = offsetB;
offsetB = tmp;
tmp = angleA;
angleA = angleB;
angleB = tmp;
tmp = bodyA;
bodyA = bodyB;
bodyB = tmp;
}
// Loop over 2 points in convex B
for(var j=closestEdgeB; j<closestEdgeB+2; j++){
// Get world point
var v = shapeB.vertices[(j+shapeB.vertices.length)%shapeB.vertices.length];
vec2.rotate(worldPoint, v, angleB);
add(worldPoint, worldPoint, offsetB);
var insideNumEdges = 0;
// Loop over the 3 closest edges in convex A
for(var i=closestEdgeA-1; i<closestEdgeA+2; i++){
var v0 = shapeA.vertices[(i +shapeA.vertices.length)%shapeA.vertices.length],
v1 = shapeA.vertices[(i+1+shapeA.vertices.length)%shapeA.vertices.length];
// Construct the edge
vec2.rotate(worldPoint0, v0, angleA);
vec2.rotate(worldPoint1, v1, angleA);
add(worldPoint0, worldPoint0, offsetA);
add(worldPoint1, worldPoint1, offsetA);
sub(worldEdge, worldPoint1, worldPoint0);
vec2.rotate90cw(worldNormal, worldEdge); // Normal points out of convex 1
vec2.normalize(worldNormal,worldNormal);
sub(dist, worldPoint, worldPoint0);
var d = dot(worldNormal,dist);
if((i === closestEdgeA && d <= precision) || (i !== closestEdgeA && d <= 0)){
insideNumEdges++;
}
}
if(insideNumEdges >= 3){
if(justTest){
return true;
}
// worldPoint was on the "inside" side of each of the 3 checked edges.
// Project it to the center edge and use the projection direction as normal
// Create contact
var c = this.createContactEquation(bodyA,bodyB,shapeA,shapeB);
numContacts++;
// Get center edge from body A
var v0 = shapeA.vertices[(closestEdgeA) % shapeA.vertices.length],
v1 = shapeA.vertices[(closestEdgeA+1) % shapeA.vertices.length];
// Construct the edge
vec2.rotate(worldPoint0, v0, angleA);
vec2.rotate(worldPoint1, v1, angleA);
add(worldPoint0, worldPoint0, offsetA);
add(worldPoint1, worldPoint1, offsetA);
sub(worldEdge, worldPoint1, worldPoint0);
vec2.rotate90cw(c.normalA, worldEdge); // Normal points out of convex A
vec2.normalize(c.normalA,c.normalA);
sub(dist, worldPoint, worldPoint0); // From edge point to the penetrating point
var d = dot(c.normalA,dist); // Penetration
vec2.scale(penetrationVec, c.normalA, d); // Vector penetration
sub(c.contactPointA, worldPoint, offsetA);
sub(c.contactPointA, c.contactPointA, penetrationVec);
add(c.contactPointA, c.contactPointA, offsetA);
sub(c.contactPointA, c.contactPointA, bodyA.position);
sub(c.contactPointB, worldPoint, offsetB);
add(c.contactPointB, c.contactPointB, offsetB);
sub(c.contactPointB, c.contactPointB, bodyB.position);
this.contactEquations.push(c);
// Todo reduce to 1 friction equation if we have 2 contact points
if(!this.enableFrictionReduction){
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
}
}
}
}
if(this.enableFrictionReduction){
if(this.enableFriction && numContacts){
this.frictionEquations.push(this.createFrictionFromAverage(numContacts));
}
}
return numContacts;
};
// .projectConvex is called by other functions, need local tmp vectors
var pcoa_tmp1 = vec2.fromValues(0,0);
/**
* Project a Convex onto a world-oriented axis
* @method projectConvexOntoAxis
* @static
* @param {Convex} convexShape
* @param {Array} convexOffset
* @param {Number} convexAngle
* @param {Array} worldAxis
* @param {Array} result
*/
Narrowphase.projectConvexOntoAxis = function(convexShape, convexOffset, convexAngle, worldAxis, result){
var max=null,
min=null,
v,
value,
localAxis = pcoa_tmp1;
// Convert the axis to local coords of the body
vec2.rotate(localAxis, worldAxis, -convexAngle);
// Get projected position of all vertices
for(var i=0; i<convexShape.vertices.length; i++){
v = convexShape.vertices[i];
value = dot(v,localAxis);
if(max === null || value > max){
max = value;
}
if(min === null || value < min){
min = value;
}
}
if(min > max){
var t = min;
min = max;
max = t;
}
// Project the position of the body onto the axis - need to add this to the result
var offset = dot(convexOffset, worldAxis);
vec2.set( result, min + offset, max + offset);
};
// .findSeparatingAxis is called by other functions, need local tmp vectors
var fsa_tmp1 = vec2.fromValues(0,0)
, fsa_tmp2 = vec2.fromValues(0,0)
, fsa_tmp3 = vec2.fromValues(0,0)
, fsa_tmp4 = vec2.fromValues(0,0)
, fsa_tmp5 = vec2.fromValues(0,0)
, fsa_tmp6 = vec2.fromValues(0,0);
/**
* Find a separating axis between the shapes, that maximizes the separating distance between them.
* @method findSeparatingAxis
* @static
* @param {Convex} c1
* @param {Array} offset1
* @param {Number} angle1
* @param {Convex} c2
* @param {Array} offset2
* @param {Number} angle2
* @param {Array} sepAxis The resulting axis
* @return {Boolean} Whether the axis could be found.
*/
Narrowphase.findSeparatingAxis = function(c1,offset1,angle1,c2,offset2,angle2,sepAxis){
var maxDist = null,
overlap = false,
found = false,
edge = fsa_tmp1,
worldPoint0 = fsa_tmp2,
worldPoint1 = fsa_tmp3,
normal = fsa_tmp4,
span1 = fsa_tmp5,
span2 = fsa_tmp6;
if(c1 instanceof Box && c2 instanceof Box){
for(var j=0; j!==2; j++){
var c = c1,
angle = angle1;
if(j===1){
c = c2;
angle = angle2;
}
for(var i=0; i!==2; i++){
// Get the world edge
if(i === 0){
vec2.set(normal, 0, 1);
} else if(i === 1) {
vec2.set(normal, 1, 0);
}
if(angle !== 0){
vec2.rotate(normal, normal, angle);
}
// Project hulls onto that normal
Narrowphase.projectConvexOntoAxis(c1,offset1,angle1,normal,span1);
Narrowphase.projectConvexOntoAxis(c2,offset2,angle2,normal,span2);
// Order by span position
var a=span1,
b=span2,
swapped = false;
if(span1[0] > span2[0]){
b=span1;
a=span2;
swapped = true;
}
// Get separating distance
var dist = b[0] - a[1];
overlap = (dist <= 0);
if(maxDist===null || dist > maxDist){
vec2.copy(sepAxis, normal);
maxDist = dist;
found = overlap;
}
}
}
} else {
for(var j=0; j!==2; j++){
var c = c1,
angle = angle1;
if(j===1){
c = c2;
angle = angle2;
}
for(var i=0; i!==c.vertices.length; i++){
// Get the world edge
vec2.rotate(worldPoint0, c.vertices[i], angle);
vec2.rotate(worldPoint1, c.vertices[(i+1)%c.vertices.length], angle);
sub(edge, worldPoint1, worldPoint0);
// Get normal - just rotate 90 degrees since vertices are given in CCW
vec2.rotate90cw(normal, edge);
vec2.normalize(normal,normal);
// Project hulls onto that normal
Narrowphase.projectConvexOntoAxis(c1,offset1,angle1,normal,span1);
Narrowphase.projectConvexOntoAxis(c2,offset2,angle2,normal,span2);
// Order by span position
var a=span1,
b=span2,
swapped = false;
if(span1[0] > span2[0]){
b=span1;
a=span2;
swapped = true;
}
// Get separating distance
var dist = b[0] - a[1];
overlap = (dist <= 0);
if(maxDist===null || dist > maxDist){
vec2.copy(sepAxis, normal);
maxDist = dist;
found = overlap;
}
}
}
}
/*
// Needs to be tested some more
for(var j=0; j!==2; j++){
var c = c1,
angle = angle1;
if(j===1){
c = c2;
angle = angle2;
}
for(var i=0; i!==c.axes.length; i++){
var normal = c.axes[i];
// Project hulls onto that normal
Narrowphase.projectConvexOntoAxis(c1, offset1, angle1, normal, span1);
Narrowphase.projectConvexOntoAxis(c2, offset2, angle2, normal, span2);
// Order by span position
var a=span1,
b=span2,
swapped = false;
if(span1[0] > span2[0]){
b=span1;
a=span2;
swapped = true;
}
// Get separating distance
var dist = b[0] - a[1];
overlap = (dist <= Narrowphase.convexPrecision);
if(maxDist===null || dist > maxDist){
vec2.copy(sepAxis, normal);
maxDist = dist;
found = overlap;
}
}
}
*/
return found;
};
// .getClosestEdge is called by other functions, need local tmp vectors
var gce_tmp1 = vec2.fromValues(0,0)
, gce_tmp2 = vec2.fromValues(0,0)
, gce_tmp3 = vec2.fromValues(0,0);
/**
* Get the edge that has a normal closest to an axis.
* @method getClosestEdge
* @static
* @param {Convex} c
* @param {Number} angle
* @param {Array} axis
* @param {Boolean} flip
* @return {Number} Index of the edge that is closest. This index and the next spans the resulting edge. Returns -1 if failed.
*/
Narrowphase.getClosestEdge = function(c,angle,axis,flip){
var localAxis = gce_tmp1,
edge = gce_tmp2,
normal = gce_tmp3;
// Convert the axis to local coords of the body
vec2.rotate(localAxis, axis, -angle);
if(flip){
vec2.scale(localAxis,localAxis,-1);
}
var closestEdge = -1,
N = c.vertices.length,
maxDot = -1;
for(var i=0; i!==N; i++){
// Get the edge
sub(edge, c.vertices[(i+1)%N], c.vertices[i%N]);
// Get normal - just rotate 90 degrees since vertices are given in CCW
vec2.rotate90cw(normal, edge);
vec2.normalize(normal,normal);
var d = dot(normal,localAxis);
if(closestEdge === -1 || d > maxDot){
closestEdge = i % N;
maxDot = d;
}
}
return closestEdge;
};
var circleHeightfield_candidate = vec2.create(),
circleHeightfield_dist = vec2.create(),
circleHeightfield_v0 = vec2.create(),
circleHeightfield_v1 = vec2.create(),
circleHeightfield_minCandidate = vec2.create(),
circleHeightfield_worldNormal = vec2.create(),
circleHeightfield_minCandidateNormal = vec2.create();
/**
* @method circleHeightfield
* @param {Body} bi
* @param {Circle} si
* @param {Array} xi
* @param {Body} bj
* @param {Heightfield} sj
* @param {Array} xj
* @param {Number} aj
*/
Narrowphase.prototype[Shape.CIRCLE | Shape.HEIGHTFIELD] =
Narrowphase.prototype.circleHeightfield = function( circleBody,circleShape,circlePos,circleAngle,
hfBody,hfShape,hfPos,hfAngle, justTest, radius ){
var data = hfShape.heights,
radius = radius || circleShape.radius,
w = hfShape.elementWidth,
dist = circleHeightfield_dist,
candidate = circleHeightfield_candidate,
minCandidate = circleHeightfield_minCandidate,
minCandidateNormal = circleHeightfield_minCandidateNormal,
worldNormal = circleHeightfield_worldNormal,
v0 = circleHeightfield_v0,
v1 = circleHeightfield_v1;
// Get the index of the points to test against
var idxA = Math.floor( (circlePos[0] - radius - hfPos[0]) / w ),
idxB = Math.ceil( (circlePos[0] + radius - hfPos[0]) / w );
/*if(idxB < 0 || idxA >= data.length)
return justTest ? false : 0;*/
if(idxA < 0){
idxA = 0;
}
if(idxB >= data.length){
idxB = data.length-1;
}
// Get max and min
var max = data[idxA],
min = data[idxB];
for(var i=idxA; i<idxB; i++){
if(data[i] < min){
min = data[i];
}
if(data[i] > max){
max = data[i];
}
}
if(circlePos[1]-radius > max){
return justTest ? false : 0;
}
/*
if(circlePos[1]+radius < min){
// Below the minimum point... We can just guess.
// TODO
}
*/
// 1. Check so center of circle is not inside the field. 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.
var found = false;
// Check all edges first
for(var i=idxA; i<idxB; i++){
// Get points
vec2.set(v0, i*w, data[i] );
vec2.set(v1, (i+1)*w, data[i+1]);
vec2.add(v0,v0,hfPos);
vec2.add(v1,v1,hfPos);
// Get normal
vec2.sub(worldNormal, v1, v0);
vec2.rotate(worldNormal, worldNormal, Math.PI/2);
vec2.normalize(worldNormal,worldNormal);
// Get point on circle, closest to the edge
vec2.scale(candidate,worldNormal,-radius);
vec2.add(candidate,candidate,circlePos);
// Distance from v0 to the candidate point
vec2.sub(dist,candidate,v0);
// Check if it is in the element "stick"
var d = vec2.dot(dist,worldNormal);
if(candidate[0] >= v0[0] && candidate[0] < v1[0] && d <= 0){
if(justTest){
return true;
}
found = true;
// Store the candidate point, projected to the edge
vec2.scale(dist,worldNormal,-d);
vec2.add(minCandidate,candidate,dist);
vec2.copy(minCandidateNormal,worldNormal);
var c = this.createContactEquation(hfBody,circleBody,hfShape,circleShape);
// Normal is out of the heightfield
vec2.copy(c.normalA, minCandidateNormal);
// Vector from circle to heightfield
vec2.scale(c.contactPointB, c.normalA, -radius);
add(c.contactPointB, c.contactPointB, circlePos);
sub(c.contactPointB, c.contactPointB, circleBody.position);
vec2.copy(c.contactPointA, minCandidate);
vec2.sub(c.contactPointA, c.contactPointA, hfBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push( this.createFrictionFromContact(c) );
}
}
}
// Check all vertices
found = false;
if(radius > 0){
for(var i=idxA; i<=idxB; i++){
// Get point
vec2.set(v0, i*w, data[i]);
vec2.add(v0,v0,hfPos);
vec2.sub(dist, circlePos, v0);
if(vec2.squaredLength(dist) < Math.pow(radius, 2)){
if(justTest){
return true;
}
found = true;
var c = this.createContactEquation(hfBody,circleBody,hfShape,circleShape);
// Construct normal - out of heightfield
vec2.copy(c.normalA, dist);
vec2.normalize(c.normalA,c.normalA);
vec2.scale(c.contactPointB, c.normalA, -radius);
add(c.contactPointB, c.contactPointB, circlePos);
sub(c.contactPointB, c.contactPointB, circleBody.position);
sub(c.contactPointA, v0, hfPos);
add(c.contactPointA, c.contactPointA, hfPos);
sub(c.contactPointA, c.contactPointA, hfBody.position);
this.contactEquations.push(c);
if(this.enableFriction){
this.frictionEquations.push(this.createFrictionFromContact(c));
}
}
}
}
if(found){
return 1;
}
return 0;
};
var convexHeightfield_v0 = vec2.create(),
convexHeightfield_v1 = vec2.create(),
convexHeightfield_tilePos = vec2.create(),
convexHeightfield_tempConvexShape = new Convex({ vertices: [vec2.create(),vec2.create(),vec2.create(),vec2.create()] });
/**
* @method circleHeightfield
* @param {Body} bi
* @param {Circle} si
* @param {Array} xi
* @param {Body} bj
* @param {Heightfield} sj
* @param {Array} xj
* @param {Number} aj
*/
Narrowphase.prototype[Shape.BOX | Shape.HEIGHTFIELD] =
Narrowphase.prototype[Shape.CONVEX | Shape.HEIGHTFIELD] =
Narrowphase.prototype.convexHeightfield = function( convexBody,convexShape,convexPos,convexAngle,
hfBody,hfShape,hfPos,hfAngle, justTest ){
var data = hfShape.heights,
w = hfShape.elementWidth,
v0 = convexHeightfield_v0,
v1 = convexHeightfield_v1,
tilePos = convexHeightfield_tilePos,
tileConvex = convexHeightfield_tempConvexShape;
// Get the index of the points to test against
var idxA = Math.floor( (convexBody.aabb.lowerBound[0] - hfPos[0]) / w ),
idxB = Math.ceil( (convexBody.aabb.upperBound[0] - hfPos[0]) / w );
if(idxA < 0){
idxA = 0;
}
if(idxB >= data.length){
idxB = data.length-1;
}
// Get max and min
var max = data[idxA],
min = data[idxB];
for(var i=idxA; i<idxB; i++){
if(data[i] < min){
min = data[i];
}
if(data[i] > max){
max = data[i];
}
}
if(convexBody.aabb.lowerBound[1] > max){
return justTest ? false : 0;
}
var found = false;
var numContacts = 0;
// Loop over all edges
// TODO: If possible, construct a convex from several data points (need o check if the points make a convex shape)
for(var i=idxA; i<idxB; i++){
// Get points
vec2.set(v0, i*w, data[i] );
vec2.set(v1, (i+1)*w, data[i+1]);
vec2.add(v0,v0,hfPos);
vec2.add(v1,v1,hfPos);
// Construct a convex
var tileHeight = 100; // todo
vec2.set(tilePos, (v1[0] + v0[0])*0.5, (v1[1] + v0[1] - tileHeight)*0.5);
vec2.sub(tileConvex.vertices[0], v1, tilePos);
vec2.sub(tileConvex.vertices[1], v0, tilePos);
vec2.copy(tileConvex.vertices[2], tileConvex.vertices[1]);
vec2.copy(tileConvex.vertices[3], tileConvex.vertices[0]);
tileConvex.vertices[2][1] -= tileHeight;
tileConvex.vertices[3][1] -= tileHeight;
// Do convex collision
numContacts += this.convexConvex( convexBody, convexShape, convexPos, convexAngle,
hfBody, tileConvex, tilePos, 0, justTest);
}
return numContacts;
};
