Пример #1
0
		internal void DoDamping()
		{
			if( bodyID == dBodyID.Zero )
				return;

			Ode.dMass mass = new Ode.dMass();
			Ode.dBodyGetMass( bodyID, ref mass );

			// Linear damping
			if( LinearDamping != 0 )
			{
				// The damping force depends on the damping amount, mass, and velocity 
				// (i.e. damping amount and momentum).
				float factor = -LinearDamping * mass.mass;
				Vec3 force = LinearVelocity * factor;

				// Add a global force opposite to the global linear velocity.
				Ode.dBodyAddForce( bodyID, force.X, force.Y, force.Z );
			}

			// Angular damping
			if( AngularDamping != 0 )
			{
				Vec3 localVelocity;
				{
					Ode.dVector3 aVelLocal = new Ode.dVector3();
					Ode.dBodyVectorFromWorld( bodyID, AngularVelocity.X, AngularVelocity.Y,
						AngularVelocity.Z, ref aVelLocal );
					localVelocity = Convert.ToNet( aVelLocal );
				}

				// The damping force depends on the damping amount, mass, and velocity 
				// (i.e. damping amount and momentum).
				float factor = -AngularDamping;

				Vec3 momentum = new Vec3(
					Vec3.Dot( new Vec3( mass.I.M00, mass.I.M01, mass.I.M02 ), localVelocity ),
					Vec3.Dot( new Vec3( mass.I.M10, mass.I.M11, mass.I.M12 ), localVelocity ),
					Vec3.Dot( new Vec3( mass.I.M20, mass.I.M21, mass.I.M22 ), localVelocity ) );

				Vec3 torque = momentum * factor;

				// Add a local torque opposite to the local angular velocity.
				Ode.dBodyAddRelTorque( bodyID, torque.X, torque.Y, torque.Z );
			}
		}
Пример #2
0
		void CalculateBodyMass()
		{
			float totalVolume = 0;
			if( MassMethod == MassMethods.Manually )
			{
				for( int nShape = 0; nShape < Shapes.Length; nShape++ )
					totalVolume += Shapes[ nShape ].Volume;
				if( totalVolume == 0 )
					totalVolume = .001f;
			}

			Ode.dMass bodyMass = new Ode.dMass();
			bool bodyMassInitialized = false;

			for( int nShape = 0; nShape < Shapes.Length; nShape++ )
			{
				Shape shape = Shapes[ nShape ];

				Ode.dMass shapeMass = new Ode.dMass();

				float shapeDensity;
				if( MassMethod == MassMethods.Manually )
					shapeDensity = Mass / totalVolume;
				else
					shapeDensity = shape.Density;

				if( shapeDensity <= 0 )
					shapeDensity = .0001f;

				switch( shape.ShapeType )
				{
				case Shape.Type.Box:
					{
						BoxShape boxShape = (BoxShape)shape;
						Ode.dMassSetBox( ref shapeMass, shapeDensity, boxShape.Dimensions.X,
							boxShape.Dimensions.Y, boxShape.Dimensions.Z );
					}
					break;

				case Shape.Type.Sphere:
					{
						SphereShape sphereShape = (SphereShape)shape;
						Ode.dMassSetSphere( ref shapeMass, shapeDensity, sphereShape.Radius );
					}
					break;

				case Shape.Type.Capsule:
					{
						CapsuleShape capsuleShape = (CapsuleShape)shape;
						Ode.dMassSetCapsule( ref shapeMass, shapeDensity, 3, capsuleShape.Radius,
							capsuleShape.Length );
					}
					break;

				case Shape.Type.Cylinder:
					{
						CylinderShape cylinderShape = (CylinderShape)shape;
						Ode.dMassSetCylinder( ref shapeMass, shapeDensity, 3, cylinderShape.Radius,
							cylinderShape.Length );
					}
					break;

				case Shape.Type.Mesh:
					{
						GeomData geomData = geomDatas[ nShape ];

						//ignore this shape
						if( geomData == null )
							continue;

						IntPtr geomID;

						if( geomData.transformID == dGeomID.Zero )
							geomID = geomData.geomID;
						else
							geomID = geomData.transformID;

						//ignore this shape
						if( geomID == dGeomID.Zero )
							continue;

						Ode.Aabb aabb = new Ode.Aabb();
						Ode.dGeomGetAABB( geomID, ref aabb );
						Ode.dMassSetBox( ref shapeMass, shapeDensity, aabb.maxx - aabb.minx,
							aabb.maxy - aabb.miny, aabb.maxz - aabb.minz );

						//correct
						shapeMass.mass = shape.Volume * shapeDensity;
						if( shapeMass.mass <= 0 )
							shapeMass.mass = .001f;
					}
					break;

				default:
					Trace.Assert( false );
					break;
				}

				if( shape.Rotation != Quat.Identity )
				{
					Mat3 mat3;
					shape.Rotation.ToMat3( out mat3 );
					Ode.dMatrix3 odeMat3;
					Convert.ToODE( ref mat3, out odeMat3 );
					Ode.dMassRotate( ref shapeMass, ref odeMat3 );
				}

				if( shape.Position != Vec3.Zero )
				{
					Ode.dMassTranslate( ref shapeMass, shape.Position.X,
						shape.Position.Y, shape.Position.Z );
				}

				if( !bodyMassInitialized )
				{
					bodyMass = shapeMass;
					bodyMassInitialized = true;
				}
				else
					Ode.dMassAdd( ref bodyMass, ref shapeMass );
			}

			if( MassMethod == MassMethods.Manually )
			{
				bodyMass.mass = Mass;
				if( bodyMass.mass <= 0 )
					bodyMass.mass = .0001f;
			}

			if( bodyMass.mass != 0 )
			{
				//if( CenterOfMassAuto )
				//   Log.Warning( "ODEBody: CenterOfMassAuto is not supported on ODE physics." );

				//!!!!!!тут вручную введенное положение цента масс

				Ode.dMassTranslate( ref bodyMass, -bodyMass.c.X, -bodyMass.c.Y, -bodyMass.c.Z );
				Ode.dBodySetMass( bodyID, ref bodyMass );
			}

			////calculate mNonSymmetricInertia
			//nonSymmetricInertia =
			//   !AreEqual( bodyMass.I.M00, bodyMass.I.M11 ) ||
			//   !AreEqual( bodyMass.I.M11, bodyMass.I.M22 );
		}