/PhysicsEngines/Bullet/BulletPhysics.cs

using BulletXNA;

using BulletXNA.BulletCollision;

using BulletXNA.BulletDynamics;

using Delta.PhysicsEngines.Enums;

using Delta.Utilities;

using Delta.Utilities.Datatypes;

using BulletVector3 = Microsoft.Xna.Framework.Vector3;

using BulletMatrix = Microsoft.Xna.Framework.Matrix;



namespace Delta.PhysicsEngines.Bullet

{

	/// <summary>

	/// Bullet physics engine implementation into DeltaEngine.

	/// <remarks>

	/// Reference <see cref="http://code.google.com/p/bullet-xna/"/> for more info.

	/// </remarks>

	/// </summary>

	public class BulletPhysics : Physics

	{

		#region Constants

		private const int MaxProxies = 100000;



		private const float PlaneHeight = 1.0f;

		#endregion



		#region Internal



		#region bulletWorld (Internal)

		internal DynamicsWorld bulletWorld;

		#endregion



		#region bulletCollisionConf (Internal)

		internal ICollisionConfiguration bulletCollisionConf;

		#endregion



		#region bulletDispatcher (Internal)

		internal CollisionDispatcher bulletDispatcher;

		#endregion



		#region bulletBroadphase (Internal)

		internal IBroadphaseInterface bulletBroadphase;

		#endregion



		#region bulletSolver (Internal)

		internal IConstraintSolver bulletSolver;

		#endregion



		#endregion



		#region Constructors

		/// <summary>

		/// Initializes a new instance of the <see cref="BulletPhysics"/> class.

		/// </summary>

		public BulletPhysics()

			: base(false, "Bullet")

		{

			// Collision configuration contains default setup for memory.

			bulletCollisionConf = new DefaultCollisionConfiguration();

			bulletDispatcher = new CollisionDispatcher(bulletCollisionConf);



			// Now create broad phase.

			IOverlappingPairCache pairCache = null;

			bulletBroadphase = new SimpleBroadphase(MaxProxies, pairCache);



			// Now create solver

			bulletSolver = new SequentialImpulseConstraintSolver();



			// Add possibility for different world in future.

			bulletWorld = new DiscreteDynamicsWorld(bulletDispatcher,

				bulletBroadphase,

				bulletSolver,

				bulletCollisionConf);



			var bulletVector = BulletDatatypesMapping.Convert(DefaultGravity);

			bulletWorld.SetGravity(ref bulletVector);



			CreateGroundBody();

		}

		#endregion



		#region IsShapeSupported (Public)

		/// <summary>

		/// Implementation for getting which shapes are supported by Bullet.

		/// </summary>

		/// <param name="shapeType">The type of shape to check if supported.</param>

		/// <returns>True if supported, false otherwise.</returns>

		public override bool IsShapeSupported(ShapeType shapeType)

		{

			switch (shapeType)

			{

				case ShapeType.Box:

				case ShapeType.Sphere:

				case ShapeType.Capsule:

				case ShapeType.Triangle:

					//case Enums.ShapeType.Terrain:

					// Bullet supports this too.

				case ShapeType.Cone:

				case ShapeType.Cylinder:

					return true;

			}



			return false;

		}

		#endregion



		#region IsJointSupported (Public)

		/// <summary>

		/// Implementation for getting which shapes are supported by Bullet.

		/// </summary>

		/// <param name="jointType">The type of joint to check if supported.</param>

		/// <returns>True if supported, false otherwise.</returns>

		public override bool IsJointSupported(JointType jointType)

		{

			switch (jointType)

			{

					//case Enums.JointType.FixedAngle:

					//case Enums.JointType.SingleBodyPointOnLine:

					//case Enums.JointType.PointOnLine:

				case JointType.PointOnPoint:

					//case Enums.JointType.PointPointDistance:

				case JointType.Hinge:

					//case Enums.JointType.Prismatic:

					return true;

			}



			return false;

		}

