C++ Point::Norm方法代码示例

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;
}