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 void RemoveObstacle(int obstacleId)
{
LinkedList <GridCell> cells;
if (!Obstacles.Find(ref obstacleId, out cells))
{
return;
}
foreach (GridCell cell in cells)
{
GVD.UnsetObstacle(cell);
OccupiedCells[cell.C, cell.R]--;
}
}
private void buildVoronoiFieldBitmap(GraphicsDevice device)
{
voronoiFieldBitmap = new Texture2D(device, NumColumns, NumRows);
Color[] voro = new Color[NumColumns * NumRows];
for (int c = 0; c < NumColumns; c++)
{
for (int r = 0; r < NumRows; r++)
{
float val = GVD.GetPathCost(c, r);
voro[c + r * NumColumns] = new Color(Vector3.One - new Color(val, val, val).ToVector3());
}
}
voronoiFieldBitmap.SetData <Color>(voro);
}
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);
}
private void addBodyToGrid(Body body, int obstacleId)
{
BodyList.Add(body);
Vertices vertices = ((PolygonShape)body.FixtureList[0].Shape).Vertices;
for (int i = 0; i < vertices.Count; i++)
{
int to = i == vertices.Count - 1 ? 0 : i + 1;
foreach (GridCell cell in rasterizeLine(body.GetWorldPoint(vertices[i]), body.GetWorldPoint(vertices[to])))
{
Obstacles[obstacleId].Add(cell);
GVD.SetObstacle(cell, obstacleId);
OccupiedCells[cell.C, cell.R]++;
LinkedList <Body> obs = ObstaclesByCell[cell.C, cell.R];
if (obs == null)
{
obs = ObstaclesByCell[cell.C, cell.R] = new LinkedList <Body>();
}
obs.Add(body);
}
}
}
public void BuildGVD()
{
GVD.Update();
distanceFieldBitmap = null;
voronoiFieldBitmap = null;
}
public Vector2 GetNearestObstacleCellLocation(Vector2 pos)
{
return(CellPositionToPoint(GVD.GetNearestObstacle(PointToCellPosition(pos))) + new Vector2(HalfResolution));
}
public void UpdateVoronoiFieldParameters(float alpha, float dmax)
{
GVD.UpdatePathCostParameters(alpha, dmax);
voronoiFieldBitmap = null;
}
