Menge
Modular Pedestrian Simulation Framework for Research and Development
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
ORCATypeAgentContext.h
Go to the documentation of this file.
1 /*
2 
3 License
4 
5 Menge
6 Copyright and trademark 2012-14 University of North Carolina at Chapel Hill.
7 All rights reserved.
8 
9 Permission to use, copy, modify, and distribute this software and its documentation
10 for educational, research, and non-profit purposes, without fee, and without a
11 written agreement is hereby granted, provided that the above copyright notice,
12 this paragraph, and the following four paragraphs appear in all copies.
13 
14 This software program and documentation are copyrighted by the University of North
15 Carolina at Chapel Hill. The software program and documentation are supplied "as is,"
16 without any accompanying services from the University of North Carolina at Chapel
17 Hill or the authors. The University of North Carolina at Chapel Hill and the
18 authors do not warrant that the operation of the program will be uninterrupted
19 or error-free. The end-user understands that the program was developed for research
20 purposes and is advised not to rely exclusively on the program for any reason.
21 
22 IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS
23 BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
24 DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
25 DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE
26 AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 
28 THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY
29 DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
30 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY WARRANTY
31 OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND
32 THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE NO OBLIGATIONS
33 TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
34 
35 Any questions or comments should be sent to the authors {menge,geom}@cs.unc.edu
36 
37 */
38 
48 #ifndef __ORCA_TYPE_AGENT_CONTEXT_H__
49 #define __ORCA_TYPE_AGENT_CONTEXT_H__
50 
51 #include "BaseAgentContext.h"
52 #include "shapes.h"
53 #include "VisAgent.h"
54 #include <sstream>
55 #include <iomanip>
56 
61 template < class Agent >
63 public:
70  ORCATypeAgentContext( Menge::VisAgent ** agents, unsigned int agtCount );
71 
77  virtual std::string contextName() const { return "UNDEFINED ORCA TYPE"; }
78 
88 
93  virtual void update();
94 
95 protected:
96 
103  virtual void draw3DGL( bool select=false );
104 
117  void drawHalfPlane( const Menge::Math::Line & line, const Vector2 & pos, float r, float g, float b, float Y ) const;
118 
127  void drawORCALine( const Agent * agent, const Menge::Math::Line & line, bool isAgent ) const;
128 
135  void drawOptVelocity( Agent * agent ) const;
136 
145  virtual std::string agentText( const Menge::Agents::BaseAgent * agent ) const;
146 
151 
157  void drawORCALines( const Agent * agt ) const;
158 
163 
167  size_t _visNbrID;
168 
175  void visORCA( const Agent * agt ) const;
176 };
177 
179 
180 template< class Agent >
181 ORCATypeAgentContext< Agent >::ORCATypeAgentContext( Menge::VisAgent ** agents, unsigned int agtCount ): Menge::BaseAgentContext(agents,agtCount), _showOrcaLines(false),_visualizeORCA(false),_visNbrID(0) {
182 }
183 
185 
186 template< class Agent >
188  if ( this->_selected && _visNbrID ) {
189  const Agent * agt = dynamic_cast< const Agent * >( this->_selected->getAgent() );
190  if ( _visNbrID > 0 ) {
191  size_t NBR_COUNT = agt->_nearAgents.size();
192  if ( _visNbrID > NBR_COUNT ) {
193  _visNbrID = NBR_COUNT;
194  }
195  }
196  }
197 }
198 
200 
201 template< class Agent >
203  Menge::SceneGraph::ContextResult result = BaseAgentContext::handleKeyboard( e );
204  if ( !result.isHandled() ) {
205  SDLMod mods = e.key.keysym.mod;
206  bool hasCtrl = ( mods & KMOD_CTRL ) > 0;
207  bool hasAlt = ( mods & KMOD_ALT ) > 0;
208  bool hasShift = ( mods & KMOD_SHIFT ) > 0;
209  bool noMods = !(hasCtrl || hasAlt || hasShift );
210  if ( e.type == SDL_KEYDOWN ) {
211  if ( noMods ) {
212  if ( e.key.keysym.sym == SDLK_c ) {
213  _showOrcaLines = !_showOrcaLines;
214  result.set( true, true );
215  } else if ( e.key.keysym.sym == SDLK_z ) {
216  _visualizeORCA = !_visualizeORCA;
217  _visNbrID = 0;
218  result.set( true, true );
219  } else if ( e.key.keysym.sym == SDLK_UP ) {
220  if ( _visualizeORCA && this->_selected ) {
221  const Agent * agt = dynamic_cast< const Agent * >( this->_selected->getAgent() );
222  ++_visNbrID;
223  if ( _visNbrID >= agt->_nearAgents.size() ) _visNbrID = 0;
224  result.set( true, true );
225  }
226  } else if ( e.key.keysym.sym == SDLK_DOWN ) {
227  if ( _visualizeORCA && this->_selected ) {
228  const Agent * agt = dynamic_cast< const Agent * >( this->_selected->getAgent() );
229  if ( _visNbrID == 0 ) _visNbrID = agt->_nearAgents.size() - 1;
230  else --_visNbrID;
231  result.set( true, true );
232  }
233  }
234  }
235  }
236  }
237  return result;
238 }
239 
241 
242 template< class Agent >
245  if ( !select && this->_selected ) {
246  glPushAttrib( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_ENABLE_BIT | GL_LINE_BIT | GL_POLYGON_BIT );
247  glDepthMask( GL_FALSE );
248  glDisable( GL_DEPTH_TEST );
249  const Agent * agt = dynamic_cast< const Agent * >( this->_selected->getAgent() );
250  drawORCALines( agt );
251  visORCA( agt );
252  glPopAttrib();
253  }
254 }
255 
257 
258 template< class Agent >
260  const Agent * agent = dynamic_cast< const Agent * >( agt );
261  std::string m = Menge::BaseAgentContext::agentText( agent );
262  std::stringstream ss;
263  ss << std::setiosflags(std::ios::fixed) << std::setprecision( 2 );
264 
265  ss << "\n_________________________";
266  ss << "\nDraw OR(C)A lines";
267  if ( _showOrcaLines ) {
268  const size_t LINE_COUNT = agent->_orcaLines.size();
269  const size_t AGT_COUNT = agent->_nearAgents.size();
270  const size_t OBST_COUNT = LINE_COUNT - AGT_COUNT;
271  ss << "\n " << OBST_COUNT << " obstacle lines";
272  ss << "\n " << AGT_COUNT << " agent lines";
273  }
274  ss << "\nVisuali(z)e ORCA";
275  if ( _visualizeORCA ) {
276  if ( agent->_nearAgents.size() == 0 ) {
277  ss << "\n No nearby agents.";
278  } else {
279  size_t id = (agent->_nearAgents[_visNbrID].agent)->_id;
280  ss << "\n Showing agent: " << id << " (up/down arrow to change)";
281  }
282  }
283  return m + ss.str();
284 }
285 
287 
288 template< class Agent >
289 void ORCATypeAgentContext< Agent >::drawHalfPlane( const Menge::Math::Line & line, const Vector2 & pos, float r, float g, float b, float Y ) const {
290  const float DIST = 35.f;
291  Vector2 norm( -line._direction.y(), line._direction.x() );
292  Vector2 p0 = line._point + line._direction * DIST + pos;
293  Vector2 p1 = p0 - norm * DIST;
294  Vector2 p2 = p1 - line._direction * ( 2 * DIST );
295  Vector2 p3 = p2 + norm * DIST;
296 
297  glColor4f( r, g, b, 0.1f );
298  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
299  glEnable( GL_BLEND );
300 
301  glBegin( GL_QUADS );
302  glVertex3f( p0.x(), Y, p0.y() );
303  glVertex3f( p1.x(), Y, p1.y() );
304  glVertex3f( p2.x(), Y, p2.y() );
305  glVertex3f( p3.x(), Y, p3.y() );
306  glEnd();
307  glDisable( GL_BLEND );
308 
309  glBegin( GL_LINES );
310  glVertex3f( p0.x(), Y, p0.y() );
311  glVertex3f( p3.x(), Y, p3.y() );
312  glEnd();
313 }
314 
316 
317 template< class Agent >
318 void ORCATypeAgentContext< Agent >::drawORCALines( const Agent * agent ) const {
319  if ( _showOrcaLines && this->_selected ) {
320  Agent * agt = const_cast< Agent * >( agent );
321  agt->computeORCALines();
322  const size_t LINE_COUNT = agt->_orcaLines.size();
323  const size_t FIRST_AGENT = LINE_COUNT - agt->_nearAgents.size();
324  const float DIST = 35.f;
325 
326  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
327  glEnable( GL_BLEND );
328  // Obstacle color
329  glColor4f( 0.75f, 0.75f, 0.75f, 0.1f );
330  glBegin( GL_QUADS );
331  for ( size_t i = 0; i < LINE_COUNT; ++i ) {
332  // Agent color
333  if ( i == FIRST_AGENT ) {
334  glColor4f( 1.f, 0.f, 0.f, 0.1f );
335  }
336  const Menge::Math::Line & line = agt->_orcaLines[ i ];
337  // Find the nearest point on the orca line to the agent -- use that as the center
338  Vector2 norm( -line._direction.y(), line._direction.x() );
339  float t = line._direction * ( -line._point );
340  Vector2 nearPt = line._point + t * line._direction;
341  Vector2 p0 = nearPt + line._direction * DIST + agt->_pos;
342  Vector2 p1 = p0 - norm * DIST;
343  Vector2 p2 = p1 - line._direction * ( 2 * DIST );
344  Vector2 p3 = p2 + norm * DIST;
345 
346  glVertex3f( p0.x(), this->Y, p0.y() );
347  glVertex3f( p1.x(), this->Y, p1.y() );
348  glVertex3f( p2.x(), this->Y, p2.y() );
349  glVertex3f( p3.x(), this->Y, p3.y() );
350  }
351  glEnd();
352  glDisable( GL_BLEND );
353 
354  glColor4f( 0.75f, 0.75f, 0.75f, 0.1f );
355  glBegin( GL_LINES );
356  for ( size_t i = 0; i < LINE_COUNT; ++i ) {
357  if ( i == FIRST_AGENT ) {
358  glColor4f( 1.f, 0.f, 0.f, 0.1f );
359  }
360  const Menge::Math::Line & line = agt->_orcaLines[ i ];
361  float t = line._direction * ( -line._point );
362  Vector2 nearPt = line._point + t * line._direction;
363  Vector2 p0 = nearPt + line._direction * DIST + agt->_pos;
364  Vector2 p1 = nearPt - line._direction * DIST + agt->_pos;
365  glVertex3f( p0.x(), this->Y, p0.y() );
366  glVertex3f( p1.x(), this->Y, p1.y() );
367  }
368  glEnd();
369  // Label the orca lines from agents
370  glColor4f( 1.f, 0.f, 0.f, 1.f );
371  for ( size_t i = FIRST_AGENT; i < LINE_COUNT; ++i ) {
372  std::stringstream ss;
373  const Menge::Agents::BaseAgent * nbr = agent->_nearAgents[ i - FIRST_AGENT ].agent;
374  ss << nbr->_id;
375  Vector2 d = agent->_orcaLines[ i ].nearestPt( Vector2(0.f,0.f) );
376  Vector2 p = d + agent->_pos;
377  this->writeTextRadially( ss.str(), p, d, true );
378  this->writeAlignedText( ss.str(), nbr->_pos, Menge::SceneGraph::TextWriter::CENTERED, true );
379  }
380  }
381 }
382 
384 
385 template< class Agent >
386 void ORCATypeAgentContext< Agent >::visORCA( const Agent * agt ) const {
387  if ( _visualizeORCA && this->_selected ) {
388  if ( agt->_nearAgents.size() > 0 ) {
389  Vector2 velPref = agt->_velPref.getPreferredVel();
390  const float RAD_TO_DEG = 180.f * 3.1415927f;
391  glColor3f( 0.1f, 1.f, 0.1f );
392  Agent * agent = const_cast< Agent * >( agt );
393  agent->computeORCALines();
394  const Agent * nbr = static_cast< const Agent * >( agent->_nearAgents[ _visNbrID ].agent );
395  float R = agent->_radius + nbr->_radius;
396  Vector2 disp = nbr->_pos - agent->_pos;
397  float dist = abs( disp );
398  Vector2 dir = disp / dist;
399  Vector2 perp( -dir.y(), dir.x() );
400 
401  // Compute the tangent portions of the minkowski sum
402  float cosPhi = R / dist;
403  float sinPhi = sqrtf( 1 - cosPhi * cosPhi );
404  float cx = cosPhi * -dir.x();
405  float sx = sinPhi * -dir.x();
406  float cy = cosPhi * -dir.y();
407  float sy = sinPhi * -dir.y();
408 
409  Vector2 r0 = disp + R * Vector2( cx - sy, sx + cy );
410  Vector2 l0 = disp + R * Vector2( cx + sy, -sx + cy );
411  // modify the positions of r0 and l0 so that they project onto the center
412  //
413  float l = dist / ( r0 * dir );
414  r0 *= l;
415  l0 *= l;
416  r0 += agent->_pos;
417  l0 += agent->_pos;
418 
419  // What's the closest circle?
420  const float TAU = agent->_timeHorizon;
421  float minVel = dist / TAU;
422  float Rmin = R / TAU;
423  Vector2 center( agent->_pos + dir * minVel );
424  // First, draw leading circle
425  glPushMatrix();
426  glTranslatef( center.x(), this->Y, center.y() );
427  Menge::SceneGraph::Circle::drawCircle( Rmin, 0.1f, 1.f, 0.1f, 0.75f, GL_LINE );
428  glPopMatrix();
429 
430  Vector2 r1 = center + Rmin * Vector2( cx - sy, sx + cy );
431  Vector2 l1 = center + Rmin * Vector2( cx + sy, -sx + cy );
432 
433  glBegin( GL_LINES );
434  glVertex3f( r0.x(), this->Y, r0.y() );
435  glVertex3f( r1.x(), this->Y, r1.y() );
436  glVertex3f( l0.x(), this->Y, l0.y() );
437  glVertex3f( l1.x(), this->Y, l1.y() );
438  glEnd();
439 
440  // Use right of way to compute velocities
441  float row = agent->_priority - nbr->_priority;
442  Vector2 agtVel = agent->_vel;
443  Vector2 nbrVel = nbr->_vel;
444  Vector2 nbrVelPref = nbr->_velPref.getPreferredVel();
445  if ( row > 0.f ) {
446  // agent's advantage
447  row = row > 1.f ? 1.f : row;
448  if ( dir * velPref > dir * agent->_vel ) {
449  agtVel = velPref * row + ( 1.f - row ) * agent->_vel;
450  }
451  } else if ( row < 0.f ) {
452  // nbr's advantage
453  row = row < -1.f ? 1.f : -row;
454  if ( dir * nbrVelPref < dir * nbr->_vel ) {
455  nbrVel = nbrVelPref * row + ( 1.f - row ) * nbr->_vel;
456  }
457  }
458 
459  // Other guy's velocity
460  glColor3f( 0.1f, 0.1f, 0.8f );
461  glBegin( GL_LINES );
462  glVertex3f( nbr->_pos.x(), this->Y, nbr->_pos.y() );
463  glVertex3f( nbr->_pos.x() + nbrVel.x(), this->Y, nbr->_pos.y() + nbrVel.y() );
464  glEnd();
465  this->writeTextRadially( "v_j", nbr->_pos + nbrVel, nbrVel, true);
466 
467  // My velocity
468  glColor3f( 0.1f, 0.8f, 0.1f );
469  glBegin( GL_LINES );
470  glVertex3f( agent->_pos.x(), this->Y, agent->_pos.y() );
471  glVertex3f( agent->_pos.x() + agtVel.x(), this->Y, agent->_pos.y() + agtVel.y() );
472  glEnd();
473  this->writeTextRadially( "v_i", agent->_pos + agtVel, agtVel, true );
474 
475  // Relative velocity
476  glColor3f( 0.1f, 0.8f, 0.8f );
477  glBegin( GL_LINES );
478  Vector2 rel = agtVel - nbrVel;
479  glVertex3f( agent->_pos.x(), this->Y, agent->_pos.y() );
480  glVertex3f( agent->_pos.x() + rel.x(), this->Y, agent->_pos.y() + rel.y() );
481  glEnd();
482  this->writeTextRadially( "v_ij", agent->_pos + rel, rel, true );
483 
484 
485  // Draw the ORCA line
486  // Determine which line it is
487  size_t NBR_COUNT = agent->_nearAgents.size();
488  size_t FIRST_NBR = agent->_orcaLines.size() - NBR_COUNT;
489  drawORCALine( agent, agent->_orcaLines[ FIRST_NBR + _visNbrID ], true );
490 
491  // optimized velocity in transformed space
492  drawOptVelocity( agent );
493  }
494  }
495 }
496 
498 
499 template< class Agent >
500 void ORCATypeAgentContext< Agent >::drawORCALine( const Agent * agent, const Menge::Math::Line & line, bool isAgent ) const {
501  if ( isAgent ) {
502  drawHalfPlane( line, agent->_pos, 1.f, 0.f, 0.f, this->Y );
503  } else {
504  drawHalfPlane( line, agent->_pos, 0.75f, 0.75f, 0.75f, this->Y );
505  }
506 }
507 
509 
510 template< class Agent >
512  // Draw the optimized velocity (transformed and untransformed
513  agent->computeNewVelocity();
514  // NORMAL space
515  glPushAttrib( GL_POINT_BIT );
516  glPointSize( 3.f );
517  glColor3f( 0.2f, 0.2f, 1.f );
518  glBegin( GL_POINTS );
519  glVertex3f( agent->_pos.x() + agent->_velNew.x(), this->Y, agent->_pos.y() + agent->_velNew.y() );
520  glEnd();
521  this->writeTextRadially( " v_new ", agent->_pos + agent->_velNew, agent->_velNew, true );
522 }
523 
524 
525 #endif // __ORCA_TYPE_AGENT_CONTEXT_H__
static void drawCircle(float radius, float r, float g, float b, float a, GLenum style=GL_FILL)
Static function for drawing circles in the context with out instances.
Definition: shapes.cpp:179
void drawORCALine(const Agent *agent, const Menge::Math::Line &line, bool isAgent) const
Draws the given ORCA line for the given agent.
Definition: ORCATypeAgentContext.h:500
SDL_Event SDL_Event
Forward declaration of the SDL event type.
Definition: Context.h:53
void drawHalfPlane(const Menge::Math::Line &line, const Vector2 &pos, float r, float g, float b, float Y) const
Helper function for drawing a halfplane.
Definition: ORCATypeAgentContext.h:289
The core namespace. All elements of Menge are contained in this namespace.
Definition: AgentGenerator.cpp:43
ORCATypeAgentContext(Menge::VisAgent **agents, unsigned int agtCount)
Construtor.
Definition: ORCATypeAgentContext.h:181
Simple, cylindrical visualization for agents.
Context class for displaying various aspects of the ORCA-type agent computation.
Definition: ORCATypeAgentContext.h:62
Vector2 _pos
The current 2D position of the agent.
Definition: BaseAgent.h:208
virtual void draw3DGL(bool select=false)
Draw context elements into the 3D world.
Definition: ORCATypeAgentContext.h:243
virtual std::string agentText(const Agents::BaseAgent *agent) const
Creates a formatted string to be printed in the context for a particular agent.
Definition: BaseAgentContext.cpp:333
virtual void draw3DGL(bool select=false)
Draw context elements into the 3D world.
Definition: BaseAgentContext.cpp:202
std::vector< NearAgent > _nearAgents
The nearby agents to which the agent should respond.
Definition: BaseAgent.h:313
bool _showOrcaLines
Determines if the ORCA lines are drawn.
Definition: ORCATypeAgentContext.h:150
size_t _id
A globally unique identifier for each agent.
Definition: BaseAgent.h:287
Vector2 _direction
The direction of the directed line.
Definition: Line.h:91
virtual std::string contextName() const
Returns the name of the context for display.
Definition: ORCATypeAgentContext.h:77
virtual Menge::SceneGraph::ContextResult handleKeyboard(SDL_Event &e)
Give the context the opportunity to respond to a keyboard event.
Definition: ORCATypeAgentContext.h:202
A library of simple renderable OpenGL shapes.
bool isHandled() const
Reports if the result considers the event handled.
Definition: Context.h:105
Context class for displaying various characteristics of the Agents::BaseAgent class.
Definition: BaseAgentContext.h:73
void set(bool handled, bool redraw)
Sets the handled and needs redraw state simultaneously.
Definition: Context.h:127
Defines the result of the context's consideration of user input.
Definition: Context.h:74
The basic agent visualization class: a selectable cylinder.
Definition: VisAgent.h:66
void visORCA(const Agent *agt) const
The function that draws the visualization of the orca construction.
Definition: ORCATypeAgentContext.h:386
Defines the basic agent properties and functionality that all simulation agents share.
Definition: BaseAgent.h:123
MENGE_API const float RAD_TO_DEG
Scale factor for converting radians to degrees.
Definition: geomQuery.cpp:51
virtual void update()
Allow the context to update any time-dependent state it might have to the given global time...
Definition: ORCATypeAgentContext.h:187
Defines a directed line.
Definition: Line.h:57
void drawOptVelocity(Agent *agent) const
Draw the optimized velocity for the current set of orca lines.
Definition: ORCATypeAgentContext.h:511
Type x() const
Get the x-value.
Definition: Vector2.h:106
A basic context for interacting with and displaying basic agent parameters.
void drawORCALines(const Agent *agt) const
Function for drawing the ORCA lines acting on agt.
Definition: ORCATypeAgentContext.h:318
static const float Y
The drawing depth for the 3D elements.
Definition: BaseAgentContext.h:179
Type y() const
Get the y-value.
Definition: Vector2.h:113
bool _visualizeORCA
Determines if the ORCA line construction is visualized.
Definition: ORCATypeAgentContext.h:162
virtual std::string agentText(const Menge::Agents::BaseAgent *agent) const
Creates a formatted string to be printed in the context for a particular agent.
Definition: ORCATypeAgentContext.h:259
size_t _visNbrID
The agent to visualize.
Definition: ORCATypeAgentContext.h:167
Vector2 _point
A point on the directed line.
Definition: Line.h:86