public void addRootNode(RRTNode node)
{
int x = (int)node.X() - 25;
int y = 400 - (int)node.Z();
Cv2.Circle(pic, x, y, 2, Scalar.Blue, -4, LineTypes.Link8);
}
public RRTNode(float theX, float theZ, RRTNode theFather)
{
x = theX;
z = theZ;
father = theFather;
distanceToFather = Mathf.Sqrt(Mathf.Pow(father.X() - this.x, 2) + Mathf.Pow(father.Z() - this.z, 2));
nodeID = z * 500 + x;
}
private List <RRTNode> findPathOnRRT(RRTNode curr, RRTNode dest) // curr: current position, dest: destination
{
if (curr == dest)
{
return(new List <RRTNode>());
}
List <RRTNode> ancestors = new List <RRTNode>();
RRTNode r = dest;
bool b = true;
ancestors.Add(r);
while (b) // dont directly use "true", because visual studio will report an unreasonable error
{
if (r == root)
{
break;
}
r = r.Father();
if (r == curr)
{
ancestors.Reverse();
return(ancestors);
}
ancestors.Add(r);
}
List <RRTNode> path = new List <RRTNode>();
r = curr;
while (b)
{
r = r.Father();
path.Add(r);
List <RRTNode> commonParentToDest = new List <RRTNode>();
for (int i = 0; i < ancestors.Count; i++)
{
if (ancestors[i] == r)
{
for (int j = i - 1; j >= 0; j--)
{
commonParentToDest.Add(ancestors[j]);
}
foreach (RRTNode node in commonParentToDest)
{
path.Add(node);
}
b = false; // dont directly use "break", because visual studio will report an unreasonable error
}
else
{
commonParentToDest.Clear();
}
}
}
return(path);
}
public void addNode(RRTNode node)
{
int x = (int)node.X() - 25;
int y = 400 - (int)node.Z();
Cv2.Circle(pic, x, y, 2, Scalar.Blue, -1, LineTypes.Link8);
int fatherX = (int)node.Father().X() - 25;
int fatherZ = 400 - (int)node.Father().Z();
Cv2.Line(pic, x, y, fatherX, fatherZ, Scalar.Green);
}
// Start is called before the first frame update
void Start()
{
distanceCounter = this.gameObject.GetComponent <DistanceCounter>();
rotation = this.gameObject.GetComponent <RotationSimulator>();
demoGraph = this.gameObject.GetComponent <RRTDrawer>();
map = this.gameObject.GetComponent <MapBuilder>();
drone = this.gameObject.GetComponent <PowerfulEngine>();
root = new RRTNode(50, 75);
theRRT = new List <RRTNode>();
theRRT.Add(root);
demoGraph.addRootNode(root);
minDisNode = root;
requestList = new LinkedList <(RRTNode, Vector3)>();
b = true;
}
void FixedUpdate()
{
if (uiManager.getLock()) // if locked, do nothing
{
return;
}
timer += Time.deltaTime;
if (timer >= 0.5) // run per 0.5 second
{
timer = 0;
float[] distances = new float[8];
for (int i = 1; i <= 8; i++)
{
distances[i - 1] = distanceCounter.getDistance(i);
}
State currState = distanceCounter.getCurrentStateWhileFindingDistance();
map.updateMap(distances, currState.getCurrentOrientation(), currState.getCurrentPosition() + new Vector3(0, 0, 1));
if (uiManager.getGameMode() == InterfaceManager.SELF_DRIVING_MODE)
{
if (b)
{
b = false;
StartCoroutine("buildingRRT");
}
if (drone.getIfIdle())
{
if (requestList.Count != 0)
{
RRTNode nearestNode = findNearestNode();
RRTNode nearestNodeInRequestList = requestList.First.Value.Item1;
float minDistance = nearestNodeInRequestList.distanceTo(this.transform.position.x, this.transform.position.z);
//foreach ((RRTNode before, Vector3 after) in requestList)
var node = requestList.First;
while (node != null)
{
(RRTNode before, Vector3 after) = node.Value;
var next = node.Next;
if (map.ifThisPointIsChecked(after))
{
requestList.Remove(node);
}
node = next;
}
for (var item = requestList.First; item != null; item = item.Next)
{
(RRTNode before, Vector3 after) = item.Value;
float distance = before.distanceTo(this.transform.position.x, this.transform.position.z);
if (distance < minDistance)
{
minDistance = distance;
nearestNodeInRequestList = before;
}
}
requestList.Clear();
letDroneFly(nearestNode, nearestNodeInRequestList);
//rotation.setRotatedAngle(90 - flyAngle);
}
}
}
}
}
public void addChild(RRTNode child)
{
children.Add(child);
}
public float distanceTo(RRTNode other)
{
float distanceSquare = Mathf.Pow(other.X() - x, 2) + Mathf.Pow(other.Z() - z, 2);
return(Mathf.Sqrt(distanceSquare));
}
