[Opal-commits] opal/src/ODE ODESimulator.cpp,1.97,1.98 ODESimulator.h,1.69,1.70 ODESolid.cpp,1.83,1.84 ODESolid.h,1.68,1.69

[tylerstreeter <tyl...@us...>]

Update of /cvsroot/opal/opal/src/ODE
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4177/src/ODE

Modified Files:
	ODESimulator.cpp ODESimulator.h ODESolid.cpp ODESolid.h 
Log Message:
added a rotational stability fix for ODESolids

Index: ODESimulator.h
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESimulator.h,v
retrieving revision 1.69
retrieving revision 1.70
diff -C2 -d -r1.69 -r1.70
*** ODESimulator.h	29 Mar 2005 03:05:47 -0000	1.69
--- ODESimulator.h	12 Apr 2005 06:46:44 -0000	1.70
***************
*** 90,95 ****
  		virtual void OPAL_CALL setSolverAccuracy(SolverAccuracyLevel level);
  
- 		virtual void OPAL_CALL internal_stepPhysics();
- 
  		virtual dWorldID OPAL_CALL internal_getWorldID()const;
  
--- 90,93 ----
***************
*** 116,119 ****
--- 114,119 ----
  
  	protected:
+ 		virtual void stepPhysics();
+ 
  		/// The ODE world ID used by this Simulator.  
  		dWorldID mWorldID;

Index: ODESolid.h
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESolid.h,v
retrieving revision 1.68
retrieving revision 1.69
diff -C2 -d -r1.68 -r1.69
*** ODESolid.h	6 Apr 2005 04:48:45 -0000	1.68
--- ODESolid.h	12 Apr 2005 06:46:45 -0000	1.69
***************
*** 143,146 ****
--- 143,156 ----
  		const std::vector<GeomData*>* OPAL_CALL internal_getGeomDataList()const;
  
+ 		/// Fix angular velocities for freely-spinning bodies that have 
+ 		/// gained angular velocity through explicit integrator inaccuracy.  
+ 		/// This usually only happens for long, thin objects.
+ 		void OPAL_CALL internal_doAngularVelFix();
+ 
+ 		/// Sets whether this object is freely-spinning (i.e. no recent 
+ 		/// physical contacts, no user-generated forces, no user-defined 
+ 		/// velocity changes).
+ 		void OPAL_CALL internal_setFreelySpinning(bool fs);
+ 
  	protected:
  		/// Adds a new GeomData object to the internal list and sets up the 
***************
*** 158,161 ****
--- 168,174 ----
  		void addMass(dMass& newMass, const Matrix44r& offset);
  
+ 		/// Returns true if the given mass has a non-symmetric inertia tensor.
+ 		bool isInertiaNonSymmetric(const dMass& mass)const;
+ 
  		/// The ODE body ID.  
  		dBodyID mBodyID;
***************
*** 178,181 ****
--- 191,205 ----
  		bool mIsPlaceable;
  
+ 		/// True if the ODESolid has a non-symmetric inertia tensor.
+ 		bool mNonSymmetricInertia;
+ 
+ 		/// Used to improve ODE's angular velocity calculations for objects 
+ 		/// with non-symmetric inertia tensors.
+ 		bool mIsFreelySpinning;
+ 
+ 		/// Used to improve ODE's angular velocity calculations for objects 
+ 		/// with non-symmetric inertia tensors.
+ 		real mPrevAngVelMagSquared;
+ 
  	private:
  	};

Index: ODESimulator.cpp
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESimulator.cpp,v
retrieving revision 1.97
retrieving revision 1.98
diff -C2 -d -r1.97 -r1.98
*** ODESimulator.cpp	9 Apr 2005 06:23:58 -0000	1.97
--- ODESimulator.cpp	12 Apr 2005 06:46:44 -0000	1.98
***************
*** 126,130 ****
  	}
  
! 	void ODESimulator::internal_stepPhysics()
  	{
  		// Apply linear and angular damping; if using the "add opposing 
--- 126,130 ----
  	}
  
! 	void ODESimulator::stepPhysics()
  	{
  		// Apply linear and angular damping; if using the "add opposing 
***************
*** 253,257 ****
  		}
  
! 		//Do collision detection; add contacts to contact joint group
  		dSpaceCollide(mRootSpaceID, this, 
  			&ode_hidden::internal_collisionCallback);
--- 253,257 ----
  		}
  
! 		// Do collision detection; add contacts to the contact joint group.
  		dSpaceCollide(mRootSpaceID, this, 
  			&ode_hidden::internal_collisionCallback);
***************
*** 267,272 ****
  		}
  
! 		//Remove all joints from the contact group.
  		dJointGroupEmpty(mContactJointGroupID);
  	}
  
--- 267,283 ----
  		}
  
! 		// Remove all joints from the contact group.
  		dJointGroupEmpty(mContactJointGroupID);
+ 
+ 		// Fix angular velocities for freely-spinning bodies that have 
+ 		// gained angular velocity through explicit integrator inaccuracy.
+ 		for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter)
+ 		{
+ 			ODESolid* s = (ODESolid*)(*iter);
+ 			if (!s->isSleeping() && !s->isStatic())
+ 			{
+ 				s->internal_doAngularVelFix();
+ 			}
+ 		}
  	}
  
