diff --git a/Ejemplo.bat b/Ejemplo.bat new file mode 100644 index 00000000..693f3ab6 --- /dev/null +++ b/Ejemplo.bat @@ -0,0 +1,14 @@ +cd Exe +menge.exe -p ../examples/sergio/ejemplo5.xml +menge.exe -p ../examples/sergio/defrente.xml +menge.exe -p ../examples/sergio/Obstaculo.xml +menge.exe -p ../examples/sergio/seguimiento.xml +menge.exe -p ../examples/sergio/puerta.xml +menge.exe -p ../examples/sergio/Ejemplo8.xml +menge.exe -p ../examples/sergio/Ejemplo10.xml +menge.exe -p ../examples/sergio/dosgrupos.xml +menge.exe -p ../examples/sergio/circle.xml +menge.exe -p ../examples/sergio/peqgrupo.xml +menge.exe -p ../examples/sergio/pruebaMid.xml + +cmd /k diff --git a/examples/sergio/Ejemplo10.xml b/examples/sergio/Ejemplo10.xml new file mode 100644 index 00000000..740bf133 --- /dev/null +++ b/examples/sergio/Ejemplo10.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/Ejemplo10/B.xml b/examples/sergio/Ejemplo10/B.xml new file mode 100644 index 00000000..c26fd238 --- /dev/null +++ b/examples/sergio/Ejemplo10/B.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo10/S.xml b/examples/sergio/Ejemplo10/S.xml new file mode 100644 index 00000000..8d15086d --- /dev/null +++ b/examples/sergio/Ejemplo10/S.xml @@ -0,0 +1,288 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo10/V.xml b/examples/sergio/Ejemplo10/V.xml new file mode 100644 index 00000000..5385d7dd --- /dev/null +++ b/examples/sergio/Ejemplo10/V.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo5.xml b/examples/sergio/Ejemplo5.xml new file mode 100644 index 00000000..409c78cf --- /dev/null +++ b/examples/sergio/Ejemplo5.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/Ejemplo5/Ejemplo5B.xml b/examples/sergio/Ejemplo5/Ejemplo5B.xml new file mode 100644 index 00000000..d53fdc48 --- /dev/null +++ b/examples/sergio/Ejemplo5/Ejemplo5B.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo5/Ejemplo5S.xml b/examples/sergio/Ejemplo5/Ejemplo5S.xml new file mode 100644 index 00000000..8e19718a --- /dev/null +++ b/examples/sergio/Ejemplo5/Ejemplo5S.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo5/Ejemplo5V.xml b/examples/sergio/Ejemplo5/Ejemplo5V.xml new file mode 100644 index 00000000..a4f5d5ec --- /dev/null +++ b/examples/sergio/Ejemplo5/Ejemplo5V.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo8.xml b/examples/sergio/Ejemplo8.xml new file mode 100644 index 00000000..75cde7bb --- /dev/null +++ b/examples/sergio/Ejemplo8.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/Ejemplo8/Ejemplo8B.xml b/examples/sergio/Ejemplo8/Ejemplo8B.xml new file mode 100644 index 00000000..1c84dae7 --- /dev/null +++ b/examples/sergio/Ejemplo8/Ejemplo8B.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo8/Ejemplo8S.xml b/examples/sergio/Ejemplo8/Ejemplo8S.xml new file mode 100644 index 00000000..4f229de2 --- /dev/null +++ b/examples/sergio/Ejemplo8/Ejemplo8S.xml @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Ejemplo8/Ejemplo8V.xml b/examples/sergio/Ejemplo8/Ejemplo8V.xml new file mode 100644 index 00000000..330ab409 --- /dev/null +++ b/examples/sergio/Ejemplo8/Ejemplo8V.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/MidPlan.xml b/examples/sergio/MidPlan.xml new file mode 100644 index 00000000..56d1a4f3 --- /dev/null +++ b/examples/sergio/MidPlan.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/Obstaculo.xml b/examples/sergio/Obstaculo.xml new file mode 100644 index 00000000..460d5501 --- /dev/null +++ b/examples/sergio/Obstaculo.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/Obstaculo/ObstaculoB.xml b/examples/sergio/Obstaculo/ObstaculoB.xml new file mode 100644 index 00000000..e908c5a5 --- /dev/null +++ b/examples/sergio/Obstaculo/ObstaculoB.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Obstaculo/ObstaculoS.xml b/examples/sergio/Obstaculo/ObstaculoS.xml new file mode 100644 index 00000000..8b132095 --- /dev/null +++ b/examples/sergio/Obstaculo/ObstaculoS.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/Obstaculo/ObstaculoV.xml b/examples/sergio/Obstaculo/ObstaculoV.xml new file mode 100644 index 00000000..86094178 --- /dev/null +++ b/examples/sergio/Obstaculo/ObstaculoV.xml @@ -0,0 +1,9 @@ + + + + + + + + // + \ No newline at end of file diff --git a/examples/sergio/circle.xml b/examples/sergio/circle.xml new file mode 100644 index 00000000..e12ef2be --- /dev/null +++ b/examples/sergio/circle.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/circle/circleB.xml b/examples/sergio/circle/circleB.xml new file mode 100644 index 00000000..67f362b9 --- /dev/null +++ b/examples/sergio/circle/circleB.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/circle/circleS.xml b/examples/sergio/circle/circleS.xml new file mode 100644 index 00000000..83e25430 --- /dev/null +++ b/examples/sergio/circle/circleS.xml @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/circle/circleV.xml b/examples/sergio/circle/circleV.xml new file mode 100644 index 00000000..30fcb117 --- /dev/null +++ b/examples/sergio/circle/circleV.xml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/defrente.xml b/examples/sergio/defrente.xml new file mode 100644 index 00000000..79eb5062 --- /dev/null +++ b/examples/sergio/defrente.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/defrente/defrenteB.xml b/examples/sergio/defrente/defrenteB.xml new file mode 100644 index 00000000..51c4e4d1 --- /dev/null +++ b/examples/sergio/defrente/defrenteB.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/defrente/defrenteS.xml b/examples/sergio/defrente/defrenteS.xml new file mode 100644 index 00000000..b9846ac4 --- /dev/null +++ b/examples/sergio/defrente/defrenteS.xml @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/defrente/defrenteV.xml b/examples/sergio/defrente/defrenteV.xml new file mode 100644 index 00000000..86094178 --- /dev/null +++ b/examples/sergio/defrente/defrenteV.xml @@ -0,0 +1,9 @@ + + + + + + + + // + \ No newline at end of file diff --git a/examples/sergio/doorwayTwoway.xml b/examples/sergio/doorwayTwoway.xml new file mode 100644 index 00000000..6156e725 --- /dev/null +++ b/examples/sergio/doorwayTwoway.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/dosgrupos.xml b/examples/sergio/dosgrupos.xml new file mode 100644 index 00000000..003eeb0e --- /dev/null +++ b/examples/sergio/dosgrupos.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/dosgrupos/B.xml b/examples/sergio/dosgrupos/B.xml new file mode 100644 index 00000000..c96ff66d --- /dev/null +++ b/examples/sergio/dosgrupos/B.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/dosgrupos/S.xml b/examples/sergio/dosgrupos/S.xml new file mode 100644 index 00000000..6656b482 --- /dev/null +++ b/examples/sergio/dosgrupos/S.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/dosgrupos/V.xml b/examples/sergio/dosgrupos/V.xml new file mode 100644 index 00000000..b76897af --- /dev/null +++ b/examples/sergio/dosgrupos/V.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/parado.xml b/examples/sergio/parado.xml new file mode 100644 index 00000000..bf3a6615 --- /dev/null +++ b/examples/sergio/parado.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/peqgrupo.xml b/examples/sergio/peqgrupo.xml new file mode 100644 index 00000000..eb4c9270 --- /dev/null +++ b/examples/sergio/peqgrupo.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/peqgrupo/B.xml b/examples/sergio/peqgrupo/B.xml new file mode 100644 index 00000000..9bc3422a --- /dev/null +++ b/examples/sergio/peqgrupo/B.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/peqgrupo/S.xml b/examples/sergio/peqgrupo/S.xml new file mode 100644 index 00000000..5415f35b --- /dev/null +++ b/examples/sergio/peqgrupo/S.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/peqgrupo/V.xml b/examples/sergio/peqgrupo/V.xml new file mode 100644 index 00000000..330ab409 --- /dev/null +++ b/examples/sergio/peqgrupo/V.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/pruebaMid.xml b/examples/sergio/pruebaMid.xml new file mode 100644 index 00000000..179adfb2 --- /dev/null +++ b/examples/sergio/pruebaMid.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/pruebaMid/pruebaMidB.xml b/examples/sergio/pruebaMid/pruebaMidB.xml new file mode 100644 index 00000000..2df720cd --- /dev/null +++ b/examples/sergio/pruebaMid/pruebaMidB.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/pruebaMid/pruebaMidS.xml b/examples/sergio/pruebaMid/pruebaMidS.xml new file mode 100644 index 00000000..97b24c78 --- /dev/null +++ b/examples/sergio/pruebaMid/pruebaMidS.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/pruebaMid/pruebaMidV.xml b/examples/sergio/pruebaMid/pruebaMidV.xml new file mode 100644 index 00000000..09d894b8 --- /dev/null +++ b/examples/sergio/pruebaMid/pruebaMidV.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/puerta.xml b/examples/sergio/puerta.xml new file mode 100644 index 00000000..584c4030 --- /dev/null +++ b/examples/sergio/puerta.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/puerta/puertaB.xml b/examples/sergio/puerta/puertaB.xml new file mode 100644 index 00000000..df1682b8 --- /dev/null +++ b/examples/sergio/puerta/puertaB.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/puerta/puertaS.xml b/examples/sergio/puerta/puertaS.xml new file mode 100644 index 00000000..ac3cc564 --- /dev/null +++ b/examples/sergio/puerta/puertaS.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/puerta/puertaV.xml b/examples/sergio/puerta/puertaV.xml new file mode 100644 index 00000000..ce9b169e --- /dev/null +++ b/examples/sergio/puerta/puertaV.xml @@ -0,0 +1,9 @@ + + + + + + + + // + \ No newline at end of file diff --git a/examples/sergio/seguimiento.xml b/examples/sergio/seguimiento.xml new file mode 100644 index 00000000..32e3c216 --- /dev/null +++ b/examples/sergio/seguimiento.xml @@ -0,0 +1,9 @@ + + + diff --git a/examples/sergio/seguimiento/seguimientoB.xml b/examples/sergio/seguimiento/seguimientoB.xml new file mode 100644 index 00000000..e0ea0a47 --- /dev/null +++ b/examples/sergio/seguimiento/seguimientoB.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/seguimiento/seguimientoS.xml b/examples/sergio/seguimiento/seguimientoS.xml new file mode 100644 index 00000000..c09d5662 --- /dev/null +++ b/examples/sergio/seguimiento/seguimientoS.xml @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/sergio/seguimiento/seguimientoV.xml b/examples/sergio/seguimiento/seguimientoV.xml new file mode 100644 index 00000000..86094178 --- /dev/null +++ b/examples/sergio/seguimiento/seguimientoV.xml @@ -0,0 +1,9 @@ + + + + + + + + // + \ No newline at end of file diff --git a/projects/VS2017/Menge/log.css b/projects/VS2017/Menge/log.css new file mode 100644 index 00000000..ff2c1b51 --- /dev/null +++ b/projects/VS2017/Menge/log.css @@ -0,0 +1,53 @@ +table, th, td { + border-collapse:collapse; + border: 1px solid grey; +} +table { + table-layout : fixed; + width: 950 px; + max-width: 900 px; + background-color : #292929; +} +td { + min-width: 200px; +} +#content { + position : relative; +} +body, html { + background: #000000; + width: 1000px; + font-family: Arial; + font-size: 16px; + color: #C0C0C0; +} +h1 { + font-size : 50px; + line-height : 100px; + color : #FFFFFF; + border-bottom : 1px dotted #888888; +} +#logo { + position: absolute; + top: 0px; + right: 0px; +} +.divider { + background : #DDD; +} +.box { + padding : 0px; +} +.inf { + color: #C0C0C0; + min-width : 1000px; + max-width : 1000px; +} +.err { + color: #EE1100; + font-weight: bold +} +.war { + color: #FFCC00; + font-weight: bold +} \ No newline at end of file diff --git a/projects/VS2017/Menge/log.html b/projects/VS2017/Menge/log.html new file mode 100644 index 00000000..5548f967 --- /dev/null +++ b/projects/VS2017/Menge/log.html @@ -0,0 +1,228 @@ + + + +Menge Log + + + + +
+

Menge Log

+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
initialized logger
+
Plugin path: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug
Loading Menge core-simulation plugins...
Loaded: Dummy Pedestrian Model
    A simple example of a pedestrian model. The dummy model computes a new velocity by randomly perturbing the preferred velocity.
Loaded: GCF 2010 Pedestrian Model
    A pedestran plugin based on the generalized centifugal force model proposed in 2010 by Chraibi et al.
Loaded: Helbing 2000 Pedestrian Model
    A pedestran plugin based on the model proposed in 2000 by Helbing et al.
Loaded: Johansson 2007 Pedestrian Model
    A pedestran plugin based on the model proposed in 2007 by Johansson et al.
Loaded: Karamouzas 2009 Pedestrian Model
    A pedestran plugin based on the model proposed in 2009 by Karamouzas et al.
Loaded: Zanlungo Pedestrian Model
    A pedestran plugin based on the model proposed in 2011 by Zanlungo et al.
Loaded: Aircraft_utils
    Utilties for simulating aircraft loading and unloading including the following:
    Action "set_xproperty" - which sets properties according to the agent's position along the x-axis
    Condition "clear_AABB" - Transition into a state according to whether or not an AABB is clear
Loaded: formations
    Utilities for adding formations to ped simsincluding the following:
    Modifier "formation" - forces agents to move in formation.
Loaded: fundamental_diagram
    Velocity Modifier plugin for adherence to the Fundamental Diagramincluding the following:
    Modifier "fundamantal_diagram" - adjusts the agent's preferred velocity to match the FD.
Loaded: StressGAS_utils
    Utilties for modelling stress according to the GAS model including the following:
    Action "timed_stress" - registers an agent to accumulate stress up to 100% stressed in a specified number of seconds.
Loaded: Terrain_utils
    Simulation elements based on a height field including:
    Elevation element
    HeightField resource
    HeightField velocity modifier
Parsing project XML: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio\puerta.xml
+
Project root: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio
+
Scene root: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio\puerta.
Missing the "dist" attribute on line 34
Agent generator on line 34 has no valid noise definition. No noise applied.
Missing the "dist" attribute on line 42
Agent generator on line 42 has no valid noise definition. No noise applied.
+
Loading behavior from xml: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio\puerta\puertaB.xml
Behavior root: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio\puerta
Expecting a missing size_t attribute (capacity) on line 5. Using default value: 18446744073709551615.
Expecting a missing float attribute (weight) on line 5. Using default value: 1.
Expecting a missing size_t attribute (capacity) on line 6. Using default value: 18446744073709551615.
Expecting a missing float attribute (weight) on line 6. Using default value: 1.
Expecting a missing size_t attribute (capacity) on line 7. Using default value: 18446744073709551615.
Expecting a missing float attribute (weight) on line 7. Using default value: 1.
Expecting a missing boolean attribute (persistent) on line 11. Using default value: 0.
Expecting a missing boolean attribute (persistent) on line 15. Using default value: 0.
Expecting a missing boolean attribute (persistent) on line 19. Using default value: 0.
Expecting a missing boolean attribute (persistent) on line 23. Using default value: 0.
Expecting a missing boolean attribute (persistent) on line 27. Using default value: 0.
There are 0 registered tasks.
No elevation implementation specified. Using "flat" implementation.
For logical time step: 0.1 and 0 sub steps, effective time step is: 0.1
+
Initializing visualization...
Loading Menge visualization plugins...
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\AgtDummy_d.dll as a plug-in
Loaded: GCF 2010 Pedestrian Model
    A pedestran plugin based on the generalized centifugal force model proposed in 2010 by Chraibi et al.
