예제 #1
0
파일: colcylinder.cpp 프로젝트: skopp/rush
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
예제 #2
0
	void PhyCylinder::SetCylinderParam(float radius, float length)
	{
		dGeomCylinderSetParams(mOdeGeom, radius, length);
	}
예제 #3
0
//! 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++) {
	}
}
예제 #4
0
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;
}