/// <summary>
/// Gets distance from node A to node B
/// </summary>
/// <param name="a">node A</param>
/// <param name="b">node B</param>
/// <returns>Distance between node A and node B</returns>
protected override double Distance(Node3D a, Node3D b)
{
var xd = Math.Abs(a.X - b.X);
var yd = Math.Abs(a.Y - b.Y);
var zd = Math.Abs(a.Z - b.Z);
return MathExtensions.Max(MathExtensions.NearestInt(xd), MathExtensions.NearestInt(yd), MathExtensions.NearestInt(zd));
}
/// <summary>
/// Gets distance from node A to node B
/// </summary>
/// <param name="a">node A</param>
/// <param name="b">node B</param>
/// <returns>Distance between node A and node B</returns>
protected override double Distance(Node3D a, Node3D b)
{
var xd = a.X - b.X;
var yd = a.Y - b.Y;
var zd = a.Z - b.Z;
return Math.Ceiling(Math.Sqrt(xd * xd + yd * yd + zd * zd));
}
//Searches in a 3x3 block around the node
public List <Node3D> GetNeighbours(Node3D node)
{
List <Node3D> neighbours = new List <Node3D>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}
int checkX = node.gridX + x;
int checkY = node.gridY + y;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbours.Add(grid[checkX, checkY]);
}
}
}
return(neighbours);
}
void CreateGrid()
{
grid = new Node3D[gridSizeX, gridSizeY];
Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2;
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius);
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask)); //Checks collision with wall in node
int movementPenalty = 0;
Ray ray = new Ray(worldPoint + Vector3.up * 50, Vector3.down);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, 100, walkableMask))
{
walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
}
if (!walkable)
{
movementPenalty += obstacleProximityPenalty;
}
grid[x, y] = new Node3D(walkable, worldPoint, x, y, movementPenalty);
}
}
BlurPenaltyMap(3);
}
public WorldFileManager(RenderManager RenderManager)
{
Render = RenderManager;
RootNode = Render.RootNode;
InitGrid();
ChunkSelectorQuad = new CollisionQuad(new Vector3(), new Vector3(), new Vector3(), new Vector3());
}
/// <summary>
/// Gets distance from node A to node B
/// </summary>
/// <param name="a">node A</param>
/// <param name="b">node B</param>
/// <returns>Distance between node A and node B</returns>
protected override double Distance(Node3D a, Node3D b)
{
var xd = a.X - b.X;
var yd = a.Y - b.Y;
var zd = a.Z - b.Z;
return MathExtensions.NearestInt(Math.Sqrt(xd * xd + yd * yd + zd * zd));
}
public void RenderNode(Node3D node)
{
if (node is Entity3D)
{
var ent = node as Entity3D;
FXG.Local = ent.World;
OutFX.Bind();
foreach (var mesh in ent.Meshes)
{
mesh.Material.Bind();
mesh.Viz.SetMesh(mesh);
mesh.Viz.Bind();
mesh.Viz.Visualize();
mesh.Viz.Release();
mesh.Material.Release();
}
OutFX.Release();
}
foreach (var s_node in node.Sub)
{
RenderNode(s_node);
}
}
public void SetNode(Node3D node)
{
Link.Node = node;
if (node is Entity3D)
{
Link.Entity = node as Entity3D;
}
}
/// <summary>
/// Gets distance from node A to node B
/// </summary>
/// <param name="a">node A</param>
/// <param name="b">node B</param>
/// <returns>Distance between node A and node B</returns>
protected override double Distance(Node3D a, Node3D b)
{
var xd = Math.Abs(a.X - b.X);
var yd = Math.Abs(a.Y - b.Y);
var zd = Math.Abs(a.Z - b.Z);
return(MathExtensions.Max(MathExtensions.NearestInt(xd), MathExtensions.NearestInt(yd), MathExtensions.NearestInt(zd)));
}
/// <summary>
/// Gets distance from node A to node B
/// </summary>
/// <param name="a">node A</param>
/// <param name="b">node B</param>
/// <returns>Distance between node A and node B</returns>
protected override double Distance(Node3D a, Node3D b)
{
var xd = a.X - b.X;
var yd = a.Y - b.Y;
var zd = a.Z - b.Z;
return(MathExtensions.NearestInt(Math.Sqrt(xd * xd + yd * yd + zd * zd)));
}
/// <summary>
/// Gets distance from node A to node B
/// </summary>
/// <param name="a">node A</param>
/// <param name="b">node B</param>
/// <returns>Distance between node A and node B</returns>
protected override double Distance(Node3D a, Node3D b)
{
var xd = a.X - b.X;
var yd = a.Y - b.Y;
var zd = a.Z - b.Z;
return(Math.Ceiling(Math.Sqrt(xd * xd + yd * yd + zd * zd)));
}
public static Node3D ImportAnimNode(string path)
{
string key = new FileInfo(path).Extension.ToLower();
Importer imp = Imports[key];
Node3D r = imp.LoadAnimNode(path);
return(r);
}
void Set(Vector3[] positions)
{
//ノード初期化
Node3D[] nodes = new Node3D[positions.Length];
for (int i = 0; i < positions.Length; i++)
{
nodes[i].Position = positions[i];
}
//
foreach (Node3D current in nodes)
{
foreach (Node3D node in nodes)
{
if (current != node)
{
//if(current.L)
//{
//}
}
}
}
float l = float.MinValue;
float r = float.MaxValue;
Vector3 lPos = Vector3.zero; bool lFlag = false;
Vector3 rPos = Vector3.zero; bool rFlag = false;
//foreach (Vector3 v in positions)
//{
// float x = v.x - current.x;
// if(x > 0f)
// {
// if(x < r)
// {
// r = x;
// rPos = v;
// rFlag = true;
// }
// }
// else
// {
// if (x > l)
// {
// l = x;
// lPos = v;
// rFlag = true;
// }
// }
//}
//if (rFlag) list.Add(rPos);
//if (lFlag) list.Add(lPos);
}
//Distance between nodes to help calculate gcost and hcost. Diagonal is 14, other is 10
int GetDistance(Node3D nodeA, Node3D nodeB)
{
int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
if (dstX > dstY)
{
return(14 * dstY + 10 * (dstX - dstY));
}
return(14 * dstX + 10 * (dstY - dstX));
}
public SceneRenderTarget(string Name, string SName, Node3D Scene, GraphicsDevice Graphics, Vector3 CameraPosition, float Close, float Far, float PlaneDirection, int ResolutionX, int ResolutionY, RenderType TargetType)
{
ParameterName = SName;
SceneNode = Scene;
ClipPlaneDirection = PlaneDirection;
RenderCamera = new Camera(CameraPosition, ResolutionX, ResolutionY, Far, Close, MathHelper.PiOver4);
RenderMode = TargetType;
GenerateTarget(Graphics, ResolutionX, ResolutionY);
}
public void Compile(Node3D node)
{
if (Compiled)
{
return;
}
Compiled = true;
script.Node = node;
System.Console.WriteLine("Script:" + FilePath + " Compiled.");
}
private void GenerateTerrainGeometries()
{
SettingsContainer Settings = FileManager.MasteryFile.Settings;
List <WorldFile> ActiveWorldFiles = FileManager.MasteryFile.ActiveWorldFiles;
for (int i = 0; i < ActiveWorldFiles.Count; i++)
{
bool HasGeometry = false;
for (int j = 0; j < TerrainGeometries.Count; j++)
{
if (ActiveWorldFiles[i] == TerrainGeometries[j].WorldFile)
{
HasGeometry = true;
}
}
if (!HasGeometry)
{
Node3D Node = null;
for (int j = 0; j < RenderNodeLayers.Count; j++)
{
if (RenderNodeLayers[j].Name.Replace("Layer:", "") == ActiveWorldFiles[i].LODID + "")
{
Node = RenderNodeLayers[j];
}
}
if (Node != null)
{
TerrainGeometryContainer NewContainer = new TerrainGeometryContainer(Render, Settings, ActiveWorldFiles[i]);
//TerrainDeformableCoverContainer NewDeformedCover = new TerrainDeformableCoverContainer(Render,ActiveWorldFiles[i],NewContainer, Settings,FromBelowDepthTarget);
TerrainWaterContainer WaterContainer = new TerrainWaterContainer(
Render, NewContainer, ReflectionNode, RefractionNode, PropNode, ActiveWorldFiles[i], Settings, ActiveWorldFiles[i].GetPosition(), ReflectionRenderTarget, RefractionRenderTarget, DepthMapRenderTarget, FromBelowDepthTarget);
DepthMapRenderTarget.DebugTextureName = "SceneWaterDepthDebug";
TerrainGeometries.Add(NewContainer);
//DeformedTerrainGeometries.Add(NewDeformedCover);
WaterGeometries.Add(WaterContainer);
ReflectionNode.Attach(NewContainer.TerrainGeometry);
RefractionNode.Attach(NewContainer.TerrainGeometry);
Node.Attach(WaterContainer.Geom);
//Node.Attach(NewDeformedCover.Geom);
Node.Attach(NewContainer.TerrainGeometry);
}
}
}
}
public void Compile(Node3D node)
{
if (Compiled)
{
return;
}
Compiled = true;
script = CSScript.Evaluator.LoadCode(System.IO.File.ReadAllText(FilePath));
script.Node = node;
System.Console.WriteLine("Script:" + FilePath + " Compiled.");
}
protected void Start()
{
style.alignment = TextAnchor.MiddleCenter;
style.normal.textColor = Color.white;
var nodes = new List <Node>();
var edges = new List <Edge>();
var half = -Vector3.one * 0.5f;
var offset = -new Vector3(
width - ((width % 2 == 0) ? 1f : 0f),
0f,
height - ((height % 2 == 0) ? 1f : 0f)
) * 0.5f;
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
var noise = new Vector3(Random.value, Random.value, Random.value) + half;
var node = new Node3D(new Vector3(x, 0, y) + offset + noise);
nodes.Add(node);
}
}
for (int y = 0; y < height; y++)
{
var yoff = y * width;
for (int x = 0; x < width; x++)
{
var idx = yoff + x;
var node = nodes[idx];
if (x < width - 1)
{
var to = nodes[idx + 1];
var e = node.Connect(to, Vector3.Distance((node as Node3D).Position, (to as Node3D).Position));
edges.Add(e);
}
if (y < height - 1)
{
var to = nodes[idx + width];
var e = node.Connect(to, Vector3.Distance((node as Node3D).Position, (to as Node3D).Position));
edges.Add(e);
}
}
}
graph = new Graph(nodes, edges);
path = Dijkstra.Find(graph, source % (graph.Nodes.Count));
}
public override void LoadNode()
{
_Width = IOHelp.ReadFloat();
_Depth = IOHelp.ReadFloat();
_YScale = IOHelp.ReadFloat();
_HeightMapPath = IOHelp.ReadString();
Node3D prev_t = SceneGraph3D.CurScene.FindNode("TerrainObjNode");
if (prev_t != null)
{
TerrainNode = prev_t as Terrain3D;
// SceneGraph3D.CurScene.Remove(prev_t);
}
}
private static T GetOrAddAnimator <T>(AnimationData animation, Node3D n, string propName) where T : IAnimator, new()
{
if (n.Animators.TryFind(propName, out var a))
{
return((T)a);
}
var animator = new T {
AnimationId = animation.AnimationStackName,
TargetPropertyPath = propName
};
n.Animators.Add(animator);
return(animator);
}
Vector3[] RetracePath(Node3D startNode, Node3D endNode)
{
List <Node3D> path = new List <Node3D>();
Node3D currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
Vector3[] waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return(waypoints);
}
public SceneRenderTarget(string SName, Node3D Scene, GraphicsDevice Graphics, Vector3 CameraPosition, float Close, float Far, float PlaneDirection, int ResolutionX, int ResolutionY, RenderType TargetType, int FrustrumWidth, int FrustrumHeight)
{
Name = SName;
ParameterName = SName;
SceneNode = Scene;
ClipPlaneDirection = PlaneDirection;
RenderMode = TargetType;
RenderCamera = new Camera(CameraPosition, FrustrumWidth, FrustrumHeight, Far, Close);
RenderMode = TargetType;
GenerateTarget(Graphics, ResolutionX, ResolutionY);
}
/**
* Adds/Removes Nodes based on LOD Setting
**/
private void CalculateActiveNodes()
{
if (FileManager.MasteryFile == null)
{
//clear all nodes
RenderNodeLayers.Clear();
ChunkLODStates.Clear();
}
else if (ChunkLODStates.Count > 0)
{
int LayerCount = Settings.LOD.Length;
if (LayerCount > RenderNodeLayers.Count)
{
//add nodes
for (int i = 0; i < LayerCount; i++)
{
bool HasLayer = false;
for (int j = 0; j < RenderNodeLayers.Count; j++)
{
int LayerLOD = Int32.Parse(RenderNodeLayers[j].Name.Replace("Layer:", ""));
if (i == LayerLOD)
{
HasLayer = true;
break;
}
}
if (!HasLayer)
{
Node3D NewNode = new Node3D("Layer:" + i);
RenderNode.Attach(NewNode);
RenderNodeLayers.Add(NewNode);
}
}
}
else if (LayerCount < RenderNodeLayers.Count)
{
//remove nodes
for (int i = 0; i < RenderNodeLayers.Count; i++)
{
int LayerLOD = Int32.Parse(RenderNodeLayers[i].Name.Replace("Layer:", ""));
if (LayerLOD > LayerCount)
{
RenderNodeLayers.RemoveAt(i);
}
}
}
}
}
public static Node3D ImportNode(string path)
{
string key = new FileInfo(path).Extension.ToLower();
if (Imports.ContainsKey(key))
{
Importer imp = Imports[key];
Node3D r = imp.LoadNode(path);
return(r);
}
else
{
return(null);
}
}
public MasterFileRenderManager(WorldFileManager WorldFileManager, RenderManager RenderManager, CameraManager CameraManager)
{
Render = RenderManager;
FileManager = WorldFileManager;
RenderNode = Render.RootNode;
Camera = CameraManager;
ReflectionNode = new Node3D("WorldReflection");
RefractionNode = new Node3D("WorldRefraction");
PropNode = new Node3D("PropNode");
SkyBox = new SkyBoxModel(Render, 1000, "SkyBox");
RenderNode.Attach(SkyBox.Geom);
ReflectionNode.Attach(SkyBox.Geom);
}
private Lime.Node ImportNodes(FbxNode root, Node parent = null)
{
Node3D node = null;
if (root == null)
{
return(null);
}
switch (root.Attribute.Type)
{
case FbxNodeAttribute.FbxNodeType.Mesh:
var meshAttribute = root.Attribute as FbxMeshAttribute;
var mesh = new Mesh3D {
Id = root.Name,
SkinningMode = meshAttribute.SkinningMode
};
foreach (var submesh in meshAttribute.Submeshes)
{
mesh.Submeshes.Add(ImportSubmesh(submesh, root));
}
node = mesh;
if (mesh.Submeshes.Count != 0)
{
mesh.SetLocalTransform(root.LocalTranform);
mesh.RecalcBounds();
mesh.RecalcCenter();
}
break;
case FbxNodeAttribute.FbxNodeType.Camera:
var cam = root.Attribute as FbxCameraAttribute;
node = new Camera3D {
Id = root.Name,
FieldOfView = cam.FieldOfView * Mathf.DegToRad,
AspectRatio = cam.AspectRatio,
NearClipPlane = cam.NearClipPlane,
FarClipPlane = cam.FarClipPlane,
ProjectionMode = cam.ProjectionMode,
OrthographicSize = cam.OrthoZoom,
};
node.SetLocalTransform(CorrectCameraTransform(root.LocalTranform));
break;
default:
node = new Node3D {
Id = root.Name
};
node.SetLocalTransform(root.LocalTranform);
break;
}
if (node != null)
{
if (parent != null)
{
parent.Nodes.Add(node);
}
foreach (var child in root.Children)
{
ImportNodes(child, node);
}
}
return(node);
}
// Heuristic function for A*. Use Euclidean distance for now
private float HeuristicValue(Node3D node, Node3D targetNode)
{
return Vector3.Distance(graph.WorldPosition(node), graph.WorldPosition(targetNode));
}
/// <summary> /// Gets distance from node A to node B /// </summary> /// <param name="a">node A</param> /// <param name="b">node B</param> /// <returns>Distance between node A and node B</returns> protected abstract double Distance(Node3D a, Node3D b);
/// <summary>
/// No distance implementation possible for Geographical location in 3D
/// </summary>
/// <exception cref="NotSupportedException">Not supported</exception>
protected override double Distance(Node3D a, Node3D b)
{
throw new NotSupportedException("No distance implementation possible for Geographical location in 3D");
}
private Lime.Node ImportNodes(FbxImporter.Node root, Lime.Node parent = null)
{
Node3D node = null;
if (root == null)
{
return(null);
}
switch (root.Attribute.Type)
{
case NodeAttribute.FbxNodeType.MESH:
var meshAttribute = root.Attribute as MeshAttribute;
var mesh = new Mesh3D {
Id = root.Name
};
foreach (var submesh in meshAttribute.Submeshes)
{
mesh.Submeshes.Add(ImportSubmesh(submesh, root));
}
if (platform == TargetPlatform.Unity)
{
mesh.CullMode = CullMode.CullCounterClockwise;
}
node = mesh;
if (mesh.Submeshes.Count != 0)
{
mesh.SetLocalTransform(root.LocalTranform);
mesh.RecalcBounds();
mesh.RecalcCenter();
}
break;
case NodeAttribute.FbxNodeType.CAMERA:
var cam = root.Attribute as CameraAttribute;
node = new Camera3D {
Id = root.Name,
FieldOfView = cam.FieldOfView * Mathf.DegToRad,
AspectRatio = cam.AspectRatio,
NearClipPlane = cam.NearClipPlane,
FarClipPlane = cam.FarClipPlane,
};
node.SetLocalTransform(CorrectCameraTransform(root.LocalTranform));
break;
default:
node = new Node3D {
Id = root.Name
};
node.SetLocalTransform(root.LocalTranform);
break;
}
if (node != null)
{
if (parent != null)
{
parent.Nodes.Add(node);
}
foreach (var child in root.Children)
{
ImportNodes(child, node);
}
}
return(node);
}
private static ContentSizeComponent ProcessNode3D(Node3D node3D, Dictionary <Node, ContentSizeComponent> children)
{
// TODO: Content size for 3D nodes
return(null);
}
private void ProcessNode3DLinks(IReadOnlyDictionary <Node3D, HashSet <SplineGear3D> > links, Node3D node3D, RollNodeView view) => ProcessLinks <SplineGear3DLinkIndicatorButton, Node3D, SplineGear3D, Spline3D>(links, node3D, view);
public void Visit(Node3D node3d)
{
System.Console.WriteLine("Counting by a new method: Node3D");
Nodes3d++;
}
/// <summary>
/// No distance implementation possible for PseudoEuclidean in 3D
/// </summary>
/// <exception cref="NotSupportedException">Not supported</exception>
protected override double Distance(Node3D a, Node3D b)
{
throw new NotSupportedException("No distance implementation possible for PseudoEuclidean in 3D");
}
// Run A* and update the path of nodes we want to travel
private void ComputePath(Vector3 startPos, Vector3 targetPos)
{
Node3D startNode = graph.NearestNode(startPos);
Node3D targetNode = graph.NearestNode(targetPos);
HashSet<Node3D> visited = new HashSet<Node3D>();
PriorityQueue<float, Node3D> frontier = new PriorityQueue<float, Node3D>();
frontier.Enqueue(0f, startNode);
// initialize map of parents (in-edge neighbor to each vertex) for path reconstruction
Node3D[,,] parents = new Node3D[graph.xgrid + 1, graph.ygrid + 1, graph.zgrid + 1];
parents[startNode.x, startNode.y, startNode.z] = startNode;
// initialize costs
float[,,] costs = new float[graph.xgrid + 1, graph.ygrid + 1, graph.zgrid + 1];
for (int i = 0; i < graph.xgrid; i++) {
for (int j = 0; j < graph.ygrid; j++) {
for (int k = 0; k < graph.zgrid; k++) {
if (i == startNode.x && j == startNode.y && k == startNode.z) {
costs[i, j, k] = 0f;
} else {
costs[i, j, k] = -1f;
}
}
}
}
bool foundPath = false;
Node3D current;
while (!frontier.IsEmpty) {
current = frontier.DequeueValue();
if (current == targetNode) {
foundPath = true;
break;
}
visited.Add(current);
Node3D[] neighbors = graph.neighbors[current.x][current.y][current.z];
for (int i = 0; i < neighbors.Length; i++) {
if (visited.Contains(neighbors[i])) {
continue;
}
float cost = costs[current.x, current.y, current.z] + graph.dist;
// If we found a better cost/distance, add to frontier
if (costs[neighbors[i].x, neighbors[i].y, neighbors[i].z] < 0f || cost < costs[neighbors[i].x, neighbors[i].y, neighbors[i].z]) {
costs[neighbors[i].x, neighbors[i].y, neighbors[i].z] = cost;
parents[neighbors[i].x, neighbors[i].y, neighbors[i].z] = current;
float heuristic = HeuristicValue(neighbors[i], targetNode);
frontier.Enqueue(cost + heuristic, neighbors[i]);
}
}
}
// Either we found a path, or finished searching with no results
if (!foundPath) {
return;
}
// Reconstruct path using parents
current = targetNode;
List<Vector3> newPath = new List<Vector3>();
newPath.Add(graph.WorldPosition(current));
while (current != startNode) {
current = parents[current.x, current.y, current.z];
newPath.Add(graph.WorldPosition(current));
}
// Smooth path by deleting unneeded nodes (going in reverse)
path.Clear();
int p0 = newPath.Count - 1;
int p1 = newPath.Count - 2;
path.Add(newPath[p0]);
RaycastHit hit = new RaycastHit();
while (p1 >= 0) {
float dist = Vector3.Distance(newPath[p0], newPath[p1]);
Vector3 dir = newPath[p1] - newPath[p0];
Vector3 normal = Vector3.Normalize(Vector3.Cross(dir, Vector3.up)) * 0.5f;
if (p1 == 0 || newPath[p0].y != newPath[p1].y || newPath[p0].y > 2f || newPath[p1].y > 2f || p0 - p1 > 3 ||
(Physics.Raycast(newPath[p0], dir, out hit, dist) && hit.collider.tag == graph.collisionTag) ||
(Physics.Raycast(newPath[p0] + normal, dir, out hit, dist) && hit.collider.tag == graph.collisionTag) ||
(Physics.Raycast(newPath[p0] - normal, dir, out hit, dist) && hit.collider.tag == graph.collisionTag)) {
path.Add(newPath[p1 + 1]);
p0 = p1;
}
p1--;
}
}
/// <summary>
/// No distance implementation possible for Geographical location in 3D
/// </summary>
/// <exception cref="NotSupportedException">Not supported</exception>
protected override double Distance(Node3D a, Node3D b)
{
throw new NotSupportedException("No distance implementation possible for Geographical location in 3D");
}
// Use this for initialization
void Start()
{
RaycastHit hit = new RaycastHit();
float width = Mathf.Abs(endpos.x - startpos.x);
float depth = Mathf.Abs(endpos.y - startpos.y);
float height = Mathf.Abs(endpos.z - startpos.z);
xgrid = Mathf.CeilToInt(width / dist);
ygrid = Mathf.CeilToInt(depth / vdist);
zgrid = Mathf.CeilToInt(height / dist);
startx = Mathf.Min(startpos.x, endpos.x);
starty = Mathf.Min(startpos.y, endpos.y);
startz = Mathf.Min(startpos.z, endpos.z);
int i, j, k;
// Compute valid vertices (not floating in space)
valid = new bool[xgrid + 1][][];
for (i = 0; i <= xgrid; i++) {
valid[i] = new bool[ygrid + 1][];
for (j = 0; j <= ygrid; j++) {
valid[i][j] = new bool[zgrid + 1];
for (k = zgrid; k >= 0; k--) {
Vector3 currpos = new Vector3(startx + i*dist, starty + j*vdist, startz + k*dist);
if (Physics.Raycast(currpos, Vector3.down, out hit, vdist) && hit.collider.tag == collisionTag) {
float toGround = hit.distance;
bool rightleft = Physics.Raycast(currpos + checkwidth * Vector3.right, Vector3.down, out hit, toGround + 0.001f) && hit.collider.tag == collisionTag
&& Physics.Raycast(currpos + checkwidth * Vector3.left, Vector3.down, out hit, toGround + 0.001f) && hit.collider.tag == collisionTag;
bool forwardback = Physics.Raycast(currpos + checkwidth * Vector3.forward, Vector3.down, out hit, toGround + 0.001f) && hit.collider.tag == collisionTag
&& Physics.Raycast(currpos + checkwidth * Vector3.back, Vector3.down, out hit, toGround + 0.001f) && hit.collider.tag == collisionTag;
if (rightleft && forwardback) {
valid[i][j][k] = true;
} else if (rightleft && Physics.Raycast(currpos + checkwidth * Vector3.forward, Vector3.down, out hit, toGround + checkwidth/2f + 0.001f) && hit.collider.tag == collisionTag
&& Physics.Raycast(currpos + checkwidth * Vector3.back, Vector3.down, out hit, toGround + checkwidth/2f + 0.001f) && hit.collider.tag == collisionTag) {
valid[i][j][k] = true;
} else if (forwardback && Physics.Raycast(currpos + checkwidth * Vector3.right, Vector3.down, out hit, toGround + checkwidth/2f + 0.001f) && hit.collider.tag == collisionTag
&& Physics.Raycast(currpos + checkwidth * Vector3.left, Vector3.down, out hit, toGround + checkwidth/2f + 0.001f) && hit.collider.tag == collisionTag) {
valid[i][j][k] = true;
}
} else {
valid[i][j][k] = false;
}
continue;
}
}
}
// Compute neighbors and edges
neighbors = new Node3D[xgrid + 1][][][];
for (i = 0; i <= xgrid; i++) {
neighbors[i] = new Node3D[ygrid + 1][][];
for (j = 0; j <= ygrid; j++) {
neighbors[i][j] = new Node3D[zgrid + 1][];
Node3D[] temp = new Node3D[12];
float ramplen = Mathf.Sqrt(Mathf.Pow(dist, 2f) + Mathf.Pow(vdist, 2f));
for (k = 0; k <= zgrid; k++) {
if (!is_valid(i, j, k)) {
neighbors[i][j][k] = new Node3D[0];
continue;
}
int count = 0;
Vector3 currpos = new Vector3(startx + i*dist, starty + j*vdist, startz + k*dist);
if (is_valid(i, j, k+1) && !(Physics.Raycast(currpos, Vector3.forward, out hit, dist) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i, j, k+1);
count++;
}
if (is_valid(i, j, k-1) && !(Physics.Raycast(currpos, Vector3.back, out hit, dist) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i, j, k-1);
count++;
}
if (is_valid(i-1, j, k) && !(Physics.Raycast(currpos, Vector3.left, out hit, dist) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i-1, j, k);
count++;
}
if (is_valid(i+1, j, k) && !(Physics.Raycast(currpos, Vector3.right, out hit, dist) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i+1, j, k);
count++;
}
if (is_valid(i, j+1, k+1) && !(Physics.Raycast(currpos, dist * Vector3.forward + vdist * Vector3.up, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i, j+1, k+1);
count++;
}
if (is_valid(i, j+1, k-1) && !(Physics.Raycast(currpos, dist * Vector3.back + vdist * Vector3.up, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i, j+1, k-1);
count++;
}
if (is_valid(i+1, j+1, k) && !(Physics.Raycast(currpos, dist * Vector3.right + vdist * Vector3.up, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i+1, j+1, k);
count++;
}
if (is_valid(i-1, j+1, k) && !(Physics.Raycast(currpos, dist * Vector3.left + vdist * Vector3.up, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i-1, j+1, k);
count++;
}
if (is_valid(i, j-1, k+1) && !(Physics.Raycast(currpos, dist * Vector3.forward + vdist * Vector3.down, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i, j-1, k+1);
count++;
}
if (is_valid(i, j-1, k-1) && !(Physics.Raycast(currpos, dist * Vector3.back + vdist * Vector3.down, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i, j-1, k-1);
count++;
}
if (is_valid(i+1, j-1, k) && !(Physics.Raycast(currpos, dist * Vector3.right + vdist * Vector3.down, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i+1, j-1, k);
count++;
}
if (is_valid(i-1, j-1, k) && !(Physics.Raycast(currpos, dist * Vector3.left + vdist * Vector3.down, out hit, ramplen) && hit.collider.tag == collisionTag)) {
temp[count] = new Node3D(i-1, j-1, k);
count++;
}
int l;
neighbors[i][j][k] = new Node3D[count];
for (l = 0; l < count; l++) {
neighbors[i][j][k][l] = new Node3D(temp[l].x, temp[l].y, temp[l].z);
}
}
}
}
}
public Vector3 WorldPosition(Node3D pos)
{
Vector3 result = new Vector3(startx + pos.x * dist, starty + pos.y * vdist, startz + pos.z * dist);
return result;
}
