Skip to content
Snippets Groups Projects
Commit 8b03e71f authored by karthik's avatar karthik
Browse files

changed linear model

parent 638ed5be
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 148 additions and 153 deletions
math/ORCAModel.cpp
+ 148
− 153
View file @ 8b03e71f
......@@ -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,34 +122,30 @@ 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;
_coneRad = (_pedRad + maxB / _tau);
Line hPlane = ComputeHalfPlane(Ped, nPed);
if (hPlane != Line())
halfPlane.push_back(ComputeHalfPlane(Ped, nPed));
}
{
......@@ -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);
}
//}
}
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,6 +324,7 @@ Point ORCAModel :: NearestPointOnCurve(const Line& tanPoints, const Point& VelAB
std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>& hPlane, bool &intersect_FLAG) const{
if (intersect_FLAG == true) {
if (hPlane.size() == 1) {
Point temp;
Point norm = hPlane[0].GetPoint2();
......@@ -330,61 +336,31 @@ std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>&
Poly.push_back(temp);
count++;
}
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;
}
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?
}
else if(flag == INTERSECTION) {
//Poly.push_back(temp);
for (int i = EMARGIN; i < 2; )
return result;
}
//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 {
......@@ -392,43 +368,37 @@ std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>&
}*/
}
else
return std::vector<Point>();
}
std::vector<Point> ORCAModel :: BoostPolyIntersect( std::vector<Point>& poly_1,
std::vector<Point>& poly_2,
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;
......@@ -455,8 +425,13 @@ Point ORCAModel :: LinearProgram(const std::vector<Line>& halfPlane, const Point
if (intersect_FLAG == false)
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());
//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[std::distance(dist.begin(), result)];*/
//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();
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment