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)); }