public bool IsSafe(Pose pose, float safetyFactor)
{
Vector2 pos = Car.GetCenterPosition(pose);
GridCell cellPos = PointToCellPosition(pos);
if (cellPos == GridCell.Unknown)
{
return(false);
}
float obstDist = GVD.GetObstacleDistance(cellPos);
//if (obstDist > Car.SAFE_RADIUS * safetyFactor + DiagonalResolution)
//return true;
GridCell nearestObstCell = GVD.GetNearestObstacle(cellPos);
LinkedList <Body> obstacles = ObstaclesByCell[nearestObstCell.C, nearestObstCell.R];
PolygonShape carShape = new PolygonShape(1f);
Transform carXform = new Transform();
carShape.SetAsBox(Car.HALF_CAR_LENGTH * safetyFactor, Car.HALF_CAR_WIDTH * safetyFactor);
carXform.Set(pos, pose.Orientation);
foreach (Body obs in obstacles)
{
Transform obstXform;
obs.GetTransform(out obstXform);
if (obs.FixtureList != null && AABB.TestOverlap(carShape, 0, obs.FixtureList[0].Shape, 0, ref carXform, ref obstXform))
{
return(false);
}
}
return(true);
}
public Vector2 GetNearestObstacleCellLocation(Vector2 pos)
{
return(CellPositionToPoint(GVD.GetNearestObstacle(PointToCellPosition(pos))) + new Vector2(HalfResolution));
}
