cyclone-physics
Version:
Pure Javascript physics engine based on http://procyclone.com/
1,221 lines (1,053 loc) • 198 kB
JavaScript
'use strict';
/**
* colliders
*/
elation.require(['physics.common', 'utils.math'], function() {
elation.extend("physics.colliders.helperfuncs", new function() {
this.sphere_sphere = function() {
// closure scratch variables
var thispos = new THREE.Vector3(),
otherpos = new THREE.Vector3(),
thisvel = new THREE.Vector3(),
othervel = new THREE.Vector3(),
midline = new THREE.Vector3(),
scaledVelocity = new THREE.Vector3(),
intersectionPoint = new THREE.Vector3();
return function(obj1, obj2, contacts, dt) {
if (!contacts) contacts = [];
// Work in world space
obj1.body.localToWorldPos(thispos.set(0,0,0));
obj2.body.localToWorldPos(otherpos.set(0,0,0));
let dynamic = (obj1.body.velocity.lengthSq() > 0 || obj2.body.velocity.lengthSq() > 0); // TODO - this should either be a flag on rigid bodies, or a configurable threshold based on velocity
if (!dynamic) {
midline.subVectors(otherpos, thispos),
size = midline.length();
midline.divideScalar(size);
var normal = midline.clone(); // allocate normal
var point = thispos.clone().add(midline.multiplyScalar(obj1.radius)); // allocate point
if (!(size <= 0 || size >= obj1.radius + obj2.radius)) {
var penetration = (obj1.radius + obj2.radius - size);
// Collision point is on the outer shell of obj1
contact = new elation.physics.contact({
normal: normal,
point: point,
penetration: -penetration,
bodies: [obj1.body, obj2.body]
});
contacts.push(contact);
//console.log('crash a sphere-sphere', contact);
}
} else {
let r = obj1.radius + obj2.radius;
// FIXME - probably need to transform velocity into world coordinates as well
obj1.body.localToWorldDir(thisvel.copy(obj1.body.velocity));
obj2.body.localToWorldDir(othervel.copy(obj2.body.velocity));
let v = scaledVelocity.copy(thisvel).sub(othervel).multiplyScalar(dt);
midline.copy(thispos).sub(otherpos).normalize();
//if (midline.dot(scaledVelocity) > 0) return; // moving away, can't collide
let endpos = midline.copy(thispos).add(v);
let intersection = elation.physics.colliders.helperfuncs.line_sphere(thispos, endpos, otherpos, r, intersectionPoint);
if (intersection && intersection.t <= 1) {
let t = intersection.t;
thispos.add(scaledVelocity.copy(obj1.body.velocity).multiplyScalar(t * dt));
otherpos.add(scaledVelocity.copy(obj2.body.velocity).multiplyScalar(t * dt));
//console.log(intersection, thispos, v, otherpos, r, obj1, obj2);
let normal = otherpos.clone().sub(thispos).normalize(); // allocate normal
var contact = new elation.physics.contact_dynamic({
normal: normal,
point: normal.clone().multiplyScalar(obj1.radius).add(thispos), // allocate point
penetrationTime: intersection.t,
bodies: [obj1.body, obj2.body],
});
contacts.push(contact);
return contacts;
}
}
return contacts;
}
}();
this.sphere_plane = function() {
// closure scratch variables
var pos = new THREE.Vector3();
return function(sphere, plane, contacts, dt) {
if (!contacts) contacts = [];
var contact = false;
var position = sphere.body.localToWorldPos(pos.set(0,0,0));
var norm = plane.body.localToWorldDir(plane.normal.clone()); // allocate normal
var distance = norm.dot(pos) - sphere.radius - plane.offset;
if (distance < 0) {
var sepspeed = sphere.body.velocity.dot(plane.normal);
if (sepspeed <= 0) {
var point = position.clone().sub(norm.clone().multiplyScalar(distance + sphere.radius)); // allocate point
contact = new elation.physics.contact({
normal: norm,
point: point,
penetration: -distance,
bodies: [sphere.body, plane.body]
});
contacts.push(contact);
//console.log('crash a sphere-plane!', contact);
}
}
return contacts;
}
}();
this.box_sphere = function() {
// closure scratch variables
var center = new THREE.Vector3(), // center of sphere, box-space coordinates (scaled)
centerWorld = new THREE.Vector3(), // center of sphere, world-space coordinates
diff = new THREE.Vector3(),
closest = new THREE.Vector3(),
closestWorld = new THREE.Vector3(),
invQuat = new THREE.Quaternion();
return function(box, sphere, contacts, dt) {
if (!contacts) contacts = [];
// Get sphere position in world space
if (sphere.offset) {
sphere.body.localToWorldPos(centerWorld.copy(sphere.offset));
} else {
sphere.body.localToWorldPos(centerWorld.set(0,0,0));
}
// Transform sphere center to box's SCALED local space
// (subtract position, apply inverse rotation, but do NOT divide by scale)
// This matches the coordinate system of box.min/max which are pre-scaled
center.copy(centerWorld).sub(box.body.positionWorld);
invQuat.copy(box.body.orientationWorld).invert();
center.applyQuaternion(invQuat);
// box.min/max are already in scaled local space, and so is center now
// sphere.radius is in world units which matches scaled local space
// Early out if any of the axes are separating
if ((center.x + sphere.radius < box.min.x || center.x - sphere.radius > box.max.x) ||
(center.y + sphere.radius < box.min.y || center.y - sphere.radius > box.max.y) ||
(center.z + sphere.radius < box.min.z || center.z - sphere.radius > box.max.z)) {
return false;
}
// Find closest point on box (in scaled local space)
closest.x = elation.utils.math.clamp(center.x, box.min.x, box.max.x);
closest.y = elation.utils.math.clamp(center.y, box.min.y, box.max.y);
closest.z = elation.utils.math.clamp(center.z, box.min.z, box.max.z);
// Check distance (all in scaled local space = world units)
diff.subVectors(closest, center);
var dist = diff.lengthSq();
if (dist > sphere.radius * sphere.radius) {
return 0;
}
// Transform closest point back to world space
closestWorld.copy(closest);
closestWorld.applyQuaternion(box.body.orientationWorld);
closestWorld.add(box.body.positionWorld);
var contact = new elation.physics.contact({
point: closestWorld.clone(), // allocate point
normal: centerWorld.clone().sub(closestWorld).normalize(), // allocate normal
penetration: -(sphere.radius - Math.sqrt(dist)),
bodies: [box.body, sphere.body]
});
contacts.push(contact);
return contacts;
}
}();
this.box_plane = function() {
// closure scratch variables
var worldpos = new THREE.Vector3();
return function(box, plane, contacts, dt) {
if (!contacts) contacts = [];
var vertices = [
[box.min.x, box.min.y, box.min.z],
[box.min.x, box.min.y, box.max.z],
[box.min.x, box.max.y, box.min.z],
[box.min.x, box.max.y, box.max.z],
[box.max.x, box.min.y, box.min.z],
[box.max.x, box.min.y, box.max.z],
[box.max.x, box.max.y, box.min.z],
[box.max.x, box.max.y, box.max.z],
];
for (var i = 0; i < vertices.length; i++) {
// Pass world position of vertex to vertex_plane collider
// No allocations needed here, since they're done in vertex_plane
box.body.localToWorldPos(worldpos.set(vertices[i][0], vertices[i][1], vertices[i][2]));
var contact = this.vertex_plane(worldpos, plane);
if (contact) {
contact.bodies = [box.body, plane.body];
contacts.push(contact);
}
}
return contacts;
}
}();
this.vertex_vertex = function(v1, v2, contacts, dt) {
if (!contacts) contacts = [];
let distance = v1.distanceTo(v2);
if (distance < 1e6) {
var contact = new elation.physics.contact({
point: v1.clone(), // allocate point
//normal: normal.clone(), // allocate normal
penetration: distance
});
contacts.push(contact);
}
return contacts;
};
this.vertex_sphere = function() {
let localvert = new THREE.Vector3();
return function(vertex, sphere, contacts, dt) {
if (!contacts) contacts = [];
sphere.body.worldToLocal(localvert.copy(vertex));
let distance = localvert.length();
if (distance <= sphere.body.radius) {
var contact = new elation.physics.contact({
point: vertex.clone(), // allocate point
normal: sphere.body.localToWorldDir(localvert.clone().divideScalar(distance)), // allocate normal
penetration: distance
});
contacts.push(contact);
}
return contacts;
};
}();
this.vertex_box = function() {
var relpos = new THREE.Vector3();
return function(vertex, box, contacts, dt) {
if (!contacts) contacts = [];
// Get point in box-local coordinates
box.body.worldToLocalPos(relpos.copy(vertex));
// check x axis
var min_depth = box.halfsize.x - Math.abs(relpos.x);
if (min_depth < 0) return false;
// normal = ...
// check y axis
var depth = box.halfsize.y - Math.abs(relpos.y);
if (depth < 0) return false;
else if (depth < min_depth) {
min_depth = depth;
//normal = ...
}
// check z axis
depth = box.halfsize.z - Math.abs(relpos.z);
if (depth < 0) return false;
else if (depth < min_depth) {
min_depth = depth;
//normal = ...
}
var contact = new elation.physics.contact({
point: vertex.clone(), // allocate point
//normal: normal.clone(), // allocate normal
penetration: min_depth
});
contacts.push(contact);
return contacts;
}
}();
this.vertex_triangle = function(vertex, triangle, contacts, dt) {
if (triangle.containsPoint(vertex)) {
var contact = new elation.physics.contact({
point: vertex.clone(), // allocate point
normal: triangle.normal.clone(), // allocate normal, FIXME - transform into world coords
penetration: 0,
bodies: [vertex.body, triangle.body]
});
contacts.push(contact);
}
return contacts;
};
this.vertex_plane = function(vertex, plane) {
// FIXME - Only one contact possible...should this return a single-element array to be consistent?
var contact = false;
var distance = vertex.dot(plane.normal);
if (distance <= plane.offset) {
contact = new elation.physics.contact({
normal: plane.normal.clone(), // allocate normal
//point: plane.normal.clone().multiplyScalar((distance - plane.offset) / 2).add(vertex), // allocate point
point: vertex.clone().sub(plane.normal.clone().multiplyScalar(distance)),
penetration: plane.offset - distance
});
//console.log('crash a vertex-plane', contact.point.toArray(), contact.normal.toArray());
}
return contact;
}
this.vertex_capsule = (function() {
const closest = new THREE.Vector3();
return function(vertex, capsule) {
let capsuleDims = capsule.getDimensions();
let scaledRadius = capsuleDims.scaledRadius;
elation.physics.colliders.helperfuncs.closest_point_on_line(capsuleDims.start, capsuleDims.end, vertex, closest);
let distSq = closest.distanceToSquared(vertex);
if (distSq <= scaledRadius * scaledRadius) {
let dist = Math.sqrt(distSq);
let normal = closest.clone().sub(vertex).divideScalar(dist);
let point = closest.clone();
point.x += normal.x * scaledRadius;
point.y += normal.y * scaledRadius;
point.z += normal.z * scaledRadius;
let contact = new elation.physics.contact({
normal: normal,
point: point,
penetration: dist - scaledRadius,
});
return contact;
}
}
})();
this.box_box_old = function() {
// closure scratch variables
var diff = new THREE.Vector3(),
thispos = new THREE.Vector3(),
otherpos = new THREE.Vector3(),
matrix1 = new THREE.Matrix4(),
matrix2 = new THREE.Matrix4(),
axis = new THREE.Vector3(),
axis2 = new THREE.Vector3(),
corner = new THREE.Vector3(),
smallestPenetration, smallestIndex, best;
var axes = [
new THREE.Vector3(1,0,0),
new THREE.Vector3(0,1,0),
new THREE.Vector3(0,0,1)
];
// static helper functions
var tmpaxis = new THREE.Vector3();
function transformToAxis(box, axis) {
return (box.halfsize.x * Math.abs(axis.dot(box.body.localToWorldDir(tmpaxis.set(1,0,0))))) +
(box.halfsize.y * Math.abs(axis.dot(box.body.localToWorldDir(tmpaxis.set(0,1,0))))) +
(box.halfsize.z * Math.abs(axis.dot(box.body.localToWorldDir(tmpaxis.set(0,0,1)))));
}
function penetrationOnAxis(box1, box2, axis, diff) {
var oneProject = transformToAxis(box1, axis),
twoProject = transformToAxis(box2, axis),
distance = Math.abs(diff.dot(axis));
//console.log(axis.toArray(), oneProject, twoProject, distance, oneProject + twoProject - distance);
return oneProject + twoProject - distance;
}
function testOverlap(box1, box2, axis, diff, index) {
if (axis.lengthSq() < 0.0001) return true;
axis.normalize();
var penetration = penetrationOnAxis(box1, box2, axis, diff);
if (penetration < 0) return false;
if (penetration < smallestPenetration) {
smallestPenetration = penetration;
smallestIndex = index;
}
return true;
}
function getAxis(obj, index, taxis) {
if (!taxis) taxis = axis;
matrix1.makeRotationFromQuaternion(obj.body.orientationWorld);
var m1 = matrix1.elements;
var offset = index * 4;
taxis.set(m1[offset], m1[offset+1], m1[offset+2]);
return taxis;
}
function fillPointFaceBoxBox(box1, box2, toCenter, best, penetration) {
var point = new THREE.Vector3(); // allocate point
var normal = new THREE.Vector3(0,1,0); // allocate normal
getAxis(box1, best, normal);
if (normal.dot(toCenter) < 0) {
normal.multiplyScalar(-1);
}
point.copy(box2.halfsize);
if (getAxis(box2, 0, axis).dot(normal) < 0) point.x = -point.x;
if (getAxis(box2, 1, axis).dot(normal) < 0) point.y = -point.y;
if (getAxis(box2, 2, axis).dot(normal) < 0) point.z = -point.z;
var contact = new elation.physics.contact({
point: box2.body.localToWorldPos(point),
normal: normal.normalize(),
penetration: -penetration,
restitution: box1.body.restitution * box2.body.restitution,
bodies: [box1.body, box2.body]
});
return contact;
}
return function(box1, box2, contacts, dt) {
if (!contacts) contacts = [];
box1.body.localToWorldPos(thispos.set(0,0,0));
box2.body.localToWorldPos(otherpos.set(0,0,0));
diff.subVectors(otherpos, thispos);
matrix1.makeRotationFromQuaternion(box1.body.orientationWorld);
matrix2.makeRotationFromQuaternion(box2.body.orientationWorld);
var m1 = matrix1.elements,
m2 = matrix2.elements;
smallestPenetration = Infinity;
smallestIndex = false;
// box1's primary axes
if (!testOverlap(box1, box2, getAxis(box1, 0, axis), diff, 0)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 1, axis), diff, 1)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 2, axis), diff, 2)) return false;
// box 2's primary axes
if (!testOverlap(box1, box2, getAxis(box2, 0, axis), diff, 3)) return false;
if (!testOverlap(box1, box2, getAxis(box2, 1, axis), diff, 4)) return false;
if (!testOverlap(box1, box2, getAxis(box2, 2, axis), diff, 5)) return false;
var bestSingleAxis = smallestIndex;
// perpendicular axes
if (!testOverlap(box1, box2, getAxis(box1, 0, axis).cross(getAxis(box2, 0, axis2)), diff, 6)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 0, axis).cross(getAxis(box2, 1, axis2)), diff, 7)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 0, axis).cross(getAxis(box2, 2, axis2)), diff, 8)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 1, axis).cross(getAxis(box2, 0, axis2)), diff, 9)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 1, axis).cross(getAxis(box2, 1, axis2)), diff, 10)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 1, axis).cross(getAxis(box2, 2, axis2)), diff, 11)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 2, axis).cross(getAxis(box2, 0, axis2)), diff, 12)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 2, axis).cross(getAxis(box2, 1, axis2)), diff, 13)) return false;
if (!testOverlap(box1, box2, getAxis(box1, 2, axis).cross(getAxis(box2, 2, axis2)), diff, 14)) return false;
// Separating axis theorem returned positive overlap, generate contacts
if (false) {
// check box1's vertices against box2
for (var i = 0; i < 8; i++) {
box1.body.localToWorldPos(box1.getCorner(i, corner));
var contact = elation.physics.colliders.helperfuncs.vertex_box(corner, box2);
if (contact) {
contact.bodies = [box1, box2];
contacts.push(contact);
}
}
// check box2's vertices against box1
for (var i = 0; i < 8; i++) {
box1.body.localToWorldPos(box1.getCorner(i, corner));
var contact = elation.physics.colliders.helperfuncs.vertex_box(corner, box2);
if (contact) {
contact.bodies = [box1, box2];
contacts.push(contact);
}
}
// check box1's edges against box2's edges
/*
for (var i = 0; i < 12; i++) {
var edge = box1.getEdge(i);
var edge = box1.getEdge(i);
}
*/
//console.log(contacts);
return contacts;
} else {
var contact = false;
if (smallestIndex < 3) {
contact = fillPointFaceBoxBox(box1, box2, diff, smallestIndex, smallestPenetration);
} else if (smallestIndex < 6) {
contact = fillPointFaceBoxBox(box1, box2, diff.multiplyScalar(-1), smallestIndex - 3, smallestPenetration);
} else {
console.log('uh oh hard part', smallestIndex, smallestPenetration, box1, box2);
}
if (contact) {
contacts.push(contact);
}
return contacts;
}
}
}();
this.box_box = (function() {
// closure scratch variables - reused to avoid per-frame allocations
var box1Axes = [new THREE.Vector3(), new THREE.Vector3(), new THREE.Vector3()];
var box2Axes = [new THREE.Vector3(), new THREE.Vector3(), new THREE.Vector3()];
var crossAxis = new THREE.Vector3();
var diff = new THREE.Vector3();
var box1Center = new THREE.Vector3();
var box2Center = new THREE.Vector3();
var contactNormal = new THREE.Vector3();
var contactPoint = new THREE.Vector3();
var tmpVec = new THREE.Vector3();
var tmpVec2 = new THREE.Vector3();
var edge1Point = new THREE.Vector3();
var edge2Point = new THREE.Vector3();
var closestOnEdge1 = new THREE.Vector3();
var closestOnEdge2 = new THREE.Vector3();
var invQuat = new THREE.Quaternion();
// Reusable projection results to avoid allocation
var proj1 = { min: 0, max: 0 };
var proj2 = { min: 0, max: 0 };
// Get world-space axes for a box using orientationWorld for hierarchy support
// orientationWorld is local-to-world
function getWorldAxes(box, axes) {
axes[0].set(1, 0, 0).applyQuaternion(box.body.orientationWorld);
axes[1].set(0, 1, 0).applyQuaternion(box.body.orientationWorld);
axes[2].set(0, 0, 1).applyQuaternion(box.body.orientationWorld);
return axes;
}
// Get box center in world space
function getBoxCenter(box, out) {
// For standard physics bodies, position IS the center
// If there's an offset in box.min/max, account for it
out.addVectors(box.min, box.max).multiplyScalar(0.5);
if (out.lengthSq() > 0) {
// There's an offset - transform it to world space
// orientationWorld is local-to-world
out.applyQuaternion(box.body.orientationWorld);
}
out.add(box.body.positionWorld);
return out;
}
// Project box onto axis, storing result in outProj to avoid allocation
function projectBox(box, boxCenter, boxAxes, axis, outProj) {
var halfExtents = box.halfsize; // pre-scaled at collider creation time
var centerProj = boxCenter.dot(axis);
// Project each axis extent onto the test axis
var extent =
halfExtents.x * Math.abs(boxAxes[0].dot(axis)) +
halfExtents.y * Math.abs(boxAxes[1].dot(axis)) +
halfExtents.z * Math.abs(boxAxes[2].dot(axis));
outProj.min = centerProj - extent;
outProj.max = centerProj + extent;
}
// Test overlap on a single axis, return penetration or false if separating
function testAxis(box1, box2, box1Center, box2Center, box1Axes, box2Axes, axis) {
// Skip degenerate axes (near-zero length from parallel edges)
var lenSq = axis.lengthSq();
if (lenSq < 1e-8) return Infinity; // Treat as non-separating
// Normalize the axis
var invLen = 1 / Math.sqrt(lenSq);
axis.x *= invLen;
axis.y *= invLen;
axis.z *= invLen;
projectBox(box1, box1Center, box1Axes, axis, proj1);
projectBox(box2, box2Center, box2Axes, axis, proj2);
// Check for separation
if (proj1.max < proj2.min || proj2.max < proj1.min) {
return false; // Separating axis found
}
// Return overlap amount
return Math.min(proj1.max - proj2.min, proj2.max - proj1.min);
}
// Find closest points between two line segments
// Returns parameter t for point on segment 1: P1 + t * D1
function closestPointsOnSegments(p1, d1, halfLen1, p2, d2, halfLen2, outPoint1, outPoint2) {
// Direction from p1 to p2
tmpVec2.subVectors(p2, p1);
var d1d1 = d1.dot(d1);
var d2d2 = d2.dot(d2);
var d1d2 = d1.dot(d2);
var d1r = d1.dot(tmpVec2);
var d2r = d2.dot(tmpVec2);
var denom = d1d1 * d2d2 - d1d2 * d1d2;
var t, s;
if (Math.abs(denom) < 1e-8) {
// Parallel segments - use midpoint
t = 0;
s = d2r / d2d2;
} else {
t = (d1d2 * d2r - d2d2 * d1r) / denom;
s = (d1d1 * d2r - d1d2 * d1r) / denom;
}
// Clamp to segment bounds
t = Math.max(-halfLen1, Math.min(halfLen1, t));
s = Math.max(-halfLen2, Math.min(halfLen2, s));
// Compute closest points
outPoint1.copy(d1).multiplyScalar(t).add(p1);
outPoint2.copy(d2).multiplyScalar(s).add(p2);
}
// Get edge center and direction for a box edge
// edgeAxisIndex: which axis the edge runs along (0=X, 1=Y, 2=Z)
// signs: which quadrant of the perpendicular plane (-1 or +1 for each of the other two axes)
function getBoxEdge(boxCenter, boxAxes, halfsize, edgeAxisIndex, sign1, sign2, outPoint, outDir, outHalfLen) {
var ax1 = (edgeAxisIndex + 1) % 3;
var ax2 = (edgeAxisIndex + 2) % 3;
var hs = [halfsize.x, halfsize.y, halfsize.z];
outPoint.copy(boxCenter);
outPoint.addScaledVector(boxAxes[ax1], sign1 * hs[ax1]);
outPoint.addScaledVector(boxAxes[ax2], sign2 * hs[ax2]);
outDir.copy(boxAxes[edgeAxisIndex]);
return hs[edgeAxisIndex]; // half-length along edge
}
return function(box1, box2, contacts, dt) {
if (!contacts) contacts = [];
// Get box centers and axes in world space
getBoxCenter(box1, box1Center);
getBoxCenter(box2, box2Center);
getWorldAxes(box1, box1Axes);
getWorldAxes(box2, box2Axes);
// Vector from box1 center to box2 center
diff.subVectors(box2Center, box1Center);
var minPenetration = Infinity;
var minAxisIndex = -1;
var penetration;
// Test box1's 3 face axes
for (var i = 0; i < 3; i++) {
penetration = testAxis(box1, box2, box1Center, box2Center, box1Axes, box2Axes,
tmpVec.copy(box1Axes[i]));
if (penetration === false) return false;
if (penetration < minPenetration) {
minPenetration = penetration;
minAxisIndex = i;
contactNormal.copy(tmpVec);
}
}
// Test box2's 3 face axes
for (var i = 0; i < 3; i++) {
penetration = testAxis(box1, box2, box1Center, box2Center, box1Axes, box2Axes,
tmpVec.copy(box2Axes[i]));
if (penetration === false) return false;
if (penetration < minPenetration) {
minPenetration = penetration;
minAxisIndex = 3 + i;
contactNormal.copy(tmpVec);
}
}
// Track best face axis penetration for biasing
var bestFacePenetration = minPenetration;
// Test 9 edge-edge cross product axes
// Apply a small bias (5%) so face axes are preferred when penetrations are close.
// This prevents unstable edge-edge normals during shallow sliding contacts where
// the edge-edge and face penetrations are nearly equal.
for (var i = 0; i < 3; i++) {
for (var j = 0; j < 3; j++) {
crossAxis.crossVectors(box1Axes[i], box2Axes[j]);
penetration = testAxis(box1, box2, box1Center, box2Center, box1Axes, box2Axes,
tmpVec.copy(crossAxis));
if (penetration === false) return false;
// Edge-edge must beat face by at least 5% margin to be selected
if (penetration * 1.05 < minPenetration) {
minPenetration = penetration;
minAxisIndex = 6 + i * 3 + j;
contactNormal.copy(tmpVec);
}
}
}
// Ensure normal points from box1 to box2
if (contactNormal.dot(diff) < 0) {
contactNormal.negate();
}
// Calculate contact point based on collision type
if (minAxisIndex < 6) {
// Face-face or face-vertex contact (axes 0-5 are face normals)
// Contact point: project other box's center onto the contact face, clamped to face bounds
// Then offset by half penetration into the contact
// Get the face normal axis index (0-2 for the face box)
var faceAxisIndex, faceBox, faceCenter, faceAxes, faceHalfsize, otherCenter;
if (minAxisIndex < 3) {
// box1's face
faceAxisIndex = minAxisIndex;
faceBox = box1;
faceCenter = box1Center;
faceAxes = box1Axes;
faceHalfsize = box1.halfsize;
otherCenter = box2Center;
} else {
// box2's face
faceAxisIndex = minAxisIndex - 3;
faceBox = box2;
faceCenter = box2Center;
faceAxes = box2Axes;
faceHalfsize = box2.halfsize;
otherCenter = box1Center;
}
// The two tangent axis indices (perpendicular to face normal)
var tangent1 = (faceAxisIndex + 1) % 3;
var tangent2 = (faceAxisIndex + 2) % 3;
// Project other box's center onto the face plane
// First, get vector from face center to other center
tmpVec.subVectors(otherCenter, faceCenter);
// Project onto the face's tangent axes (not the normal) and clamp to face bounds
var hs = [faceHalfsize.x, faceHalfsize.y, faceHalfsize.z];
// Start at face center, offset by face normal to reach the face surface
var normalSign = tmpVec.dot(faceAxes[faceAxisIndex]) > 0 ? 1 : -1;
contactPoint.copy(faceCenter);
contactPoint.addScaledVector(faceAxes[faceAxisIndex], normalSign * hs[faceAxisIndex]);
// Project onto the two tangent axes and clamp
var proj1 = tmpVec.dot(faceAxes[tangent1]);
proj1 = Math.max(-hs[tangent1], Math.min(hs[tangent1], proj1));
contactPoint.addScaledVector(faceAxes[tangent1], proj1);
var proj2 = tmpVec.dot(faceAxes[tangent2]);
proj2 = Math.max(-hs[tangent2], Math.min(hs[tangent2], proj2));
contactPoint.addScaledVector(faceAxes[tangent2], proj2);
// Move contact point to midway through penetration (into the face)
contactPoint.addScaledVector(contactNormal, -minPenetration * 0.5);
} else {
// Edge-edge contact (axes 6-14 are cross products)
// Find the two edges and compute closest points
var edgeIndex = minAxisIndex - 6;
var edge1Axis = Math.floor(edgeIndex / 3); // 0, 1, or 2
var edge2Axis = edgeIndex % 3; // 0, 1, or 2
// Determine which of the 4 parallel edges on each box to use
// Pick the edges closest to each other
var hs1 = [box1.halfsize.x, box1.halfsize.y, box1.halfsize.z];
var hs2 = [box2.halfsize.x, box2.halfsize.y, box2.halfsize.z];
// For edge on box1 along axis edge1Axis, the other two axes determine position
var ax1_1 = (edge1Axis + 1) % 3;
var ax1_2 = (edge1Axis + 2) % 3;
var sign1_1 = diff.dot(box1Axes[ax1_1]) > 0 ? 1 : -1;
var sign1_2 = diff.dot(box1Axes[ax1_2]) > 0 ? 1 : -1;
// For edge on box2 along axis edge2Axis
var ax2_1 = (edge2Axis + 1) % 3;
var ax2_2 = (edge2Axis + 2) % 3;
var sign2_1 = diff.dot(box2Axes[ax2_1]) < 0 ? 1 : -1;
var sign2_2 = diff.dot(box2Axes[ax2_2]) < 0 ? 1 : -1;
// Get edge 1 (on box1)
edge1Point.copy(box1Center);
edge1Point.addScaledVector(box1Axes[ax1_1], sign1_1 * hs1[ax1_1]);
edge1Point.addScaledVector(box1Axes[ax1_2], sign1_2 * hs1[ax1_2]);
var halfLen1 = hs1[edge1Axis];
// Get edge 2 (on box2)
edge2Point.copy(box2Center);
edge2Point.addScaledVector(box2Axes[ax2_1], sign2_1 * hs2[ax2_1]);
edge2Point.addScaledVector(box2Axes[ax2_2], sign2_2 * hs2[ax2_2]);
var halfLen2 = hs2[edge2Axis];
// Find closest points on the two edges
closestPointsOnSegments(
edge1Point, box1Axes[edge1Axis], halfLen1,
edge2Point, box2Axes[edge2Axis], halfLen2,
closestOnEdge1, closestOnEdge2
);
// Contact point is midway between the two closest points
contactPoint.addVectors(closestOnEdge1, closestOnEdge2).multiplyScalar(0.5);
}
var contact = new elation.physics.contact({
point: contactPoint.clone(),
normal: contactNormal.clone(),
penetration: -minPenetration,
bodies: [box1.body, box2.body]
});
contacts.push(contact);
return contacts;
};
})();
/**
* Box-Triangle collision using SAT (Separating Axis Theorem)
* Tests 13 axes: 1 triangle normal + 3 box faces + 9 edge cross products
*/
this.box_triangle = (function() {
// Closure scratch variables
var boxAxes = [new THREE.Vector3(), new THREE.Vector3(), new THREE.Vector3()];
var triEdges = [new THREE.Vector3(), new THREE.Vector3(), new THREE.Vector3()];
var triNormal = new THREE.Vector3();
var boxCenter = new THREE.Vector3();
var triCenter = new THREE.Vector3();
var diff = new THREE.Vector3();
var contactNormal = new THREE.Vector3();
var invQuat = new THREE.Quaternion();
var contactPoint = new THREE.Vector3();
var tmpVec = new THREE.Vector3();
var tmpVec2 = new THREE.Vector3();
var crossAxis = new THREE.Vector3();
var triVerts = [new THREE.Vector3(), new THREE.Vector3(), new THREE.Vector3()];
// Project box onto axis
function projectBox(boxCenter, boxAxes, halfsize, axis, out) {
var centerProj = boxCenter.dot(axis);
var extent =
halfsize.x * Math.abs(boxAxes[0].dot(axis)) +
halfsize.y * Math.abs(boxAxes[1].dot(axis)) +
halfsize.z * Math.abs(boxAxes[2].dot(axis));
out.min = centerProj - extent;
out.max = centerProj + extent;
}
// Project triangle onto axis
function projectTriangle(v0, v1, v2, axis, out) {
var p0 = v0.dot(axis);
var p1 = v1.dot(axis);
var p2 = v2.dot(axis);
out.min = Math.min(p0, p1, p2);
out.max = Math.max(p0, p1, p2);
}
// Test if projections overlap, return overlap amount or false
function testOverlap(proj1, proj2) {
if (proj1.max < proj2.min || proj2.max < proj1.min) {
return false; // Separating axis found
}
return Math.min(proj1.max - proj2.min, proj2.max - proj1.min);
}
var proj1 = { min: 0, max: 0 };
var proj2 = { min: 0, max: 0 };
return function(box, triangle, contacts, dt) {
if (!contacts) contacts = [];
// Get triangle world points
var worldpoints = triangle.getWorldPoints();
triVerts[0].copy(worldpoints.p1);
triVerts[1].copy(worldpoints.p2);
triVerts[2].copy(worldpoints.p3);
// Triangle edges
triEdges[0].subVectors(triVerts[1], triVerts[0]);
triEdges[1].subVectors(triVerts[2], triVerts[1]);
triEdges[2].subVectors(triVerts[0], triVerts[2]);
// Triangle normal (from cached world normal)
triNormal.copy(worldpoints.normal);
// Triangle center
triCenter.copy(triVerts[0]).add(triVerts[1]).add(triVerts[2]).divideScalar(3);
// For double-sided triangles, flip normal if box is on the back side
// This ensures we collide correctly from either side without duplicating triangles
if (triangle.doubleSided) {
// Check which side of triangle plane the box center is on
// boxCenter isn't computed yet, so compute it temporarily
tmpVec.addVectors(box.min, box.max).multiplyScalar(0.5);
if (tmpVec.lengthSq() > 0) {
tmpVec.applyQuaternion(box.body.orientationWorld);
}
tmpVec.add(box.body.positionWorld);
// If box is on back side (negative side of plane), flip normal
var sideCheck = tmpVec.clone().sub(triVerts[0]).dot(triNormal);
if (sideCheck < 0) {
triNormal.negate();
}
}
// Get box center and axes in world space
// Box center from min/max (handles offset colliders)
// orientationWorld is local-to-world
boxCenter.addVectors(box.min, box.max).multiplyScalar(0.5);
if (boxCenter.lengthSq() > 0) {
boxCenter.applyQuaternion(box.body.orientationWorld);
}
boxCenter.add(box.body.positionWorld);
// Box world axes
boxAxes[0].set(1, 0, 0).applyQuaternion(box.body.orientationWorld);
boxAxes[1].set(0, 1, 0).applyQuaternion(box.body.orientationWorld);
boxAxes[2].set(0, 0, 1).applyQuaternion(box.body.orientationWorld);
// Vector from box center to triangle center
diff.subVectors(triCenter, boxCenter);
var minPenetration = Infinity;
var minAxisType = -1; // 0=triNormal, 1-3=boxAxes, 4-12=edge cross
var minAxisIndex = -1;
var penetration;
var faceNormalPenetration = false; // Track face normal overlap separately
// Test 1: Triangle normal
tmpVec.copy(triNormal);
if (tmpVec.lengthSq() > 1e-8) {
tmpVec.normalize();
projectBox(boxCenter, boxAxes, box.halfsize, tmpVec, proj1);
projectTriangle(triVerts[0], triVerts[1], triVerts[2], tmpVec, proj2);
penetration = testOverlap(proj1, proj2);
if (penetration === false) return false;
faceNormalPenetration = penetration;
if (penetration < minPenetration) {
minPenetration = penetration;
minAxisType = 0;
contactNormal.copy(tmpVec);
}
}
// Test 2-4: Box face normals
for (var i = 0; i < 3; i++) {
tmpVec.copy(boxAxes[i]);
projectBox(boxCenter, boxAxes, box.halfsize, tmpVec, proj1);
projectTriangle(triVerts[0], triVerts[1], triVerts[2], tmpVec, proj2);
penetration = testOverlap(proj1, proj2);
if (penetration === false) return false;
if (penetration < minPenetration) {
minPenetration = penetration;
minAxisType = 1;
minAxisIndex = i;
contactNormal.copy(tmpVec);
}
}
// Test 5-13: Cross products of triangle edges with box edges
for (var i = 0; i < 3; i++) {
for (var j = 0; j < 3; j++) {
crossAxis.crossVectors(triEdges[i], boxAxes[j]);
var lenSq = crossAxis.lengthSq();
if (lenSq < 1e-8) continue; // Skip degenerate (parallel) axes
tmpVec.copy(crossAxis).normalize();
projectBox(boxCenter, boxAxes, box.halfsize, tmpVec, proj1);
projectTriangle(triVerts[0], triVerts[1], triVerts[2], tmpVec, proj2);
penetration = testOverlap(proj1, proj2);
if (penetration === false) return false;
if (penetration < minPenetration) {
minPenetration = penetration;
minAxisType = 2;
minAxisIndex = i * 3 + j;
contactNormal.copy(tmpVec);
}
}
}
// Always use the triangle face normal for contact generation.
// The SAT minimum penetration axis is unstable for deep interpenetration:
// as overlap changes between frames, the minimum axis switches, causing
// the contact normal to flip wildly. The triangle face normal is geometrically
// stable and correct for mesh collisions (which are the primary use case).
// SAT still tests all 13 axes for separation detection (early-out above).
if (faceNormalPenetration !== false) {
contactNormal.copy(triNormal).normalize();
minAxisType = 0;
}
// Ensure normal points from box toward triangle
if (contactNormal.dot(diff) < 0) {
contactNormal.negate();
}
// Calculate contact point(s)
if (minAxisType === 0) {
// Triangle normal was minimum - box vertex vs triangle face contact
// Generate contact for EACH penetrating vertex (like Cyclone's boxAndHalfSpace)
var hs = [box.halfsize.x, box.halfsize.y, box.halfsize.z];
// First pass: collect all penetrating vertices inside triangle
var penetratingVertices = [];
var maxDepth = 0;
for (var vx = -1; vx <= 1; vx += 2) {
for (var vy = -1; vy <= 1; vy += 2) {
for (var vz = -1; vz <= 1; vz += 2) {
tmpVec.copy(boxCenter);
tmpVec.addScaledVector(boxAxes[0], vx * hs[0]);
tmpVec.addScaledVector(boxAxes[1], vy * hs[1]);
tmpVec.addScaledVector(boxAxes[2], vz * hs[2]);
// Calculate depth: how far the vertex is past the triangle plane toward the triangle
// Use contactNormal (points from box toward triangle) for correct sign
// signedDist > 0 means vertex is on the triangle side of the plane (penetrating)
var signedDist = tmpVec.clone().sub(triVerts[0]).dot(contactNormal);
var depth = signedDist; // Positive when penetrating (vertex past plane toward triangle)
if (depth > 0) {
// Project vertex onto triangle plane (move back along contactNormal)
contactPoint.copy(tmpVec);
contactPoint.addScaledVector(contactNormal, -depth);
// Only count if projected point is inside triangle bounds
if (elation.physics.colliders.helperfuncs.point_in_triangle(contactPoint, triVerts[0], triVerts[1], triVerts[2])) {
penetratingVertices.push({
vertex: tmpVec.clone(),
depth: depth
});
if (depth > maxDepth) maxDepth = depth;
}
}
}
}
}
// Second pass: create contacts with their actual penetration depths
var numContacts = penetratingVertices.length;
if (numContacts > 0) {
for (var i = 0; i < numContacts; i++) {
var v = penetratingVertices[i];
// Contact point: vertex offset by half its depth toward the plane
contactPoint.copy(v.vertex);
contactPoint.addScaledVector(triNormal, v.depth * 0.5);
var contact = new elation.physics.contact({
point: contactPoint.clone(),
normal: contactNormal.clone(),
penetration: -v.depth, // Each contact has its own penetration
bodies: [box.body, triangle.body],
triangle: triangle
});
contacts.push(contact);
}
}
// Return early - we've already added all contacts
return contacts.length > 0 ? contacts : false;
} else if (minAxisType === 1) {
// Box face normal was minimum - contact is on box face
// Project triangle center onto box face, clamped to face bounds
var faceAxisIndex = minAxisIndex;
var tangent1 = (faceAxisIndex + 1) % 3;
var tangent2 = (faceAxisIndex + 2) % 3;
var hs = [box.halfsize.x, box.halfsize.y, box.halfsize.z];
// Start at box face
var normalSign = diff.dot(boxAxes[faceAxisIndex]) > 0 ? 1 : -1;
contactPoint.copy(boxCenter);
contactPoint.addScaledVector(boxAxes[faceAxisIndex], normalSign * hs[faceAxisIndex]);
// Project triangle center onto face tangent axes
tmpVec.subVectors(triCenter, boxCenter);
var proj1Val = tmpVec.dot(boxAxes[tangent1]);
proj1Val = Math.max(-hs[tangent1], Math.min(hs[tangent1], proj1Val));
contactPoint.addScaledVector(boxAxes[tangent1], proj1Val);
var proj2Val = tmpVec.dot(boxAxes[tangent2]);
proj2Val = Math.max(-hs[tangent2], Math.min(hs[tangent2], proj2Val));
contactPoint.addScaledVector(boxAxes[tangent2], proj2Val);
// Move into contact by half penetration
contactPoint.addScaledVector(contactNormal, -minPenetration * 0.5);
} else {
// Edge-edge contact
var triEdgeIdx = Math.floor(minAxisIndex / 3);
var boxEdgeIdx = minAxisIndex % 3;
// Get triangle edge endpoints
var triEdgeStart = triVerts[triEdgeIdx];
var triEdgeEnd = triVerts[(triEdgeIdx + 1) % 3];
// Get box edge - find the edge closest to the triangle
var hs = [box.halfsize.x, box.halfsize.y, box.halfsize.z];
var ax1 = (boxEdgeIdx + 1) % 3;
var ax2 = (boxEdgeIdx + 2) % 3;
// Pick signs based on direction to triangle
var sign1 = diff.dot(boxAxes[ax1]) > 0 ? 1 : -1;
var sign2 = diff.dot(boxAxes[ax2]) > 0 ? 1 : -1;
// Box edge center and half-length
tmpVec.copy(boxCenter);
tmpVec.addScaledVector(boxAxes[ax1], sign1 * hs[ax1]);
tmpVec.addScaledVector(boxAxes[ax2], sign2 * hs[ax2]);
var boxEdgeHalfLen = hs[boxEdgeIdx];
// Find closest points on the two edges
// Use line-line closest point calculation
var d1 = boxAxes[boxEdgeIdx];
var d2 = triEdges[triEdgeIdx].clone().normalize();
var triEdgeLen = triEdges[triEdgeIdx].length();
tmpVec2.subVectors(triEdgeStart, tmpVec);
var d1d1 = 1; // d1 is unit length
var d2d2 = 1; // d2 is normalized
var d1d2 = d1.dot(d2);
var d1r = d1.dot(tmpVec2);
var d2r = d2.dot(tmpVec2);
var denom = d1d1 * d2d2 - d1d2 * d1d2;
var t, s;
if (Math.abs(denom) < 1e-8) {
t = 0;
s = 0;
} else {
t = (d1d2 * d2r - d2d2 * d1r) / denom;
s = (d1d1 * d2r - d1d2 * d1r) / denom;
}
// Clamp to edge bounds
t = Math.max(-boxEdgeHalfLen, Math.min(boxEdgeHalfLen, t));
s = Math.max(0, Math.min(triEdgeLen, s));
// Compute contact point as midpoint
var boxEdgePoint = tmpVec.clone().addScaledVector(d1, t);
var triEdgePoint = triEdgeStart.clone().addScaledVector(d2, s);
contactPoint.addVectors(boxEdgePoint, triEdgePoint).multiplyScalar(0.5);
}
var contact = new elation.physics.contact({
point: contactPoint.clone(),
normal: contactNormal.clone(),
penetration: -minPenetration,
bodies: [box.body, triangle.body],
triangle: triangle
});
contacts.push(contact);
return contacts;
};
})();
/**
* Triangle-Box collision (reverses box-triangle)
*/
this.triangle_box = function(triangle, box, contacts, dt) {
return this.box_triangle(box, triangle, contacts, dt);
};
/**
* Mesh-Box collision - iterates over mesh triangles
*/
this.mesh_box = (function() {
var boxBoundingSphere = { radius: 0 };
var sphereContacts = [];
return function(mesh, box, contacts, dt) {
if (!contacts) contacts = [];
// Lazy triangle extraction - if mesh has geometry but no triangles, try extracting now
if (mesh.triangles.length === 0 && ((mesh.mesh && mesh.mesh.geometry) || mesh.modeldata)) {
mesh.triangles = mesh.extractTriangles(mesh.mesh);
}
// Broad phase: check box against mesh bounding sphere
// Compute box bounding sphere radius
var boxDiag = Math.sqrt(
box.halfsize.x * box.halfsize.x +
box.halfsize.y * box.halfsize.y +
box.halfsize.z * box.halfsize.z
);
// Quick bounding sphere check
var boxCenter = box.body.positionWorld;
var meshCenter = mesh.body.positionWorld;
var centerDist = boxCenter.distanceTo(meshCenter);
// Compute scaled bounding radius dynamically from current scaleWorld
var meshScale = mesh.body.scaleWorld;
var scaledMeshRadius = (mesh.localRadius || mesh.radius) * Math.max(meshScale.x, meshScale.y, meshScale.z);
var maxDist = boxDiag + scaledMeshRadius;
if (centerDist > maxDist) {
return false; // Too far apart
}
// Narrow phase: test box against each triangle
var localcontacts = [];
var boxMaxDistSq = Math.pow(boxDiag + box.body.velocity.length(), 2);
for (var i = 0; i < mesh.triangles.length; i++) {
var triangle = mesh.triangles[i];
var worldpoints = triangle.getWorldPoints();
// Quick distance check to triangle center
var distToCenter = worldpoints.center.distanceToSquared(boxCenter);
var triRadius = worldpoints.radius || 0;
if (distToCenter <= boxMaxDistSq + triRadius * triRadius + boxDiag * boxDiag) {
elation.physics.colliders.helperfuncs.box_triangle(box, triangle, localcontacts, dt);
}
}
if (localcontacts.length > 0) {
var meshRoot = mesh.getRoot();
if (localcontacts.length > 1) {
// Step A: Deduplicate contacts at the same point.
// A single box vertex can touch multiple adjacent triangles on the same
// mesh face (each quad = 2 triangles). These are redundant — keep one
// contact per distinct box corner, with the shallowest penetration.
var pointTol = 0.01; // contacts within 1cm are from the same vertex
var pointTolSq = pointTol * pointTol;
var deduped = [localcontacts[0]];
for (var i = 1; i < localcontacts.length; i++) {
var merged = false;
for (var j = 0; j < deduped.length; j++) {
if (localcontacts[i].point.distanceToSquared(deduped