Line2::Line2(const Side* side)
{
try {
if (side->getNbNodes() != 2)
THROW_RT("Line2(Side *): Illegal number of side nodes: " + itos(side->getNbNodes()));
}
CATCH("Line2");
_sh.resize(2);
_node.resize(2);
_x.resize(2);
_dsh.resize(2);
_dshl.resize(2);
for (size_t i=0; i<2; i++) {
Node *node = side->getPtrNode(i+1);
_x[i] = node->getCoord();
_node[i] = node->n();
}
_c = 0.5*(_x[0] + _x[1]);
_sd = side;
_el = NULL;
Point<real_t> dl = 0.5*(_x[1] - _x[0]);
_det = dl.Norm();
try {
if (_det == 0.0)
THROW_RT("Line2(Side *): Determinant of jacobian is null");
}
CATCH("Line2");
_length = 2*_det;
}
Line2::Line2(const Edge* edge)
{
_sh.resize(2);
_node.resize(2);
_x.resize(2);
_dsh.resize(2);
_dshl.resize(2);
for (size_t i=0; i<2; i++) {
Node *node = edge->getPtrNode(i+1);
_x[i] = node->getCoord();
_node[i] = node->n();
}
_c = 0.5*(_x[0] + _x[1]);
_ed = edge;
_sd = NULL;
_el = NULL;
Point<real_t> dl = 0.5*(_x[1] - _x[0]);
_det = dl.Norm();
try {
if (_det == 0.0)
THROW_RT("Line2(Edge *): Determinant of jacobian is null");
}
CATCH("Line2");
_length = 2*_det;
}
double Point::GetAngle(const Point& p_in) const
{
double loc;
double norm = Norm()*p_in.Norm();
if (norm > 0.0) {
loc = this->Dot(p_in) / norm;
return std::acos(loc);
}
else
return 0.0; // returning unvalid value
}
Point VelocityModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& centroid, bool inside) const
{
Point F_wrep = Point(0.0, 0.0);
Point pt = w.ShortestPoint(ped->GetPos());
Point dist = pt - ped->GetPos(); // x- and y-coordinate of the distance between ped and p
const double EPS = 0.000; // molified see Koester2013
double Distance = dist.Norm() + EPS; // distance between the centre of ped and point p
//double vn = w.NormalComp(ped->GetV()); //normal component of the velocity on the wall
Point e_iw; // x- and y-coordinate of the normalized vector between ped and pt
//double K_iw;
double l = ped->GetEllipse().GetBmax();
double R_iw;
double min_distance_to_wall = 0.001; // 10 cm
if (Distance > min_distance_to_wall) {
e_iw = dist / Distance;
}
else {
Log->Write("WARNING:\t Velocity: forceRepWall() ped %d is too near to the wall (dist=%f)", ped->GetID(), Distance);
Point new_dist = centroid - ped->GetPos();
new_dist = new_dist/new_dist.Norm();
printf("new distance = (%f, %f) inside=%d\n", new_dist._x, new_dist._y, inside);
e_iw = (inside ? new_dist:new_dist*-1);
}
//-------------------------
const Point& pos = ped->GetPos();
double distGoal = ped->GetExitLine()->DistToSquare(pos);
if(distGoal < J_EPS_GOAL*J_EPS_GOAL)
return F_wrep;
//-------------------------
R_iw = - _aWall * exp((l-Distance)/_DWall);
F_wrep = e_iw * R_iw;
return F_wrep;
}
bool VelocityModel::Init (Building* building)
{
if(dynamic_cast<DirectionFloorfield*>(_direction)){
Log->Write("INFO:\t Init DirectionFloorfield starting ...");
//fix using defaults; @fixme ar.graf (pass params from argument parser to ctor?)
double _deltaH = 0.0625;
double _wallAvoidDistance = 0.4;
bool _useWallAvoidance = true;
dynamic_cast<DirectionFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Log->Write("INFO:\t Init DirectionFloorfield done");
}
if(dynamic_cast<DirectionLocalFloorfield*>(_direction)){
Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ...");
//fix using defaults; @fixme ar.graf (pass params from argument parser to ctor?)
double _deltaH = 0.0625;
double _wallAvoidDistance = 0.4;
bool _useWallAvoidance = true;
dynamic_cast<DirectionLocalFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Log->Write("INFO:\t Init DirectionLOCALFloorfield done");
}
if(dynamic_cast<DirectionSubLocalFloorfield*>(_direction)){
Log->Write("INFO:\t Init DirectionSubLOCALFloorfield starting ...");
//fix using defaults; @fixme ar.graf (pass params from argument parser to ctor?)
double _deltaH = 0.0625;
double _wallAvoidDistance = 0.4;
bool _useWallAvoidance = true;
dynamic_cast<DirectionSubLocalFloorfield*>(_direction)->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
Log->Write("INFO:\t Init DirectionSubLOCALFloorfield done");
}
const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
size_t peds_size = allPeds.size();
for(unsigned int p=0;p < peds_size;p++)
{
Pedestrian* ped = allPeds[p];
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);
p--;
peds_size--;
continue;
}
Point target = ped->GetExitLine()->LotPoint(ped->GetPos());
Point d = target - ped->GetPos();
double dist = d.Norm();
if (dist != 0.0) {
cosPhi = d._x / dist;
sinPhi = d._y / dist;
} else {
Log->Write(
"ERROR: \tallPeds::Init() cannot initialise phi! "
"dist to target is 0\n");
return false;
}
ped->InitV0(target);
JEllipse E = ped->GetEllipse();
E.SetCosPhi(cosPhi);
E.SetSinPhi(sinPhi);
ped->SetEllipse(E);
}
return true;
}