private void buildDistanceFieldBitmap(GraphicsDevice device)
{
distanceFieldBitmap = new Texture2D(device, NumColumns, NumRows);
Color[] distance = new Color[NumColumns * NumRows];
for (int c = 0; c < NumColumns; c++)
{
for (int r = 0; r < NumRows; r++)
{
float val = GVD.GetObstacleDistance(c, r);
if (val == 0f)
{
distance[c + r * NumColumns] = Color.Red;
}
//else if (GVD.GetVoronoiDistance(new GridCell(c, r)) == 0f)
//distance[c + r * NumColumns] = Color.White;
else
{
val /= 32;
//val = 255;
distance[c + r * NumColumns] = new Color(val, val, val);
}
}
}
distanceFieldBitmap.SetData <Color>(distance);
}
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);
}