void PointGrid::getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, vector< list<geometry_msgs::Point32>* >& points){
points.clear();
//compute the other corners of the box so we can get cells indicies for them
geometry_msgs::Point upper_left, lower_right;
upper_left.x = lower_left.x;
upper_left.y = upper_right.y;
lower_right.x = upper_right.x;
lower_right.y = lower_left.y;
//get the grid coordinates of the cells matching the corners of the range
unsigned int gx, gy;
//if the grid coordinates are outside the bounds of the grid... return
if(!gridCoords(lower_left, gx, gy))
return;
//get the associated index
unsigned int lower_left_index = gridIndex(gx, gy);
if(!gridCoords(lower_right, gx, gy))
return;
unsigned int lower_right_index = gridIndex(gx, gy);
if(!gridCoords(upper_left, gx, gy))
return;
unsigned int upper_left_index = gridIndex(gx, gy);
//compute x_steps and y_steps
unsigned int x_steps = lower_right_index - lower_left_index + 1;
unsigned int y_steps = (upper_left_index - lower_left_index) / width_ + 1;
/*
* (0, 0) ---------------------- (width, 0)
* | |
* | |
* | |
* | |
* | |
* (0, height) ----------------- (width, height)
*/
//get an iterator
vector< list<geometry_msgs::Point32> >::iterator cell_iterator = cells_.begin() + lower_left_index;
//printf("Index: %d, Width: %d, x_steps: %d, y_steps: %d\n", lower_left_index, width_, x_steps, y_steps);
for(unsigned int i = 0; i < y_steps; ++i){
for(unsigned int j = 0; j < x_steps; ++j){
list<geometry_msgs::Point32>& cell = *cell_iterator;
//if the cell contains any points... we need to push them back to our list
if(!cell.empty()){
points.push_back(&cell);
}
if(j < x_steps - 1)
cell_iterator++; //move a cell in the x direction
}
cell_iterator += width_ - (x_steps - 1); //move down a row
}
}
void PointGrid::insert(geometry_msgs::Point32 pt){
//get the grid coordinates of the point
unsigned int gx, gy;
//if the grid coordinates are outside the bounds of the grid... return
if(!gridCoords(pt, gx, gy))
return;
//if the point is too close to its nearest neighbor... return
if(nearestNeighborDistance(pt) < sq_min_separation_)
return;
//get the associated index
unsigned int pt_index = gridIndex(gx, gy);
//insert the point into the grid at the correct location
cells_[pt_index].push_back(pt);
//printf("Index: %d, size: %d\n", pt_index, cells_[pt_index].size());
}
double PointGrid::nearestNeighborDistance(geometry_msgs::Point32& pt){
//get the grid coordinates of the point
unsigned int gx, gy;
gridCoords(pt, gx, gy);
//get the bounds of the grid cell in world coords
geometry_msgs::Point lower_left, upper_right;
getCellBounds(gx, gy, lower_left, upper_right);
//now we need to check what cells could contain the nearest neighbor
geometry_msgs::Point32 check_point;
double sq_dist = DBL_MAX;
double neighbor_sq_dist = DBL_MAX;
//left
if(gx > 0){
check_point.x = lower_left.x;
check_point.y = pt.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy));
}
//upper left
if(gx > 0 && gy < height_ - 1){
check_point.x = lower_left.x;
check_point.y = upper_right.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy + 1));
}
//top
if(gy < height_ - 1){
check_point.x = pt.x;
check_point.y = upper_right.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy + 1));
}
//upper right
if(gx < width_ - 1 && gy < height_ - 1){
check_point.x = upper_right.x;
check_point.y = upper_right.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy + 1));
}
//right
if(gx < width_ - 1){
check_point.x = upper_right.x;
check_point.y = pt.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy));
}
//lower right
if(gx < width_ - 1 && gy > 0){
check_point.x = upper_right.x;
check_point.y = lower_left.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy - 1));
}
//bottom
if(gy > 0){
check_point.x = pt.x;
check_point.y = lower_left.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy - 1));
}
//lower left
if(gx > 0 && gy > 0){
check_point.x = lower_left.x;
check_point.y = lower_left.y;
sq_dist = sq_distance(pt, check_point);
if(sq_dist < sq_min_separation_)
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy - 1));
}
//we must also check within the cell we're in for a nearest neighbor
neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy));
return neighbor_sq_dist;
}
int main() {
Ptr<CTinyJS> js = make<CTinyJS>();
registerFunctions(js.get());
registerMathFunctions(js.get());
registerStringFunctions(js.get());
//Add print function.
js->addNative("function print(str)", js::funcs::print, 0);
//Create console object.
js->executeSafe("var console = {log: print};console.log('Console created.');");
sf::RenderWindow window;
window.create(sf::VideoMode(1280, 800), "ALife");
//window.setVerticalSyncEnabled(true);
window.setFramerateLimit(60);
sf::Clock clock;
float dt = 0.017f;
ants::World world;
std::shared_ptr<sf::Texture> gridTexture = std::make_shared<sf::Texture>();
gridTexture->loadFromFile("resources/grid.png");
std::vector<sf::Color> colors(4);
colors[0] = sf::Color::Cyan;
colors[1] = sf::Color::Red;
colors[2] = sf::Color::Magenta;
colors[3] = sf::Color::Green;
world.create(128, 128, colors, gridTexture);
for (int i = 0; i < 5; i++) {
std::uniform_int_distribution<int> nestDistX(1, world.getWidth() - 2);
std::uniform_int_distribution<int> nestDistY(1, world.getHeight() - 2);
sf::Vector2i pos = sf::Vector2i(nestDistX(world._generator), nestDistY(world._generator));
for (int dx = -1; dx <= 1; dx++)
for (int dy = -1; dy <= 1; dy++) {
Ptr<ants::Nest> nest = make<ants::Nest>();
world.add(nest, sf::Vector2i(pos.x + dx, pos.y + dy));
}
for (int i = 0; i < 50; i++) {
Ptr<ants::TestAnt> testAnt = make<ants::TestAnt>();
world.add(testAnt, pos);
}
}
for (int i = 0; i < 8; i++) {
std::uniform_int_distribution<int> foodDistX(1, world.getWidth() - 2);
std::uniform_int_distribution<int> foodDistY(1, world.getHeight() - 2);
sf::Vector2i pos = sf::Vector2i(foodDistX(world._generator), foodDistY(world._generator));
for (int dx = -1; dx <= 1; dx++)
for (int dy = -1; dy <= 1; dy++) {
Ptr<ants::Food> food = make<ants::Food>();
world.add(food, sf::Vector2i(pos.x + dx, pos.y + dy));
}
}
Ptr<ants::JSAnt> jsAnt = std::make_shared<ants::JSAnt>(js, "js/ant.js");
world.add(jsAnt, sf::Vector2i(32, 32));
sf::View view;
sf::View newView;
view.setSize(sf::Vector2f(static_cast<float>(window.getSize().x), static_cast<float>(window.getSize().y)));
view.setCenter(sf::Vector2f(world.getWidth() * 0.5f * world.getCellWidth(), world.getHeight() * 0.5f * world.getCellHeight()));
newView = view;
float zoomRate = 0.2f;
float viewInterpolateRate = 20.0f;
sf::Vector2i prevMousePos = sf::Mouse::getPosition();
float updateTimer = 0.0f;
const float updateTime = 0.1f;
bool quit = false;
do {
sf::Event windowEvent;
while (window.pollEvent(windowEvent)) {
switch (windowEvent.type)
{
case sf::Event::Closed:
quit = true;
break;
case sf::Event::MouseWheelMoved:
int dWheel = windowEvent.mouseWheel.delta;
newView = view;
window.setView(newView);
sf::Vector2f mouseZoomDelta0 = window.mapPixelToCoords(sf::Mouse::getPosition(window));
newView.setSize(view.getSize() + newView.getSize() * (-zoomRate * dWheel));
window.setView(newView);
sf::Vector2f mouseZoomDelta1 = window.mapPixelToCoords(sf::Mouse::getPosition(window));
window.setView(view);
newView.setCenter(view.getCenter() + mouseZoomDelta0 - mouseZoomDelta1);
break;
}
}
view.setCenter(view.getCenter() + (newView.getCenter() - view.getCenter()) * viewInterpolateRate * dt);
view.setSize(view.getSize() + (newView.getSize() - view.getSize()) * viewInterpolateRate * dt);
if (sf::Mouse::isButtonPressed(sf::Mouse::Left)) {
sf::Vector2i dPos = sf::Mouse::getPosition() - prevMousePos;
view.setCenter(view.getCenter() - sf::Vector2f(dPos.x * (view.getSize().x / window.getSize().x), dPos.y * (view.getSize().x / window.getSize().x)));
newView.setCenter(view.getCenter());
}
if (sf::Mouse::isButtonPressed(sf::Mouse::Right)) {
sf::Vector2f mouseCoords = window.mapPixelToCoords(sf::Mouse::getPosition(window));
sf::Vector2i gridCoords(mouseCoords.x / world.getCellWidth(), mouseCoords.y / world.getCellHeight());
if (gridCoords.x >= 0 && gridCoords.y >= 0 && gridCoords.x < world.getWidth() && gridCoords.y < world.getHeight()) {
bool hasObstacle = false;
for (std::set<int>::const_iterator cit = world.getCell(gridCoords)._occupantIds.begin(); cit != world.getCell(gridCoords)._occupantIds.end(); cit++) {
if (world.getGridObject(*cit)->getName() == "obstacle") {
hasObstacle = true;
break;
}
}
if (!hasObstacle) {
Ptr<ants::Obstacle> obstacle = make<ants::Obstacle>();
world.add(obstacle, gridCoords);
}
}
}
prevMousePos = sf::Mouse::getPosition();
window.clear();
window.setView(view);
updateTimer += dt;
if (updateTimer > updateTime) {
world.update();
updateTimer -= updateTime;
}
world.render(window);
window.display();
dt = clock.getElapsedTime().asSeconds();
clock.restart();
} while (!quit);
return 0;
}
