/**
* LittleJS 3D Math Plugin
* - Vector3 and Matrix4 for 3D games and plugins
* - Right handed, Y up, angles in radians
* - Used by the Render3D plugin, but has no rendering dependencies
* @namespace Math3D
*/
'use strict';
///////////////////////////////////////////////////////////////////////////////
/**
* Create a 3D vector, can take 0, 1, 2 or 3 numbers
* - vec3() is zero, vec3(s) fills all three, vec3(x, y) sets z to 0
* @param {number} [x]
* @param {number} [y]
* @param {number} [z]
* @return {Vector3}
* @memberof Math3D
*/
function vec3(x=0, y, z)
{
return y === undefined ? new Vector3(x, x, x) : new Vector3(x, y, z === undefined ? 0 : z);
}
/**
* Check if the object is a valid Vector3
* @param {any} v
* @return {boolean}
* @memberof Math3D
*/
function isVector3(v) { return v instanceof Vector3 && v.isValid(); }
// debug check that a value is a usable Vector3, stripped in release like the 2D one
function ASSERT_VECTOR3_VALID(v) { ASSERT(isVector3(v), 'Vector3 is invalid.', v); }
/**
* Returns a random Vector3 of a given length, pointing any direction evenly, or within a cone around +Y
* @param {number} [length]
* @param {number} [coneAngle] - Half angle of the cone around +Y in radians, PI is every direction
* @return {Vector3}
* @memberof Math3D
*/
function randVector3(length=1, coneAngle=PI)
{
// a random height on the sphere is uniform over its surface, then a random turn around Y
const y = rand(cos(coneAngle), 1), s = (1 - y * y) ** .5, a = rand(2 * PI);
return new Vector3(s * cos(a) * length, y * length, s * sin(a) * length);
}
/**
* Returns a random Vector3 inside a sphere, spread evenly through its volume, the 3D twin of randInCircle
* @param {number} [radius]
* @param {number} [minRadius] - Leave a hollow middle this big
* @return {Vector3}
* @memberof Math3D
*/
function randInSphere(radius=1, minRadius=0)
{
// the volume inside a radius grows with its cube, so that is what has to come out even
if (radius <= 0) return new Vector3;
const ratio = clamp(minRadius / radius);
return randVector3(radius * rand(ratio**3, 1) ** (1/3));
}
/**
* 3D Vector object, right handed with Y up
* - Methods return new vectors except set and setFrom
* @memberof Math3D
* @example
* const a = vec3(1, 2, 3);
* const b = a.add(vec3(0, 1, 0)).normalize();
*/
class Vector3
{
/** Create a 3D vector
* @param {number} [x]
* @param {number} [y]
* @param {number} [z] */
constructor(x=0, y=0, z=0)
{
ASSERT(isNumber(x) && isNumber(y) && isNumber(z), 'Vector3 components must be numbers');
/** @property {number} - X axis location */
this.x = x;
/** @property {number} - Y axis location */
this.y = y;
/** @property {number} - Z axis location */
this.z = z;
}
/** Sets values of this vector and returns self
* @param {number} [x]
* @param {number} [y]
* @param {number} [z]
* @return {Vector3} */
set(x=0, y=0, z=0) { this.x = x; this.y = y; this.z = z; ASSERT_VECTOR3_VALID(this); return this; }
/** Copies the values of another vector into this one and returns self
* @param {Vector3} v
* @return {Vector3} */
setFrom(v) { return this.set(v.x, v.y, v.z); }
/** Returns a new vector that is a copy of this
* @return {Vector3} */
copy() { return new Vector3(this.x, this.y, this.z); }
/** Returns a copy of this vector plus the vector passed in
* @param {Vector3} v
* @return {Vector3} */
add(v) { return new Vector3(this.x + v.x, this.y + v.y, this.z + v.z); }
/** Returns a copy of this vector minus the vector passed in
* @param {Vector3} v
* @return {Vector3} */
subtract(v) { return new Vector3(this.x - v.x, this.y - v.y, this.z - v.z); }
/** Returns a copy of this vector times the vector passed in
* @param {Vector3} v
* @return {Vector3} */
multiply(v) { return new Vector3(this.x * v.x, this.y * v.y, this.z * v.z); }
/** Returns a copy of this vector divided by the vector passed in
* @param {Vector3} v
* @return {Vector3} */
divide(v) { return new Vector3(this.x / v.x, this.y / v.y, this.z / v.z); }
/** Returns a copy of this vector scaled by the number passed in
* @param {number} s
* @return {Vector3} */
scale(s) { return new Vector3(this.x * s, this.y * s, this.z * s); }
/** Returns the length of this vector
* @return {number} */
length() { return this.lengthSquared()**.5; }
/** Returns the length of this vector squared
* @return {number} */
lengthSquared() { return this.x**2 + this.y**2 + this.z**2; }
/** Returns a copy of this vector reflected by a surface normal
* @param {Vector3} normal - Surface normal, should be normalized
* @param {number} [restitution] - How much to bounce, 1 is a perfect bounce, 0 slides along the surface
* @return {Vector3} */
reflect(normal, restitution=1) { return this.subtract(normal.scale((1 + restitution) * this.dot(normal))); }
/** Returns the distance from this vector to the vector passed in
* @param {Vector3} v
* @return {number} */
distance(v) { return this.distanceSquared(v)**.5; }
/** Returns the distance squared from this vector to the vector passed in
* @param {Vector3} v
* @return {number} */
distanceSquared(v) { return (this.x - v.x)**2 + (this.y - v.y)**2 + (this.z - v.z)**2; }
/** Returns a new vector in the same direction with the length passed in, zero stays zero
* @param {number} [length]
* @return {Vector3} */
normalize(length=1)
{
const l = this.length();
return l ? this.scale(length/l) : new Vector3;
}
/** Returns a new vector clamped to the length passed in
* @param {number} [length]
* @return {Vector3} */
clampLength(length=1)
{
const l = this.length();
return l > length ? this.scale(length/l) : this.copy();
}
/** Returns the dot product of this vector and the vector passed in
* @param {Vector3} v
* @return {number} */
dot(v) { return this.x*v.x + this.y*v.y + this.z*v.z; }
/** Returns a vector at right angles to both this and the one passed in
* @param {Vector3} v
* @return {Vector3} */
cross(v)
{
return new Vector3(
this.y*v.z - this.z*v.y,
this.z*v.x - this.x*v.z,
this.x*v.y - this.y*v.x);
}
/** Returns a new vector interpolated between this and the vector passed in, percent is clamped to 0-1
* @param {Vector3} v
* @param {number} percent
* @return {Vector3} */
lerp(v, percent)
{
ASSERT_VECTOR3_VALID(v);
return this.add(v.subtract(this).scale(clamp(percent)));
}
/** Returns a new vector turned around an axis, counter clockwise when the axis points at you
* @param {Vector3} axis - Unit length
* @param {number} angle - Radians
* @return {Vector3} */
rotate(axis, angle)
{
ASSERT_VECTOR3_VALID(axis); // unlike Vector2.rotate this takes an axis first
// Rodrigues' formula: the part along the axis stays, the rest turns
const c = cos(angle), s = sin(angle), d = axis.dot(this) * (1 - c);
return this.scale(c).add(axis.cross(this).scale(s)).add(axis.scale(d));
}
/** Returns a new vector turned around the X axis, the way a positive pitch in rotation3D turns things
* @param {number} angle - Radians
* @return {Vector3} */
rotateX(angle)
{
const c = cos(angle), s = sin(angle);
return new Vector3(this.x, this.y*c - this.z*s, this.y*s + this.z*c);
}
/** Returns a new vector turned around the Y axis, the way a positive yaw in rotation3D turns things
* @param {number} angle - Radians
* @return {Vector3} */
rotateY(angle)
{
const c = cos(angle), s = sin(angle);
return new Vector3(this.x*c + this.z*s, this.y, this.z*c - this.x*s);
}
/** Returns a new vector turned around the Z axis, the way a positive roll in rotation3D turns things
* @param {number} angle - Radians
* @return {Vector3} */
rotateZ(angle)
{
const c = cos(angle), s = sin(angle);
return new Vector3(this.x*c - this.y*s, this.x*s + this.y*c, this.z);
}
/** Returns a new vector with the absolute value of each component
* @return {Vector3} */
abs() { return new Vector3(abs(this.x), abs(this.y), abs(this.z)); }
/** Returns a new vector with each component floored
* @return {Vector3} */
floor() { return new Vector3(floor(this.x), floor(this.y), floor(this.z)); }
/** Returns a new vector with each component rounded
* @return {Vector3} */
round() { return new Vector3(round(this.x), round(this.y), round(this.z)); }
/** Returns a new vector snapped down to a grid, grid is the number of steps per unit like Vector2.snap
* @param {number} grid - Snap steps per unit, 2 snaps to halves
* @return {Vector3} */
snap(grid)
{
ASSERT_NUMBER_VALID(grid);
return new Vector3(floor(this.x*grid)/grid, floor(this.y*grid)/grid, floor(this.z*grid)/grid);
}
/** Returns this point transformed by a matrix, translation included
* @param {Matrix4} matrix
* @return {Vector3} */
transform(matrix) { return matrix.transformPoint(this); }
/** Returns this direction transformed by a matrix, rotation and scale only
* @param {Matrix4} matrix
* @return {Vector3} */
transformDirection(matrix) { return matrix.transformDirection(this); }
/** Checks if this is a valid vector
* @return {boolean} */
isValid() { return isNumber(this.x) && isNumber(this.y) && isNumber(this.z); }
/** Returns a string representation of this vector for debugging
* @param {number} [digits] - Number of digits to display
* @return {string} */
toString(digits=3)
{
if (!this.isValid())
return `(${this.x},${this.y},${this.z})`; // show the bad values instead of throwing
const f = (v)=> (v < 0 ? '' : ' ') + v.toFixed(digits);
return `(${f(this.x)},${f(this.y)},${f(this.z)} )`;
}
}
///////////////////////////////////////////////////////////////////////////////
// scratch for multiply, nothing keeps a reference to it
const matrix4Scratch = new Float32Array(16);
const matrix4Identity = new Float32Array([1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1]);
/**
* 4x4 transform matrix for moving, rotating and scaling points in 3D
* - Static builders like Matrix4.translation return a new matrix
* - Methods on a matrix change it in place and return it, so calls can chain
* - a.multiply(b) means b happens first, then a
* - Stored the way WebGL wants it, so it can be sent to a shader as is
* @memberof Math3D
* @example
* const m = buildMatrix(vec3(0, 1, 0), vec3(0, PI/2, 0)); // rotate then move up
* const p = m.transformPoint(vec3(1, 0, 0));
*/
class Matrix4
{
/** Create a matrix, identity by default
* @param {Float32Array|Array<number>} [m] - 16 column major values */
constructor(m)
{
/** @property {Float32Array} - The 16 column major values */
this.m = new Float32Array(16);
ASSERT(!m || m.length == 16, 'Matrix4 takes 16 values, use copy() to duplicate a matrix');
if (m)
this.m.set(m);
else
this.m[0] = this.m[5] = this.m[10] = this.m[15] = 1;
}
/** Returns a new identity matrix
* @return {Matrix4} */
static identity() { return new Matrix4; }
/** Returns a new translation matrix
* @param {Vector3} v
* @return {Matrix4} */
static translation(v)
{
ASSERT_VECTOR3_VALID(v);
const r = new Matrix4;
r.m[12] = v.x; r.m[13] = v.y; r.m[14] = v.z;
return r;
}
/** Returns a rotation matrix, rolled first, then pitched, then yawed
* @param {Vector3} euler - vec3(pitch, yaw, roll) in radians
* @param {Matrix4} [matrix] - Written into instead of a new one, for a loop that builds many
* @return {Matrix4} */
static rotation(euler, matrix=new Matrix4)
{
ASSERT_VECTOR3_VALID(euler);
const cx = cos(euler.x), sx = sin(euler.x);
const cy = cos(euler.y), sy = sin(euler.y);
const cz = cos(euler.z), sz = sin(euler.z);
const m = matrix.m;
// R = Ry * Rx * Rz written out, column major, every element set so a reused matrix comes out clean
m[0] = cy*cz + sy*sx*sz; m[1] = cx*sz; m[2] = -sy*cz + cy*sx*sz; m[3] = 0;
m[4] = -cy*sz + sy*sx*cz; m[5] = cx*cz; m[6] = sy*sz + cy*sx*cz; m[7] = 0;
m[8] = sy*cx; m[9] = -sx; m[10] = cy*cx; m[11] = 0;
m[12] = m[13] = m[14] = 0; m[15] = 1;
return matrix;
}
/** Returns a new scale matrix
* @param {Vector3} v
* @return {Matrix4} */
static scaling(v)
{
ASSERT_VECTOR3_VALID(v);
const r = new Matrix4;
r.m[0] = v.x; r.m[5] = v.y; r.m[10] = v.z;
return r;
}
/** Returns a new perspective projection, camera looks down -Z
* @param {number} fov - Vertical field of view in radians
* @param {number} aspect - Width divided by height
* @param {number} near - Closest visible distance
* @param {number} far - Furthest visible distance, Infinity is allowed
* @return {Matrix4} */
static perspective(fov, aspect, near, far)
{
ASSERT(near > 0 && far > near, 'a perspective projection needs 0 < near < far, or nothing is visible', near, far);
const f = 1 / tan(fov/2);
const r = new Matrix4;
const m = r.m;
m[0] = f / aspect;
m[5] = f;
m[10] = far == Infinity ? -1 : (far + near) / (near - far); // the infinite case is the limit of the formula
m[11] = -1;
m[14] = far == Infinity ? -2 * near : 2 * far * near / (near - far);
m[15] = 0;
return r;
}
/** Returns a new orthographic projection, camera looks down -Z
* @param {number} left - Edge of the visible box
* @param {number} right - Edge of the visible box
* @param {number} bottom - Edge of the visible box
* @param {number} top - Edge of the visible box
* @param {number} near - Closest visible distance
* @param {number} far - Furthest visible distance, Infinity is not allowed here
* @return {Matrix4} */
static orthographic(left, right, bottom, top, near, far)
{
// an infinite far plane has no orthographic form: every depth would land on the near plane,
// and the formula below works out to NaN, which quietly clips the whole scene away
ASSERT(far > near && far != Infinity, 'an orthographic projection needs a real far plane past near, Infinity is perspective only', near, far);
const r = new Matrix4;
const m = r.m;
m[0] = 2 / (right - left);
m[5] = 2 / (top - bottom);
m[10] = -2 / (far - near);
m[12] = -(right + left) / (right - left);
m[13] = -(top + bottom) / (top - bottom);
m[14] = -(far + near) / (far - near);
return r;
}
/** Returns the transform of something at eye turned to face target
* - Invert it to get a view matrix for a camera there
* @param {Vector3} eye
* @param {Vector3} target
* @param {Vector3} [up]
* @return {Matrix4} */
static lookAt(eye, target, up=vec3(0, 1, 0))
{
let z = eye.subtract(target).normalize();
if (!z.lengthSquared())
z = vec3(0, 0, 1); // eye is on the target, face -Z
let x = up.cross(z).normalize();
// up is along the view direction, pick another; looking straight down or up keeps +X as the right axis,
// as three.js does and as a view tilting there from the +Z side ends up
if (!x.lengthSquared())
x = (abs(z.y) > .99 ? vec3(0, 0, z.y > 0 ? -1 : 1) : vec3(0, 1, 0)).cross(z).normalize();
const y = z.cross(x);
return new Matrix4([x.x, x.y, x.z, 0, y.x, y.y, y.z, 0, z.x, z.y, z.z, 0, eye.x, eye.y, eye.z, 1]);
}
/** Returns a new matrix that is a copy of this
* @return {Matrix4} */
copy() { return new Matrix4(this.m); }
/** Multiply this matrix by another and return this, the other happens first
* @param {Matrix4} matrix
* @return {Matrix4} */
multiply(matrix)
{
const a = this.m, b = matrix.m, r = matrix4Scratch;
for (let j = 0; j < 4; ++j)
for (let i = 0; i < 4; ++i)
r[j*4 + i] = a[i]*b[j*4] + a[4 + i]*b[j*4 + 1] + a[8 + i]*b[j*4 + 2] + a[12 + i]*b[j*4 + 3];
this.m.set(r);
return this;
}
/** Append a translation, returns self
* @param {Vector3} v
* @return {Matrix4} */
translate(v) { return this.multiply(Matrix4.translation(v)); }
/** Append a rotation, returns self
* @param {Vector3} euler - vec3(pitch, yaw, roll) in radians
* @return {Matrix4} */
rotate(euler) { return this.multiply(Matrix4.rotation(euler)); }
/** Append a scale, returns self
* @param {Vector3} v
* @return {Matrix4} */
scale(v) { return this.multiply(Matrix4.scaling(v)); }
/** Transpose this matrix in place, returns self
* @return {Matrix4} */
transpose()
{
const m = this.m;
for (let i = 0; i < 4; ++i)
for (let j = i + 1; j < 4; ++j)
{
const t = m[i*4 + j];
m[i*4 + j] = m[j*4 + i];
m[j*4 + i] = t;
}
return this;
}
/** Flip this matrix so it undoes itself, returns this and does nothing if it cannot be inverted
* @return {Matrix4} */
invert()
{
const m = this.m;
const [a00, a01, a02, a03, a10, a11, a12, a13, a20, a21, a22, a23, a30, a31, a32, a33] = m;
const b00 = a00*a11 - a01*a10, b01 = a00*a12 - a02*a10, b02 = a00*a13 - a03*a10;
const b03 = a01*a12 - a02*a11, b04 = a01*a13 - a03*a11, b05 = a02*a13 - a03*a12;
const b06 = a20*a31 - a21*a30, b07 = a20*a32 - a22*a30, b08 = a20*a33 - a23*a30;
const b09 = a21*a32 - a22*a31, b10 = a21*a33 - a23*a31, b11 = a22*a33 - a23*a32;
let det = b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06;
if (!det)
return this;
det = 1 / det;
m[0] = (a11*b11 - a12*b10 + a13*b09) * det;
m[1] = (a02*b10 - a01*b11 - a03*b09) * det;
m[2] = (a31*b05 - a32*b04 + a33*b03) * det;
m[3] = (a22*b04 - a21*b05 - a23*b03) * det;
m[4] = (a12*b08 - a10*b11 - a13*b07) * det;
m[5] = (a00*b11 - a02*b08 + a03*b07) * det;
m[6] = (a32*b02 - a30*b05 - a33*b01) * det;
m[7] = (a20*b05 - a22*b02 + a23*b01) * det;
m[8] = (a10*b10 - a11*b08 + a13*b06) * det;
m[9] = (a01*b08 - a00*b10 - a03*b06) * det;
m[10] = (a30*b04 - a31*b02 + a33*b00) * det;
m[11] = (a21*b02 - a20*b04 - a23*b00) * det;
m[12] = (a11*b07 - a10*b09 - a12*b06) * det;
m[13] = (a00*b09 - a01*b07 + a02*b06) * det;
m[14] = (a31*b01 - a30*b03 - a32*b00) * det;
m[15] = (a20*b03 - a21*b01 + a22*b00) * det;
return this;
}
/** Transform a point, translation included
* @param {Vector3} v
* @return {Vector3} */
transformPoint(v)
{
const m = this.m;
return new Vector3(
m[0]*v.x + m[4]*v.y + m[8]*v.z + m[12],
m[1]*v.x + m[5]*v.y + m[9]*v.z + m[13],
m[2]*v.x + m[6]*v.y + m[10]*v.z + m[14]);
}
/** Transform a direction, rotation and scale only
* @param {Vector3} v
* @return {Vector3} */
transformDirection(v)
{
const m = this.m;
return new Vector3(
m[0]*v.x + m[4]*v.y + m[8]*v.z,
m[1]*v.x + m[5]*v.y + m[9]*v.z,
m[2]*v.x + m[6]*v.y + m[10]*v.z);
}
/** Returns the translation part of this matrix
* @return {Vector3} */
getTranslation() { return new Vector3(this.m[12], this.m[13], this.m[14]); }
/** Returns the determinant of the rotation and scale part: negative when the matrix mirrors, 0 when it flattens a
* shape and has no inverse
* @return {number} */
determinant()
{
const m = this.m;
return m[0]*(m[5]*m[10] - m[6]*m[9]) - m[4]*(m[1]*m[10] - m[2]*m[9]) + m[8]*(m[1]*m[6] - m[2]*m[5]);
}
/** Returns the scale part of this matrix, the length of each axis; a mirroring matrix shows as a negative x
* @return {Vector3} */
getScale()
{
const m = this.m;
const x = hypot(m[0], m[1], m[2]), y = hypot(m[4], m[5], m[6]), z = hypot(m[8], m[9], m[10]);
return new Vector3(this.determinant() < 0 ? -x : x, y, z);
}
/** Returns the rotation part of this matrix as vec3(pitch, yaw, roll), the angles Matrix4.rotation builds it from
* - The scale is divided out first, so it works on a full transform
* - A matrix with shear, from a scaled parent with a turned child, has no exact answer and gets the nearest
* @return {Vector3} */
getRotation()
{
// the axes at unit length, see Matrix4.rotation for which element is which
const m = this.m, s = this.getScale();
const sx = s.x || 1, sy = s.y || 1, sz = s.z || 1;
const m1 = m[1] / sx, m5 = m[5] / sy, m8 = m[8] / sz, m9 = m[9] / sz, m10 = m[10] / sz;
const pitch = Math.asin(clamp(-m9, -1, 1));
if (abs(m9) < 1 - 1e-6)
return new Vector3(pitch, atan2(m8, m10), atan2(m1, m5));
// straight up or down: yaw and roll turn about the same axis, so the roll is zero and yaw takes it all
return new Vector3(pitch, atan2(m[4] / sy * -m9, m[0] / sx), 0);
}
/** Returns a string representation of this matrix for debugging
* @return {string} */
toString()
{
const m = this.m, f = (i)=> m[i].toFixed(2).padStart(7);
let s = '';
for (let row = 0; row < 4; ++row)
s += `[${f(row)} ${f(4 + row)} ${f(8 + row)} ${f(12 + row)} ]\n`;
return s;
}
}
///////////////////////////////////////////////////////////////////////////////
/**
* Build a transform for an object from its position, rotation and scale
* - A point is scaled first, then rotated, then moved, which is what you want for a game object
* @param {Vector3} [pos]
* @param {Vector3} [rotation] - vec3(pitch, yaw, roll) in radians
* @param {Vector3} [scale]
* @param {Matrix4} [matrix] - Written into instead of a new one, for a loop that builds many
* @return {Matrix4}
* @memberof Math3D
*/
function buildMatrix(pos, rotation, scale, matrix=new Matrix4)
{
ASSERT(!pos || isVector3(pos), 'pos must be a Vector3', pos);
ASSERT(!scale || isVector3(scale), 'scale must be a Vector3', scale);
ASSERT(matrix instanceof Matrix4, 'the matrix to write into must be a Matrix4');
// scale the rotation columns and drop the position in, instead of multiplying three matrices
// an object that is not turned at all is most of a big scene, and identity is what the six
// trig calls would have worked out to anyway
const turned = rotation && (rotation.x || rotation.y || rotation.z), m = matrix.m;
turned ? Matrix4.rotation(rotation, matrix) : m.set(matrix4Identity);
if (scale)
{
m[0] *= scale.x; m[1] *= scale.x; m[2] *= scale.x;
m[4] *= scale.y; m[5] *= scale.y; m[6] *= scale.y;
m[8] *= scale.z; m[9] *= scale.z; m[10] *= scale.z;
}
if (pos)
m[12] = pos.x, m[13] = pos.y, m[14] = pos.z;
return matrix;
}
///////////////////////////////////////////////////////////////////////////////
/**
* Ray3D - A start point and a direction, what screenToRay returns and the raycast helpers take
* - The direction need not be unit length, the distances that come back are in units of it
* @memberof Math3D
* @example
* const ray = render3D.screenToRay(mousePosScreen);
* const distance = raycastPlane(ray, vec3(), vec3(0, 1, 0));
* if (distance !== undefined)
* ball.pos3D = ray.getPosition(distance);
*/
class Ray3D
{
/** Create a ray
* @param {Vector3} [origin]
* @param {Vector3} [direction] - Defaults to -Z, forward */
constructor(origin=vec3(), direction=vec3(0, 0, -1))
{
ASSERT_VECTOR3_VALID(origin);
ASSERT_VECTOR3_VALID(direction);
/** @property {Vector3} - Where the ray starts */
this.origin = origin;
/** @property {Vector3} - Which way it goes */
this.direction = direction;
}
/** Returns the point a distance along the ray
* @param {number} distance - What the raycast helpers return
* @return {Vector3} */
getPosition(distance) { return this.origin.add(this.direction.scale(distance)); }
/** Returns a new ray that is a copy of this
* @return {Ray3D} */
copy() { return new Ray3D(this.origin.copy(), this.direction.copy()); }
}
///////////////////////////////////////////////////////////////////////////////
// 3D collision helpers, none of them change anything that is passed in
// Boxes sit centered on pos and take a full size, like drawRect, upright unless given a rotation, an Euler vec3 like
// rotation3D, which turns them about their center
// Cylinders stand up the Y axis, centered on pos, with a full height
// Names that could be mistaken for 2D functions get a 3D suffix
// whether a rotation turns anything, a missing one does not
const isTurned3D = (rotation)=> !!rotation && !!(rotation.x || rotation.y || rotation.z);
// a turned box's three unit axes, the columns of its rotation
function boxAxes3D(rotation)
{
const m = buildMatrix(undefined, rotation).m;
return [vec3(m[0], m[1], m[2]), vec3(m[4], m[5], m[6]), vec3(m[8], m[9], m[10])];
}
// a point in a turned box's own space, from its center, and a vector from that space back to the world's
function boxLocal3D(point, pos, axes)
{
const d = point.subtract(pos);
return vec3(d.dot(axes[0]), d.dot(axes[1]), d.dot(axes[2]));
}
const boxWorld3D = (v, axes)=> axes[0].scale(v.x).add(axes[1].scale(v.y)).add(axes[2].scale(v.z));
/**
* Check if a point is inside a box, boundary is inclusive
* @param {Vector3} point
* @param {Vector3} pos - Center of the box
* @param {Vector3} size - Full size of the box
* @param {Vector3} [rotation] - How the box is turned, upright when left out
* @return {boolean}
* @memberof Math3D
*/
function isPointInBox3D(point, pos, size, rotation)
{
if (isTurned3D(rotation))
return isPointInBox3D(boxLocal3D(point, pos, boxAxes3D(rotation)), vec3(), size);
return abs(point.x - pos.x) <= size.x/2 &&
abs(point.y - pos.y) <= size.y/2 &&
abs(point.z - pos.z) <= size.z/2;
}
/**
* Check if two boxes are overlapping, touching edges do not overlap
* @param {Vector3} posA
* @param {Vector3} sizeA - Full size of box A
* @param {Vector3} posB
* @param {Vector3} [sizeB] - Full size of box B, zero for a point
* @param {Vector3} [rotationA] - How box A is turned, upright when left out
* @param {Vector3} [rotationB] - How box B is turned
* @return {boolean}
* @memberof Math3D
*/
function isOverlapping3D(posA, sizeA, posB, sizeB=vec3(), rotationA, rotationB)
{
if (isTurned3D(rotationA) || isTurned3D(rotationB))
return !!collideBoxBox3D(posA, sizeA, posB, sizeB, rotationA, rotationB);
const d = posA.subtract(posB);
return abs(d.x) < (sizeA.x + sizeB.x)/2 &&
abs(d.y) < (sizeA.y + sizeB.y)/2 &&
abs(d.z) < (sizeA.z + sizeB.z)/2;
}
/**
* Returns the vector to move sphere A by so it no longer overlaps sphere B, or undefined
* @param {Vector3} posA
* @param {number} radiusA
* @param {Vector3} posB
* @param {number} radiusB
* @return {Vector3|undefined}
* @memberof Math3D
*/
function collideSphereSphere(posA, radiusA, posB, radiusB)
{
const d = posA.subtract(posB);
const r = radiusA + radiusB;
const dist = d.length();
if (dist >= r)
return undefined;
if (!dist)
return vec3(0, r, 0); // coincident centers, push straight up
return d.normalize(r - dist);
}
/**
* Returns the vector to move a sphere out of a box, or undefined
* @param {Vector3} pos - Sphere center
* @param {number} radius
* @param {Vector3} boxPos
* @param {Vector3} boxSize - Full size of the box
* @param {Vector3} [boxRotation] - How the box is turned, upright when left out
* @return {Vector3|undefined}
* @memberof Math3D
*/
function collideSphereBox(pos, radius, boxPos, boxSize, boxRotation)
{
if (isTurned3D(boxRotation))
return collideSphereOrientedBox3D(pos, radius, boxPos, boxSize, boxAxes3D(boxRotation));
const h = boxSize.scale(.5);
const closest = vec3(
clamp(pos.x, boxPos.x - h.x, boxPos.x + h.x),
clamp(pos.y, boxPos.y - h.y, boxPos.y + h.y),
clamp(pos.z, boxPos.z - h.z, boxPos.z + h.z));
const d = pos.subtract(closest), distSq = d.lengthSquared();
if (distSq)
return distSq >= radius*radius ? undefined : d.normalize(radius - distSq**.5);
// center is inside the box, push out along the axis of least penetration
const offset = pos.subtract(boxPos);
return pushOutAxis3D(offset, h.x - abs(offset.x), h.y - abs(offset.y), h.z - abs(offset.z), radius);
}
/**
* Returns the vector to move a sphere back inside an axis aligned box, or undefined when it is all inside
* - The inside out twin of collideSphereBox, for keeping things in a room or an arena
* - A sphere too big for the box on some axis is held at the middle of it on that axis
* @param {Vector3} pos - Sphere center
* @param {number} radius
* @param {Vector3} boxPos
* @param {Vector3} boxSize - Full size of the box
* @return {Vector3|undefined}
* @memberof Math3D
*/
function collideSphereInBox(pos, radius, boxPos, boxSize)
{
// the box shrunk by the radius is everywhere the center can be, so clamp the center into it
const x = max(0, boxSize.x/2 - radius), y = max(0, boxSize.y/2 - radius), z = max(0, boxSize.z/2 - radius);
const push = vec3(
clamp(pos.x, boxPos.x - x, boxPos.x + x) - pos.x,
clamp(pos.y, boxPos.y - y, boxPos.y + y) - pos.y,
clamp(pos.z, boxPos.z - z, boxPos.z + z) - pos.z);
return push.lengthSquared() ? push : undefined;
}
// the axis with the smallest penetration, pointing the way d does, with extra distance added
function pushOutAxis3D(d, penX, penY, penZ, extra=0)
{
const s = (v)=> v >= 0 ? 1 : -1; // sign() gives 0 on a tie, which would be no push
if (penX <= penY && penX <= penZ)
return vec3(s(d.x)*(penX + extra), 0, 0);
if (penY <= penZ)
return vec3(0, s(d.y)*(penY + extra), 0);
return vec3(0, 0, s(d.z)*(penZ + extra));
}
/**
* Returns the vector to move a sphere out of a vertical cylinder, or undefined
* @param {Vector3} pos - Sphere center
* @param {number} radius
* @param {Vector3} cylinderPos
* @param {number} cylinderRadius
* @param {number} cylinderHeight - Full height along Y
* @return {Vector3|undefined}
* @memberof Math3D
*/
function collideSphereCylinder(pos, radius, cylinderPos, cylinderRadius, cylinderHeight)
{
const halfHeight = cylinderHeight/2;
const offsetX = pos.x - cylinderPos.x;
const offsetZ = pos.z - cylinderPos.z;
const offsetY = pos.y - cylinderPos.y;
const radialDist = (offsetX**2 + offsetZ**2)**.5;
const radialScale = radialDist ? min(radialDist, cylinderRadius)/radialDist : 0; // pull the point onto the wall, or the axis
const closest = vec3(
cylinderPos.x + offsetX*radialScale,
clamp(pos.y, cylinderPos.y - halfHeight, cylinderPos.y + halfHeight),
cylinderPos.z + offsetZ*radialScale);
const d = pos.subtract(closest), distSq = d.lengthSquared();
if (distSq)
return distSq >= radius*radius ? undefined : d.normalize(radius - distSq**.5);
// center is inside the cylinder, push out through the nearer surface
const sidePen = cylinderRadius - radialDist;
const capPen = halfHeight - abs(offsetY);
if (sidePen <= capPen)
{
const dir = radialDist ? vec3(offsetX/radialDist, 0, offsetZ/radialDist) : vec3(1, 0, 0);
return dir.scale(sidePen + radius);
}
return vec3(0, (offsetY >= 0 ? 1 : -1)*(capPen + radius), 0);
}
/**
* Returns the vector to move box A by so it no longer overlaps box B, the shortest way out, or undefined
* - The 3D twin of collideBoxBox
* - Turned boxes are tested by the separating axis test, the push is along the direction they overlap least on
* @param {Vector3} posA
* @param {Vector3} sizeA - Full size of box A
* @param {Vector3} posB
* @param {Vector3} sizeB - Full size of box B
* @param {Vector3} [rotationA] - How box A is turned, upright when left out
* @param {Vector3} [rotationB] - How box B is turned
* @return {Vector3|undefined}
* @memberof Math3D
*/
function collideBoxBox3D(posA, sizeA, posB, sizeB, rotationA, rotationB)
{
if (isTurned3D(rotationA) || isTurned3D(rotationB))
return collideOrientedBoxes3D(posA, sizeA, boxAxes3D(rotationA), posB, sizeB, boxAxes3D(rotationB));
const d = posA.subtract(posB);
const overlapX = (sizeA.x + sizeB.x)/2 - abs(d.x);
const overlapY = (sizeA.y + sizeB.y)/2 - abs(d.y);
const overlapZ = (sizeA.z + sizeB.z)/2 - abs(d.z);
if (overlapX <= 0 || overlapY <= 0 || overlapZ <= 0)
return undefined;
return pushOutAxis3D(d, overlapX, overlapY, overlapZ);
}
// the push to move a sphere out of a box turned to these axes: in the box's own space it is upright, and the push
// goes back out turned
function collideSphereOrientedBox3D(pos, radius, boxPos, boxSize, axes)
{
const push = collideSphereBox(boxLocal3D(pos, boxPos, axes), radius, vec3(), boxSize);
return push && boxWorld3D(push, axes);
}
// the world's axes, for an upright box among turned ones
const BOX_WORLD_AXES = Object.freeze([vec3(1, 0, 0), vec3(0, 1, 0), vec3(0, 0, 1)]);
// the shortest push to move box A out of box B by the separating axis test: each box's three face directions and
// the nine across an edge of each; a gap on any of them is no touch, and the push is along the least overlap, from
// B to A; an edge direction has to overlap clearly less to win, so a box lying on another leaves by the face
function collideOrientedBoxes3D(posA, sizeA, axesA, posB, sizeB, axesB)
{
const d = posA.subtract(posB), ha = sizeA.scale(.5), hb = sizeB.scale(.5);
const reach = (h, axes, n)=> h.x * abs(axes[0].dot(n)) + h.y * abs(axes[1].dot(n)) + h.z * abs(axes[2].dot(n));
let push, least = Infinity;
const separated = (axis, weight)=>
{
const lengthSquared = axis.lengthSquared();
if (lengthSquared < 1e-12) return false; // parallel edges give no direction to test
const n = axis.scale(lengthSquared ** -.5), along = d.dot(n);
const overlap = reach(ha, axesA, n) + reach(hb, axesB, n) - abs(along);
if (overlap <= 0) return true;
if (overlap * weight < least)
least = overlap * weight, push = n.scale(along < 0 ? -overlap : overlap);
return false;
};
for (const axis of [...axesA, ...axesB])
if (separated(axis, 1)) return;
for (const a of axesA)
for (const b of axesB)
if (separated(a.cross(b), 1.05)) return;
return push;
}
/**
* Returns the distance along the ray to the first intersection with a sphere, or undefined
* - The hit is ray.getPosition(distance), a direction that is not unit length scales the distance
* - A ray starting inside the sphere is already there, so it gets back 0
* @param {Ray3D} ray
* @param {Vector3} pos - Sphere center
* @param {number} radius
* @return {number|undefined}
* @memberof Math3D
*/
function raycastSphere(ray, pos, radius)
{
const {origin, direction} = ray;
const oc = origin.subtract(pos);
const c = oc.dot(oc) - radius*radius;
if (c < 0)
return 0; // origin is inside the sphere, even for a ray of no length, as raycastBox gives
const a = direction.dot(direction);
if (!a)
return undefined;
const b = 2*oc.dot(direction);
const discriminant = b*b - 4*a*c;
if (discriminant < 0)
return undefined;
const t = (-b - discriminant**.5)/(2*a);
return t >= 0 ? t : undefined;
}
/**
* Returns the distance along the ray to a plane, or undefined if parallel or behind
* - The hit is ray.getPosition(distance), a direction that is not unit length scales the distance
* @param {Ray3D} ray
* @param {Vector3} planePos
* @param {Vector3} planeNormal
* @return {number|undefined}
* @memberof Math3D
*/
function raycastPlane(ray, planePos, planeNormal)
{
const {origin, direction} = ray;
const denominator = direction.dot(planeNormal);
if (abs(denominator) < 1e-9)
return undefined;
const t = planePos.subtract(origin).dot(planeNormal)/denominator;
return t < 0 ? undefined : t;
}
/**
* Returns the distance along the ray to the first intersection with a box, or undefined
* - The hit is ray.getPosition(distance), a direction that is not unit length scales the distance
* - A ray starting inside the box is already there, so it gets back 0
* @param {Ray3D} ray
* @param {Vector3} pos - Center of the box
* @param {Vector3} size - Full size of the box
* @param {Vector3} [rotation] - How the box is turned, upright when left out
* @return {number|undefined}
* @memberof Math3D
*/
function raycastBox(ray, pos, size, rotation)
{
if (isTurned3D(rotation))
{
// the ray in the box's own space, where the box is upright; turning keeps the distances
const axes = boxAxes3D(rotation), d = ray.direction;
const direction = vec3(d.dot(axes[0]), d.dot(axes[1]), d.dot(axes[2]));
return raycastBox(new Ray3D(boxLocal3D(ray.origin, pos, axes), direction), vec3(), size);
}
const {origin, direction} = ray;
const h = size.scale(.5);
const boxMin = pos.subtract(h), boxMax = pos.add(h);
let tMin = 0, tMax = Infinity;
for (const axis of 'xyz')
{
const o = origin[axis], d = direction[axis];
const mn = boxMin[axis], mx = boxMax[axis];
if (!d)
{
if (o < mn || o > mx)
return undefined; // ray is parallel to this slab and outside it
continue;
}
let t0 = (mn - o)/d;
let t1 = (mx - o)/d;
if (t0 > t1)
[t0, t1] = [t1, t0];
tMin = max(tMin, t0);
tMax = min(tMax, t1);
if (tMin > tMax)
return undefined;
}
return tMin;
}