Diff: odephysics.h - odephysics.h

	odephysics.h	odephysics.h

	skipping to change at line 250	skipping to change at line 250
	virtual int GetPhysicsOptions() const	virtual int GetPhysicsOptions() const
	{	{
	return _options;	return _options;
	}	}

	virtual bool SetPhysicsOptions(std::ostream& sout, std::istream& sinput )	virtual bool SetPhysicsOptions(std::ostream& sout, std::istream& sinput )
	{	{
	return false;	return false;
	}	}


		virtual void Clone(InterfaceBaseConstPtr preference, int cloningoptions
		)
		{
		PhysicsEngineBase::Clone(preference,cloningoptions);
		boost::shared_ptr<ODEPhysicsEngine const> r = boost::dynamic_pointe
		r_cast<ODEPhysicsEngine const>(preference);

		SetGravity(r->_gravity);
		_options = r->_options;
		_globalfriction = r->_globalfriction;
		_globalcfm = r->_globalcfm;
		_globalerp = r->_globalerp;
		if( !!_odespace && _odespace->IsInitialized() ) {
		dWorldSetERP(_odespace->GetWorld(),_globalerp);
		dWorldSetCFM(_odespace->GetWorld(),_globalcfm);
		}
		}

	virtual bool SetLinkVelocity(KinBody::LinkPtr plink, const Vector& _lin earvel, const Vector& angularvel)	virtual bool SetLinkVelocity(KinBody::LinkPtr plink, const Vector& _lin earvel, const Vector& angularvel)
	{	{
	_odespace->Synchronize(KinBodyConstPtr(plink->GetParent()));	_odespace->Synchronize(KinBodyConstPtr(plink->GetParent()));
	dBodyID body = _odespace->GetLinkBody(plink);	dBodyID body = _odespace->GetLinkBody(plink);
	if( !body ) {	if( !body ) {
	return false;	return false;
	}	}
	Vector linearvel = _linearvel + angularvel.cross(plink->GetTransfor m()*plink->GetCOMOffset() - plink->GetTransform().trans);	Vector linearvel = _linearvel + angularvel.cross(plink->GetTransfor m()*plink->GetCOMOffset() - plink->GetTransform().trans);
	dBodySetLinearVel(body, linearvel.x,linearvel.y,linearvel.z);	dBodySetLinearVel(body, linearvel.x,linearvel.y,linearvel.z);
	dBodySetAngularVel(body, angularvel.x, angularvel.y, angularvel.z);	dBodySetAngularVel(body, angularvel.x, angularvel.y, angularvel.z);

	skipping to change at line 417	skipping to change at line 433
	_odespace->Synchronize(KinBodyConstPtr(pjoint->GetParent()));	_odespace->Synchronize(KinBodyConstPtr(pjoint->GetParent()));
	std::vector<dReal> vtorques(pTorques.size());	std::vector<dReal> vtorques(pTorques.size());
	std::copy(pTorques.begin(),pTorques.end(),vtorques.begin());	std::copy(pTorques.begin(),pTorques.end(),vtorques.begin());
	_jointadd[dJointGetType(joint)](joint, &vtorques[0]);	_jointadd[dJointGetType(joint)](joint, &vtorques[0]);
	return true;	return true;
	}	}

	virtual void SetGravity(const Vector& gravity)	virtual void SetGravity(const Vector& gravity)
	{	{
	_gravity = gravity;	_gravity = gravity;

	if( _odespace->IsInitialized() ) {	if( !!_odespace && _odespace->IsInitialized() ) {
	dWorldSetGravity(_odespace->GetWorld(),_gravity.x, _gravity.y, _gravity.z);	dWorldSetGravity(_odespace->GetWorld(),_gravity.x, _gravity.y, _gravity.z);
	}	}
	}	}

	virtual Vector GetGravity()	virtual Vector GetGravity()
	{	{
	return _gravity;	return _gravity;
	}	}

	virtual void SimulateStep(OpenRAVE::dReal fTimeElapsed)	virtual void SimulateStep(OpenRAVE::dReal fTimeElapsed)

	skipping to change at line 461	skipping to change at line 477
	}	}

	dWorldQuickStep(_odespace->GetWorld(), fTimeElapsed);	dWorldQuickStep(_odespace->GetWorld(), fTimeElapsed);
	dJointGroupEmpty (_odespace->GetContactGroup());	dJointGroupEmpty (_odespace->GetContactGroup());

	// synchronize all the objects from the ODE world to the OpenRAVE w orld	// synchronize all the objects from the ODE world to the OpenRAVE w orld
	Transform t;	Transform t;
	FOREACHC(itbody, vbodies) {	FOREACHC(itbody, vbodies) {
	ODESpace::KinBodyInfoPtr pinfo = GetPhysicsInfo(*itbody);	ODESpace::KinBodyInfoPtr pinfo = GetPhysicsInfo(*itbody);
	BOOST_ASSERT( pinfo->vlinks.size() == (*itbody)->GetLinks().siz e());	BOOST_ASSERT( pinfo->vlinks.size() == (*itbody)->GetLinks().siz e());

		vector<Transform> vtrans(pinfo->vlinks.size());
	for(size_t i = 0; i < pinfo->vlinks.size(); ++i) {	for(size_t i = 0; i < pinfo->vlinks.size(); ++i) {
	const dReal* prot = dBodyGetQuaternion(pinfo->vlinks[i]->bo dy);	const dReal* prot = dBodyGetQuaternion(pinfo->vlinks[i]->bo dy);
	Vector vrot(prot[0],prot[1],prot[2],prot[3]);	Vector vrot(prot[0],prot[1],prot[2],prot[3]);
	if( vrot.lengthsqr4() == 0 ) {	if( vrot.lengthsqr4() == 0 ) {
	RAVELOG_ERROR(str(boost::format("odephysics in body %s is returning invalid rotation!")%(*itbody)->GetName()));	RAVELOG_ERROR(str(boost::format("odephysics in body %s is returning invalid rotation!")%(*itbody)->GetName()));
	continue;	continue;
	}	}
	const dReal* ptrans = dBodyGetPosition(pinfo->vlinks[i]->bo dy);	const dReal* ptrans = dBodyGetPosition(pinfo->vlinks[i]->bo dy);

	(*itbody)->GetLinks()[i]->SetTransform(Transform(vrot,Vecto r(ptrans[0],ptrans[1],ptrans[2])));	vtrans.at(i) = Transform(vrot,Vector(ptrans[0],ptrans[1],pt rans[2]));
	}	}

		(*itbody)->SetLinkTransformations(vtrans,pinfo->_vdofbranches);
	pinfo->nLastStamp = (*itbody)->GetUpdateStamp();	pinfo->nLastStamp = (*itbody)->GetUpdateStamp();
	}	}

	_listcallbacks.clear();	_listcallbacks.clear();
	}	}

	private:	private:
	static ODESpace::KinBodyInfoPtr GetPhysicsInfo(KinBodyConstPtr pbody) {	static ODESpace::KinBodyInfoPtr GetPhysicsInfo(KinBodyConstPtr pbody) {
	return boost::dynamic_pointer_cast<ODESpace::KinBodyInfo>(pbody->Ge tPhysicsData());	return boost::dynamic_pointer_cast<ODESpace::KinBodyInfo>(pbody->Ge tPhysicsData());
	}	}

End of changes. 5 change blocks.
	3 lines changed or deleted	22 lines changed or added
This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/