public Dijkstra(TravelerController TC) { // initialize the traveler profile catalog TravelerProfileCatalog.Initialize(); Actor = TC; }
// Moved running dijkstra inside this function to allow re-running easily void runDijkstra(ref VertexScript NodeA, ref VertexScript NodeB, Dijkstra.CostMethodType costMethod) { // Dijkstra is running dijkstraIsRunning = true; // either have no path yet or path data is stale (no longer correct) so get new path //path = algorithm.GetPath(StartNode, GoalNode, TravelerProfileCatalog.GetProfile(Type), CostMethod); path = algorithm.GetPath(NodeA, NodeB, TravelerProfileCatalog.GetProfile(Type), costMethod); //Dijkstra is not running dijkstraIsRunning = false; // Actor now has a path to use dijkstraHasPath = true; }
/////////////////////////////////// /// Here Lies the Dijkstra Call /// /////////////////////////////////// // Update is called once per frame void Update() { // If there is not startNode and no Goal node if (StartNode != null && GoalNode != null) { // have somewhere to go if (path == null || path.StartNode != StartNode || path.GoalNode != GoalNode) { // either have no path yet or path data is stale (no longer correct) so get new path path = algorithm.GetPath(StartNode, GoalNode, TravelerProfileCatalog.GetProfile(Type), CostMethod); if (path != null && path.GetCurrentVertex() != null && StartAtStartNode) { transform.position = path.GetCurrentVertex().transform.position; StartAtStartNode = false; } } } else { // have nowhere to go path = null; } if (Move && path != null && path.GetCurrentVertex() != null) { VertexScript nextNode = path.GetCurrentVertex(); // <- In PathData.cs, returns the current vertex currentNode = path.GetCurrentVertex(); // returns the current vertex the actor is at // TODO: Upon getting current vertex, if the actors heartrate > maxBPM // Wait untill heartrate is at safe BPM // after reaching safe BPM, continue with movement Vector3 nextDestination = nextNode.transform.position; Vector3 currentPosition = transform.position; Vector3 difference = nextDestination - currentPosition; // difference.Normalize (); // scales vector to have length = 1 FacingDirection = difference; float distanceToDest = difference.magnitude; if (distanceToDest <= DELTA) { // we're already there, so just correct the position // and advance to the next vertex in the path transform.position = nextDestination; path.AdvanceToNextVertex(); } else if (distanceToDest < Speed * Time.deltaTime) { // we're close enough to arrive there this frame transform.position = nextDestination; } else { // it'll take more than one frame to arrive there Vector3 normalizedVelocity = difference.normalized; // velocity vector Vector3 movementThisFrame = Speed * normalizedVelocity * Time.deltaTime; transform.position += movementThisFrame; } } }
public PathData GetPath( VertexScript startNode, VertexScript goalNode, TravelerProfile traveler = null, CostMethodType costMethod = CostMethodType.Steepness, // <- Default cost method int maxDepth = int.MaxValue) { PathData data = null; if (startNode == null || goalNode == null) { return(null); } else if (startNode == goalNode) { Debug.Log("ERROR: startNode == goalNode in GetPath."); return(null); } if (traveler == null) { traveler = TravelerProfileCatalog.GetProfile(TravelerProfileCatalog.TravelerType.Heart); // <- Setting defaults to heart } PriorityQueue <VertexWrapper> priorityQueue = new PriorityQueue <VertexWrapper> (); Dictionary <VertexScript, VertexWrapper> vertexDict = new Dictionary <VertexScript, VertexWrapper> (); Dictionary <EdgeScript, EdgeWrapper> edgeDict = new Dictionary <EdgeScript, EdgeWrapper> (); EdgeWrapper edgeWrapper; bool reachedGoal = false; VertexWrapper nodeWrapper = GetOrMakeVertexWrapper(startNode, vertexDict); nodeWrapper.LowestCostSoFar = 0; priorityQueue.Enqueue(nodeWrapper); while (!reachedGoal && priorityQueue.Count != 0) { VertexWrapper tempWrapper = priorityQueue.Dequeue(); if (tempWrapper != null) { nodeWrapper = tempWrapper; if (nodeWrapper.Vertex == goalNode) { reachedGoal = true; break; } if (nodeWrapper.Depth == maxDepth) { break; } VertexScript currentNode = nodeWrapper.Vertex; foreach (EdgeScript edge in currentNode.Edges) { if (edge == null) { continue; } VertexScript otherNode = edge.GetOtherVertex(currentNode); VertexWrapper otherNodeWrapper = GetOrMakeVertexWrapper(otherNode, vertexDict); if (otherNode != null) { // returns the edge cost // float cost = GetCost(nodeWrapper, otherNodeWrapper, edge, costMethod, traveler, startNode, goalNode); actor.setCurBPM(cost); // // // // // // // // // Debug.Assert(cost >= 0f, "ERROR: Dijkstra's Algorithm does not accept negative edge weights."); float costOfPathPlusThisEdge = nodeWrapper.LowestCostSoFar + cost; if (costOfPathPlusThisEdge < otherNodeWrapper.LowestCostSoFar) { edgeWrapper = GetOrMakeEdgeWrapper(edge, edgeDict); edgeWrapper.Cost = cost; otherNodeWrapper.LowestCostSoFar = costOfPathPlusThisEdge; otherNodeWrapper.LowestCostEdgeSoFar = edgeWrapper; otherNodeWrapper.Depth = nodeWrapper.Depth + 1; priorityQueue.Enqueue(otherNodeWrapper); } } } } } data = new PathData(startNode, goalNode, nodeWrapper.LowestCostSoFar); data.InsertPreviousStep(nodeWrapper.Vertex); // traverse the shortest path from the goalNode backwards to the startNode edgeWrapper = nodeWrapper.LowestCostEdgeSoFar; while (edgeWrapper != null) { nodeWrapper = GetOrMakeVertexWrapper(edgeWrapper.Edge.GetOtherVertex(nodeWrapper.Vertex), vertexDict); edgeWrapper = nodeWrapper.LowestCostEdgeSoFar; data.InsertPreviousStep(nodeWrapper.Vertex); } Debug.Assert(nodeWrapper.Vertex == startNode, "ERROR: shortest path did not terminate at startNode in MakeShortestPath."); priorityQueue.Clear(); vertexDict.Clear(); edgeDict.Clear(); return(data); }
// // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // TODO: Take this specific function, Modify it to only calculate BPM cost // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // // public float GetCost(VertexWrapper fromNodeWrapper, VertexWrapper toNodeWrapper, EdgeScript edge, CostMethodType method, TravelerProfile Traveler = null, VertexScript startNode = null, VertexScript goalNode = null) { VertexScript fromNode = fromNodeWrapper.Vertex; VertexScript toNode = toNodeWrapper.Vertex; // the baseCost is our interpretation of the physical cost of travel, ignoring any personality traits float baseCost = 0f; switch (method) { case CostMethodType.ClosestToCompass: // pick the path with the smallest angle compared to the vector between you and the destination Vector3 differenceForEdge = toNode.transform.position - fromNode.transform.position; Vector3 differenceForGoal = goalNode.transform.position - fromNode.transform.position; baseCost = Mathf.Abs(Vector3.Angle(differenceForEdge, differenceForGoal)); break; case CostMethodType.DistanceHorizontal: // use only the pure horizontal distance, ignoring any vertical distance Vector3 difference = toNode.transform.position - fromNode.transform.position; difference.y = 0; baseCost = difference.magnitude; break; case CostMethodType.DistanceTrue: // use the typical 3D distance between the two points baseCost = (toNode.transform.position - fromNode.transform.position).magnitude; break; case CostMethodType.DistanceVertical: // use only the pure vertical distance, ignoring any horizontal distance // this is equivalent to saying "walking on flat ground is easy, but any change in elevation is hard" baseCost = Mathf.Abs(toNode.transform.position.y - fromNode.transform.position.y); break; case CostMethodType.NumberOfStops: // counts the number the vertices between the start and the goal nodes baseCost = 1f; break; case CostMethodType.PeakLover: // for people who enjoy walking along the tops of hills and mountains so they can enjoy the view // there is a cost for going down, but not for going up baseCost = Mathf.Max(0f, fromNode.transform.position.y - toNode.transform.position.y); break; case CostMethodType.SmoothTurns: // when you're driving very fast or driving a very large vehicle it's important that your turns are smooth // the cost is smaller if the edge is a straight-forward continuation of the last edge and larger if you have to make a sharp turn if (fromNodeWrapper.LowestCostEdgeSoFar != null) { // all nodes except the start node will have a "lowest cost edge so far", so we can use it Vector3 differenceNext = toNode.transform.position - fromNode.transform.position; Vector3 differencePrevious = fromNode.transform.position - fromNodeWrapper.LowestCostEdgeSoFar.Edge.GetOtherVertex(fromNode).transform.position; // ignore changes in elevation so that only horizontal turning is taken into account differenceNext.y = 0; differencePrevious.y = 0; // the cost should be higher if the angle is larger or if the distance traveled is shorter (i.e. it's a sharper turn) // the cost should be lower if the angle is smaller or if the distance traveled is larger (i.e. it's a long smooth turn) baseCost = Mathf.Abs(Vector3.Angle(differencePrevious, differenceNext)) / (differencePrevious.magnitude + differenceNext.magnitude); } else { // the start node has no "lowest cost edge so far" so just use 0 (no need to turn when leaving the start node) baseCost = 0f; } break; /////////////////// case CostMethodType.Steepness: // <- Use this for calculating edge BPM Cost // the cost is the angle that the edge makes with the horizon // so flat paths are good but the steeper the edge (going up or going down) the more it costs Vector3 differenceFull = toNode.transform.position - fromNode.transform.position; Vector3 differenceFlat = new Vector3(differenceFull.x, differenceFull.z, 0f); baseCost = Mathf.Abs(Vector3.Angle(differenceFlat, differenceFull)); break; case CostMethodType.UpSucksDownOkay: // <- work with both // for people who hate walking uphill but don't mind going downhill // there is a cost for going up but no cost for going down baseCost = Mathf.Max(0f, toNode.transform.position.y - fromNode.transform.position.y); break; //////////////////// default: // if nothing else, just use the physical length of the edge baseCost = (edge.transform.localScale.y * 2f); break; } // end of baseCost calculation // retrieve the traveler profile, then check if the profile is null to prevent errors if (Traveler == null) { Traveler = TravelerProfileCatalog.GetProfile(TravelerProfileCatalog.TravelerType.Neutral); } // if profile is not null use the average of the trait indexes (which will be -1 to 1) // but if profile is null then just use zero float compatibility = (Traveler != null ? (Traveler.LikesToWalk * edge.PedestrianFriendly + Traveler.LikesToBicycle * edge.BicyclistFriendly + Traveler.LikesToDrive * edge.CarFriendly + Traveler.LikesVistas * edge.Beautiful + Traveler.LikesFood * edge.FoodAvailable + Traveler.LikesHeart * edge.Heart) / 5f : 0f); // (1 - compatibility) gives a value that is 0 (good) to 2 (bad) // this translates to: // -- cost is reduced when the traveler likes the edge // -- cost is not affected when the traveler has no opinion of the edge // -- cost is increased when the traveler dislikes the edge float personalityCost = (1f - compatibility); // using personalityCost * baseCost means "the traveler's emotions can make anything easy or anything hard" return(personalityCost * baseCost); }