void InitMyWorld()
{
//Intializing test environment Faces included in a FacePart
Face suelo(Transformation3D(0,0,0),-20,-20,20,20);
suelo.setColor(0.3f, 0.3f, 0.4f, 1);
Face paredfondo1(Transformation3D(-20,0,0,Y_AXIS,PI/2),-3,-20,0,20);
Face paredfondo2(Transformation3D(20,0,0,Y_AXIS,PI/2),-3,-20,0,20);
Face paredfondo3(Transformation3D(20,-20,0,X_AXIS,PI/2),0, 0, -40, 3);
Face paredfondo4(Transformation3D(-20,20,0,X_AXIS,PI/2),0, 0, 40, 3);
paredfondo1.setColor(0.5f, 0.5f, 0.0f, 1);
paredfondo2.setColor(0.5f, 0.5f, 0.0f, 1);
paredfondo3.setColor(0.5f, 0.5f, 0.0f, 1);
paredfondo4.setColor(0.5f, 0.5f, 0.0f, 1);
FaceSetPart *building=new FaceSetPart;
building->addFace(suelo);
building->addFace(paredfondo1);
building->addFace(paredfondo2);
building->addFace(paredfondo3);
building->addFace(paredfondo4);
for (int i = 0; i < 2; i++) {
PrismaticPart *mypart = new PrismaticPart;
vector<Vector2D> list;
list.push_back(Vector2D(0, 0));
list.push_back(Vector2D(0, 2));
list.push_back(Vector2D(2, 2));
list.push_back(Vector2D(2, 0));
mypart->setPolygonalBase(list);
mypart->setRelativePosition(Vector3D(6+4*i, 4+6*i, 0));
mypart->setRelativeOrientation(0, 0, PI / 2);
mypart->setHeight(1);
world += mypart;
}
PrismaticPart *mypart = new PrismaticPart;
vector<Vector2D> list;
list.push_back(Vector2D(0, 0));
list.push_back(Vector2D(0, 0.5));
list.push_back(Vector2D(0.5, 0.5));
list.push_back(Vector2D(0.5, 0));
mypart->setPolygonalBase(list);
mypart->setRelativePosition(Vector3D(6.5, 8.5, 0));
mypart->setRelativeOrientation(0, 0, PI / 2);
mypart->setHeight(1);
world += mypart;
world+=building;
StreamFile myfile("myRoom.world",false);
world.writeToStream(myfile);
//myfile.write(&world);fails: FIXME
}
void WheeledBaseSim::drawGL()
{
//constructor: create a physical representation of the robot only for drawing
double height=wheel_radius*3;
double clearance=wheel_radius;
vector<Vector2D> list;
PrismaticPart bod;
list.push_back(Vector2D(large/2,width/2));
list.push_back(Vector2D(large/2,-width/2));
list.push_back(Vector2D(-large/2,-width/2));
list.push_back(Vector2D(-large/2,width/2));
bod.setPolygonalBase(list);
bod.setHeight(height-clearance);
bod.setRelativePosition(Vector3D(0,0,clearance));
bod.setColor(1,0,0);
//las ruedas no se pueden añadir hasta no tener un mecanismo de exclusión de detección
CylindricalPart wheel(wheel_width,wheel_radius);
wheel.setColor(0.1,0.1,0.1);
glPushMatrix();
getAbsoluteT3D().transformGL();
//body
bod.drawGL();
//each wheel
wheel.setRelativeOrientation(X_AXIS,-PI/2);
wheel.setRelativePosition(Vector3D(large/2,width/2,wheel_radius+0.002));
wheel.drawGL();
wheel.setRelativeOrientation(X_AXIS,-PI/2);
wheel.setRelativePosition(Vector3D(-large/2,width/2,wheel_radius+0.002));
wheel.drawGL();
wheel.setRelativeOrientation(X_AXIS,PI/2);
wheel.setRelativePosition(Vector3D(large/2,-width/2,wheel_radius+0.002));
wheel.drawGL();
wheel.setRelativeOrientation(X_AXIS,PI/2);
wheel.setRelativePosition(Vector3D(-large/2,-width/2,wheel_radius+0.002));
wheel.drawGL();
glPopMatrix();
return;
}