private void RRT()
{
//if(onePoint) { onePoint=false; addFixedPoint(new Vector3(-12f, 1.5f, 3f)); return;}
//if(twoPoint) { twoPoint=false; addFixedPoint(new Vector3(-14f, 1.5f, -18f)); return;}
ArrayList dubinWay = freeCarPath(tree.last.point, tree.last.forward, destination);
draw.drawLine(tree.last.point, destination, Color.blue);
if (dubinWay.Count > 0)
{
rrtNode d = new rrtNode(destination, tree.last);
dubinWay.Reverse();
d.connection = dubinWay;
//draw.drawMultipleLines (d.connection, Color.magenta);
d.forward = ((Vector3)dubinWay[dubinWay.Count - 1] - (Vector3)dubinWay[dubinWay.Count - 2]).normalized;
tree.addNode(d);
calculating = false;
path = tree.getPath();
draw.drawMultipleLines(path, Color.magenta);
}
else
{
//if(onePoint) { onePoint=false; addRandomNode (tree, dimX, dimZ); }
//addRandomNode (tree, dimX, dimZ);
}
}
private void RRT()
{
if (tree.getCount() > numPoints)
{
calculating = false;
return;
}
ArrayList dubinConn = freePath(tree.last.point, destination);
if (dubinConn.Count > 0)
{
Debug.DrawLine(tree.last.point, destination, Color.blue, timeDebug, false);
rrtNode d = new rrtNode(destination, tree.last);
//d.connection = dubinConn;
tree.addNode(d);
calculating = false;
//path = rrTree.optimizePath(tree.getPath());
path = tree.getPath();
//printPath (path);
}
else
{
addRandomNode(tree, dim);
}
}
// private float turnSpeed = 10000;
// private bool onePoint = true;
// private bool twoPoint = true;
private void inizializeTree(Vector3 source, Vector3 dest)
{
tree = new rrDubinTree();
rrtNode node = new rrtNode(source, null);
node.forward = transform.forward;
tree.addNode(node);
}
private bool addRandomNode(rrDubinTree tree, int dim)
{
Vector3 newPoint = new Vector3(myRnd(dim), y, myRnd(dim));
createPoint(newPoint);
rrtNode point = tree.findClosestNode(newPoint);
ArrayList dubinConn = freePath(point.point, newPoint);
if (point != null && dubinConn.Count > 0)
{
//if it is possible to reach the point from the previous point
//add the connection
rrtNode p2 = new rrtNode(newPoint, point);
//p2.connection = dubinConn;
tree.addNode(p2);
//myDrawLine (point.point, newPoint, Color.blue);
return(true);
}
return(true);
}
private bool addRandomNode(rrDubinTree tree, int dimX, int dimZ)
{
Vector3 newPoint = new Vector3(myRnd(dimX), mainY, myRnd(dimZ));
draw.drawLine(newPoint, new Vector3(newPoint.x + 1, newPoint.y, newPoint.z + 1), Color.green);
rrtNode point = tree.findClosestNode(newPoint);
ArrayList dubinWay = freeCarPath(point.point, point.forward, newPoint);
if (point != null && dubinWay.Count > 0)
{
//if it is possible to reach the point from the previous point
//add the connection
rrtNode p2 = new rrtNode(newPoint, point);
p2.forward = ((Vector3)dubinWay[dubinWay.Count - 1] - (Vector3)dubinWay[dubinWay.Count - 2]).normalized;
//conn.Reverse();
p2.connection = dubinWay;
tree.addNode(p2);
draw.drawMultipleLines(p2.connection, Color.magenta);
return(true);
}
return(true);
}
private void moveKinematic()
{
if (Input.GetMouseButtonDown(0))
{
destination = DirectionUtility.getMouseDirection();
destination.y = transform.position.y;
//go = true;
calculating = true;
tree = new rrDubinTree();
tree.addNode(new rrtNode(transform.position, null));
draw.clean();
}
else
{
if (go && path.Count > 0)
{
destination = (Vector3)path[path_index];
float d = Vector3.Distance(transform.position, destination);
if (d > 0.5)
{
Vector3 direction = (destination - transform.position).normalized;
DirectionUtility.makeKinematicMove(rigidbody, direction, speed);
}
else
{
rigidbody.velocity = Vector3.zero;
transform.position = destination;
path_index++;
if (path_index == path.Count)
{
go = false; path_index = 0;
}
}
}
}
}