***************
*** 510,513 ****
--- 521,528 ----
  				if (makeContacts)
  				{
+ 					// Invalidate the "freely-spinning" parameters.
+ 					((ODESolid*)solid0)->internal_setFreelySpinning(false);
+ 					((ODESolid*)solid1)->internal_setFreelySpinning(false);
+ 
  					for(int i=0; i<numContacts; ++i)
  					{

Index: ODESolid.cpp
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESolid.cpp,v
retrieving revision 1.83
retrieving revision 1.84
diff -C2 -d -r1.83 -r1.84
*** ODESolid.cpp	10 Apr 2005 23:09:19 -0000	1.83
--- ODESolid.cpp	12 Apr 2005 06:46:44 -0000	1.84
***************
*** 43,46 ****
--- 43,49 ----
  		mIsPlaceable = true;
  		mCollisionCount = 0;
+ 		mNonSymmetricInertia = false;
+ 		mIsFreelySpinning = true;
+ 		mPrevAngVelMagSquared = 0;
  
  		if (!mData.isStatic)
***************
*** 480,483 ****
--- 483,489 ----
  			dMassTranslate(&m, offset[0], offset[1], offset[2]);
  			dBodySetMass(mBodyID, &m);
+ 
+ 			// Update this since the mass changed.
+ 			mNonSymmetricInertia = isInertiaNonSymmetric(m);
  		}
  	}
***************
*** 650,653 ****
--- 656,662 ----
  			dBodyVectorToWorld(mBodyID, vel[0], vel[1], vel[2], worldVel);
  			dBodySetLinearVel(mBodyID, worldVel[0], worldVel[1], worldVel[2]);
+ 
+ 			// Invalidate the "freely-spinning" parameter.
+ 			internal_setFreelySpinning(false);
  		}
  	}
***************
*** 698,701 ****
--- 707,713 ----
  				degToRad(worldVel[2]));
  			dBodySetAngularVel(mBodyID, velRad[0], velRad[1], velRad[2]);
+ 
+ 			// Invalidate the "freely-spinning" parameter.
+ 			internal_setFreelySpinning(false);
  		}
  	}
***************
*** 724,727 ****
--- 736,742 ----
  		{
  			dBodySetLinearVel(mBodyID, vel[0], vel[1], vel[2]);
+ 
+ 			// Invalidate the "freely-spinning" parameter.
+ 			internal_setFreelySpinning(false);
  		}
  	}
***************
*** 761,764 ****
--- 776,782 ----
  			Vec3r velRad(degToRad(vel[0]), degToRad(vel[1]), degToRad(vel[2]));
  			dBodySetAngularVel(mBodyID, velRad[0], velRad[1], velRad[2]);
+ 
+ 			// Invalidate the "freely-spinning" parameter.
+ 			internal_setFreelySpinning(false);
  		}
  	}
***************
*** 972,975 ****
--- 990,996 ----
  				break;
  		}
+ 
+ 		// Invalidate the "freely-spinning" parameter.
+ 		internal_setFreelySpinning(false);
  	}
  
***************
*** 1005,1008 ****
--- 1026,1034 ----
  			dBodySetMass(mBodyID, &totalMass);
  		}
+ 
+ 		// Update this since the mass changed.
+ 		dMass m;
+ 		dBodyGetMass(mBodyID, &m);
+ 		mNonSymmetricInertia = isInertiaNonSymmetric(m);
  	}
  
***************
*** 1011,1013 ****
--- 1037,1090 ----
  		return &mGeomDataList;
  	}
+ 
+ 	void ODESolid::internal_doAngularVelFix()
+ 	{
+ 		if (mNonSymmetricInertia)
+ 		{
+ 			Vec3r vel = getGlobalAngularVel();
+ 			real currentAngVelMagSquared = vel.lengthSquared();
+ 			
+ 			if (mIsFreelySpinning)
+ 			{
+ 				// If the current angular velocity magnitude is greater than 
+ 				// that of the previous step, scale it by that of the previous 
+ 				// step; otherwise, update the previous value to that of the 
+ 				// current step.  This ensures that angular velocity never 
+ 				// increases for freely-spinning objects.
+ 
+ 				if (currentAngVelMagSquared > mPrevAngVelMagSquared)
+ 				{
+ 					real currentAngVelMag = sqrt(currentAngVelMagSquared);
+ 					vel = vel / currentAngVelMag;
+ 					// Vel is now a unit vector.  Next, scale this vector 
+ 					// by the previous angular velocity magnitude.
+ 					real prevAngVelMag = sqrt(mPrevAngVelMagSquared);
+ 					setGlobalAngularVel(vel * prevAngVelMag);
+ 				}
+ 			}
+ 
+ 			mPrevAngVelMagSquared = currentAngVelMagSquared;
+ 		}
+ 
+ 		// Reset the "freely-spinning" parameter for the next time step.
+ 		internal_setFreelySpinning(true);
+ 	}
+ 
+ 	void ODESolid::internal_setFreelySpinning(bool fs)
+ 	{
+ 		mIsFreelySpinning = fs;
+ 	}
+ 
+ 	bool ODESolid::isInertiaNonSymmetric(const dMass& mass)const
+ 	{
+ 		if (opal::abs(mass.I[0] - mass.I[5]) > globals::OPAL_EPSILON 
+ 			|| opal::abs(mass.I[5] - mass.I[10]) > globals::OPAL_EPSILON)
+ 		{
+ 			return true;
+ 		}
+ 		else
+ 		{
+ 			return false;
+ 		}
+ 	}
  }