		#endregion



		#region IsFeatureSupported (Public)

		/// <summary>

		/// Implementation for getting which features are supported by Bullet.

		/// </summary>

		/// <param name="support">The type of feature to check if supported.</param>

		/// <returns>True if supported, false otherwise.</returns>

		public override bool IsFeatureSupported(FeatureSupport support)

		{

			switch (support)

			{

				case FeatureSupport.Joint:

					return true;

			}



			return false;

		}

		#endregion



		#region SetGroundPlane (Public)

		/// <summary>

		/// Implementation of SetGroundPlane

		/// </summary>

		/// <param name="enable">True to enable the ground plane, false otherwise.</param>

		/// <param name="height">The height of the plane.</param>

		public override void SetGroundPlane(bool enable, float height)

		{

			RigidBody groundRigidBody = ((BulletBody)groundBody).bulletBody;

			BulletVector3 translate = new BulletVector3(height - (PlaneHeight * 0.5f));

			groundRigidBody.Translate(ref translate);



			// Check if we need to add it

			if (enable)

			{

				if (groundRigidBody.IsInWorld() == false)

				{

					bulletWorld.AddRigidBody(groundRigidBody);

				}

			}

			else

			{

				if (groundRigidBody.IsInWorld())

				{

					bulletWorld.RemoveRigidBody(groundRigidBody);

				}

			}

		}

		#endregion



		#region Methods (Private)



		#region CreateBody

		/// <summary>

		/// Creates a BulletBody from base implementation.

		/// </summary>

		/// <param name="is2DBody">True if the body is 2D, false means that is 3D.</param>

		/// <param name="shape">The shape to attach to the new create body.</param>

		/// <param name="initialPosition">Initial position of the body.</param>

		/// <returns>New PhysicsBody instance or null if not supported.</returns>

		protected override PhysicsBody CreateBody(bool is2DBody,

			PhysicsShape shape, Vector initialPosition)

		{

			if (is2DBody)

			{

				Log.Warning("Bullet does not support 2D bodies.");

				return null;

			}



			PhysicsBody body = new BulletBody(this, shape, initialPosition);

			bodies.Add(body);

			return body;

		}

		#endregion



		#region CreateJoint

		/// <summary>

		/// Creates a BulletJoint from base implementation.

		/// </summary>

		/// <param name="jointType">The type of joint to create.</param>

		/// <param name="bodyA">The first required body.</param>

		/// <param name="bodyB">The second [optional] body.</param>

		/// <param name="args">Array of args to pass to Joint implementation.</param>

		/// <returns></returns>

		public override PhysicsJoint CreateJoint(

			JointType jointType,

			PhysicsBody bodyA, PhysicsBody bodyB, object[] args)

		{

			if (IsJointSupported(jointType) == false)

			{

				Log.Warning("Current module does not support " +

				            "the type of joint " + jointType);

				return null;

			}



			PhysicsJoint joint =

				new BulletJoint(this, jointType, bodyA, bodyB, args);



			if (joint != null)

			{

				joints.Add(joint);

			}



			return joint;

		}

		#endregion



		#region CreateGroundBody

		/// <summary>

		/// Create the body that will be used as ground.

		/// </summary>

		private void CreateGroundBody()

		{

			BulletVector3 vectorSize =

				new BulletVector3(20000.0f, 20000.0f, PlaneHeight / 2.0f);



			CollisionShape groundShape = new BoxShape(ref vectorSize);

			DefaultMotionState groundMotionState =

				new DefaultMotionState(BulletMatrix.Identity, BulletMatrix.Identity);



			RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(

				0.0f, groundMotionState, groundShape, BulletVector3.Zero);

			rbInfo.m_restitution = 1.0f;

			rbInfo.m_mass = 0.0f;

			rbInfo.m_friction = 0.5f;

			rbInfo.m_angularDamping = 0.3f;

			rbInfo.m_linearDamping = 0.3f;



			RigidBody groundRigidBody = new RigidBody(rbInfo);



			groundBody = new BulletBody(this, groundRigidBody, groundShape);

			groundBody.Name = "PlaneBody";

		}

