diff --git a/math/ORCAModel.cpp b/math/ORCAModel.cpp
index 8cf37f3e7b53b8d382e30a6b80ee951157ea719f..0c24254a309ca60017a2ad459f1a65aa0a5fc890 100644
--- a/math/ORCAModel.cpp
+++ b/math/ORCAModel.cpp
@@ -54,7 +54,7 @@ ORCAModel :: ORCAModel(DirectionStrategy* dir) {
_direction = dir;
_Origin = Point();
_Center = Point();
- _tau = 3;
+ _tau = 2;
_pedRad = 0;
_coneRad = 0;
@@ -71,10 +71,10 @@ bool ORCAModel :: Init (Building* building) {
double cosPhi, sinPhi;
//a destination could not be found for that pedestrian
if (ped->FindRoute() == -1) {
- Log->Write(
- "ERROR:\tVelocityModel::Init() cannot initialise route. ped %d is deleted.\n",ped->GetID());
- building->DeletePedestrian(ped);
- continue;
+ Log->Write(
+ "ERROR:\tVelocityModel::Init() cannot initialise route. ped %d is deleted.\n",ped->GetID());
+ building->DeletePedestrian(ped);
+ continue;
}
Point target = ped->GetExitLine()->LotPoint(ped->GetPos());
Point d = target - ped->GetPos();
@@ -111,11 +111,12 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
{ // Create a bounding box for the halfplanes
_Origin = Ped->GetPos();
JEllipse elpsA = Ped->GetEllipse();
- _pedRad = (elpsA.GetEA() > elpsA.GetEB()) ? elpsA.GetEA() / _tau : elpsA.GetEB() / _tau;
- double x_max = _Origin.GetX() + 25, // check for the extent of the bounding box
- x_min = _Origin.GetX() - 25,
- y_max = _Origin.GetY() + 25,
- y_min = _Origin.GetY() - 25;
+ //_pedRad = (elpsA.GetEA() > elpsA.GetEB()) ? elpsA.GetEA() / _tau : elpsA.GetEB() / _tau;
+ _pedRad = 1.56 / _tau;
+ double x_max = _Origin.GetX() + 5, // check for the extent of the bounding box
+ x_min = _Origin.GetX() - 5,
+ y_max = _Origin.GetY() + 5,
+ y_min = _Origin.GetY() - 5;
_Margin[EMARGIN] = Line(Point(x_min, y_max), Point(x_min, y_min));
_Margin[WMARGIN] = Line(Point(x_max, y_max), Point(x_max, y_min));
@@ -129,7 +130,7 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
SubRoom* subroom = building->GetRoom(Ped->GetRoomID())->GetSubRoom(Ped->GetSubRoomID());
Room* room = building->GetRoom(Ped->GetRoomID());
- vector<Line> halfPlane; // Point1 is Line Location, Point2 is unit normal to the line
+ vector<Line> halfPlane; // Point1 is point on halfplane Line, Point2 is unit normal to the plane
for (auto nPed : neighbourPed) {
Point p2 = nPed->GetPos();
std::vector<SubRoom*> emptyVector;
@@ -144,9 +145,9 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
JEllipse elpsB = nPed->GetEllipse();
double maxB = (elpsB.GetEA() > elpsB.GetEB()) ? elpsB.GetEA() : elpsB.GetEB();
_coneRad = (_pedRad + maxB / _tau);
- Line hPlane = ComputeHalfPlane(Ped, nPed);
+ Line hPlane = ComputeHalfPlane(Ped, nPed, deltaT);
if (hPlane != Line())
- halfPlane.push_back(ComputeHalfPlane(Ped, nPed));
+ halfPlane.push_back(hPlane);
}
{
const vector<Wall>& Walls = subroom->GetAllWalls();
@@ -160,30 +161,16 @@ void ORCAModel :: ComputeNextTimeStep(double current, double deltaT, Building* b
//TODO: haplfPlane for walls & obstacles
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();
- 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 );
- std::cout << "next pos = " << (_Origin + vel_norm * Ped->GetV0Norm() * deltaT ).toString() << std::endl;
- Ped->SetV(vel_norm);
- Ped->SetPhiPed();
- //}
- /*else { //ped stays put.
- Ped->SetPos(_Origin);
- Ped->SetV(Ped->GetV());
- Ped->SetPhiPed();
- }*/
-
-
- //std::cout<<"new Pos = " << .toString()<< std::endl;
- //vector <Wall> neighbourWall; // visible or all walls
- //vector <Obstacle> wallsofobs: // all obstacles
+ Ped->SetPos(_Origin + vel_norm * Ped->GetV0Norm() * deltaT );
+ Ped->SetV(vel_norm);
+ Ped->SetPhiPed();
+
+ //TODO: include obstacles
+ //
+ //
// v0 is the desired velocity vector
// v is the current velocity
}
@@ -199,14 +186,16 @@ string ORCAModel::GetDescription()
return rueck;
}
-Line ORCAModel :: ComputeHalfPlane(const Pedestrian* PedA, const Pedestrian* nPedB) const{
+Line ORCAModel :: ComputeHalfPlane(const Pedestrian* PedA, const Pedestrian* nPedB, const double dT) const{
Line tanPoints = ComputeVelCone();
Line VelCone1(tanPoints.GetPoint1(), _Origin + (tanPoints.GetPoint1() - _Origin) * 10);
Line VelCone2(tanPoints.GetPoint2(), _Origin + (tanPoints.GetPoint2() - _Origin) * 10);
- Point VelAB = PedA->GetV() - nPedB->GetV();
+ Point VelAB = (_Origin + PedA->GetV() * PedA->GetV0Norm() * 1) -
+ (nPedB->GetPos() + nPedB->GetV() * nPedB->GetV0Norm() * 1) ;
+
double shrtDist2Cone1 = VelCone1.DistTo(VelAB);
double shrtDist2Cone2 = VelCone2.DistTo(VelAB);
Point nearPt2Curve = NearestPointOnCurve(tanPoints, VelAB);
@@ -214,14 +203,14 @@ 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) {
+ int dirFlag = CheckContainsInCone(VelCone1, VelCone2, VelAB); //function return +1 for inside -1 for outside
+ if (dirFlag != -1) { // TODO: verify procedure for condition 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);
+ return Line(VelAB - nVector * shrtDist2Cone1 / 2, nVector * dirFlag);
else
return Line(VelAB + nVector * shrtDist2Cone1 / 2, nVector * dirFlag);
}
@@ -229,7 +218,7 @@ Line ORCAModel :: ComputeHalfPlane(const Pedestrian* PedA, const Pedestrian* nPe
uVector = VelCone2.ShortestPoint(VelAB);
nVector = (uVector - VelAB) / shrtDist2Cone2; // outward normal to the boundary
if (dirFlag == -1)
- return Line(VelAB, nVector * dirFlag);
+ return Line(VelAB - nVector * shrtDist2Cone2 / 2, nVector * dirFlag);
else
return Line(VelAB + nVector * shrtDist2Cone2 / 2, nVector * dirFlag);
@@ -238,11 +227,13 @@ Line ORCAModel :: ComputeHalfPlane(const Pedestrian* PedA, const Pedestrian* nPe
uVector = nearPt2Curve;
nVector = uVector / shrtDist2Curve; // outward normal to the boundary
if (dirFlag == -1)
- return Line(VelAB, nVector * dirFlag);
+ return Line(VelAB - nVector * shrtDist2Curve / 2, nVector * dirFlag);
else
return Line(VelAB + nVector * shrtDist2Curve / 2, nVector * dirFlag);
}
- //}
+ }
+ else
+ return Line();
}
@@ -322,7 +313,8 @@ Point ORCAModel :: NearestPointOnCurve(const Line& tanPoints, const Point& VelAB
return tanPoints.GetPoint2();
}
-std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>& hPlane, bool &intersect_FLAG) const{
+std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>& hPlane,
+ bool &intersect_FLAG, int count) const{
if (intersect_FLAG == true) {
if (hPlane.size() == 1) {
@@ -332,7 +324,7 @@ std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>&
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) {
+ if (flag == INTERSECTION) { //TODO: if overlap??
Poly.push_back(temp);
count++;
}
@@ -376,8 +368,6 @@ std::vector<Point> ORCAModel :: ConvexPolyIntersection(const std::vector<Line>&
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);
if (poly_1.size() < 2) return poly_2;
if (poly_2.size() < 2) return poly_1;
@@ -427,54 +417,12 @@ Point ORCAModel :: LinearProgram(const std::vector<Line>& halfPlane, const Point
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;
-
- for (auto it : poly)
- 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()); */
-
- //Point loc = _Origin + Pref_V;
bool flag1 = bg::covered_by(Pref_V, poly);
+
if (flag1 == true) {
- std::cout << "pref V is chosen " << std::endl;
- 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(); ++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[index] - _Origin) / (poly[index] - _Origin).Norm();
- /*
- std::vector<double> angle;
- std::cout << "pref V is not chosen " << std::endl;
- for (auto it : poly)
- {
- //Point nPref_V = (_Origin - Pref_V) / (Pref_V - _Origin).Norm();
- Point npoly = (_Origin - it) / (it - _Origin).Norm();
- 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)]; */
-
+ else {
std::vector<Point> nearestPoint;
std::vector<double> distance;
for (unsigned int i = 0; i < poly.size() - 1 && poly.size() >= 2; ++i) {
@@ -485,8 +433,6 @@ Point ORCAModel :: LinearProgram(const std::vector<Line>& halfPlane, const Point
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();
}
diff --git a/math/ORCAModel.h b/math/ORCAModel.h
index 092331602a2b63d81408379f66ecf74908ef1750..74ed576fa7a4cd8b522dbb8e5d2d4de1073fd97d 100644
--- a/math/ORCAModel.h
+++ b/math/ORCAModel.h
@@ -49,37 +49,117 @@ class ORCAModel : public OperationalModel {
public:
+ // Constructor
ORCAModel(DirectionStrategy* dir);
+
+ // Destructor
virtual ~ORCAModel(void);
+
+ /**
+ * initialize the phi angle
+ * @param building
+ */
+ virtual bool Init (Building* building);
+
+ /**
+ * Compute the next simulation step;
+ * Update the positions and velocities
+ * @param current the actual time
+ * @param deltaT the next timestep
+ * @param building the geometry object
+ */
virtual void ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic);
+
+ /**
+ * @return all model parameters in a nicely formatted string
+ */
virtual std::string GetDescription();
- virtual bool Init (Building* building);
+
private:
+ // parameters
double _tau;
- Point _Origin;
- Point _Center;
- double _pedRad;
- double _coneRad;
- //bool _intersect_FLAG;
+ Point _Origin; // position of Pedestrian A
+ Point _Center; // Vector B - A or pos(B) - pos(A)
+ double _pedRad; // Radius of the pedestrian
+ double _coneRad; // Radius of the circle at the tip of velocity cone
std::array<Line, 4> _Margin;
DirectionStrategy* _direction;
- Line ComputeHalfPlane(const Pedestrian* A, const Pedestrian* B) const;
+
+ /**
+ * @return half plane for Pedestrian A with respect to Pedestrian B
+ */
+ Line ComputeHalfPlane(const Pedestrian* A, const Pedestrian* B, const double dT) const;
+
+ /**
+ * @return half plane for Pedestrian A with respect to all walls in the room/Sub-room
+ * @param Walls all walls in the room/Sub-room where the Pedestria A is in.
+ */
std::vector<Line> ComputeHalfPlane(const std::vector<Wall>& Walls) const;
+
+ /**
+ * Compute the velocity cone
+ * @return the tangent points on the circle at the tip of velocity cone
+ * @as Line(tan_Point_left, tan_Point_right)
+ */
Line ComputeVelCone() const;
+
+ /**
+ * @return nearest point 'u' on velocity cone from velocity A - velocity B
+ * @param tanPoints the tangent points on the circle at the tip of velocity cone
+ * @param VelAB velocity A - velocity B
+ */
Point NearestPointOnCurve(const Line& tanPoints, const Point& VelAB) const;
- std::vector<Point> ConvexPolyIntersection(const std::vector<Line>& halfPlane, bool &intersect_FLAG) const;
+
+ /**
+ * Recursive funtion to Compute the polygon of permitted velocities
+ * @param halfPlane - The half planes of Pedestrian A
+ */
+ std::vector<Point> ConvexPolyIntersection(const std::vector<Line>& halfPlane,
+ bool &intersect_FLAG, int count = 0) const;
+
+ /**
+ * Compute polygon_1 intersection polygon_2
+ * @return polygon_1 intersection polygon_2
+ * @param poly_1 polygon 1
+ * @param poly_2 polygon 2
+ * @intersect_FLAG default is true. if no intersection it is set to False
+ */
std::vector<Point> BoostPolyIntersect( std::vector<Point>& poly_1,
std::vector<Point>& poly_2,
bool &intersect_FLAG) const;
+
+ /**
+ * The desired direction of pedestrian
+ *
+ * @param ped: Pointer to Pedestrians
+ * @param room: Pointer to room
+ *
+ * @return Point
+ */
Point e0(Pedestrian* ped, Room* room) const;
+
+ /**
+ * Compute velocity that is closest to the preferred Velocity
+ * @param halfPlane - The half planes of Pedestrian A
+ * @param Pref_V preferred velocity
+ *
+ * @return chosen veloctiy
+ */
Point LinearProgram(const std::vector<Line>& halfPlane, const Point& Pref_V, bool &intersect_FLAG) const;
+
+ /**
+ * Check whether VelAB is inside or outside velocity cone
+ *
+ * @return int +1 for inside, -1 for outside
+ * @param Line1 left tangent line of velocity cone
+ * @param Line2 right tangent line of velocity cone
+ * @param VelAB relative velocity. velocity A - velocity B
+ *
+ */
int CheckContainsInCone(const Line& Line1, const Line& Line2, const Point& VelAB) const;
-
-
-
};
#endif