private void createPoint(Vector3 point)
{
Vector3 n = new Vector3(point.x + 0.5f, point.y, point.z + 0.5f);
myDrawLine(point, n, Color.green);
draw.drawLine(point, n, Color.green);
}
public ArrayList RSR(Vector3 pos, Vector3 fwd, Vector3 destination)
{
mainY = pos.y;
Vector3 c1 = Vector3.zero, c2 = Vector3.zero;
ArrayList circleFrom = calculateCircle(pos, -tangent(fwd),
radius, true, ref c1);
circleFrom.Reverse();
Vector3 tan = tangent((pos - destination).normalized);
ArrayList circleTo = calculateCircle(destination, tan, radius, true, ref c2);
Vector3 connOnCircles = tangent((c2 - c1).normalized) * radius;
Vector3 conn1 = connOnCircles + c1;
Vector3 conn2 = connOnCircles + c2;
if (debug)
{
draw.drawLine(conn1, conn2, Color.blue);
}
ArrayList way = createPath(circleFrom, circleTo, pos, destination, conn1, conn2);
return(way);
}
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 printLineNew(Vector3 p1, Vector3 p2)
{
p1.y = mainY;
p2.y = mainY;
Vector3 dir = (p2 - p1);
Ray ray = new Ray(p1, dir);
RaycastHit hit = new RaycastHit();
bool collision = Physics.Raycast(ray, out hit, 200);
if (collision)
{
draw.drawLine(p1, hit.point, Color.red);
// Debug.Log("Collision: "+hit.collider.name);
}
if (collision && hit.collider.tag == "robot")
{
ray = new Ray(hit.point, dir);
Physics.Raycast(ray, out hit, 200);
draw.drawLine(p1, hit.point, Color.red);
}
}
public void drawConnections()
{
MyLine draw = new MyLine();
for (int i = 0; i < nodes.Length - 1; i++)
{
for (int j = i; j < nodes.Length; j++)
{
if (adjacency[i][j] == 1)
{
draw.drawLine(nodes[i].area.center3D, nodes[j].area.center3D, Color.red);
}
}
}
}
private void createPoint(Vector3 point)
{
Vector3 n = new Vector3(point.x + 2f, point.y, point.z + 2f);
draw.drawLine(point, n, Color.red);
}