Loaded: Helbing 2000 Pedestrian Model
    A pedestran plugin based on the model proposed in 2000 by Helbing et al.
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\AgtJohansson_d.dll as a plug-in
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\AgtKaramouzas_d.dll as a plug-in
Loaded: Zanlungo Pedestrian Model
    A pedestran plugin based on the model proposed in 2011 by Zanlungo et al.
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\Aircraft_d.dll as a plug-in
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\Formations_d.dll as a plug-in
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\FundDiag_d.dll as a plug-in
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\StressGas_d.dll as a plug-in
Could not find exported function
Failed loading G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug\Terrain_d.dll as a plug-in
+
View root: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio\puerta
Watermark specification on line 8 references a file that doesn't exist: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\mengeLogo.png. No watermark will be created.
Dumping png to: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\examples\sergio\puerta\imagenes
Unable to find a 'vis_agent' vis element for the selected element: 'johannson'. Using default visualization element.
+
Simulation time: 39
+
+
+ + diff --git a/projects/VS2017/Plugins/AgtOSM.vcxproj b/projects/VS2017/Plugins/AgtOSM.vcxproj new file mode 100644 index 00000000..115edb10 --- /dev/null +++ b/projects/VS2017/Plugins/AgtOSM.vcxproj @@ -0,0 +1,166 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + + + + + + + + + + 16.0 + Win32Proj + {bb346c6f-6ea1-4afe-b92b-07f1586c3632} + AgtOSM + 10.0 + + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + DynamicLibrary + false + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + + + $(BuildRoot)\lib\x64\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + $(ProjectName)_d + .dll + true + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + + + WIN32;_DEBUG;_WINDOWS;_USRDLL;OSM_EXPORT;%(PreprocessorDefinitions) + Default + MultiThreadedDebugDLL + EnableFastChecks + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + + + Windows + true + $(OutDir)$(ProjectName)_d.dll + $(SolutionDir)..\..\resources\win\lib\x64;$(MengeLib)\x64;%(AdditionalLibraryDirectories) + MengeCore_d.lib;tinyxml_d.lib;%(AdditionalDependencies) + + + + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + + \ No newline at end of file diff --git a/projects/VS2017/Plugins/AgtOSM.vcxproj.filters b/projects/VS2017/Plugins/AgtOSM.vcxproj.filters new file mode 100644 index 00000000..e0645307 --- /dev/null +++ b/projects/VS2017/Plugins/AgtOSM.vcxproj.filters @@ -0,0 +1,54 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + \ No newline at end of file diff --git a/projects/VS2017/Plugins/BanPrueba.vcxproj b/projects/VS2017/Plugins/BanPrueba.vcxproj new file mode 100644 index 00000000..ce795ce1 --- /dev/null +++ b/projects/VS2017/Plugins/BanPrueba.vcxproj @@ -0,0 +1,215 @@ + + + + + Debug + Win32 + + + Debug + x64 + + + Release + Win32 + + + Release + x64 + + + + {4DF24D01-FDAF-4E24-AD0A-4C1030510695} + MidPlan + Win32Proj + 10.0 + + + + DynamicLibrary + v143 + Unicode + true + + + DynamicLibrary + v143 + Unicode + true + + + DynamicLibrary + v143 + Unicode + + + DynamicLibrary + v143 + Unicode + + + + + + + + + + + + + + + + + + + + + + + + + + + <_ProjectFileVersion>11.0.50727.1 + + + $(BuildRoot)\lib\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + true + $(ProjectName)_d + + + true + $(BuildRoot)\lib\x64\ + $(ProjectName)_d + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + + + $(BuildRoot)\lib\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + false + + + false + $(BuildRoot)\lib\x64\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + + + + Disabled + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;_DEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + true + EnableFastChecks + MultiThreadedDebugDLL + + Level3 + EditAndContinue + + + MengeCore_d.lib;tinyxml_d.lib;%(AdditionalDependencies) + $(OutDir)$(ProjectName)_d.dll + $(SolutionDir)..\..\resources\win\lib\x86;$(MengeLib);%(AdditionalLibraryDirectories) + true + Windows + MachineX86 + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins\debug md $(ExeDir)\plugins\debug +xcopy /y $(TargetPath) $(ExeDir)\plugins\debug\ + + + + + + Disabled + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;_DEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + EnableFastChecks + MultiThreadedDebugDLL + + + Level3 + ProgramDatabase + + + MengeCore_d.lib;tinyxml_d.lib;%(AdditionalDependencies) + $(OutDir)$(ProjectName)_d.dll + $(SolutionDir)..\..\resources\win\lib\x64;$(MengeLib)\x64;%(AdditionalLibraryDirectories) + true + Windows + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins\debug md $(ExeDir)\plugins\debug +xcopy /y $(TargetPath) $(ExeDir)\plugins\debug\ + + + + + + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + MultiThreadedDLL + true + + Level3 + ProgramDatabase + + + MengeCore.lib;tinyxml.lib;%(AdditionalDependencies) + $(SolutionDir)..\..\resources\win\lib\x86;$(MengeLib);%(AdditionalLibraryDirectories) + true + Windows + true + true + MachineX86 + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins md $(ExeDir)\plugins +xcopy /y $(TargetPath) $(ExeDir)\plugins\ + + + + + + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + MultiThreadedDLL + true + + + Level3 + ProgramDatabase + + + MengeCore.lib;tinyxml.lib;%(AdditionalDependencies) + $(SolutionDir)..\..\resources\win\lib\x64;$(MengeLib)\x64;%(AdditionalLibraryDirectories) + true + Windows + true + true + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins md $(ExeDir)\plugins +xcopy /y $(TargetPath) $(ExeDir)\plugins\ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/projects/VS2017/Plugins/BanPrueba.vcxproj.filters b/projects/VS2017/Plugins/BanPrueba.vcxproj.filters new file mode 100644 index 00000000..df129de7 --- /dev/null +++ b/projects/VS2017/Plugins/BanPrueba.vcxproj.filters @@ -0,0 +1,27 @@ + + + + + Source Files + + + Source Files + + + + + Header Files + + + Header Files + + + + + {bb19f173-9c28-4d92-a819-ae7346dd4460} + + + {79aa227a-e347-4fb7-bbe4-91767f45964d} + + + \ No newline at end of file diff --git a/projects/VS2017/Plugins/MidPlan.vcxproj b/projects/VS2017/Plugins/MidPlan.vcxproj new file mode 100644 index 00000000..5661cf08 --- /dev/null +++ b/projects/VS2017/Plugins/MidPlan.vcxproj @@ -0,0 +1,216 @@ + + + + + Debug + Win32 + + + Debug + x64 + + + Release + Win32 + + + Release + x64 + + + + {CE64D6DC-51EE-4D0A-B4D1-BAFFC38A9D67} + MidPlan + Win32Proj + 10.0 + + + + DynamicLibrary + v143 + Unicode + true + + + DynamicLibrary + v143 + Unicode + true + + + DynamicLibrary + v143 + Unicode + + + DynamicLibrary + v143 + Unicode + + + + + + + + + + + + + + + + + + + + + + + + + + + <_ProjectFileVersion>11.0.50727.1 + + + $(BuildRoot)\lib\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + true + $(ProjectName)_d + + + true + $(BuildRoot)\lib\x64\ + $(ProjectName)_d + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + + + $(BuildRoot)\lib\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + false + + + false + $(BuildRoot)\lib\x64\ + $(BuildRoot)\$(Platform)\$(Configuration)\$(ProjectName)\ + + + + Disabled + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;_DEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + true + EnableFastChecks + MultiThreadedDebugDLL + + Level3 + EditAndContinue + + + MengeCore_d.lib;tinyxml_d.lib;%(AdditionalDependencies) + $(OutDir)$(ProjectName)_d.dll + $(SolutionDir)..\..\resources\win\lib\x86;$(MengeLib);%(AdditionalLibraryDirectories) + true + Windows + MachineX86 + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins\debug md $(ExeDir)\plugins\debug +xcopy /y $(TargetPath) $(ExeDir)\plugins\debug\ + + + + + + Disabled + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;_DEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + EnableFastChecks + MultiThreadedDebugDLL + + + Level3 + ProgramDatabase + + + MengeCore_d.lib;tinyxml_d.lib;%(AdditionalDependencies) + $(OutDir)$(ProjectName)_d.dll + $(SolutionDir)..\..\resources\win\lib\x64;$(MengeLib)\x64;%(AdditionalLibraryDirectories) + true + Windows + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins\debug md $(ExeDir)\plugins\debug +xcopy /y $(TargetPath) $(ExeDir)\plugins\debug\ + + + + + + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + MultiThreadedDLL + true + + Level3 + ProgramDatabase + + + MengeCore.lib;tinyxml.lib;%(AdditionalDependencies) + $(SolutionDir)..\..\resources\win\lib\x86;$(MengeLib);%(AdditionalLibraryDirectories) + true + Windows + true + true + MachineX86 + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins md $(ExeDir)\plugins +xcopy /y $(TargetPath) $(ExeDir)\plugins\ + + + + + + $(MengeSrc)\;$(MengeSrc)\..\;$(MengeSrc)\..\include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;_WINDOWS;_USRDLL;MPMODIFIER_EXPORT;%(PreprocessorDefinitions) + MultiThreadedDLL + true + + + Level3 + ProgramDatabase + + + MengeCore.lib;tinyxml.lib;%(AdditionalDependencies) + $(SolutionDir)..\..\resources\win\lib\x64;$(MengeLib)\x64;%(AdditionalLibraryDirectories) + true + Windows + true + true + + + Copy DLL to exectuable folder + if not exist $(ExeDir)\plugins md $(ExeDir)\plugins +xcopy /y $(TargetPath) $(ExeDir)\plugins\ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/projects/VS2017/Plugins/MidPlan.vcxproj.filters b/projects/VS2017/Plugins/MidPlan.vcxproj.filters new file mode 100644 index 00000000..ce3a7dde --- /dev/null +++ b/projects/VS2017/Plugins/MidPlan.vcxproj.filters @@ -0,0 +1,30 @@ + + + + + {471fb84c-d15a-46ec-adbe-597304640e72} + + + {f59ac094-0396-403c-8997-1dc236623883} + + + + + Source Files + + + Source Files + + + + + Header Files + + + Header Files + + + Header Files + + + \ No newline at end of file diff --git a/projects/VS2017/Plugins/log.css b/projects/VS2017/Plugins/log.css new file mode 100644 index 00000000..ff2c1b51 --- /dev/null +++ b/projects/VS2017/Plugins/log.css @@ -0,0 +1,53 @@ +table, th, td { + border-collapse:collapse; + border: 1px solid grey; +} +table { + table-layout : fixed; + width: 950 px; + max-width: 900 px; + background-color : #292929; +} +td { + min-width: 200px; +} +#content { + position : relative; +} +body, html { + background: #000000; + width: 1000px; + font-family: Arial; + font-size: 16px; + color: #C0C0C0; +} +h1 { + font-size : 50px; + line-height : 100px; + color : #FFFFFF; + border-bottom : 1px dotted #888888; +} +#logo { + position: absolute; + top: 0px; + right: 0px; +} +.divider { + background : #DDD; +} +.box { + padding : 0px; +} +.inf { + color: #C0C0C0; + min-width : 1000px; + max-width : 1000px; +} +.err { + color: #EE1100; + font-weight: bold +} +.war { + color: #FFCC00; + font-weight: bold +} \ No newline at end of file diff --git a/projects/VS2017/Plugins/log.html b/projects/VS2017/Plugins/log.html new file mode 100644 index 00000000..c201644b --- /dev/null +++ b/projects/VS2017/Plugins/log.html @@ -0,0 +1,69 @@ + + + +Menge Log + + + + +
+

Menge Log

+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
initialized logger
+
Plugin path: G:\UNED\Proyecto\Componentes\Menge-master\Menge-master\Exe\plugins\debug
Loading Menge core-simulation plugins...
Loaded: Dummy Pedestrian Model
    A simple example of a pedestrian model. The dummy model computes a new velocity by randomly perturbing the preferred velocity.
Loaded: GCF 2010 Pedestrian Model
    A pedestran plugin based on the generalized centifugal force model proposed in 2010 by Chraibi et al.
Loaded: Helbing 2000 Pedestrian Model
    A pedestran plugin based on the model proposed in 2000 by Helbing et al.
Loaded: Johansson 2007 Pedestrian Model
    A pedestran plugin based on the model proposed in 2007 by Johansson et al.
Loaded: Karamouzas 2009 Pedestrian Model
    A pedestran plugin based on the model proposed in 2009 by Karamouzas et al.
Loaded: Zanlungo Pedestrian Model
    A pedestran plugin based on the model proposed in 2011 by Zanlungo et al.
Loaded: Aircraft_utils
    Utilties for simulating aircraft loading and unloading including the following:
    Action "set_xproperty" - which sets properties according to the agent's position along the x-axis
    Condition "clear_AABB" - Transition into a state according to whether or not an AABB is clear
Loaded: formations
    Utilities for adding formations to ped simsincluding the following:
    Modifier "formation" - forces agents to move in formation.
Loaded: fundamental_diagram
    Velocity Modifier plugin for adherence to the Fundamental Diagramincluding the following:
    Modifier "fundamantal_diagram" - adjusts the agent's preferred velocity to match the FD.
Loaded: midterm_planning
    Velocity Modifier plugin for adherence to the Fundamental Diagramincluding the following:
    Modifier "fundamantal_diagram" - adjusts the agent's preferred velocity to match the MP.
Loaded: StressGAS_utils
    Utilties for modelling stress according to the GAS model including the following:
    Action "timed_stress" - registers an agent to accumulate stress up to 100% stressed in a specified number of seconds.
Loaded: Terrain_utils
    Simulation elements based on a height field including:
    Elevation element
    HeightField resource
    HeightField velocity modifier
Parsing project XML: ../examples/sergio/MidPlan.xml
+
+
+ + diff --git a/src/Plugins/AgtOSM/OSM.cpp b/src/Plugins/AgtOSM/OSM.cpp new file mode 100644 index 00000000..b7b0a19e --- /dev/null +++ b/src/Plugins/AgtOSM/OSM.cpp @@ -0,0 +1,45 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +/*! + * @file OSM.cpp + * @brief Plugin for orca pedestrian. + */ + +#include "OSMConfig.h" +#include "OSMDBEntry.h" +#include "MengeCore/PluginEngine/CorePluginEngine.h" + +using Menge::PluginEngine::CorePluginEngine; + +extern "C" { +/*! + * @brief Retrieves the name of the plug-in. + * + * @returns The name of the plug in. + */ +OSM_API const char* getName() { return "Optimal Step Model 2012"; } + +/*! + * @brief Description of the plug-in. + * + * @returns A description of the plugin. + */ +OSM_API const char* getDescription() { + return "A pedestrian plugin based on the model proposed in 2012 by M. J. Seitz and G. Köster"; +} + +/*! + * @brief Registers the plug-in with the PluginEngine + * + * @param engine A pointer to the plugin engine. + */ +OSM_API void registerCorePlugin(CorePluginEngine* engine) { + engine->registerModelDBEntry(new OSM::DBEntry()); +} +} diff --git a/src/Plugins/AgtOSM/OSM.h b/src/Plugins/AgtOSM/OSM.h new file mode 100644 index 00000000..509f0caa --- /dev/null +++ b/src/Plugins/AgtOSM/OSM.h @@ -0,0 +1,20 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +/*! + @file OSM.h + @brief Collection of Optimal Step Model 2012 Agent and Simulator. + */ + +#ifndef __OSM_H__ +#define __OSM_H__ + +#include "OSMAgent.h" +#include "OSMSimulator.h" + +#endif // __OSM_H__ diff --git a/src/Plugins/AgtOSM/OSMAgent.cpp b/src/Plugins/AgtOSM/OSMAgent.cpp new file mode 100644 index 00000000..235b6827 --- /dev/null +++ b/src/Plugins/AgtOSM/OSMAgent.cpp @@ -0,0 +1,170 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +#include "OSMAgent.h" +#include "OSMSimulator.h" +#include "MengeCore/Math/geomQuery.h" + +#include +#include + +namespace OSM { + +using Menge::Agents::BaseAgent; +using Menge::Agents::Obstacle; +using Menge::Math::Vector2; + +//////////////////////////////////////////////////////////////// +// Implementation of Karamouzas::Agent +//////////////////////////////////////////////////////////////// + +const std::string Agent::NAME = "osm"; + +//////////////////////////////////////////////////////////////// + +Agent::Agent() : BaseAgent() { + _perSpace = 1.f; + _anticipation = 3.f; +} + +//////////////////////////////////////////////////////////////// + +Agent::~Agent() {} + +//////////////////////////////////////////////////////////////// + +void Agent::computeNewVelocity() { + const float EPSILON = 0.01f; // this eps from Ioannis + const float FOV = Simulator::COS_FOV_ANGLE; + + Vector2 force((_velPref.getPreferredVel() - _vel) / Simulator::REACTION_TIME); + const float SAFE_DIST = Simulator::WALL_DISTANCE + _radius; + const float SAFE_DIST2 = SAFE_DIST * SAFE_DIST; + for (size_t o = 0; o < _nearObstacles.size(); ++o) { + // TODO: Interaction with obstacles is, currently, defined strictly + // by COLLISIONS. Only if I'm going to collide with an obstacle is + // a force applied. This may be too naive. + // I'll have to investigate this. + const Obstacle* obst = _nearObstacles[o].obstacle; + Vector2 nearPt; // set by distanceSqToPoint + float sqDist; // set by distanceSqToPoint + if (obst->distanceSqToPoint(_pos, nearPt, sqDist) == Obstacle::LAST) continue; + if (SAFE_DIST2 > sqDist) { + // A repulsive force is actually possible + float dist = sqrtf(sqDist); + float num = SAFE_DIST - dist; + float distMradius = (dist - _radius) < EPSILON ? EPSILON : dist - _radius; + float denom = powf(distMradius, Simulator::WALL_STEEPNESS); + Vector2 dir = norm(_pos - nearPt); + float mag = num / denom; + force += dir * mag; + } + } + + Vector2 desVel = _vel + force * Simulator::TIME_STEP; + float desSpeed = abs(desVel); + force.set(0.f, 0.f); + //#if 0 + // // iteratively evaluate neighbors + //#else + // Weight all neighbors + bool colliding = false; + int collidingCount = Simulator::COLLIDING_COUNT; + bool VERBOSE = false; // _id == 1; + if (VERBOSE) std::cout << "Agent " << _id << "\n"; + float totalTime = 1.f; + std::list > collidingSet; + for (size_t j = 0; j < _nearAgents.size(); ++j) { + const BaseAgent* otherBase = _nearAgents[j].agent; + const Agent* const other = static_cast(otherBase); + float circRadius = _perSpace + other->_radius; + Vector2 relVel = desVel - other->_vel; + Vector2 relPos = other->_pos - _pos; + + if (absSq(relPos) < circRadius * circRadius) { /// collision! + if (!colliding) { + colliding = true; + collidingSet.clear(); + } + collidingSet.push_back(std::pair(.0f, other)); + if (static_cast(collidingSet.size()) > collidingCount) ++collidingCount; + continue; + } + + // TODO: evalute field of view + // If relPos is not within the field of view around preferred direction, continue + Vector2 relDir = norm(relPos); + if ((relDir * _orient) < FOV) continue; + float tc = rayCircleTTC(relVel, relPos, circRadius); + if (tc < _anticipation && !colliding) { + if (VERBOSE) std::cout << "\tAgent " << other->_id << " t_c: " << tc << "\n"; + // totalTime += tc; + // insert into colliding set (in order) + std::list >::iterator itr = collidingSet.begin(); + while (itr != collidingSet.end() && tc > itr->first) ++itr; + collidingSet.insert(itr, std::pair(tc, other)); + } + } + // if ( collidingSet.size() > 0 ) { + int count = 0; + std::list >::iterator itr = collidingSet.begin(); + for (int i = 0; i < collidingCount; ++i, ++itr) { + if (itr == collidingSet.end()) break; + const Agent* const other = itr->second; + float tc = itr->first; + // future positions + Vector2 myPos = _pos + desVel * tc; + Vector2 hisPos = other->_pos + other->_vel * tc; + Vector2 forceDir = myPos - hisPos; + // float futureDist = abs( forceDir ); + // forceDir /= futureDist; + // float D = desSpeed * tc + futureDist - _radius - other->_radius; + float fDist = abs(forceDir); + forceDir /= fDist; + float collisionDist = fDist - _radius - other->_radius; + float D = std::max(desSpeed * tc + (collisionDist < 0 ? 0 : collisionDist), EPSILON); + + // determine magnitude + + float mag; + if (D < Simulator::D_MIN) { + mag = Simulator::AGENT_FORCE * Simulator::D_MIN / D; + } else if (D < Simulator::D_MID) { + mag = Simulator::AGENT_FORCE; + } else if (D < Simulator::D_MAX) { + // D -= Simulator::D_MID; + // mag = D * Simulator::AGENT_FORCE / ( Simulator::D_MAX - Simulator::D_MID ) + + // Simulator::D_MID ; + mag = Simulator::AGENT_FORCE * (Simulator::D_MAX - D) / (Simulator::D_MAX - Simulator::D_MID); + } else { + continue; // magnitude is zero + } + float weight = pow(colliding ? 1.f : 0.8f, count++); + // float weight = ( totalTime - tc ) / totalTime; //1.f / ( tc * totalTime ); + if (VERBOSE) { + std::cout << "\tAgent " << other->_id; + std::cout << " magnitude: " << mag; + std::cout << " weight: " << weight; + std::cout << " total force: " << (mag * weight); + std::cout << " D: " << D << "\n"; + } + force += forceDir * (mag * weight); + } + // Add some noise to avoid deadlocks and introduce variation + // float angle = rand() * 2.0f * M_PI / RAND_MAX; + float angle = rand() * 2.0f * 3.1415f / RAND_MAX; + float dist = rand() * 0.001f / RAND_MAX; + force += dist * Vector2(cos(angle), sin(angle)); + // do we need a drag force? + + // Cap the force to maxAccel + if (abs(force) > _maxAccel) force = norm(force) * _maxAccel; + + _velNew = desVel + force * Simulator::TIME_STEP; // assumes unit mass +} +} // namespace Karamouzas diff --git a/src/Plugins/AgtOSM/OSMAgent.h b/src/Plugins/AgtOSM/OSMAgent.h new file mode 100644 index 00000000..a8467283 --- /dev/null +++ b/src/Plugins/AgtOSM/OSMAgent.h @@ -0,0 +1,64 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +/*! + @file OSMAgent.h + @brief Contains the OSM::Agent class. + + Implements OSM "Natural discretization of pedestrian movement in continuous space" + */ + +#ifndef __OSM_AGENT_H__ +#define __OSM_AGENT_H__ + +#include "MengeCore/Agents/BaseAgent.h" + +namespace OSM { +/*! + @brief The agent definition for the Karamouzas 2009 agent. + */ +class Agent : public Menge::Agents::BaseAgent { + public: + /*! + @brief A variant of the copy constructor. + */ + Agent(); + + /*! + @brief Destroys this agent instance. + */ + ~Agent(); + + /*! + @brief Computes the new velocity of this agent. + */ + void computeNewVelocity(); + + /*! + @brief Used by the plugin system to know what artifacts to associate with agents of this type. + + Every sub-class of must return a globally unique value if it should be associated with unique + artifacts. + */ + virtual std::string getStringId() const { return NAME; } + + /*! @brief The name identifier for this agent type. */ + static const std::string NAME; + + /*! + @brief The personal space (in meters) of the agent + */ + float _perSpace; + + /*! + @brief The anticipation time (in seconds) of the agent + */ + float _anticipation; +}; +} // namespace OSM +#endif // __OSM_AGENT_H__ diff --git a/src/Plugins/AgtOSM/OSMConfig.h b/src/Plugins/AgtOSM/OSMConfig.h new file mode 100644 index 00000000..08c4121e --- /dev/null +++ b/src/Plugins/AgtOSM/OSMConfig.h @@ -0,0 +1,55 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +/*! + @file OSMConfig.h + @brief Configures the shared library aspect of the includes. + */ + +#ifndef __OSM_CONFIG_H__ +#define __OSM_CONFIG_H__ + +// Now set up compiler-dependent export/import symbols + +#if defined(_MSC_VER) + +#if defined(OSM_STATICLIB) +#define OSM_API +#else +#if defined(OSM_EXPORT) +// We are building the DLL, export the symbols tagged like this +#define OSM_API __declspec(dllexport) +#else +// If we are consuming the DLL, import the symbols tagged like this +#define OSM_API __declspec(dllimport) +#endif +#endif + +#elif defined(__GNUC__) + +#if defined(OSM_STATICLIB) +#define OSM_API +#else +#if defined(OSM_EXPORT) +#define OSM_API __attribute__((visibility("default"))) +#else +// If you use -fvisibility=hidden in GCC, exception handling and RTTI +// would break if visibility wasn't set during export _and_ import +// because GCC would immediately forget all type infos encountered. +// See http://gcc.gnu.org/wiki/Visibility +#define OSM_API __attribute__((visibility("default"))) +#endif +#endif + +#else + +#error Unknown compiler, please implement shared library macros + +#endif // Compiler-detection + +#endif // __OSM_CONFIG_H__ \ No newline at end of file diff --git a/src/Plugins/AgtOSM/OSMDBEntry.cpp b/src/Plugins/AgtOSM/OSMDBEntry.cpp new file mode 100644 index 00000000..7da28a7b --- /dev/null +++ b/src/Plugins/AgtOSM/OSMDBEntry.cpp @@ -0,0 +1,47 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +#include "OSMDBEntry.h" +#include "OSMInitializer.h" +#include "MengeCore/Runtime/SimulatorDB.h" + +namespace OSM { + +using Menge::Agents::SimulatorInterface; + +///////////////////////////////////////////////////////////////////////////// +// Implementation of KaramouzasDBEntry +///////////////////////////////////////////////////////////////////////////// + +::std::string DBEntry::briefDescription() const { + return "Simulator based on the model proposed in 2012 by M. J. Seitz and G. Köster"; +} +///////////////////////////////////////////////////////////////////////////// + +::std::string DBEntry::longDescription() const { + return "Simulator based on Seitz's 2012 pedestrian model\n" + "\tThe model is based on M. J. Seitz's 2012 paper.\n"; +} + +///////////////////////////////////////////////////////////////////////////// + +::std::string DBEntry::viewerName() const { return "Karamouzas 2009"; } + +///////////////////////////////////////////////////////////////////////////// + +SimulatorInterface* DBEntry::getNewSimulator() { return new Simulator(); } + +///////////////////////////////////////////////////////////////////////////// + +Menge::Agents::AgentInitializer* DBEntry::getAgentInitalizer() const { + return new AgentInitializer(); +} + +///////////////////////////////////////////////////////////////////////////// + +} // namespace OSM diff --git a/src/Plugins/AgtOSM/OSMDBEntry.h b/src/Plugins/AgtOSM/OSMDBEntry.h new file mode 100644 index 00000000..1dc56bb8 --- /dev/null +++ b/src/Plugins/AgtOSM/OSMDBEntry.h @@ -0,0 +1,76 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +/*! + @file OSMDBEntry.h + @brief The simulator database entry for the OSM. + */ + +#ifndef __OSM_DB_ENTRY_H__ +#define __OSM_DB_ENTRY_H__ + +#include "OSM.h" +#include "MengeCore/Runtime/SimulatorDBEntry.h" + +namespace OSM { +/*! + @brief The simulator database entry for the Karamouzas 2009 simulator. + */ +class DBEntry : public Menge::SimulatorDBEntry { + public: + /*! + @brief Gives a brief description of the simulator. + + @returns A brief description of the simulator and pedestrian model. + */ + virtual ::std::string briefDescription() const; + + /*! + @brief Gives a long description of the simulator. + + @returns A long description of the simulator and pedestrian model. + */ + virtual ::std::string longDescription() const; + + /*! + @brief Gives a label to apply to the interactive viewer. + + @returns The name for display on the interactive viewer. + */ + virtual ::std::string viewerName() const; + + /*! + @brief Gives a unique name to be used as a command-line parameter. + + This name MUST satisfy two constraints: + - It must contain no spaces. + - It must be unique from that used by all other simulators. + + @returns A single string (with no spaces) that can be used as a command line parameter to + uniquely identify this model. + */ + virtual ::std::string commandLineName() const { return Agent::NAME; } + + /*! + @brief Returns a pointer to this model's Simulator instance. + + This must be overridden by a derived class + + @returns A newly instantiated simulator instance of a type appropriate for this database entry. + */ + virtual Menge::Agents::SimulatorInterface* getNewSimulator(); + + /*! + @brief Provides an AgentInitializer appropriate to this simulator class. + + @returns A pointer to an agent initializer. The caller is responsible for freeing up the memory. + */ + virtual Menge::Agents::AgentInitializer* getAgentInitalizer() const; +}; +} // namespace OSM +#endif // __OSM_DB_ENTRY_H__ diff --git a/src/Plugins/AgtOSM/OSMInitializer.cpp b/src/Plugins/AgtOSM/OSMInitializer.cpp new file mode 100644 index 00000000..9f9d3528 --- /dev/null +++ b/src/Plugins/AgtOSM/OSMInitializer.cpp @@ -0,0 +1,124 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +#include "OSMInitializer.h" +#include "OSMAgent.h" +#include "MengeCore/Math/RandGenerator.h" +#include "MengeCore/Runtime/Logger.h" + +namespace OSM { + +using Menge::Logger; +using Menge::logger; +using Menge::Agents::BaseAgent; +using Menge::Math::ConstFloatGenerator; + +//////////////////////////////////////////////////////////////// +// Implementation of Karamouzas::AgentInitializer +//////////////////////////////////////////////////////////////// + +// Default values +const float PER_SPACE = 1.f; ///< The default personal space for the agent. +const float ANTICIPATION = 3.f; ///< The anticipation time of the agent. + +//////////////////////////////////////////////////////////////// + +AgentInitializer::AgentInitializer() : Menge::Agents::AgentInitializer() { + _perSpace = new ConstFloatGenerator(PER_SPACE); + _anticipation = new ConstFloatGenerator(ANTICIPATION); +} + +//////////////////////////////////////////////////////////////// + +AgentInitializer::AgentInitializer(const AgentInitializer& init) + : Menge::Agents::AgentInitializer(init) { + _perSpace = init._perSpace->copy(); + _anticipation = init._anticipation->copy(); +} + +//////////////////////////////////////////////////////////////// + +AgentInitializer::~AgentInitializer() { + delete _perSpace; + delete _anticipation; +} + +//////////////////////////////////////////////////////////////// + +bool AgentInitializer::setProperties(BaseAgent* agent) { + Agent* a = dynamic_cast(agent); + if (a == 0x0) return false; + a->_perSpace = _perSpace->getValue(); + a->_anticipation = _anticipation->getValue(); + + return Menge::Agents::AgentInitializer::setProperties(agent); +} + +//////////////////////////////////////////////////////////////// + +bool AgentInitializer::isRelevant(const ::std::string& tagName) { + return (tagName == "OSM") || Menge::Agents::AgentInitializer::isRelevant(tagName); +} + +//////////////////////////////////////////////////////////////// + +Menge::Agents::AgentInitializer::ParseResult AgentInitializer::setFromXMLAttribute( + const ::std::string& paramName, const ::std::string& value) { + ParseResult result = IGNORED; + if (paramName == "personal_space") { + result = constFloatGenerator(_perSpace, value); + } else if (paramName == "anticipation") { + result = constFloatGenerator(_anticipation, value); + } + + if (result == FAILURE) { + logger << Logger::WARN_MSG << "Attribute \"" << paramName; + logger << "\" had an incorrectly formed value: \"" << value; + logger << "\". Using default value."; + result = ACCEPTED; + } else if (result == IGNORED) { + return Menge::Agents::AgentInitializer::setFromXMLAttribute(paramName, value); + } + return result; +} + +//////////////////////////////////////////////////////////////// + +AgentInitializer::ParseResult AgentInitializer::processProperty(::std::string propName, + TiXmlElement* node) { + ParseResult result = IGNORED; + if (propName == "personal_space") { + result = getFloatGenerator(_perSpace, node); + } else if (propName == "anticipation") { + result = getFloatGenerator(_anticipation, node); + } + + if (result == FAILURE) { + logger << Logger::ERR_MSG << "Error extracting value distribution from Property "; + logger << propName << "."; + return result; + } else if (result == IGNORED) { + return Menge::Agents::AgentInitializer::processProperty(propName, node); + } + return result; +} + +//////////////////////////////////////////////////////////////// + +void AgentInitializer::setDefaults() { + if (_perSpace) delete _perSpace; + _perSpace = new ConstFloatGenerator(PER_SPACE); + if (_anticipation) delete _anticipation; + _anticipation = new ConstFloatGenerator(ANTICIPATION); + + Menge::Agents::AgentInitializer::setDefaults(); +} + +//////////////////////////////////////////////////////////////// + +} // namespace Karamouzas diff --git a/src/Plugins/AgtOSM/OSMInitializer.h b/src/Plugins/AgtOSM/OSMInitializer.h new file mode 100644 index 00000000..55dac2a9 --- /dev/null +++ b/src/Plugins/AgtOSM/OSMInitializer.h @@ -0,0 +1,132 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +/*! + @file OSMInitializer.h + @brief The AgentInitializer for the OSM simulator. + */ +#ifndef __OSM_INITIALIZER_H__ +#define __OSM_INITIALIZER_H__ + +#include "MengeCore/Agents/AgentInitializer.h" + +namespace OSM { +/*! + @brief Class which determines the agent properties for each new Karamouzas agent. + */ +class AgentInitializer : public Menge::Agents::AgentInitializer { + public: + /*! + @brief Constructor. + + The values for each agent take a hard-coded default values. + */ + AgentInitializer(); + + /*! + @brief Copy Constructor. + + @param init The initializer to copy values from + */ + AgentInitializer(const AgentInitializer& init); + + /*! + @brief Destructor. + */ + virtual ~AgentInitializer(); + + /*! + @brief Sets the properties of the given agent based on the initializer's values. + + This needs to be overridden by sub-classes. The first thing the sub-class should do is + dynamic_cast the argument to its expected type to make sure it is the proper agent type. If not, + this should be considered failure. Then it should set its unique properties an+d then call the + super class's setProperties function. + + @param agent The agent whose properties are to be set. + @returns True if the properties were set successfully. + */ + virtual bool setProperties(Menge::Agents::BaseAgent* agent); + + /*! + @brief Sets all generators to default values. + + Resets all number generators to default const values. This assumes that all required number + generators already exist and will delete them appropriately. *Do not* call this in the + constructor. + */ + virtual void setDefaults(); + + /*! + @brief Creates a copy of this AgentInitializer instance. + + @returns A pointer to a new AgentInitializer with all of the same values as this. The caller is + responsible for freeing up the new instance. + */ + virtual AgentInitializer* copy() const { return new AgentInitializer(*this); } + + protected: + /*! + @brief Reports if this AgentInitializer cares about the given AgentSet property XML tag. + * + This is the mechanism by which new sub-classes can extend the parameter space. Each pedestrian + model which introduces new per-agent properties that must override this function. However, the + overriden function must, in turn, call the parent class if it doesn't consider the tag relevant, + giving the parent class a chance to determine if the tag is relevant. This is the mechanism by + which derived classes will also benefit from the `` parameter set. + + @param tagName The tag to test for relevancy. + @returns True if the tag is relevant. + */ + virtual bool isRelevant(const ::std::string& tagName); + + /*! + @brief Defines a constant value for an agent property as specified by the attribute of an agent + property tag. + + Derived classes should override this function, but possibly call the parent class's + implementation. First, it should test to see if the paramName is expected by the derived class. + If so, the derived class can determine fail or accept. If it is not expected, it should call the + parent class's implementation and returns its value. + + @param paramName A string containing the parameter name. + @param value A string containing the value for the parameter. + @returns The result of the parse: failure, ignored, or accepted. + */ + virtual Menge::Agents::AgentInitializer::ParseResult setFromXMLAttribute( + const ::std::string& paramName, const ::std::string& value); + + /*! + @brief Process the given `` tag. + + As a pre-condition to this function, the XML node contains a `` tag and has been + confirmed to have, at least, a name attribute. Nothing else about the tag has been validated. + + If the property name is unexpected, it will be ignored. If it is expected, this function will + attempt to interpret the XML tag as a number distribution for a valid agent attribute. If it can + do so, it is successful, if it can't, it fails. + + @param propName The extractd "name" property from the Property tag. + @param node The XML node for the Property tag. + @returns True if parsing was successful. + */ + virtual ParseResult processProperty(::std::string propName, TiXmlElement* node); + + /*! + @brief The personal space (in meters) of the agent + */ + Menge::Math::FloatGenerator* _perSpace; + + /*! + @brief The anticipation time (in seconds) of the agent + */ + Menge::Math::FloatGenerator* _anticipation; +}; +} // namespace OSM + +#endif // __OSM_INITIALIZER_H__ diff --git a/src/Plugins/AgtOSM/OSMSimulator.cpp b/src/Plugins/AgtOSM/OSMSimulator.cpp new file mode 100644 index 00000000..71f2861d --- /dev/null +++ b/src/Plugins/AgtOSM/OSMSimulator.cpp @@ -0,0 +1,75 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +#include "OSMSimulator.h" +#include "MengeCore/Runtime/Utils.h" + +namespace OSM { + +using Menge::DEG_TO_RAD; +using Menge::toFloat; +using Menge::toInt; +using Menge::UtilException; +using Menge::Agents::SimulatorBase; +using Menge::Agents::XMLParamException; + +//////////////////////////////////////////////////////////////// +// Implementation of Karamouzas::Simulator +//////////////////////////////////////////////////////////////// + +// These values come directly from the Karamouzas paper +float Simulator::ORIENT_WEIGHT = 0.8f; +float Simulator::COS_FOV_ANGLE = cos(100.f * DEG_TO_RAD); // 100 degrees (200 total) +float Simulator::REACTION_TIME = 0.4f; +float Simulator::WALL_STEEPNESS = 2.f; +float Simulator::WALL_DISTANCE = 2.f; // 2 meters (from the author's email...seem's quite large.) +int Simulator::COLLIDING_COUNT = 5; // K most dangerous neighbors considered +float Simulator::D_MIN = 1.f; +float Simulator::D_MID = 8.f; +float Simulator::D_MAX = 10.f; +float Simulator::AGENT_FORCE = 3.f; // how many Newtons? + +//////////////////////////////////////////////////////////////// + +bool Simulator::setExpParam(const std::string& paramName, + const std::string& value) throw(XMLParamException) { + try { + if (paramName == "orient_weight") { + ORIENT_WEIGHT = toFloat(value); + } else if (paramName == "fov") { + COS_FOV_ANGLE = cos(0.5f * toFloat(value) * + DEG_TO_RAD); // convert degrees to radians - operate on half angle + } else if (paramName == "reaction_time") { + REACTION_TIME = toFloat(value); + } else if (paramName == "wall_steepness") { + WALL_STEEPNESS = toFloat(value); + } else if (paramName == "wall_distance") { + WALL_DISTANCE = toFloat(value); + } else if (paramName == "colliding_count") { + COLLIDING_COUNT = toInt(value); + } else if (paramName == "d_min") { + D_MIN = toFloat(value); + } else if (paramName == "d_mid") { + D_MID = toFloat(value); + } else if (paramName == "d_max") { + D_MAX = toFloat(value); + } else if (paramName == "agent_force") { + AGENT_FORCE = toFloat(value); + } else if (!SimulatorBase::setExpParam(paramName, value)) { + // Simulator base didn't recognize the parameter either + return false; + } + } catch (UtilException) { + throw XMLParamException(std::string("Karamouzas parameter \"") + paramName + + std::string("\" value couldn't be converted to the correct " + "type. Found the value: ") + + value); + } + return true; +} +} // namespace OSM diff --git a/src/Plugins/AgtOSM/OSMSimulator.h b/src/Plugins/AgtOSM/OSMSimulator.h new file mode 100644 index 00000000..015f7af8 --- /dev/null +++ b/src/Plugins/AgtOSM/OSMSimulator.h @@ -0,0 +1,117 @@ +/* + OSM Simulation plugin + + Sergio Lozano Mateos + UNED + +*/ + +#ifndef __OSM_SIMULATOR_H__ +#define __OSM_SIMULATOR_H__ + +/*! + @file OSMSimulator.h + @brief Contains the OSM::Simulator class + + Implements Seitz's 2012 pedestrian model "Natural discretization of pedestrian movement in continuous space" + */ + +#include "OSMAgent.h" +#include "MengeCore/Agents/SimulatorBase.h" +#include "MengeCore/mengeCommon.h" + +/*! + @namespace OSM + @brief Contains the specification of the pedestrian model from the Seitz and Köster, 2012 paper. + */ +namespace OSM { +/*! + @brief Defines the simulator operating on a Karamouzas::Agent. + */ +class Simulator : public Menge::Agents::SimulatorBase { + public: + /*! + @brief Constructor. + */ + Simulator() : Menge::Agents::SimulatorBase() {} + + /*! + @brief Reports if there are non-common Experiment parameters that this simulator requires in + the XML file. + @returns By default, the simulator base ONLY uses common parameters. Always returns false. + */ + virtual bool hasExpTarget() { return true; } + + /*! + @brief Reports if the given Experiment attribute tag name belongs to this simulator. + @param tagName the name of the considered tag + @returns By default, the simulator base ONLY uses common parameters. Always returns false. + */ + virtual bool isExpTarget(const std::string& tagName) { return tagName == "OSM"; } + + /*! + @brief Given an Experiment parameter name and value, sets the appropriate simulator + parameter. + @param paramName A string containing the parameter name for the experiment. + @param value A string containing the value for the parameter. + @returns True if the parameters were successfully set. + */ + virtual bool setExpParam(const std::string& paramName, + const std::string& value) throw(Menge::Agents::XMLParamException); + + protected: + friend class Agent; + /*! + @brief Agent orientation is a weighted average between preferred direction and actual + direction; this is that weight. + */ + static float ORIENT_WEIGHT; + + /*! + @brief The half angle that defines the field of view around orientation + */ + static float COS_FOV_ANGLE; + + /*! + @brief The reaction time used to define the driving force + */ + static float REACTION_TIME; + + /*! + @brief The steepness that the wall forces fall off: K > 0 + */ + static float WALL_STEEPNESS; + + /*! + @brief The safe distance agents prefer to keep from the walls + */ + static float WALL_DISTANCE; + + /*! + @brief The number of agents considered in the colliding set + */ + static int COLLIDING_COUNT; + + /*! + @brief The critical distance at which agent response force climbs. + */ + static float D_MIN; + + /*! + @brief The distance at which a constant agent response force begins. + */ + static float D_MID; + + /*! + @brief The distance at which there is no longer an agent response force. + */ + static float D_MAX; + + /*! + @brief The level of response force in the constant region. + */ + static float AGENT_FORCE; +}; +} // namespace OSM + +#endif // __OSM_SIMULATOR_H__ diff --git a/src/Plugins/BanPrueba/MidPlan.cpp b/src/Plugins/BanPrueba/MidPlan.cpp new file mode 100644 index 00000000..b7a896b2 --- /dev/null +++ b/src/Plugins/BanPrueba/MidPlan.cpp @@ -0,0 +1,85 @@ +/* + +License + +Menge +Copyright © and trademark ™ 2012-14 University of North Carolina at Chapel Hill. +All rights reserved. + +Permission to use, copy, modify, and distribute this software and its documentation +for educational, research, and non-profit purposes, without fee, and without a +written agreement is hereby granted, provided that the above copyright notice, +this paragraph, and the following four paragraphs appear in all copies. + +This software program and documentation are copyrighted by the University of North +Carolina at Chapel Hill. The software program and documentation are supplied "as is," +without any accompanying services from the University of North Carolina at Chapel +Hill or the authors. The University of North Carolina at Chapel Hill and the +authors do not warrant that the operation of the program will be uninterrupted +or error-free. The end-user understands that the program was developed for research +purposes and is advised not to rely exclusively on the program for any reason. + +IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS +BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS +DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE +AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY WARRANTY +OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND +THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE NO OBLIGATIONS +TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Any questions or comments should be sent to the authors {menge,geom}@cs.unc.edu + +*/ + +/*! + * @file MidPlan.cpp + * @brief Plugin for Mid-term planning + */ + +#if defined(_MSC_VER) +#define PLUGIN_EXPORT __declspec(dllexport) +#elif defined(__GNUC__) +#define PLUGIN_EXPORT __attribute__((visibility("default"))) +#endif + +#include "MidPlanConfig.h" +#include "MidPlanModifier.h" +#include "MengeCore/PluginEngine/CorePluginEngine.h" + +using Menge::PluginEngine::CorePluginEngine; + +extern "C" { + +/*! + * @brief Retrieves the name of the plug-in. + * + * @returns The name of the plug in. + */ +extern "C" BPMODIFIER_API const char* getName() { return "banprueba"; } + +/*! + * @brief Description of the plug-in. + * + * @returns A description of the plugin. + */ +extern "C" BPMODIFIER_API const char* getDescription() { + return "Velocity Modifier plugin for adherence to the Fundamental Diagram" + "including the following:\n" + "\tModifier \"fundamantal_diagram\" - adjusts the agent's preferred velocity to match the " + "MP. "; +} + +/*! + * @brief Registers the plug-in with the PluginEngine + * + * @param engine A pointer to the plugin engine. + */ +extern "C" BPMODIFIER_API void registerCorePlugin(CorePluginEngine* engine) { + engine->registerVelModFactory(new BPModifier::BPModifierFactory()); +} +} \ No newline at end of file diff --git a/src/Plugins/BanPrueba/MidPlanConfig.h b/src/Plugins/BanPrueba/MidPlanConfig.h new file mode 100644 index 00000000..13404b65 --- /dev/null +++ b/src/Plugins/BanPrueba/MidPlanConfig.h @@ -0,0 +1,65 @@ +/* + Menge Crowd Simulation Framework + + Copyright and trademark 2012-17 University of North Carolina at Chapel Hill + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + or + LICENSE.txt in the root of the Menge repository. + + Any questions or comments should be sent to the authors menge@cs.unc.edu + + +*/ + +/*! + @file FundamentalDiagramConfig.h + @brief Configures the shared library aspect of the includes. + */ + +#ifndef __BPMODIFIER_CONFIG_H__ +#define __BPMODIFIER_CONFIG_H__ + +// Now set up compiler-dependent export/import symbols + +#if defined(_MSC_VER) + +#if defined(MPMODIFIER_STATICLIB) +#define BPMODIFIER_API +#else +#if defined(MPMODIFIER_EXPORT) +// We are building the DLL, export the symbols tagged like this +#define BPMODIFIER_API __declspec(dllexport) +#else +// If we are consuming the DLL, import the symbols tagged like this +#define BPMODIFIER_API __declspec(dllimport) +#endif +#endif + +#elif defined(__GNUC__) + +#if defined(FDMODIFIER_STATICLIB) +#define BPMODIFIER_API +#else +#if defined(FDMODIFIER_EXPORT) +#define BPMODIFIER_API __attribute__((visibility("default"))) +#else +// If you use -fvisibility=hidden in GCC, exception handling and RTTI +// would break if visibility wasn't set during export _and_ import +// because GCC would immediately forget all type infos encountered. +// See http://gcc.gnu.org/wiki/Visibility +#define BPMODIFIER_API __attribute__((visibility("default"))) +#endif +#endif + +#else + +#error Unknown compiler, please implement shared library macros + +#endif // Compiler-detection + +#endif // __BPMODIFIER_CONFIG_H__ diff --git a/src/Plugins/BanPrueba/MidPlanModifier.cpp b/src/Plugins/BanPrueba/MidPlanModifier.cpp new file mode 100644 index 00000000..15c19db0 --- /dev/null +++ b/src/Plugins/BanPrueba/MidPlanModifier.cpp @@ -0,0 +1,352 @@ +/* + +License + +Menge +Copyright © and trademark ™ 2012-14 University of North Carolina at Chapel Hill. +All rights reserved. + +Permission to use, copy, modify, and distribute this software and its documentation +for educational, research, and non-profit purposes, without fee, and without a +written agreement is hereby granted, provided that the above copyright notice, +this paragraph, and the following four paragraphs appear in all copies. + +This software program and documentation are copyrighted by the University of North +Carolina at Chapel Hill. The software program and documentation are supplied "as is," +without any accompanying services from the University of North Carolina at Chapel +Hill or the authors. The University of North Carolina at Chapel Hill and the +authors do not warrant that the operation of the program will be uninterrupted +or error-free. The end-user understands that the program was developed for research +purposes and is advised not to rely exclusively on the program for any reason. + +IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS +BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS +DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE +AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY WARRANTY +OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND +THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE NO OBLIGATIONS +TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Any questions or comments should be sent to the authors {menge,geom}@cs.unc.edu + +*/ + +#include "MidPlanModifier.h" +#include "SortedList.h" + +#include "MengeCore/Agents/BaseAgent.h" +#include "MengeCore/Agents/Obstacle.h" +#include "MengeCore/Agents/SpatialQueries/SpatialQueryStructs.h" +#include "MengeCore/Math/Line.h" +#include +#include +#include + + + +namespace BPModifier { + +using Menge::TWOPI; +using Menge::Agents::BaseAgent; +using Menge::Agents::Obstacle; +using Menge::Agents::PrefVelocity; +using Menge::BFSM::VelModFactory; +using Menge::BFSM::VelModifier; +using Menge::Math::FloatGenerator; +using Menge::Math::Vector2; +using Menge::Math::Line; + +const BaseAgent* evalAgent; +const BaseAgent* collAgent; +vector filteredNeighbours; +//multimap mapa; +float prefSpeed; +Vector2 prefVel; +//Vector2 prefDir; +Vector2 critPt; +float norm; +float density; +float areaSq2Inv; +double elapsedTime; + +int agtCol; +double ttca; + +int contador = 1; + + +///////////////////////////////////////////////////////////////////// +// Implementation of BPModifier +///////////////////////////////////////////////////////////////////// + +BPModifier::BPModifier() + : VelModifier(), _bufferGen(0x0), _factorGen(0x0), _sigmaAgent(1.5f), _sigmaObstacle(0.75f) {} + +///////////////////////////////////////////////////////////////////// + +BPModifier::BPModifier(FloatGenerator* buffer, FloatGenerator* factor, float sigmaAgent, + float sigmaObstacle) + : VelModifier(), + _bufferGen(buffer), + _factorGen(factor), + _sigmaAgent(sigmaAgent), + _sigmaObstacle(sigmaObstacle) {} + +///////////////////////////////////////////////////////////////////// + +VelModifier* BPModifier::copy() const { + return new BPModifier(_bufferGen->copy(), _factorGen->copy(), _sigmaAgent, _sigmaAgent); +}; + +float dotProduct(Vector2 u, Vector2 v) { return (u._x * v._x) + (u._y * v._y); } + +float vecProduct(Vector2 u, Vector2 v) { return (u._x * v._y) - (u._y * v._x); } + + +///////////////////////////////////////////////////////////////////// + +list interLines(Line l1, Line l2) { + list foundInt; + float a1 = l1._direction._y; + float b1 = -l1._direction._x; + float c1 = l1._direction._x * l1._point._y - l1._direction._y * l1._point._x; + float a2 = l2._direction._y; + float b2 = -l2._direction._x; + float c2 = l2._direction._x * l2._point._y - l2._direction._y * l2._point._x; + float x = (b1 * c2 - b2 * c1) / (a1 * b2 - a2 * b1); + float y = (c1 * a2 - c2 * a1) / (a1 * b2 - a2 * b1); + cout << " Linea l1 " << l1._direction << " l2 " << l2._direction << endl; + //cout << "Linea" << x << "," << y << endl; + if (x != INFINITY && y != INFINITY) { + foundInt.push_back(Vector2(x, y)); + } + return foundInt; +} + +list interCircLine(Vector2 pos, Vector2 rad, Line line) { + list foundInt; + float radius = rad.Length(); + // Intersección entre línea y circunferencia + Vector2 nearestPt = line.nearestPt(pos); + Vector2 d1 = nearestPt - pos; + float minDist = d1.Length(); + // Check if there is intersection + //cout << "mindist " << minDist << " radio " << radius << endl; + if (minDist > radius) return foundInt; + float c = (float) sqrt(pow(radius, 2) - pow(minDist, 2)); + // Tenemos magnitud, sacamos vector + //cout << "Direccion de la recta " << line._direction << " c " << c << endl; + Vector2 vc = line._direction * c; + // Sumamos y restamos + //cout << "Vector minDist " << d1 << " vector vc " << vc << endl; + Vector2 s1 = d1 + vc + pos; + Vector2 s2 = d1 - vc + pos; + //cout << "Vector minDist " << d1 << " vector vc " << vc << endl; + foundInt.push_back(s1); + if (s1 != s2) foundInt.push_back(s2); + return foundInt; +} + +pair minDistance(Vector2 pos1, Vector2 pos2, Vector2 vel1, Vector2 vel2) { + // Distance vector from 2 to 1 + Vector2 d21 = (pos2 - pos1); + // Relative velocity of 2 wrt 1 + Vector2 v21 = (vel2 - vel1); + // Minimal distance between 1 and 2 + //float producto = vecProduct(d21, v21); + //cout << "Producto vec " << producto << " distancia " << v21.Length() << endl; + double minDist = abs(vecProduct(d21, v21) / v21.Length()); + //cout << " -- Velocidades " << vel1 << vel2 << endl; + //cout << " Pos 2 " << pos2 << endl; + double distance = sqrt(pow(d21.Length(), 2) - pow(minDist, 2)); + //cout << " distance " << distance << endl; + double ttca21 = abs(distance / v21.Length()); + //cout << " minDist " << minDist << " ttca " << ttca21 << endl; + // cout << "Colision " << item.first << endl; + return pair(minDist, ttca21); +} + +/// + +int sign(float x) { + if (x > 0) return 1; + if (x < 0) return -1; + return 0; +} +bool sameOrient(Vector2 v, Vector2 u) { + if (sign(v._x) == sign(u._x) && sign(v._y) == sign(u._y)) return true; + return false; +} + +void FilterNeighbours() { + // Variables for back space delimit + Vector2 orient = prefVel; + orient.normalize(); + Vector2 pos = evalAgent->_pos; + float speed = prefSpeed; + float maxSpeed = evalAgent->_maxSpeed; + Vector2 vel = orient * speed; + Vector2 perp(-orient.y(), orient.x()); // Vector perpendicular to the orient + Line backLine(pos, perp); // Line delimiting back space + //cout << "Linea trasera " << backLine._direction << endl; + multimap filtered; + //cout << "Agentes a filtrar " << evalAgent->_nearAgents.size() << endl; + for (int i = 0; i < evalAgent->_nearAgents.size(); ++i) { + //filtered.emplace(0, i); + //continue; + + const BaseAgent* const other = evalAgent->_nearAgents[i].agent; + Vector2 otherVel = other->_velPref.getPreferredVel(); + + // Filtering the agent + // First step. Visibility + float distance = Vector2(other->_pos - pos).Length(); + Vector2 nearestPt = backLine.nearestPt(other->_pos); + //cout << "Punto mas cercano " << nearestPt << endl; + Vector2 distanceToLine = other->_pos - nearestPt; + distanceToLine.normalize(); + // Discard backwards agents + // cout << "velocidad " << orient << " vector distancia " << distanceToLine << endl; + // if (!sameOrient(orient,distanceToLine) && distance >= 1.f) { + if (!sameOrient(orient, distanceToLine) && distance >= 1.f) continue; + + // Second step. Calculate MIT + + Vector2 centerVel = other->_pos + otherVel; + Vector2 p1p2 = other->_pos - pos; + p1p2.normalize(); + Line p1p2Line(other->_pos, p1p2); + Vector2 np1p2(-p1p2._y, p1p2._x); + np1p2.normalize(); + Line perpLine(other->_pos, np1p2); + + list interPoints = interCircLine(centerVel, orient*maxSpeed, p1p2Line); + if (interPoints.size() == 0) { + //cout << "fuera " << endl; + continue; + } + float velPos = 50.f; + Vector2 point; + for (Vector2 item : interPoints) { + float distTo = (item - pos).Length(); + if (distTo < p1p2.Length()) continue; + if (distTo < velPos) { + velPos = distTo; + point = item; + } + } + Vector2 IS = centerVel - point; + double MIT = minDistance(pos, other->_pos, IS, otherVel).second; + filtered.emplace(MIT, i); + + } + //cout << "Agentes filtrados " << filtered.size() << endl; + // "Best" neighbours number. TODO implement as param + const int bestNumber = 5; + multimap::iterator itr; + int j = 1; + for (itr = filtered.begin(); itr != filtered.end(); ++itr) { + filteredNeighbours.push_back(evalAgent->_nearAgents[itr->second].agent); + j = ++j; + if (j > bestNumber) break; + } + +} + + +pair CIWpoint(Vector2 pos1, Vector2 pos2, Vector2 vel1, Vector2 vel2) { + pair minDistTime = minDistance(pos1, pos2, vel1, vel2); + double time = + minDistTime.second + min(abs(minDistTime.first) / vel1.Length(), abs(minDistTime.first) / vel2.Length()); + cout << " Vel " << vel1 << vel1.Length() << vel2 << " Mindist " << abs(minDistTime.first) << " Tiempo real " + << minDistTime.second + << " Tiempo " << time << endl; + pair CIWpoint(pos1 + vel1 * (float) time, time); + return CIWpoint; +} + +///////////////////////////////////////////////////////////////////// + + +///////////////////////////////////////////////////////////////////// + + +void BPModifier::adaptPrefVelocity(const BaseAgent* agent, PrefVelocity& pVel) { + filteredNeighbours.clear(); + elapsedTime = 0.f; + evalAgent = agent; + cout << endl; + cout << "AGENTE: " << agent->_id << " paso " << contador << " pos " << agent->_pos << " vel " << agent->_vel << agent->_vel.Length() << endl; + contador++; + float strideConst, speedConst; + _paramLock.lock(); + + HASH_MAP::iterator itr = _strideParams.find(agent->_id); + if (itr == _strideParams.end()) { + BPParam param(_factorGen->getValue(), _bufferGen->getValue()); + _strideParams[agent->_id] = param; + strideConst = param._strideConst; + speedConst = param._speedConst; + } else { + strideConst = itr->second._strideConst; + speedConst = itr->second._speedConst; + } + _paramLock.release(); + + + // define temporary vars + + // TODO param + float mass = 1; + prefSpeed = pVel.getSpeed(); + Vector2 prefDir = pVel.getPreferred(); + prefVel = prefDir * prefSpeed; + Vector2 newVel(1,0.1); + newVel.normalize(); + float newSpeed = 1.08f; + + pVel.setSpeed(newSpeed); + pVel.setSingle(newVel); + cout << " NewVel" << newVel*newSpeed << " " << newSpeed << endl; + cout << " CurrentVel" << agent->_vel << agent->_vel.Length() << endl; + cout << " AgentPref" << agent->_velPref.getPreferredVel() << agent->_velPref.getSpeed() << endl; + cout << endl; + + // Compute the maximum speed I could take for the available space + /* float maxSpeed = speedConst * availSpace * availSpace; + if (maxSpeed < prefSpeed) pVel.setSpeed(maxSpeed);*/ +} + +///////////////////////////////////////////////////////////////////// +// Implementation of MPModFactory +///////////////////////////////////////////////////////////////////// + +BPModifierFactory::BPModifierFactory() : VelModFactory() { + _factorID = _attrSet.addFloatDistAttribute("factor_", true, 0.f, 1.f); + _bufferID = _attrSet.addFloatDistAttribute("buffer_", true, 0.f, 1.f); + _sigmaAgentID = _attrSet.addFloatAttribute("sigma_agent", false, 1.5f); + _sigmaObstacleID = _attrSet.addFloatAttribute("sigma_obstacle", false, 0.75f); +} + +///////////////////////////////////////////////////////////////////// + +bool BPModifierFactory::setFromXML(VelModifier* modifier, TiXmlElement* node, + const std::string& behaveFldr) const { + BPModifier* MPMod = dynamic_cast(modifier); + assert(MPMod != 0x0 && "Trying to set property modifier properties on an incompatible object"); + + if (!VelModFactory::setFromXML(modifier, node, behaveFldr)) return false; + + // set the params we need + MPMod->setBuffer(_attrSet.getFloatGenerator(_bufferID)); + MPMod->setFactor(_attrSet.getFloatGenerator(_factorID)); + MPMod->setSigmaAgent(_attrSet.getFloat(_sigmaAgentID)); + MPMod->setSigmaObstacle(_attrSet.getFloat(_sigmaObstacleID)); + return true; +} +} // namespace BPModifier diff --git a/src/Plugins/BanPrueba/MidPlanModifier.h b/src/Plugins/BanPrueba/MidPlanModifier.h new file mode 100644 index 00000000..dce9180c --- /dev/null +++ b/src/Plugins/BanPrueba/MidPlanModifier.h @@ -0,0 +1,328 @@ +/* + Menge Crowd Simulation Framework + + Copyright and trademark 2012-17 University of North Carolina at Chapel Hill + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + or + LICENSE.txt in the root of the Menge repository. + + Any questions or comments should be sent to the authors menge@cs.unc.edu + + +*/ + +/*! + @file FundamentalDiagramModifier.h + @brief Definiton of a Velocity Modifier to enforce FD adherance + + Uses the model presented in http://gamma.cs.unc.edu/DenseSense/. + */ +#ifndef __BPMODIFIER_MODIFIER_H__ +#define __BPMODIFIER_MODIFIER_H__ + +#include "MidPlanConfig.h" + +#include "MengeCore/BFSM/FSMEnumeration.h" +#include "MengeCore/BFSM/VelocityModifiers/VelModifier.h" +#include "MengeCore/BFSM/VelocityModifiers/VelModifierFactory.h" +#include "MengeCore/Runtime/SimpleLock.h" +#include "MengeCore/mengeCommon.h" + + +/*! + @namespace BPModifier + @brief The name space for the Fundamental Diagram adherence model + + This namespace contains a velocity modifier which varies preferred speed based on local density + conditions. + */ +namespace BPModifier { + + +class BPModifierFactory; + +/*! + @brief Velocity modifier that adapts preferred velocity to account for local density. + + This produces a density-dependent behavior which can conform to the fundamental diagram (depending + on the settings.) + + This is an implementation of the work found at + *http://gamma.cs.unc.edu/PEDS/download/curtis12_FD.pdf + It is a model of the underlying causes of the so-called *fundamental diagram* -- the name for the + density-dependent behavior observed in pedestrians. As the crowd gets denser, the crowd moves more + slowly. It uses two parameter: stride factor and stride buffer. The stride *factor* models + physiological factors that correlate available space to stride length (and therefore walking + speed). A typical mean value is 1.57. The stride *buffer* models psychological factors which + determine how the agent perceives the physical space as a *comfortable* space. Typical values are + in the range 0.5-0.9 + + To specify a fundamental velocity modifier, use the following syntax: + + ```xml + + ``` + + The parameters have the following meanings: + - The value for `buffer_dist` defines the numerical distribution for the stride *buffer* value. + In the example given above, it is a constant distribution. For other distributions (i.e., + uniform (`u`) or normal (`n`), `buffer_value` would be replaced by the pairs `buffer_min` and + `buffer_max` or `buffer_mean` and `buffer_stddev`, respectively. + - The value for `factor_dist` defines the numerical distribution for the stride *factor* value. + In the example given above, it is a constant distribution. For other distributions (i.e., + uniform (`u`) or normal (`n`), `factor_value` would be replaced by the pairs `factor_min` and + `factor_max` or `factor_mean` and `factor_stddev`, respectively. + */ +class BPMODIFIER_API BPModifier : public Menge::BFSM::VelModifier { + public: + + + + /*! + @brief Constructor. + */ + BPModifier(); + + /*! + @brief Constructor. + + @param buffer The stride buffer gemerator. This modifier takes ownership of the + provided generator. + @param factor The stride factor gemerator. This modifier takes ownership of the + provided generator. + @param sigmaAgent Sigma for agent density estimation + @param sigmaObstacle Sigma for obstacle density estimation + */ + BPModifier(Menge::Math::FloatGenerator* buffer, Menge::Math::FloatGenerator* factor, + float sigmaAgent, float sigmaObstacle); + + /*! + @brief Copy method for this velocity modifier. + */ + Menge::BFSM::VelModifier* copy() const; + + /*! + @brief Filter the near agents + + @param bc vector a calcular + */ + //vector BPModifier::getAgtFiltered() { return _nearAgents; } + + /*! + @brief Evaluate mid-term collision + + @param bc vector a calcular + */ + + /*! + @brief Compute the CIW points + + @param bc vector a calcular + */ + + + /*! + @brief Adapts the given agent's preferred velocity to adhere to the fundamental diagram. + + @param agent The agent on whom we are operating. + @param pVel The input preferred velocity of the agent. + */ + void adaptPrefVelocity(const Menge::Agents::BaseAgent* agent, Menge::Agents::PrefVelocity& pVel); + + /*! + @brief Sets the stride buffer. + + @param buffer The stride buffer for the adherence model + */ + void setBuffer(Menge::Math::FloatGenerator* buffer) { + if (_bufferGen) delete _bufferGen; + _bufferGen = buffer; + } + + /*! + @brief Sets the stride factor. + +@param factor The stride factor for the adherence model + */ + void setFactor(Menge::Math::FloatGenerator* factor) { + if (_factorGen) delete _factorGen; + _factorGen = factor; + } + + /*! + @brief Sets the agent sigma. + + @param sigma Sigma for agent density estimation + */ + void setSigmaAgent(float sigma) { _sigmaAgent = sigma; } + + /*! + @brief Sets the agent sigma. + + @param sigma Obstacle sigma for Probability Density Estimation + */ + void setSigmaObstacle(float sigma) { _sigmaObstacle = sigma; } + + friend class BPModifierFactory; + + protected: + /*! + @brief Defines the parameters which define the density-aware behavior. + */ + struct BPParam { + /*! + @brief Default constructor + */ + BPParam() : _strideConst(1.f), _speedConst(1.f) {} + + /*! + @brief Constructor. + + @param factor The stride factor. + @param buffer The stride buffer. + */ + BPParam(float factor, float buffer) { + _strideConst = 0.5f * (1.f + buffer) / factor; + _speedConst = 1.f / (_strideConst * _strideConst); + } + + /*! + @brief One of two derived constants for fundamental diagram compliance. + + This is a function of the stride factor and stride buffer + */ + float _strideConst; + + /*! + @brief The second of two derived constants for fundamental diagram compliance. + + This is a function of the stride factor and stride buffer + */ + float _speedConst; + }; + + /*! + @brief The readers-writer lock to preserve thread-safety on _strideParams. + */ + Menge::SimpleLock _paramLock; + + /*! + @brief The per-agent parameters. + */ + HASH_MAP _strideParams; + + /*! + @brief The Stride buffer value generator. + */ + Menge::Math::FloatGenerator* _bufferGen; + + /*! + @brief The Stride factor value generator. + */ + Menge::Math::FloatGenerator* _factorGen; + + /*! + @brief Agent sigma for density calculation + */ + float _sigmaAgent; + + /*! + @brief Sigma for obstacle density estimation + */ + float _sigmaObstacle; +}; + +//////////////////////////////////////////////////////////////////////// + +/*! + @brief The factory for the BPModifier class. + */ +class BPMODIFIER_API BPModifierFactory : public Menge::BFSM::VelModFactory { + public: + /*! + @brief Constructor. + */ + BPModifierFactory(); + + /*! + @brief The name of the modifier + + The modifier's name must be unique among all registered modifier. + Each modifier factory must override this function. + + @returns A string containing the unique modifier name. + */ + virtual const char* name() const { return "banprueba"; } + + /*! + @brief A description of the modifier + + Each modifier factory must override this function. + + @returns A string containing the modifier description. + */ + virtual const char* description() const { + return "Adjusts the agent's preferred speed to adhere to the fundamental diagram "; + }; + + protected: + /*! + @brief Create an instance of this class's modifier. + + All modifierFactory sub-classes must override this by creating (on the heap) a new instance of + its corresponding modifier type. The various field values of the instance will be set in a + subsequent call to modifierFactory::setFromXML(). The caller of this function takes ownership of + the memory. + + @returns A pointer to a newly instantiated modifier class. + */ + Menge::BFSM::VelModifier* instance() const { return new BPModifier(); } + + /*! + @brief Given a pointer to an modifier instance, sets the appropriate fields from the provided + XML node. + + It is assumed that the value of the `type` attribute is this modifier's type (i.e. + modifierFactory::thisFactory() has already been called and returned true). If sub-classes of + modifierFactory introduce *new* modifier parameters, then the sub-class should override this + method but explicitly call the parent class's version. + + @param modifier A pointer to the modifier whose attributes are to be set. + @param node The XML node containing the modifier attributes. + @param behaveFldr The path to the behavior file. If the modifier references resources in the + file system, it should be defined relative to the behavior file location. + This is the folder containing that path. + @returns True if successful. + */ + virtual bool setFromXML(Menge::BFSM::VelModifier* modifier, TiXmlElement* node, + const std::string& behaveFldr) const; + + /*! + @brief The identifier for the "stride_buffer" float attribute + */ + size_t _bufferID; + + /*! + @brief The identifier for the "stride_factor" float attribute + */ + size_t _factorID; + + /*! + @brief The identifier for the "sigma_agent" float attribute + */ + size_t _sigmaAgentID; + + /*! + @brief The identifier for the "sigma_obstacle" float attribute + */ + size_t _sigmaObstacleID; +}; +} // namespace BPModifier +#endif // __BPMODIFIER_MODIFIER_H__ diff --git a/src/Plugins/BanPrueba/SortedList.h b/src/Plugins/BanPrueba/SortedList.h new file mode 100644 index 00000000..1cc6110b --- /dev/null +++ b/src/Plugins/BanPrueba/SortedList.h @@ -0,0 +1,84 @@ +#ifndef __SLIST_H__ +#define __SLIST_H__ + +#include + +using namespace std; + +class SortedList { + private: + class Node { + public: + int info; + Node* next; + }; + + Node* root; + + public: + SortedList(); + ~SortedList(); + void insert(int x); + void print(); +}; + +SortedList::SortedList() { root = NULL; } + +SortedList::~SortedList() { + Node* iter = root; + Node* del; + while (iter != NULL) { + del = iter; + iter = iter->next; + delete del; + } +} + +void SortedList::insert(int x) { + Node* newElement = new Node(); + newElement->info = x; + if (root == NULL) { + root = newElement; + } else { + if (x < root->info) { + newElement->next = root; + root = newElement; + } else { + Node* iter = root; + Node* back = root; + while (x >= iter->info && iter->next != NULL) { + back = iter; + iter = iter->next; + } + if (x >= iter->info) { + iter->next = newElement; + } else { + newElement->next = iter; + back->next = newElement; + } + } + } +} + +void SortedList::print() { + Node* reco = root; + cout << "Full list.\n"; + while (reco != NULL) { + cout << reco->info << "-"; + reco = reco->next; + } + cout << "\n"; +} + +int main() { + SortedList* list = new SortedList(); + list->insert(10); + list->insert(5); + list->insert(7); + list->insert(50); + list->print(); + delete list; + return 0; +} + +#endif // __SLIST_H__ diff --git a/src/Plugins/MidPlan/MidPlan.cpp b/src/Plugins/MidPlan/MidPlan.cpp new file mode 100644 index 00000000..3b9fc27a --- /dev/null +++ b/src/Plugins/MidPlan/MidPlan.cpp @@ -0,0 +1,54 @@ +/* + +Sergio Lozano Mateos +UNED, Spain + +*/ + +/*! + * @file MidPlan.cpp + * @brief Plugin for Mid-term planning + */ + +#if defined(_MSC_VER) +#define PLUGIN_EXPORT __declspec(dllexport) +#elif defined(__GNUC__) +#define PLUGIN_EXPORT __attribute__((visibility("default"))) +#endif + +#include "MidPlanConfig.h" +#include "MidPlanModifier.h" +#include "MengeCore/PluginEngine/CorePluginEngine.h" + +using Menge::PluginEngine::CorePluginEngine; + +extern "C" { + +/*! + * @brief Retrieves the name of the plug-in. + * + * @returns The name of the plug in. + */ +extern "C" MPMODIFIER_API const char* getName() { return "midterm_planning"; } + +/*! + * @brief Description of the plug-in. + * + * @returns A description of the plugin. + */ +extern "C" MPMODIFIER_API const char* getDescription() { + return "Velocity Modifier plugin for adherence to EACS algorithm" + "including the following:\n" + "\tModifier \"midterm_planning\" - adjusts the agent's preferred velocity to match the " + "Mid-term Plan adaptation. "; +} + +/*! + * @brief Registers the plug-in with the PluginEngine + * + * @param engine A pointer to the plugin engine. + */ +extern "C" MPMODIFIER_API void registerCorePlugin(CorePluginEngine* engine) { + engine->registerVelModFactory(new MPModifier::MPModifierFactory()); +} +} \ No newline at end of file diff --git a/src/Plugins/MidPlan/MidPlanConfig.h b/src/Plugins/MidPlan/MidPlanConfig.h new file mode 100644 index 00000000..05f06c94 --- /dev/null +++ b/src/Plugins/MidPlan/MidPlanConfig.h @@ -0,0 +1,54 @@ +/* + +Sergio Lozano Mateos +UNED, Spain + +*/ + +/*! + @file MidPlanConfig.h + @brief Configures the shared library aspect of the includes. + */ + +#ifndef __MPMODIFIER_CONFIG_H__ +#define __MPMODIFIER_CONFIG_H__ + +// Now set up compiler-dependent export/import symbols + +#if defined(_MSC_VER) + +#if defined(MPMODIFIER_STATICLIB) +#define MPMODIFIER_API +#else +#if defined(MPMODIFIER_EXPORT) +// We are building the DLL, export the symbols tagged like this +#define MPMODIFIER_API __declspec(dllexport) +#else +// If we are consuming the DLL, import the symbols tagged like this +#define MPMODIFIER_API __declspec(dllimport) +#endif +#endif + +#elif defined(__GNUC__) + +#if defined(FDMODIFIER_STATICLIB) +#define MPMODIFIER_API +#else +#if defined(FDMODIFIER_EXPORT) +#define MPMODIFIER_API __attribute__((visibility("default"))) +#else +// If you use -fvisibility=hidden in GCC, exception handling and RTTI +// would break if visibility wasn't set during export _and_ import +// because GCC would immediately forget all type infos encountered. +// See http://gcc.gnu.org/wiki/Visibility +#define MPMODIFIER_API __attribute__((visibility("default"))) +#endif +#endif + +#else + +#error Unknown compiler, please implement shared library macros + +#endif // Compiler-detection + +#endif // __MPMODIFIER_CONFIG_H__ diff --git a/src/Plugins/MidPlan/MidPlanModifier.cpp b/src/Plugins/MidPlan/MidPlanModifier.cpp new file mode 100644 index 00000000..4e5aad6f --- /dev/null +++ b/src/Plugins/MidPlan/MidPlanModifier.cpp @@ -0,0 +1,745 @@ +/* + +Sergio Lozano Mateos +UNED, Spain + +*/ + +#include "MidPlanModifier.h" +#include "MengeCore/Agents/BaseAgent.h" +#include "MengeCore/Agents/Obstacle.h" +#include "MengeCore/Agents/SpatialQueries/SpatialQueryStructs.h" +#include "MengeCore/Math/Line.h" +#include "Segment.h" +#include +#include +#include +#include +#include +#include +#include + +namespace MPModifier { + +using Menge::Agents::BaseAgent; +using Menge::Agents::Obstacle; +using Menge::Agents::PrefVelocity; +using Menge::BFSM::VelModFactory; +using Menge::BFSM::VelModifier; +using Menge::Math::FloatGenerator; +using Menge::Math::Line; +using Menge::Math::Vector2; + +// Params +float mass = 1; +float detourDistance = 10; +bool trace; +int maxPeriod = 10; +int minPeriod = 0; + +// Shared variables +unordered_map> freqTab; +int stepCount = 1; + +mutex m; + +void incrementCounter() { + m.lock(); + stepCount++; + m.unlock(); +} + +int getCounter() { + int readCount; + m.lock(); + readCount=stepCount; + m.unlock(); + return readCount; +} + +void writeOnTab(size_t id, int per, Vector2 vel) { + m.lock(); + freqTab[id] = make_pair(per, vel); + m.unlock(); +} + +Vector2 decrementOnTab(size_t id) { + m.lock(); + Vector2 sameVel = freqTab[id].second; + freqTab[id].first = freqTab[id].first - 1; + m.unlock(); + return sameVel; +} + +bool isInTab(size_t id) { + m.lock(); + auto tabNumber = freqTab.find(id); + if (tabNumber == freqTab.end()) { + m.unlock(); + return false; + } + m.unlock(); + return true; +} + +bool zeroOnTab(size_t id) { + m.lock(); + if (freqTab[id].first != 0) { + m.unlock(); + return false; + } + m.unlock(); + return true; +} + +///////////////////////////////////////////////////////////////////// +// Implementation of MPModifier +///////////////////////////////////////////////////////////////////// + +MPModifier::MPModifier() + : VelModifier(), _followDist(0.5f), _maxExpansions(5), _maxMITneigh(5), _trace(false) {} + +///////////////////////////////////////////////////////////////////// + +MPModifier::MPModifier(float followDist, int maxExpansions, int _maxMITneigh, bool trace) + : VelModifier(), + _followDist(followDist), + _maxExpansions(maxExpansions), + _maxMITneigh(5), _trace(trace) {} + +///////////////////////////////////////////////////////////////////// + +VelModifier* MPModifier::copy() const { + return new MPModifier(_followDist, _maxExpansions, _maxMITneigh, _trace); +}; + +float dotProduct(Vector2 u, Vector2 v) { return (u._x * v._x) + (u._y * v._y); } + +float vecProduct(Vector2 u, Vector2 v) { return (u._x * v._y) - (u._y * v._x); } + +///////////////////////////////////////////////////////////////////// + + +// Function to obtain the intersection points between two lines +list interLines(Line l1, Line l2) { + list foundInt; + float a1 = -l1._direction._y; + float b1 = l1._direction._x; + float c1 = l1._direction._y * l1._point._x - l1._direction._x * l1._point._y; + float a2 = -l2._direction._y; + float b2 = l2._direction._x; + float c2 = l2._direction._y * l2._point._x - l2._direction._x * l2._point._y; + float x = (b1 * c2 - b2 * c1) / (a1 * b2 - a2 * b1); + float y = (c1 * a2 - c2 * a1) / (a1 * b2 - a2 * b1); + if (x == x && y == y) { + foundInt.push_back(Vector2(x, y)); + } + return foundInt; +} + +// Function to obtain the intersection points between circumference and line +list interCircLine(Vector2 pos, Vector2 rad, Line line) { + list foundInt; + float radius = rad.Length(); + // Intersection between line and circumference + Vector2 nearestPt = line.nearestPt(pos); + Vector2 d1 = nearestPt - pos; + float minDist = d1.Length(); + // Check if there is intersection + // if no intersection + if (minDist > radius) return foundInt; + // if is tangent + if (minDist == radius) { + foundInt.push_back(nearestPt); + return foundInt; + } + // if is secant + float c = (float)sqrt(pow(radius, 2) - pow(minDist, 2)); + // We have magnitude, getting vector + Vector2 vc = line._direction * c; + // Add and substract normal vector + Vector2 s1 = d1 + vc + pos; + Vector2 s2 = d1 - vc + pos; + foundInt.push_back(s1); + foundInt.push_back(s2); + return foundInt; +} + +// Function to get de minimal distance two moving objects will be +pair minDistance(Vector2 pos1, Vector2 pos2, Vector2 vel1, Vector2 vel2) { + // Distance vector from 2 to 1 + Vector2 d21 = pos2 - pos1; + float distance12 = d21.Length(); + // Relative velocity of 2 wrt 1 + Vector2 v21 = (vel2 - vel1); + // Detect if they move away or are parallel to rule out subsequent calculations + Vector2 ptest1 = pos1 + vel1; + Vector2 ptest2 = pos2 + vel2; + float distTest = (ptest1 - ptest2).Length(); + if (distTest >= distance12) { + return pair(distance12, 0); + } + // Minimal distance between 1 and 2 + double minDist = abs(vecProduct(d21, v21) / v21.Length()); + double distance = sqrt(pow(distance12, 2) - pow(minDist, 2)); + double ttca21 = abs(distance / v21.Length()); + return pair(minDist, ttca21); +} + +// Function to get the normal of a vector +Vector2 getNormal(Vector2 v) { return Vector2(-v._y, v._x); } + + +// Calculates the cosen of the angle between two vectors. If they are paralel, cos > 0, they are on +// the same direction +float vectorsCos(Vector2 u, Vector2 v) { return dotProduct(u, v) / (u.Length() * v.Length()); } + +// Function to check if two points are on the same side of a line +bool sameSide(Line l, Vector2 a, Vector2 b) { + Vector2 nearestToA = l.nearestPt(a); + Vector2 nearestToB = l.nearestPt(b); + Vector2 distA = a - nearestToA; + Vector2 distB = b - nearestToB; + if (vectorsCos(distA,distB) > 0) + return true; + return false; +} + +// Function to filter neighbours according to EACS +void MPModifier::FilterNeighbours(const BaseAgent* evalAgent, vector &filteredNeighbours, Vector2 prefVel, double &minMIT, int maxMITneigh) { + // Variables for back space delimit + Vector2 pos = evalAgent->_pos; + float maxSpeed = evalAgent->_maxSpeed; + Vector2 vel = prefVel; + Vector2 orient = vel; + orient.normalize(); + Vector2 frontPoint = pos + orient; + Line backLine(pos, getNormal(orient)); // Line delimiting back space + multimap filtered; + for (int i = 0; i < evalAgent->_nearAgents.size(); ++i) { + const BaseAgent* const other = evalAgent->_nearAgents[i].agent; + Vector2 otherVel = other->_velPref.getPreferredVel(); + // Filtering the agent + // First step. Visibility + float distance = Vector2(other->_pos - pos).Length(); + // Discard backwards agents + if (!sameSide(backLine, frontPoint, other->_pos) && distance >= 1.f) continue; + // Second step. Calculate MIT + Vector2 centerVel = other->_pos + otherVel; + Vector2 p1p2 = other->_pos - pos; + Vector2 dir12 = p1p2; + dir12.normalize(); + Line p1p2Line(other->_pos, dir12); + Vector2 ndir12(-dir12._y, dir12._x); + Line perpLine(other->_pos, ndir12); + list interPoints = interCircLine(centerVel, orient * maxSpeed, p1p2Line); + // Discard when none intersection points found + if (interPoints.size() == 0) continue; + double MIT = 100; + for (Vector2 item : interPoints) { + float distTo = (item - pos).Length(); + // Discard when the intersection point is at the other side of de perp line + if (!sameSide(perpLine, pos, item)) continue; + Vector2 IS = centerVel - item; + Vector2 v2v1 = otherVel - IS; + double newMIT = p1p2.Length() / v2v1.Length(); + if (newMIT < MIT) MIT = newMIT; + } + // insert agent in the list with their MIT + filtered.insert({MIT, i}); + } + // Define the minMIT to further calculation of the frecuency + if (filtered.empty()) { + minMIT = evalAgent->_neighborDist / (evalAgent->_maxSpeed - evalAgent->_prefSpeed); + } else { + minMIT = filtered.begin()->first; + } + // Limit to max MIT neighbours number. + multimap::iterator itr; + int j = 1; + for (itr = filtered.begin(); itr != filtered.end(); ++itr) { + filteredNeighbours.push_back(evalAgent->_nearAgents[itr->second].agent); + j = ++j; + if (j > maxMITneigh) break; + } +} + +// Function to check if the evaluation agent collides with any other +bool MPModifier::Collision(Segment bc, const BaseAgent* evalAgent, const BaseAgent*& collAgent, + vector filteredNeighbours, double &ttca) { + bool collision = false; + // Variables + Vector2 pos1 = bc.start; + Vector2 vel1 = bc.length / (float)bc.time * bc.getOrient(); + for (size_t i = 0; i < filteredNeighbours.size(); ++i) { + const BaseAgent* other = filteredNeighbours[i]; + Vector2 vel2 = other->_velPref.getPreferredVel(); + Vector2 pos2 = other->_pos + vel2 * (float)bc.eTime; + float critDist = 0.98f * (other->_radius + evalAgent->_radius); + pair minDist = minDistance(pos1, pos2, vel1, vel2); + Vector2 mindistPoint = pos1 + vel1 * (float)minDist.second; + if ((minDist.first < critDist) && + (bc.length + evalAgent->_radius + other->_radius > (mindistPoint - pos1).Length())) { + collision = true; + if (trace) + cout << "(" << evalAgent->_id << ") " + << "Collision with: " << other->_id << " point " << mindistPoint << " mindist " + << minDist.first << " ttca " << minDist.second << " vel2 " << other->_vel + << other->_vel.Length() << endl; + ttca = minDist.second; + collAgent = other; + break; + } + } + return collision; +} + +// Function to get the CIW points of a colliding agent +list> MPModifier::ComputeCIW(Segment bc, const BaseAgent* evalAgent, + const BaseAgent* collAgent, double ttca) { + list> lista; + list intersecPoints; + list CIW; + Vector2 pos1 = bc.start; + Vector2 vel1 = bc.getOrient() * bc.length / (float)bc.time; + Vector2 vel2 = collAgent->_velPref.getPreferredVel(); + Vector2 pos2 = collAgent->_pos + vel2 * bc.eTime; + Vector2 d21 = (pos2 - pos1); + double dist21 = d21.Length(); + double critDist = (collAgent->_radius + evalAgent->_radius); + double sinA = critDist / dist21; + double cosA = (float)sqrt(1 - pow(sinA, 2)); + double x = d21._x; + double y = d21._y; + // Line in the direction of vel1 + Line l(pos1, vel1); + // end of vel1 placed in pos1 + Vector2 v1end = pos1 + vel1; + // Tangents to 2 increased through 1 reduced. + Vector2 t1((float)(x * cosA - y * sinA), (float)(x * sinA + y * cosA)); + Vector2 t2((float)(x * cosA + y * sinA), (float)(-x * sinA + y * cosA)); + t1.normalize(); + t2.normalize(); + Line lt1(pos1 + vel2, t1); + Line lt2(pos1 + vel2, t2); + // Intersection between tangent lines and circumference of vel1 + // with lt1 + list distl = interCircLine(pos1, vel1, lt1); + // If there are two points, discard the one furthest from the tip of vel1 + if (distl.size() > 1) { + Vector2 sol = distl.front(); + float distTov1end1 = abs((v1end - sol).Length()); + sol = distl.back(); + float distTov1end2 = abs((v1end - sol).Length()); + if (distTov1end1 < distTov1end2) + distl.pop_back(); + else + distl.pop_front(); + } + // Left cut + intersecPoints.splice(intersecPoints.end(), distl); + distl = interCircLine(pos1, vel1, lt2); + // with lt2 + // If there are two points, discard the one furthest from the tip of vel1 + if (distl.size() > 1) { + Vector2 sol = distl.front(); + float distTov1end1 = abs((v1end - sol).Length()); + sol = distl.back(); + float distTov1end2 = abs((v1end - sol).Length()); + if (distTov1end1 < distTov1end2) + distl.pop_back(); + else + distl.pop_front(); + } + // Rigth cut + intersecPoints.splice(intersecPoints.end(), distl); + // Check if there is a following or an opposing case + bool isFollowing = false; + bool isOpposition = false; + double vel1Angle = vectorsCos(vel1, d21); + // in case of agent 2 is not moving + double vel2Angle = 1; + if (vel2.Length() != 0.f) vel2Angle = vectorsCos(vel2, d21); + + // Following and facing detection + if (abs(vel1Angle) >= abs(cosA) && abs(vel2Angle) >= abs(cosA) && vectorsCos(vel1, vel2) > 0) { + // Case detected + if (vectorsCos(vel1, vel2) > 0) { + // Same directions. Following case + if (trace) + cout << "(" << evalAgent->_id << ") " + << " Following case " << endl; + isFollowing = true; + } else { + // Oposite directions. Encounter + if (trace) + cout << "(" << evalAgent->_id << ") " + << " Facing case " << endl; + isOpposition = true; + } + } else { + // Intersection between the line of vel1 and t1 and t2 + distl = interLines(l, lt1); + intersecPoints.splice(intersecPoints.end(), distl); + distl = interLines(l, lt2); + intersecPoints.splice(intersecPoints.end(), distl); + } + for (Vector2 item : intersecPoints) { + if (trace) + cout << "(" << evalAgent->_id << ") " + << "CIW velocity " << item - pos1 << (item - pos1).Length() << endl; + // Discard if the new speed is equal to that of agent 2 + if (item - pos1 == vel2) continue; + Vector2 nvel1 = item - pos1; + pair minDist = minDistance(pos1, pos2, nvel1, vel2); + double time = minDist.second + + min(abs(minDist.first) / nvel1.Length(), abs(minDist.first) / vel2.Length()); + if (time != time) time = 0; + pair newPos(pos1 + nvel1 * (float)time, time); + // Discard points within obstacles + bool collision = false; + for (size_t i = 0; i < evalAgent->_nearObstacles.size(); ++i) { + Menge::Agents::NearObstacle obstaculo = evalAgent->_nearObstacles[i]; + const Obstacle* const obst = evalAgent->_nearObstacles[i].obstacle; + Vector2 nearPt; + float distSq; + Vector2 paso=nvel1; + paso.normalize(); + float distancia = (newPos.first - pos1).Length(); + if (distancia > 10) distancia = 10; + float mintowall = 200; + for (float a = 0; a < distancia; a = a + 0.1) { + Vector2 punto = pos1 + paso * a; + obst->distanceSqToPoint(punto, nearPt, distSq); + if (sqrt(distSq) < mintowall) mintowall = sqrt(distSq); + } + // Discard inside wall + float tiempo = obst->circleIntersection(nvel1, pos1, evalAgent->_radius); + obst->distanceSqToPoint(pos1 + nvel1 * tiempo, nearPt, distSq); + if ( tiempo < time) { + if (trace) + cout << "(" << evalAgent->_id << ") " + << "Discarded CIW point " << newPos.first << " inside obstacle" << endl; + collision = true; + break; + } + } + if (!collision) { + if (trace) + cout << "(" << evalAgent->_id << ") " + << "Accepted CIW point " << newPos.first << endl; + lista.push_back(newPos); + } + } + // Watch when no valid points + if (lista.empty()) { + if (trace) + cout << "(" << evalAgent->_id << ") " + << "No valid turn points" << endl; + // Add new points when following + if (isFollowing) { + if (dist21 - critDist > _followDist) { + // Approaching + if (trace) + cout << "(" << evalAgent->_id << ") " + << "New aproaching point " << endl; + Vector2 colPoint = pos1 + vel1 * ttca; + Vector2 colDist = colPoint - pos1; + float nspeed = (colDist.Length() - _followDist - critDist) / ttca; + Vector2 nVel = vel1; + nVel.normalize(); + nVel = nVel * nspeed; + if (trace) cout << "(" << evalAgent->_id << ") " << pos1 + nVel * ttca << endl; + lista.push_back(make_pair(pos1 + nVel * ttca, ttca)); + } else { + // Equalization + if (trace) + cout << "(" << evalAgent->_id << ") " + << "New equalization points " << endl; + lista.push_back(make_pair(bc.end, bc.length / vel2.Length())); + lista.push_back(make_pair(pos1 + vel2 * bc.time, bc.time)); + } + } else { + // Possible facing case. Not implemented + } + } + return lista; +} + +// Function to get the frecuency from the agent`s speed +int frecuency(const BaseAgent* evalAgent, double prefSpeed, double minMIT) { + double maxMIT = + (evalAgent->_neighborDist + evalAgent->_radius * 2) / (evalAgent->_maxSpeed + prefSpeed); + double freq = minPeriod + (minMIT * (maxPeriod - minPeriod) / maxMIT); + if (trace) + cout << "(" << evalAgent->_id << ") " + << " MaxMIT " << maxMIT << " freq " << freq << endl; + if (freq > maxPeriod) freq = maxPeriod; + return freq; +} + +// Function to get the cost of a segment +double costCalc(Segment ba, double prefSpeed) { + if (ba.length == 0) return 0; + double speed = ba.length / ba.time; + double ew = 2.23 / pow(prefSpeed, 2); + double cost = mass * (2.23 / speed + ew * speed) * ba.length; + return cost; +} + +// Function to get the path from the collision-free list +Vector2 MPModifier::buildPath(list collisionFreeList, const BaseAgent* evalAgent) { + Vector2 target = evalAgent->_velPref.getTarget(); + Segment sol = collisionFreeList.front(); + for (auto item : collisionFreeList) { + if (_trace) + cout << "(" << evalAgent->_id << ") " + << " Segment to process " << item.start << item.end << " time " << item.time << endl; + if (item.end != target) { + if (trace) + cout << "(" << evalAgent->_id << ") " + << " No match " << endl; + continue; + } + if (item.start == evalAgent->_pos) { + if (trace) + cout << "(" << evalAgent->_id << ") " + << "Found solution " << endl; + sol = item; + break; + } + target = item.start; + } + if (trace) + cout << "(" << evalAgent->_id << ") " + << " Solution segment " << sol.start << sol.end << " time " << sol.time << endl; + Vector2 newVel = sol.getOrient() * sol.length / (float)sol.time; + return newVel; +} + +// Function to check if a segment is used +bool usedSegment(Segment segment, vector usedSegs) { + for (int i = 0; i < usedSegs.size(); i++) { + if (usedSegs[i].equal(segment)) return true; + } + return false; +} + +///////////////////////////////////////////////////////////////////// + +// MAIN FUNCTION +void MPModifier::adaptPrefVelocity(const BaseAgent* agent, PrefVelocity& pVel) { + // Variables + const BaseAgent* evalAgent = agent; + const BaseAgent* collAgent = agent; + vector filteredNeighbours; + vector usedSegs; + double minMIT = 0; + double ttca; + + trace = _trace; + int maxMITneigh = _maxMITneigh; + if (trace) cout << "(" << agent->_id << ") " << endl + << "AGENT: " << agent->_id << " substep " << stepCount << " pos " << agent->_pos << " vel " + << agent->_vel << agent->_vel.Length() << endl; + incrementCounter(); + //return; + + // Plan remainder + + if (!isInTab(agent->_id)) { + writeOnTab(agent->_id, 0, agent->_vel); + if (trace) cout << "(" << agent->_id << ") " << " Inicialized with 0 steps" << endl; + return; + } else { + if (!zeroOnTab(agent->_id)) { + Vector2 sameVel = decrementOnTab(agent->_id); + Vector2 sameDir = sameVel; + sameDir.normalize(); + pVel.setSpeed(sameVel.Length()); + pVel.setSingle(sameDir); + if (trace) cout << "(" << agent->_id << ") " << " Remaining steps to actualization " << freqTab[agent->_id].first + 1 << endl; + return; + } + } + + // Condition to deactivate midplan when it is already close to the objective + if ((agent->_pos - agent->_velPref.getTarget()).Length() > 1.5) { + filteredNeighbours.clear(); + usedSegs.clear(); + double elapsedTime = 0.f; + Vector2 prefVel = pVel.getPreferredVel(); + Vector2 prefDir = prefVel; + prefDir.normalize(); + double prefSpeed = prefVel.Length(); + + + // define temporary vars + list collisionFreeList; + multimap checkList; + double collisionCost = mass * 2 * (2.23 / prefSpeed) * detourDistance; + + // EACS ALGORITHM + FilterNeighbours(evalAgent,filteredNeighbours, prefVel, minMIT, maxMITneigh); + if (trace) cout << "(" << agent->_id << ") " << "Filtered agents " << filteredNeighbours.size() << endl; + if (filteredNeighbours.size() == 0) return; + Segment sg(agent->_pos, agent->_velPref.getTarget(), 0, 0); + sg.time = sg.length / prefSpeed; + sg.cost = costCalc(sg, prefSpeed); + sg.collision = -1; + sg.expansions = 0; + usedSegs.push_back(sg); + checkList.emplace(sg.cost, sg); + bool pathFound = false; + while (!pathFound) { + if (trace) cout << "(" << agent->_id << ") " << "Checklist" << endl; + // If trace, print checklist + if (trace) { + for (auto segmento : checkList) { + cout << "(" << agent->_id << ") " + << " Segment (" << segmento.second.start << "," << segmento.second.end << ") cost " + << segmento.first << endl; + } + } + if (checkList.empty()) { + collisionFreeList.push_front(sg); + pathFound = true; + break; + } + Segment bc = checkList.begin()->second; + checkList.erase(checkList.begin()); + if (trace) cout << "(" << agent->_id << ") " << "Segment to check " << bc.start << "," << bc.end << endl; + + if (Collision(bc, evalAgent, collAgent, filteredNeighbours, ttca)) { + + // Check number of expansions + if (trace) cout << "(" << agent->_id << ") " << "Expansions: " << bc.expansions << endl; + if (bc.expansions >= 5) { + if (trace) cout << "(" << agent->_id << ") " << "No more expasions allowed" << endl; + continue; + } + // Check checklist length + if (checkList.size() > 30) { + if (trace) cout << "(" << agent->_id << ") " << "No more segments allowed" << endl; + continue; + } + // Check if the colliding agent is over the target point. + if ((collAgent->_pos - evalAgent->_velPref.getTarget()).Length() < (evalAgent->_radius + collAgent->_radius)) { + if (trace) cout << "(" << agent->_id << ") " << " Colliding agent on target point. Accept segment" << endl; + collisionFreeList.push_front(bc); + if (bc.end == sg.end) { + pathFound = true; + } else { + // Accept collision adding extracost + bc.cost = bc.cost + collisionCost; + Segment cg(bc.end, sg.end, bc.eTime + bc.time, bc.eCost + bc.cost); + cg.time = cg.length / prefSpeed; + cg.cost = costCalc(cg, prefSpeed); + cg.expansions = ++bc.expansions; + checkList.emplace(cg.eCost + cg.cost, cg); + } + continue; + } + // Check if the collision is with the same agent than origin of expanssion + if (collAgent->_id == bc.collision) { + // Collision with same agent + if (trace) cout << "(" << agent->_id << ") " << " Issue with same agent as before. Accept segment with extracost and continue" << endl; + collisionFreeList.push_front(bc); + if (bc.end == sg.end) { + pathFound = true; + } else { + // Accept collision adding extracost + bc.cost = bc.cost + collisionCost; + Segment cg(bc.end, sg.end, bc.eTime + bc.time, + bc.eCost + bc.cost); + cg.time = cg.length / prefSpeed; + cg.cost = costCalc(cg, prefSpeed); + cg.expansions = ++bc.expansions; + checkList.emplace(cg.eCost + cg.cost, cg); + } + } else { + // Collision with new agent + list> pointsList = ComputeCIW(bc, evalAgent, collAgent, ttca); + // If no valid points + if (pointsList.empty()) { + if (trace) cout << "(" << agent->_id << ") " << "No valid points. Create same segment with extracost" << endl; + bc.collision = collAgent->_id; + bc.cost = bc.cost + collisionCost; + checkList.emplace(bc.eCost + bc.cost, bc); + } else { + if (bc.expansions >= _maxExpansions) continue; + for (pair item : pointsList) { + Segment ba(bc.start, item.first, bc.eTime, bc.eCost); + if (usedSegment(ba, usedSegs)) continue; + ba.time = item.second; + ba.collision = collAgent->_id; + ba.cost = costCalc(ba, prefSpeed); + ba.expansions = bc.expansions; + Segment ag(ba.end, sg.end, ba.eTime + ba.time, ba.eCost + ba.cost); + ag.time = ag.length / prefSpeed; + ag.cost = costCalc(ag, prefSpeed); + usedSegs.emplace(usedSegs.begin(),ba); + if (trace) cout << "(" << agent->_id << ") " << "CIW" << item.first << " Cost to segment " << ag.eCost << " to G " << ag.cost + << " Total " << ag.eCost + ag.cost << endl; + checkList.emplace(ag.eCost + ag.cost, ba); + } + } + } + } else { + if (trace) cout << "(" << agent->_id << ") " << "-Free" << endl; + collisionFreeList.push_front(bc); + if (bc.end != sg.end) { + Segment cg(bc.end, sg.end, bc.eTime + bc.time, + bc.eCost + bc.cost); + cg.time = cg.length / prefSpeed; + cg.cost = costCalc(cg, prefSpeed); + cg.collision = bc.collision; + cg.expansions = ++bc.expansions; + checkList.emplace(cg.eCost + cg.cost, cg); + } else + pathFound = true; + } + } + Vector2 newVel = buildPath(collisionFreeList, evalAgent); + float newSpeed = newVel.Length(); + Vector2 newDir = newVel; + newDir.normalize(); + + pVel.setSpeed(newSpeed); + pVel.setSingle(newDir); + + int newPeriod = frecuency(evalAgent, prefSpeed, minMIT); + writeOnTab(agent->_id, newPeriod, newVel); + if (trace) cout << "(" << agent->_id << ") " << " New steps to actualization " << newPeriod + 1 << endl; + if (trace) cout << "(" << agent->_id << ") " << " NewVel" << newVel << " " << newSpeed << endl << endl; + } +} + +///////////////////////////////////////////////////////////////////// +// Implementation of MPModFactory +///////////////////////////////////////////////////////////////////// + +MPModifierFactory::MPModifierFactory() : VelModFactory() { + _followDistID = _attrSet.addFloatAttribute("follow_dist", false, 0.5f); + _maxExpansionsID = _attrSet.addFloatAttribute("max_expan", false, 5); + _maxMITneighID = _attrSet.addFloatAttribute("max_neigh", false, 5); + _traceID = _attrSet.addBoolAttribute("trace", false, false); +} + +///////////////////////////////////////////////////////////////////// + +bool MPModifierFactory::setFromXML(VelModifier* modifier, TiXmlElement* node, + const std::string& behaveFldr) const { + MPModifier* MPMod = dynamic_cast(modifier); + assert(MPMod != 0x0 && "Trying to set property modifier properties on an incompatible object"); + + if (!VelModFactory::setFromXML(modifier, node, behaveFldr)) return false; + + // set the params we need + MPMod->setFollowDist(_attrSet.getFloat(_followDistID)); + MPMod->setMaxExpansions(_attrSet.getFloat(_maxExpansionsID)); + MPMod->setMaxMITneigh(_attrSet.getFloat(_maxMITneighID)); + MPMod->setTrace(_attrSet.getBool(_traceID)); + return true; +} +} // namespace MPModifier diff --git a/src/Plugins/MidPlan/MidPlanModifier.h b/src/Plugins/MidPlan/MidPlanModifier.h new file mode 100644 index 00000000..9b1550bd --- /dev/null +++ b/src/Plugins/MidPlan/MidPlanModifier.h @@ -0,0 +1,278 @@ +/* + +Sergio Lozano Mateos +UNED, Spain + +*/ + +/*! + @file MidPlanModifier.h + @brief Definiton of a Velocity Modifier to EACS + + Uses the model presented in http://gamma.cs.unc.edu/DenseSense/. + */ +#ifndef __MPMODIFIER_MODIFIER_H__ +#define __MPMODIFIER_MODIFIER_H__ + +#include "MidPlanConfig.h" + +#include "MengeCore/BFSM/FSMEnumeration.h" +#include "MengeCore/BFSM/VelocityModifiers/VelModifier.h" +#include "MengeCore/BFSM/VelocityModifiers/VelModifierFactory.h" +#include "MengeCore/Runtime/SimpleLock.h" +#include "MengeCore/mengeCommon.h" +#include "Segment.h" + + +/*! + @namespace MPModifier + @brief The name space for the EACS adherence model + + This namespace contains a velocity modifier which varies preferred speed based on EACS algorithm + */ +namespace MPModifier { + +using Menge::Agents::BaseAgent; + + +class MPModifierFactory; + +/*! + @brief Velocity modifier that adapts EACS algorithm + + This is an implementation of the work EACS: Effective Avoidance Combination Strategy + J. Bruneau and J. Pettre + Inria Rennes, France + + To specify a Midplan modifier, use the following syntax: + + ```xml + + ``` + */ + +class MPMODIFIER_API MPModifier : public Menge::BFSM::VelModifier { + public: + + /*! + @brief Constructor. + */ + MPModifier(); + + /*! + @brief Constructor. + + @param followDist Distance of following + */ + MPModifier(float followDist, int maxExpansions, int maxMITneigh, bool trace); + + /*! + @brief Copy method for this velocity modifier. + */ + Menge::BFSM::VelModifier* copy() const; + + /*! + @brief Evaluate mid-term collision + */ + bool Collision(Segment bc, const BaseAgent* evalAgent, const BaseAgent* &collAgent, + vector filteredNeighbours, double &ttca); + + /*! + @brief Filter neighbours as EACS + @param bc vector a calcular + */ + void FilterNeighbours(const BaseAgent* evalAgent, vector &filteredNeighbours, Vector2 prefVel, double &minMIT, int maxMITneigh); + + /*! + @brief Compute the CIW points + */ + std::list> ComputeCIW(Segment bc, const BaseAgent* evalAgent, + const BaseAgent* collAgent, double ttca); + + /*! + @brief Build path from collisionFreeList + */ + Vector2 MPModifier::buildPath(list collisionFreeList, const BaseAgent* evalAgent); + + /*! + @brief Adapts the given agent's preferred velocity to adhere EACS. + + @param agent The agent on whom we are operating. + @param pVel The input preferred velocity of the agent. + */ + void adaptPrefVelocity(const Menge::Agents::BaseAgent* agent, Menge::Agents::PrefVelocity& pVel); + + + /*! + @brief Sets the following distance. + + @param followDist + */ + void setFollowDist(float followDist) { _followDist = followDist; } + + /*! + @brief Sets the max expansions number. + + @param followDist + */ + void setMaxExpansions(float maxExpansions) { _maxExpansions = maxExpansions; } + + /*! + @brief Sets the max neighbours number. + + @param followDist + */ + void setMaxMITneigh(float maxMITneigh) { _maxMITneigh = maxMITneigh; } + + /*! + @brief Sets the trace + + @param followDist + */ + void setTrace(bool trace) { _trace = trace; } + + friend class MPModifierFactory; + + protected: + /*! + @brief Defines the parameters which define the Midplan behavior. + */ + struct MPParam { + /*! + @brief Default constructor + */ + MPParam() : _followDist(0.5f), _maxExpansions(5), _maxMITneigh(5), _trace(false) {} + + /*! + @brief Constructor. + + @param followDist The following distance. + */ + MPParam(float distance, int maxExpansions, int maxMITneigh, bool trace) { + _followDist = distance; + _maxExpansions = maxExpansions; + _maxMITneigh = maxMITneigh; + _trace = trace; + } + + /*! + @brief Params + */ + float _followDist; + int _maxExpansions; + int _maxMITneigh; + bool _trace; + ; + }; + + /*! + @brief The readers-writer lock to preserve thread-safety on _strideParams. + */ + Menge::SimpleLock _paramLock; + + /*! + @brief The following distance + */ + float _followDist; + /*! + @brief The max expansions number + */ + int _maxExpansions; + /*! + @brief The max neighbours number in MIT + */ + int _maxMITneigh; + /*! + @brief If the trace should be shown + */ + bool _trace; +}; + +//////////////////////////////////////////////////////////////////////// + +/*! + @brief The factory for the MPModifier class. + */ +class MPMODIFIER_API MPModifierFactory : public Menge::BFSM::VelModFactory { + public: + /*! + @brief Constructor. + */ + MPModifierFactory(); + + /*! + @brief The name of the modifier + + The modifier's name must be unique among all registered modifier. + Each modifier factory must override this function. + + @returns A string containing the unique modifier name. + */ + virtual const char* name() const { return "midterm_planning"; } + + /*! + @brief A description of the modifier + + Each modifier factory must override this function. + + @returns A string containing the modifier description. + */ + virtual const char* description() const { + return "Adjusts the agent's preferred speed to adhere to the EACS "; + }; + + protected: + /*! + @brief Create an instance of this class's modifier. + + All modifierFactory sub-classes must override this by creating (on the heap) a new instance of + its corresponding modifier type. The various field values of the instance will be set in a + subsequent call to modifierFactory::setFromXML(). The caller of this function takes ownership of + the memory. + + @returns A pointer to a newly instantiated modifier class. + */ + Menge::BFSM::VelModifier* instance() const { return new MPModifier(); } + + /*! + @brief Given a pointer to an modifier instance, sets the appropriate fields from the provided + XML node. + + It is assumed that the value of the `type` attribute is this modifier's type (i.e. + modifierFactory::thisFactory() has already been called and returned true). If sub-classes of + modifierFactory introduce *new* modifier parameters, then the sub-class should override this + method but explicitly call the parent class's version. + + @param modifier A pointer to the modifier whose attributes are to be set. + @param node The XML node containing the modifier attributes. + @param behaveFldr The path to the behavior file. If the modifier references resources in the + file system, it should be defined relative to the behavior file location. + This is the folder containing that path. + @returns True if successful. + */ + virtual bool setFromXML(Menge::BFSM::VelModifier* modifier, TiXmlElement* node, + const std::string& behaveFldr) const; + + /*! + @brief The identifier for the "follow_distance" float attribute + */ + size_t _followDistID; + /*! + @brief The identifier for the "max_expan" int attribute + */ + size_t _maxExpansionsID; + /*! + @brief The identifier for the "max_neigh" int attribute + */ + size_t _maxMITneighID; + /*! + @brief The identifier for the "trace" bool attribute + */ + size_t _traceID; +}; +} // namespace MPModifier +#endif // __MPMODIFIER_MODIFIER_H__ diff --git a/src/Plugins/MidPlan/Segment.h b/src/Plugins/MidPlan/Segment.h new file mode 100644 index 00000000..dc64cded --- /dev/null +++ b/src/Plugins/MidPlan/Segment.h @@ -0,0 +1,91 @@ +/* + +Sergio Lozano Mateos +UNED, Spain + +*/ + +#ifndef SEGMENT_H +#define SEGMENT_H + +#include "MengeCore/Math/Vector2.h" +#include +#include +using namespace std; + +using Menge::Math::Vector2; + +// Segment struct used in calculus +struct Segment { + Vector2 start; + Vector2 end; + float length; + double time; + double eTime; + double cost; + double eCost; + int collision; + int expansions; + + /*! + @brief Segment constructor. + */ + Segment(); + /*! + @brief Segment constructor. + + @param instart The start point of the vector. + @param inend The end point of the vector. + @param intime The time to travel the segment. + @param incost The cost of the segment. + */ + Segment(Vector2 instart, Vector2 inend, double intime, double incost) { + start = instart; + end = inend; + length = (end - start).Length(); + eTime = intime; + eCost = incost; + } + + /*! + @brief Get orientation. + */ + Vector2 getOrient() { + Vector2 seg = end - start; + seg.normalize(); + return seg; + } + + /*! + @brief Get vector. + */ + Vector2 getVector() { + Vector2 vec = end - start; + return vec; + } + + /*! + @brief Get pair of points + */ + pair getVectors() { + Vector2 vec = end - start; + return make_pair(start, end); + } + + /*! + @brief Check if the segment has the same points than other + @param Segment + */ + bool equal(Segment ab) { + //cout << "Comparacion " << start << end << " " << ab.start << ab.end; + if ((abs(start._x - ab.start._x) < 0.001f) && (abs(start._y - ab.start._y) < 0.001f) && + (abs(end._x - ab.end._x) < 0.001f) && (abs(end._y - ab.end._y) < 0.001f)) { + //cout << " Iguales " << endl; + return true; + } + //cout << " Diferentes " << endl; + return false; + + } +}; +#endif