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);
};
}
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));
}
}
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();
}
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);
}
/*
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);
}
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));
}
}
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());
}
}
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();
}
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;
}
Obstacles::Obstacles()
{
m_obstacles.push_back(Obstacle());
}
/**
* @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);
}
