Commit 00301910 authored by Thomas Jund's avatar Thomas Jund
Browse files

Merge cgogn:~pitiot/CGoGN_Apps/SocialAgents2

parents 75eee1dd 59ec48c3
......@@ -31,7 +31,8 @@ public:
MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal=0, Dart d=NIL, ArticulatedObstacle * art=NULL, int indParent=-1);
void initGL();
void draw();
void addGeneralCell ( Dart d);
bool removeGeneralCell (Dart d);
bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
// void contournerBatiment();
void updateAgentNeighbors() ;
......@@ -91,7 +92,7 @@ public:
Obstacle* * obstacles_;
std::vector<Dart> * belonging_cells;
std::vector<Dart> * neighbor_cells;
std::set<Dart> general_belonging;
std::vector<std::pair<Dart, int> > general_belonging;
VEC3 front;
VEC3 finalGoal;
......
......@@ -142,6 +142,9 @@ public:
ThreadUpdateInfo* tc2 ;
ThreadUpdateInfo* tc3 ;
ThreadUpdateInfo* tc4 ;
long int time_agent;
long int time_obstacle;
};
#endif
......@@ -168,6 +168,8 @@ public:
Utils::QT::uiDockInterface dock ;
QTimer* timer ;
time_t elapsedTime;
long int time_update;
/* nbIterations counts the current number of iterations
* When maxIterations is reached the simulation is stopped
......@@ -209,6 +211,7 @@ public:
unsigned int agentSlider;
int posXSlider;
int posYSlider;
bool display_times;
public slots:
void animate() ;
......@@ -216,6 +219,7 @@ public slots:
void slot_timer(bool b)
{
b ? timer->start() : timer->stop() ;
// display_times =b;
}
void slot_drawEnvLines(bool b)
{
......
......@@ -5,7 +5,7 @@
New behaviour algorithm based on a dynamic model
\*=====================================================================*/
#define POTENTIAL
#include "agent.h"
#include "simulator.h"
#include "Geometry/frame.h"
......@@ -716,6 +716,8 @@ static int moAppend(MovingObstacle **moving_obstacles, int nb_mos, int max, Movi
//-----------------------------------------------------------------
//-----------------------------------------------------------------
// Search for the best new velocity.
#ifdef POTENTIAL
void Agent::computeNewVelocity()
{
// The objective is to compute the sum of forces exerted on the agent.
......@@ -978,21 +980,530 @@ void Agent::computeNewVelocity()
}
//-----------------------------------------------------------------
#else
/* Search for the best new velocity. */
void Agent::computeNewVelocity() // RVO2 : évitement agents entres eux et avec les obstacles fixes + ajout obstacles mobiles
{
obstacle_priority(&prefVelocity_);//prise en compte obstacles mobiles
std::vector<Line> orcaLines_ ;
orcaLines_.reserve(obstacleNeighbors_.size() + agentNeighbors_.size()) ;
float invTimeHorizonObst = 1.0f / timeHorizonObst_ ;
/* Create obstacle ORCA lines. */
for (std::vector<std::pair<float, Obstacle*> >::iterator it = obstacleNeighbors_.begin() ;
it != obstacleNeighbors_.end() ; ++it)
{
const Obstacle* obst = it->second ;
const VEC3 relativePosition1(obst->p1 - getPosition()) ;
const VEC3 relativePosition2(obst->p2 - getPosition()) ;
/*
* Check if velocity obstacle of obstacle is already taken care of by
* previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false ;
for (unsigned int j = 0 ; j < orcaLines_.size() ; ++j)
{
if (det2D(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point,
orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON
&& det2D(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point,
orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON)
{
alreadyCovered = true ;
break ;
}
}
if (alreadyCovered)
continue ;
/* Not yet covered. Check for collisions. */
const float distSq1 = relativePosition1.norm2() ;
const float distSq2 = relativePosition2.norm2() ;
const float radiusSq = radius_ * radius_ ;
const VEC3 obstacleVector(obst->p2 - obst->p1) ;
const VEC3 nextObstacleVector(obst->nextP - obst->p2) ;
const VEC3 previousObstacleVector(obst->p1 - obst->prevP) ;
const float s = (-1.0f * relativePosition1 * obstacleVector) / obstacleVector.norm2() ;
const float distSqLine = (-1.0f * relativePosition1 - s * obstacleVector).norm2() ;
Line line ;
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
// if (obstacle1->isConvex_)
// {
line.point = VEC3(0) ;
line.direction = VEC3(-relativePosition1[1], relativePosition1[0], 0) ;
line.direction.normalize() ;
orcaLines_.push_back(line) ;
// }
continue ;
}
else if (s > 1 && distSq2 <= radiusSq)
{
/* Collision with right vertex. Ignore if non-convex
* or if it will be taken care of by neighboring obstacle */
// VEC3 nextObstacleVectorN(nextObstacleVector);
// nextObstacleVectorN.normalize();
// if (/*obstacle2->isConvex_ && */det2D(relativePosition2, nextObstacleVectorN/*obstacle2->unitDir_*/) >= 0)
// {
line.point = VEC3(0) ;
line.direction = VEC3(-relativePosition2[1], relativePosition2[0], 0) ;
line.direction.normalize() ;
orcaLines_.push_back(line) ;
// }
continue ;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = VEC3(0) ;
line.direction = VEC3(-1.0f * obstacleVector) ;
line.direction.normalize() ;
orcaLines_.push_back(line) ;
continue ;
}
/*
* No collision.
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. Legs extend cut-off line when nonconvex vertex.
*/
VEC3 leftLegDirection, rightLegDirection ;
bool obst1EQobst2 = false ;
bool obst2EQobst1 = false ;
if (s < 0 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
// if (!obstacle1->isConvex_)
// {
// /* Ignore obstacle. */
// continue;
// }
// obst2 = obst1;
obst2EQobst1 = true ;
const float leg1 = std::sqrt(distSq1 - radiusSq) ;
leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_,
relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ;
rightLegDirection = VEC3(relativePosition1[0] * leg1 + relativePosition1[1] * radius_,
-relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ;
}
else if (s > 1 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that right vertex
* defines velocity obstacle.
*/
// if (!obstacle2->isConvex_)
// {
// /* Ignore obstacle. */
// continue;
// }
// obst1 = obst2;
obst1EQobst2 = true ;
const float leg2 = std::sqrt(distSq2 - radiusSq) ;
leftLegDirection = VEC3(relativePosition2[0] * leg2 - relativePosition2[1] * radius_,
relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ;
rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_,
-relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ;
}
else
{
/* Usual situation. */
// if (obstacle1->isConvex_)
// {
const float leg1 = std::sqrt(distSq1 - radiusSq) ;
leftLegDirection = VEC3(relativePosition1[0] * leg1 - relativePosition1[1] * radius_,
relativePosition1[0] * radius_ + relativePosition1[1] * leg1, 0) / distSq1 ;
// }
// else
// {
// /* Left vertex non-convex; left leg extends cut-off line. */
// leftLegDirection = -obstacle1->unitDir_;
// }
// if (obstacle2->isConvex_)
// {
const float leg2 = std::sqrt(distSq2 - radiusSq) ;
rightLegDirection = VEC3(relativePosition2[0] * leg2 + relativePosition2[1] * radius_,
-relativePosition2[0] * radius_ + relativePosition2[1] * leg2, 0) / distSq2 ;
// }
// else
// {
// /* Right vertex non-convex; right leg extends cut-off line. */
// rightLegDirection = obstacle1->unitDir_;
// }
}
/*
* Legs can never point into neighboring edge when convex vertex,
* take cutoff-line of neighboring edge instead. If velocity projected on
* "foreign" leg, no constraint is added.
*/
bool isLeftLegForeign = false ;
bool isRightLegForeign = false ;
VEC3 leftObstacle = obst1EQobst2 ? obstacleVector : previousObstacleVector ;
leftObstacle.normalize() ;
if (/*obstacle1->isConvex_ && */det2D(leftLegDirection, -1.0f * leftObstacle) > 0.0f)
{
/* Left leg points into obstacle. */
leftLegDirection = -1.0f * leftObstacle ;
isLeftLegForeign = true ;
}
VEC3 rightObstacle = obst2EQobst1 ? obstacleVector : nextObstacleVector ;
rightObstacle.normalize() ;
if (/*obstacle2->isConvex_ && */det2D(rightLegDirection, rightObstacle) < 0.0f)
{
/* Right leg points into obstacle. */
rightLegDirection = rightObstacle ;
isRightLegForeign = true ;
}
/* Compute cut-off centers. */
const VEC3 leftCutoff = invTimeHorizonObst
* ((obst1EQobst2 ? obst->p2 : obst->p1) - getPosition()) ;
const VEC3 rightCutoff = invTimeHorizonObst
* ((obst2EQobst1 ? obst->p1 : obst->p2) - getPosition()) ;
const VEC3 cutoffVec = rightCutoff - leftCutoff ;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
const float t = (/*obst1 == obst2*/
obst1EQobst2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / cutoffVec.norm2()) ;
const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection) ;
const float tRight = ((velocity_ - rightCutoff) * rightLegDirection) ;
if ((t < 0.0f && tLeft < 0.0f)
|| (/*obst1 == obst2*/obst1EQobst2 && tLeft < 0.0f && tRight < 0.0f))
{
/* Project on left cut-off circle. */
VEC3 unitW = velocity_ - leftCutoff ;
unitW.normalize() ;
line.direction = VEC3(unitW[1], -unitW[0], 0) ;
line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW ;
orcaLines_.push_back(line) ;
continue ;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
VEC3 unitW = velocity_ - rightCutoff ;
unitW.normalize() ;
line.direction = VEC3(unitW[1], -unitW[0], 0) ;
line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW ;
orcaLines_.push_back(line) ;
continue ;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is closest
* to velocity.
*/
VEC3 v1 = velocity_ - (leftCutoff + t * cutoffVec) ;
VEC3 v2 = velocity_ - (leftCutoff + tLeft * leftLegDirection) ;
VEC3 v3 = velocity_ - (rightCutoff + tRight * rightLegDirection) ;
const float distSqCutoff =
(t < 0.0f || t > 1.0f || /*obst1 == obst2*/obst1EQobst2) ? std::numeric_limits<float>::infinity() : v1.norm2() ;
const float distSqLeft =
(tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : v2.norm2() ;
const float distSqRight =
(tRight < 0.0f) ? std::numeric_limits<float>::infinity() : v3.norm2() ;
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
VEC3 dir = obst1EQobst2 ? nextObstacleVector : obstacleVector ;
dir.normalize() ;
line.direction = -1.0f * dir ;
line.direction.normalize() ;
line.point = leftCutoff
+ radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ;
orcaLines_.push_back(line) ;
continue ;
}
else if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign) continue ;
line.direction = leftLegDirection ;
line.point = leftCutoff
+ radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ;
orcaLines_.push_back(line) ;
continue ;
}
else
{
/* Project on right leg. */
if (isRightLegForeign) continue ;
line.direction = -1.0f * rightLegDirection ;
line.point = rightCutoff
+ radius_ * invTimeHorizonObst * VEC3(-line.direction[1], line.direction[0], 0) ;
orcaLines_.push_back(line) ;
continue ;
}
}
const unsigned int numObstLines = orcaLines_.size() ;
const float invTimeHorizon = 1.0f / timeHorizon_ ;
/* Create agent ORCA lines. */
unsigned int nbA = 0 ;
for (std::vector<std::pair<float, Agent*> >::iterator it = agentNeighbors_.begin() ;
it != agentNeighbors_.end() && nbA < maxNeighbors_ ; ++it, ++nbA)
{
Agent* other = it->second ;
const VEC3 relativePosition(other->getPosition() - getPosition()) ;
const VEC3 relativeVelocity(velocity_ - other->velocity_) ;
const float distSq = relativePosition.norm2() ;
const float combinedRadius = radius_ + other->radius_ ;
const float combinedRadiusSq = combinedRadius * combinedRadius ;
bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
Line line ;
VEC3 u ;
/* No collision. */
if (distSq > combinedRadiusSq)
{
const VEC3 w(relativeVelocity - invTimeHorizon * relativePosition) ;
const float wLengthSq = w.norm2() ;
const float dotProduct1 = w * relativePosition ;
/* Project on cut-off circle */
if (dotProduct1 < 0.0f && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq)
{
const float wLength = std::sqrt(wLengthSq) ;
const VEC3 unitW = w / wLength ;
line.direction = VEC3(unitW[1], -unitW[0], 0) ;
u = (combinedRadius * invTimeHorizon - wLength) * unitW ;
}
/* Project on legs */
else
{
const float leg = std::sqrt(distSq - combinedRadiusSq) ;
if (det2D(relativePosition, w) > 0.0f) // Project on left leg
line.direction = VEC3(
relativePosition[0] * leg - relativePosition[1] * combinedRadius,
relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0)
/ distSq ;
else
// Project on right leg
line.direction = -1.0f
* VEC3(relativePosition[0] * leg + relativePosition[1] * combinedRadius,
-relativePosition[0] * combinedRadius + relativePosition[1] * leg, 0)
/ distSq ;
const float dotProduct2 = relativeVelocity * line.direction ;
u = dotProduct2 * line.direction - relativeVelocity ;
}
}
/* Collision. */
else
{
/* Project on cut-off circle of time timeStep. */
const float invTimeStep = 1.0f / sim_->timeStep_ ;
/* Vector from cutoff center to relative velocity. */
const VEC3 w = relativeVelocity - invTimeStep * relativePosition ;
const float wLength = w.norm() ;
const VEC3 unitW = w / wLength ;
line.direction = VEC3(unitW[1], -unitW[0], 0) ;
u = (combinedRadius * invTimeStep - wLength) * unitW ;
}
line.point = velocity_ + 0.5f * u ;
orcaLines_.push_back(line) ;
}
unsigned int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false,
newVelocity_) ;
if (lineFail < orcaLines_.size()) linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_,
newVelocity_) ;
}
bool Agent::linearProgram1(const std::vector<Line>& lines, unsigned int lineNo, float radius,
const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
const float dotProduct = lines[lineNo].point * lines[lineNo].direction ;
const float discriminant = dotProduct * dotProduct + radius * radius
- lines[lineNo].point.norm2() ;
/* Max speed circle fully invalidates line lineNo. */
if (discriminant < 0.0f) return false ;
const float sqrtDiscriminant = std::sqrt(discriminant) ;
float tLeft = -dotProduct - sqrtDiscriminant ;
float tRight = -dotProduct + sqrtDiscriminant ;
for (unsigned int i = 0 ; i < lineNo ; ++i)
{
const float denominator = det2D(lines[lineNo].direction, lines[i].direction) ;
const float numerator = det2D(lines[i].direction, lines[lineNo].point - lines[i].point) ;
if (std::fabs(denominator) <= RVO_EPSILON) // Lines lineNo and i are (almost) parallel.
{
if (numerator < 0.0f)
return false ;
else
continue ;
}
const float t = numerator / denominator ;
if (denominator >= 0.0f) // Line i bounds line lineNo on the right
tRight = std::min(tRight, t) ;
else
// Line i bounds line lineNo on the left
tLeft = std::max(tLeft, t) ;
if (tLeft > tRight) return false ;
}
/* Optimize direction. */
if (directionOpt)
{
if (optVelocity * lines[lineNo].direction > 0.0f) // Take right extreme
result = lines[lineNo].point + tRight * lines[lineNo].direction ;
else
// Take left extreme
result = lines[lineNo].point + tLeft * lines[lineNo].direction ;
}
/* Optimize closest point. */
else
{
const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point) ;
if (t < tLeft)
result = lines[lineNo].point + tLeft * lines[lineNo].direction ;
else if (t > tRight)
result = lines[lineNo].point + tRight * lines[lineNo].direction ;
else
result = lines[lineNo].point + t * lines[lineNo].direction ;
}
return true ;
}
//-----------------------------------------------------------------
unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius, const VEC3& optVelocity, bool directionOpt, VEC3& result)
unsigned int Agent::linearProgram2(const std::vector<Line>& lines, float radius,
const VEC3& optVelocity, bool directionOpt, VEC3& result)
{
return 1;
}
/*
* Optimize direction. Note that the optimization velocity is of unit
* length in this case.
*/
if (directionOpt)
result = optVelocity * radius ;
/* Optimize closest point and outside circle. */
else if (optVelocity.norm2() > radius * radius)
{
VEC3 optVelocityN(optVelocity) ;
optVelocityN.normalize() ;
result = optVelocityN * radius ;
}
/* Optimize closest point and inside circle. */
else
result = optVelocity ;
//-----------------------------------------------------------------
for (unsigned int i = 0 ; i < lines.size() ; ++i)
{
if (det2D(lines[i].direction, lines[i].point - result) > 0.0f)
{
/* Result does not satisfy constraint i. Compute new optimal result. */
const VEC3 tempResult(result) ;
if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result))
{
result = tempResult ;
return i ;
}
}
}
return lines.size() ;
}
void Agent::linearProgram3(const std::vector<Line>& lines, unsigned int numObstLines, unsigned int beginLine, float radius, VEC3& result)
void Agent::linearProgram3(const std::vector<Line>& lines, unsigned int numObstLines,
unsigned int beginLine, float radius, VEC3& result)
{
float distance = 0.0f ;
for (unsigned int i = beginLine ; i < lines.size() ; ++i)
{
if (det2D(lines[i].direction, lines[i].point - result) > distance)
{
/* Result does not satisfy constraint of line i. */
std::vector<Line> projLines(lines.begin(), lines.begin() + numObstLines) ;
for (unsigned int j = numObstLines ; j < i ; ++j)
{
Line line ;
float determinant = det2D(lines[i].direction, lines[j].direction) ;
if (std::fabs(determinant) <= RVO_EPSILON) // Line i and line j are (almost) parallel.
{
if (lines[i].direction * lines[j].direction > 0.0f)
continue ; // Line i and line j point in the same direction.
else
line.point = 0.5f * (lines[i].point + lines[j].point) ;// Line i and line j point in opposite direction.
}
else
line.point = lines[i].point
+ (det2D(lines[j].direction, lines[i].point - lines[j].point) / determinant)
* lines[i].direction ;
line.direction = lines[j].direction - lines[i].direction ;
line.direction.normalize() ;
projLines.push_back(line) ;
}
const VEC3 tempResult = result ;
/* This should in principle not happen. The result is by definition
* already in the feasible region of this linear program. If it fails,
* it is due to small floating point error, and the current result is
* kept.
*/
if (linearProgram2(projLines, radius,
VEC3(-lines[i].direction[1], lines[i].direction[0], 0.0f), true,
result) < projLines.size()) result = tempResult ;
distance = det2D(lines[i].direction, lines[i].point - result) ;
}
}
}