Commit 45a0f7fa authored by Arne Graf's avatar Arne Graf

variant model compiling, fix needed

parent 75c74f9a
This diff is collapsed.
......@@ -46,6 +46,7 @@ class GradientModel : public OperationalModel {
private:
/// define the strategy for crossing a door (used for calculating the driving force)
DirectionStrategy* _direction;
DirectionFloorfield* _floorfield;
/// Modellparameter
double _nuPed;
......
......@@ -135,7 +135,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
Point diff = (p1 - p2).Normalized() * d;
Line e_neu = Line(p1 - diff, p2 + diff);
Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos());
Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if
// it intersects a wall || obstacle.
// check for intersection with walls
......@@ -164,13 +164,13 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
printf("Check wall number %d. Dist = %f (%f)\n", i, dist, minDist);
printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY());
#endif
}
}//walls
//============================ WALLS ===========================
//============================ OBST ===========================
const vector<Obstacle*>& obstacles = subroom->GetAllObstacles();
for(unsigned int obs=0; obs<obstacles.size(); ++obs) {
......@@ -198,11 +198,11 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
if(iObs >= 0){ // obstacle is nearest
const vector<Wall>& owalls = obstacles[iObs]->GetAllWalls();
angle = tmpDirection.GetObstacleDeviationAngle(owalls);
// angle = tmpDirection.GetDeviationAngle(owalls[inear].enlarge(2*ped->GetLargerAxis()));
#if DEBUG
printf("COLLISION WITH OBSTACLE %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1().GetX(),owalls[inear].GetPoint1().GetY(), owalls[inear].GetPoint2().GetX(),owalls[inear].GetPoint2().GetY());
#endif
} //iObs
else{ // wall is nearest
......@@ -223,7 +223,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
}
////////////////////////////////////////////////////////////
Point G;
if (fabs(angle) > J_EPS)
//G = tmpDirection.GetPoint2().Rotate(cos(angle), sin(angle)) ;
......@@ -231,7 +231,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
else {
if(ped->GetNewOrientationFlag()) //this pedestrian could not see the target and now he can see it clearly.
ped->SetSmoothTurning(); // so the turning should be adapted accordingly.
G = NextPointOnLine;
}
......@@ -247,7 +247,7 @@ Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
// if( ped->GetID() == 21)
// fprintf(stderr, "%.2f %.2f %.2f %.2f %f %f %d %.2f %.2f %.2f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY(),
// fprintf(stderr, "%.2f %.2f %.2f %.2f %f %f %d %.2f %.2f %.2f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY(),
// ped->GetPos().GetX(), ped->GetPos().GetY(), G.GetX(), G.GetY(), ped->GetID(), ped->GetV0().GetX(), ped->GetV0().GetY(), ped->GetGlobalTime());
// this stderr output can be used with plot_desired_velocity.py
......@@ -273,6 +273,13 @@ Point DirectionFloorfield::GetTarget(Room* room, Pedestrian* ped) const
exit(EXIT_FAILURE);
}
Point DirectionFloorfield::GetDir2Wall(Pedestrian* ped) const
{
Point p;
ffviafm->getDir2WallAt(ped->GetPos(), p);
return p;
}
void DirectionFloorfield::Init(Building* building, double stepsize, double threshold, bool useDistancMap) {
//implement mechanic, that can read-in an existing floorfield (from a previous run)
ffviafm = new FloorfieldViaFM(building, stepsize, stepsize, threshold, useDistancMap);
......
......@@ -78,6 +78,7 @@ public:
~DirectionFloorfield();
//void Init();
virtual Point GetTarget(Room* room, Pedestrian* ped) const;
virtual Point GetDir2Wall(Pedestrian* ped) const;
private:
FloorfieldViaFM* ffviafm;
......
......@@ -47,6 +47,7 @@ FloorfieldViaFM::~FloorfieldViaFM()
if (speedInitial) delete[] speedInitial;
if (cost) delete[] cost;
if (neggrad) delete[] neggrad;
if (dist2Wall) delete[] dist2Wall;
if (trialfield) delete[] trialfield;
}
......@@ -100,6 +101,12 @@ void FloorfieldViaFM::getDirectionAt(const Point& position, Point& direction){
direction.SetY(neggrad[key].GetY());
}
void FloorfieldViaFM::getDir2WallAt(const Point& position, Point& direction){
long int key = grid->getKeyAtPoint(position);
direction.SetX(dirToWall[key].GetX());
direction.SetY(dirToWall[key].GetY());
}
void FloorfieldViaFM::parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY) {
//init min/max before parsing
double xMin = FLT_MAX;
......@@ -176,6 +183,7 @@ void FloorfieldViaFM::parseBuilding(const Building* const buildingArg, const dou
speedInitial = new double[grid->GetnPoints()];
cost = new double[grid->GetnPoints()];
neggrad = new Point[grid->GetnPoints()];
dirToWall = new Point[grid->GetnPoints()];
trialfield = new Trial[grid->GetnPoints()]; //created with other arrays, but not initialized yet
//linescan using (std::vector<Wall*>)
......@@ -513,6 +521,8 @@ void FloorfieldViaFM::calculateDistanceField(const double thresholdArg) { //if
while (iter != nullptr) {
iter[0].cost[0] = trialfield[keyOfSmallest].cost[0];
iter[0].flag[0] = 3;
dirToWall[iter[0].key].SetX(0.);
dirToWall[iter[0].key].SetY(0.);
iter = iter->child;
}
......@@ -521,6 +531,8 @@ void FloorfieldViaFM::calculateDistanceField(const double thresholdArg) { //if
if (flag[i] == 0) {
flag[i] = 3;
dist2Wall[i] = dist2Wall[keyOfSmallest];
dirToWall[i].SetX(0.);
dirToWall[i].SetY(0.);
}
}
smallest = nullptr;
......@@ -573,6 +585,8 @@ void FloorfieldViaFM::checkNeighborsAndCalcDist2Wall(const long int key) {
double row;
double col;
long int aux;
bool pointsUp;
bool pointsRight;
row = 100000.;
col = 100000.;
......@@ -587,6 +601,7 @@ void FloorfieldViaFM::checkNeighborsAndCalcDist2Wall(const long int key) {
(flag[aux] != 0)) //gridpoint holds a calculated value
{
row = trialfield[aux].cost[0];
pointsRight = true;
if (row < 0) {
std::cerr << "hier ist was schief " << row << " " << aux << " " << flag[aux] << std::endl;
row = 100000;
......@@ -599,6 +614,7 @@ void FloorfieldViaFM::checkNeighborsAndCalcDist2Wall(const long int key) {
(trialfield[aux].cost[0] < row)) //calculated value promises smaller cost
{
row = trialfield[aux].cost[0];
pointsRight = false;
//todo: add directioninfo to calc neggradient later OR recheck neighbor later again
}
......@@ -609,6 +625,7 @@ void FloorfieldViaFM::checkNeighborsAndCalcDist2Wall(const long int key) {
(flag[aux] != 0)) //gridpoint holds a calculated value
{
col = trialfield[aux].cost[0];
pointsUp = true;
if (col < 0) {
std::cerr << "hier ist was schief " << col << " " << aux << " " << flag[aux] << std::endl;
col = 100000;
......@@ -621,17 +638,34 @@ void FloorfieldViaFM::checkNeighborsAndCalcDist2Wall(const long int key) {
(trialfield[aux].cost[0] < col)) //calculated value promises smaller cost
{
col = trialfield[aux].cost[0];
pointsUp = false;
//todo: add directioninfo to calc neggradient later OR recheck neighbor later again
}
if (col == 100000.) { //one sided update with row
trialfield[key].cost[0] = onesidedCalc(row, grid->Gethx());
trialfield[key].flag[0] = 1;
if (pointsRight) {
dirToWall[key].SetX(-(cost[key+1]-cost[key])/grid->Gethx());
dirToWall[key].SetY(0.);
} else {
dirToWall[key].SetX(-(cost[key]-cost[key-1])/grid->Gethx());
dirToWall[key].SetY(0.);
}
dirToWall[key] = dirToWall[key].Normalized();
return;
}
if (row == 100000.) { //one sided update with col
trialfield[key].cost[0] = onesidedCalc(col, grid->Gethy());
trialfield[key].flag[0] = 1;
if (pointsUp) {
dirToWall[key].SetX(0.);
dirToWall[key].SetY(-(cost[key+(grid->GetiMax())]-cost[key])/grid->Gethy());
} else {
dirToWall[key].SetX(0.);
dirToWall[key].SetY(-(cost[key]-cost[key-(grid->GetiMax())])/grid->Gethy());
}
dirToWall[key] = dirToWall[key].Normalized();
return;
}
......@@ -640,9 +674,26 @@ void FloorfieldViaFM::checkNeighborsAndCalcDist2Wall(const long int key) {
if (precheck >= 0) {
trialfield[key].cost[0] = precheck;
trialfield[key].flag[0] = 2;
if (pointsUp && pointsRight) {
dirToWall[key].SetX(-(cost[key+1]-cost[key])/grid->Gethx());
dirToWall[key].SetY(-(cost[key+(grid->GetiMax())]-cost[key])/grid->Gethy());
}
if (pointsUp && !pointsRight) {
dirToWall[key].SetX(-(cost[key]-cost[key-1])/grid->Gethx());
dirToWall[key].SetY(-(cost[key+(grid->GetiMax())]-cost[key])/grid->Gethy());
}
if (!pointsUp && pointsRight) {
dirToWall[key].SetX(-(cost[key+1]-cost[key])/grid->Gethx());
dirToWall[key].SetY(-(cost[key]-cost[key-(grid->GetiMax())])/grid->Gethy());
}
if (!pointsUp && !pointsRight) {
dirToWall[key].SetX(-(cost[key]-cost[key-1])/grid->Gethx());
dirToWall[key].SetY(-(cost[key]-cost[key-(grid->GetiMax())])/grid->Gethy());
}
} else {
std::cerr << "else in twosided &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&" << std::endl;
}
dirToWall[key] = dirToWall[key].Normalized();
}
void FloorfieldViaFM::checkNeighborsAndCalcFloorfield(const long int key) {
......
......@@ -57,6 +57,7 @@ class FloorfieldViaFM
//FloorfieldViaFM& operator=(const FloorfieldViaFM& other);
void getDirectionAt(const Point& position, Point& direction);
void getDir2WallAt(const Point& position, Point& direction);
void parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
void resetGoalAndCosts(const Goal* const goalArg);
......@@ -101,6 +102,7 @@ class FloorfieldViaFM
double* cost;
long int* secKey; //secondary key to address ... not used yet
Point* neggrad; //gradients
Point* dirToWall;
Trial* trialfield;
double threshold;
......
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