Menge
Modular Pedestrian Simulation Framework for Research and Development
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
PedVOAgentContext.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 
45 #ifndef __PEDVO_AGENT_CONTEXT_H__
46 #define __PEDVO_AGENT_CONTEXT_H__
47 
48 #include "../Orca/ORCATypeAgentContext.h"
49 #include "PedVOAgent.h"
50 
51 namespace PedVO {
56 }
57 
59 // Implementation of PedVOAgentContext
61 
62 // Specialization
63 template <>
64 std::string ORCATypeAgentContext< PedVO::Agent >::contextName() const { return "PedVO"; }
65 
67 
74 template<>
76  // Draw the optimized velocity (transformed and untransformed
77  agent->computeNewVelocity();
78  glPushAttrib( GL_POINT_BIT );
79  glPointSize( 3.f );
80  // NORMAL space
81  glColor3f( 0.2f, 0.2f, 1.f );
82  glBegin( GL_POINTS );
83  glVertex3f( agent->_pos.x() + agent->_velNew.x(), Y, agent->_pos.y() + agent->_velNew.y() );
84  glEnd();
85  writeTextRadially( " v_new ", agent->_pos + agent->_velNew, agent->_velNew, true );
86 
87  Vector2 prefDir( agent->_velPref.getPreferred() );
88  Vector2 n( -prefDir.y(), prefDir.x() );
89  float vx = agent->_velNew * prefDir;
90  float vy = agent->_velNew * n;
91  vy /= agent->_turningBias;
92  glColor3f( 1.f, 0.1f, 1.f );
93  glBegin( GL_POINTS );
94  glVertex3f( agent->_pos.x() + vx, Y, agent->_pos.y() + vy );
95  glEnd();
96  Vector2 vn( vx, vy );
97  writeTextRadially( " v_new^x", agent->_pos + vn, vn, true );
98  glPopAttrib();
99 }
100 
102 
111 template<>
112 void ORCATypeAgentContext< PedVO::Agent >::drawORCALine( const PedVO::Agent * agent, const Menge::Math::Line & line, bool isAgent ) const {
113  if ( agent->_turningBias != 1.f ) {
114  // Transform line into new line
115  Menge::Math::Line lEuclid; // the line transformed, fully, back into Euclidian space
116  lEuclid._point = line._point;
117  lEuclid._direction = line._direction;
118  Menge::Math::Line lAffine; // the line (with scale) rotated back into Euclidian
119  float prefSpeed = agent->_velPref.getSpeed();
120  // Transformation is dependent on prefSpeed being non-zero
121  bool rotated = false;
122  if ( prefSpeed > Menge::EPS ) {
123  Vector2 prefDir( agent->_velPref.getPreferred() );
124  Vector2 n( prefDir.y(), prefDir.x() );
125  // rotate and scale all of the lines
126  float turnInv = 1.f / agent->_turningBias;
127 
128  // scale
129  Vector2 p( line._point.x(), line._point.y() * agent->_turningBias );
130  Vector2 d ( line._direction.x(), line._direction.y() * agent->_turningBias );
131  // rotate
132  float px = p * prefDir;
133  float py = p * n;
134  float dx = d * prefDir;
135  float dy = d * n ;
136  // set
137  lEuclid._point.set( px, py );
138  lEuclid._direction.set( dx, dy );
139 
140  px = line._point * prefDir;
141  py = line._point * n;
142  dx = line._direction * prefDir;
143  dy = line._direction * n ;
144  lAffine._point.set( px, py );
145  lAffine._direction.set( dx, dy );
146  rotated = true;
147  }
148  if ( rotated ) {
149  if ( isAgent ) {
150  drawHalfPlane( lAffine, agent->_pos, 1.f, 0.5f, 0.f, Y );
151  if ( ! _showOrcaLines ) drawHalfPlane( lEuclid, agent->_pos, 1.f, 0.f, 0.f, Y );
152  } else {
153  drawHalfPlane( lAffine, agent->_pos, 0.5f, 0.5f, 0.5f, Y );
154  if ( ! _showOrcaLines ) drawHalfPlane( lEuclid, agent->_pos, 0.75f, 0.75f, 0.75f, Y );
155  }
156  } else {
157  if ( isAgent ) {
158  drawHalfPlane( lEuclid, agent->_pos, 1.f, 0.f, 0.f, Y );
159  } else {
160  drawHalfPlane( lEuclid, agent->_pos, 0.75f, 0.75f, 0.75f, Y );
161  }
162  }
163 
164  } else {
165  if ( isAgent ) {
166  drawHalfPlane( line, agent->_pos, 1.f, 0.f, 0.f, Y );
167  } else {
168  drawHalfPlane( line, agent->_pos, 0.75f, 0.75f, 0.75f, Y );
169  }
170  }
171 }
172 
173 #endif // __PEDVO_AGENT_CONTEXT_H__
PrefVelocity _velPref
The 2D preferred velocity of the agent.
Definition: BaseAgent.h:218
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
ORCATypeAgentContext< Agent > PedVOAgentContext
Declaration of ORCA-type agent context for PedVO agents.
Definition: PedVOAgentContext.h:55
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
MENGE_API const float EPS
Suitably small number for testing for functional zero values.
Definition: geomQuery.cpp:52
Contains the specification of the pedestrian model based on Pedestrian Velocity Obstacles.
Definition: PedVOAgent.cpp:46
Vector2 _direction
The direction of the directed line.
Definition: Line.h:91
void set(Type x, Type y)
Set the x- and y-values from scalar values.
Definition: Vector2.h:121
virtual std::string contextName() const
Returns the name of the context for display.
Definition: ORCATypeAgentContext.h:77
Vector2 _velNew
The new velocity computed in computeNewVelocity.
Definition: BaseAgent.h:226
Contains the PedVOAgent class.
float _turningBias
Turning bias.
Definition: PedVOAgent.h:163
Defines an agent in the simulation.
Definition: PedVOAgent.h:54
float getSpeed() const
Returns the speed of the preferred velocity.
Definition: PrefVelocity.h:145
Vector2 getPreferred() const
Returns the preferred direction of the span.
Definition: PrefVelocity.h:129
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
void computeNewVelocity()
Computes the new velocity of this agent.
Definition: PedVOAgent.cpp:562
Type y() const
Get the y-value.
Definition: Vector2.h:113
Vector2 _point
A point on the directed line.
Definition: Line.h:86