/// <summary>
/// Adds a point. Returns a list of Point3Ds that are part of dangerous bumps as a result.
/// </summary>
public List <Point3D> AddPoint(Vector3D point, int timeout)
{
// Center the edge detection collider on the new point
edgeDetectionCollider_.Position = point;
// Find possible dangerous edges
var candidatePoints = new List <Point3D>();
points_.GetCollisions(ref candidatePoints, edgeDetectionCollider_);
// Decide whether each point pairing is a dangerous edge
var dangerousPoints = new List <Point3D>();
bool pointIsDangerous = false;
foreach (var point2 in candidatePoints)
{
if (IsEdgeDangerous(point, point2.Position))
{
dangerousPoints.Add(point2);
pointIsDangerous = true;
}
}
var newPoint = new Point3D(point, pointsAdded_, timeout);
if (pointIsDangerous)
{
dangerousPoints.Add(newPoint);
points_.AddPoint(newPoint);
pointsAdded_++;
}
else
{
points_.AddPoint(newPoint);
pointsAdded_++;
}
return(dangerousPoints);
}