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())); }