Commit 26e58f18 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

Now visualizing the cost function of the selected agent.

parent 130d74c0
......@@ -75,6 +75,17 @@ Vector2D Policy::ComputeNewVelocity(Agent* agent, WorldBase * world)
return clampVector(currentVelocity + acceleration * dt, agent->getMaximumSpeed());
}
float Policy::ComputeCostForVelocity(const Vector2D& velocity, Agent* agent, WorldBase* world)
{
// compute the cost for this velocity
float totalCost = 0;
for (auto& costFunction : cost_functions_)
totalCost += costFunction.second * costFunction.first->GetCost(velocity, agent, world);
return totalCost;
}
Vector2D Policy::getAccelerationFromGradient(Agent* agent, WorldBase * world)
{
const Vector2D& CurrentVelocity = agent->getVelocity();
......
......@@ -140,6 +140,13 @@ public:
/// <param name="world">The world in which the simulation takes place.</param>
Vector2D ComputeNewVelocity(Agent* agent, WorldBase* world);
/// <summary>Computes and returns the cost that this Policy assigns to a (hypothetical) velocity for a given agent.
/// This is useful for e.g. visualizing a cost function. For most purposes, ComputeNewVelocity() is better because it encapsulates more work.</summary>
/// <param name="velocity">A velocity vector for which the cost should be computed.</param>
/// <param name="agent">The agent for which a new velocity should be computed.</param>
/// <param name="world">The world in which the simulation takes place.</param>
float ComputeCostForVelocity(const Vector2D& velocity, Agent* agent, WorldBase* world);
/// <summary>Computes and returns a 2D vector describing the contact forces experienced by a given agent due to collisions.
/// This force is already scaled by the scaling factor stored in this Policy.</summary>
/// <param name="agent">The agent for which a new velocity should be computed.</param>
......
......@@ -357,6 +357,17 @@ void UMANSOpenGLWidget::addSegmentsToBuffer(const std::vector<LineSegment2D>& se
}
}
void UMANSOpenGLWidget::addContourToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth)
{
auto vis = visualizationData[target];
for (size_t i = 0; i < points.size(); ++i)
{
vis->AddData(pointToQVector3D(points[i], depth), color);
vis->AddData(pointToQVector3D(points[(i + 1) % points.size()], depth), color);
}
}
void UMANSOpenGLWidget::addPointsToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth)
{
auto vis = visualizationData[target];
......@@ -378,7 +389,7 @@ void UMANSOpenGLWidget::drawSimulation()
drawAgent(*agent);
}
void UMANSOpenGLWidget::drawAgent(const Agent& agent)
void UMANSOpenGLWidget::drawAgent(Agent& agent)
{
const bool isActiveAgent = activeAgent != nullptr && agent.getID() == activeAgent->getID();
......@@ -410,6 +421,9 @@ void UMANSOpenGLWidget::drawAgent(const Agent& agent)
// draw the goal
addPointsToBuffer(approximateDisk_Triangles(agent.getGoal(), radius, 8), QColor(0, 255, 128), Target_Agents_Solid, Depth_Agents);
// draw the values of the agent's cost function
drawAgentCostCircle(agent);
}
/*// draw the trajectory that the agent has traversed so far
......@@ -419,6 +433,114 @@ void UMANSOpenGLWidget::drawAgent(const Agent& agent)
addPointsToBuffer(approximateDisk_Triangles(pt, radius / 3.0), agentQColorLight, Target_DynamicData_Solid, Depth_Obstacles);*/
}
QColor colorFromCost(float cost, float minCost, float maxCost)
{
// convert the cost to a fraction in [0,1] between min and max
const float minCost2 = std::max(minCost, -100.f); //0
const float maxCost2 = std::min(maxCost, 100.f); //50
const float cost2 = std::min(std::max(cost, minCost2), maxCost2);
float frac = (cost2 - minCost2) / (maxCost2 - minCost2);
frac = HelperFunctions::Clamp(frac, 0.f, 1.f);
if (isnan(cost) || minCost2 >= maxCost2)
frac = 1;
// convert this to a HSV representation: from blue to purple
const int minH = 360 + 250; const int maxH = 300;
int H = (int)(minH + frac * (maxH - minH)) % 360;
return QColor::fromHsv(H, 255, 255);
}
void UMANSOpenGLWidget::drawAgentCostCircle_AddPoint(const Vector2D& basePos, const std::pair<Vector2D, float>& data, const float minCost, const float maxCost)
{
Vector2D v(basePos + data.first);
QVector3D vec3D(v.x, v.y, (float)(Depth_Agents - 0.01));
visualizationData[Target_Agents_Solid]->AddData(vec3D, colorFromCost(data.second, minCost, maxCost));
}
void UMANSOpenGLWidget::drawAgentCostCircle(Agent& agent)
{
// - collect cost values for a set of sample velocities
WorldBase* world = simulator->GetWorld();
const float maxSpeed = agent.getMaximumSpeed();
const float ds = 0.025f;
const int dA = 1;
std::vector<std::vector<std::pair<Vector2D, float>>> costs;
Vector2D zeroVector(0, 0);
const float zeroVectorCost = agent.getPolicy()->ComputeCostForVelocity(zeroVector, &agent, world);
float minCost = zeroVectorCost;
float maxCost = zeroVectorCost;
costs.push_back({ {zeroVector, zeroVectorCost } });
for (float s = ds; s < maxSpeed; s += ds)
{
Vector2D base(s, 0);
std::vector<std::pair<Vector2D, float>> costsForSpeed;
for (int A = 0; A < 360; A += dA)
{
float angleRadians = (float)(A / 180.0 * PI);
const Vector2D& v = rotateCounterClockwise(base, angleRadians);
float cost = agent.getPolicy()->ComputeCostForVelocity(v, &agent, world);
minCost = std::min(cost, minCost);
maxCost = std::max(cost, maxCost);
costsForSpeed.push_back({ v, cost });
}
costs.push_back(costsForSpeed);
}
// - draw these values in a circle
const Vector2D& agentPos = agent.getPosition();
for (size_t i = 1; i < costs.size(); ++i)
{
const auto& currentList = costs[i];
const auto& prevList = costs[i - 1];
for (size_t j = 0; j < currentList.size(); ++j)
{
const auto& data1 = currentList[j];
const auto& data2 = currentList[(j + 1) % currentList.size()];
if (i == 1)
{
// 1 triangle
const auto& data3 = prevList[0];
drawAgentCostCircle_AddPoint(agentPos, data1, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data2, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data3, minCost, maxCost);
}
else
{
// 2 triangles
const auto& data3 = prevList[(j + 1) % prevList.size()];
const auto& data4 = prevList[j];
drawAgentCostCircle_AddPoint(agentPos, data1, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data2, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data3, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data1, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data3, minCost, maxCost);
drawAgentCostCircle_AddPoint(agentPos, data4, minCost, maxCost);
}
}
}
// - highlight certain velocities
addContourToBuffer(
approximateDisk_Outline(agentPos + agent.getPreferredVelocity(), 0.1f),
QColor(128, 128, 128), Target_Agents_Contours, Depth_Agents + 0.2);
addContourToBuffer(approximateDisk_Outline(
agentPos + agent.getPolicy()->ComputeNewVelocity(&agent, world), 0.1f),
QColor(255, 255, 255), Target_Agents_Contours, Depth_Agents + 0.2);
}
void UMANSOpenGLWidget::drawEnvironment(const bool refresh)
{
const WorldBase* world = simulator->GetWorld();
......@@ -457,13 +579,14 @@ void UMANSOpenGLWidget::drawGrid(bool refresh)
bbox = dynamic_cast<const WorldToric*>(world)->GetBoundingBox();
// draw the bounding box in a different color
std::vector<LineSegment2D> grid_boundary = {
LineSegment2D({bbox.first.x, bbox.first.y }, {bbox.first.x, bbox.second.y}),
LineSegment2D({bbox.second.x, bbox.first.y }, {bbox.second.x, bbox.second.y}),
LineSegment2D({bbox.first.x, bbox.first.y }, {bbox.second.x, bbox.first.y }),
LineSegment2D({bbox.first.x, bbox.second.y}, {bbox.second.x, bbox.second.y})
std::vector<Vector2D> grid_boundary = {
{bbox.first.x, bbox.first.y },
{bbox.first.x, bbox.second.y},
{bbox.second.x, bbox.second.y},
{bbox.second.x, bbox.first.y }
};
addSegmentsToBuffer(grid_boundary, QColor(255, 0, 0), Target_Grid, Depth_GridBoundary);
addContourToBuffer(grid_boundary, QColor(255, 0, 0), Target_Grid, Depth_GridBoundary);
}
// - otherwise, use a default range of -100 to +100
else
......
......@@ -78,11 +78,16 @@ private:
QSizeF getScreenToWorldScale() const;
void drawSimulation();
void drawAgent(const Agent& agent);
void drawAgent(Agent& agent);
void drawAgentCostCircle(Agent& agent);
void drawAgentCostCircle_AddPoint(const Vector2D& basePos, const std::pair<Vector2D, float>& data, const float minCost, const float maxCost);
void drawEnvironment(const bool refresh = false);
void drawGrid(const bool refresh = false);
void addPointsToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth);
void addSegmentsToBuffer(const std::vector<LineSegment2D>& segments, const QColor& color, const std::string& target, const double depth);
void addContourToBuffer(const std::vector<Vector2D>& points, const QColor& color, const std::string& target, const double depth);
void updateCursor();
void updateSimulationTimerText();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment