public void AddPoint(Vector3D point, int timeout)
{
var dangerousPoints = terrainMap_.AddPoint(point, timeout);
var groundPlane = new PlaneD(CurrentLocation, UpDirection);
/*var referencePoint = new Vector3D(point);
* var projectedPoint = groundPlane.ProjectPoint(ref referencePoint);
* var projectedX = (projectedPoint - CurrentLocation).Dot(X_Axis);
* var projectedY = (projectedPoint - CurrentLocation).Dot(Y_Axis);
* movementPlanner_.AddPoint(new Vector2D(projectedX, projectedY), numPoints_, dangerousPoints.Count() > 0, timeout);
* numPoints_++;*/
Vector3D referencePoint, projectedPoint;
double projectedX, projectedY;
foreach (var dangerousPoint in dangerousPoints)
{
// Add to movement planner
referencePoint = new Vector3D(dangerousPoint.Position);
projectedPoint = groundPlane.ProjectPoint(ref referencePoint);
projectedX = (projectedPoint - CurrentLocation).Dot(X_Axis);
projectedY = (projectedPoint - CurrentLocation).Dot(Y_Axis);
movementPlanner_.AddPoint(new Vector2D(projectedX, projectedY), dangerousPoint.ID, true, dangerousPoint.Timeout);
}
}
