BezierCurvePath FindPath(Vector3 start_position, Vector3 target_position)
{
this.UpdateMeshFilterBounds();
var path_list = Astar.FindPathAstar(source: start_position,
destination: target_position,
search_boundary: this._search_boundary,
grid_granularity: this._grid_granularity,
agent_size: this._actor_size,
near_stopping_distance: this._approach_distance);
if (path_list != null && path_list.Count > 0)
{
path_list = Astar.SimplifyPath(path: path_list, sphere_cast_radius: this._actor_size);
path_list.Add(item: target_position);
}
else
{
path_list = new List <Vector3> {
start_position, target_position
};
}
var path = new BezierCurvePath(start_position: start_position,
target_position: target_position,
game_object: this._bezier_curve,
path_list: path_list);
return(path);
}
BezierCurvePath FindPath(Vector3 start_position, Vector3 target_position)
{
this.UpdateMeshFilterBounds();
var path_list = PathFinding.FindPathAstar(
start_position,
target_position,
this._search_boundary,
this._grid_granularity,
this._actor_size,
this._approach_distance);
if (path_list != null && path_list.Count > 0)
{
path_list = PathFinding.SimplifyPath(path_list, this._actor_size);
path_list.Add(target_position);
}
else
{
path_list = new List <Vector3> {
start_position, target_position
};
}
var path = new BezierCurvePath(start_position, target_position, this._bezier_curve, path_list);
return(path);
}
void MaybeClawIsClosed() {
if (!(Vector3.Distance(this._motor.transform.localPosition, this._closed_motor_position)
< this._precision)) {
return;
}
this._state.GripperIsClosed();
this._path = this.FindPath(this.transform.position, this._reset_position);
this._intermediate_target = this._path.Next(0.001f);
this._state.ReturnToStartPosition();
}
public void FindTargetAndUpdatePath() {
var pair = this.GetOptimalTargetAndGrasp();
if (pair == null || pair.Item1 == null || pair.Item2 == null) {
this._state.PathFindingState = PathFindingState.Returning_;
this._path = this.FindPath(this.transform.position, this._reset_position);
return;
}
this._target_game_object = pair.Item1;
this._target_grasp = pair.Item2;
this._approach_position = this._target_grasp.transform.position
- this._target_grasp.transform.forward * this._approach_distance;
if (Vector3.Distance(this.transform.position, this._approach_position) > this._search_boundary) {
return;
}
this._path = this.FindPath(this.transform.position, this._approach_position);
this._intermediate_target = this._path.Next(this._step_size);
}
/// <summary>
/// Create a instance of itself with all required components
/// </summary>
/// <param name="curveIndex"></param>
/// <param name="path"></param>
/// <returns></returns>
public static BezierCurveMesh Instantiate(int curveIndex, BezierCurvePath path)
{
GameObject tmpObject;
var curveData = path.curveDatas[curveIndex];
var curveObject = new GameObject();
curveObject.name = "Curve" + curveIndex;
curveObject.transform.SetParent(path.transform);
var curveMesh = curveObject.AddComponent <BezierCurveMesh>();
tmpObject = new GameObject();
tmpObject.name = "PathHelper";
curveMesh.pathHelper = tmpObject.transform;
curveMesh.pathHelper.SetParent(curveObject.transform);
curveMesh.anchors = new Transform[4];
for (int i = 0; i < 4; i++)
{
tmpObject = new GameObject();
tmpObject.name = "Anchor" + i;
tmpObject.transform.SetParent(curveObject.transform);
tmpObject.transform.localPosition = curveData.points[i];
curveMesh.anchors[i] = tmpObject.transform;
}
curveMesh.planes = new Dictionary <BezierCurveMeshPlane.types, BezierCurveMeshPlane>();
foreach (BezierCurveMeshPlane.types type in BezierCurveMeshPlane.GetTypesArray())
{
curveMesh.planes.Add(type, BezierCurveMeshPlane.Instantiate(type, curveMesh));
}
curveMesh.curveIndex = curveIndex;
curveMesh.path = path;
return(curveMesh);
}
BezierCurvePath FindPath(Vector3 start_position, Vector3 target_position)
{
this.UpdateMeshFilterBounds();
var path_list = PathFinding.FindPathAstar(
start_position,
target_position,
this._search_boundary,
this._grid_granularity,
this._actor_size,
this._approach_distance);
if (path_list != null && path_list.Count > 0)
{
path_list = PathFinding.SimplifyPath(path_list, this._actor_size);
path_list.Add(target_position);
}
else
{
path_list = new List <Vector3> {
start_position, target_position
}
};
var path = new BezierCurvePath(start_position, target_position, this._bezier_curve, path_list);
return(path);
}
void ApproachTarget(float step_size)
{
this.transform.position = Vector3.MoveTowards(
this.transform.position,
this._target_grasp.transform.position,
step_size);
if (this._debugging)
{
Debug.DrawLine(this.transform.position, this._target_grasp.transform.position, Color.green);
}
}
void FollowPathToApproach(float step_size, Quaternion rotation, bool rotate = true)
{
if (Vector3.Distance(this.transform.position, this._intermediate_target) <= this._precision)
{
this._intermediate_target = this._path.Next(step_size);
}
if (this._debugging)
{
Debug.DrawRay(this._intermediate_target, this.transform.forward, Color.magenta);
}
if (rotate)
{
this.transform.rotation = Quaternion.RotateTowards(this.transform.rotation, rotation, step_size * 50);
}
this.transform.position = Vector3.MoveTowards(
this.transform.position,
this._intermediate_target,
step_size);
}
#endregion
}
