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