protected override void SolveInstance(IGH_DataAccess DA)
{
string title = "";
List <double> xValues = new List <double>();
List <double> yValues = new List <double>();
string xName = "";
string yName = "";
Color color = new Color();
DA.GetData(0, ref title);
DA.GetDataList(1, xValues);
DA.GetDataList(2, yValues);
DA.GetData(3, ref xName);
DA.GetData(4, ref yName);
DA.GetData(5, ref color);
PointGraph graphObject = new PointGraph();
dialogImage = graphObject;
PterodactylGrasshopperBitmapGoo GH_bmp = new PterodactylGrasshopperBitmapGoo();
graphObject.PointGraphData(true, title, xValues, yValues, xName, yName, color, GH_bmp.ReferenceTag);
using (Bitmap b = graphObject.ExportBitmap())
{
GH_bmp.Value = b.Clone(new Rectangle(0, 0, b.Width, b.Height), b.PixelFormat);
GH_bmp.ReportPart = graphObject.Create();
DA.SetData(0, GH_bmp);
}
}
public PointGraph CalculateShortestPathFromSource(PointGraph graph, PointNode source)
{
source.Distance = 0;
var settledNodes = new HashSet <PointNode>();
var unsettledNodes = new HashSet <PointNode>
{
source
};
while (unsettledNodes.Count != 0)
{
var currentNode = GetLowestDistanceNode(unsettledNodes);
unsettledNodes.Remove(currentNode);
foreach ((var adjacentNode, var edgeWeight) in currentNode.AdjacentNodes)
{
if (!settledNodes.Contains(adjacentNode))
{
CalculateMinimumDistance(adjacentNode, edgeWeight, currentNode);
unsettledNodes.Add(adjacentNode);
}
}
settledNodes.Add(currentNode);
}
return(graph);
}
protected override void SolveInstance(IGH_DataAccess DA)
{
bool showGraph = false;
string title = "";
List <double> xValues = new List <double>();
List <double> yValues = new List <double>();
string xName = "";
string yName = "";
Color color = new Color();
string path = "";
DA.GetData(0, ref showGraph);
DA.GetData(1, ref title);
DA.GetDataList(2, xValues);
DA.GetDataList(3, yValues);
DA.GetData(4, ref xName);
DA.GetData(5, ref yName);
DA.GetData(6, ref color);
DA.GetData(7, ref path);
PointGraph graphObject = new PointGraph();
graphObject.PointGraphData(showGraph, title, xValues, yValues, xName, yName, color, path);
if (showGraph)
{
graphObject.ShowDialog();
}
graphObject.Export();
string reportPart = graphObject.Create();
DA.SetData(0, reportPart);
}
void CreateGraph(AstarPath astar)
{
if (!points)
{
Debug.LogWarning("missing points root for autopointgraph " + name);
}
var graphs = new List<NavGraph>(astar.graphs);
if (graphs.Contains(graph))
{
return;
}
graph = new PointGraph();
graph.active = astar;
graph.name = "AutoPointGraph (" + name + ")";
graph.root = points;
graph.raycast = true;
graph.maxDistance = 0;
graph.recursive = true;
graph.mask = -1;
graphs.Add(graph);
astar.graphs = graphs.ToArray();
}
public override void OnDrawGizmos()
{
PointGraph graph = target as PointGraph;
//Debug.Log ("Gizmos "+(graph == null)+" "+target);
if (graph == null)
{
return;
}
//Handles.color = new Color (0.161F,0.341F,1F,0.5F);
Gizmos.color = new Color(0.161F, 0.341F, 1F, 0.5F);
//for (int i=0;i<graph.nodes.Length;i++) {
if (graph.root != null)
{
DrawChildren(graph, graph.root);
}
else
{
GameObject[] gos = GameObject.FindGameObjectsWithTag(graph.searchTag);
for (int i = 0; i < gos.Length; i++)
{
Gizmos.DrawCube(gos[i].transform.position, Vector3.one * HandleUtility.GetHandleSize(gos[i].transform.position) * 0.1F);
}
}
}
public override void OnInspectorGUI(NavGraph target)
{
PointGraph graph = target as PointGraph;
/*
#if UNITY_3_3
* graph.root = (Transform)EditorGUILayout.ObjectField (new GUIContent ("Root","All childs of this object will be used as nodes, if it is not set, a tag search will be used instead (see below)"),graph.root,typeof(Transform));
#else
* graph.root = (Transform)EditorGUILayout.ObjectField (new GUIContent ("Root","All childs of this object will be used as nodes, if it is not set, a tag search will be used instead (see below)"),graph.root,typeof(Transform),true);
#endif
*/
//Debug.Log (EditorGUI.indentLevel);
graph.root = ObjectField(new GUIContent("Root", "All childs of this object will be used as nodes, if it is not set, a tag search will be used instead (see below)"), graph.root, typeof(Transform), true) as Transform;
graph.recursive = EditorGUILayout.Toggle(new GUIContent("Recursive", "Should childs of the childs in the root GameObject be searched"), graph.recursive);
graph.searchTag = EditorGUILayout.TagField(new GUIContent("Tag", "If root is not set, all objects with this tag will be used as nodes"), graph.searchTag);
if (graph.root != null)
{
GUILayout.Label("All childs " + (graph.recursive ? "and sub-childs ":"") + "of 'root' will be used as nodes\nSet root to null to use a tag search instead", AstarPathEditor.helpBox);
}
else
{
GUILayout.Label("All object with the tag '" + graph.searchTag + "' will be used as nodes" + (graph.searchTag == "Untagged" ? "\nNote: the tag 'Untagged' cannot be used" : ""), AstarPathEditor.helpBox);
}
graph.maxDistance = EditorGUILayout.FloatField(new GUIContent("Max Distance", "The max distance in world space for a connection to be valid. A zero counts as infinity"), graph.maxDistance);
EditorGUIUtility.LookLikeControls();
#if UNITY_4
graph.limits = EditorGUILayout.Vector3Field("Max Distance (axis aligned)", graph.limits);
#else
EditorGUILayoutx.BeginIndent();
graph.limits = EditorGUILayout.Vector3Field("Max Distance (axis aligned)", graph.limits);
EditorGUILayoutx.EndIndent();
#endif
EditorGUIUtility.LookLikeInspector();
graph.raycast = EditorGUILayout.Toggle(new GUIContent("Raycast", "Use raycasting to check if connections are valid between each pair of nodes"), graph.raycast);
editor.GUILayoutx.BeginFadeArea(graph.raycast, "raycast");
EditorGUI.indentLevel++;
graph.thickRaycast = EditorGUILayout.Toggle(new GUIContent("Thick Raycast", "A thick raycast checks along a thick line with radius instead of just along a line"), graph.thickRaycast);
editor.GUILayoutx.BeginFadeArea(graph.thickRaycast, "thickRaycast");
graph.thickRaycastRadius = EditorGUILayout.FloatField(new GUIContent("Raycast Radius", "The radius in world units for the thick raycast"), graph.thickRaycastRadius);
editor.GUILayoutx.EndFadeArea();
//graph.mask = 1 << EditorGUILayout.LayerField ("Mask",(int)Mathf.Log (graph.mask,2));
graph.mask = EditorGUILayoutx.LayerMaskField(/*new GUIContent (*/ "Mask" /*,"Used to mask which layers should be checked")*/, graph.mask);
EditorGUI.indentLevel--;
editor.GUILayoutx.EndFadeArea();
}
public void DrawChildren(PointGraph graph, Transform tr)
{
foreach (Transform child in tr) {
Gizmos.DrawCube (child.position,Vector3.one*HandleUtility.GetHandleSize(child.position)*0.1F);
//Handles.CubeCap (-1,graph.nodes[i].position,Quaternion.identity,HandleUtility.GetHandleSize(graph.nodes[i].position)*0.1F);
//Gizmos.DrawCube (nodes[i].position,Vector3.one);
if (graph.recursive) DrawChildren (graph, child);
}
}
static void AddScenePointNode()
{
AstarPath aStarPath = GameObject.FindObjectOfType <AstarPath>();
if (aStarPath != null)
{
Selection.activeGameObject = aStarPath.gameObject;
EditorUtility.DisplayDialog("注意", "当前场景已经有路点节点", "关闭");
}
else
{
GameObject go = new GameObject("AStarPath");
GameObject pointRoot = new GameObject("NodeRoot");
pointRoot.transform.parent = go.transform;
GameObject node0 = GameObject.CreatePrimitive(PrimitiveType.Cube);
node0.name = "pathNode";
node0.layer = LayerMask.NameToLayer("Scenes_mesh");
node0.transform.parent = pointRoot.transform;
Collider coll = node0.GetComponent <Collider>();
if (coll)
{
GameObject.DestroyImmediate(coll);
}
MeshFilter filter = node0.GetComponent <MeshFilter>();
if (filter)
{
GameObject.DestroyImmediate(filter);
}
Renderer ren = node0.GetComponent <Renderer>();
if (ren)
{
GameObject.DestroyImmediate(ren);
}
go.AddComponent <AstarPathGo>();
aStarPath = go.AddComponent <AstarPath>();
aStarPath.colorSettings = aStarPath.colorSettings ?? new AstarColor();
aStarPath.data = new AstarData();
PointGraph graph = System.Activator.CreateInstance(typeof(PointGraph)) as PointGraph;
graph.root = pointRoot.transform;
graph.maxDistance = 1;
graph.raycast = true;
graph.optimizeForSparseGraph = true;
graph.mask = 1 << LayerMask.NameToLayer("Scenes_mesh");
graph.active = aStarPath;
aStarPath.data.graphs = new NavGraph[1];
aStarPath.data.graphs[0] = graph;
NodeAttachFloor();
}
UnityEditor.SceneManagement.EditorSceneManager.MarkSceneDirty(UnityEditor.SceneManagement.EditorSceneManager.GetActiveScene());
}
public void DrawChildren(PointGraph graph, Transform tr)
{
foreach (Transform child in tr)
{
Gizmos.DrawCube(child.position, Vector3.one * HandleUtility.GetHandleSize(child.position) * 0.1F);
//Handles.CubeCap (-1,graph.nodes[i].position,Quaternion.identity,HandleUtility.GetHandleSize(graph.nodes[i].position)*0.1F);
//Gizmos.DrawCube (nodes[i].position,Vector3.one);
if (graph.recursive)
{
DrawChildren(graph, child);
}
}
}
/**
* creates the navmesh for our grid
*/
private void CreateGraph()
{
AstarData data = AstarPath.active.data;
var graphs = data.graphs.ToList();
graphs.ForEach(x => data.RemoveGraph(x));
PointGraph graph = data.AddGraph(typeof(PointGraph)) as PointGraph;
AstarPath.active.Scan(graph);
// Make sure we only modify the graph when all pathfinding threads are paused
AstarPath.active.AddWorkItem(new AstarWorkItem(ctx => {
//create the graph
//first make node array
PointNode[,] nodeArray = new PointNode[map.GetUpperBound(0) + 1, map.GetUpperBound(1) + 1];
for (int y = 0; y <= map.GetUpperBound(0); y++)
{
for (int x = 0; x <= map.GetUpperBound(1); x++)
{
nodeArray[x, y] = graph.AddNode((Int3) new Vector3(x, y, 0));
}
}
int connections = 0;
//now connect nodes
for (int y = 0; y <= map.GetUpperBound(0); y++)
{
for (int x = 0; x <= map.GetUpperBound(1); x++)
{
for (int i = x - 1; i <= x + 1; i++)
{
for (int j = y - 1; j <= y + 1; j++)
{
if (InBounds(nodeArray, i, j))
{
nodeArray[x, y].AddConnection(nodeArray[i, j], (uint)map[x, y].MoveCost);
connections++;
}
}
}
}
}
}));
// Run the above work item immediately
AstarPath.active.FlushWorkItems();
}
// Use this for initialization
public override void OnEnter()
{
FsmNavGraph go = graph.Value as FsmNavGraph;
if (go.Value as PointGraph == null)
{
Finish(); return;
}
g = go.Value as PointGraph;
DoStuff();
if (!everyFrame.Value)
{
Finish();
}
}
public override void OnEnter()
{
mo = graph.Value as FsmNavGraph;
if ((mo == null) || (mo.Value == null) || alwaysNew.Value)
{
AstarPath.active.astarData.AddGraph(mo.Value);
g = FsmConverter.GetNavGraph(graph) as PointGraph;
Debug.Log("Creating New Point Graph");
graph.Value = FsmConverter.SetNavGraph(g as NavGraph);
}
else
{
g = FsmConverter.GetNavGraph(graph) as PointGraph;
}
DoStuff();
Finish();
}
public override void OnEnter()
{
mo = graph.Value as FsmPointGraph;
//Debug.Log(typeof(mo));
if ((mo == null) || (mo.Value == null) || alwaysNew.Value)
{
AstarPath.active.astarData.AddGraph(mo.Value);
g = FsmConverter.GetPointGraph(graph) as PointGraph;
Debug.Log("Creating New Point Graph");
graph.Value = FsmConverter.SetPointGraph(g);
} // create a PointGraph if the variable does not contain a valid one already.
else
{
g = FsmConverter.GetPointGraph(graph) as PointGraph;
}
DoStuff();
Finish();
}
public override void OnInspectorGUI(NavGraph target)
{
PointGraph graph = target as PointGraph;
graph.root = ObjectField(new GUIContent("Root", "All children of this object will be used as nodes, if it is not set, a tag search will be used instead (see below)"), graph.root, typeof(Transform), true) as Transform;
graph.recursive = EditorGUILayout.Toggle(new GUIContent("Recursive", "Should children of the children in the root GameObject be searched"), graph.recursive);
graph.searchTag = EditorGUILayout.TagField(new GUIContent("Tag", "If root is not set, all objects with this tag will be used as nodes"), graph.searchTag);
if (graph.root != null)
{
EditorGUILayout.HelpBox("All children " + (graph.recursive ? "and sub-children " : "") + "of 'root' will be used as nodes\nSet root to null to use a tag search instead", MessageType.None);
}
else
{
EditorGUILayout.HelpBox("All object with the tag '" + graph.searchTag + "' will be used as nodes" + (graph.searchTag == "Untagged" ? "\nNote: the tag 'Untagged' cannot be used" : ""), MessageType.None);
}
graph.maxDistance = EditorGUILayout.FloatField(new GUIContent("Max Distance", "The max distance in world space for a connection to be valid. A zero counts as infinity"), graph.maxDistance);
graph.limits = EditorGUILayout.Vector3Field("Max Distance (axis aligned)", graph.limits);
graph.raycast = EditorGUILayout.Toggle(new GUIContent("Raycast", "Use raycasting to check if connections are valid between each pair of nodes"), graph.raycast);
if (graph.raycast)
{
EditorGUI.indentLevel++;
graph.thickRaycast = EditorGUILayout.Toggle(new GUIContent("Thick Raycast", "A thick raycast checks along a thick line with radius instead of just along a line"), graph.thickRaycast);
if (graph.thickRaycast)
{
graph.thickRaycastRadius = EditorGUILayout.FloatField(new GUIContent("Raycast Radius", "The radius in world units for the thick raycast"), graph.thickRaycastRadius);
}
graph.mask = EditorGUILayoutx.LayerMaskField(/*new GUIContent (*/ "Mask" /*,"Used to mask which layers should be checked")*/, graph.mask);
EditorGUI.indentLevel--;
}
}
void scanSelf()
{
if (this.Visible)
{
PointGraph navg = (PointGraph)AstarPath.active.graphs[0];
AstarPath.active.AddWorkItem(new AstarPath.AstarWorkItem(delegate() {
//float f = Time.realtimeSinceStartup;
PointNode p = navg.AddNode((Int3)pathLocation);
if (p != null)
{
Dictionary <GraphNode, float> cons = new Dictionary <GraphNode, float>();
navg.GetNodes((delegate(GraphNode node) {
float distance;
if (navg.IsValidConnection(p, node, out distance))
{
//Debug.Log(distance);
cons[node] = distance;
}
return(true);
}));
foreach (GraphNode g in cons.Keys)
{
p.Area = 0;
g.Area = 0;
p.AddConnection(g, (uint)cons[g]);
g.AddConnection(p, (uint)cons[g]);
}
//Debug.Log("Found " + cons.Count + " valid connections");
//Debug.Log("Scan time: " + (Time.realtimeSinceStartup - f));
}
}, null));
}
}
public static List <PointGraph> BuildPointGraph(Vector3[] vertices, int[] indices)
{
List <PointGraph> list = new List <PointGraph>();
// Pour chaque vertex, on cherche ses voisins
for (int i = 0; i < vertices.Length; i++)
{
PointGraph p = new PointGraph();
p.neighbours = new List <int>();
p.index = i;
// Pour chaque triangles
for (int j = 0; j < indices.Length / 3; j++)
{
// si un des indices correspond à self
//if (vertices[indices[(j * 3)+0]] == vertices[i] || vertices[indices[(j * 3)+1]] == vertices[i] || vertices[indices[(j * 3)+2]] == vertices[i])
if (indices[(j * 3) + 0] == i || indices[(j * 3) + 1] == i || indices[(j * 3) + 2] == i)
{
// Pour les 3 points du triangle
for (int k = 0; k < 3; k++)
{
// Si c'est ni self ni déjà présent dans la liste on l'ajoute
//if (vertices[indices[(j * 3) + k]] != vertices[i] && !p.neighbours.Contains(indices[(j * 3) + k]))
if (indices[(j * 3) + k] != i && !p.neighbours.Contains(indices[(j * 3) + k]))
{
p.neighbours.Add(indices[(j * 3) + k]);
}
}
}
}
//UnityEngine.Debug.Log(p.neighbours.Count);
list.Add(p);
}
return(list);
}
public void DrawChildren (PointGraph graph, Transform tr) {
foreach (Transform child in tr) {
Gizmos.DrawCube (child.position,Vector3.one*HandleUtility.GetHandleSize(child.position)*0.1F);
if (graph.recursive) DrawChildren (graph, child);
}
}
public void Setup()
{
_underlyingDirectedGraph = A.Fake <IDirectedGraph <IPoint, PointEquation> >();
_pointUpdateSubscriber = A.Fake <IPointUpdateSubscriber>();
_pointGraph = new PointGraph(_underlyingDirectedGraph, _pointUpdateSubscriber);
}
void Start()
{
int startTime = Environment.TickCount;
var objects = GameObject.FindGameObjectsWithTag("Obstacle");
var objectCount = objects.Length;
foreach (var obj in objects)
{
Vector3 center = obj.GetComponent <Collider>().bounds.center;
Vector3 size = obj.GetComponent <Collider>().bounds.size;
Vector3 boundPoint1 = obj.GetComponent <Collider>().bounds.min;
Vector3 boundPoint2 = obj.GetComponent <Collider>().bounds.max;
Vector3[] boundPoints = new Vector3[] {
new Vector3(boundPoint1.x - offset, boundPoint1.y - offset, boundPoint1.z - offset),
new Vector3(boundPoint2.x + offset, boundPoint2.y + offset, boundPoint2.z + offset),
new Vector3(boundPoint1.x - offset, boundPoint1.y - offset, boundPoint2.z + offset),
new Vector3(boundPoint1.x - offset, boundPoint2.y + offset, boundPoint1.z - offset),
new Vector3(boundPoint2.x + offset, boundPoint1.y - offset, boundPoint1.z - offset),
new Vector3(boundPoint1.x - offset, boundPoint2.y + offset, boundPoint2.z + offset),
new Vector3(boundPoint2.x + offset, boundPoint1.y - offset, boundPoint2.z + offset),
new Vector3(boundPoint2.x + offset, boundPoint2.y + offset, boundPoint1.z - offset)
};
foreach (Vector3 point in boundPoints)
{
points.Add(point);
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
cube.transform.localScale = new Vector3(.1f, .1f, .1f);
cube.transform.position = point;
cube.GetComponent <Renderer>().material.color = Color.red;
}
}
// Iterate through each point and try to connect to each other point
bool[,] connectedGrid = new bool[points.Count, points.Count];
int numedges = 0;
for (int i = 0; i < points.Count; i++)
{
Vector3 point = points[i];
for (int j = 0; j < points.Count; j++)
{
if (i == j)
{
continue;
}
RaycastHit hit;
if (Physics.Raycast(points[i], points[j] - points[i], out hit, Vector3.Distance(points[i], points[j])))
{
// Debug.DrawRay(points[i], points[j] - points[i] * hit.distance, Color.yellow, 100, false);
// Debug.Log("Did Hit");
connectedGrid[i, j] = false;
}
else
{
// Debug.DrawRay(points[i], points[j] - points[i], Color.cyan, 100, false);
connectedGrid[i, j] = true;
// Debug.Log("Did not Hit");
numedges++;
}
}
}
int endTime = Environment.TickCount;
Debug.Log(string.Format("Time to load: {0}", endTime - startTime));
Debug.Log(String.Format("Num edges: {0}", numedges));
PointGraph graph = new PointGraph();
graph.initialize(points, connectedGrid);
graph.createGraph();
Astar astar = new Astar(graph);
float[] floatorigin = new float[] { 0, 0, 0 };
float[] floattarget = new float[] { 1, 1, 1 };
startTime = Environment.TickCount;
var answer = astar.search(floatorigin, floattarget);
endTime = Environment.TickCount;
Debug.Log(string.Format("Time to search: {0}", endTime - startTime));
List <Vector3> vectorPath;
if (answer == null)
{
Debug.Log("ASTAR: No valid path");
vectorPath = null;
}
else
{
startTime = Environment.TickCount;
vectorPath = astar.PathToVectors();
endTime = Environment.TickCount;
Debug.Log(string.Format("Time to convert to vector: {0}", endTime - startTime));
}
foreach (Vector3 vec in vectorPath)
{
// Debug.Log(vec.ToString());
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Sphere);
cube.transform.localScale = new Vector3(1f, 1f, 1f);
cube.transform.position = vec;
cube.GetComponent <Renderer>().material.color = Color.green;
}
for (int i = 1; i < vectorPath.Count; i++)
{
Debug.DrawRay(vectorPath[i], vectorPath[i - 1] - vectorPath[i], Color.green, 1000, false);
//Vector3.Distance(vectorPath[i], vectorPath[i-1])
}
Debug.Log(string.Format("StraightLineRatio: {0}", astar.StraightLineRatio()));
}
void Start()
{
int startTime = Environment.TickCount;
var objects = GameObject.FindGameObjectsWithTag("Obstacle");
var objectCount = objects.Length;
foreach (var obj in objects)
{
// Create bounding points for each obstacle
Vector3 center = obj.GetComponent <Collider>().bounds.center;
Vector3 size = obj.GetComponent <Collider>().bounds.size;
Vector3 boundPoint1 = obj.GetComponent <Collider>().bounds.min;
Vector3 boundPoint2 = obj.GetComponent <Collider>().bounds.max;
Vector3[] boundPoints = new Vector3[] {
new Vector3(boundPoint1.x - offset, boundPoint1.y - offset, boundPoint1.z - offset),
new Vector3(boundPoint2.x + offset, boundPoint2.y + offset, boundPoint2.z + offset),
new Vector3(boundPoint1.x - offset, boundPoint1.y - offset, boundPoint2.z + offset),
new Vector3(boundPoint1.x - offset, boundPoint2.y + offset, boundPoint1.z - offset),
new Vector3(boundPoint2.x + offset, boundPoint1.y - offset, boundPoint1.z - offset),
new Vector3(boundPoint1.x - offset, boundPoint2.y + offset, boundPoint2.z + offset),
new Vector3(boundPoint2.x + offset, boundPoint1.y - offset, boundPoint2.z + offset),
new Vector3(boundPoint2.x + offset, boundPoint2.y + offset, boundPoint1.z - offset)
};
foreach (Vector3 point in boundPoints)
{
points.Add(point);
// Show point with a red box
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
cube.transform.localScale = new Vector3(.1f, .1f, .1f);
cube.transform.position = point;
cube.GetComponent <Renderer>().material.color = Color.red;
}
}
/* Delauny triangulation
*
* Defined as a set of tetrahedra with vertices in S such that the sphere defined by the set of
* vertices of each tetrahedron does not contain any nodes in S
*
* Steps:
*
* 1. Create enclosing tetrahedron
* 2. Add point from S
* 3. Iterate through all spheres to check if the sphere contains the point
* 3.1 If the sphere does not, keep the tetrahedron
* 3.2 If it does, delete the tetrahedron and connect all points in the
* tetrahedron to the current point to create smaller terrahedra.
* 4. Do this for all points in s.
*
* Runtime:
*
* Should run in O(n^5/3) for 3d but not sure how they check if points are in stuff etc.
*
*/
// Bounding tetrahedron points
Vector3 point5 = new Vector3(11, 620, 15);
Vector3 point6 = new Vector3(-512, -115, 340);
Vector3 point7 = new Vector3(4, -115, -495);
Vector3 point8 = new Vector3(544, -115, 340);
List <Vector3> tetbounding = new List <Vector3>();
tetbounding.Add(point5);
tetbounding.Add(point6);
tetbounding.Add(point7);
tetbounding.Add(point8);
// Create initial tetrahedron
var initialtet = new Tetrahedron(point5, point6, point7, point8);
// initialtet.display();
List <Tetrahedron> tetrahedra = new List <Tetrahedron>();
List <Tetrahedron> badTetrahedra = new List <Tetrahedron>();
tetrahedra.Add(initialtet);
int c = 0;
// Sorting points dramatically reduces runtime
points.Sort((a, b) => a[0].CompareTo(b[0]));
foreach (Vector3 p in points)
{
// Iterate through points adding one at a time
badTetrahedra.Clear();
Debug.Log("Starting first tetrahedron");
foreach (Tetrahedron tet in tetrahedra)
{
if (tet.contains(p))
{
badTetrahedra.Add(tet);
// Debug.Log("Found bad tetrahedron");
}
}
Debug.Log(String.Format("Number of bad tetrahedra: {0}", badTetrahedra.Count));
List <Face> faces = new List <Face>();
foreach (Tetrahedron tet in badTetrahedra)
{
// Need to add triangle face which is not shared by any other tetrahedron
var tetfaces = tet.faces();
// Add all faces
foreach (Face face in tetfaces)
{
faces.Add(face);
}
}
// Check how many instances of a face there are and remove all if there is more than 1
List <Face> newfaces = new List <Face>();
foreach (Face face in faces)
{
int num = 0;
foreach (Face newface in faces)
{
if (newface.equals(face))
{
num++;
}
}
if (num == 1)
{
newfaces.Add(face);
}
}
Debug.Log(String.Format("Number of faces: {0}", newfaces.Count));
foreach (Face face in newfaces)
{
// Create a new tetrahedron using this face and the test point
var newtet = new Tetrahedron(p, face.vertices[0], face.vertices[1], face.vertices[2]);
tetrahedra.Add(newtet);
// Debug.Log("Created new tetrahedron");
}
foreach (Tetrahedron tet in badTetrahedra)
{
// Want to remove that tetrahedron and create new ones to fill space
tetrahedra.Remove(tet);
}
Debug.Log("Ending first tetrahedron");
c++;
}
// Delete tetrahedra which have one of the bounding vertices
List <Tetrahedron> filteredTetrahedra = new List <Tetrahedron>();
foreach (Tetrahedron tet in tetrahedra)
{
if (!(tet.hasPoint(point5) || tet.hasPoint(point6) || tet.hasPoint(point7) || tet.hasPoint(point8)))
{
filteredTetrahedra.Add(tet);
}
}
int endTime = Environment.TickCount;
Debug.Log(string.Format("Time to load: {0}", endTime - startTime));
// foreach(Tetrahedron tet in filteredTetrahedra){
// tet.display();
// }
Debug.Log(String.Format("Number of tetrahedra: {0}", filteredTetrahedra.Count));
foreach (Vector3 a in points)
{
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
cube.transform.localScale = new Vector3(.1f, .1f, .1f);
cube.transform.position = a;
cube.GetComponent <Renderer>().material.color = Color.red;
}
foreach (Vector3 a in tetbounding)
{
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
cube.transform.localScale = new Vector3(.1f, .1f, .1f);
cube.transform.position = a;
cube.GetComponent <Renderer>().material.color = Color.green;
}
// Create a point graph that describes whether each point is connected to another point
bool[,] connectedGrid = new bool[points.Count, points.Count];
// Create a dictionary which maps a vector3 to an integer representing its place in points
Dictionary <Vector3, int> positionmap = new Dictionary <Vector3, int>();
for (int i = 0; i < points.Count; i++)
{
positionmap.Add(points[i], i);
}
int numedges = 0;
int attempts = 0;
foreach (Tetrahedron tet in filteredTetrahedra)
{
List <Vector3> vertices = tet.vertices;
// For each pair of points see if the line hits an obstacle
for (int i = 0; i < vertices.Count; i++)
{
for (int j = 0; j < vertices.Count; j++)
{
attempts++;
if (i == j)
{
continue;
}
RaycastHit hit;
if (Physics.Raycast(vertices[i], vertices[j] - vertices[i], out hit, Vector3.Distance(vertices[i], vertices[j])))
{
// Debug.DrawLine(vertices[i], vertices[j], Color.red, 100000, false);
// Debug.Log("Did Hit");
connectedGrid[positionmap[vertices[i]], positionmap[vertices[j]]] = false;
}
else
{
// Debug.DrawRay(vertices[i], vertices[j] - vertices[i], Color.white, 100000, false);
// connectedGrid[i, j] = true;
connectedGrid[positionmap[vertices[i]], positionmap[vertices[j]]] = true;
// Debug.Log("Did not Hit");
}
}
}
}
for (int i = 0; i < points.Count; i++)
{
for (int j = 0; j < points.Count; j++)
{
if (connectedGrid[i, j])
{
numedges++;
}
}
}
Debug.Log(String.Format("Number of edges: {0}", numedges / 2));
Debug.Log(String.Format("Attempts: {0}", attempts));
PointGraph graph = new PointGraph();
graph.initialize(points, connectedGrid);
graph.createGraph();
Astar astar = new Astar(graph);
float[] floatorigin = new float[] { 0, 0, 0 };
float[] floattarget = new float[] { 1, 1, 1 };
startTime = Environment.TickCount;
var answer = astar.search(floatorigin, floattarget);
endTime = Environment.TickCount;
Debug.Log(string.Format("Time to search: {0}", endTime - startTime));
List <Vector3> vectorPath;
if (answer == null)
{
Debug.Log("ASTAR: No valid path");
vectorPath = null;
}
else
{
startTime = Environment.TickCount;
vectorPath = astar.PathToVectors();
endTime = Environment.TickCount;
Debug.Log(string.Format("Time to convert to vector: {0}", endTime - startTime));
}
foreach (Vector3 vec in vectorPath)
{
// Debug.Log(vec.ToString());
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Sphere);
cube.transform.localScale = new Vector3(1f, 1f, 1f);
cube.transform.position = vec;
cube.GetComponent <Renderer>().material.color = Color.green;
}
for (int i = 1; i < vectorPath.Count; i++)
{
Debug.DrawRay(vectorPath[i], vectorPath[i - 1] - vectorPath[i], Color.green, 1000, false);
//Vector3.Distance(vectorPath[i], vectorPath[i-1])
}
Debug.Log(string.Format("StraightLineRatio: {0}", astar.StraightLineRatio()));
}