		#endregion



		#region RemoveBodyImpl

		/// <summary>

		/// Implementation of RemoveBody.

		/// </summary>

		/// <param name="body">The body to remove.</param>

		protected override void RemoveBodyImpl(PhysicsBody body)

		{

			// Remove from bullet world

			bulletWorld.RemoveRigidBody((body as BulletBody).bulletBody);

		}

		#endregion



		#region RemoveJointImpl

		/// <summary>

		/// Implementation of RemoveJoint.

		/// </summary>

		/// <param name="body">The joint to remove.</param>

		protected override void RemoveJointImpl(PhysicsJoint joint)

		{

			BulletJoint bulletJoint = joint as BulletJoint;



			bulletWorld.RemoveConstraint(bulletJoint.Constraint);

		}

		#endregion



		#region RayCastImpl

		/// <summary>

		/// Performs ray cast in Bullet world.

		/// </summary>

		/// <param name="ray"></param>

		/// <param name="checkGround"></param>

		/// <param name="foundBody"></param>

		/// <param name="surfaceNormal"></param>

		/// <param name="fraction"></param>

		/// <param name="userData"></param>

		/// <returns></returns>

		protected override bool RayCastImpl(Ray ray, bool checkGround,

			out PhysicsBody foundBody, out Vector surfaceNormal, out float fraction,

			out object userData)

		{

			foundBody = null;

			surfaceNormal = Vector.Zero;

			fraction = 0.0f;

			userData = null;



			var rayPos = BulletDatatypesMapping.Convert(ray.Position);

			var rayDir = BulletDatatypesMapping.Convert(ray.Direction);

			ClosestRayResultCallback resultCallback

				= new ClosestRayResultCallback(ref rayPos, ref rayDir);

			bulletWorld.RayTest(ref rayPos, ref rayDir, resultCallback);

			if (resultCallback.HasHit() == false)

			{

				return false;

			}



			RigidBody rigBody = RigidBody.Upcast(resultCallback.m_collisionObject);

			BulletBody body = rigBody.GetUserPointer() as BulletBody;



			if (body != null)

			{

				foundBody = body;

				surfaceNormal = BulletDatatypesMapping.Convert(

					resultCallback.m_hitNormalWorld);

				fraction = resultCallback.m_closestHitFraction;

				return true;

			}



			return false;

		}

		#endregion



		#region UpdateSimulation

		/// <summary>

		/// Update simulation implementation of Physics.

		/// </summary>

		protected override void UpdateSimulation(float timeStep)

		{

			// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_the_World

			if (IsPaused == false)

			{

				bulletWorld.StepSimulation(timeStep, 10);

			}

		}

		#endregion



		#region SetGravity

		/// <summary>

		/// Sets gravity implementation.

		/// </summary>

		/// <param name="gravity">The 3D gravity vector.</param>

		protected override void SetGravity(Vector gravity)

		{

			var bulletVector = BulletDatatypesMapping.Convert(gravity);

			bulletWorld.SetGravity(ref bulletVector);

		}

		#endregion



		#region SetMultithreading

		/// <summary>

		/// Sets multithreading on Bullet implementation.

		/// </summary>

		/// <param name="enable">True to enable, false otherwise.</param>

		protected override void SetMultithreading(bool enable)

		{

			// Not supported

			if (enable)

			{

				Log.Warning("BulleXNA does not support multi-threading yet!");

			}

		}

		#endregion



		#region GetTotalPhysicsTime

		/// <summary>

		/// Gets total time step of physics simulation.

		/// </summary>

		/// <returns></returns>

		protected override double GetTotalPhysicsTime()

		{

			return bulletWorld.GetDispatchInfo().GetTimeStep();

		}

		#endregion



		#endregion

	}

}