Use distance for g score

src/AStar.js

@@ -1,4 +1,4 @@
-import { BinaryHeap } from './BinaryHeap';
+import { BinaryHeap } from './BinaryHeap.js';
 import { Utils } from './Utils.js';
 
 class AStar {
@@ -77,7 +77,7 @@ class AStar {
 
         // The g score is the shortest distance from start to current node.
         // We need to check if the path we have arrived at this neighbour is the shortest one we have seen yet.
-        const gScore = currentNode.g + neighbour.cost;
+        const gScore = currentNode.g + neighbour.cost * this.heuristic(currentNode.centroid, neighbour.centroid);
         const beenVisited = neighbour.visited;
 
         if (!beenVisited || gScore < neighbour.g) {
@@ -106,7 +106,7 @@ class AStar {
   }
 
   static heuristic (pos1, pos2) {
-    return Utils.distanceToSquared(pos1, pos2);
+    return Math.sqrt(Utils.distanceToSquared(pos1, pos2));
   }
 
   static neighbours (graph, node) {

src/Builder.js

@@ -1,6 +1,6 @@
 import { Vector3 } from 'three';
 
-import { Utils } from './Utils';
+import { Utils } from './Utils.js';
 
 class Builder {
   /**

src/Channel.js

@@ -1,4 +1,4 @@
-import { Utils } from './Utils';
+import { Utils } from './Utils.js';
 
 class Channel {
   constructor () {

src/Pathfinding.js

@@ -4,10 +4,10 @@ import {
 	Triangle,
 } from 'three';
 
-import { Utils } from './Utils';
-import { AStar } from './AStar';
-import { Builder } from './Builder';
-import { Channel } from './Channel';
+import { Utils } from './Utils.js';
+import { AStar } from './AStar.js';
+import { Builder } from './Builder.js';
+import { Channel } from './Channel.js';
 
 /**
  * Defines an instance of the pathfinding module, with one or more zones.

src/Utils.js

@@ -1,17 +1,17 @@
-import { BufferAttribute, BufferGeometry } from 'three';
+import { BufferAttribute, BufferGeometry, Vector3 } from 'three';
 
 class Utils {
 
-  static roundNumber (value, decimals) {
+  static roundNumber(value, decimals) {
     const factor = Math.pow(10, decimals);
     return Math.round(value * factor) / factor;
   }
 
-  static sample (list) {
+  static sample(list) {
     return list[Math.floor(Math.random() * list.length)];
   }
 
-  static distanceToSquared (a, b) {
+  static distanceToSquared(a, b) {
 
     var dx = a.x - b.x;
     var dy = a.y - b.y;
@@ -23,13 +23,13 @@ class Utils {
 
   //+ Jonas Raoni Soares Silva
   //@ http://jsfromhell.com/math/is-point-in-poly [rev. #0]
-  static isPointInPoly (poly, pt) {
+  static isPointInPoly(poly, pt) {
     for (var c = false, i = -1, l = poly.length, j = l - 1; ++i < l; j = i)
       ((poly[i].z <= pt.z && pt.z < poly[j].z) || (poly[j].z <= pt.z && pt.z < poly[i].z)) && (pt.x < (poly[j].x - poly[i].x) * (pt.z - poly[i].z) / (poly[j].z - poly[i].z) + poly[i].x) && (c = !c);
     return c;
   }
 
-  static isVectorInPolygon (vector, polygon, vertices) {
+  static isVectorInPolygon(vector, polygon, vertices) {
 
     // reference point will be the centroid of the polygon
     // We need to rotate the vector as well as all the points which the polygon uses
@@ -52,18 +52,42 @@ class Utils {
     return false;
   }
 
-  static triarea2 (a, b, c) {
+  static triarea2(a, b, c) {
     var ax = b.x - a.x;
     var az = b.z - a.z;
     var bx = c.x - a.x;
     var bz = c.z - a.z;
     return bx * az - ax * bz;
   }
 
-  static vequal (a, b) {
+  static vequal(a, b) {
     return this.distanceToSquared(a, b) < 0.00001;
   }
 
+  /**
+   * Find the intersection of line ab projected on line cd (project along negative-y-axis)
+   */
+  static p4intersect(a, b, c, d) {
+    const x1 = a.x, z1 = a.z;
+    const x2 = b.x, z2 = b.z;
+    const x3 = c.x, z3 = c.z;
+    const x4 = d.x, z4 = d.z;
+
+    const denom = (x1 - x2) * (z3 - z4) - (z1 - z2) * (x3 - x4);
+    if (Math.abs(denom) < 1e-9) return null; // parallel or coincident
+
+    // t coefficient on AB
+    const t = ((x1 - x3) * (z3 - z4) - (z1 - z3) * (x3 - x4)) / denom;
+    // u coefficient on CD
+    const u = ((x1 - x3) * (z1 - z2) - (z1 - z3) * (x1 - x2)) / denom;
+
+    const px = x3 + u * (x4 - x3);
+    const pz = z3 + u * (z4 - z3);
+    const py = c.y + u * (d.y - c.y);
+
+    return new Vector3(px, py, pz);
+  }
+
   /**
    * Modified version of BufferGeometryUtils.mergeVertices, ignoring vertex
    * attributes other than position.
@@ -72,15 +96,15 @@ class Utils {
    * @param {number} tolerance
    * @return {THREE.BufferGeometry>}
    */
-  static mergeVertices (geometry, tolerance = 1e-4) {
+  static mergeVertices(geometry, tolerance = 1e-4) {
 
-    tolerance = Math.max( tolerance, Number.EPSILON );
+    tolerance = Math.max(tolerance, Number.EPSILON);
 
     // Generate an index buffer if the geometry doesn't have one, or optimize it
     // if it's already available.
     var hashToIndex = {};
     var indices = geometry.getIndex();
-    var positions = geometry.getAttribute( 'position' );
+    var positions = geometry.getAttribute('position');
     var vertexCount = indices ? indices.count : positions.count;
 
     // Next value for triangle indices.
@@ -90,36 +114,36 @@ class Utils {
     var newPositions = [];
 
     // Convert the error tolerance to an amount of decimal places to truncate to.
-    var decimalShift = Math.log10( 1 / tolerance );
-    var shiftMultiplier = Math.pow( 10, decimalShift );
+    var decimalShift = Math.log10(1 / tolerance);
+    var shiftMultiplier = Math.pow(10, decimalShift);
 
-    for ( var i = 0; i < vertexCount; i ++ ) {
+    for (var i = 0; i < vertexCount; i++) {
 
-      var index = indices ? indices.getX( i ) : i;
+      var index = indices ? indices.getX(i) : i;
 
       // Generate a hash for the vertex attributes at the current index 'i'.
       var hash = '';
 
       // Double tilde truncates the decimal value.
-      hash += `${ ~ ~ ( positions.getX( index ) * shiftMultiplier ) },`;
-      hash += `${ ~ ~ ( positions.getY( index ) * shiftMultiplier ) },`;
-      hash += `${ ~ ~ ( positions.getZ( index ) * shiftMultiplier ) },`;
+      hash += `${~ ~(positions.getX(index) * shiftMultiplier)},`;
+      hash += `${~ ~(positions.getY(index) * shiftMultiplier)},`;
+      hash += `${~ ~(positions.getZ(index) * shiftMultiplier)},`;
 
       // Add another reference to the vertex if it's already
       // used by another index.
-      if ( hash in hashToIndex ) {
+      if (hash in hashToIndex) {
 
-        newIndices.push( hashToIndex[ hash ] );
+        newIndices.push(hashToIndex[hash]);
 
       } else {
 
-        newPositions.push( positions.getX( index ) );
-        newPositions.push( positions.getY( index ) );
-        newPositions.push( positions.getZ( index ) );
+        newPositions.push(positions.getX(index));
+        newPositions.push(positions.getY(index));
+        newPositions.push(positions.getZ(index));
 
-        hashToIndex[ hash ] = nextIndex;
-        newIndices.push( nextIndex );
-        nextIndex ++;
+        hashToIndex[hash] = nextIndex;
+        newIndices.push(nextIndex);
+        nextIndex++;
 
       }
 
@@ -128,14 +152,14 @@ class Utils {
     // Construct merged BufferGeometry.
 
     const positionAttribute = new BufferAttribute(
-      new Float32Array( newPositions ),
+      new Float32Array(newPositions),
       positions.itemSize,
       positions.normalized
     );
 
     const result = new BufferGeometry();
-    result.setAttribute( 'position', positionAttribute );
-    result.setIndex( newIndices );
+    result.setAttribute('position', positionAttribute);
+    result.setIndex(newIndices);
 
     return result;
 

src/index.js

@@ -1,4 +1,4 @@
-import { Pathfinding } from './Pathfinding';
-import { PathfindingHelper } from './PathfindingHelper';
+import { Pathfinding } from './Pathfinding.js';
+import { PathfindingHelper } from './PathfindingHelper.js';
 
 export { Pathfinding, PathfindingHelper };