Skip to content

Instantly share code, notes, and snippets.

@mikeant42

mikeant42/AStarMap.cs

Last active March 29, 2021 01:10
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save mikeant42/c474d27b434a57349727f3fd38a3b08d to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save mikeant42/c474d27b434a57349727f3fd38a3b08d to your computer and use it in GitHub Desktop.
Download ZIP
Raw
AStarMap.cs
using Godot;
using System;
using System.Collections.Generic;
// TODO - run all of this in a new thread
public class AStarMap : Spatial
{
private Dictionary<string, int> points = new Dictionary<string, int>();
private AStar aStar;
private Godot.Collections.Array<Vector3> cells;
private List<Vector3> locations = new List<Vector3>();
[Export]
public NodePath gridPath;
private GridMap map;
[Export]
public int gridStep = 5;
private int[] xNeighbors;
private int[] yNeighbors;
private int[] zNeighbors;
private Vector3 FindClosestNeighbor(Vector3 point)
{
Vector3 closestNeighbor = Vector3.Zero;
float closestDistance = 500000;
foreach (var cPoint in cells)
{
var dist = point.DistanceTo(cPoint);
if (dist < closestDistance)
{
closestNeighbor = cPoint;
closestDistance = dist;
}
}
return (closestNeighbor);
}
private void AddAllPointsOld()
{
map = (GridMap)GetNode(gridPath);
aStar = new AStar();
cells = new Godot.Collections.Array<Vector3>(map.GetUsedCells());
xNeighbors = new int[] {-gridStep, 0, gridStep};
yNeighbors = new int[] {-gridStep, 0,1,2,3,4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24};
zNeighbors = new int[] {-gridStep, 0, gridStep};
foreach (var cell in cells)
{
var index = aStar.GetAvailablePointId();
aStar.AddPoint(index, map.MapToWorld((int)cell.x, (int)cell.y, (int)cell.z));
//GD.Print((int)cell.y);
//points[v3ToIndex(cell)] = index;
//GD.Print(v3ToIndex(cell));
points.Add(v3ToIndex(cell), index);
}
// please god clean this up!!
// grab the additional points around a point inside the dictionary and connect him to his neighbors
foreach (var cell in cells)
{
foreach (int x in xNeighbors)
{
// we don't know how far a potential vertical neighbor is - since the height is arbitrary
foreach(int y in yNeighbors)
{
foreach (int z in zNeighbors)
{
var v3 = new Vector3(x, y, z);
// this point has already been added, so skip re-adding it
if (v3.Equals(Vector3.Zero))
continue;
if (points.ContainsKey(v3ToIndex(v3 + cell)))
{
var ind1 = points[v3ToIndex(cell)];
var ind2 = points[v3ToIndex(cell + v3)];
//GD.Print("forming connections");
if (!aStar.ArePointsConnected(ind1, ind2))
{
GD.Print(ind1 + " , " + ind2);
aStar.ConnectPoints(ind1, ind2, true);
}
}
// v3.y = 0;
// if (!points.ContainsKey(v3ToIndex(v3 + cell)))
// {
// GD.Print("edge");
// aStar.SetPointWeightScale(points[v3ToIndex(cell)], 0.1f);
// }
}
}
}
}
}
private void AddAllPoints()
{
map = (GridMap)GetNode(gridPath);
aStar = new AStar();
cells = new Godot.Collections.Array<Vector3>(map.GetUsedCells());
xNeighbors = new int[] {-gridStep, 0, gridStep};
yNeighbors = new int[] {126, 14, 2, 21, 22, 28, 35, 37, 68, 7};
zNeighbors = new int[] {-gridStep, 0, gridStep};
foreach (var cell in cells)
{
var index = aStar.GetAvailablePointId();
aStar.AddPoint(index, map.MapToWorld((int)cell.x, (int)cell.y, (int)cell.z));
//GD.Print((int)cell.y);
//points[v3ToIndex(cell)] = index;
//GD.Print(v3ToIndex(cell));
points.Add(v3ToIndex(cell), index);
}
// please god clean this up!!
// grab the additional points around a point inside the dictionary and connect him to his neighbors
foreach (var cell in cells)
{
var neighbor = FindClosestNeighbor(cell);
if (points.ContainsKey(v3ToIndex(neighbor)))
{
var ind1 = points[v3ToIndex(cell)];
var ind2 = points[v3ToIndex(neighbor)];
//GD.Print("forming connections");
if (!aStar.ArePointsConnected(ind1, ind2))
{
GD.Print(ind1 + " , " + ind2);
aStar.ConnectPoints(ind1, ind2, true);
}
}
}
}
public override void _Ready()
{
//AddAllPointsOld();
}
private string v3ToIndex(Vector3 v3)
{
return (string)((int)(Math.Round(v3.x)) + "," + (int)Math.Round(v3.y) + "," + (int)Math.Round(v3.z));
}
public Vector3[] GetPlottedPath(Vector3 start, Vector3 end)
{
//find closest point to position 1st way is faster, if it's not in map use slower a* method
var gmStart = v3ToIndex(map.WorldToMap(start));
var gmEnd = v3ToIndex(map.WorldToMap(end));
int startID, endID = 0;
if (points.ContainsKey(gmStart))
startID = points[gmStart];
else
startID = aStar.GetClosestPoint(start);
if (points.ContainsKey(gmEnd))
endID = points[gmEnd];
else
endID = aStar.GetClosestPoint(end);
//var startID = aStar.GetClosestPoint(start);
//var endID = aStar.GetClosestPoint(end);
//GD.Print(startID);
//GD.Print("/n " + endID);
return aStar.GetPointPath(startID, endID);
}
// // Called every frame. 'delta' is the elapsed time since the previous frame.
// public override void _Process(float delta)
// {
//
// }
}
Raw
grid_gen.gd
tool
extends Node
# todo - exclude out undesirable pathing
export var grid_path := NodePath()
var grid
export var bounds := Vector3()
export var grid_step = 1;
export var ray_path := NodePath()
var cast
export var ceiling = 100
export var cornerMargin = 12
export var rayCastWallCheckHeightOffset = 1
export var wallAngle = 90
var active_floors = {}
func _enter_tree():
pass
#connect("pressed", self, "clicked")
func generate():
cast = get_node(ray_path)
cast.cast_to = Vector3(0,-ceiling, 0)
grid = get_node(grid_path)
grid.clear()
for x in range(-bounds.x, bounds.x, grid_step):
for z in range(-bounds.z, bounds.z, grid_step):
cast.cast_to = Vector3(0,-ceiling, 0)
cast.global_transform.origin.x = x
cast.global_transform.origin.z = z
cast.global_transform.origin.y = ceiling
cast.force_raycast_update()
if cast.is_colliding():
var collider = cast.get_collider()
if (collider.is_in_group("terrain")):
var col_height = int(cast.get_collision_point().y)
# check for surrounding objects
cast.global_transform.origin.y = col_height + rayCastWallCheckHeightOffset
cast.cast_to = Vector3(cornerMargin, 0, 0)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
cast.cast_to = Vector3(-cornerMargin, 0, 0)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
cast.cast_to = Vector3(0,0,cornerMargin)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
cast.cast_to = Vector3(0,0,-cornerMargin)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
# check diagonally as well
cast.cast_to = Vector3(-cornerMargin,0,-cornerMargin)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
cast.cast_to = Vector3(cornerMargin,0,cornerMargin)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
cast.cast_to = Vector3(-cornerMargin,0,cornerMargin)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
cast.cast_to = Vector3(cornerMargin,0,-cornerMargin)
cast.force_raycast_update()
if cast.is_colliding() and cast.get_collider().is_in_group("terrain"):
var normal = cast.get_collision_normal().normalized()
var temp = normal.cross(Vector3.DOWN)
var groundSlopeDir = temp.cross(normal)
var slope = rad2deg(normal.angle_to(Vector3.UP))
if (slope) >= wallAngle:
continue
if (!(col_height in active_floors)):
active_floors[col_height] = "d"
_set_grid(cast.get_collision_point())
print(active_floors)
# The aabb method of avoiding staticbodies may work but we need to change the way we gen
# the grid. we need to go directly to the terrain and not work with the terrain mesh
func _set_grid(cell_pos):
grid.set_cell_item(cell_pos.x,
cell_pos.y, cell_pos.z, 0)
func get_class(): return "GridGenerator"
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment