在查找路径的算法中查找错误

Question

我正在尝试实现“ A star”算法来查找路径。 首先，我实现了Dijkstra算法（长搜索，短途），然后实现了贪心算法（短搜索，也许长途）。之后，我将两种算法的值相加得到``A Star''，作为一种启发式方法，我使用了以三次坐标计算的曼海顿距离，然后将其转换为我的坐标系。

搜索没有给出简短的路径。我应该解决什么问题，以便搜索总是给出一条短路径，但保持快速？

查找路径功能：

    findPath = (start, goal) => {
    finalPathNeighbors = [];
    time = performance.now();
    const frontier = PriorityQueue();  // List of the places to explore next
    const came = [];// List of where we've already been - "from" and The price we paid to go there - "cost"
    let found = false;

    frontier.push(start, 0);
    came.push({to: start, from: undefined, cost: 0});

    while (frontier.length() > 0) {

      const current = frontier.pop();
      const currentHex = findHexMap(current);

      const neighbors = currentHex.neighbors;
      //const neighbors = ArrayHelper.shuffle(currentHex.neighbors);    // randomize

      // Early exit (stop exploring map when goal is reached)
      if (HexMath.hexEqual(current, goal)) {
        found = true;
        break;
      }

      for (let n = 0; n < neighbors.length; n++) {

        let next = neighbors[n],
          newCost = (findFromHex(came, current).cost + 1), //plus one step
        distance = HexMath.offsetDistance(next, goal);//manhattan distance

        if (findFromHex(came, next) === undefined || newCost < findFromHex(came, next).cost) {
            frontier.push(next, newCost + distance);
            came.push({to: next, from: current, cost: newCost});
        }
      }
    }

    // BUILD PATH BACK FROM GOAL
    if (goal && found) {
      let current = goal;
      let path = [goal];

      while (!HexMath.hexEqual(current, start)) {
        current = findFromHex(came, current).from;
        path.push(current);
      }

      finalPath = path.reverse();
      time = performance.now() - time;
      return finalPath;

    } else {
      return undefined;
    }
  };

完整代码：

function percentToHEXColor(percent) {
  if (percent === 100) {
    percent = 99
  }
  var r, g, b;


  // yellow to red
  b = Math.floor(255 * ((percent % 100) / 100));
  g = 0;
  r = 0;

  return ((1 << 24) + (r << 16) + (g << 8) + b).toString(16).slice(1);
}

function arrayMin(arr, number) {
  var len = arr.length, min = arr[0][number];
  while (len--) {
    if (arr[len][number] < min) {
      min = arr[len][number];
    }
  }
  return min;
};

function arrayMax(arr, number) {
  var len = arr.length, max = arr[0][number];
  while (len--) {
    if (arr[len][number] > max) {
      max = arr[len][number];
    }
  }
  return max;
};

const getPercentage = (cost, min, max)=>((cost-min)/(max-min)*100);

const Units = {
  Point : (x, y) => ({x, y}),

  Cube : (x, y, z) => ({x, y, z}),

  Hex : (q, r) => ({q, r}),
};

const ArrayHelper = {
  // ARRAY SHUFFLE
  // From: http://stackoverflow.com/questions/2450954/how-to-randomize-shuffle-a-javascript-array
  shuffle: (array) => {
    var currentIndex = array.length, temporaryValue, randomIndex;

    // While there remain elements to shuffle...
    while (0 !== currentIndex) {

      // Pick a remaining element...
      randomIndex = Math.floor(Math.random() * currentIndex);
      currentIndex -= 1;

      // And swap it with the current element.
      temporaryValue = array[currentIndex];
      array[currentIndex] = array[randomIndex];
      array[randomIndex] = temporaryValue;
    }

    return array;
  },

  multiArray: (x, y) => Array(...Array(x)).map(() => Array(y))
};

// PRIORITY QUEUE
// From: https://jsfiddle.net/GRIFFnDOOR/r7tvg/
// Savagely adapted/mangled!

