private void addFixedPoint(Vector3 newPoint) { 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; dubinWay.Reverse(); p2.connection = dubinWay; tree.addNode(p2); draw.drawMultipleLines(p2.connection, Color.magenta); } }
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); }