|
|
|
@ -1,145 +1,212 @@
|
|
|
|
|
import * as THREE from 'three';
|
|
|
|
|
import { CoordinateSystem } from '../CoordinateSystem';
|
|
|
|
|
import { Vector3, Matrix4 } from 'three';
|
|
|
|
|
|
|
|
|
|
// Quote from:
|
|
|
|
|
// https://github.com/Mugen87/yume/blob/master/src/javascript/engine/etc/OBB.js
|
|
|
|
|
// 即obb.js(本项目中已存在)
|
|
|
|
|
|
|
|
|
|
// Reference material:
|
|
|
|
|
//https://stackoverflow.com/questions/28499800/oriented-box-intersection-in-threejs
|
|
|
|
|
//http://www.cnblogs.com/iamzhanglei/archive/2012/06/07/2539751.html
|
|
|
|
|
//https://github.com/Mugen87/yume/blob/master/src/javascript/engine/etc/OBB.js
|
|
|
|
|
|
|
|
|
|
export class OBB
|
|
|
|
|
{
|
|
|
|
|
m_CoordinateSystem: CoordinateSystem;
|
|
|
|
|
halfSizes: THREE.Vector3
|
|
|
|
|
constructor(postion?: CoordinateSystem, size?: THREE.Vector3)
|
|
|
|
|
_EPSILON = 1e-3;
|
|
|
|
|
|
|
|
|
|
public center: Vector3;
|
|
|
|
|
|
|
|
|
|
constructor(public ocs: Matrix4, public halfSizes: Vector3)
|
|
|
|
|
{
|
|
|
|
|
this.m_CoordinateSystem = postion || new CoordinateSystem();
|
|
|
|
|
this.halfSizes = size || new THREE.Vector3();
|
|
|
|
|
this.center = halfSizes.clone().applyMatrix4(ocs);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
getCenter(): THREE.Vector3
|
|
|
|
|
intersectsOBB(obb: OBB): boolean
|
|
|
|
|
{
|
|
|
|
|
let xAxisA = new Vector3();
|
|
|
|
|
let yAxisA = new Vector3();
|
|
|
|
|
let zAxisA = new Vector3();
|
|
|
|
|
|
|
|
|
|
let xAxisB = new Vector3();
|
|
|
|
|
let yAxisB = new Vector3();
|
|
|
|
|
let zAxisB = new Vector3();
|
|
|
|
|
|
|
|
|
|
let translation = new Vector3();
|
|
|
|
|
|
|
|
|
|
let vector = new Vector3();
|
|
|
|
|
|
|
|
|
|
let axisA: Vector3[] = [];
|
|
|
|
|
let axisB: Vector3[] = [];
|
|
|
|
|
let rotationMatrix = [[], [], []];
|
|
|
|
|
let rotationMatrixAbs = [[], [], []];
|
|
|
|
|
|
|
|
|
|
let halfSizeA: number, halfSizeB: number;
|
|
|
|
|
let t: number, i: number;
|
|
|
|
|
|
|
|
|
|
// extract each axis
|
|
|
|
|
this.ocs.extractBasis(xAxisA, yAxisA, zAxisA);
|
|
|
|
|
obb.ocs.extractBasis(xAxisB, yAxisB, zAxisB);
|
|
|
|
|
|
|
|
|
|
// push basis vectors into arrays, so you can access them via indices
|
|
|
|
|
axisA.push(xAxisA, yAxisA, zAxisA);
|
|
|
|
|
axisB.push(xAxisB, yAxisB, zAxisB);
|
|
|
|
|
|
|
|
|
|
// get displacement vector
|
|
|
|
|
vector.subVectors(obb.center, this.center);
|
|
|
|
|
|
|
|
|
|
// express the translation vector in the coordinate frame of the current
|
|
|
|
|
// OBB (this)
|
|
|
|
|
for (i = 0; i < 3; i++)
|
|
|
|
|
{
|
|
|
|
|
let v = this.m_CoordinateSystem.Postion.clone();
|
|
|
|
|
v.add(this.m_CoordinateSystem.XAxis.clone().multiplyScalar(this.halfSizes.x * 0.5))
|
|
|
|
|
v.add(this.m_CoordinateSystem.YAxis.clone().multiplyScalar(this.halfSizes.y * 0.5))
|
|
|
|
|
v.add(this.m_CoordinateSystem.ZAxis.clone().multiplyScalar(this.halfSizes.y * 0.5))
|
|
|
|
|
return v;
|
|
|
|
|
translation.setComponent(i, vector.dot(axisA[i]));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// generate a rotation matrix that transforms from world space to the
|
|
|
|
|
// OBB's coordinate space
|
|
|
|
|
for (i = 0; i < 3; i++)
|
|
|
|
|
{
|
|
|
|
|
for (let j = 0; j < 3; j++)
|
|
|
|
|
{
|
|
|
|
|
rotationMatrix[i][j] = axisA[i].dot(axisB[j]);
|
|
|
|
|
rotationMatrixAbs[i][j] = Math.abs(rotationMatrix[i][j]) + this._EPSILON;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
intersectsOBB(obb: OBB)
|
|
|
|
|
|
|
|
|
|
// test the three major axes of this OBB
|
|
|
|
|
for (i = 0; i < 3; i++)
|
|
|
|
|
{
|
|
|
|
|
// assumes the position of each box to be an orthonormal basis
|
|
|
|
|
var pos1 = this.m_CoordinateSystem.getMatrix4()
|
|
|
|
|
var pos2 = obb.m_CoordinateSystem.getMatrix4()
|
|
|
|
|
var center1 = this.getCenter();
|
|
|
|
|
var center2 = obb.getCenter();
|
|
|
|
|
var centerDifference = center1.clone().sub(center2)
|
|
|
|
|
vector.set(rotationMatrixAbs[i][0], rotationMatrixAbs[i][1], rotationMatrixAbs[i][2]);
|
|
|
|
|
|
|
|
|
|
var results = {
|
|
|
|
|
intersects: true,
|
|
|
|
|
resolution: null
|
|
|
|
|
};
|
|
|
|
|
halfSizeA = this.halfSizes.getComponent(i);
|
|
|
|
|
halfSizeB = obb.halfSizes.dot(vector);
|
|
|
|
|
|
|
|
|
|
// broad phase
|
|
|
|
|
var maxDiameter1 = this.halfSizes.length();
|
|
|
|
|
var maxDiameter2 = obb.halfSizes.length();
|
|
|
|
|
if (centerDifference.length() > maxDiameter1 + maxDiameter2)
|
|
|
|
|
if (Math.abs(translation.getComponent(i)) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
results.intersects = false;
|
|
|
|
|
return results;
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// narrow phase
|
|
|
|
|
// test the three major axes of other OBB
|
|
|
|
|
for (i = 0; i < 3; i++)
|
|
|
|
|
{
|
|
|
|
|
vector.set(rotationMatrixAbs[0][i], rotationMatrixAbs[1][i], rotationMatrixAbs[2][i]);
|
|
|
|
|
|
|
|
|
|
// get the axis vectors of the first box
|
|
|
|
|
var ax1 = new THREE.Vector3()
|
|
|
|
|
var ay1 = new THREE.Vector3()
|
|
|
|
|
var az1 = new THREE.Vector3()
|
|
|
|
|
this.m_CoordinateSystem.extractBasis(ax1, ay1, az1);
|
|
|
|
|
// get the axis vectors of the second box
|
|
|
|
|
var ax2 = new THREE.Vector3()
|
|
|
|
|
var ay2 = new THREE.Vector3()
|
|
|
|
|
var az2 = new THREE.Vector3()
|
|
|
|
|
obb.m_CoordinateSystem.extractBasis(ax2, ay2, az2);
|
|
|
|
|
halfSizeA = this.halfSizes.dot(vector);
|
|
|
|
|
halfSizeB = obb.halfSizes.getComponent(i);
|
|
|
|
|
|
|
|
|
|
// keep them in a list
|
|
|
|
|
var axes = [ax1, ay1, az1, ax2, ay2, az2];
|
|
|
|
|
vector.set(rotationMatrix[0][i], rotationMatrix[1][i], rotationMatrix[2][i]);
|
|
|
|
|
t = translation.dot(vector);
|
|
|
|
|
|
|
|
|
|
// get the orientated radii vectors of the first box
|
|
|
|
|
var radX1 = ax1.clone().multiplyScalar(this.halfSizes.x)
|
|
|
|
|
var radY1 = ay1.clone().multiplyScalar(this.halfSizes.y)
|
|
|
|
|
var radZ1 = az1.clone().multiplyScalar(this.halfSizes.z)
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// get the orientated radii vectors of the second box
|
|
|
|
|
var radX2 = ax2.clone().multiplyScalar(obb.halfSizes.x)
|
|
|
|
|
var radY2 = ay2.clone().multiplyScalar(obb.halfSizes.y)
|
|
|
|
|
var radZ2 = az2.clone().multiplyScalar(obb.halfSizes.z)
|
|
|
|
|
// test the 9 different cross-axes
|
|
|
|
|
|
|
|
|
|
var smallestDifference = Infinity;
|
|
|
|
|
// there are 15 axes to check, so loop through all of them until a separation plane is found
|
|
|
|
|
var zeros = new THREE.Vector3()
|
|
|
|
|
for (var i = 0; i < 15; i++)
|
|
|
|
|
{
|
|
|
|
|
var axis: THREE.Vector3;
|
|
|
|
|
// A.x <cross> B.x
|
|
|
|
|
halfSizeA = this.halfSizes.y * rotationMatrixAbs[2][0] + this.halfSizes.z * rotationMatrixAbs[1][0];
|
|
|
|
|
halfSizeB = obb.halfSizes.y * rotationMatrixAbs[0][2] + obb.halfSizes.z * rotationMatrixAbs[0][1];
|
|
|
|
|
|
|
|
|
|
t = translation.z * rotationMatrix[1][0] - translation.y * rotationMatrix[2][0];
|
|
|
|
|
|
|
|
|
|
// the first 6 axes are just the axes of each bounding box
|
|
|
|
|
if (i < 6)
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
axis = axes[i];
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
// the last 9 axes are the cross product of all combinations of the first 6 axes
|
|
|
|
|
else
|
|
|
|
|
|
|
|
|
|
// A.x < cross> B.y
|
|
|
|
|
halfSizeA = this.halfSizes.y * rotationMatrixAbs[2][1] + this.halfSizes.z * rotationMatrixAbs[1][1];
|
|
|
|
|
halfSizeB = obb.halfSizes.x * rotationMatrixAbs[0][2] + obb.halfSizes.z * rotationMatrixAbs[0][0];
|
|
|
|
|
|
|
|
|
|
t = translation.z * rotationMatrix[1][1] - translation.y * rotationMatrix[2][1];
|
|
|
|
|
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
var offset = i - 6;
|
|
|
|
|
var j = Math.floor(offset / 3);
|
|
|
|
|
var k = offset % 3;
|
|
|
|
|
axis = axes[j].clone().cross(axes[k + 3])
|
|
|
|
|
if (axis.equals(zeros))
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// A.x <cross> B.z
|
|
|
|
|
halfSizeA = this.halfSizes.y * rotationMatrixAbs[2][2] + this.halfSizes.z * rotationMatrixAbs[1][2];
|
|
|
|
|
halfSizeB = obb.halfSizes.x * rotationMatrixAbs[0][1] + obb.halfSizes.y * rotationMatrixAbs[0][0];
|
|
|
|
|
|
|
|
|
|
t = translation.z * rotationMatrix[1][2] - translation.y * rotationMatrix[2][2];
|
|
|
|
|
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
// axes must be collinear, ignore
|
|
|
|
|
continue;
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// A.y <cross> B.x
|
|
|
|
|
halfSizeA = this.halfSizes.x * rotationMatrixAbs[2][0] + this.halfSizes.z * rotationMatrixAbs[0][0];
|
|
|
|
|
halfSizeB = obb.halfSizes.y * rotationMatrixAbs[1][2] + obb.halfSizes.z * rotationMatrixAbs[1][1];
|
|
|
|
|
|
|
|
|
|
t = translation.x * rotationMatrix[2][0] - translation.z * rotationMatrix[0][0];
|
|
|
|
|
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// A.y <cross> B.y
|
|
|
|
|
halfSizeA = this.halfSizes.x * rotationMatrixAbs[2][1] + this.halfSizes.z * rotationMatrixAbs[0][1];
|
|
|
|
|
halfSizeB = obb.halfSizes.x * rotationMatrixAbs[1][2] + obb.halfSizes.z * rotationMatrixAbs[1][0];
|
|
|
|
|
|
|
|
|
|
// get the projections of the first half box onto the axis
|
|
|
|
|
var projAx1 = Math.abs(radX1.dot(axis));
|
|
|
|
|
var projAy1 = Math.abs(radY1.dot(axis));
|
|
|
|
|
var projAz1 = Math.abs(radZ1.dot(axis));
|
|
|
|
|
t = translation.x * rotationMatrix[2][1] - translation.z * rotationMatrix[0][1];
|
|
|
|
|
|
|
|
|
|
// get the projections of the second half box onto the axis
|
|
|
|
|
var projAx2 = Math.abs(radX2.dot(axis));
|
|
|
|
|
var projAy2 = Math.abs(radY2.dot(axis));
|
|
|
|
|
var projAz2 = Math.abs(radZ2.dot(axis));
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// sum the projections
|
|
|
|
|
var projectionBoxesSum = projAx1 + projAy1 + projAz1 + projAx2 + projAy2 + projAz2;
|
|
|
|
|
// A.y <cross> B.z
|
|
|
|
|
halfSizeA = this.halfSizes.x * rotationMatrixAbs[2][2] + this.halfSizes.z * rotationMatrixAbs[0][2];
|
|
|
|
|
halfSizeB = obb.halfSizes.x * rotationMatrixAbs[1][1] + obb.halfSizes.y * rotationMatrixAbs[1][0];
|
|
|
|
|
|
|
|
|
|
// get the projection of the center difference onto the axis
|
|
|
|
|
var projectionDifference = Math.abs(centerDifference.dot(axis));
|
|
|
|
|
t = translation.x * rotationMatrix[2][2] - translation.z * rotationMatrix[0][2];
|
|
|
|
|
|
|
|
|
|
if (projectionDifference >= projectionBoxesSum * 0.5)
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
// If the projection of the center difference onto the axis is greater
|
|
|
|
|
// than the sum of the box projections, then we found a separating plane!
|
|
|
|
|
// The bounding boxes therefore must not intersect
|
|
|
|
|
results.intersects = false;
|
|
|
|
|
break;
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
|
|
|
|
|
// A.z <cross> B.x
|
|
|
|
|
halfSizeA = this.halfSizes.x * rotationMatrixAbs[1][0] + this.halfSizes.y * rotationMatrixAbs[0][0];
|
|
|
|
|
halfSizeB = obb.halfSizes.y * rotationMatrixAbs[2][2] + obb.halfSizes.z * rotationMatrixAbs[2][1];
|
|
|
|
|
|
|
|
|
|
t = translation.y * rotationMatrix[0][0] - translation.x * rotationMatrix[1][0];
|
|
|
|
|
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
// keep track of the difference, the smallest gives the minimum distance
|
|
|
|
|
// and direction to move the boxes such that they no longer intersect
|
|
|
|
|
var difference = projectionBoxesSum - projectionDifference;
|
|
|
|
|
if (difference < smallestDifference)
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// A.z <cross> B.y
|
|
|
|
|
halfSizeA = this.halfSizes.x * rotationMatrixAbs[1][1] + this.halfSizes.y * rotationMatrixAbs[0][1];
|
|
|
|
|
halfSizeB = obb.halfSizes.x * rotationMatrixAbs[2][2] + obb.halfSizes.z * rotationMatrixAbs[2][0];
|
|
|
|
|
|
|
|
|
|
t = translation.y * rotationMatrix[0][1] - translation.x * rotationMatrix[1][1];
|
|
|
|
|
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
results.resolution = axis.clone().multiplyScalar(difference);
|
|
|
|
|
smallestDifference = difference;
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// A.z <cross> B.z
|
|
|
|
|
halfSizeA = this.halfSizes.x * rotationMatrixAbs[1][2] + this.halfSizes.y * rotationMatrixAbs[0][2];
|
|
|
|
|
halfSizeB = obb.halfSizes.x * rotationMatrixAbs[2][1] + obb.halfSizes.y * rotationMatrixAbs[2][0];
|
|
|
|
|
|
|
|
|
|
t = translation.y * rotationMatrix[0][2] - translation.x * rotationMatrix[1][2];
|
|
|
|
|
|
|
|
|
|
if (Math.abs(t) > halfSizeA + halfSizeB)
|
|
|
|
|
{
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// no separating axis exists, so the two OBB don't intersect
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// could not find a separating plane, they must intersect
|
|
|
|
|
return results;
|
|
|
|
|
};
|
|
|
|
|
// setFromObject(obj: THREE.Mesh): OBB;
|
|
|
|
|
// setFromAABB(aabb: THREE.Box3): OBB;
|
|
|
|
|
|
|
|
|
@ -165,5 +232,3 @@ export class OBB
|
|
|
|
|
// clone(obb: OBB): OBB
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|