public void CheckAndAddVertexToEdge(Collider other)
{
if (!other) // shorthand for (other == null)
{
return;
}
VertexScript vertex = other.GetComponent <VertexScript> ();
if (vertex) // shorthand for (vertex != null)
{
if (VertexA == null && VertexB != vertex)
{
VertexA = vertex;
}
else if (VertexB == null && VertexA != vertex)
{
VertexB = vertex;
}
else if (VertexA && VertexB)
{
System.Console.WriteLine("ERROR: Edge with two vertices intersecting new vertex in OnTriggerEnter.");
}
}
}
public PathData(VertexScript startNode, VertexScript goalNode, float cost)
{
vertices = new List <VertexScript> ();
this.startNode = startNode;
this.goalNode = goalNode;
this.cost = cost;
}
public VertexWrapper(VertexScript Vertex)
{
this.Vertex = Vertex;
this.LowestCostSoFar = float.PositiveInfinity;
this.LowestCostEdgeSoFar = null;
this.Depth = 0;
}
/*
*
*
*/
private VertexWrapper GetOrMakeVertexWrapper(VertexScript vertex, Dictionary <VertexScript, VertexWrapper> vertexDict)
{
VertexWrapper wrapper;
if (!vertexDict.TryGetValue(vertex, out wrapper))
{
wrapper = new VertexWrapper(vertex);
vertexDict [vertex] = wrapper;
}
return(wrapper);
}
void OnTriggerEnter(Collider col)
{
// Gets the current node
if (col.gameObject.tag == ("Node"))
{
GameObject thisCurrentNode = col.gameObject;
CurrentNode = thisCurrentNode.GetComponent <VertexScript>();
// Ensures each node traversed is a new start node. Allows nodes to change mid travel
StartNode = CurrentNode;
}
}
public void StretchEdgeBetweenTwoVertices(VertexScript vertexA, VertexScript vertexB, EdgeScript edge)
{
Vector3 leftVertexPosition = vertexA.transform.position;
Vector3 rightVertexPosition = vertexB.transform.position;
edge.transform.position = Vector3.Lerp(leftVertexPosition, rightVertexPosition, 0.5f);
Vector3 positionDifferences = rightVertexPosition - leftVertexPosition;
edge.transform.localScale = new Vector3(EdgeThickness, positionDifferences.magnitude * 0.5f, EdgeThickness);
edge.transform.rotation = Quaternion.LookRotation(positionDifferences);
edge.transform.Rotate(new Vector3(90, 0, 0));
}
public void InsertPreviousStep(VertexScript vertex)
{
if (currentVertexIndex == -1)
{
// only insert a step if we have not yet begun walking down the path
vertices.Insert(0, vertex);
}
else
{
// ERROR: shouldn't change path data while traversing path
UnityEngine.Debug.LogError("ERROR: shouldn't change path data in PathData.InsertPreviousStep() while traversing path.");
}
}
// 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;
}
public void CheckAndRemoveVertexFromEdge(Collider other)
{
if (!other) // shorthand for (other == null)
{
return;
}
VertexScript vertex = other.GetComponent <VertexScript> ();
if (vertex) // shorthand for (vertex != null)
{
if (VertexA == vertex)
{
VertexA = null;
}
else if (VertexB == vertex)
{
VertexB = null;
}
}
}
public VertexScript GetOtherVertex(VertexScript vertex)
{
VertexScript value = null;
if (vertex == null)
{
System.Console.WriteLine("ERROR: null vertex in GetOtherVertex.");
}
else if (vertex == VertexA)
{
value = VertexB;
}
else if (vertex == VertexB)
{
value = VertexA;
}
else
{
System.Console.WriteLine("ERROR: vertex not connected to edge in GetOtherVertex.");
}
return(value);
}
void safeBPMCheck()
{
// If the currentBPM is >= to the maxBPM the actor can have
if (currentBPM >= maxBPM || StartNode == GoalNode)
{
// Sets the current node to the start node
StartNode = CurrentNode;
// If the decrement coroutine has ran or not
if (decrementHasRan == false)
{
// Sets decrementHasRan to true
decrementHasRan = true;
// Actor cannot move
Move = false;
// Starts BPMdecrement Coroutine
StartCoroutine(DecrementCurrentBPMToSafeBPM());
}
}
}
///////////////////////////////////
/// 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);
}
///////////////////////////////////
/// Here Lies the Dijkstra Call ///
///////////////////////////////////
// Update is called once per frame
void Update()
{
// Always checks to ensure the actor is at a safeBPM
safeBPMCheck();
// If there is a startNode and a Goal node
if (StartNode != null && GoalNode != null)
{
// have somewhere to go
if (path == null || path.StartNode != StartNode || path.GoalNode != GoalNode)
{
// Checks if Dijkstra has already ran before
if (dijkstraIsRunning == false && dijkstraHasPath == false || GoalNode != CurrentNode)
{
// runs Dijkstra's algorithm
runDijkstra(ref StartNode, ref GoalNode, CostMethod);
}
if (path != null && path.GetCurrentVertex() != null && StartAtStartNode)
{
transform.position = path.GetCurrentVertex().transform.position;
StartAtStartNode = false;
}
}
// Sets the current node as the start node
if (CurrentNode == GoalNode && dijkstraHasPath == true)
{
dijkstraHasPath = false;
StartNode = GoalNode;
}
}
else
{
// have nowhere to go
path = null;
}
// If the actor can move
if (Move && path != null && path.GetCurrentVertex() != null)
{
VertexScript nextNode = path.GetCurrentVertex(); // <- In PathData.cs, returns the current vertex
NextNode = 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;
}
}
}
// Update is called once per frame
void Update()
{
if (Input.GetMouseButtonDown(0))
{
var ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit hit = new RaycastHit();
if (Physics.Raycast(ray, out hit))
{
if (hit.collider.gameObject.tag == "plane" && state == 0)
{
GameObject instance = Instantiate(vertex, hit.point + new Vector3(0, 0, -0.00001f), Quaternion.identity);
VertexScript script = instance.GetComponent <VertexScript>();
if (CollidesWithOthers(instance))
{
Destroy(instance);
}
else
{
script.setId(vertexCount);
vertexCount++;
if (graph == null)
{
graph = new float[1, 1];
}
else
{
graph = ResizeArray(graph, vertexCount, vertexCount);
}
verticesPositions.Add(instance.transform.position);
}
}
else if (hit.collider.gameObject.tag == "vertex")
{
state = 1;
counter++;
if (counter == 1)
{
start = hit.collider.gameObject;
m = hit.collider.gameObject.GetComponent <Renderer>().material;
m.color = Color.green;
}
if (counter == 2)
{
if (!hit.collider.gameObject.transform.position.Equals(start.transform.position))
{
end = hit.collider.gameObject;
Vector3 v1, v2, v3, v4, k;
float a, b, c, d, kx, ky;
bool isInside = false;
for (int i = 0; i < graph.GetLength(0); i++)
{
for (int j = 0; j < i; j++)
{
//Debug.Log (j + "---" + i);
if (graph[i, j] != 0)
{
v1 = start.transform.position;
v2 = verticesPositions [i];
v3 = end.transform.position;
v4 = verticesPositions [j];
if (v1 == v2 || v1 == v4 || v3 == v2 || v3 == v4)
{
//Debug.Log ("wspólne wierzchołki");
continue;
}
//Debug.Log (v1 + " " + v2 + " " + v3 + " " + v4);
a = (v1.y - v3.y) / (v1.x - v3.x);
b = v1.y - a * v1.x;
c = (v2.y - v4.y) / (v2.x - v4.x);
d = v2.y - c * v2.x;
//Debug.Log (a + " " + b + " " + c + " " + d);
if (c != a)
{
kx = (b - d) / (c - a);
ky = a * kx + b;
//Debug.Log (kx + " " + ky);
k.x = kx;
k.y = ky;
k.z = 0;
//GameObject instance = Instantiate(vertex, k, Quaternion.identity);
}
else
{
//Debug.Log ("równoległe");
continue;
}
isInside = false;
isInside = checkIfInside(v1, v2, k, isInside);
isInside = checkIfInside(v2, v3, k, isInside);
isInside = checkIfInside(v3, v4, k, isInside);
isInside = checkIfInside(v4, v1, k, isInside);
if (isInside)
{
//Debug.Log ("jest zły");
goto waypoint;
}
}
}
}
waypoint:
if (!isInside)
{
GameObject instance = (Instantiate(line) as GameObject).GetComponent <LineScript>().setPoints(start.transform.position, end.transform.position);
int id1 = start.GetComponent <VertexScript>().getId();
int id2 = end.GetComponent <VertexScript>().getId();
graph[id1, id2] = Vector3.Distance(start.transform.position * multiplier, end.transform.position * multiplier);
graph[id2, id1] = graph[id1, id2];
}
m.color = Color.black;
state = 0;
counter = 0;
}
else
{
counter = 0;
state = 0;
m.color = Color.black;
}
}
}
else if (hit.collider.gameObject.tag == "button" && state == 0)
{
LevelManager.Load(1, graph, verticesPositions);
}
}
}
}
