void ColCylinder::Synchronize( bool bFromSolver )
{
ColGeom::Synchronize( bFromSolver );
if (!bFromSolver)
{
float scale = GetBodyScale()*PhysicsServer::s_pInstance->GetWorldScale();
dGeomCylinderSetParams( GetID(), m_Radius*scale, m_Height*scale );
}
} // ColCylinder::Synchronize
void PhyCylinder::SetCylinderParam(float radius, float length)
{
dGeomCylinderSetParams(mOdeGeom, radius, length);
}
//! setting of scale
void SSimEntity::setScale(Vector3d scale)
{
m_scale.set(scale.x(), scale.y(), scale.z());
// if a part is already added
int geomNum = getGeomNum();
if (geomNum != 0) {
dBodyID body = m_parts.body;
// loop for all of the parts
for (int i = 0; i < geomNum; i++) {
// refer the geometry
dGeomID geom = m_parts.geoms[i];
// refer the position (gap from CoG)
const dReal *pos = dGeomGetPosition(geom);
// Reflection of scale
dGeomSetPosition(geom, pos[0]*scale.x(), pos[1]*scale.y(), pos[2]*scale.z());
// Refer the type of the geometory
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
// average of scale
double mean = (scale.x() + scale.y() + scale.z())/ 3;
// refer the radius
dReal radius = dGeomSphereGetRadius(geom);
// reflection of scale
dGeomSphereSetRadius(geom, radius*mean);
}
// box
else if (type == 1) {
// refer the size
dVector3 size;
dGeomBoxGetLengths(geom, size);
// reflection of scale
dGeomBoxSetLengths(geom, size[0]*scale.x(), size[1]*scale.y(), size[2]*scale.z());
}
// cylinder
else if (type == 3) {
dReal radius, length;
dGeomCylinderGetParams(geom, &radius, &length);
// average of scale in horizontal plane
double mean = (scale.x() + scale.z()) / 2;
// TODO: confirm: is 2 suitable for long axis?
dGeomCylinderSetParams(geom, radius*mean, length*scale.y());
}
} // for (int i = 0; i < partsNum; i++) {
}
}
bool BodyVerifier::verify(const RobotRepresentation &robotRep, int &errorCode,
std::vector<std::pair<std::string, std::string> > &affectedBodyParts) {
bool success = true;
errorCode = INTERNAL_ERROR;
// Initialize ODE
dInitODE();
dWorldID odeWorld = dWorldCreate();
dWorldSetGravity(odeWorld, 0, 0, 0);
dSpaceID odeSpace = dHashSpaceCreate(0);
CollisionData *collisionData = new CollisionData();
/* don't want visual debugging
#ifdef VISUAL_DEBUG
// Initialize OSG
osgViewer::Viewer viewer;
viewer.setUpViewInWindow(200, 200, 800, 600);
osg::ref_ptr<KeyboardHandler> keyboardEvent(new KeyboardHandler());
viewer.addEventHandler(keyboardEvent.get());
osg::ref_ptr<osg::Camera> camera = viewer.getCamera();
#endif*/
// parse robot message
robogenMessage::Robot robotMessage = robotRep.serialize();
// parse robot
boost::shared_ptr<Robot> robot(new Robot);
if (!robot->init(odeWorld, odeSpace, robotMessage)) {
std::cout << "Problem when initializing robot in body verifier!"
<< std::endl;
success = false;
}
if (success) {
std::vector<boost::shared_ptr<Model> > bodyParts = robot->getBodyParts();
/* no need for this
#ifdef VISUAL_DEBUG
// body part rendering
osg::ref_ptr<osg::Group> root = osg::ref_ptr<osg::Group>(new osg::Group);
std::vector<boost::shared_ptr<RenderModel> > renderModels;
for (unsigned int i = 0; i < bodyParts.size(); ++i) {
boost::shared_ptr<RenderModel> renderModel =
RobogenUtils::createRenderModel(bodyParts[i]);
if (renderModel == NULL) {
std::cout << "Cannot create a render model for model " << i
<< std::endl;
}
if (!renderModel->initRenderModel()) {
std::cout
<< "Cannot initialize a render model for one of the components. "
<< std::endl
<< "Please check that the models/ folder exists in the parent folder of this executable."
<< std::endl;
}
renderModels.push_back(renderModel);
root->addChild(renderModels[i]->getRootNode());
}
// viewer setup
viewer.setSceneData(root.get());
viewer.realize();
if (!viewer.getCameraManipulator()
&& viewer.getCamera()->getAllowEventFocus()) {
viewer.setCameraManipulator(new osgGA::TrackballManipulator());
}
viewer.setReleaseContextAtEndOfFrameHint(false);
#endif*/
// build map from ODE bodies to body part ID's: needed to identify
// offending body parts
std::map<dBodyID, std::string> dBodyToPartID;
for (unsigned int i = 0; i < bodyParts.size(); ++i) {
std::vector<dBodyID> dBodies = bodyParts[i]->getBodies();
for (unsigned int j = 0; j < dBodies.size(); ++j) {
dBodyToPartID[dBodies[j]] = bodyParts[i]->getId();
}
}
dSpaceCollide(odeSpace, (void *) collisionData, collisionCallback);
if (collisionData->offendingBodies.size()) {
success = false;
errorCode = SELF_INTERSECTION;
} else if(collisionData->cylinders.size() > 0) {
// hack to make sure we have separation between wheels
for(unsigned int i=0; i<collisionData->cylinders.size(); ++i) {
dReal radius;
dReal length;
dGeomCylinderGetParams(collisionData->cylinders[i],
&radius, &length);
dGeomCylinderSetParams(collisionData->cylinders[i],
radius + WHEEL_SEPARATION, length);
}
dSpaceCollide(odeSpace, (void *) collisionData, collisionCallback);
if ( collisionData->offendingBodies.size() ) {
success = false;
errorCode = SELF_INTERSECTION;
}
collisionData->cylinders.clear();
}
for (unsigned int i = 0; i < collisionData->offendingBodies.size(); i++) {
// TODO unlikely event that body not found in map?
affectedBodyParts.push_back(
std::pair<std::string, std::string>(
dBodyToPartID[collisionData->offendingBodies[i].first],
dBodyToPartID[collisionData->offendingBodies[i].second
]));
}
/*
#ifdef VISUAL_DEBUG
// show robot in viewer
while (!keyboardEvent->isQuit() && !viewer.done()) {
viewer.frame();
};
#endif certainly don't need */
}
// destruction
collisionData->offendingBodies.clear();
delete collisionData;
robot.reset();
dSpaceDestroy(odeSpace);
dWorldDestroy(odeWorld);
dCloseODE();
return success;
}
