Exemplo n.º 1
0
	Environment() : ground(Transformation(Vector3(0,0,0)))
	{
		suns[0] = Sun(Vector3(-1.0, 1.0,-1.0));
		suns[1] = Sun(Vector3( 1.0, 1.0,-1.0));
		suns[2] = Sun(Vector3( 1.0, 0.2, 1.0));

		for (int i = 0; i < 20; i++)
		{
			float x = rand() % 10000 * 0.008;
			if (x < 40)
			{
				x += 10;
			}
			else
			{
				x *= -1;
				x += 30;
			}

			float z = rand() % 10000 * 0.008;
			if (z < 40)
			{
				z += 10;
			}
			else
			{
				z *= -1;
				z += 30;
			}

			obstacles[i] = Obstacle(Transformation(Vector3(x, 0, z)));
		}
	}
Obstacle ObjectFactory::createObstacle(int x, int y, int w, int h, int obsType) {
		switch (obsType) {
		case 0: return Obstacle(x,y,w,h);
		case 1: return ObstacleDmg(x,y,w,h,1);
		case 2: return ObstacleSlow(x,y,w,h,1);
		};
	}
Exemplo n.º 3
0
void CinematicMap::SetLaser(LaserData laserdata)
{
    
    pointsCloud= laserdata.getPoints();
    angleCloud = laserdata.getAngles();
    rangeCloud = laserdata.getRanges();

    Obstacle();
    
}
void TeamPlayersLocator::update(TeamPlayersModel& teamPlayersModel)
{
  teamPlayersModel.obstacles.clear();

  std::vector<Obstacle> ownTeam;

  teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
  teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
  teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
  teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);

  if(theRobotInfo.penalty == PENALTY_NONE && theGroundContactState.contact)
  {
    ownTeam.emplace_back(theRobotPose.covariance.topLeftCorner(2, 2), theRobotPose.translation); // pose and covariance of the robot itself

    if(theFallDownState.state == theFallDownState.upright) //why is this?
      for(const auto& obstacle : theObstacleModel.obstacles)
      {
        if(obstacle.type == GOALPOST)
          continue;
        // if seen robots are of the opponent team and are not detected outside the field
        const Vector2f p = theRobotPose * obstacle.center;
        if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
          teamPlayersModel.obstacles.emplace_back(obstacle.covariance, p, obstacle.type);
      }
  }

  for(auto const& teammate : theTeammateData.teammates)
  {
    if(teammate.status == Teammate::FULLY_ACTIVE)
    {
      if(teammate.pose.deviation < 50.f) //todo, magic number will be replaced by parameter. 50.f might be too low
        ownTeam.emplace_back(teammate.pose.covariance.topLeftCorner(2, 2), teammate.pose.translation); //position of teammates

      for(const auto& obstacle : teammate.obstacleModel.obstacles)
      {
        if(obstacle.type == GOALPOST)
          continue;
        // if seen robots are of the opponent team and are not detected outside the field
        const Vector2f p = teammate.pose * obstacle.center;
        if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
        {
          Matrix2f covariance = (Matrix2f() << obstacle.covXX, obstacle.covXY, obstacle.covXY, obstacle.covYY).finished();
          Obstacle converted = Obstacle(rotateCovariance(covariance, teammate.pose.rotation), p, obstacle.type);
          merge(converted, teamPlayersModel.obstacles);
        }
      }
    }
  }
  for(auto& teammate : ownTeam)
  {
    removeAround(teammate, teamPlayersModel.obstacles);
  }
}
Obstacle::Obstacle(Vector3 A, Vector3 B, unsigned int Npoints) //line
{
	if(Npoints <= 0)
	{
		std::cout << "error at creating line" <<std::endl;
	}
	center = (A+B)/2;
	for(unsigned int i = 0; i<1+Npoints; i++)
	{
		Components.push_back(Obstacle(A+(B-A)/Npoints*i));
	}
}
Exemplo n.º 6
0
void CinematicMap::SetLaser(LaserData laserdata)
{
    vector<Vector2D> points2D=laserdata.getPoints();
    double zpos=0.4f;   //high laser2D
    pointsCloud.resize(points2D.size());
    for(int i=0;i<points2D.size();i++)
    {
        pointsCloud[i]=Vector3D(points2D[i].x,points2D[i].y,zpos);
    }
    angleCloud = laserdata.getAngles();
    rangeCloud = laserdata.getRanges();

    Obstacle();
    
}
Exemplo n.º 7
0
GameController::GameController(sf::RenderWindow &window)
{
    w=&window;
    view.reset(sf::FloatRect(0,0,SCREEN_W,SCREEN_H));
    HtH a(300,sf::FloatRect(10,10,0,FLOOR_Y),1,1);
    hero=Hero(0,sf::FloatRect(0,FLOOR_Y-200,100,100),sf::Vector2f(0,0),100, a, 3,0);

    int obsID=200;
    int i=0;
    obstacleTab[i]=Obstacle(obsID+i, sf::FloatRect(0,FLOOR_Y,FLOOR_LENGTH,10), 0);
    obstacleTab[i++].setDestructible(false);

    obstacleTab[i]=Obstacle(obsID+i, sf::FloatRect(200,FLOOR_Y-100,200,10), 0);
    obstacleTab[i++].setDestructible(false);

    obstacleTab[i]=Obstacle(obsID+i, sf::FloatRect(400,FLOOR_Y-200,200,10), 0);
    obstacleTab[i++].setDestructible(false);

    obstacleTab[i]=Obstacle(obsID+i, sf::FloatRect(200,FLOOR_Y-300,200,10), 0);
    obstacleTab[i++].setDestructible(false);

    obstacleTab[i]=Obstacle(obsID+i, sf::FloatRect(500,FLOOR_Y-400,500,10), 0);
    obstacleTab[i++].setDestructible(false);
}
Exemplo n.º 8
0
/*
void BSWalkTo::addObstacleWithMaxFactor(const Vector2<>& pos, float expandLength, float avoidanceMinRadius, float avoidanceMaxRadius, float maxFactor)
{
  if(obstacleCount >= sizeof(obstacles) / sizeof(*obstacles))
    return;

  const Vector2<> expand = Vector2<>(-pos.y, pos.x).normalize(expandLength);
  Vector2<> left = pos + expand;
  Vector2<> right = pos - expand;
  const float radius = avoidanceMinRadius + (avoidanceMaxRadius - avoidanceMinRadius) * maxFactor;
  left += Vector2<>(-left.y, left.x).normalize(radius);
  right += Vector2<>(right.y, -right.x).normalize(radius);
  obstacles[obstacleCount++] = Obstacle(left.angle(), right.angle(), pos);
}
*/
void BSWalkTo::addObstacle2(const Vector2<>& left,
                            const Vector2<>& right,
                            const Vector2<>& center,
                            float obstacleDistance,
                            float avoidanceMinRadius,
                            float avoidanceMaxRadius,
                            float avoidanceDistance)
{
  if(obstacleCount >= sizeof(obstacles) / sizeof(*obstacles))
    return;

  const float radius = avoidanceMinRadius + (avoidanceMaxRadius - avoidanceMinRadius) * (avoidanceDistance - obstacleDistance) / avoidanceDistance;
  float leftAngle = (left + Vector2<>(-left.y, left.x).normalize(radius)).angle();
  float rightAngle = (right + Vector2<>(right.y, -right.x).normalize(radius)).angle();
  obstacles[obstacleCount++] = Obstacle(left, right, center, leftAngle, rightAngle);
}
Exemplo n.º 9
0
void BSWalkTo::addObstacle2(const Vector2<>& pos,
                            float expandLength,
                            float avoidanceMinRadius,
                            float avoidanceMaxRadius,
                            float avoidanceDistance)
{
  if(obstacleCount >= sizeof(obstacles) / sizeof(*obstacles))
    return;

  const Vector2<> expand = Vector2<>(-pos.y, pos.x).normalize(expandLength);
  Vector2<> left = pos + expand;
  Vector2<> right = pos - expand;
  const float radius = avoidanceMinRadius + (avoidanceMaxRadius - avoidanceMinRadius) * (avoidanceDistance - pos.abs()) / avoidanceDistance;
  float leftAngle = (left + Vector2<>(-left.y, left.x).normalize(radius)).angle();
  float rightAngle = (right + Vector2<>(right.y, -right.x).normalize(radius)).angle();
  obstacles[obstacleCount++] = Obstacle(left, right, pos, leftAngle, rightAngle);
}
Obstacle::Obstacle(Vector3 A, Vector3 B, Vector3 C, double H, unsigned int ABpoints, unsigned int BCpoints, unsigned int Hpoints)//A,B,C should on the xy plane
{
	if(ABpoints<= 0 || BCpoints<=0 || Hpoints<=0)
	{
		std::cout << "error at creating Cube" <<std::endl;
	}
	center = (A+C)/2 + Vector3(0,0,-1)*H/2;
	radius = sqrt((A - C).Magnitude()*(A - C).Magnitude()/4+H*H/4);

	for(unsigned int i=0; i<Hpoints+1; i++)
	{
		Vector3 Ai = A + Vector3(0,0,-1)*H/Hpoints*i;
		Vector3 Bi = B + Vector3(0,0,-1)*H/Hpoints*i;
		Vector3 Ci = C + Vector3(0,0,-1)*H/Hpoints*i;
		Components.push_back(Obstacle(Ai,Bi,Ci,ABpoints,BCpoints));
	}
}
Exemplo n.º 11
0
void Obstacles::Update(float dtime)
{
	for (unsigned i = 0; i < m_obstacles.size(); ++i)
	{
		m_obstacles[i].Update(dtime);
	}

	if (m_obstacles[m_obstacles.size() - 1].RequireNewObstacle())
	{
		m_obstacles.push_back(Obstacle());
	}

	if (m_obstacles[0].RequireDelete())
	{
		m_obstacles.erase(m_obstacles.begin());
	}
}
Exemplo n.º 12
0
void CinematicMap::SetLaser3D(PointCloud kinectData)
{
    //Obtain points from kinect with PCL
    
    //....
    
    //Filter with PCL
    
    //...
    
    //And then, fill pointsCloud with points
    pointsCloud=kinectData.points;
    
    rangeCloud.resize(pointsCloud.size());
    for(int i=0;i<pointsCloud.size();i++)
    {
        //Calculate the r²=x²+y² for know the range of all points
        
        double xx=pointsCloud[i].x*pointsCloud[i].x;
        double yy=pointsCloud[i].y*pointsCloud[i].y;
        double range=sqrt(xx+yy);
        rangeCloud[i]=range;
        
        //rangeCloud[i]=5.0;      //Obstacle always
        
    }
    
    angleCloud.resize(pointsCloud.size());
    for(int i=0;i<pointsCloud.size();i++)
    {
        angleCloud[i]=mr::Angle();      //Angle empty
    }
    
    
    Obstacle();
    
    
}
Exemplo n.º 13
0
rbt::pose<double> COccupancyGridWithObstacleList::fit(rbt::pose<double> const& poseWorld, SScanLine const& scanline) {
    if(m_iEndSorted<m_vecptfOccupied.size()) {
        auto itptfEndSorted = m_vecptfOccupied.begin()+m_iEndSorted;
        std::sort(itptfEndSorted, m_vecptfOccupied.end());
        m_vecptfOccupied.erase(std::unique(itptfEndSorted, m_vecptfOccupied.end()), m_vecptfOccupied.end());
        std::inplace_merge(m_vecptfOccupied.begin(), itptfEndSorted, m_vecptfOccupied.end());
        m_iEndSorted = m_vecptfOccupied.size();
    }
    
    if(m_vecptfOccupied.size()<10) return poseWorld;
    
    std::vector<rbt::point<double>> vecptfTemplate;
    scanline.ForEachScan(poseWorld, [&](rbt::pose<double> const& poseScan, double fRadAngle, int nDistance) {
        vecptfTemplate.emplace_back(ToGridCoordinate(Obstacle(poseScan, fRadAngle, nDistance)));
    });
        
#ifdef ENABLE_SCANMATCH_LOG
    static int c_nCount = 0;
    {
        std::stringstream ss;
        ss << "scanmatch" << c_nCount << "_a.png";
        DebugOutputScan(ObstacleMap(), vecptfTemplate, ss.str().c_str());
    }
    LOG(c_nCount);
#endif
        
    rbt::pose<double> poseGrid(ToGridCoordinate(poseWorld));
        
    // Transformation matrix from pose
    Matrix R = Matrix::eye(2);
    Matrix t(2,1);
    static_assert(sizeof(rbt::point<double>)==2*sizeof(double), "");
        
    // Use libicp, an iterative closest point implementation (http://www.cvlibs.net/software/libicp/)
    IcpPointToPoint icp(&m_vecptfOccupied[0].x, m_vecptfOccupied.size(), 2);
    icp.fit(&vecptfTemplate[0].x,vecptfTemplate.size(), R, t, 250);
    
#ifdef ENABLE_SCANMATCH_LOG
    LOG("ICP: R = " << R << " t = " << t);
#endif

    // Pose from transformation matrix
    Matrix vecLastPose = (R * Matrix(2, 1, &poseGrid.m_pt.x)) + t;
    rbt::pose<double> const poseWorldCorrected(
        ToWorldCoordinate(
            rbt::pose<double>(
                rbt::point<double>(vecLastPose.val[0][0], vecLastPose.val[1][0]),
                poseWorld.m_fYaw - asin(R.val[0][1])
            )
        ));
    
#ifdef ENABLE_SCANMATCH_LOG
    LOG(" Corrected Pose: " << poseWorldCorrected);
     {
     
        std::vector<rbt::point<double>> vecptfTemplateCorrected;
        scanline.ForEachScan(poseWorldCorrected, [&](rbt::pose<double> const& poseScan, double fRadAngle, int nDistance) {
            vecptfTemplateCorrected.emplace_back(ToGridCoordinate(Obstacle(poseScan, fRadAngle, nDistance)));
        });

        std::stringstream ss;
        ss << "scanmatch" << c_nCount << "_b.png";
        DebugOutputScan(ObstacleMap(), vecptfTemplateCorrected, ss.str().c_str());
    }
    ++c_nCount;
#endif

    return poseWorldCorrected;
}
Exemplo n.º 14
0
Obstacles::Obstacles()
{
	m_obstacles.push_back(Obstacle());
}
Exemplo n.º 15
0
/**
 * @brief Loads the tileset from its file by creating all tile patterns.
 */
