public ArrayList freeCarPath(Vector3 p1, Vector3 p2, Vector3 forward)
{
ArrayList dubinPath;
dubinPath = car.RSR(p1, forward, p2);
draw.drawMultipleLines(dubinPath, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
dubinPath = car.LSR(p1, forward, p2);
draw.drawMultipleLines(dubinPath, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
dubinPath = car.LSL(p1, forward, p2);
draw.drawMultipleLines(dubinPath, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
dubinPath = car.RSL(p1, forward, p2);
draw.drawMultipleLines(dubinPath, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
return(new ArrayList());
}
public ArrayList freePath(Vector3 p1, Vector3 p2)
{
ArrayList dubinPath;
dubinPath = car.RSR(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
dubinPath = car.LSR(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
dubinPath = car.LSL(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
dubinPath = car.RSL(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(dubinPath))
{
return(dubinPath);
}
return(new ArrayList());
}
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 printPath(ArrayList path)
{
for (int i = 0; i < path.Count - 1; i++)
{
myDrawLine((Vector3)path[i], (Vector3)path[i + 1], Color.red);
}
draw.drawMultipleLines(path, Color.red);
}
/*
* Tangent has in the direction of the radius
*/
private ArrayList calculateCircle(Vector3 point, Vector3 tangent, float radius, bool paint, ref Vector3 c)
{
Vector3 center = radius * tangent;
ArrayList circle = new ArrayList();
for (float a = 0; a < Mathf.PI * 2; a += (Mathf.PI / numPointsCircle))
{
float x = Mathf.Cos(a);
float y = Mathf.Sin(a);
Vector3 pc = new Vector3(x, 0, y) * radius + center + point;
pc.y = mainY;
circle.Add(pc);
}
if (debug)
{
draw.drawMultipleLines(circle, Color.red);
}
c = center + point;
return(circle);
}
// Update is called once per frame
void Update()
{
if (debug_print)
{
debug_print = false;
string log = "";
for (int i = 0; i < path.Count; i++)
{
log += path[i] + " ";
}
Debug.Log(log);
}
if (go && path != null && path.Count != 0)
{
if (!AstarReady)
{
// Debug.Log(path[index]);
pathAstar = Astar.getPath(transform.position, (Vector3)path[index]);
pathAstar.Reverse();
pathAstar.Add(path[index]);
//if(printAstarPath) foreach (Vector3 p in pathAstar) createPoint(p);
if (printAstarPath)
{
draw.drawMultipleLines(pathAstar, Color.red);
}
AstarReady = true;
indexAstar = 0;
//Debug.Log("Count "+pathAstar.Count);
}
if (pathAstar == null || pathAstar.Count == 0)
{
index++;
if (index == path.Count)
{
go = false;
index = 0;
}
return;
}
Vector3 destination = (Vector3)pathAstar[indexAstar];
//Debug.Log(indexAstar);
destination.y = mainY;
float d = Vector3.Distance(transform.position, destination);
if (d > 0.5f)
{
//makeDifferentialMove (transform, rigidbody, destination, 15f, 10f);
Vector3 direction = (destination - transform.position).normalized;
DirectionUtility.makeKinematicMove(rigidbody, direction, speed);
}
else
{
//Debug.Log(index);
transform.position = destination; //snap to destination
rigidbody.velocity = Vector3.zero;
rigidbody.angularVelocity = Vector3.zero;
indexAstar++;
if (indexAstar == pathAstar.Count)
{
//go = false;
// Debug.Log(indexAstar);
indexAstar = 0;
AstarReady = false;
index++;
if (index == path.Count)
{
go = false;
index = 0;
}
}
}
}
}
void Update()
{
if (Input.GetMouseButtonDown(0))
{
destination = DirectionUtility.getMouseDirection();
destination.y = transform.position.y;
calcPath = true;
}
if (calcPath)
{
calcPath = false;
go = true;
path = car.RSR(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(path))
{
return;
}
path = car.LSR(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(path))
{
return;
}
path = car.LSL(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(path))
{
return;
}
path = car.RSL(transform.position, transform.forward, destination);
draw.drawMultipleLines(path, Color.magenta);
if (free(path))
{
return;
}
go = false;
}
else if (go && path.Count > 0)
{
destination = (Vector3)path[path_index];
float d = Vector3.Distance(transform.position, destination);
if (d > 2)
{
Vector3 direction = (destination - transform.position).normalized;
correctAngle(direction);
DirectionUtility.makeKinematicMove(rigidbody, direction, speed);
}
else
{
rigidbody.velocity = Vector3.zero;
transform.position = destination;
path_index++;
if (path_index == path.Count)
{
go = false; path_index = 1;
}
}
}
}
private void printTrinagle(Triangle t)
{
MyLine draw = new MyLine();
draw.drawMultipleLines(t.getVertex3DArray(), Color.red);
}