const PriorityQueue = (arr) => {
  const queue = {

    heap: [],

    logHeap: function() {
      let output = "HEAP - "
      for (let i = 0; i < this.heap.length; i++) {
        output += "[" + this.heap[i][0] +  " / " + this.heap[i][1] + "]";
      }
      console.log(output);
    },

    length: function() {
      return this.heap.length;
    },

    push: function(data, priority) {
      var node = [data, priority];
      this.bubble(this.heap.push(node) - 1);
    },

    // removes and returns the data of lowest priority
    pop: function() {
      return this.heap.pop()[0];
    },

    // removes and returns the data of highest priority
    popHigh: function() {
      return this.heap.shift()[0];
    },

    // bubbles node i up the binary tree based on
    // priority until heap conditions are restored
    bubble: function(i) {
      while (i > 0) {
        // var parentIndex = i >> 1; // <=> floor(i/2)	// legacy code
        var parentIndex = i - 1;

        // if equal, no bubble (maintains insertion order)
        if (!this.isHigherPriority(i, parentIndex)) break;

        this.swap(i, parentIndex);
        i = parentIndex;
      }
    },

    // swaps the addresses of 2 nodes
    swap: function(i,j) {
      var temp = this.heap[i];
      this.heap[i] = this.heap[j];
      this.heap[j] = temp;
    },

    // returns true if node i is higher priority than j
    isHigherPriority: function(i,j) {
      return this.heap[i][1] > this.heap[j][1];
    }

  };

  if (arr) for (i=0; i< arr.length; i++)
    queue.heap.push(arr[i][0], arr[i][1]);

  return queue;
}

const Conversion = {

  offset2Cube:(hex) => {
    var x = hex.q - (hex.r - (hex.r&1)) / 2
    var z = hex.r;
    return Units.Cube(x,-x-z,z);
  },

  cube2Offset:(cube) => {
    var q = cube.x + (cube.z - (cube.z&1)) / 2;
    var r = cube.z;
    return Units.Hex(q, r);
  },

  pixel2Cube:(point) => {
    var x = Math.sqrt(3) * point.x / 3 / size.x - point.y / 3 / size.y;
    var z = 2/3 * point.y / size.y;
    return Units.Cube(x, -x-z, z);
  },

  pixel2Offset:(point)=>{
    return Conversion.cube2Offset(Conversion.cubeRound(Conversion.pixel2Cube(point)))
  },

  hex_to_pixel:(hex)=>{
    var x = size.x * (Math.sqrt(3) * hex.q + (Math.sqrt(3) / 2) * (hex.r&1));
    var y = size.y * 3/2 * hex.r;
    return Units.Point(x, y);
  },

  pixel_to_axial:(point)=>{
    var q = Math.sqrt(3) * point.x / 3 / size.x - point.y / 3 / size.y;
    var r = (2/3 * point.y) / size.y;
    return Units.Hex(q, r)
  },

  cubeRound:(cube)=> {
    var x = Math.round(cube.x);
    var y = Math.round(cube.y);
    var z = Math.round(cube.z);

    var x_diff = Math.abs(x - cube.x);
    var y_diff = Math.abs(y - cube.y);
    var z_diff = Math.abs(z - cube.z);

    if (x_diff > y_diff && x_diff > z_diff)
      x = -y - z;
    else if (y_diff > z_diff)
      y = -x - z;
    else
      z = -x - y;

    return Units.Cube(x,y,z);
  }
};

