module.exports = Narrowphase;
var Shape = require('../shapes/Shape');
var Vec3 = require('../math/Vec3');
var Transform = require('../math/Transform');
var ConvexPolyhedron = require('../shapes/ConvexPolyhedron');
var Quaternion = require('../math/Quaternion');
var Solver = require('../solver/Solver');
var Vec3Pool = require('../utils/Vec3Pool');
var ContactEquation = require('../equations/ContactEquation');
/**
* Helper class for the World. Generates ContactEquations.
* @class Narrowphase
* @constructor
* @todo Sphere-ConvexPolyhedron contacts
* @todo Contact reduction
* @todo should move methods to prototype
*/
function Narrowphase(){
/**
* Turns on or off contact reduction. Can be handy to turn off when debugging new collision types.
* @property bool contactReduction
*/
this.contactReduction = false;
/**
* Internal storage of pooled contact points.
* @property {Array} contactPointPool
*/
this.contactPointPool = [];
/**
* Pooled vectors.
* @property {Vec3Pool} v3pool
*/
this.v3pool = new Vec3Pool();
}
/**
* Swaps the body references in the contact
* @method swapResult
* @param object r
*/
Narrowphase.prototype.swapResult = function(r){
var temp;
temp = r.ri;
r.ri = r.rj;
r.rj = temp;
r.ni.negate(r.ni);
temp = r.bi;
r.bi = r.bj;
r.bj = temp;
};
/**
* Removes unnecessary members of an array of ContactEquation.
* @method reduceContacts
* @param {Array} contacts
*/
Narrowphase.prototype.reduceContacts = function(contacts){
};
/**
* Make a contact object, by using the internal pool or creating a new one.
* @method makeResult
* @return {ContactEquation}
*/
Narrowphase.prototype.makeResult = function(bi, bj, si, sj, rsi, rsj){
var c;
if(this.contactPointPool.length){
c = this.contactPointPool.pop();
c.bi = bi;
c.bj = bj;
} else {
c = new ContactEquation(bi, bj);
}
c.enabled = true;
c.si = rsi || si;
c.sj = rsj || sj;
return c;
};
var tmpVec1 = new Vec3();
var tmpVec2 = new Vec3();
var tmpQuat1 = new Quaternion();
var tmpQuat2 = new Quaternion();
/**
* Generate all contacts between a list of body pairs
* @method getContacts
* @param {array} p1 Array of body indices
* @param {array} p2 Array of body indices
* @param {World} world
* @param {array} result Array to store generated contacts
* @param {array} oldcontacts Optional. Array of reusable contact objects
*/
Narrowphase.prototype.getContacts = function(p1,p2,world,result,oldcontacts){
// Save old contact objects
this.contactPointPool = oldcontacts;
var qi = tmpQuat1;
var qj = tmpQuat2;
var xi = tmpVec1;
var xj = tmpVec2;
for(var k=0, N=p1.length; k!==N; k++){
// Get current collision bodies
var bi = p1[k],
bj = p2[k];
for (var i = 0; i < bi.shapes.length; i++) {
bi.quaternion.mult(bi.shapeOrientations[i], qi);
bi.quaternion.vmult(bi.shapeOffsets[i], xi);
xi.vadd(bi.position, xi);
var si = bi.shapes[i];
for (var j = 0; j < bj.shapes.length; j++) {
// Compute world transform of shapes
bj.quaternion.mult(bj.shapeOrientations[j], qj);
bj.quaternion.vmult(bj.shapeOffsets[j], xj);
xj.vadd(bj.position, xj);
var sj = bj.shapes[j];
// Get contacts
var resolver = this[si.type | sj.type];
if(resolver){
if (si.type < sj.type) {
resolver.call(this, result, si,sj,xi,xj,qi,qj,bi,bj,si,sj);
} else {
resolver.call(this, result, sj,si,xj,xi,qj,qi,bj,bi,si,sj);
}
}
/*
this.narrowphase(
result,
bi.shapes[i],
bj.shapes[j],
xi, xj,
qi, qj,
bi, bj
);
*/
}
}
}
};
var numWarnings = 0;
var maxWarnings = 10;
function warn(msg){
if(numWarnings > maxWarnings){
return;
}
numWarnings++;
console.warn(msg);
}
/**
* Narrowphase calculation. Get the ContactEquations given two shapes: i and j
* @method narrowphase
* @param {array} result The result one will get back with all the contact point information
* @param {Shape} si Colliding shape. If not given, particle is assumed.
* @param {Shape} sj
* @param {Vec3} xi Position of the center of mass
* @param {Vec3} xj
* @param {Quaternion} qi Rotation around the center of mass
* @param {Quaternion} qj
*/
/*
Narrowphase.prototype.narrowphase = function(result,si,sj,xi,xj,qi,qj,bi,bj){
var swapped = false,
types = Shape.types,
SPHERE = types.SPHERE,
PLANE = types.PLANE,
PARTICLE = types.PARTICLE,
BOX = types.BOX,
COMPOUND = types.COMPOUND,
CONVEXPOLYHEDRON = types.CONVEXPOLYHEDRON,
HEIGHTFIELD = types.HEIGHTFIELD;
if(si.type > sj.type){
var temp;
temp = sj;
sj = si;
si = temp;
temp = xj;
xj = xi;
xi = temp;
temp = qj;
qj = qi;
qi = temp;
temp = bj;
bj = bi;
bi = temp;
swapped = true;
}
if(si && sj){
if(si.type === SPHERE){
switch(sj.type){
case SPHERE: // sphere-sphere
this.sphereSphere(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case PLANE: // sphere-plane
this.spherePlane(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case BOX: // sphere-box
this.sphereBox(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case CONVEXPOLYHEDRON: // sphere-convexpolyhedron
this.sphereConvex(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case HEIGHTFIELD: // sphere-heightfield
this.sphereHeightfield(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case PARTICLE: // Particle vs sphere
this.particleSphere(result,sj,si,xj,xi,qj,qi,bj,bi);
break;
default:
warn("Collision between Shape.types.SPHERE and "+sj.type+" not implemented yet.");
break;
}
} else if(si.type === types.PLANE){
switch(sj.type){
case PLANE: // plane-plane
// Should not give collision
break;
case BOX: // plane-box
this.planeBox(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case CONVEXPOLYHEDRON: // plane-convex polyhedron
this.planeConvex(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case PARTICLE: // plane-convex polyhedron
this.particlePlane(result,sj,si,xj,xi,qj,qi,bj,bi);
break;
default:
warn("Collision between Shape.types.PLANE and "+sj.type+" not implemented yet.");
break;
}
} else if(si.type===types.BOX){
switch(sj.type){
case BOX: // box-box
// Do convex/convex instead
this.convexConvex(result,si.convexPolyhedronRepresentation,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj,si,sj);
break;
case CONVEXPOLYHEDRON: // box-convexpolyhedron
// Do convex/convex instead
this.convexConvex(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
break;
case PARTICLE: // Particle vs box
this.convexParticle(result,sj,si.convexPolyhedronRepresentation,xj,xi,qj,qi,bj,bi,si,sj);
break;
case HEIGHTFIELD: // Box vs heightfield
this.convexHeightfield(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
break;
default:
warn("Collision between Shape.BOX and "+sj.type+" not implemented yet.");
break;
}
} else if(si.type===types.CONVEXPOLYHEDRON){
switch(sj.type){
case types.CONVEXPOLYHEDRON: // convex polyhedron - convex polyhedron
this.convexConvex(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case types.PARTICLE: // particle-convex
this.convexParticle(result,sj,si,xj,xi,qj,qi,bj,bi);
break;
default:
warn("Collision between Shape.types.CONVEXPOLYHEDRON and "+sj.type+" not implemented yet.");
break;
}
} else if(si.type===types.HEIGHTFIELD){
switch(sj.type){
case types.SPHERE: // heightfield/sphere
this.sphereHeightfield(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
default:
warn("Collision between Shape.types.HEIGHTFIELD and "+sj.type+" not implemented yet.");
break;
}
} else if(si.type === PARTICLE){
// Particle!
switch(sj.type){
case types.PLANE: // Particle vs plane
this.particlePlane(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case types.SPHERE: // Particle vs sphere
this.particleSphere(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
case types.BOX: // Particle vs box
this.convexParticle(result,si,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj);
break;
case types.CONVEXPOLYHEDRON: // particle-convex
this.convexParticle(result,si,sj,xi,xj,qi,qj,bi,bj);
break;
default:
console.warn("Collision between Particle and "+sj.type+" not implemented yet.");
break;
}
}
}
// Swap back if we swapped bodies in the beginning
for(var i=0, Nresults=result.length; swapped && i!==Nresults; i++){
this.swapResult(result[i]);
}
};
*/
Narrowphase.prototype[Shape.types.BOX | Shape.types.BOX] =
Narrowphase.prototype.boxBox = function(result,si,sj,xi,xj,qi,qj,bi,bj){
this.convexConvex(result,si.convexPolyhedronRepresentation,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj,si,sj);
};
Narrowphase.prototype[Shape.types.BOX | Shape.types.CONVEXPOLYHEDRON] =
Narrowphase.prototype.boxConvex = function(result,si,sj,xi,xj,qi,qj,bi,bj){
this.convexConvex(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
};
Narrowphase.prototype[Shape.types.BOX | Shape.types.PARTICLE] =
Narrowphase.prototype.boxParticle = function(result,si,sj,xi,xj,qi,qj,bi,bj){
this.convexParticle(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj,si,sj);
};
/**
* @method sphereSphere
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.SPHERE] =
Narrowphase.prototype.sphereSphere = function(result,si,sj,xi,xj,qi,qj,bi,bj){
// We will have only one contact in this case
var r = this.makeResult(bi,bj,si,sj);
// Contact normal
bj.position.vsub(xi, r.ni);
r.ni.normalize();
// Contact point locations
r.ri.copy(r.ni);
r.rj.copy(r.ni);
r.ri.mult(si.radius, r.ri);
r.rj.mult(-sj.radius, r.rj);
result.push(r);
};
var point_on_plane_to_sphere = new Vec3();
var plane_to_sphere_ortho = new Vec3();
/**
* @method spherePlane
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.SPHERE | Shape.types.PLANE] =
Narrowphase.prototype.spherePlane = function(result,si,sj,xi,xj,qi,qj,bi,bj){
// We will have one contact in this case
var r = this.makeResult(bi,bj,si,sj);
// Contact normal
r.ni.set(0,0,1);
qj.vmult(r.ni, r.ni);
r.ni.negate(r.ni); // body i is the sphere, flip normal
r.ni.normalize(); // Needed?
// Vector from sphere center to contact point
r.ni.mult(si.radius, r.ri);
// Project down sphere on plane
xi.vsub(xj, point_on_plane_to_sphere);
r.ni.mult(r.ni.dot(point_on_plane_to_sphere), plane_to_sphere_ortho);
point_on_plane_to_sphere.vsub(plane_to_sphere_ortho,r.rj); // The sphere position projected to plane
if(plane_to_sphere_ortho.norm2() <= si.radius * si.radius){
result.push(r);
// Make it relative to the body
var ri = r.ri;
var rj = r.rj;
ri.vadd(xi, ri);
ri.vsub(bi.position, ri);
rj.vadd(xj, rj);
rj.vsub(bj.position, rj);
}
};
// See http://bulletphysics.com/Bullet/BulletFull/SphereTriangleDetector_8cpp_source.html
var pointInPolygon_edge = new Vec3();
var pointInPolygon_edge_x_normal = new Vec3();
var pointInPolygon_vtp = new Vec3();
function pointInPolygon(verts, normal, p){
var positiveResult = null;
var N = verts.length;
for(var i=0; i!==N; i++){
var v = verts[i];
// Get edge to the next vertex
var edge = pointInPolygon_edge;
verts[(i+1) % (N)].vsub(v,edge);
// Get cross product between polygon normal and the edge
var edge_x_normal = pointInPolygon_edge_x_normal;
//var edge_x_normal = new Vec3();
edge.cross(normal,edge_x_normal);
// Get vector between point and current vertex
var vertex_to_p = pointInPolygon_vtp;
p.vsub(v,vertex_to_p);
// This dot product determines which side of the edge the point is
var r = edge_x_normal.dot(vertex_to_p);
// If all such dot products have same sign, we are inside the polygon.
if(positiveResult===null || (r>0 && positiveResult===true) || (r<=0 && positiveResult===false)){
if(positiveResult===null){
positiveResult = r>0;
}
continue;
} else {
return false; // Encountered some other sign. Exit.
}
}
// If we got here, all dot products were of the same sign.
return true;
}
var box_to_sphere = new Vec3();
var sphereBox_ns = new Vec3();
var sphereBox_ns1 = new Vec3();
var sphereBox_ns2 = new Vec3();
var sphereBox_sides = [new Vec3(),new Vec3(),new Vec3(),new Vec3(),new Vec3(),new Vec3()];
var sphereBox_sphere_to_corner = new Vec3();
var sphereBox_side_ns = new Vec3();
var sphereBox_side_ns1 = new Vec3();
var sphereBox_side_ns2 = new Vec3();
/**
* @method sphereBox
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.SPHERE | Shape.types.BOX] =
Narrowphase.prototype.sphereBox = function(result,si,sj,xi,xj,qi,qj,bi,bj){
var v3pool = this.v3pool;
// we refer to the box as body j
var sides = sphereBox_sides;
xi.vsub(xj,box_to_sphere);
sj.getSideNormals(sides,qj);
var R = si.radius;
var penetrating_sides = [];
// Check side (plane) intersections
var found = false;
// Store the resulting side penetration info
var side_ns = sphereBox_side_ns;
var side_ns1 = sphereBox_side_ns1;
var side_ns2 = sphereBox_side_ns2;
var side_h = null;
var side_penetrations = 0;
var side_dot1 = 0;
var side_dot2 = 0;
var side_distance = null;
for(var idx=0,nsides=sides.length; idx!==nsides && found===false; idx++){
// Get the plane side normal (ns)
var ns = sphereBox_ns;
ns.copy(sides[idx]);
var h = ns.norm();
ns.normalize();
// The normal/distance dot product tells which side of the plane we are
var dot = box_to_sphere.dot(ns);
if(dot<h+R && dot>0){
// Intersects plane. Now check the other two dimensions
var ns1 = sphereBox_ns1;
var ns2 = sphereBox_ns2;
ns1.copy(sides[(idx+1)%3]);
ns2.copy(sides[(idx+2)%3]);
var h1 = ns1.norm();
var h2 = ns2.norm();
ns1.normalize();
ns2.normalize();
var dot1 = box_to_sphere.dot(ns1);
var dot2 = box_to_sphere.dot(ns2);
if(dot1<h1 && dot1>-h1 && dot2<h2 && dot2>-h2){
var dist = Math.abs(dot-h-R);
if(side_distance===null || dist < side_distance){
side_distance = dist;
side_dot1 = dot1;
side_dot2 = dot2;
side_h = h;
side_ns.copy(ns);
side_ns1.copy(ns1);
side_ns2.copy(ns2);
side_penetrations++;
}
}
}
}
if(side_penetrations){
found = true;
var r = this.makeResult(bi,bj,si,sj);
side_ns.mult(-R,r.ri); // Sphere r
r.ni.copy(side_ns);
r.ni.negate(r.ni); // Normal should be out of sphere
side_ns.mult(side_h,side_ns);
side_ns1.mult(side_dot1,side_ns1);
side_ns.vadd(side_ns1,side_ns);
side_ns2.mult(side_dot2,side_ns2);
side_ns.vadd(side_ns2,r.rj);
// Make relative to bodies
r.ri.vadd(xi, r.ri);
r.ri.vsub(bi.position, r.ri);
r.rj.vadd(xj, r.rj);
r.rj.vsub(bj.position, r.rj);
result.push(r);
}
// Check corners
var rj = v3pool.get();
var sphere_to_corner = sphereBox_sphere_to_corner;
for(var j=0; j!==2 && !found; j++){
for(var k=0; k!==2 && !found; k++){
for(var l=0; l!==2 && !found; l++){
rj.set(0,0,0);
if(j){
rj.vadd(sides[0],rj);
} else {
rj.vsub(sides[0],rj);
}
if(k){
rj.vadd(sides[1],rj);
} else {
rj.vsub(sides[1],rj);
}
if(l){
rj.vadd(sides[2],rj);
} else {
rj.vsub(sides[2],rj);
}
// World position of corner
xj.vadd(rj,sphere_to_corner);
sphere_to_corner.vsub(xi,sphere_to_corner);
if(sphere_to_corner.norm2() < R*R){
found = true;
var r = this.makeResult(bi,bj,si,sj);
r.ri.copy(sphere_to_corner);
r.ri.normalize();
r.ni.copy(r.ri);
r.ri.mult(R,r.ri);
r.rj.copy(rj);
// Make relative to bodies
r.ri.vadd(xi, r.ri);
r.ri.vsub(bi.position, r.ri);
r.rj.vadd(xj, r.rj);
r.rj.vsub(bj.position, r.rj);
result.push(r);
}
}
}
}
v3pool.release(rj);
rj = null;
// Check edges
var edgeTangent = v3pool.get();
var edgeCenter = v3pool.get();
var r = v3pool.get(); // r = edge center to sphere center
var orthogonal = v3pool.get();
var dist = v3pool.get();
var Nsides = sides.length;
for(var j=0; j!==Nsides && !found; j++){
for(var k=0; k!==Nsides && !found; k++){
if(j%3 !== k%3){
// Get edge tangent
sides[k].cross(sides[j],edgeTangent);
edgeTangent.normalize();
sides[j].vadd(sides[k], edgeCenter);
r.copy(xi);
r.vsub(edgeCenter,r);
r.vsub(xj,r);
var orthonorm = r.dot(edgeTangent); // distance from edge center to sphere center in the tangent direction
edgeTangent.mult(orthonorm,orthogonal); // Vector from edge center to sphere center in the tangent direction
// Find the third side orthogonal to this one
var l = 0;
while(l===j%3 || l===k%3){
l++;
}
// vec from edge center to sphere projected to the plane orthogonal to the edge tangent
dist.copy(xi);
dist.vsub(orthogonal,dist);
dist.vsub(edgeCenter,dist);
dist.vsub(xj,dist);
// Distances in tangent direction and distance in the plane orthogonal to it
var tdist = Math.abs(orthonorm);
var ndist = dist.norm();
if(tdist < sides[l].norm() && ndist<R){
found = true;
var res = this.makeResult(bi,bj,si,sj);
edgeCenter.vadd(orthogonal,res.rj); // box rj
res.rj.copy(res.rj);
dist.negate(res.ni);
res.ni.normalize();
res.ri.copy(res.rj);
res.ri.vadd(xj,res.ri);
res.ri.vsub(xi,res.ri);
res.ri.normalize();
res.ri.mult(R,res.ri);
// Make relative to bodies
res.ri.vadd(xi, res.ri);
res.ri.vsub(bi.position, res.ri);
res.rj.vadd(xj, res.rj);
res.rj.vsub(bj.position, res.rj);
result.push(res);
}
}
}
}
v3pool.release(edgeTangent,edgeCenter,r,orthogonal,dist);
};
var convex_to_sphere = new Vec3();
var sphereConvex_edge = new Vec3();
var sphereConvex_edgeUnit = new Vec3();
var sphereConvex_sphereToCorner = new Vec3();
var sphereConvex_worldCorner = new Vec3();
var sphereConvex_worldNormal = new Vec3();
var sphereConvex_worldPoint = new Vec3();
var sphereConvex_worldSpherePointClosestToPlane = new Vec3();
var sphereConvex_penetrationVec = new Vec3();
var sphereConvex_sphereToWorldPoint = new Vec3();
/**
* @method sphereConvex
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.SPHERE | Shape.types.CONVEXPOLYHEDRON] =
Narrowphase.prototype.sphereConvex = function(result,si,sj,xi,xj,qi,qj,bi,bj){
var v3pool = this.v3pool;
xi.vsub(xj,convex_to_sphere);
var normals = sj.faceNormals;
var faces = sj.faces;
var verts = sj.vertices;
var R = si.radius;
var penetrating_sides = [];
// Check corners
for(var i=0; i!==verts.length; i++){
var v = verts[i];
// World position of corner
var worldCorner = sphereConvex_worldCorner;
qj.vmult(v,worldCorner);
xj.vadd(worldCorner,worldCorner);
var sphere_to_corner = sphereConvex_sphereToCorner;
worldCorner.vsub(xi, sphere_to_corner);
if(sphere_to_corner.norm2() < R * R){
found = true;
var r = this.makeResult(bi,bj,si,sj);
r.ri.copy(sphere_to_corner);
r.ri.normalize();
r.ni.copy(r.ri);
r.ri.mult(R,r.ri);
worldCorner.vsub(xj,r.rj);
// Should be relative to the body.
r.ri.vadd(xi, r.ri);
r.ri.vsub(bi.position, r.ri);
// Should be relative to the body.
r.rj.vadd(xj, r.rj);
r.rj.vsub(bj.position, r.rj);
result.push(r);
return;
}
}
// Check side (plane) intersections
var found = false;
for(var i=0, nfaces=faces.length; i!==nfaces && found===false; i++){
var normal = normals[i];
var face = faces[i];
// Get world-transformed normal of the face
var worldNormal = sphereConvex_worldNormal;
qj.vmult(normal,worldNormal);
// Get a world vertex from the face
var worldPoint = sphereConvex_worldPoint;
qj.vmult(verts[face[0]],worldPoint);
worldPoint.vadd(xj,worldPoint);
// Get a point on the sphere, closest to the face normal
var worldSpherePointClosestToPlane = sphereConvex_worldSpherePointClosestToPlane;
worldNormal.mult(-R, worldSpherePointClosestToPlane);
xi.vadd(worldSpherePointClosestToPlane, worldSpherePointClosestToPlane);
// Vector from a face point to the closest point on the sphere
var penetrationVec = sphereConvex_penetrationVec;
worldSpherePointClosestToPlane.vsub(worldPoint,penetrationVec);
// The penetration. Negative value means overlap.
var penetration = penetrationVec.dot(worldNormal);
var worldPointToSphere = sphereConvex_sphereToWorldPoint;
xi.vsub(worldPoint, worldPointToSphere);
if(penetration < 0 && worldPointToSphere.dot(worldNormal)>0){
// Intersects plane. Now check if the sphere is inside the face polygon
var faceVerts = []; // Face vertices, in world coords
for(var j=0, Nverts=face.length; j!==Nverts; j++){
var worldVertex = v3pool.get();
qj.vmult(verts[face[j]], worldVertex);
xj.vadd(worldVertex,worldVertex);
faceVerts.push(worldVertex);
}
if(pointInPolygon(faceVerts,worldNormal,xi)){ // Is the sphere center in the face polygon?
found = true;
var r = this.makeResult(bi,bj,si,sj);
worldNormal.mult(-R, r.ri); // Contact offset, from sphere center to contact
worldNormal.negate(r.ni); // Normal pointing out of sphere
var penetrationVec2 = v3pool.get();
worldNormal.mult(-penetration, penetrationVec2);
var penetrationSpherePoint = v3pool.get();
worldNormal.mult(-R, penetrationSpherePoint);
//xi.vsub(xj).vadd(penetrationSpherePoint).vadd(penetrationVec2 , r.rj);
xi.vsub(xj,r.rj);
r.rj.vadd(penetrationSpherePoint,r.rj);
r.rj.vadd(penetrationVec2 , r.rj);
// Should be relative to the body.
r.rj.vadd(xj, r.rj);
r.rj.vsub(bj.position, r.rj);
// Should be relative to the body.
r.ri.vadd(xi, r.ri);
r.ri.vsub(bi.position, r.ri);
v3pool.release(penetrationVec2);
v3pool.release(penetrationSpherePoint);
result.push(r);
// Release world vertices
for(var j=0, Nfaceverts=faceVerts.length; j!==Nfaceverts; j++){
v3pool.release(faceVerts[j]);
}
return; // We only expect *one* face contact
} else {
// Edge?
for(var j=0; j!==face.length; j++){
// Get two world transformed vertices
var v1 = v3pool.get();
var v2 = v3pool.get();
qj.vmult(verts[face[(j+1)%face.length]], v1);
qj.vmult(verts[face[(j+2)%face.length]], v2);
xj.vadd(v1, v1);
xj.vadd(v2, v2);
// Construct edge vector
var edge = sphereConvex_edge;
v2.vsub(v1,edge);
// Construct the same vector, but normalized
var edgeUnit = sphereConvex_edgeUnit;
edge.unit(edgeUnit);
// p is xi projected onto the edge
var p = v3pool.get();
var v1_to_xi = v3pool.get();
xi.vsub(v1, v1_to_xi);
var dot = v1_to_xi.dot(edgeUnit);
edgeUnit.mult(dot, p);
p.vadd(v1, p);
// Compute a vector from p to the center of the sphere
var xi_to_p = v3pool.get();
p.vsub(xi, xi_to_p);
// Collision if the edge-sphere distance is less than the radius
// AND if p is in between v1 and v2
if(dot > 0 && dot*dot<edge.norm2() && xi_to_p.norm2() < R*R){ // Collision if the edge-sphere distance is less than the radius
// Edge contact!
var r = this.makeResult(bi,bj,si,sj);
p.vsub(xj,r.rj);
p.vsub(xi,r.ni);
r.ni.normalize();
r.ni.mult(R,r.ri);
// Should be relative to the body.
r.rj.vadd(xj, r.rj);
r.rj.vsub(bj.position, r.rj);
// Should be relative to the body.
r.ri.vadd(xi, r.ri);
r.ri.vsub(bi.position, r.ri);
result.push(r);
// Release world vertices
for(var j=0, Nfaceverts=faceVerts.length; j!==Nfaceverts; j++){
v3pool.release(faceVerts[j]);
}
v3pool.release(v1);
v3pool.release(v2);
v3pool.release(p);
v3pool.release(xi_to_p);
v3pool.release(v1_to_xi);
return;
}
v3pool.release(v1);
v3pool.release(v2);
v3pool.release(p);
v3pool.release(xi_to_p);
v3pool.release(v1_to_xi);
}
}
// Release world vertices
for(var j=0, Nfaceverts=faceVerts.length; j!==Nfaceverts; j++){
v3pool.release(faceVerts[j]);
}
}
}
};
var planeBox_normal = new Vec3();
var plane_to_corner = new Vec3();
/**
* @method planeBox
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.PLANE | Shape.types.BOX] =
Narrowphase.prototype.planeBox = function(result,si,sj,xi,xj,qi,qj,bi,bj){
this.planeConvex(result,si,sj.convexPolyhedronRepresentation,xi,xj,qi,qj,bi,bj);
};
var planeConvex_v = new Vec3();
var planeConvex_normal = new Vec3();
var planeConvex_relpos = new Vec3();
var planeConvex_projected = new Vec3();
/**
* @method planeConvex
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.PLANE | Shape.types.CONVEXPOLYHEDRON] =
Narrowphase.prototype.planeConvex = function(
result,
planeShape,
convexShape,
planePosition,
convexPosition,
planeQuat,
convexQuat,
planeBody,
convexBody
){
// Simply return the points behind the plane.
var worldVertex = planeConvex_v,
worldNormal = planeConvex_normal;
worldNormal.set(0,0,1);
planeQuat.vmult(worldNormal,worldNormal); // Turn normal according to plane orientation
var relpos = planeConvex_relpos;
for(var i = 0; i !== convexShape.vertices.length; i++){
// Get world convex vertex
worldVertex.copy(convexShape.vertices[i]);
convexQuat.vmult(worldVertex, worldVertex);
convexPosition.vadd(worldVertex, worldVertex);
worldVertex.vsub(planePosition, relpos);
var dot = worldNormal.dot(relpos);
if(dot <= 0.0){
var r = this.makeResult(planeBody, convexBody, planeShape, convexShape);
// Get vertex position projected on plane
var projected = planeConvex_projected;
worldNormal.mult(worldNormal.dot(relpos),projected);
worldVertex.vsub(projected, projected);
projected.vsub(planePosition, r.ri); // From plane to vertex projected on plane
r.ni.copy(worldNormal); // Contact normal is the plane normal out from plane
// rj is now just the vector from the convex center to the vertex
worldVertex.vsub(convexPosition, r.rj);
// Make it relative to the body
r.ri.vadd(planePosition, r.ri);
r.ri.vsub(planeBody.position, r.ri);
r.rj.vadd(convexPosition, r.rj);
r.rj.vsub(convexBody.position, r.rj);
result.push(r);
}
}
};
var convexConvex_sepAxis = new Vec3();
var convexConvex_q = new Vec3();
/**
* @method convexConvex
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.CONVEXPOLYHEDRON] =
Narrowphase.prototype.convexConvex = function(result,si,sj,xi,xj,qi,qj,bi,bj,rsi,rsj){
var sepAxis = convexConvex_sepAxis;
if(si.findSeparatingAxis(sj,xi,qi,xj,qj,sepAxis)){
var res = [];
var q = convexConvex_q;
si.clipAgainstHull(xi,qi,sj,xj,qj,sepAxis,-100,100,res);
for(var j = 0; j !== res.length; j++){
var r = this.makeResult(bi,bj,si,sj,rsi,rsj),
ri = r.ri,
rj = r.rj;
sepAxis.negate(r.ni);
res[j].normal.negate(q);
q.mult(res[j].depth, q);
res[j].point.vadd(q, ri);
rj.copy(res[j].point);
// Contact points are in world coordinates. Transform back to relative
ri.vsub(xi,ri);
rj.vsub(xj,rj);
// Make relative to bodies
ri.vadd(xi, ri);
ri.vsub(bi.position, ri);
rj.vadd(xj, rj);
rj.vsub(bj.position, rj);
result.push(r);
}
}
};
var particlePlane_normal = new Vec3();
var particlePlane_relpos = new Vec3();
var particlePlane_projected = new Vec3();
/**
* @method particlePlane
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.PLANE | Shape.types.PARTICLE] =
Narrowphase.prototype.planeParticle = function(result,sj,si,xj,xi,qj,qi,bj,bi){
var normal = particlePlane_normal;
normal.set(0,0,1);
bj.quaternion.vmult(normal,normal); // Turn normal according to plane orientation
var relpos = particlePlane_relpos;
xi.vsub(bj.position,relpos);
var dot = normal.dot(relpos);
if(dot <= 0.0){
var r = this.makeResult(bi,bj,si,sj);
r.ni.copy(normal); // Contact normal is the plane normal
r.ni.negate(r.ni);
r.ri.set(0,0,0); // Center of particle
// Get particle position projected on plane
var projected = particlePlane_projected;
normal.mult(normal.dot(xi),projected);
xi.vsub(projected,projected);
//projected.vadd(bj.position,projected);
// rj is now the projected world position minus plane position
r.rj.copy(projected);
result.push(r);
}
};
var particleSphere_normal = new Vec3();
/**
* @method particleSphere
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.PARTICLE | Shape.types.SPHERE] =
Narrowphase.prototype.sphereParticle = function(result,sj,si,xj,xi,qj,qi,bj,bi){
// The normal is the unit vector from sphere center to particle center
var normal = particleSphere_normal;
normal.set(0,0,1);
xi.vsub(xj,normal);
var lengthSquared = normal.norm2();
if(lengthSquared <= sj.radius * sj.radius){
var r = this.makeResult(bi,bj,si,sj);
normal.normalize();
r.rj.copy(normal);
r.rj.mult(sj.radius,r.rj);
r.ni.copy(normal); // Contact normal
r.ni.negate(r.ni);
r.ri.set(0,0,0); // Center of particle
result.push(r);
}
};
// WIP
var cqj = new Quaternion();
var convexParticle_local = new Vec3();
var convexParticle_normal = new Vec3();
var convexParticle_penetratedFaceNormal = new Vec3();
var convexParticle_vertexToParticle = new Vec3();
var convexParticle_worldPenetrationVec = new Vec3();
/**
* @method convexParticle
* @param {Array} result
* @param {Shape} si
* @param {Shape} sj
* @param {Vec3} xi
* @param {Vec3} xj
* @param {Quaternion} qi
* @param {Quaternion} qj
* @param {Body} bi
* @param {Body} bj
*/
Narrowphase.prototype[Shape.types.PARTICLE | Shape.types.CONVEXPOLYHEDRON] =
Narrowphase.prototype.convexParticle = function(result,sj,si,xj,xi,qj,qi,bj,bi){
var penetratedFaceIndex = -1;
var penetratedFaceNormal = convexParticle_penetratedFaceNormal;
var worldPenetrationVec = convexParticle_worldPenetrationVec;
var minPenetration = null;
var numDetectedFaces = 0;
// Convert particle position xi to local coords in the convex
var local = convexParticle_local;
local.copy(xi);
local.vsub(xj,local); // Convert position to relative the convex origin
qj.conjugate(cqj);
cqj.vmult(local,local);
if(sj.pointIsInside(local)){
if(sj.worldVerticesNeedsUpdate){
sj.computeWorldVertices(xj,qj);
}
if(sj.worldFaceNormalsNeedsUpdate){
sj.computeWorldFaceNormals(qj);
}
// For each world polygon in the polyhedra
for(var i=0,nfaces=sj.faces.length; i!==nfaces; i++){
// Construct world face vertices
var verts = [ sj.worldVertices[ sj.faces[i][0] ] ];
var normal = sj.worldFaceNormals[i];
// Check how much the particle penetrates the polygon plane.
xi.vsub(verts[0],convexParticle_vertexToParticle);
var penetration = -normal.dot(convexParticle_vertexToParticle);
if(minPenetration===null || Math.abs(penetration)<Math.abs(minPenetration)){
minPenetration = penetration;
penetratedFaceIndex = i;
penetratedFaceNormal.copy(normal);
numDetectedFaces++;
}
}
if(penetratedFaceIndex!==-1){
// Setup contact
var r = this.makeResult(bi,bj,si,sj);
penetratedFaceNormal.mult(minPenetration, worldPenetrationVec);
// rj is the particle position projected to the face
worldPenetrationVec.vadd(xi,worldPenetrationVec);
worldPenetrationVec.vsub(xj,worldPenetrationVec);
r.rj.copy(worldPenetrationVec);
//var projectedToFace = xi.vsub(xj).vadd(worldPenetrationVec);
//projectedToFace.copy(r.rj);
//qj.vmult(r.rj,r.rj);
penetratedFaceNormal.negate( r.ni ); // Contact normal
r.ri.set(0,0,0); // Center of particle
var ri = r.ri,
rj = r.rj;
// Make relative to bodies
ri.vadd(xi, ri);
ri.vsub(bi.position, ri);
rj.vadd(xj, rj);
rj.vsub(bj.position, rj);
result.push(r);
} else {
console.warn("Point found inside convex, but did not find penetrating face!");
}
}
};
Narrowphase.prototype[Shape.types.BOX | Shape.types.HEIGHTFIELD] =
Narrowphase.prototype.boxHeightfield = function (result,si,sj,xi,xj,qi,qj,bi,bj){
this.convexHeightfield(result,si.convexPolyhedronRepresentation,sj,xi,xj,qi,qj,bi,bj);
};
var convexHeightfield_tmp1 = new Vec3();
var convexHeightfield_tmp2 = new Vec3();
/**
* @method sphereHeightfield
*/
Narrowphase.prototype[Shape.types.CONVEXPOLYHEDRON | Shape.types.HEIGHTFIELD] =
Narrowphase.prototype.convexHeightfield = function (
result,
convexShape,
hfShape,
convexPos,
hfPos,
convexQuat,
hfQuat,
convexBody,
hfBody
){
var data = hfShape.data,
w = hfShape.elementSize,
radius = convexShape.boundingSphereRadius,
worldPillarOffset = convexHeightfield_tmp2;
// Get sphere position to heightfield local!
var localConvexPos = convexHeightfield_tmp1;
Transform.pointToLocalFrame(hfPos, hfQuat, convexPos, localConvexPos);
// Get the index of the data points to test against
var iMinX = Math.floor((localConvexPos.x - radius) / w) - 1,
iMaxX = Math.ceil((localConvexPos.x + radius) / w) + 1,
iMinY = Math.floor((localConvexPos.y - radius) / w) - 1,
iMaxY = Math.ceil((localConvexPos.y + radius) / w) + 1;
// Bail out if we are out of the terrain
if(iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMaxY > data[0].length){
return;
}
// Clamp index to edges
if(iMinX < 0){ iMinX = 0; }
if(iMaxX < 0){ iMaxX = 0; }
if(iMinY < 0){ iMinY = 0; }
if(iMaxY < 0){ iMaxY = 0; }
if(iMinX >= data.length){ iMinX = data.length - 1; }
if(iMaxX >= data.length){ iMaxX = data.length - 1; }
if(iMaxY >= data[0].length){ iMaxY = data[0].length - 1; }
if(iMinY >= data[0].length){ iMinY = data[0].length - 1; }
var minMax = [];
hfShape.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax);
var min = minMax[0];
var max = minMax[1];
// Bail out if we're cant touch the bounding height box
if(localConvexPos.z - radius > max || localConvexPos.z + radius < min){
return;
}
for(var i = iMinX; i < iMaxX; i++){
for(var j = iMinY; j < iMaxY; j++){
// Lower triangle
hfShape.getConvexTrianglePillar(i, j, false);
Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
this.convexConvex(result, convexShape, hfShape.pillarConvex, convexPos, worldPillarOffset, convexQuat, hfQuat, convexBody, hfBody);
// Upper triangle
hfShape.getConvexTrianglePillar(i, j, true);
Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
this.convexConvex(result, convexShape, hfShape.pillarConvex, convexPos, worldPillarOffset, convexQuat, hfQuat, convexBody, hfBody);
}
}
};
var sphereHeightfield_tmp1 = new Vec3();
var sphereHeightfield_tmp2 = new Vec3();
/**
* @method sphereHeightfield
*/
Narrowphase.prototype[Shape.types.SPHERE | Shape.types.HEIGHTFIELD] =
Narrowphase.prototype.sphereHeightfield = function (
result,
sphereShape,
hfShape,
spherePos,
hfPos,
sphereQuat,
hfQuat,
sphereBody,
hfBody
){
var data = hfShape.data,
radius = sphereShape.radius,
w = hfShape.elementSize,
worldPillarOffset = sphereHeightfield_tmp2;
// Get sphere position to heightfield local!
var localSpherePos = sphereHeightfield_tmp1;
Transform.pointToLocalFrame(hfPos, hfQuat, spherePos, localSpherePos);
// Get the index of the data points to test against
var iMinX = Math.floor((localSpherePos.x - radius) / w) - 1,
iMaxX = Math.ceil((localSpherePos.x + radius) / w) + 1,
iMinY = Math.floor((localSpherePos.y - radius) / w) - 1,
iMaxY = Math.ceil((localSpherePos.y + radius) / w) + 1;
// Bail out if we are out of the terrain
if(iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMaxY > data[0].length){
return;
}
// Clamp index to edges
if(iMinX < 0){ iMinX = 0; }
if(iMaxX < 0){ iMaxX = 0; }
if(iMinY < 0){ iMinY = 0; }
if(iMaxY < 0){ iMaxY = 0; }
if(iMinX >= data.length){ iMinX = data.length - 1; }
if(iMaxX >= data.length){ iMaxX = data.length - 1; }
if(iMaxY >= data[0].length){ iMaxY = data[0].length - 1; }
if(iMinY >= data[0].length){ iMinY = data[0].length - 1; }
var minMax = [];
hfShape.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax);
var min = minMax[0];
var max = minMax[1];
// Bail out if we're cant touch the bounding height box
if(localSpherePos.z - radius > max || localSpherePos.z + radius < min){
return;
}
for(var i = iMinX; i < iMaxX; i++){
for(var j = iMinY; j < iMaxY; j++){
var numContactsBefore = result.length;
// Lower triangle
hfShape.getConvexTrianglePillar(i, j, false);
Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
this.sphereConvex(result, sphereShape, hfShape.pillarConvex, spherePos, worldPillarOffset, sphereQuat, hfQuat, sphereBody, hfBody);
// Upper triangle
hfShape.getConvexTrianglePillar(i, j, true);
Transform.pointToWorldFrame(hfPos, hfQuat, hfShape.pillarOffset, worldPillarOffset);
this.sphereConvex(result, sphereShape, hfShape.pillarConvex, spherePos, worldPillarOffset, sphereQuat, hfQuat, sphereBody, hfBody);
var numContacts = result.length - numContactsBefore;
if(numContacts > 2){
return;
}
/*
// Skip all but 1
for (var k = 0; k < numContacts - 1; k++) {
result.pop();
}
*/
}
}
};