void Tileset::load() {

  // compute the file name, depending on the id
  std::ostringstream oss;
  oss << "tilesets/tileset" << std::setfill('0') << std::setw(4) << id << ".dat";

  // open the tileset file
  std::string file_name = oss.str();
  std::istream &tileset_file = FileTools::data_file_open(file_name);

  // parse the tileset file
  std::string line;

  // first line: tileset general info
  if (!std::getline(tileset_file, line)) {
    Debug::die(StringConcat() << "Empty file '" << file_name << "'");
  }

  int r, g, b;

  std::istringstream iss(line);
  FileTools::read(iss, r);
  FileTools::read(iss, g);
  FileTools::read(iss, b);
  background_color = Color(r, g, b);

  // read the tile patterns
  int tile_pattern_id, animation, obstacle, default_layer;
  while (std::getline(tileset_file, line)) {

    iss.str(line);
    iss.clear();
    FileTools::read(iss, tile_pattern_id);
    FileTools::read(iss, animation);
    FileTools::read(iss, obstacle);
    FileTools::read(iss, default_layer);

    int width, height;

    if (animation != 1) { // simple tile pattern, parallax scrolling or scrolling

      int x, y;

      FileTools::read(iss, x);
      FileTools::read(iss, y);
      FileTools::read(iss, width);
      FileTools::read(iss, height);

      TilePattern *pattern = NULL;
      if (animation == 0) {
	pattern = new SimpleTilePattern(Obstacle(obstacle), x, y, width, height);
      }
      else if (animation == 2) {
	pattern = new ParallaxTilePattern(Obstacle(obstacle), x, y, width, height);
      }
      else if (animation == 3) {
        pattern = new ScrollingTilePattern(Obstacle(obstacle), x, y, width, height);
      }
      else {
	Debug::die(StringConcat() << "Unknown tile pattern animation: " << animation);
      }
      add_tile_pattern(tile_pattern_id, pattern);
    }
    else { // animated tile pattern
      int sequence, x1, y1, x2, y2, x3, y3;

      FileTools::read(iss, sequence);
      FileTools::read(iss, width);
      FileTools::read(iss, height);
      FileTools::read(iss, x1);
      FileTools::read(iss, y1);
      FileTools::read(iss, x2);
      FileTools::read(iss, y2);
      FileTools::read(iss, x3);
      FileTools::read(iss, y3);
      add_tile_pattern(tile_pattern_id, new AnimatedTilePattern(Obstacle(obstacle),
	    AnimatedTilePattern::AnimationSequence(sequence),
	    width, height, x1, y1, x2, y2, x3, y3));
    }
  }
  FileTools::data_file_close(tileset_file);

  // load the tileset images
  oss.str("");
  oss << "tilesets/tileset" << std::setfill('0') << std::setw(4) << id << "_tiles.png";
  tiles_image = new Surface(oss.str(), Surface::DIR_DATA);

  oss.str("");
  oss << "tilesets/tileset" << std::setfill('0') << std::setw(4) << id << "_entities.png";
  entities_image = new Surface(oss.str(), Surface::DIR_DATA);
}