const HexMath = {

  cubeAdd: (cube1, cube2) => ({
    x: cube1.x + cube2.x,
    y: cube1.y + cube2.y,
    z: cube1.z + cube2.z
  }),

  cubeSub: (cube1, cube2) => ({
    x: cube1.x - cube2.x,
    y: cube1.y - cube2.y,
    z: cube1.z - cube2.z
  }),

  hexEqual: (hex1, hex2) => hex1.q === hex2.q && hex1.r === hex2.r,

  cubeDistance: (cube1, cube2) => (Math.abs(cube1.x - cube2.x) + Math.abs(cube1.y - cube2.y) + Math.abs(cube1.z - cube2.z)) / 2,

  offsetDistance: (hex1, hex2) => HexMath.cubeDistance( Conversion.offset2Cube(hex1), Conversion.offset2Cube(hex2) ),

  moreEuristic: (start, current, goal) => Math.abs(current.q - goal.q*start.r - goal.r - start.q - goal.q*current.r - goal.r)

};


  let mapSize = {q: 15, r: 22};
  let map = ArrayHelper.multiArray(mapSize.q, mapSize.r);
  let size = {x: 32, y: 16};
  let center = {x: 0, y: 0};
  var graphics;
  var text;
  var player = {coord: {q: 5, r: 5}};
  var coord;
  var finalPath = [];
  var finalPathNeighbors = [];
  var add;
  var stages = 0;
  var path1, path2;
  var time = 0;
  var debug = false;
  var algo = 1;

  //Fill Map Collision
  for (let q = 0; q < mapSize.q; q++) {
    for (let r = 0; r < mapSize.r; r++) {
      //map[q][r] = {collision: Math.random() >= 0.8};
      map[q][r] = {collision: false};
    }
  }

  map[3][2] = {collision: true};
  map[4][2] = {collision: true};
  map[5][2] = {collision: true};
  map[6][2] = {collision: true};
  map[7][2] = {collision: true};
  map[7][3] = {collision: true};
  map[7][4] = {collision: true};
  map[7][5] = {collision: true};
  map[7][6] = {collision: true};
  map[7][7] = {collision: true};
  map[7][8] = {collision: true};
  map[7][9] = {collision: true};
  map[7][10] = {collision: true};
  map[7][11] = {collision: true};
  map[7][12] = {collision: true};
  map[7][13] = {collision: true};
  map[7][14] = {collision: true};
  map[7][15] = {collision: true};
  map[7][16] = {collision: true};
  map[7][17] = {collision: true};
  map[7][18] = {collision: true};
  map[7][19] = {collision: true};
  map[7][20] = {collision: true};
  map[6][20] = {collision: true};
  map[5][20] = {collision: true};
  map[4][20] = {collision: true};
  map[3][20] = {collision: true};

  map[11][5] = {collision: true};

  const offsetDirections = [
    [[+1, 0], [0, -1], [-1, -1],
      [-1, 0], [-1, +1], [0, +1]],
    [[+1, 0], [+1, -1], [0, -1],
      [-1, 0], [0, +1], [+1, +1]],
  ];

  const cubeDirections = [
      Units.Cube(+1, -1, 0), Units.Cube(+1, 0, -1), Units.Cube(0, +1, -1),
      Units.Cube(-1, +1, 0), Units.Cube(-1, 0, +1), Units.Cube(0, -1, +1)
    ];

  offsetNeighbor = (hex, direction) => {
    var parity = hex.r & 1;
    var dir = offsetDirections[parity][direction];
    var hexf = Units.Hex(hex.q + dir[0], hex.r + dir[1]);
    return hexf
  };


  cubeDirection = (d) => cubeDirections[d];

  cubeNeighbor = (h, d) => HexMath.cubeAdd(h, cubeDirection(d));

  cubeNeighbors = (h) => {
      const neighbors = [];
      for (let d = 0; d < 6; d++) {
        neighbors.push(cubeNeighbor(h, d));
      }

      return neighbors;
  };

  //get coord corner dot hex by count
  hex_corners = (center, size, corners) => {

    var coords = [];
    for (i = 1; i <= corners; i++) {
      var angle_deg = 60 * i - 30;
      var angle_rad = Math.PI / 180 * angle_deg;
      coords.push(Units.Point(center.x + size.x * Math.cos(angle_rad),
        center.y + size.y * Math.sin(angle_rad)));
    }
    return coords;
  };

  findHexMap = (hex) => {
    //const hexOffset = Conversion.cube2Offset(hex);
    const hexOffset = hex;
    if (map[hexOffset.q]) {
      return map[hexOffset.q][hexOffset.r];
    } else {
      return undefined;
    }
  };

  // Generate data of Hexes, filling map array neighbors, coordinates, collision
  initHexes = () => {

    for (let q = 0; q < mapSize.q; q++) {
      for (let r = 0; r < mapSize.r; r++) {

        const hex = Units.Hex(q, r),
          cube = Conversion.offset2Cube(hex),
          neighbors = [];

        if (!map[q][r].collision) {

          // Each (eventual) neighbor of the cell
          for (let i = 0; i < 6; i++) {
            const neighbor_offset = offsetNeighbor(hex, i);

            // Is the neighbor on/in the map?
            if (neighbor_offset.q >= 0 && neighbor_offset.r >= 0
              && neighbor_offset.q < mapSize.q && neighbor_offset.r < mapSize.r) {

              if (!map[neighbor_offset.q][neighbor_offset.r].collision) {
                neighbors.push(neighbor_offset); // add an edge to the graph
              }
            }
          }
        }

        // fill things into cell
        map[q][r].cube = cube;
        map[q][r].hex = hex;
        map[q][r].neighbors = neighbors;
      }
    }
  };

  //////////////////////////////////////////////////////////////////////////////
  // PATH FINDING
  // From:
  //	http://www.redblobgames.com/pathfinding/a-star/introduction.html
  //	http://www.redblobgames.com/pathfinding/a-star/implementation.html

  //get "to" hexes
  getIndexHexes = (cames) => {
    const hexes = [];
    for (let h = 0; h < came.length; h++) {
      hexes.push(came[h].to);
    }
    return hexes;
  };

  //find hex from came to current hex
  findFromHex = (cames, hex) => {
    for (let h = 0; h < cames.length; h++) {
      if (HexMath.hexEqual(cames[h].to, hex)) {
        return cames[h];
      }
    }
    return undefined;
  };

  findPath = (start, goal) => {
    finalPathNeighbors = [];
    time = performance.now();
    const frontier = PriorityQueue();  // List of the places to explore next
    const came = [];// List of where we've already been - "from" and The price we paid to go there - "cost"
    let found = false;

    frontier.push(start, 0);
    came.push({to: start, from: undefined, cost: 0});

    while (frontier.length() > 0) {

      const current = frontier.pop();
      const currentHex = findHexMap(current);

      const neighbors = currentHex.neighbors;
      //const neighbors = ArrayHelper.shuffle(currentHex.neighbors);	// randomize

      // Early exit (stop exploring map when goal is reached)
      if (HexMath.hexEqual(current, goal)) {
        found = true;
        break;
      }

      for (let n = 0; n < neighbors.length; n++) {

        let next = neighbors[n],
          newCost = (findFromHex(came, current).cost + 1), //plus one step
        distance = HexMath.offsetDistance(next, goal);//manhattan distance

          //distance += HexMath.moreEuristic(start, current, goal);

        let eurist = newCost + distance;
        if (findFromHex(came, next) === undefined || newCost < findFromHex(came, next).cost) {
            finalPathNeighbors.push([next, eurist]);
            frontier.push(next, eurist);
            came.push({to: next, from: current, cost: newCost});
        }
      }
    }

    // BUILD PATH BACK FROM GOAL
    if (goal && found) {
      let current = goal;
      let path = [goal];

      while (!HexMath.hexEqual(current, start)) {
        current = findFromHex(came, current).from;
        path.push(current);
      }

      finalPath = path.reverse();
      time = performance.now() - time;
      return finalPath;

    } else {
      return undefined;
    }
  };

  //////////////////////////////////////////

  var game = new Phaser.Game({
    //type: Phaser.AUTO,
    type: Phaser.CANVAS,
    width: 1920,
    height: 1080,
    scene: {
      create: create,
      update: update
    }
  });

  function create() {





    initHexes();
    graphics = this.add.graphics({lineStyle: {width: 3, color: 0xe83331}});
    text = this.add.text(40, 40, '');
    coord = this.add.text(40, 60);
    add = this.add;


    this.input.on('pointermove', pointermove);
    this.input.on('pointerdown', pointerdown);

    for (let q = 0; q < mapSize.q; q++) {
      for (let r = 0; r < mapSize.r; r++) {
        let textPix = Conversion.hex_to_pixel(({q,r}));

        map[q][r].text = this.add.text();
        map[q][r].text.coord = {x:textPix.x,y:textPix.y};


        // map[q][r].text.text = q+':'+r;
        // map[q][r].text.x = map[q][r].text.coord.x-map[q][r].text.width/2;
        // map[q][r].text.y = map[q][r].text.coord.y-map[q][r].text.height/2;


      }
    }
  }

  function update() {

    //this.add.bitmapText({x:16, y:10, text:'Arial'});
    text.setText("FPS: " + game.loop.actualFps);

    graphics.clear();
    if (center.y >= 0 && center.x >= 0) {
      let oddr = Conversion.pixel2Offset(center)
      if (oddr.q < mapSize.q && oddr.r < mapSize.r) {
        graphics.strokePoints(hex_corners(center, size, 6), true);
      }
    }

    for (let q = 0; q < mapSize.q; q++) {
      for (let r = 0; r < mapSize.r; r++) {
        map[q][r].text.text = '';
        try {
          if (map[q][r].collision) {
            graphics.strokePoints(hex_corners(Conversion.hex_to_pixel({
              q: q,
              r: r
            }), size, 6), true).fillStyle(0x00ff00, 1).fillPath();
          }
        } catch (e) {
          debugger;
        }

      }
    }

    if (finalPathNeighbors.length > 0){
      let min = arrayMin(finalPathNeighbors, 1);
      let max = arrayMax(finalPathNeighbors, 1);
      finalPathNeighbors.forEach((hex) => {
        let q =hex[0].q;
        let r = hex[0].r;
        let percentage = getPercentage(hex[1], min, max);
        graphics.strokePoints(hex_corners(Conversion.hex_to_pixel(({q,r})), size, 6), true).fillStyle("0x"+percentToHEXColor(percentage), 1).fillPath();


        map[q][r].text.text = hex[1];
        map[q][r].text.x = map[q][r].text.coord.x-map[q][r].text.width/2;
        map[q][r].text.y = map[q][r].text.coord.y-map[q][r].text.height/2;
      });
    }

    if (finalPath.length > 0)
      finalPath.forEach((hex) => {
        graphics.strokePoints(hex_corners(Conversion.hex_to_pixel({
          q: hex.q,
          r: hex.r
        }), {x:size.x/2,y:size.y/2}, 6), true).fillStyle(0x6600a5, 1).fillPath();
      });



  }

  let pointerdown = pointer => {
    let hex = Conversion.pixel2Offset({x: pointer.x, y: pointer.y});
    if (
      center.y >= 0 &&
      center.x >= 0 &&
      hex.q < mapSize.q &&
      hex.r < mapSize.r &&
      map[hex.q][hex.r].collision === false
    ) {
      switch (stages) {
        case 0:
          path1 = hex;
          stages = 1;
          break;
        case 1:
          path2 = hex;
          debug = true;
          findPath(path1, path2);
          stages = 2;
          console.log(time);
          break;
        case 2:
          debug = false;
          finalPath = [];
          finalPathNeighbors = [];
          path1 = undefined;
          path2 = undefined;
          stages = 0;
          break;
      }
    }
  }

  let pointermove = pointer => {

    var cube = Conversion.pixel2Cube({x: pointer.x, y: pointer.y});
    var round_cube = Conversion.cubeRound(cube);
    var round_hex = Conversion.cube2Offset(round_cube);
    center = Conversion.hex_to_pixel(round_hex);
    path2 = round_hex;

    var offsetToCube = Conversion.offset2Cube(round_hex)
    if (
      center.y >= 0 &&
      center.x >= 0 &&
      round_hex.q < mapSize.q &&
      round_hex.r < mapSize.r &&
      path1!== undefined &&
      stages === 1 &&
      map[round_hex.q][round_hex.r].collision === false
    ) {
          findPath(path1,path2);
    }

    // let neghs = APuthSearch(round_hex, function (hex) {
    //   let oddr = pixel_to_oddr({x:game.config.width,y:game.config.height});
    //
    //   return hex.q < 0 || hex.r < 0 || hex.q > oddr.q+1 || hex.r > oddr.r+1
    //
    // });


    coord.text = 'Cursor\n x:' + pointer.x + "\n" + ' y:' + pointer.y + "\n" +
      'Hex\n r:' + round_hex.r + "\n" + ' q:' + round_hex.q + "\n" +
      'Cube\n x:' + round_cube.x + "\n" + ' y:' + round_cube.y + "\n" + ' z:' + round_cube.z + "\n";

  };

function changeAlgo(){
  if(algo===1)
    algo=0
  else
    algo=1
}

<script src="https://cdnjs.cloudflare.com/ajax/libs/phaser/3.17.0/phaser.min.js"></script>
<!doctype html>
<html>
<head>
  <title>Socket.IO</title>
</head>
<body>
<style>
/**  * { cursor: none; } **/
  body {
    overflow: hidden;
    margin: 0px;
    height: 1080px;
    width: 1920px;
  }
</style>