public static List <Attractor> Create(Manipulator manipulator, int count, float goalThreshold, bool discardOutliers = true)
{
var attractors = new List <Attractor>();
//// adding goal attractor
//Vector3 attrPoint = manipulator.Goal;
//attractors.Add(new Attractor(attrPoint));
// adding ancillary attractors
while (attractors.Count < count)
{
// generating attractor point
Vector3 point = RandomThreadStatic.NextPointSphere(manipulator.WorkspaceRadius) + manipulator.Base;
// checking whether the attractor is inside any obstacle or not if requested
if (!(discardOutliers && ObstacleHandler.ContainmentTest(point, out _))) // TODO: consider creating a list of bad attractors; they may serve as repulsion points
{
// adding attractor to the list
Vector3 attrPoint = point;
attractors.Add(new Attractor(attrPoint));
}
}
return(attractors);
}
public void Run(Manipulator manipulator, CancellationToken cancellationToken = default)
{
try
{
// start measuring execution time
Timer.Restart();
// turn the controller on
State = ControllerState.Running;
// start motion control if a path has been found
if (manipulator.Path != null)
{
using (var manipulatorCopy = manipulator.DeepCopy())
{
// execute motion control
Path.Node gripperPos = manipulator.Path.Current;
while (gripperPos.Child != null)
{
cancellationToken.ThrowIfCancellationRequested();
var current = gripperPos.Child;
while (current.Child != null) // last point may not be deformed, since it is a goal point
{
for (int j = current.Points.Length - 1; j > 0; j--)
{
if (ObstacleHandler.ContainmentTest(current.Points[j], out Obstacle obstacle))
{
//Deform(manipulatorCopy, obstacle, current, j);
}
}
current = current.Child;
}
//Discretize(manipulatorCopy, gripperPos);
gripperPos = manipulator.Path.Current;
}
}
}
// turn the controller off
State = ControllerState.Idle;
}
catch (OperationCanceledException oce)
{
// indicate that the process has been aborted
State = ControllerState.Aborted;
}
finally
{
// stop measuring execution time
Timer.Stop();
}
}