Commit 6d8fe37c authored by Ulrich Kemloh's avatar Ulrich Kemloh
Browse files

Add a Fallback when a route cannot be found. The issue still need to be

solved
parent 4f9d69a1
......@@ -568,19 +568,19 @@ void Simulation::UpdateFlowAtDoors(const Pedestrian& ped) const
{
trans->IncreaseDoorUsage(1, ped.GetGlobalTime());
}
if(ped.GetExitIndex()==0)
{
cout<<"exi 1t: "<<endl;exit(0);
}
if(trans->GetUniqueID()==0)
{
cout<<"exit 2: "<<endl;exit(0);
}
if(trans->GetID()==0)
{
cout<<"exit 3: "<<endl;exit(0);
}
// if(ped.GetExitIndex()==0)
// {
// cout<<"exi 1t: "<<endl;exit(0);
// }
//
// if(trans->GetUniqueID()==0)
// {
// cout<<"exit 2: "<<endl;exit(0);
// }
// if(trans->GetID()==0)
// {
// cout<<"exit 3: "<<endl;exit(0);
// }
}
}
......
......@@ -61,7 +61,7 @@
#endif
// precision error
#define J_EPS 0.001
#define J_EPS 0.001//0.001
#define J_EPS_EVENT 0.00001 //zum pruefen des aktuellen Zeitschrittes auf events
#define J_EPS_DIST 0.05// [m]
......
......@@ -15,7 +15,7 @@
<vertex px="0.0" py="4.0" />
<vertex px="0.0" py="3.0" />
</polygon>
<obstacle id="0">
<obstacle_ id="0">
<polygon>
<vertex px="1" py="3"></vertex>
<vertex px="2" py="3"></vertex>
......@@ -23,7 +23,7 @@
<vertex px="1" py="2"></vertex>
<vertex px="1" py="3"></vertex>
</polygon>
</obstacle>
</obstacle_>
</subroom>
<subroom id="1" class="stair" A_x="-1.2" B_y="0" C="0">
......
......@@ -22,27 +22,27 @@ xsi:noNamespaceSchemaLocation="../../xsd/jps_ini_core.xsd">
</traffic_constraints>
<routing>
<goals>
<goal id="0" final="true" caption="goal 0">
<polygon>
<vertex px="0.0" py="-5.0" />
<vertex px="2.0" py="-5.0" />
<vertex px="2.0" py="-7.0" />
<vertex px="0.0" py="-7.0" />
<vertex px="0.0" py="-5.0" />
</polygon>
</goal>
</goals>
<!-- <goals> -->
<!-- <goal id="0" final="true" caption="goal 0"> -->
<!-- <polygon> -->
<!-- <vertex px="0.0" py="-5.0" /> -->
<!-- <vertex px="2.0" py="-5.0" /> -->
<!-- <vertex px="2.0" py="-7.0" /> -->
<!-- <vertex px="0.0" py="-7.0" /> -->
<!-- <vertex px="0.0" py="-5.0" /> -->
<!-- </polygon> -->
<!-- </goal> -->
<!-- </goals> -->
</routing>
<!--persons information and distribution -->
<agents operational_model_id="2">
<agents_distribution>
<group group_id="3" agent_parameter_id="1" room_id="0" subroom_id="0" number="20" goal_id="-1" router_id="1" />
<group group_id="1" agent_parameter_id="1" room_id="0" subroom_id="0" number="10" goal_id="-1" router_id="1" />
<group group_id="2" agent_parameter_id="1" room_id="0" subroom_id="0" number="10" goal_id="-1" router_id="1"
pre_movement_mean="15" pre_movement_sigma="0.6" />
<group group_id="0" agent_parameter_id="1" room_id="0" subroom_id="0" number="10" goal_id="-1" router_id="1" />
<group group_id="1" agent_parameter_id="1" room_id="0" subroom_id="0" number="20" goal_id="-1" router_id="1" />
<group group_id="2" agent_parameter_id="1" room_id="0" subroom_id="0" number="20" goal_id="-1" router_id="1"
pre_movement_mean="-15" pre_movement_sigma="0.6" />
</agents_distribution>
<!-- frequency in persons/minute -->
......@@ -97,14 +97,16 @@ xsi:noNamespaceSchemaLocation="../../xsd/jps_ini_core.xsd">
<route_choice_models>
<router router_id="1" description="global_shortest">
<parameters>
<navigation_lines file="routing.xml" />
</parameters>
</router>
<router router_id="2" description="quickest">
<parameters>
<navigation_lines file="routing.xml" />
<!-- <navigation_lines file="routing.xml" /> -->
<navigation_mesh use_for_local_planning="false" method="triangulation"/>
</parameters>
</router>
<!-- <router router_id="2" description="quickest"> -->
<!-- <parameters> -->
<!-- <navigation_lines file="routing.xml" /> -->
<!-- <navigation_mesh use_for_local_planning="true" method="triangulation"/> -->
<!-- </parameters> -->
<!-- </router> -->
</route_choice_models>
......
......@@ -3,6 +3,16 @@
<routing version="0.5"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="http://134.94.2.137/jps_routing.xsd" >
<Hlines>
<Hlines>
<!-- <Hline id="1" room_id="1" subroom_id="2"> -->
<!-- <vertex px="25" py="1" /> -->
<!-- <vertex px="27" py="1" /> -->
<!-- </Hline> -->
<!-- <Hline id="2" room_id="1" subroom_id="2"> -->
<!-- <vertex px="-5" py="1" /> -->
<!-- <vertex px="-7" py="1" /> -->
<!-- </Hline> -->
</Hlines>
</routing>
......@@ -49,7 +49,7 @@
using namespace std;
GlobalRouter::GlobalRouter() :
Router()
Router()
{
_accessPoints = map<int, AccessPoint*>();
_map_id_to_index = std::map<int, int>();
......@@ -260,8 +260,9 @@ bool GlobalRouter::Init(Building* building)
auto&& sub=it_sub.second;
//maybe truncate the elevation.
// because using a double as key to map is not exact
//double elevation = ceilf(sub->GetMaxElevation() * 100) / 100;
_subroomsAtElevation[sub->GetMaxElevation()].push_back(sub.get());
//double elevation = ceilf(sub->GetMaxElevation() * 100) / 100;
//_subroomsAtElevation[elevation].push_back(sub.get());
_subroomsAtElevation[sub->GetElevation(sub->GetCentroid())].push_back(sub.get());
}
}
......@@ -316,7 +317,11 @@ bool GlobalRouter::Init(Building* building)
//vector<SubRoom*> emptyVector;
//emptyVector.push_back(sub.get());
//add all subrooms at the same elevation
double elevation = sub->GetMaxElevation();
//double elevation = sub->GetMaxElevation();
double elevation = sub->GetElevation(sub->GetCentroid());
// special case for stairs and for convex rooms
//if()
if (building->IsVisible(nav1->GetCentre(), nav2->GetCentre(), _subroomsAtElevation[elevation],true))
{
......@@ -449,7 +454,7 @@ bool GlobalRouter::Init(Building* building)
" \tYou can solve this by enabling triangulation.",
from_AP->GetFriendlyName().c_str());
from_AP->Dump();
//return false;
return false;
}
}
_tmpPedPath.clear();
......@@ -611,59 +616,6 @@ bool GlobalRouter::GetPath(Pedestrian* ped, std::vector<NavLine*>& path)
return true;
//collect and return the navigation lines
// do
// {
// SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
// ped->GetSubRoomID());
//
// const vector<int>& accessPointsInSubRoom = sub->GetAllGoalIDs();
// for (unsigned int i = 0; i < accessPointsInSubRoom.size(); i++) {
//
// int apID = accessPointsInSubRoom[i];
// AccessPoint* ap = _accessPoints[apID];
//
// const Point& pt3 = ped->GetPos();
// double distToExit = ap->GetNavLine()->DistTo(pt3);
//
// if (distToExit > J_EPS_DIST)
// continue;
//
// //one AP is near actualize destination:
// nextDestination = ap->GetNearestTransitAPTO(
// ped->GetFinalDestination());
//
//
// if (nextDestination == -1) { // we are almost at the exit
// return ped->GetNextDestination();
// } else {
// //check that the next destination is in the actual room of the pedestrian
// if (_accessPoints[nextDestination]->isInRange(
// sub->GetUID())==false) {
// //return the last destination if defined
// int previousDestination = ped->GetNextDestination();
//
// //we are still somewhere in the initialization phase
// if (previousDestination == -1) {
// ped->SetExitIndex(apID);
// ped->SetExitLine(_accessPoints[apID]->GetNavLine());
// return apID;
// } else { // we are still having a valid destination, don't change
// return previousDestination;
// }
// } else { // we have reached the new room
// ped->SetExitIndex(nextDestination);
// ped->SetExitLine(
// _accessPoints[nextDestination]->GetNavLine());
// return nextDestination;
// }
// }
// }
//
// // still have a valid destination, so return it
// return nextDestination;
// }while (!done);
}
bool GlobalRouter::GetPath(Pedestrian*ped, int goalID, std::vector<SubRoom*>& path)
......@@ -758,13 +710,15 @@ int GlobalRouter::FindExit(Pedestrian* ped)
{
std::vector<NavLine*> path;
GetPath(ped,path);
SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
ped->GetSubRoomID());
//return the next path which is an exit
for(const auto & navLine: path)
{
//TODO: only set if the pedestrian is already in the subroom.
// cuz all lines are returned
if(IsCrossing(*navLine) || IsTransition(*navLine))
if(IsCrossing(*navLine,{sub}) || IsTransition(*navLine,{sub}))
{
int nav_id= navLine->GetUniqueID();
ped->SetExitIndex(nav_id);
......@@ -863,6 +817,7 @@ int GlobalRouter::GetBestDefaultRandomExit(Pedestrian* ped)
int bestAPsID = -1;
double minDistGlobal = FLT_MAX;
double minDistLocal = FLT_MAX;
// get the opened exits
SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
ped->GetSubRoomID());
......@@ -886,7 +841,7 @@ int GlobalRouter::GetBestDefaultRandomExit(Pedestrian* ped)
//check if visible
//only if the room is convex
//otherwise check all rooms at that level
if(_building->IsVisible(posA, posC, _subroomsAtElevation[sub->GetMaxElevation()],true)==false)
if(_building->IsVisible(posA, posC, _subroomsAtElevation[sub->GetElevation(sub->GetCentroid())],true)==false)
{
ped->RerouteIn(10);
continue;
......@@ -899,15 +854,15 @@ int GlobalRouter::GetBestDefaultRandomExit(Pedestrian* ped)
// if two doors are feasible to the final destination without much differences
// in the distances, then the nearest is preferred.
if(( (dist-minDistGlobal) / (dist+minDistGlobal)) < CBA_THRESHOLD)
{
if (dist2 < minDistLocal) {
bestAPsID = ap->GetID();
minDistGlobal = dist;
minDistLocal= dist2;
}
}
else
if(( (dist-minDistGlobal) / (dist+minDistGlobal)) < CBA_THRESHOLD)
{
if (dist2 < minDistLocal) {
bestAPsID = ap->GetID();
minDistGlobal = dist;
minDistLocal= dist2;
}
}
else
{
if (dist < minDistGlobal) {
......@@ -932,6 +887,15 @@ int GlobalRouter::GetBestDefaultRandomExit(Pedestrian* ped)
"located in room [%d] subroom [%d] going to destination [%d]",
ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID(),
ped->GetFinalDestination());
//FIXME: assign the nearest and not only a random one
{
relevantAPs[0]->GetID();
ped->SetExitIndex(relevantAPs[0]->GetID());
ped->SetExitLine(relevantAPs[0]->GetNavLine());
ped->RerouteIn(5);
return relevantAPs[0]->GetID();
}
return -1;
}
}
......@@ -968,7 +932,6 @@ void GlobalRouter::GetRelevantRoutesTofinalDestination(Pedestrian *ped, vector<A
relevantAPS.push_back(ap);
}
}
}
//quick fix for extra hlines
// it should be safe now to delete the first preceding if block
......@@ -1023,6 +986,19 @@ void GlobalRouter::GetRelevantRoutesTofinalDestination(Pedestrian *ped, vector<A
}
}
}
//fallback
if(relevantAPS.size()==0)
{
//fixme: this should also never happened. But hapen due to previvous bugs..
const vector<int>& goals=sub->GetAllGoalIDs();
for(unsigned int g1=0; g1<goals.size(); g1++)
{
relevantAPS.push_back(_accessPoints[goals[g1]]);
}
}
}
void GlobalRouter::WriteGraphGV(string filename, int finalDestination,
......@@ -1186,11 +1162,30 @@ void GlobalRouter::TriangulateGeometry()
auto&& subroom=itr_subroom.second;
auto&& room=itr_room.second;
auto&& obstacles=subroom->GetAllObstacles();
if(subroom->IsAccessible()==false) continue;
//Triangulate if obstacle or concave and no hlines ?
//if(subroom->GetAllHlines().size()==0)
if((obstacles.size()>0 ) || (subroom->IsConvex()==false ))
{
// DTriangulation* tri= new DTriangulation();
// auto outerhull=subroom->GetPolygon();
// if(subroom->IsClockwise())
// std::reverse(outerhull.begin(), outerhull.end());
//
// tri->SetOuterPolygone(outerhull);
//
// for (const auto & obst: obstacles)
// {
// auto outerhullObst=obst->GetPolygon();
// if(obst->IsClockwise())
// std::reverse(outerhullObst.begin(), outerhullObst.end());
// tri->AddHole(outerhullObst);
// }
// tri->Triangulate();
// vector<p2t::Triangle*> triangles=tri->GetTriangles();
vector<p2t::Triangle*> triangles=subroom->GetTriangles();
for (const auto & tr: triangles)
......@@ -1214,8 +1209,8 @@ void GlobalRouter::TriangulateGeometry()
if (MinAngle(P0, P1, P2) < _minAngleInTriangles)
continue;
if((IsWall(line)==false) && (IsCrossing(line)==false)
&& (IsTransition(line)==false) && (IsHline(line)==false))
if((IsWall(line,{subroom.get()})==false) && (IsCrossing(line,{subroom.get()})==false)
&& (IsTransition(line,{subroom.get()})==false) && (IsHline(line,{subroom.get()})==false))
{
//add as a Hline
int id=_building->GetAllHlines().size();
......@@ -1480,57 +1475,67 @@ bool GlobalRouter::LoadRoutingInfos(const std::string &filename)
return true;
}
bool GlobalRouter::IsWall(const Line& line) const
bool GlobalRouter::IsWall(const Line& line, const std::vector<SubRoom*>& subrooms) const
{
for(auto&& itr_room: _building->GetAllRooms())
//for(auto&& itr_room: _building->GetAllRooms())
//{
for(auto&& subroom: subrooms)
{
for(auto&& itr_subroom: itr_room.second->GetAllSubRooms())
for (auto&& obst: subroom->GetAllObstacles())
{
for (auto&& obst: itr_subroom.second->GetAllObstacles())
{
for (auto&& wall:obst->GetAllWalls())
{
if(line.operator ==(wall))
return true;
}
}
for (auto&& wall:itr_subroom.second->GetAllWalls())
for (auto&& wall:obst->GetAllWalls())
{
if(line.operator ==(wall))
return true;
}
}
for (auto&& wall:subroom->GetAllWalls())
{
if(line.operator ==(wall))
return true;
}
}
//}
return false;
}
bool GlobalRouter::IsCrossing(const Line& line) const
bool GlobalRouter::IsCrossing(const Line& line, const std::vector<SubRoom*>& subrooms) const
{
for (const auto & crossing : _building->GetAllCrossings())
for(auto&& subroom: subrooms)
{
if (crossing.second->operator ==(line))
return true;
for (const auto & crossing : subroom->GetAllCrossings())
{
if (crossing->operator ==(line))
return true;
}
}
return false;
}
bool GlobalRouter::IsTransition(const Line& line) const
bool GlobalRouter::IsTransition(const Line& line, const std::vector<SubRoom*>& subrooms) const
{
for(const auto & transition: _building->GetAllTransitions())
for(auto&& subroom: subrooms)
{
if (transition.second->operator ==(line))
return true;
for(const auto & transition: subroom->GetAllTransitions())
{
if (transition->operator ==(line))
return true;
}
}
return false;
}
bool GlobalRouter::IsHline(const Line& line) const
bool GlobalRouter::IsHline(const Line& line, const std::vector<SubRoom*>& subrooms) const
{
for(const auto & hline: _building->GetAllHlines())
for(auto&& subroom: subrooms)
{
if (hline.second->operator ==(line))
return true;
for(const auto & hline: subroom->GetAllHlines())
{
if (hline->operator ==(line))
return true;
}
}
return false;
}
......
......@@ -172,22 +172,22 @@ private:
/**
* @return true if the supplied line is a wall.
*/
bool IsWall(const Line& line) const;
bool IsWall(const Line& line, const std::vector<SubRoom*>& subrooms) const;
/**
* @return true if the supplied line is a Crossing.
*/
bool IsCrossing(const Line& line) const;
bool IsCrossing(const Line& line, const std::vector<SubRoom*>& subrooms) const;
/**
* @return true if the supplied line is a Transition.
*/
bool IsTransition(const Line& line) const;
bool IsTransition(const Line& line, const std::vector<SubRoom*>& subrooms) const;
/**
* @return true if the supplied line is a navigation line.
*/
bool IsHline(const Line& line) const;
bool IsHline(const Line& line, const std::vector<SubRoom*>& subrooms) const;
/**
* @return the minimum distance between the point and any line in the subroom.
......
......@@ -843,7 +843,7 @@ int QuickestPathRouter::GetBestDefaultRandomExit(Pedestrian* ped)
//check if visible
if(_building->IsVisible(posA, posC, _subroomsAtElevation[sub->GetMaxElevation()],true)==false)
if(_building->IsVisible(posA, posC, _subroomsAtElevation[sub->GetElevation(sub->GetCentroid())],true)==false)
//if (sub->IsVisible(posA, posC, true) == false)
{
ped->RerouteIn(10);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment