NavMesh: Improve pathfinding

build/yuka.js

@@ -2747,9 +2747,9 @@
 
 			this._buildRegions( sortedEdgeList );
 
-			// compute centroids of convex regions
+			// ensure unique node indices for all twin edges
 
-			this._buildCentrois();
+			this._buildNodeIndices();
 
 			// now build the navigation graph
 
@@ -2774,7 +2774,9 @@
 			let closesNodeIndex = null;
 			let minDistance = Infinity;
 
-			const nodes = graph.getNodes();
+			const nodes = [];
+
+			graph.getNodes( nodes );
 
 			for ( const node of nodes ) {
 
@@ -2803,13 +2805,17 @@
 
 			do {
 
-				const distance = point.squaredDistanceTo( edge.from() );
+				if ( edge.twin || edge.prev.twin ) {
 
-				if ( distance < minDistance ) {
+					const distance = point.squaredDistanceTo( edge.from() );
 
-					minDistance = distance;
+					if ( distance < minDistance ) {
+
+						minDistance = distance;
 
-					closesNodeIndex = edge.nodeIndex;
+						closesNodeIndex = edge.twin ? edge.nodeIndex : edge.prev.twin.nodeIndex;
+
+					}
 
 				}
 
@@ -2872,7 +2878,7 @@
 
 			const path = [];
 
-			// 1. ensure start and end point are inside of navmesh
+			// 1. ensure start and end point are inside the navmesh
 
 			if ( fromRegion === null || toRegion === null ) {
 
@@ -2906,6 +2912,50 @@
 
 					const shortestPath = astar.getPath();
 
+					let count = shortestPath.length;
+
+					// smoothing
+
+					for ( let i = 0, l = shortestPath.length; i < l; i ++ ) {
+
+						const index = shortestPath[ i ];
+						const node = graph.getNode( index );
+
+						if ( fromRegion.contains( node.position ) === false ) {
+
+							count = i;
+							break;
+
+						}
+
+					}
+
+					shortestPath.splice( 0, count - 1 );
+
+					//
+
+					shortestPath.reverse();
+
+					count = shortestPath.length;
+
+					for ( let i = 0, l = shortestPath.length; i < l; i ++ ) {
+
+						const index = shortestPath[ i ];
+						const node = graph.getNode( index );
+
+						if ( toRegion.contains( node.position ) === false ) {
+
+							count = i;
+							break;
+
+						}
+
+					}
+
+					shortestPath.splice( 0, count - 1 );
+
+					shortestPath.reverse();
+
 					// create final path
 
 					path.push( new Vector3().copy( from ) );
@@ -2994,83 +3044,106 @@
 
 			}
 
+			//
+
+			for ( const region of regions ) {
+
+				region.computeCentroid();
+
+			}
+
 		}
 
-		_buildCentrois() {
+		_buildNodeIndices() {
 
 			const regions = this.regions;
 
+			const indicesMap = new Map();
+			let nextNodeIndex = 0;
+
 			for ( const region of regions ) {
 
 				let edge = region.edge;
-				let count = 0;
-
-				region.centroid.set( 0, 0, 0 );
 
 				do {
 
-					region.centroid.add( edge.from() );
+					// only edges with a twin reference needs to be considered
+
+					if ( edge.twin !== null ) {
+
+						let nodeIndex = - 1;
+						const position = edge.from();
+
+						// check all existing entries
+
+						for ( const [ index, pos ] of indicesMap.entries() ) {
+
+							if ( position.equals( pos ) === true ) {
+
+								// found, use the existing index
+
+								nodeIndex = index;
+								break;
+
+							}
+
+						}
+
+						// if no suitable index was found, create a new one
+
+						if ( nodeIndex === - 1 ) {
+
+							nodeIndex = nextNodeIndex ++;
+							indicesMap.set( nodeIndex, position );
+
+						}
+
+						// assign unique node index to edge
 
-					count ++;
+						edge.nodeIndex = nodeIndex;
+
+					}
 
 					edge = edge.next;
 
 				} while ( edge !== region.edge );
 
-				region.centroid.divideScalar( count );
-
 			}
 
-			return this;
-
 		}
 
 		_buildGraph() {
 
 			const graph = this.graph;
 			const regions = this.regions;
-			let nextNodeIndex = 0;
 
-			// generate unique navigation nodes
+			// for each region, the code creates an array of directly accessible node indices
+
+			const nodeIndicesPerRegion = new Set();
 
 			for ( const region of regions ) {
 
+				const nodeIndices = new Array();
+				nodeIndicesPerRegion.add( nodeIndices );
+
 				let edge = region.edge;
 
 				do {
 
-					let nodeIndex = - 1;
-					const position = edge.from();
-
-					// check all existing nodes
+					if ( edge.twin !== null ) {
 
-					for ( const [ index, node ] of graph._nodes.entries() ) {
+						nodeIndices.push( edge.nodeIndex, edge.twin.nodeIndex );
 
-						if ( position.equals( node.position ) === true ) {
+						// add node to graph if necessary
 
-							// found, use the existing node
+						if ( graph.hasNode( edge.nodeIndex ) === false ) {
 
-							nodeIndex = index;
-							break;
+							graph.addNode( new NavNode( edge.nodeIndex, edge.from() ) );
 
 						}
 
 					}
 
-					// if no existing node was found, create a new one
-
-					if ( nodeIndex === - 1 ) {
-
-						nodeIndex = nextNodeIndex ++;
-
-						graph.addNode( new NavNode( nodeIndex, position ) );
-
-					}
-
-					// assign unique index to edge
-
-					edge.nodeIndex = nodeIndex;
-
 					edge = edge.next;
 
 				} while ( edge !== region.edge );
@@ -3079,30 +3152,30 @@
 
 			// add navigation edges
 
-			for ( const region of regions ) {
+			for ( const indices of nodeIndicesPerRegion ) {
 
-				let edge = region.edge;
+				for ( const from of indices ) {
 
-				do {
+					for ( const to of indices ) {
 
-					const from = edge.nodeIndex;
-					const to = edge.next.nodeIndex;
-					const cost = edge.length();
+						if ( from !== to ) {
 
-					graph.addEdge( new NavEdge( from, to, cost ) );
+							if ( graph.hasEdge( from, to ) === false ) {
 
-					// since it's a directed graph, the code creates the opponent navigation edge
-					// if there is no twin defined
+								const nodeFrom = graph.getNode( from );
+								const nodeTo = graph.getNode( to );
 
-					if ( edge.twin === null ) {
+								const cost = nodeFrom.position.distanceTo( nodeTo.position );
 
-						graph.addEdge( new NavEdge( to, from, cost ) );
+								graph.addEdge( new NavEdge( from, to, cost ) );
 
-					}
+							}
 
-					edge = edge.next;
+						}
 
-				} while ( edge !== region.edge );
+					}
+
+				}
 
 			}
 
@@ -3227,6 +3300,30 @@
 
 		}
 
+		computeCentroid() {
+
+			const centroid = this.centroid;
+			let edge = this.edge;
+			let count = 0;
+
+			centroid.set( 0, 0, 0 );
+
+			do {
+
+				centroid.add( edge.from() );
+
+				count ++;
+
+				edge = edge.next;
+
+			} while ( edge !== this.edge );
+
+			centroid.divideScalar( count );
+
+			return this;
+
+		}
+
 		convex() {
 
 			let edge = this.edge;