cyclone-physics
Version:
Pure Javascript physics engine based on http://procyclone.com/
1,181 lines (1,032 loc) • 112 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 <= r) {
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
centerWorld = new THREE.Vector3(), // center of sphere, world-space coordinates
diff = new THREE.Vector3(),
closest = new THREE.Vector3(),
scale = new THREE.Vector3(),
scaledmin = new THREE.Vector3(),
scaledmax = new THREE.Vector3();
return function(box, sphere, contacts, dt) {
if (!contacts) contacts = [];
// Get sphere position in world and in the box's coordinate space
if (sphere.offset) {
sphere.body.localToWorldPos(centerWorld.copy(sphere.offset));
} else {
sphere.body.localToWorldPos(centerWorld.set(0,0,0));
}
box.body.worldToLocalPos(center.copy(centerWorld));
box.body.worldToLocalScale(scale.set(1,1,1));
scaledmin.copy(scale).multiply(box.min);
scaledmax.copy(scale).multiply(box.max);
// Early out if any of the axes are separating
if ((center.x + sphere.radius < scaledmin.x || center.x - sphere.radius > scaledmax.x) ||
(center.y + sphere.radius < scaledmin.y || center.y - sphere.radius > scaledmax.y) ||
(center.z + sphere.radius < scaledmin.z || center.z - sphere.radius > scaledmax.z)) {
return false;
}
// Find closest point on box
closest.x = elation.utils.math.clamp(center.x, scaledmin.x, scaledmax.x);
closest.y = elation.utils.math.clamp(center.y, scaledmin.y, scaledmax.y);
closest.z = elation.utils.math.clamp(center.z, scaledmin.z, scaledmax.z);
// See if we're in contact
diff.subVectors(closest, center);
var dist = diff.lengthSq();
if (dist > sphere.radius * sphere.radius) {
return 0;
}
//console.log('BOING', closest.toArray(), center.toArray(), diff.toArray(), dist, sphere.radius);
// Transform back to world space
box.body.localToWorldPos(closest);
var contact = new elation.physics.contact({
point: closest.clone(), // allocate point
normal: centerWorld.clone().sub(closest).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();
elation.physics.colliders.helperfuncs.closest_point_on_line(capsuleDims.start, capsuleDims.end, vertex, closest);
let distSq = closest.distanceToSquared(vertex);
if (distSq <= capsule.radius * capsule.radius) {
let dist = Math.sqrt(distSq);
let normal = closest.clone().sub(vertex).divideScalar(dist);
let point = closest.clone();
point.x += normal.x * capsule.radius;
point.y += normal.y * capsule.radius;
point.z += normal.z * capsule.radius;
let contact = new elation.physics.contact({
normal: normal,
point: point,
penetration: dist - capsule.radius,
});
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
const scratch = {
axes: Array(15).fill(null).map(() => new THREE.Vector3()),
box1Corners: Array(8).fill(null).map(() => new THREE.Vector3()),
box2Corners: Array(8).fill(null).map(() => new THREE.Vector3()),
box1Projection: { min: 0, max: 0 },
box2Projection: { min: 0, max: 0 },
tmpVec: new THREE.Vector3(),
tmpQuat: new THREE.Quaternion(),
contactAxis: new THREE.Vector3(),
};
function getAxesToTest(box1, box2, scratchAxes) {
const axes = scratchAxes;
// Get the three local axes of both boxes in world coordinates
const box1Axes = getWorldAxes(box1, axes.slice(0, 3));
const box2Axes = getWorldAxes(box2, axes.slice(3, 6));
// Add cross products of edges (9 cross-product axes)
let axesIndex = 6;
for (let i = 0; i < 3; i++) {
for (let j = 0; j < 3; j++) {
const cross = axes[axesIndex].crossVectors(box1Axes[i], box2Axes[j]);
if (cross.lengthSq() > 1e-6) { // Avoid zero vectors
cross.normalize();
}
axesIndex++;
}
}
return axes;
}
function getWorldAxes(box, axes) {
// The orientationWorld is already in world space
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;
}
function getPenetrationOnAxis(box1, box2, axis) {
// Project both boxes onto the axis
const box1Projection = projectBoxOntoAxis(box1, axis, scratch.box1Corners, scratch.box1Projection);
const box2Projection = projectBoxOntoAxis(box2, axis, scratch.box2Corners, scratch.box2Projection);
// Check if projections overlap
const overlap = Math.min(box1Projection.max - box2Projection.min, box2Projection.max - box1Projection.min);
if (box1Projection.max < box2Projection.min || box2Projection.max < box1Projection.min) {
return false; // Separating axis found
}
return overlap; // Return penetration depth on this axis
}
function projectBoxOntoAxis(box, axis, corners, projection) {
getBoxCorners(box, corners);
projection.min = Infinity;
projection.max = -Infinity;
for (let corner of corners) {
const proj = corner.dot(axis);
projection.min = Math.min(projection.min, proj);
projection.max = Math.max(projection.max, proj);
}
return projection;
}
function getBoxCorners(box, corners) {
const halfSize = box.halfsize;
const offset = box.offset;
const scale = box.body.localToWorldScale(scratch.tmpVec.set(1,1,1));
// Define the eight corners of the box in local space
corners[0].set(halfSize.x, halfSize.y, halfSize.z).add(offset).divide(scale);
corners[1].set(halfSize.x, halfSize.y, -halfSize.z).add(offset).divide(scale);
corners[2].set(halfSize.x, -halfSize.y, halfSize.z).add(offset).divide(scale);
corners[3].set(halfSize.x, -halfSize.y, -halfSize.z).add(offset).divide(scale);
corners[4].set(-halfSize.x, halfSize.y, halfSize.z).add(offset).divide(scale);
corners[5].set(-halfSize.x, halfSize.y, -halfSize.z).add(offset).divide(scale);
corners[6].set(-halfSize.x, -halfSize.y, halfSize.z).add(offset).divide(scale);
corners[7].set(-halfSize.x, -halfSize.y, -halfSize.z).add(offset).divide(scale);
// Convert local corners to world space using localToWorld
for (let i = 0; i < 8; i++) {
box.body.localToWorldPos(corners[i]);
}
return corners;
}
function generateContacts(box1, box2, minPenetration, contactAxis, contacts) {
// The contact normal is the axis of minimum penetration
const contactNormal = contactAxis.clone().normalize();
// Find the closest points on the surface of both boxes
const box1Corners = getBoxCorners(box1, scratch.box1Corners);
const box2Corners = getBoxCorners(box2, scratch.box2Corners);
//const box1Closest = findClosestPointOnBox(box1Corners, contactNormal);
//const box2Closest = findClosestPointOnBox(box2Corners, contactNormal.clone().negate());
const box1Closest = findClosestPointOnFace(box1Corners, box2, contactNormal);
const box2Closest = findClosestPointOnFace(box2Corners, box1, contactNormal);
// Contact point is the midpoint between closest points on both boxes
const contactPoint = box1Closest.clone().add(box2Closest).multiplyScalar(0.5);
// Create the contact
const contact = new elation.physics.contact({
normal: contactNormal,
point: contactPoint.clone(),
penetration: 0, //-minPenetration, // negative value as it's penetration
//penetration: box1Closest.distanceTo(box2Closest),
bodies: [box1.body, box2.body]
});
contacts.push(contact);
return contacts;
}
function findClosestPointOnBox(corners, normal) {
let closestPoint = corners[0];
let minDistance = closestPoint.dot(normal);
for (let i = 1; i < corners.length; i++) {
const dist = corners[i].dot(normal);
if (dist < minDistance) {
closestPoint = corners[i];
minDistance = dist;
}
}
return closestPoint;
}
function findClosestPointOnFace(corners, otherBox, normal) {
let closestPoint = null;
let minDistance = Infinity;
// Check the projection of each corner onto the face of the other box
for (let i = 0; i < corners.length; i++) {
const corner = corners[i];
const projectedPoint = projectPointOntoFace(corner, otherBox, normal);
const distance = corner.distanceTo(projectedPoint);
if (distance < minDistance) {
closestPoint = projectedPoint;
minDistance = distance;
}
}
return closestPoint;
}
function projectPointOntoFace(point, box, normal) {
// First, find the center of the box in world space
const boxCenter = new THREE.Vector3();
box.body.localToWorldPos(boxCenter.set(0, 0, 0));
// Determine the plane of the face by using the box's normal and a point on the plane
// (the center of the face, which is aligned with one of the box's axes)
const halfSize = box.halfsize;
const offset = new THREE.Vector3().addVectors(box.min, box.max).multiplyScalar(.5);
const planePoint = new THREE.Vector3();
const planeNormal = normal.clone().normalize(); // Normal of the face (aligned with one of the box's axes)
// We need to determine which face we're projecting onto, so find the direction along the normal
// Set the point on the plane (face center) by adding/subtracting half the box size along the normal
for (let i = 0; i < 3; i++) {
const axisValue = planeNormal.getComponent(i);
if (axisValue !== 0) {
planePoint.setComponent(i, boxCenter.getComponent(i) + (axisValue * halfSize.getComponent(i)));
}
}
// Now, we project the point onto the plane
// To do that, find the vector from the point to the plane point
const pointToPlane = point.clone().sub(planePoint);
// Project this vector onto the normal to find how far away the point is from the plane
const distance = pointToPlane.dot(planeNormal);
// Move the point onto the plane by subtracting the distance along the normal
const projectedPoint = point.clone().sub(planeNormal.multiplyScalar(distance));
// Now we need to clamp the projected point to the bounds of the box face
// For each axis, clamp the value within the bounds of the face
for (let i = 0; i < 3; i++) {
const axisValue = planeNormal.getComponent(i);
if (axisValue === 0) {
// Clamp the coordinate to be within the face bounds (which are defined by the box size)
const minVal = boxCenter.getComponent(i) - halfSize.getComponent(i);
const maxVal = boxCenter.getComponent(i) + halfSize.getComponent(i);
const value = projectedPoint.getComponent(i);
projectedPoint.setComponent(i, Math.max(minVal, Math.min(value, maxVal)));
}
}
return projectedPoint.add(offset);
}
return function(box1, box2, contacts, dt) {
const axes = getAxesToTest(box1, box2, scratch.axes);
let hasCollision = true;
let minPenetration = Infinity;
// Check for overlap along each axis
for (let axis of axes) {
const overlap = getPenetrationOnAxis(box1, box2, axis);
if (overlap === false) {
hasCollision = false; // Separating axis found, no collision
break;
} else if (overlap < minPenetration) {
minPenetration = overlap; // Track the minimum penetration
scratch.contactAxis.copy(axis); // Store the axis with minimum penetration
}
}
if (!hasCollision) {
return false;
}
// If colliding, generate contact points and return them
return generateContacts(box1, box2, minPenetration, scratch.contactAxis, contacts);
}
})();
/* cylinder helpers */
this.cylinder_sphere = function() {
// closure scratch variables
var spherepos = new THREE.Vector3();
var up = new THREE.Vector3();
var capline = new THREE.Vector3();
return function(cylinder, sphere, contacts, dt) {
if (!contacts) contacts = [];
// Transform sphere position into cylinder's coordinate space
// TODO - account for offset
cylinder.body.worldToLocalPos(sphere.body.localToWorldPos(spherepos.set(0,0,0)));
var halfh = cylinder.height / 2,
rCylinder = cylinder.radius;
rSphere = sphere.radius;
//var type = 'none';
if (spherepos.y + rSphere - cylinder.offset.y < -halfh || spherepos.y - rSphere - cylinder.offset.y > halfh) {
// far enough above that we definitely won't hit
return false;
}
var lsq = spherepos.x * spherepos.x + spherepos.z * spherepos.z;
var rTotal = rSphere + rCylinder;
if (lsq > rTotal * rTotal) {
// Outside of cylinder radius
return false;
}
var contact = false;
if (spherepos.y - cylinder.offset.y > -halfh && spherepos.y - cylinder.offset.y < halfh) {
// Colliding with side of cylinder (center of sphere is between cylinder ends)
var penetration = (Math.sqrt(lsq) - rSphere - rCylinder) / 2;
var normal = spherepos.clone(); // allocate normal
normal.y = 0;
normal.normalize();
var point = normal.clone().multiplyScalar(rCylinder + penetration); // allocate point
point.y = spherepos.y;
contact = new elation.physics.contact({
normal: cylinder.body.localToWorldDir(normal).normalize(),
point: cylinder.body.localToWorldPos(point),
penetration: penetration,
bodies: [cylinder.body, sphere.body]
});
contacts.push(contact);
//type = 'side';
} else {
// Colliding with end caps of cylinder
up.set(0,1,0);
spherepos.sub(cylinder.offset);
capline.crossVectors(up, spherepos).cross(up).normalize();
var d = spherepos.dot(capline);
var sign = (Math.abs(spherepos.y) / spherepos.y);
if (d < cylinder.radius) {
//type = 'endcap';
// straight-on collision with end cap
var point = capline.clone().multiplyScalar(d); // allocate point
point.y = sign * cylinder.height / 2;
var penetration = spherepos.distanceTo(point) - sphere.radius;
contact = new elation.physics.contact({
normal: cylinder.body.localToWorldDir(up.clone().multiplyScalar(sign)).normalize(), // allocate normal
point: cylinder.body.localToWorldPos(point).add(cylinder.offset),
penetration: penetration,
bodies: [cylinder.body, sphere.body]
});
contacts.push(contact);
} else {
//type = 'edge';
capline.multiplyScalar(cylinder.radius);
capline.y = sign * cylinder.height / 2;
var normal = new THREE.Vector3().subVectors(capline, spherepos); // allocate normal
var penetration = sphere.radius - normal.length();
normal.divideScalar(-penetration);
contact = new elation.physics.contact({
normal: cylinder.body.localToWorldDir(normal).normalize().negate(),
point: cylinder.body.localToWorldPos(capline.clone()).add(cylinder.offset), // allocate point
penetration: penetration,
bodies: [cylinder.body, sphere.body]
});
contacts.push(contact);
}
//console.log(d, spherepos.toArray(), capline.toArray());
}
//console.log(type, contact.penetration, contact);
return contacts;
}
}();
this.sphere_cylinder = function(sphere, cylinder, contacts, dt) {
return this.cylinder_sphere(cylinder, sphere, contacts, dt);
}
this.sphere_triangle = function(sphere, triangle, contacts, dt) {
return this.triangle_sphere(triangle, sphere, contacts, dt);
}
this.cylinder_box = function(cylinder, box, contacts, dt) {
//return this.cylinder_sphere(cylinder, sphere, contacts);
}
this.cylinder_cylinder = function(cylinder, box, contacts, dt) {
//return this.cylinder_sphere(cylinder, sphere, contacts);
}
this.cylinder_plane = function() {
var up = new THREE.Vector3();
var planenorm = new THREE.Vector3();
var dir = new THREE.Vector3();
var centerpoint = new THREE.Vector3();
var point = new THREE.Vector3();
var tolerance = 1e-6;
var checkPoint = function(point, cylinder, plane, contacts) {
var contact = elation.physics.colliders.helperfuncs.vertex_plane(point, plane);
if (contact) {
contact.bodies = [cylinder.body, plane.body];
contacts.push(contact);
}
}
var checkEndPoints = function(centerpoint, offset, cylinder, plane, contacts) {
point.addVectors(centerpoint, offset);
checkPoint(point, cylinder, plane, contacts);
point.subVectors(centerpoint, offset);
checkPoint(point, cylinder, plane, contacts);
}
return function(cylinder, plane, contacts, dt) {
if (!contacts) contacts = [];
cylinder.body.localToWorldDir(up.set(0,1,0));
plane.body.localToWorldDir(planenorm.copy(plane.normal));
var dot = planenorm.dot(up);
dir.crossVectors(planenorm, up).cross(up).normalize().multiplyScalar(cylinder.radius);
// TODO - handle cases where cylinder is parallel or perpendicular to plane
/*
if (Math.abs(dot) <= tolerance) { // parallel to plane - generate two contacts, one at each end
console.log('parallel!');
} else if (Math.abs(Math.abs(dot) - 1) <= tolerance) { // perpendicular to plane - generate three contacts at 120 degree increments
console.log('perpendicular!');
} else {
*/
// top point
cylinder.body.localToWorldPos(centerpoint.set(0,cylinder.height/2, 0));
checkEndPoints(centerpoint, dir, cylinder, plane, contacts);
// bottom point
cylinder.body.localToWorldPos(centerpoint.set(0,-cylinder.height/2, 0));
checkEndPoints(centerpoint, dir, cylinder, plane, contacts);
//console.log('up:', up.toArray(), 'planenorm:', planenorm.toArray(), 'top:', top.toArray(), 'bottom:', centerpoint.toArray(), 'dir:', dir.toArray());
// }
return contacts;
}
}();
/* capsule helpers */
this.capsule_sphere = function() {
// closure scratch variables
const spherepos = new THREE.Vector3(),
point = new THREE.Vector3(),
normal = new THREE.Vector3(),
closest = new THREE.Vector3();
return function(capsule, sphere, contacts, dt) {
const capsuleScaledRadius = capsule.radius * Math.max(capsule.body.scale.x, capsule.body.scale.z),
combinedRadius = capsuleScaledRadius + sphere.radius,
capsuleDims = capsule.getDimensions();
sphere.body.localToWorldPos(point.set(0,0,0));
elation.physics.colliders.helperfuncs.closest_point_on_line(capsuleDims.start, capsuleDims.end, point, closest);
normal.subVectors(closest, point);
const distance = normal.length();
if (distance <= combinedRadius) {
normal.divideScalar(distance);
let contact = new elation.physics.contact({
normal: normal.clone(), // allocate normal
point: closest.clone().add(normal.multiplyScalar(capsuleScaledRadius)), // allocate point
penetration: combinedRadius - distance,
bodies: [capsule.body, sphere.body]
});
contacts.push(contact);
}
return contacts;
}
}();
this.capsule_capsule = function() {
// closure scratch variables
const p1 = new THREE.Vector3(),
p2 = new THREE.Vector3();
return function(capsule1, capsule2, contacts, dt) {
const capsule1Dims = capsule1.getDimensions(),
capsule2Dims = capsule2.getDimensions();
let distSquared = elation.physics.colliders.helperfuncs.distancesquared_between_lines(capsule1Dims.start, capsule1Dims.end, capsule2Dims.start, capsule2Dims.end, p1, p2);
const capsule1ScaledRadius = capsule1.radius * Math.max(capsule1.body.scale.x, capsule1.body.scale.z),
capsule2ScaledRadius = capsule2.radius * Math.max(capsule2.body.scale.x, capsule2.body.scale.z);
if (distSquared <= Math.pow(capsule1ScaledRadius + capsule2ScaledRadius, 2)) {
console.log('CAPSULE COLLIDE', capsule1, capsule2);
let normal = new THREE.Vector3().subVectors(p2, p1),
point = p1.clone();
dist = Math.sqrt(distSquared);
normal.divideScalar(dist);
point.x += normal.x * capsule1ScaledRadius;
point.y += normal.y * capsule1ScaledRadius;
point.z += normal.z * capsule1ScaledRadius;
let contact = new elation.physics.contact({
normal: normal,
point: point,
penetration: dist - (capsule1ScaledRadius + capsule2ScaledRadius),
bodies: [capsule1.body, capsule2.body]
});
contacts.push(contact);
return contacts;
}
}
}();
this.capsule_box = function() {
// closure scratch variables
var boxpos = new THREE.Vector3();
var start = new THREE.Vector3();
var end = new THREE.Vector3();
var point = new THREE.Vector3();
var normal = new THREE.Vector3();
var rigid = new elation.physics.rigidbody();
return function(capsule, box, contacts, dt) {
start.set(0,0,0);
end.set(0,capsule.length,0);
if (capsule.offset) {
start.add(capsule.offset);
end.add(capsule.offset);
}
capsule.body.localToWorldPos(start);
capsule.body.localToWorldPos(end);
//box.body.localToWorldPos(point.set(0,0,0));
// FIXME - ugly hack using two spheres
// TODO - use proper sphere-swept line calculations
rigid.velocity = capsule.body.velocity;
rigid.orientation = capsule.body.orientation;
rigid.position = start;
var sphere = new elation.physics.colliders.sphere(rigid, {radius: capsule.radius});
var head = elation.physics.colliders.helperfuncs.box_sphere(box, sphere);
rigid.position = end;
var sphere2 = new elation.physics.colliders.sphere(rigid, {radius: capsule.radius});
var tail = elation.physics.colliders.helperfuncs.box_sphere(box, sphere2);
if (head && tail) {
head[0].bodies[1] = capsule.body;
tail[0].bodies[1] = capsule.body;
//contacts.push(head[0].penetration > tail[0].penetration ? head[0] : tail[0]);
contacts.push(head[0]);
} else if (head) {
head[0].bodies[1] = capsule.body;
contacts.push(head[0]);
} else if (tail) {
tail[0].point.y -= capsule.length;
tail[0].bodies[1] = capsule.body;
contacts.push(tail[0]);
}
return contacts;
}
}();
this.capsule_cylinder = function() {
// closure scratch variables
const cylpos = new THREE.Vector3(),
start = new THREE.Vector3(),
end = new THREE.Vector3(),
point = new THREE.Vector3(),
normal = new THREE.Vector3();
return function(capsule, cylinder, contacts, dt) {
capsule.body.localToWorldPos(start.set(0,0,0));
capsule.body.localToWorldPos(end.set(0,capsule.length,0));
//box.body.localToWorldPos(point.set(0,0,0));
var sphere = new elation.physics.colliders.sphere(capsule.body, {radius: capsule.radius});
sphere.offset = new THREE.Vector3(0,0,0);
if (capsule.offset) sphere.offset.add(capsule.offset);
var head = elation.physics.colliders.helperfuncs.box_sphere(box, sphere);
sphere.offset = new THREE.Vector3(0,capsule.length,0);
if (capsule.offset) sphere.offset.add(capsule.offset);
var tail = elation.physics.colliders.helperfuncs.box_sphere(box, sphere);
if (head && tail) {
head[0].bodies[1] = capsule.body;
tail[0].bodies[1] = capsule.body;
contacts.push(head[0].penetration > tail[0].penetration ? head[0] : tail[0]);
} else if (head) {
head[0].bodies[1] = capsule.body;
contacts.push(head[0]);
} else if (tail) {
tail[0].bodies[1] = capsule.body;
contacts.push(tail[0]);
}
return contacts;
}
}();
this.triangle_sphere = function() {
// closure scratch variables
const sphereClosestPointToPlane = new THREE.Vector3(),
endpos = new THREE.Vector3(),
scaledVelocity = new THREE.Vector3(),
intersectionPoint = new THREE.Vector3(),
triangleClosestPoint = new THREE.Vector3();
// Reference: http://www.peroxide.dk/papers/collision/collision.pdf
return function(triangle, sphere, contacts, dt) {
const spherepos = sphere.body.positionWorld;
const spherevel = sphere.body.velocity;
const worldpoints = triangle.getWorldPoints();
const p1 = worldpoints.p1,
p2 = worldpoints.p2,
p3 = worldpoints.p3,
normal = worldpoints.normal;
let velNormal = spherevel.dot(normal);
// Check if we're already in contact
elation.physics.colliders.helperfuncs.closest_point_on_triangle(sphere.body.positionWorld, p1, p2, p3, triangleClosestPoint);
let triangleDistSquared = triangleClosestPoint.distanceToSquared(sphere.body.positionWorld)
if (triangleDistSquared < sphere.radius * sphere.radius && velNormal <= 0) {
let contact = new elation.physics.contact({
normal: normal.clone(), // allocate normal
point: triangleClosestPoint.clone(), // allocate point
penetration: Math.sqrt(triangleDistSquared) - sphere.radius,
bodies: [triangle.body, sphere.body],
triangle: triangle
});
contacts.push(contact);
return contacts;
}
sphereClosestPointToPlane.x = (normal.x * -sphere.radius) + spherepos.x;
sphereClosestPointToPlane.y = (normal.y * -sphere.radius) + spherepos.y;
sphereClosestPointToPlane.z = (normal.z * -sphere.radius) + spherepos.z;
let sphereOffset = sphere.offset;
if (sphereOffset) {
if (sphereOffset._target) sphereOffset = sphereOffset._target;
sphereClosestPointToPlane.add(sphereOffset);
}
scaledVelocity.x = spherevel.x * dt;
scaledVelocity.y = spherevel.y * dt;
scaledVelocity.z = spherevel.z * dt;
endpos.x = sphereClosestPointToPlane.x + scaledVelocity.x;
endpos.y = sphereClosestPointToPlane.y + scaledVelocity.y;
endpos.z = sphereClosestPointToPlane.z + scaledVelocity.z;
// Find intersection between the ray our sphere is travelling and the plane upon which our triangle rests
let intersectionPlane = elation.physics.colliders.helperfuncs.line_plane(sphereClosestPointToPlane, endpos, p1, p2, p3, intersectionPoint);
if (intersectionPlane && triangle.containsPoint(intersectionPlane.point)) {
// If the intersection point is inside of our triangle, we've collided with the triangle's face
let contact = new elation.physics.contact_dynamic({
normal: normal.clone(), // allocate normal
point: intersectionPlane.point.clone(), // allocate point
penetrationTime: intersectionPlane.t,
bodies: [triangle.body, sphere.body],
triangle: triangle
});
contacts.push(contact);
return contacts;
}
// Check sphere against edges of triangle
endpos.x = spherepos.x + scaledVelocity.x;
endpos.y = spherepos.y + scaledVelocity.y;
endpos.z = spherepos.z + scaledVelocity.z;
// FIXME - this is probably more efficient if we find the closest point on each edge first, use that to pick the closest edge, and then perform only one cylinder intersection test
let intersections = [
elation.physics.colliders.helperfuncs.line_cylinder(spherepos, endpos, p1, p2, sphere.radius),
elation.physics.colliders.helperfuncs.line_cylinder(spherepos, endpos, p2, p3, sphere.radius),
elation.physics.colliders.helperfuncs.line_cylinder(spherepos, endpos, p3, p1, sphere.radius),
];
//console.log(intersections, sphere.body.position, endpos);
let closestIntersectionDist = Infinity;
let closestIntersection = null;
for (let i = 0; i < intersections.length; i++) {
if (intersections[i] != undefined && intersections[i] < closestIntersectionDist) {
closestIntersectionDist = intersections[i];
closestIntersection = i;
}
}
if (closestIntersection !== null) {
const intersectionPoint = endpos.clone().sub(spherepos).multiplyScalar(closestIntersectionDist).add(spherepos); // allocate point
elation.physics.colliders.helperfuncs.closest_point_on_triangle(intersectionPoint, p1, p2, p3, triangleClosestPoint);
let collisionNormal = new THREE.Vector3( // allocate normal
intersectionPoint.x - triangleClosestPoint.x,
intersectionPoint.y - triangleClosestPoint.y,
intersectionPoint.z - triangleClosestPoint.z
);
collisionNormal.normalize();
intersectionPoint.x += collisionNormal.x * -sphere.radius;
intersectionPoint.y += collisionNormal.y * -sphere.radius;
intersectionPoint.z += collisionNormal.z * -sphere.radius;
let contact = new elation.physics.contact_dynamic({
normal: collisionNormal,
point: intersectionPoint,
penetrationTime: closestIntersectionDist,
bodies: [triangle.body, sphere.body],
triangle: triangle
});
contacts.push(contact);
return contacts;
}
}
}();
this.triangle_capsule = function() {
// Scratch variables
const capsuleLine = new THREE.Vector3(),
capsuleStart = new THREE.Vector3(),
capsuleEnd = new THREE.Vector3(),
closestPoint = new THREE.Vector3(),
intersectionPoint = new THREE.Vector3(),
//normal = new THREE.Vector3(),
capsuleNormal = new THREE.Vector3(),
sphereCenter = new THREE.Vector3(),
localSphere = new elation.physics.rigidbody();
return function(triangle, capsule, contacts, dt) {
//triangle.body.localToWorldDir(normal.copy(triangle.normal));
const capsuleDims = capsule.getDimensions();
capsuleNormal.subVectors(capsuleDims.end, capsuleDims.start).normalize();
localSphere.position.copy(capsule.body.position);
localSphere.positionWorld.copy(capsule.body.positionWorld);
const worldpoints = triangle.getWorldPoints();
const p1 = worldpoints.p1,
p2 = worldpoints.p2,
p3 = worldpoints.p3,
normal = worldpoints.normal;
// Find the closest point of the capsule to the triangle
let t = normal.dot(capsuleLine.subVectors(p1, capsuleDims.start).divideScalar(Math.abs(normal.dot(capsuleNormal))));
intersectionPoint.copy(capsuleNormal).multiplyScalar(t).add(capsuleDims.start);
elation.physics.colliders.helperfuncs.closest_point_on_triangle(intersectionPoint, p1, p2, p3, closestPoint);
elation.physics.colliders.helperfuncs.closest_point_on_line(capsuleDims.start, capsuleDims.end, closestPoint, localSphere.position);
localSphere.positionWorld.copy(localSphere.position);
// Perform a sphere/triangle intersection test with our sphere
if (!localSphere.collider) {
localSphere.setCollider('sphere', { radius: capsule.radius * 2});
} else {
localSphere.collider.radius = capsule.radius * 2;
}
localSphere.orientation.copy(capsule.body.orientation);
localSphere.orientationWorld.copy(capsule.body.orientationWorld);
localSphere.velocity.copy(capsule.body.velocity);
let localcontacts = [];
elation.physics.colliders.helperfuncs.triangle_sphere(triangle, localSphere.collider, loca