diff --git a/math/ORCAModel.cpp b/math/ORCAModel.cpp
index 911bb4b23c0f5067d5700fe7aa0fa005f7713f91..8cf37f3e7b53b8d382e30a6b80ee951157ea719f 100644
--- a/math/ORCAModel.cpp
+++ b/math/ORCAModel.cpp
@@ -44,8 +44,6 @@
#include <boost/geometry/geometries/register/point.hpp>
#include <iostream>
-//typedef boost::geometry::model::d2::point_xy<double> point_2d;
-
BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, cs::cartesian, _x, _y)
BOOST_GEOMETRY_REGISTER_RING(std::vector<Point>)
@@ -56,11 +54,10 @@ ORCAModel :: ORCAModel(DirectionStrategy* dir) {
_direction = dir;
_Origin = Point();
_Center = Point();
- _tau = 2.0;
+ _tau = 3;
_pedRad = 0;
_coneRad = 0;
- //_intersect_FLAG = 0;
}
ORCAModel :: ~ORCAModel() {}
@@ -108,6 +105,9 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
for (auto Ped : allPeds)
{
+ if (Ped->GetV0Norm() == 0)
+ continue;
+
{ // Create a bounding box for the halfplanes
_Origin = Ped->GetPos();
JEllipse elpsA = Ped->GetEllipse();
@@ -122,35 +122,31 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
_Margin[NMARGIN] = Line(Point(x_min, y_max), Point(x_max, y_max));
_Margin[SMARGIN] = Line(Point(x_min, y_min), Point(x_max, y_min));
}
- //Point Vel = Ped->GetV(); // check which is the right current ped velocity v or v0
- // get neighbors
+
vector <Pedestrian*> neighbourPed;
building->GetGrid()->GetNeighbourhood(Ped,neighbourPed);
- bool intersect_FLAG = true;
- //int count = 0;
SubRoom* subroom = building->GetRoom(Ped->GetRoomID())->GetSubRoom(Ped->GetSubRoomID());
Room* room = building->GetRoom(Ped->GetRoomID());
- /*for (Pedestrian* ped1 : neighbourPed)
- {
- Point p2 = ped1->GetPos();
+
+ vector<Line> halfPlane; // Point1 is Line Location, Point2 is unit normal to the line
+ for (auto nPed : neighbourPed) {
+ Point p2 = nPed->GetPos();
std::vector<SubRoom*> emptyVector;
emptyVector.push_back(subroom);
- emptyVector.push_back(building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID()));
+ emptyVector.push_back(building->GetRoom(nPed->GetRoomID())->GetSubRoom(nPed->GetSubRoomID()));
bool isVisible = building->IsVisible(_Origin, p2, emptyVector, false);
if (!isVisible)
- //neighbourPed.erase(neighbourPed.begin() + count);
continue;
- //++count;
- }*/
- vector<Line> halfPlane; // Point1 is Line Location, Point2 is unit normal to the line
- for (auto nPed : neighbourPed) {
+
_Center = (nPed->GetPos() - _Origin) / _tau;
JEllipse elpsB = nPed->GetEllipse();
double maxB = (elpsB.GetEA() > elpsB.GetEB()) ? elpsB.GetEA() : elpsB.GetEB();
- _coneRad = (_pedRad + maxB / _tau) * 0.85;
- halfPlane.push_back(ComputeHalfPlane(Ped, nPed));
+ _coneRad = (_pedRad + maxB / _tau);
+ Line hPlane = ComputeHalfPlane(Ped, nPed);
+ if (hPlane != Line())
+ halfPlane.push_back(ComputeHalfPlane(Ped, nPed));
}
{
const vector<Wall>& Walls = subroom->GetAllWalls();
@@ -160,25 +156,22 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
vector<Line> tmp = ComputeHalfPlane(Walls);
halfPlane.insert(halfPlane.end(), tmp.begin(), tmp.end());
}
- std::cout << "ID : " << Ped->GetID() << " = hplaneSize is " << halfPlane.size() << std::endl;
+
//TODO: haplfPlane for walls & obstacles
- Point Pref_V = e0(Ped, room) * Ped->GetV0Norm();
- Point vel = LinearProgram(halfPlane, Pref_V, intersect_FLAG);
+ bool intersect_FLAG = true;
+ Point Pref_V = _Origin + e0(Ped, room) * Ped->GetV0Norm();
+ //Point Pref_V = e0(Ped, room) * Ped->GetV0Norm();
+ //Point vel = LinearProgram(halfPlane, Pref_V, intersect_FLAG);
//Point vel_norm = (vel - _Origin) / (_Origin - vel).Norm();
- Point vel_norm = (_Origin - vel) / (_Origin - vel).Norm();
- std::cout << "ID : " << Ped->GetID() << " = interbool is " << intersect_FLAG << std::endl;
-
- std::cout<<"Ped Pos = " << _Origin.toString() << std::endl;
- std::cout<<"current vel = " << Ped->GetV().toString() << std::endl;
- const Point& target = _direction->GetTarget(room, Ped);
- std::cout<<"target = " << target.toString() << std::endl;
- std::cout<<"pref vel = " << Pref_V.toString() << std::endl;
- std::cout<<"new vel = " << (vel_norm * Ped->GetV0Norm()).toString()<< std::endl;
- std::cout << "--------------------------------------------------" << std::endl;
+ std::cout << " --------------------------" << std::endl << "Ped id : " << Ped->GetID() << std::endl;
+ std::cout << " pedA vel = " << _Origin.toString() << std::endl;
+ Point vel_norm = LinearProgram(halfPlane, Pref_V, intersect_FLAG);
+
//if (intersect_FLAG == true) {
//Point vel_new = (vel - _Origin) / (vel - _Origin).Norm() * Ped->GetV0Norm();
Ped->SetPos(_Origin + vel_norm * Ped->GetV0Norm() * deltaT );
- Ped->SetV(vel_norm * Ped->GetV0Norm());
+ std::cout << "next pos = " << (_Origin + vel_norm * Ped->GetV0Norm() * deltaT ).toString() << std::endl;
+ Ped->SetV(vel_norm);
Ped->SetPhiPed();
//}
/*else { //ped stays put.
@@ -222,23 +215,35 @@ Line ORCAModel :: ComputeHalfPlane(const Pedestrian* PedA, const Pedestrian* nPe
Point uVector; //
Point nVector; // outward normal to the boundary
int dirFlag = CheckContainsInCone(VelCone1, VelCone2, VelAB); //TODO: implement the function return +1 for inside -1 for outside
+ //if (dirFlag == 1) {
+ //TODO: if uVector is 0, nVector??
+ if (shrtDist2Cone1 <= shrtDist2Curve && shrtDist2Cone1 <= shrtDist2Cone2) {
+ uVector = VelCone1.ShortestPoint(VelAB);
+ nVector = (uVector - VelAB) / shrtDist2Cone1; // outward normal to the boundary
+ if (dirFlag == -1)
+ return Line(VelAB, nVector * dirFlag);
+ else
+ return Line(VelAB + nVector * shrtDist2Cone1 / 2, nVector * dirFlag);
+ }
+ else if (shrtDist2Cone2 <= shrtDist2Curve) {
+ uVector = VelCone2.ShortestPoint(VelAB);
+ nVector = (uVector - VelAB) / shrtDist2Cone2; // outward normal to the boundary
+ if (dirFlag == -1)
+ return Line(VelAB, nVector * dirFlag);
+ else
+
+ return Line(VelAB + nVector * shrtDist2Cone2 / 2, nVector * dirFlag);
+ }
+ else {
+ uVector = nearPt2Curve;
+ nVector = uVector / shrtDist2Curve; // outward normal to the boundary
+ if (dirFlag == -1)
+ return Line(VelAB, nVector * dirFlag);
+ else
+ return Line(VelAB + nVector * shrtDist2Curve / 2, nVector * dirFlag);
+ }
+ //}
- //TODO: if uVector is 0, nVector??
- if (shrtDist2Cone1 <= shrtDist2Curve && shrtDist2Cone1 <= shrtDist2Cone2) {
- uVector = VelCone1.ShortestPoint(VelAB);
- nVector = (uVector - VelAB) / shrtDist2Cone1; // outward normal to the boundary
- return Line(VelAB + nVector * shrtDist2Cone1 / 2, nVector * dirFlag);
- }
- else if (shrtDist2Cone2 <= shrtDist2Curve) {
- uVector = VelCone2.ShortestPoint(VelAB);
- nVector = (uVector - VelAB) / shrtDist2Cone2; // outward normal to the boundary
- return Line(VelAB + nVector * shrtDist2Cone2 / 2, nVector * dirFlag);
- }
- else {
- uVector = nearPt2Curve;
- nVector = uVector / shrtDist2Curve; // outward normal to the boundary
- return Line(VelAB + nVector * shrtDist2Curve / 2, nVector * dirFlag);
- }
}
std::vector<Line> ORCAModel :: ComputeHalfPlane(const std::vector<Wall>& Walls) const{
@@ -246,8 +251,8 @@ std::vector<Line> ORCAModel :: ComputeHalfPlane(const std::vector<Wall>& Walls)
for(const auto &wall: Walls) {
Line velCone = wall.Enlarge(float(1.0) / _tau);
double dist2Wall = velCone.DistTo(_Origin) - _pedRad;
- double dist2Cir1 = (velCone.GetPoint1() - _Origin).Norm() - _pedRad;
- double dist2Cir2 = (velCone.GetPoint2() - _Origin).Norm() - _pedRad;
+ double dist2Cir1 = (velCone.GetPoint1() - _Origin).Norm() - _pedRad * 1.5;
+ double dist2Cir2 = (velCone.GetPoint2() - _Origin).Norm() - _pedRad * 1.5;
if (dist2Wall < dist2Cir1 && dist2Wall < dist2Cir2) {
Point loc = velCone.ShortestPoint(_Origin);
Point Norm = (_Origin - loc) / (_Origin - loc).Norm();
@@ -319,78 +324,53 @@ Point ORCAModel :: NearestPointOnCurve(const Line& tanPoints, const Point& VelAB
std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>& hPlane, bool &intersect_FLAG) const{
- if (hPlane.size() == 1) {
- Point temp;
- Point norm = hPlane[0].GetPoint2();
- std::vector<Point> Poly;
- Line HP_1(hPlane[0].GetPoint1() + norm.Rotate(0,1) * 50, hPlane[0].GetPoint1() - norm.Rotate(0,1) * 50);
- for (int i = EMARGIN, count = 0; i < 4; ++i) {
- int flag = HP_1.IntersectionWith(_Margin[i], temp);
- if (flag == INTERSECTION) {
- Poly.push_back(temp);
- count++;
+ if (intersect_FLAG == true) {
+ if (hPlane.size() == 1) {
+ Point temp;
+ Point norm = hPlane[0].GetPoint2();
+ std::vector<Point> Poly;
+ Line HP_1(hPlane[0].GetPoint1() + norm.Rotate(0,1) * 50, hPlane[0].GetPoint1() - norm.Rotate(0,1) * 50);
+ for (int i = EMARGIN, count = 0; i < 4; ++i) {
+ int flag = HP_1.IntersectionWith(_Margin[i], temp);
+ if (flag == INTERSECTION) {
+ Poly.push_back(temp);
+ count++;
+ }
+ if (count == 2) break;
}
- std::cout << "count :: " << count << std::endl;
- if (count == 2) break;
- }
- for (int i = EMARGIN; i < 4; ++i) {
- if ((_Margin[i].GetPoint1() - hPlane[0].GetPoint1()).ScalarProduct(hPlane[0].GetPoint2()) >= 0)
- Poly.push_back(_Margin[i].GetPoint1());
- std::cout << "Poly size " << Poly.size() << std::endl;
- if((_Margin[i].GetPoint2() - hPlane[0].GetPoint1()).ScalarProduct(hPlane[0].GetPoint2()) >= 0)
- Poly.push_back(_Margin[i].GetPoint2());
- std::cout << "Poly size " << Poly.size() << std::endl;
- }
+ for (int i = EMARGIN; i < 4; ++i) {
+ if ((_Margin[i].GetPoint1() - hPlane[0].GetPoint1()).ScalarProduct(hPlane[0].GetPoint2()) >= 0)
+ Poly.push_back(_Margin[i].GetPoint1());
- std::vector<Point> result;
- bg::convex_hull(Poly, result);
- std::cout << "Poly size " << result.size() << " return convPolintersect" << std::endl;
- std :: cout << "-----------------convex POLY intersect ------------------------" << std :: endl;
- for (auto it : result)
- std::cout << it.toString() << " ---- ";
- std::cout << std::endl;
- return result;
- }
- /* -------TODO: two plane intersection, to reduce computation time
- * else if (hPlane.size() == 2) {
- Point temp;
- std::vector<Point> Poly;
-
- Point norm1 = hPlane[0].GetPoint2();
- std::vector<Line> HP_1(hPlane[0] + norm1.Rotate(0,1) * 10, hPlane[0] - norm1.Rotate(0,1) * 10);
-
- Point norm2 = hPlane[1].GetPoint2();
- std::vector<Line> HP_2(hPlane[1] + norm2.Rotate(0,1) * 10, hPlane[1] - norm2.Rotate(0,1) * 10);
-
- int flag = HP_1[0].IntersectionWith(HP_2[0], temp);
- if (flag != INTERSECTION) {
- std::vector<Point> C_1 = ConvexPolyIntersection(HP_1);
- std::vector<Point> C_2 = ConvexPolyIntersection(HP_2);
- return BoostPolyIntersect(C_1, C_2); // TODO: if the convex polygons dont intersect?
+ if((_Margin[i].GetPoint2() - hPlane[0].GetPoint1()).ScalarProduct(hPlane[0].GetPoint2()) >= 0)
+ Poly.push_back(_Margin[i].GetPoint2());
+
+ }
+
+ std::vector<Point> result;
+ bg::convex_hull(Poly, result);
+
+ return result;
}
- else if(flag == INTERSECTION) {
- //Poly.push_back(temp);
- for (int i = EMARGIN; i < 2; )
+
+ else if (hPlane.size() >= 2) {
+ std::vector<Line> HP_1 (hPlane.begin(), hPlane.begin() + hPlane.size() / 2);
+ std::vector<Line> HP_2 (hPlane.begin() + hPlane.size() / 2, hPlane.end());
+
+ std::vector<Point> C_1 = ConvexPolyIntersection(HP_1, intersect_FLAG);
+
+ std::vector<Point> C_2 = ConvexPolyIntersection(HP_2, intersect_FLAG);
+
+ return BoostPolyIntersect(C_1, C_2, intersect_FLAG);
}
+ /*else {
+ //TODO: empty vector. No half plane
- //TODO: Calc the polygon with the bounding box
- return Poly;
- }*/
- else if (hPlane.size() >= 2) {
- std::vector<Line> HP_1 (hPlane.begin(), hPlane.begin() + hPlane.size() / 2);
- std::vector<Line> HP_2 (hPlane.begin() + hPlane.size() / 2, hPlane.end());
- std::cout << "------------------------------------------------------------------" << std::endl;
- std::cout << "HP_1 size " << HP_1.size() << " calling convPolintersect" << std::endl;
- std::vector<Point> C_1 = ConvexPolyIntersection(HP_1, intersect_FLAG);
- std::cout << "HP_2 size " << HP_2.size() << " calling convPolintersect" << std::endl;
- std::vector<Point> C_2 = ConvexPolyIntersection(HP_2, intersect_FLAG);
- std::cout << " calling BStconvPolintersect" << std::endl;
- return BoostPolyIntersect(C_1, C_2, intersect_FLAG);
+ }*/
}
- /*else {
- //TODO: empty vector. No half plane
+ else
+ return std::vector<Point>();
- }*/
}
std::vector<Point> ORCAModel :: BoostPolyIntersect( std::vector<Point>& poly_1,
@@ -398,37 +378,27 @@ std::vector<Point> ORCAModel :: BoostPolyIntersect( std::vector<Point>& poly_1,
bool &intersect_FLAG) const{
//bg::correct(poly_1);
//bg::correct(poly_2);
- std::vector<Point> poly_result;
- std::cout << "--------------poly_1------------" << std::endl;
- for (auto it : poly_1)
- std::cout << it.toString() << " ---- ";
- std::cout << std::endl;
- std::cout << "--------------poly_2------------" << std::endl;
- for (auto it : poly_2)
- std::cout << it.toString() << " ---- ";
- std::cout << std::endl;
+ if (poly_1.size() < 2) return poly_2;
+ if (poly_2.size() < 2) return poly_1;
+
+ std::vector<std::vector<Point> > poly_result;
+ std::vector<Point> poly_out;
+ bg::correct(poly_1);
+ bg::correct(poly_2);
+
bg::intersection(poly_1, poly_2, poly_result);
- std::cout << "result in bstpolyint size : " << poly_result.size() << std::endl;
- std :: cout << "-----------------RESULT POLY ------------------------" << std :: endl;
- for (auto it : poly_result)
- std::cout << it.toString() << " ---- ";
- std::cout << std::endl;
+
+ for (auto poly : poly_result)
+ for (auto it : poly)
+ poly_out.push_back(it);
std::vector<Point> poly;
- bg::convex_hull(poly_result, poly);
+ bg::convex_hull(poly_out, poly);
- std :: cout << "-----------------CONVEX HULL POLY ------------------------" << std :: endl;
- for (auto it : poly)
- std::cout << it.toString() << " ---- ";
- std::cout << std::endl;
- if (poly_result.size() == 0) {
+ if (poly.size() == 0)
//TODO: what to do if polygons don't intersect?? halt the movement of the ped
intersect_FLAG = false;
- }
- std::cout << "------------------------------------------------------------------" << std::endl;
- //std::vector<Point> poly;
- //bg::convex_hull(result, poly);
return poly;
@@ -454,9 +424,14 @@ Point ORCAModel :: LinearProgram(const std::vector<Line>& halfPlane, const Point
std::vector<Point> poly = ConvexPolyIntersection(halfPlane, intersect_FLAG);
if (intersect_FLAG == false)
- return Point(0,0);
-
-
+ return Point (0,0);
+ //return _Origin;
+
+ std::cout << "---------------Poly-----------------" << std::endl;
+ for(auto it : poly)
+ std::cout << it.toString() << std::endl;
+ std::cout << "---------------Pref_V-----------------" << std::endl;
+ std::cout << Pref_V.toString() << std::endl;
/*
typedef bg::model::polygon<bd2::point_xy<double> > polygon;
polygon bst_Poly;
@@ -465,25 +440,30 @@ Point ORCAModel :: LinearProgram(const std::vector<Line>& halfPlane, const Point
bg::append(bst_Poly, bd2::point_xy<double>(it.GetX(), it.GetY()));
bg::correct(bst_Poly);
bd2::point_xy<double> pt(Pref_V.GetX(), Pref_V.GetY()); */
- std::cout << "----" << (_Origin + Pref_V).toString() << " ---- ";
- std::cout << std::endl;
- Point loc = _Origin + Pref_V;
- bool flag1 = bg::within(loc * 1.12, poly);
+
+ //Point loc = _Origin + Pref_V;
+ bool flag1 = bg::covered_by(Pref_V, poly);
if (flag1 == true) {
std::cout << "pref V is chosen " << std::endl;
-
- return _Origin + Pref_V;
+ std::cout << "Preffered vel = " << (Pref_V - _Origin).Normalized().toString() << std::endl;
+ return (Pref_V - _Origin).Normalized();
}
else {/*
std::vector<double> dist;
std::cout << "result " << poly.size() << std::endl;
// TODO: Compare angle vs distance for better approach of short deviation from pref vel
- for (std::vector<Point>::iterator it = poly.begin(); it != poly.end() - 1; ++it)
+ for (std::vector<Point>::iterator it = poly.begin(); it != poly.end(); ++it)
dist.push_back((Pref_V - *it).Norm());
std::vector<double>::iterator result = std::min_element(dist.begin(), dist.end());
-
- return poly[std::distance(dist.begin(), result)];*/
+ //std::cout << "chosen vel = " << poly[std::distance(dist.begin(), result)].toString() << std::endl;
+ int index = std::distance(dist.begin(), result);
+ std::cout << "Preffered vel = " << (Pref_V - _Origin).Normalized().toString() << std::endl;
+ std::cout << "chosen vel = " << poly[index].toString() << std::endl;
+ return poly[index]; */
+
+ //return (poly[index] - _Origin) / (poly[index] - _Origin).Norm();
+ /*
std::vector<double> angle;
std::cout << "pref V is not chosen " << std::endl;
for (auto it : poly)
@@ -493,7 +473,22 @@ Point ORCAModel :: LinearProgram(const std::vector<Line>& halfPlane, const Point
angle.push_back(abs(atan2(Pref_V.Determinant(npoly), Pref_V.ScalarProduct(npoly))));
}
std::vector<double>::iterator result = std::min_element(angle.begin(), angle.end());
- return poly[std::distance(angle.begin(), result)];
+ return poly[std::distance(angle.begin(), result)]; */
+
+ std::vector<Point> nearestPoint;
+ std::vector<double> distance;
+ for (unsigned int i = 0; i < poly.size() - 1 && poly.size() >= 2; ++i) {
+ Point temp;
+ distance.push_back(Line(poly[i], poly[i+1]).ShortPtDist(Pref_V, temp));
+ nearestPoint.push_back(temp);
+ }
+ std::vector<double>::iterator result = std::min_element(distance.begin(), distance.end());
+ //std::cout << "chosen vel = " << poly[std::distance(dist.begin(), result)].toString() << std::endl;
+ int index = std::distance(distance.begin(), result);
+ std::cout << "Preffered vel = " << (Pref_V - _Origin).Normalized().toString() << std::endl;
+ std::cout << "chosen vel = " << (nearestPoint[index]- _Origin).Normalized().toString() << std::endl;
+ return (nearestPoint[index] - _Origin).Normalized();
+
}
}