void ClientSocket::addGraspableBody(const QString & bodyName, const QString & objectName){
QTextStream os(this);
Body * b = addToWorld(QString("models/objects/"), "GraspableBody", bodyName);
if(!b)
b = addToWorld("models/object_database/", QString("GraspableBody"), bodyName);
os << (b!=NULL) << " \n";
os.flush();
std::cout << "done adding graspable body \n";
b->setName(objectName);
}
bool ClientSocket::setPlannerTarget(const QString & bodyName)
{
World * w = graspItGUI->getIVmgr()->getWorld();
//remove the current planner target if it exists
if(w->getCurrentHand())
{
GraspableBody * currentObject = w->getCurrentHand()->getGrasp()->getObject();
if(currentObject)
{
w->getCurrentHand()->getGrasp()->setObject(NULL);
w->destroyElement(currentObject);
}
}
if (!addToWorld("models/objects/", QString("GraspableBody"), bodyName) && !addToWorld("models/object_database/", QString("GraspableBody"), bodyName))
return false; // failed to add body to world.
//The body has been added. Now load the table obstacle if necessary
//Find table obstacle and move it down
Body * b = NULL;
for(int i = 0; i < w->getNumBodies(); ++i)
{
if(w->getBody(i)->getName() == "experiment_table")
b = w->getBody(i);
}
if(b == NULL)
b = addToWorld("models/objects/", "Body", "experiment_table.xml");
if(b == NULL)
return false;
//move obstacle down below object
// Get most recent GB, which we have just added
GraspableBody * gb = w->getGB(w->getNumGB() - 1);
std::vector<position> vertices;
gb->getGeometryVertices(&vertices);
double zmin = 0;
for(unsigned int iv = 0; iv < vertices.size(); ++iv)
{
if(zmin > vertices[iv].z())
zmin = vertices[iv].z();
}
int collisionThreshold = 1;
transf t(mat3::IDENTITY,vec3(0,0,zmin - collisionThreshold));
b->setTran(t);
//Disable collisions between main hand and the experiment table
w->toggleCollisions(false, w->getCurrentHand(), b);
w->emitTargetBodyChanged(gb);
return true;
}
bool ServerClearCollidableFloraNotification::positionChanged (Object& object, bool /*dueToParentChange*/, const Vector& /*oldPosition*/) const
{
removeFromWorld (object);
addToWorld (object);
return true;
}
void NoBuildNotification::rotationChanged (Object& object, bool /*dueToParentChange*/) const
{
if (object.isInWorld ())
{
removeFromWorld (object);
addToWorld (object);
}
}
Platform *Map::getPlatform (MapTile *mapTile, int *start, int *end, gridSize offset)
{
const int mapY = mapTile->getGridY() + offset + EPSILON;
if (*start == -1 || *end == -1)
getPlatformDimensions(mapTile->getGridX(), mapTile->getGridY(), start, end);
PlatformYMapConstIter iy = _platforms.find(mapY);
if (iy != _platforms.end()) {
PlatformXMapConstIter ix = iy->second.find(*start);
if (ix != iy->second.end()) {
return ix->second;
}
}
info(LOG_MAP, String::format("create a new platform at %i:%i to %i:%i", *start, mapY, *end, mapY));
const int width = *end - *start + 1;
const gridSize height = 0.015f;
const gridCoord x = *start + width / 2.0f;
const gridSize y = mapTile->getGridY() + offset;
b2PolygonShape shape;
shape.SetAsBox(width / 2.0f, height);
b2FixtureDef fixture;
fixture.shape = &shape;
fixture.friction = 0.4f;
fixture.restitution = 0.0f;
fixture.density = 0.0f;
b2BodyDef bd;
bd.position.Set(x, y);
bd.type = b2_kinematicBody;
bd.fixedRotation = true;
Platform *platform = new Platform(*this);
addToWorld(fixture, bd, platform);
loadEntity(platform);
_platforms[mapY][*start] = platform;
#ifdef DEBUG
PlatformYMapConstIter iy2 = _platforms.find(mapY);
assert(iy2 != _platforms.end());
PlatformXMapConstIter ix2 = iy2->second.find(*start);
assert(ix2 != iy->second.end());
#endif
return platform;
}
void Box2DSystem::update(ex::EntityManager&, ex::EventManager&, ex::TimeDelta dt)
{
//If we have unspawned entites, create bodies in the world for them each
for(ex::Entity e : unspawned)
addToWorld(e);
unspawned.clear();
//Step the world
const int32 velocityIterations = 8;
const int32 positionIterations = 5;
world->Step(dt, velocityIterations, positionIterations);
if(debugEnabled) {
world->DrawDebugData();
}
}
void World::
reset()
{
btCollisionObjectArray a = dynamicsWorld->getCollisionObjectArray();
for (int i = 0; i < a.size(); i++) {
auto o = a[i];
if (!o->isStaticOrKinematicObject()) {
std::cout << "Object: " << o->getUserPointer() << std::endl;
btTransform t = o->getWorldTransform();
btVector3 v = t.getOrigin();
if (v.getY() < -10) {
std::shared_ptr<app::gl::AppObject> new_object;
dynamicsWorld->removeCollisionObject(o);
for (auto i = objects.begin(); i != objects.end(); ++i) {
if (i->get() == o->getUserPointer()) {
new_object = std::make_shared<app::gl::AppObject>(*(i->get()));
v.setX(disx(gen));
v.setY(disy(gen));
v.setZ(disz(gen));
t.setOrigin(v);
new_object->setWorldTransform(t);
objects.erase(i);
break;
}
}
addToWorld(new_object,
btRigidBody::btRigidBodyConstructionInfo(new_object->getMass(),
nullptr, nullptr, new_object->getInitialInertia()));
} else {
v.setX(disx(gen));
v.setY(disy(gen));
v.setZ(disz(gen));
t.setOrigin(v);
o->setWorldTransform(t);
o->setInterpolationLinearVelocity(btVector3(0,0,0));
o->setInterpolationAngularVelocity(btVector3(0,0,0));
o->setActivationState(1);
o->activate(true);
}
}
}
}
void TextureSystem::update(ex::EntityManager&, ex::EventManager&, ex::TimeDelta)
{
//Untextured entities, deal with them
for(ex::Entity e : unspawned)
addToWorld(e);
unspawned.clear();
//Draw background first if enabled
if(imageRenderEnabled)
window.draw(bgSprite);
/* For each entity, the texture and position text info are updated from the
* Box2D component, if enabled */
auto box = ex::ComponentHandle<Box2DComponent>();
auto tex = ex::ComponentHandle<TextureComponent>();
for(ex::Entity e : entities.entities_with_components(box, tex))
{
(void)e;
b2Body* body = box->body;
b2Vec2 position = body->GetPosition();
sf::Vector2f adjusted = {pixels(position.x), pixels(position.y)};
if(imageRenderEnabled) {
sf::Sprite& sprite = tex->sprite;
sprite.setPosition(adjusted);
sprite.setRotation(body->GetAngle() * (180 / M_PI));
window.draw(sprite);
}
if(positionTextEnabled) {
char buffer[32];
std::snprintf(buffer, 32, "[%.3d,%.3d]", (int)adjusted.x, (int)adjusted.y);
sf::Text& text = tex->positionText;
text.setString(buffer);
text.setPosition(adjusted.x-28, adjusted.y-8);
window.draw(text);
}
}
}
void Graphics::addToScreen(int x, int y, std::string text, Camera * camera) {
addToWorld(x + camera->getX(), y + camera->getY(), text);
}
void ClientSocket::addObstacle(const QString & bodyName){
QTextStream os(this);
os << (addToWorld("models/obstacles/", "Body", bodyName)!=NULL) << '\n';
}
