diff --git a/example/testToRemove.html b/example/testToRemove.html new file mode 100644 index 000000000..49a724dd1 --- /dev/null +++ b/example/testToRemove.html @@ -0,0 +1,23 @@ + + + + three-mesh-bvh - Complex Geometry Raycasting + + + + + + + + diff --git a/example/testToRemove.js b/example/testToRemove.js new file mode 100644 index 000000000..b70f849fb --- /dev/null +++ b/example/testToRemove.js @@ -0,0 +1,127 @@ +import * as THREE from 'three'; +import { computeBoundsTree, CENTER } from '../src'; + +THREE.BufferGeometry.prototype.computeBoundsTree = computeBoundsTree; + +class PRNG { + + constructor( seed ) { + + this._seed = seed; + + } + + next() { + + let t = ( this._seed += 0x6d2b79f5 ); + t = Math.imul( t ^ ( t >>> 15 ), t | 1 ); + t ^= t + Math.imul( t ^ ( t >>> 7 ), t | 61 ); + return ( ( t ^ ( t >>> 14 ) ) >>> 0 ) / 4294967296; + + } + + range( min, max ) { + + return min + ( max - min ) * this.next(); + + } + +} + + +const maxSpawnPointRadius = 2; +const maxLeafTris = 4; +const strategy = CENTER; + +const tries = 1000; +const seed = 123456; + +const radius = 10; // if radius 100 and tube 0.1 and spawnRadius 100, sort works really good. +const tube = 0.1; +const segmentsMultiplier = 32; + +// const geometry = new THREE.SphereGeometry( radius, 8 * segmentsMultiplier, 4 * segmentsMultiplier ); +const geometry = new THREE.TorusKnotGeometry( radius, tube, 64 * segmentsMultiplier, 8 * segmentsMultiplier ); + +geometry.computeBoundsTree( { maxLeafTris, strategy } ); + +geometry.computeBoundsTree( { maxLeafTris, strategy } ); + +const bvh = geometry.boundsTree; +const target = {}; + +const r = new PRNG( seed ); +const points = new Array( tries ); + +function generatePoints() { + + for ( let i = 0; i < tries; i ++ ) { + + points[ i ] = new THREE.Vector3( r.range( - maxSpawnPointRadius, maxSpawnPointRadius ), r.range( - maxSpawnPointRadius, maxSpawnPointRadius ), r.range( - maxSpawnPointRadius, maxSpawnPointRadius ) ); + + } + +} + + +// TEST EQUALS RESULTS + +// generatePoints(); +// const target2 = {}; +// for ( let i = 0; i < tries; i ++ ) { + +// bvh.closestPointToPoint( points[ i ], target ); +// bvh.closestPointToPointHybrid( points[ i ], target2 ); + +// if ( target.distance !== target2.distance ) { + +// const diff = target.distance - target2.distance; +// console.error( "error: " + ( diff / target2.distance * 100 ) + "%" ); + +// } + +// } + +// TEST PERFORMANCE + +function benchmark() { + + generatePoints(); + + const startOld = performance.now(); + + for ( let i = 0; i < tries; i ++ ) { + + bvh.closestPointToPointOld( points[ i ], target ); + + } + + const endOld = performance.now() - startOld; + const startNew = performance.now(); + + for ( let i = 0; i < tries; i ++ ) { + + bvh.closestPointToPoint( points[ i ], target ); + + } + + const endNew = performance.now() - startNew; + const startSort = performance.now(); + + for ( let i = 0; i < tries; i ++ ) { + + bvh.closestPointToPointSort( points[ i ], target ); + + } + + const endSort = performance.now() - startSort; + + const bestEnd = Math.min( endSort, endNew ); + const best = bestEnd === endSort ? "Sorted" : "New"; + + console.log( `New: ${endNew.toFixed( 1 )}ms / Sorted: ${endSort.toFixed( 1 )}ms / Old: ${endOld.toFixed( 1 )}ms / Diff: ${( ( 1 - ( endOld / bestEnd ) ) * 100 ).toFixed( 2 )} % / Best: ${best}` ); + +} + +benchmark(); +setInterval( () => benchmark(), 2000 ); diff --git a/src/core/MeshBVH.js b/src/core/MeshBVH.js index 95ac647c5..92c1bb1dc 100644 --- a/src/core/MeshBVH.js +++ b/src/core/MeshBVH.js @@ -5,7 +5,9 @@ import { OrientedBox } from '../math/OrientedBox.js'; import { arrayToBox } from '../utils/ArrayBoxUtilities.js'; import { ExtendedTrianglePool } from '../utils/ExtendedTrianglePool.js'; import { shapecast } from './cast/shapecast.js'; -import { closestPointToPoint } from './cast/closestPointToPoint.js'; +import { closestPointToPoint } from './cast/closestPointToPointNew.js'; +import { closestPointToPointSort } from './cast/closestPointToPointSort.js'; +import { closestPointToPointOld } from './cast/closestPointToPoint.js'; // REMOVE AFTER TEST import { iterateOverTriangles } from './utils/iterationUtils.generated.js'; import { refit } from './cast/refit.generated.js'; @@ -519,7 +521,60 @@ export class MeshBVH { closestPointToPoint( point, target = { }, minThreshold = 0, maxThreshold = Infinity ) { - return closestPointToPoint( + const roots = this._roots; + let result = null; + + for ( let i = 0, l = roots.length; i < l; i ++ ) { + + result = closestPointToPoint( + this, + i, + point, + target, + minThreshold, + maxThreshold, + ); + + // fix here, check old result and new + + if ( result && result.distance <= minThreshold ) break; + + } + + return result; + + } + + closestPointToPointSort( point, target = { }, minThreshold = 0, maxThreshold = Infinity ) { + + const roots = this._roots; + let result = null; + + for ( let i = 0, l = roots.length; i < l; i ++ ) { + + result = closestPointToPointSort( + this, + i, + point, + target, + minThreshold, + maxThreshold, + ); + + // fix here, check old result and new + + if ( result && result.distance <= minThreshold ) break; + + } + + return result; + + } + + // REMOVE AFTER TEST + closestPointToPointOld( point, target = { }, minThreshold = 0, maxThreshold = Infinity ) { + + return closestPointToPointOld( this, point, target, diff --git a/src/core/cast/closestPointToPoint.js b/src/core/cast/closestPointToPoint.js index c5ae1a462..6f72a8ad3 100644 --- a/src/core/cast/closestPointToPoint.js +++ b/src/core/cast/closestPointToPoint.js @@ -1,9 +1,11 @@ import { Vector3 } from 'three'; +// DELETE THIS AFTER TEST + const temp = /* @__PURE__ */ new Vector3(); const temp1 = /* @__PURE__ */ new Vector3(); -export function closestPointToPoint( +export function closestPointToPointOld( bvh, point, target = { }, @@ -48,15 +50,7 @@ export function closestPointToPoint( } - if ( distSq < minThresholdSq ) { - - return true; - - } else { - - return false; - - } + return distSq < minThresholdSq; }, diff --git a/src/core/cast/closestPointToPointNew.js b/src/core/cast/closestPointToPointNew.js new file mode 100644 index 000000000..675f0c07d --- /dev/null +++ b/src/core/cast/closestPointToPointNew.js @@ -0,0 +1,109 @@ +import { Vector3 } from 'three'; +import { ExtendedTrianglePool } from '../../utils/ExtendedTrianglePool.js'; +import { BufferStack } from '../utils/BufferStack.js'; +import { closestDistanceSquaredPointToBox } from '../utils/distanceUtils.js'; +import { iterateOverTriangles } from '../utils/iterationUtils.generated.js'; +import { iterateOverTriangles_indirect } from '../utils/iterationUtils_indirect.generated.js'; +import { COUNT, IS_LEAF, LEFT_NODE, OFFSET, RIGHT_NODE } from '../utils/nodeBufferUtils.js'; + +const temp = /* @__PURE__ */ new Vector3(); +const temp1 = /* @__PURE__ */ new Vector3(); + +export function closestPointToPoint/* @echo INDIRECT_STRING */( + bvh, + root, + point, + target, + minThreshold, + maxThreshold +) { + + const minThresholdSq = minThreshold * minThreshold; + const maxThresholdSq = maxThreshold * maxThreshold; + let closestDistanceSq = Infinity; + let closestDistanceTriIndex = null; + + const triangle = ExtendedTrianglePool.getPrimitive(); + + const iterateOverTrianglesFunc = bvh.indirect ? iterateOverTriangles_indirect : iterateOverTriangles; + + BufferStack.setBuffer( bvh._roots[ root ] ); + const { float32Array, uint16Array, uint32Array } = BufferStack; + + _closestPointToPoint( root ); + + BufferStack.clearBuffer(); + ExtendedTrianglePool.releasePrimitive( triangle ); + + if ( closestDistanceSq === Infinity ) return null; + + const closestDistance = Math.sqrt( closestDistanceSq ); + + if ( ! target.point ) target.point = temp1.clone(); + else target.point.copy( temp1 ); + target.distance = closestDistance; + target.faceIndex = closestDistanceTriIndex; + + return target; + + + // early out if under minThreshold + // skip checking if over maxThreshold + // set minThreshold = maxThreshold to quickly check if a point is within a threshold + // returns Infinity if no value found + function _closestPointToPoint( nodeIndex32 ) { + + const nodeIndex16 = nodeIndex32 * 2; + const isLeaf = IS_LEAF( nodeIndex16, uint16Array ); + if ( isLeaf ) { + + const offset = OFFSET( nodeIndex32, uint32Array ); + const count = COUNT( nodeIndex16, uint16Array ); + + return iterateOverTrianglesFunc( offset, count, bvh, intersectTriangle, null, null, triangle ); + + } + + const leftIndex = LEFT_NODE( nodeIndex32 ); + const rightIndex = RIGHT_NODE( nodeIndex32, uint32Array ); + + const leftDistance = closestDistanceSquaredPointToBox( leftIndex, float32Array, point ); + const rightDistance = closestDistanceSquaredPointToBox( rightIndex, float32Array, point ); + + if ( leftDistance <= rightDistance ) { + + if ( leftDistance < closestDistanceSq && leftDistance < maxThresholdSq ) { + + if ( _closestPointToPoint( leftIndex ) ) return true; + if ( rightDistance < closestDistanceSq ) return _closestPointToPoint( rightIndex ); + + } + + } else if ( rightDistance < closestDistanceSq && rightDistance < maxThresholdSq ) { + + if ( _closestPointToPoint( rightIndex ) ) return true; + if ( leftDistance < closestDistanceSq ) return _closestPointToPoint( leftIndex ); + + } + + return false; + + } + + function intersectTriangle( triangle, triIndex ) { + + triangle.closestPointToPoint( point, temp ); + const distSq = point.distanceToSquared( temp ); + if ( distSq < closestDistanceSq ) { + + temp1.copy( temp ); + closestDistanceSq = distSq; + closestDistanceTriIndex = triIndex; + + } + + return distSq < minThresholdSq; + + } + +} diff --git a/src/core/cast/closestPointToPointSort.js b/src/core/cast/closestPointToPointSort.js new file mode 100644 index 000000000..2cc35a9ff --- /dev/null +++ b/src/core/cast/closestPointToPointSort.js @@ -0,0 +1,159 @@ +import { Vector3 } from 'three'; +import { ExtendedTrianglePool } from '../../utils/ExtendedTrianglePool.js'; +import { BufferStack } from '../utils/BufferStack.js'; +import { iterateOverTriangles } from '../utils/iterationUtils.generated.js'; +import { iterateOverTriangles_indirect } from '../utils/iterationUtils_indirect.generated.js'; +import { closestDistanceSquaredPointToBox } from '../utils/distanceUtils.js'; +import { MinHeap } from '../utils/minHeap.js'; +import { COUNT, IS_LEAF, LEFT_NODE, OFFSET, RIGHT_NODE } from '../utils/nodeBufferUtils.js'; + +const temp = /* @__PURE__ */ new Vector3(); +const temp1 = /* @__PURE__ */ new Vector3(); +const minHeap = new MinHeap(); +// const heapQueue = new HeapQueue(); + +export function closestPointToPointSort/* @echo INDIRECT_STRING */( + bvh, + root, + point, + target, + minThreshold, + maxThreshold +) { + + const minThresholdSq = minThreshold * minThreshold; + const maxThresholdSq = maxThreshold * maxThreshold; + let closestDistanceSq = Infinity; + let closestDistanceTriIndex = null; + + const triangle = ExtendedTrianglePool.getPrimitive(); + + const iterateOverTrianglesFunc = bvh.indirect ? iterateOverTriangles_indirect : iterateOverTriangles; + + BufferStack.setBuffer( bvh._roots[ root ] ); + const { float32Array, uint16Array, uint32Array } = BufferStack; + // heapQueue.reset(); + + _closestPointToPoint( { nodeIndex32: 0, distance: closestDistanceSquaredPointToBox( 0, float32Array, point ) } ); + + BufferStack.clearBuffer(); + ExtendedTrianglePool.releasePrimitive( triangle ); + + if ( closestDistanceSq === Infinity ) return null; + + const closestDistance = Math.sqrt( closestDistanceSq ); + + if ( ! target.point ) target.point = temp1.clone(); + else target.point.copy( temp1 ); + target.distance = closestDistance; + target.faceIndex = closestDistanceTriIndex; + + return target; + + + function _closestPointToPoint( node ) { + + // const minHeap = heapQueue.getMinHeap(); + minHeap.clear(); + + do { + + const { distance, nodeIndex32 } = node; + + if ( distance >= closestDistanceSq ) return; + + const nodeIndex16 = nodeIndex32 * 2; + const isLeaf = IS_LEAF( nodeIndex16, uint16Array ); + if ( isLeaf ) { + + const offset = OFFSET( nodeIndex32, uint32Array ); + const count = COUNT( nodeIndex16, uint16Array ); + if ( iterateOverTrianglesFunc( offset, count, bvh, intersectTriangle, null, null, triangle ) ) return true; + + } else if ( minHeap.isFull() ) { + + _closestPointToPointRecursive( nodeIndex32 ); + // or we can use _closestPointToPoint( node ) if we want to use minHeap again; + + } else { + + const leftIndex = LEFT_NODE( nodeIndex32 ); + const rightIndex = RIGHT_NODE( nodeIndex32, uint32Array ); + + const leftDistance = closestDistanceSquaredPointToBox( leftIndex, float32Array, point ); + const rightDistance = closestDistanceSquaredPointToBox( rightIndex, float32Array, point ); + + if ( leftDistance < closestDistanceSq && leftDistance < maxThresholdSq ) { + + minHeap.add( { nodeIndex32: leftIndex, distance: leftDistance } ); + + } + + if ( rightDistance < closestDistanceSq && rightDistance < maxThresholdSq ) { + + minHeap.add( { nodeIndex32: rightIndex, distance: rightDistance } ); + + } + + } + + } while ( ( node = minHeap.poll() ) ); + + } + + + function _closestPointToPointRecursive( nodeIndex32 ) { + + const nodeIndex16 = nodeIndex32 * 2; + const isLeaf = IS_LEAF( nodeIndex16, uint16Array ); + if ( isLeaf ) { + + const offset = OFFSET( nodeIndex32, uint32Array ); + const count = COUNT( nodeIndex16, uint16Array ); + return iterateOverTrianglesFunc( offset, count, bvh, intersectTriangle, null, null, triangle ); + + } else { + + const leftIndex = LEFT_NODE( nodeIndex32 ); + const rightIndex = RIGHT_NODE( nodeIndex32, uint32Array ); + + const leftDistance = closestDistanceSquaredPointToBox( leftIndex, float32Array, point ); + const rightDistance = closestDistanceSquaredPointToBox( rightIndex, float32Array, point ); + + if ( leftDistance <= rightDistance ) { + + if ( leftDistance < closestDistanceSq && leftDistance < maxThresholdSq ) { + + if ( _closestPointToPointRecursive( leftIndex ) ) return true; + if ( rightDistance < closestDistanceSq ) return _closestPointToPointRecursive( rightIndex ); + + } + + } else if ( rightDistance < closestDistanceSq && rightDistance < maxThresholdSq ) { + + if ( _closestPointToPointRecursive( rightIndex ) ) return true; + if ( leftDistance < closestDistanceSq ) return _closestPointToPointRecursive( leftIndex ); + + } + + } + + } + + function intersectTriangle( triangle, triIndex ) { + + triangle.closestPointToPoint( point, temp ); + const distSq = point.distanceToSquared( temp ); + if ( distSq < closestDistanceSq ) { + + temp1.copy( temp ); + closestDistanceSq = distSq; + closestDistanceTriIndex = triIndex; + + } + + return distSq < minThresholdSq; + + } + +} diff --git a/src/core/cast/closestPointToPointSort.template_likePQP.js b/src/core/cast/closestPointToPointSort.template_likePQP.js new file mode 100644 index 000000000..3a66784b7 --- /dev/null +++ b/src/core/cast/closestPointToPointSort.template_likePQP.js @@ -0,0 +1,180 @@ +import { Vector3 } from 'three'; +import { COUNT, OFFSET, LEFT_NODE, RIGHT_NODE, IS_LEAF } from '../utils/nodeBufferUtils.js'; +import { BufferStack } from '../utils/BufferStack.js'; +import { ExtendedTrianglePool } from '../../utils/ExtendedTrianglePool.js'; +import { setTriangle } from '../../utils/TriangleUtilities.js'; +import { closestDistanceSquaredPointToBox } from '../utils/distanceUtils.js'; +import { HeapQueue } from '../utils/heapQueue.js'; + +const temp = /* @__PURE__ */ new Vector3(); +const temp1 = /* @__PURE__ */ new Vector3(); +const heapQueue = new HeapQueue(); + +export function closestPointToPointSort/* @echo INDIRECT_STRING */( + bvh, + root, + point, + target, + minThreshold, + maxThreshold +) { + + const minThresholdSq = minThreshold * minThreshold; + const maxThresholdSq = maxThreshold * maxThreshold; + let closestDistanceSq = Infinity; + let closestDistanceTriIndex = null; + BufferStack.setBuffer( bvh._roots[ root ] ); + + const { geometry } = bvh; + const { index } = geometry; + const pos = geometry.attributes.position; + const triangle = ExtendedTrianglePool.getPrimitive(); + const { float32Array, uint16Array, uint32Array } = BufferStack; + heapQueue.reset(); + + _closestPointToPoint( { nodeIndex32: 0, distance: closestDistanceSquaredPointToBox( 0, float32Array, point ) } ); + + BufferStack.clearBuffer(); + + if ( closestDistanceSq === Infinity ) return null; + + const closestDistance = Math.sqrt( closestDistanceSq ); + + if ( ! target.point ) target.point = temp1.clone(); + else target.point.copy( temp1 ); + target.distance = closestDistance; + target.faceIndex = closestDistanceTriIndex; + + return target; + + + function _closestPointToPoint( node ) { + + const minHeap = heapQueue.getMinHeap(); + + do { + + const { distance, nodeIndex32 } = node; + + if ( distance >= closestDistanceSq ) return; + + const leftIndex = LEFT_NODE( nodeIndex32 ); + const rightIndex = RIGHT_NODE( nodeIndex32, uint32Array ); + + const isLeftLeaf = IS_LEAF( leftIndex * 2, uint16Array ); + const isRightLeaf = IS_LEAF( rightIndex * 2, uint16Array ); + + if ( isLeftLeaf && isRightLeaf ) { + + const leftDistance = closestDistanceSquaredPointToBox( leftIndex, float32Array, point ); + const rightDistance = closestDistanceSquaredPointToBox( rightIndex, float32Array, point ); + + if ( leftDistance < rightDistance ) { + + test( leftIndex ); + if ( rightDistance >= closestDistanceSq ) continue; + test( rightIndex ); + + } else { + + test( rightIndex ); + if ( leftDistance >= closestDistanceSq ) continue; + test( leftIndex ); + + } + + } else if ( minHeap.isFull() ) { // secondo me andrebbe sopra + + _closestPointToPoint( node ); + + } else { + + if ( isLeftLeaf ) { + + test( leftIndex ); // fare solo se distanza minore? + + const rightDistance = closestDistanceSquaredPointToBox( rightIndex, float32Array, point ); + + if ( rightDistance < closestDistanceSq && rightDistance < maxThresholdSq ) { + + minHeap.add( { nodeIndex32: rightIndex, distance: rightDistance } ); + + } + + } else if ( isRightLeaf ) { + + test( rightIndex ); // fare solo se distanza minore? + + const leftDistance = closestDistanceSquaredPointToBox( leftIndex, float32Array, point ); + + if ( leftDistance < closestDistanceSq && leftDistance < maxThresholdSq ) { + + minHeap.add( { nodeIndex32: leftIndex, distance: leftDistance } ); + + } + + } else { + + const leftDistance = closestDistanceSquaredPointToBox( leftIndex, float32Array, point ); + const rightDistance = closestDistanceSquaredPointToBox( rightIndex, float32Array, point ); + + if ( leftDistance < closestDistanceSq && leftDistance < maxThresholdSq ) { + + minHeap.add( { nodeIndex32: leftIndex, distance: leftDistance } ); + + } + + if ( rightDistance < closestDistanceSq && rightDistance < maxThresholdSq ) { + + minHeap.add( { nodeIndex32: rightIndex, distance: rightDistance } ); + + } + + } + + } + + } while ( ( node = minHeap.poll() ) ); + + } + + + function test( nodeIndex32 ) { + + const nodeIndex16 = nodeIndex32 * 2; + + const offset = OFFSET( nodeIndex32, uint32Array ); + const count = COUNT( nodeIndex16, uint16Array ); + + for ( let i = offset, l = count + offset; i < l; i ++ ) { + + /* @if INDIRECT */ + + const ti = bvh.resolveTriangleIndex( i ); + setTriangle( triangle, 3 * ti, index, pos ); + + /* @else */ + + setTriangle( triangle, i * 3, index, pos ); + + /* @endif */ + + triangle.needsUpdate = true; + + triangle.closestPointToPoint( point, temp ); + const distSq = point.distanceToSquared( temp ); + if ( distSq < closestDistanceSq ) { + + temp1.copy( temp ); + closestDistanceSq = distSq; + closestDistanceTriIndex = i; + + if ( distSq < minThresholdSq ) return; + + } + + } + + } + +} diff --git a/src/core/cast/shapecast.js b/src/core/cast/shapecast.js index 15eb0fdb0..2b01b6a5d 100644 --- a/src/core/cast/shapecast.js +++ b/src/core/cast/shapecast.js @@ -9,7 +9,7 @@ let _box1, _box2; const boxStack = []; const boxPool = /* @__PURE__ */ new PrimitivePool( () => new Box3() ); -export function shapecast( bvh, root, intersectsBounds, intersectsRange, boundsTraverseOrder, byteOffset ) { +export function shapecast( bvh, root, intersectsBoundsFunc, intersectsRangeFunc, nodeScoreFunc, byteOffset ) { // setup _box1 = boxPool.getPrimitive(); @@ -17,7 +17,9 @@ export function shapecast( bvh, root, intersectsBounds, intersectsRange, boundsT boxStack.push( _box1, _box2 ); BufferStack.setBuffer( bvh._roots[ root ] ); - const result = shapecastTraverse( 0, bvh.geometry, intersectsBounds, intersectsRange, boundsTraverseOrder, byteOffset ); + const { float32Array, uint16Array, uint32Array } = BufferStack; + + const result = shapecastTraverse( 0, byteOffset, 0 ); // cleanup BufferStack.clearBuffer(); @@ -36,177 +38,159 @@ export function shapecast( bvh, root, intersectsBounds, intersectsRange, boundsT return result; -} + function shapecastTraverse( + nodeIndex32, + nodeIndexByteOffset, // offset for unique node identifier + depth + ) { -function shapecastTraverse( - nodeIndex32, - geometry, - intersectsBoundsFunc, - intersectsRangeFunc, - nodeScoreFunc = null, - nodeIndexByteOffset = 0, // offset for unique node identifier - depth = 0 -) { + let nodeIndex16 = nodeIndex32 * 2; - const { float32Array, uint16Array, uint32Array } = BufferStack; - let nodeIndex16 = nodeIndex32 * 2; + const isLeaf = IS_LEAF( nodeIndex16, uint16Array ); + if ( isLeaf ) { - const isLeaf = IS_LEAF( nodeIndex16, uint16Array ); - if ( isLeaf ) { + const offset = OFFSET( nodeIndex32, uint32Array ); + const count = COUNT( nodeIndex16, uint16Array ); + arrayToBox( BOUNDING_DATA_INDEX( nodeIndex32 ), float32Array, _box1 ); // never used + return intersectsRangeFunc( offset, count, false, depth, nodeIndexByteOffset + nodeIndex32, _box1 ); - const offset = OFFSET( nodeIndex32, uint32Array ); - const count = COUNT( nodeIndex16, uint16Array ); - arrayToBox( BOUNDING_DATA_INDEX( nodeIndex32 ), float32Array, _box1 ); - return intersectsRangeFunc( offset, count, false, depth, nodeIndexByteOffset + nodeIndex32, _box1 ); + } else { - } else { + const left = LEFT_NODE( nodeIndex32 ); + const right = RIGHT_NODE( nodeIndex32, uint32Array ); + let c1 = left; + let c2 = right; - const left = LEFT_NODE( nodeIndex32 ); - const right = RIGHT_NODE( nodeIndex32, uint32Array ); - let c1 = left; - let c2 = right; + let score1, score2; + let box1, box2; + if ( nodeScoreFunc ) { - let score1, score2; - let box1, box2; - if ( nodeScoreFunc ) { + box1 = _box1; + box2 = _box2; - box1 = _box1; - box2 = _box2; + // bounding data is not offset + arrayToBox( BOUNDING_DATA_INDEX( c1 ), float32Array, box1 ); + arrayToBox( BOUNDING_DATA_INDEX( c2 ), float32Array, box2 ); - // bounding data is not offset - arrayToBox( BOUNDING_DATA_INDEX( c1 ), float32Array, box1 ); - arrayToBox( BOUNDING_DATA_INDEX( c2 ), float32Array, box2 ); + score1 = nodeScoreFunc( box1 ); + score2 = nodeScoreFunc( box2 ); - score1 = nodeScoreFunc( box1 ); - score2 = nodeScoreFunc( box2 ); + if ( score2 < score1 ) { - if ( score2 < score1 ) { + c1 = right; + c2 = left; - c1 = right; - c2 = left; + const temp = score1; + score1 = score2; + score2 = temp; - const temp = score1; - score1 = score2; - score2 = temp; + box1 = box2; + // box2 is always set before use below - box1 = box2; - // box2 is always set before use below + } - } - - } + } else { - // Check box 1 intersection - if ( ! box1 ) { + box1 = _box1; + arrayToBox( BOUNDING_DATA_INDEX( c1 ), float32Array, box1 ); - box1 = _box1; - arrayToBox( BOUNDING_DATA_INDEX( c1 ), float32Array, box1 ); + } - } + const isC1Leaf = IS_LEAF( c1 * 2, uint16Array ); + const c1Intersection = intersectsBoundsFunc( box1, isC1Leaf, score1, depth + 1, nodeIndexByteOffset + c1 ); - const isC1Leaf = IS_LEAF( c1 * 2, uint16Array ); - const c1Intersection = intersectsBoundsFunc( box1, isC1Leaf, score1, depth + 1, nodeIndexByteOffset + c1 ); + let c1StopTraversal; + if ( c1Intersection === CONTAINED ) { - let c1StopTraversal; - if ( c1Intersection === CONTAINED ) { + const offset = getLeftOffset( c1 ); + const end = getRightEndOffset( c1 ); + const count = end - offset; - const offset = getLeftOffset( c1 ); - const end = getRightEndOffset( c1 ); - const count = end - offset; + c1StopTraversal = intersectsRangeFunc( offset, count, true, depth + 1, nodeIndexByteOffset + c1, box1 ); - c1StopTraversal = intersectsRangeFunc( offset, count, true, depth + 1, nodeIndexByteOffset + c1, box1 ); + } else { - } else { - - c1StopTraversal = + c1StopTraversal = c1Intersection && shapecastTraverse( c1, - geometry, - intersectsBoundsFunc, - intersectsRangeFunc, - nodeScoreFunc, nodeIndexByteOffset, depth + 1 ); - } + } - if ( c1StopTraversal ) return true; + if ( c1StopTraversal ) return true; - // Check box 2 intersection - // cached box2 will have been overwritten by previous traversal - box2 = _box2; - arrayToBox( BOUNDING_DATA_INDEX( c2 ), float32Array, box2 ); + // Check box 2 intersection + // cached box2 will have been overwritten by previous traversal + box2 = _box2; + arrayToBox( BOUNDING_DATA_INDEX( c2 ), float32Array, box2 ); - const isC2Leaf = IS_LEAF( c2 * 2, uint16Array ); - const c2Intersection = intersectsBoundsFunc( box2, isC2Leaf, score2, depth + 1, nodeIndexByteOffset + c2 ); + const isC2Leaf = IS_LEAF( c2 * 2, uint16Array ); + const c2Intersection = intersectsBoundsFunc( box2, isC2Leaf, score2, depth + 1, nodeIndexByteOffset + c2 ); - let c2StopTraversal; - if ( c2Intersection === CONTAINED ) { + let c2StopTraversal; + if ( c2Intersection === CONTAINED ) { - const offset = getLeftOffset( c2 ); - const end = getRightEndOffset( c2 ); - const count = end - offset; + const offset = getLeftOffset( c2 ); + const end = getRightEndOffset( c2 ); + const count = end - offset; - c2StopTraversal = intersectsRangeFunc( offset, count, true, depth + 1, nodeIndexByteOffset + c2, box2 ); + c2StopTraversal = intersectsRangeFunc( offset, count, true, depth + 1, nodeIndexByteOffset + c2, box2 ); - } else { + } else { - c2StopTraversal = + c2StopTraversal = c2Intersection && shapecastTraverse( c2, - geometry, - intersectsBoundsFunc, - intersectsRangeFunc, - nodeScoreFunc, nodeIndexByteOffset, depth + 1 ); - } + } + + if ( c2StopTraversal ) return true; - if ( c2StopTraversal ) return true; + return false; - return false; + // Define these inside the function so it has access to the local variables needed + // when converting to the buffer equivalents + function getLeftOffset( nodeIndex32 ) { - // Define these inside the function so it has access to the local variables needed - // when converting to the buffer equivalents - function getLeftOffset( nodeIndex32 ) { + let nodeIndex16 = nodeIndex32 * 2; - const { uint16Array, uint32Array } = BufferStack; - let nodeIndex16 = nodeIndex32 * 2; + // traverse until we find a leaf + while ( ! IS_LEAF( nodeIndex16, uint16Array ) ) { - // traverse until we find a leaf - while ( ! IS_LEAF( nodeIndex16, uint16Array ) ) { + nodeIndex32 = LEFT_NODE( nodeIndex32 ); + nodeIndex16 = nodeIndex32 * 2; - nodeIndex32 = LEFT_NODE( nodeIndex32 ); - nodeIndex16 = nodeIndex32 * 2; + } + + return OFFSET( nodeIndex32, uint32Array ); } - return OFFSET( nodeIndex32, uint32Array ); + function getRightEndOffset( nodeIndex32 ) { - } + let nodeIndex16 = nodeIndex32 * 2; - function getRightEndOffset( nodeIndex32 ) { + // traverse until we find a leaf + while ( ! IS_LEAF( nodeIndex16, uint16Array ) ) { - const { uint16Array, uint32Array } = BufferStack; - let nodeIndex16 = nodeIndex32 * 2; + // adjust offset to point to the right node + nodeIndex32 = RIGHT_NODE( nodeIndex32, uint32Array ); + nodeIndex16 = nodeIndex32 * 2; - // traverse until we find a leaf - while ( ! IS_LEAF( nodeIndex16, uint16Array ) ) { + } - // adjust offset to point to the right node - nodeIndex32 = RIGHT_NODE( nodeIndex32, uint32Array ); - nodeIndex16 = nodeIndex32 * 2; + // return the end offset of the triangle range + return OFFSET( nodeIndex32, uint32Array ) + COUNT( nodeIndex16, uint16Array ); } - // return the end offset of the triangle range - return OFFSET( nodeIndex32, uint32Array ) + COUNT( nodeIndex16, uint16Array ); - } } diff --git a/src/core/cast/shapecast2.js b/src/core/cast/shapecast2.js new file mode 100644 index 000000000..199124d74 --- /dev/null +++ b/src/core/cast/shapecast2.js @@ -0,0 +1,65 @@ +import { BufferStack } from '../utils/BufferStack.js'; +import { COUNT, IS_LEAF, LEFT_NODE, OFFSET, RIGHT_NODE } from '../utils/nodeBufferUtils.js'; + +// test function optimized for score function that not uses box conversion to test performance (5-6% slower, instead of 40% slower) +export function shapecast( bvh, root, intersectsBoundsFunc, intersectsRangeFunc, nodeScoreFunc ) { + + // setup + BufferStack.setBuffer( bvh._roots[ root ] ); + const { float32Array, uint16Array, uint32Array } = BufferStack; + + const result = shapecastTraverse( 0 ); + + // cleanup + BufferStack.clearBuffer(); + + return result; + + function shapecastTraverse( nodeIndex32 ) { + + let nodeIndex16 = nodeIndex32 * 2; + + const isLeaf = IS_LEAF( nodeIndex16, uint16Array ); + if ( isLeaf ) { + + const offset = OFFSET( nodeIndex32, uint32Array ); + const count = COUNT( nodeIndex16, uint16Array ); + return intersectsRangeFunc( offset, count, false ); + + } + + const left = LEFT_NODE( nodeIndex32 ); + const right = RIGHT_NODE( nodeIndex32, uint32Array ); + let c1 = left; + let c2 = right; + + let score1, score2; + + score1 = nodeScoreFunc( c1, float32Array ); + score2 = nodeScoreFunc( c2, float32Array ); + + if ( score2 < score1 ) { + + c1 = right; + c2 = left; + + const temp = score1; + score1 = score2; + score2 = temp; + + } + + + if ( intersectsBoundsFunc( score1 ) ) { + + if ( shapecastTraverse( c1 ) ) return true; + + if ( intersectsBoundsFunc( score2 ) && shapecastTraverse( c2 ) ) return true; + + } + + return false; + + } + +} diff --git a/src/core/utils/SortedListDesc.js b/src/core/utils/SortedListDesc.js new file mode 100644 index 000000000..5ce845a89 --- /dev/null +++ b/src/core/utils/SortedListDesc.js @@ -0,0 +1,47 @@ +export class SortedListDesc { + + constructor() { + + this.array = []; + + } + + clear() { + + this.array.length = 0; + + } + + + push( node ) { + + const index = this.binarySearch( node.distance ); + this.array.splice( index, 0, node ); + + } + + pop() { + + return this.array.pop(); + + } + + binarySearch( value ) { + + const array = this.array; + + let low = 0, high = array.length; + + while ( low < high ) { + + const mid = ( low + high ) >>> 1; + if ( array[ mid ].distance > value ) low = mid + 1; + else high = mid; + + } + + return low; + + } + +} diff --git a/src/core/utils/distanceUtils.js b/src/core/utils/distanceUtils.js new file mode 100644 index 000000000..7b9e8ef7d --- /dev/null +++ b/src/core/utils/distanceUtils.js @@ -0,0 +1,20 @@ +export function closestDistanceSquaredPointToBox( nodeIndex32, array, point ) { + + const xMin = array[ nodeIndex32 + 0 ] - point.x; + const xMax = point.x - array[ nodeIndex32 + 3 ]; + let dx = xMin > xMax ? xMin : xMax; + dx = dx > 0 ? dx : 0; + + const yMin = array[ nodeIndex32 + 1 ] - point.y; + const yMax = point.y - array[ nodeIndex32 + 4 ]; + let dy = yMin > yMax ? yMin : yMax; + dy = dy > 0 ? dy : 0; + + const zMin = array[ nodeIndex32 + 2 ] - point.z; + const zMax = point.z - array[ nodeIndex32 + 5 ]; + let dz = zMin > zMax ? zMin : zMax; + dz = dz > 0 ? dz : 0; + + return dx * dx + dy * dy + dz * dz; + +} diff --git a/src/core/utils/heapQueue.js b/src/core/utils/heapQueue.js new file mode 100644 index 000000000..cf8878c02 --- /dev/null +++ b/src/core/utils/heapQueue.js @@ -0,0 +1,39 @@ +import { MinHeap } from "./minHeap"; + +export class HeapQueue { + + constructor() { + + this.pool = []; + this.count = 0; + + } + + getMinHeap() { + + const pool = this.pool; + const count = this.count; + + if ( count >= pool.length ) { + + const item = new MinHeap(); + pool.push( item ); + this.count ++; + return item; + + } + + const item = pool[ count ]; + this.count ++; + item.clear(); + return item; + + } + + reset() { + + this.count = 0; + + } + +} diff --git a/src/core/utils/minHeap.js b/src/core/utils/minHeap.js new file mode 100644 index 000000000..e0771a5b6 --- /dev/null +++ b/src/core/utils/minHeap.js @@ -0,0 +1,93 @@ +/** + * @reference https://github.com/zrwusa/data-structure-typed/blob/main/src/data-structures/heap/heap.ts + */ +export class MinHeap { + + constructor() { + + this.maxSize = 8; // we should find a good default size + this._elements = []; + + } + + add( element ) { + + this._elements.push( element ); + this._bubbleUp( this._elements.length - 1 ); + + } + + isFull() { + + return this._elements.length >= this.maxSize; + + } + + poll() { + + const elements = this._elements; + if ( elements.length === 0 ) return; + const value = elements[ 0 ]; + const last = elements.pop(); + if ( elements.length ) { + + elements[ 0 ] = last; + this._sinkDown( 0, elements.length >> 1 ); + + } + + return value; + + } + + clear() { + + this._elements.length = 0; + + } + + _bubbleUp( index ) { + + const elements = this._elements; + const element = elements[ index ]; + while ( index > 0 ) { + + const parent = ( index - 1 ) >> 1; + const parentItem = elements[ parent ]; + if ( parentItem.distance <= element.distance ) break; + elements[ index ] = parentItem; + index = parent; + + } + + elements[ index ] = element; + + } + + _sinkDown( index, halfLength ) { + + const elements = this._elements; + const element = elements[ index ]; + while ( index < halfLength ) { + + let left = ( index << 1 ) | 1; + const right = left + 1; + let minItem = elements[ left ]; + if ( right < elements.length && minItem.distance > elements[ right ].distance ) { + + left = right; + minItem = elements[ right ]; + + } + + if ( minItem.distance >= element.distance ) break; + elements[ index ] = minItem; + index = left; + + } + + elements[ index ] = element; + + } + +}