/*
* Check if two edges collide with eachother.
*/
public static bool EdgesCollide(Edge edge1, Edge edge2, NodeGraph nodeGraph1, NodeGraph nodeGraph2)
{
// Set up Edge1
Node edge1Node1 = nodeGraph1.GetNode(edge1.nodeIndex1);
Node edge1Node2 = nodeGraph1.GetNode(edge1.nodeIndex2);
Vect3 edge1Forward = (edge1Node2.coordinate - edge1Node1.coordinate).GetNormalized();
Vect3 edge1Right = Vect3.Cross(edge1Forward, Vect3.Up).GetNormalized();
Vect3 edge1Up = Vect3.Cross(edge1Right, edge1Forward).GetNormalized();
AxisBBox bBox1 = GetEdgeBoundingBox(edge1, edge1Node1.coordinate, edge1Node2.coordinate,
edge1Right, edge1Up);
// Set up Edge2
Node edge2Node1 = nodeGraph2.GetNode(edge2.nodeIndex1);
Node edge2Node2 = nodeGraph2.GetNode(edge2.nodeIndex2);
Vect3 edge2Forward = (edge2Node2.coordinate - edge2Node1.coordinate).GetNormalized();
Vect3 edge2Right = Vect3.Cross(edge2Forward, Vect3.Up).GetNormalized();
Vect3 edge2Up = Vect3.Cross(edge2Right, edge2Forward).GetNormalized();
AxisBBox bBox2 = GetEdgeBoundingBox(edge2, edge2Node1.coordinate, edge2Node2.coordinate,
edge2Right, edge2Up);
// Check BoundingBoxes
if (bBox1.Overlaps(bBox2))
{
if (IsParallel2D((edge1Node1.coordinate.x, edge1Node1.coordinate.z), (edge1Node2.coordinate.x, edge1Node2.coordinate.z),
(edge2Node1.coordinate.x, edge2Node1.coordinate.z), (edge2Node2.coordinate.x, edge2Node2.coordinate.z)))
{
return(ParallelEdgesCollide(edge1, edge2, nodeGraph1, nodeGraph2));
}
else
{
Vect3 edge1RightWall1 = edge1Node1.coordinate + ((edge1.width / 2) * edge1Right);
Vect3 edge1RightWall2 = edge1Node2.coordinate + ((edge1.width / 2) * edge1Right);
Vect3 edge2RightWall1 = edge2Node1.coordinate + ((edge2.width / 2) * edge2Right);
Vect3 edge2RightWall2 = edge2Node2.coordinate + ((edge2.width / 2) * edge2Right);
Vect3 edge1LeftWall1 = edge1Node1.coordinate - ((edge1.width / 2) * edge1Right);
Vect3 edge1LeftWall2 = edge1Node2.coordinate - ((edge1.width / 2) * edge1Right);
Vect3 edge2LeftWall1 = edge2Node1.coordinate - ((edge2.width / 2) * edge2Right);
Vect3 edge2LeftWall2 = edge2Node2.coordinate - ((edge2.width / 2) * edge2Right);
// Check all Possible wall collisions.
return(WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2RightWall1, edge2RightWall2) ||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2LeftWall1, edge2LeftWall2) ||
WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2RightWall1, edge2RightWall2) ||
WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2LeftWall1, edge2LeftWall2)
||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2RightWall1, edge2RightWall2) ||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2LeftWall1, edge2LeftWall2) ||
WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2RightWall1, edge2RightWall2) ||
WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2LeftWall1, edge2LeftWall2)
||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2RightWall1, edge2LeftWall1) ||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1RightWall2, edge2RightWall2, edge2LeftWall2) ||
WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2RightWall1, edge2LeftWall1) ||
WallsIntersect(edge1, edge2, edge1LeftWall1, edge1LeftWall2, edge2RightWall2, edge2LeftWall2)
||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2RightWall1, edge2LeftWall1) ||
WallsIntersect(edge1, edge2, edge1RightWall1, edge1LeftWall1, edge2RightWall2, edge2LeftWall2) ||
WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2RightWall1, edge2LeftWall1) ||
WallsIntersect(edge1, edge2, edge1RightWall2, edge1LeftWall2, edge2RightWall2, edge2LeftWall2));
}
}
private static NodeGraph MergeNodeGraph(NodeGraph nodeGraph, VoxelGrid voxelGrid)
{
bool proximity = true;
while (proximity)
{
proximity = false;
Edge mergableEdge = null;
foreach (Edge edge in nodeGraph.Edges)
{
if ((nodeGraph.GetCoordinateOfNode(edge.nodeIndex1) - nodeGraph.GetCoordinateOfNode(edge.nodeIndex2)).Length() < mergeThreshold * voxelGrid.resolution)
{
mergableEdge = edge;
proximity = true;
break;
}
}
if (mergableEdge != null)
{
Vect3 centerPoint = (nodeGraph.GetCoordinateOfNode(mergableEdge.nodeIndex1) + nodeGraph.GetCoordinateOfNode(mergableEdge.nodeIndex2)) / 2d;
nodeGraph.MoveNode(nodeGraph.GetNode(mergableEdge.nodeIndex1), centerPoint);
nodeGraph.MoveNode(nodeGraph.GetNode(mergableEdge.nodeIndex2), centerPoint);
}
}
nodeGraph.ReIndexNodeGraph();
return(nodeGraph);
}
/*
* Takes in a list of nodeGraphs and unify them into one nodeGraph.
*/
public static NodeGraph UnifyGraphs(List <NodeGraph> unifyingGraphs)
{
NodeGraph baseGraph = unifyingGraphs[0];
unifyingGraphs.Remove(baseGraph);
// Define Variables
var joiningEdges = new Stack <Edge>();
int newNodeIndex1;
int newNodeIndex2;
Edge joiningEdge;
Edge baseEdge;
var edgesAdded = new List <Edge>();
Vect3 collisionPoint;
int baseNodeIndex;
int extraJoiningNodeIndex;
Edge extraJoiningEdge1;
Edge extraJoiningEdge2;
// Unify each graph with the base graph
foreach (NodeGraph joiningGraph in unifyingGraphs)
{
// Fill edge stack of joining edges
joiningEdges.Clear();
for (int i = joiningGraph.Edges.Count - 1; i >= 0; i--)
{
joiningEdges.Push(joiningGraph.GetEdge(i));
}
newNodeIndex1 = -1;
newNodeIndex2 = -1;
edgesAdded.Clear();
// Join edges LOOP
// Find which node indexes that the edge should have.
// If -1 a new node needs to be created.
while (joiningEdges.Count > 0)
{
joiningEdge = joiningEdges.Pop();
newNodeIndex2 = newNodeIndex1;
newNodeIndex1 = -1;
for (int i = 0; i < baseGraph.Edges.Count; i++)
{
baseEdge = baseGraph.GetEdge(i);
if (baseEdge == null || edgesAdded.Contains(baseEdge))
{
continue;
}
// If base edge and joining edge has a collision, find where on the base graph the collision is.
// To find out if a new node needs to be created or of the graph should join an exisiting node.
if (EdgesCollide(baseEdge, joiningEdge, baseGraph, joiningGraph))
{
collisionPoint = GetEdgesCollidingPoint(baseEdge, joiningEdge, baseGraph, joiningGraph);
if (collisionPoint != null)
{
if (joiningGraph.PointCloseToNode(collisionPoint, joiningGraph.GetNode(joiningEdge.nodeIndex1)))
{
baseNodeIndex = baseGraph.GetCollisionNodeIndex(collisionPoint, baseEdge);
if (newNodeIndex1 == -1)
{
newNodeIndex1 = baseNodeIndex;
}
else if (newNodeIndex1 != baseNodeIndex)
{
newNodeIndex1 = baseGraph.UnifyNodes(baseGraph.GetNode(newNodeIndex1), baseGraph.GetNode(baseNodeIndex)).index;
baseGraph.MoveNode(baseGraph.GetNode(newNodeIndex1), baseGraph.GetNode(baseNodeIndex).coordinate);
}
}
else if (joiningGraph.PointCloseToNode(collisionPoint, joiningGraph.GetNode(joiningEdge.nodeIndex2)))
{
baseNodeIndex = baseGraph.GetCollisionNodeIndex(collisionPoint, baseEdge);
if (newNodeIndex2 == -1)
{
newNodeIndex2 = baseNodeIndex;
}
else if (newNodeIndex2 != baseNodeIndex)
{
newNodeIndex2 = baseGraph.UnifyNodes(baseGraph.GetNode(newNodeIndex2), baseGraph.GetNode(baseNodeIndex)).index;
}
}
else if (joiningGraph.PointOnEdge(collisionPoint, joiningEdge))
{
joiningGraph.AddNode(collisionPoint);
extraJoiningNodeIndex = joiningGraph.GetNode(collisionPoint).index;
extraJoiningEdge1 = joiningGraph.AddEdge(joiningEdge.nodeIndex1, extraJoiningNodeIndex,
joiningEdge.width, joiningEdge.height);
extraJoiningEdge2 = joiningGraph.AddEdge(extraJoiningNodeIndex, joiningEdge.nodeIndex2,
joiningEdge.width, joiningEdge.height);
joiningEdge = extraJoiningEdge2;
joiningEdges.Push(extraJoiningEdge1);
baseNodeIndex = baseGraph.GetCollisionNodeIndex(collisionPoint, baseEdge);
newNodeIndex1 = baseNodeIndex;
}
}
}
}
// After base graph edge loop. Add Edge to base graph correctly.
if (newNodeIndex1 == -1)
{
baseGraph.AddNode(joiningGraph.GetNode(joiningEdge.nodeIndex1));
newNodeIndex1 = baseGraph.GetNode(joiningGraph.GetNode(joiningEdge.nodeIndex1).coordinate).index;
}
if (newNodeIndex2 == -1)
{
baseGraph.AddNode(joiningGraph.GetNode(joiningEdge.nodeIndex2));
newNodeIndex2 = baseGraph.GetNode(joiningGraph.GetNode(joiningEdge.nodeIndex2).coordinate).index;
}
if (newNodeIndex1 == newNodeIndex2)
{
throw new Exception("Same new node index, " + newNodeIndex1);
}
edgesAdded.Add(baseGraph.AddEdge(newNodeIndex1, newNodeIndex2, joiningEdge.width, joiningEdge.height));
}
baseGraph.ReIndexNodeGraph();
}
return(baseGraph);
}
/**
* Generates a NodeGraph from a skeletonized voxelgrid, by traversing neighboring
* voxels and creating nodes connected by edges to maintain the topology of the voxelgrid.
*/
private static NodeGraph CreateNodeGraph(DistanceGrid distanceGrid, VoxelGrid voxelGrid)
{
bool[][][] visitedGrid = new bool[distanceGrid.xBound + 1][][];
for (int i = 0; i < visitedGrid.Length; i++)
{
visitedGrid[i] = new bool[distanceGrid.yBound + 1][];
for (int j = 0; j < visitedGrid[i].Length; j++)
{
visitedGrid[i][j] = new bool[distanceGrid.zBound + 1];
}
}
NodeGraph nodeGraph = new NodeGraph();
Queue <(Point3, Point3)> startPoints = GetStartPoints(distanceGrid);
Point3 initialPoint = startPoints.Peek().Item1;
nodeGraph.AddNode(voxelGrid.LowestPositionAtCoordinate(initialPoint));
visitedGrid[initialPoint.x][initialPoint.y][initialPoint.z] = true;
//Breadth-first traversing of the Voxels
while (startPoints.Count > 0)
{
(Point3 startPoint, Point3 currentPoint) = startPoints.Dequeue();
//Special case for when connecting to an allready visited node
if (visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z])
{
Vect3 pos1 = voxelGrid.LowestPositionAtCoordinate(startPoint);
Vect3 pos2 = voxelGrid.LowestPositionAtCoordinate(currentPoint);
Node startNode = nodeGraph.GetNode(pos1);
Node currentNode = nodeGraph.GetNode(pos2);
if (nodeGraph.GetNode(pos1) == null || nodeGraph.GetNode(pos2) == null)
{
continue;
}
bool shouldLink = true;
foreach (var neighbor in currentNode.neighbors)
{
if (neighbor.nodeIndex == startNode.index)
{
shouldLink = false;
}
else
{
Node neighborNode = nodeGraph.GetNode(neighbor.nodeIndex);
foreach (var nextNeighbor in neighborNode.neighbors)
{
if (nextNeighbor.nodeIndex == startNode.index)
{
shouldLink = false;
}
}
}
if (!shouldLink)
{
break;
}
}
if (shouldLink)
{
nodeGraph.LinkNodes(pos1, pos2);
}
else
{
continue;
}
}
Point3 cameFrom = startPoint;
List <Point3> pathPoints = new List <Point3>()
{
startPoint
};
Vect3 startCoordinate = voxelGrid.LowestPositionAtCoordinate(startPoint);
Vect3 currentCoordinate = voxelGrid.LowestPositionAtCoordinate(currentPoint);
List <Vect3> intermediateCoords = new List <Vect3>();
List <Point3> neighbors = new List <Point3>();
bool distanceWasValid = false;
//Traversing untill the Nodeplacement would make the Edge to far away from the voxels it represents,
//an already visited node is reached, an intersection is found or it's a dead end.
while (DistancesAreValid(startCoordinate, currentCoordinate, intermediateCoords, voxelGrid.resolution))
{
// Node we found was visited
if (visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z])
{
// Ensures nothing happens after we break
distanceWasValid = true;
break;
}
neighbors = distanceGrid.Get26AdjacentNeighbors(currentPoint);
pathPoints.Add(currentPoint);
if (neighbors.Count != 2)
{
visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z] = true;
distanceWasValid = true;
break;
}
// Only one new neighbor
intermediateCoords.Add(voxelGrid.LowestPositionAtCoordinate(currentPoint));
Point3 temp = currentPoint;
currentPoint = neighbors[0] == cameFrom ? neighbors[1] : neighbors[0];
currentCoordinate = voxelGrid.LowestPositionAtCoordinate(currentPoint);
cameFrom = temp;
visitedGrid[cameFrom.x][cameFrom.y][cameFrom.z] = true;
}
double maxDistanceValue = 0;
foreach (var point in pathPoints)
{
maxDistanceValue += distanceGrid.grid[point.x][point.y][point.z] * voxelGrid.resolution * SquareRootOfThree; // Distance is 26-distance from edge
// double distanceValue = distanceGrid.grid[point.x][point.y][point.z]*voxelGrid.resolution*2;
// maxDistanceValue = distanceValue > maxDistanceValue ? distanceValue : maxDistanceValue;
}
maxDistanceValue /= (double)pathPoints.Count;
double minWidth = maxDistanceValue;
double minHeight = maxDistanceValue;
if (!distanceWasValid)
{
nodeGraph.AddNode(intermediateCoords[intermediateCoords.Count - 1], minWidth, minHeight, nodeGraph.GetNode(startCoordinate).index);
visitedGrid[cameFrom.x][cameFrom.y][cameFrom.z] = true;
if (!visitedGrid[currentPoint.x][currentPoint.y][currentPoint.z])
{
startPoints.Enqueue((cameFrom, currentPoint));
}
}
else
{
nodeGraph.AddNode(currentCoordinate, minWidth, minHeight, nodeGraph.GetNode(startCoordinate).index);
foreach (var neighbor in neighbors)
{
if (neighbor == cameFrom ||
visitedGrid[neighbor.x][neighbor.y][neighbor.z])
{
continue;
}
startPoints.Enqueue((currentPoint, neighbor));
}
}
}
return(nodeGraph);
}