Пример #1
0
        /*
         * 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);
        }
Пример #2
0
        /*
         * 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));
                }
            }
Пример #3
0
        /**
         * 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);
        }