path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs

/*
  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

  This software is provided 'as-is', without any express or implied
  warranty.  In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:

  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Collections.Generic;
using System.Text;
using MonoXnaCompactMaths;

namespace XnaDevRu.BulletX.Dynamics
{
	/// <summary>
	/// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
	/// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
	/// Work in progress (is still a Hinge actually)
	/// </summary>
	public class Generic6DofConstraint : TypedConstraint
	{
		private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f };
		private static readonly int[] _axisA = { 1, 0, 0 };
		private static readonly int[] _axisB = { 2, 2, 1 };

		private JacobianEntry[] _jacLinear = new JacobianEntry[3];			// 3 orthogonal linear constraints
		private JacobianEntry[] _jacAng = new JacobianEntry[3];		// 3 orthogonal angular constraints

		private Matrix _frameInA;			// the constraint space w.r.t body A
		private Matrix _frameInB;			// the constraint space w.r.t body B

		private float[] _lowerLimit = new float[6];	// the constraint lower limits
		private float[] _upperLimit = new float[6];	// the constraint upper limits

		private float[] _accumulatedImpulse = new float[6];

		public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)
			: base(rbA, rbB)
		{
			_frameInA = frameInA;
			_frameInB = frameInB;
			//free means upper < lower, 
			//locked means upper == lower
			//limited means upper > lower
			//so start all locked
			for (int i = 0; i < 6; ++i)
			{
				_lowerLimit[i] = 0.0f;
				_upperLimit[i] = 0.0f;
				_accumulatedImpulse[i] = 0.0f;
			}
		}

		public Generic6DofConstraint() { }

		public void UpdateRHS(float timeStep) { }

		public float ComputeAngle(int axis)
		{
			float angle = 0;

			switch (axis)
			{
				case 0:
					{
						Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);
						Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
						Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);

						float s = Vector3.Dot(v1, w2);
						float c = Vector3.Dot(v1, v2);

						angle = (float)Math.Atan2(s, c);
						break;
					}
				case 1:
					{
						Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);
						Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
						Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);

						float s = Vector3.Dot(w1, u2);
						float c = Vector3.Dot(w1, w2);

						angle = (float)Math.Atan2(s, c);
						break;
					}
				case 2:
					{
						Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);
						Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
						Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);

						float s = Vector3.Dot(u1, v2);
						float c = Vector3.Dot(u1, u2);

						angle = (float)Math.Atan2(s, c);
						break;
					}
				default: BulletDebug.Assert(false); break;
			}

			return angle;
		}

		public void SetLinearLowerLimit(Vector3 linearLower)
		{
			_lowerLimit[0] = linearLower.X;
			_lowerLimit[1] = linearLower.Y;
			_lowerLimit[2] = linearLower.Z;
		}

		public void SetLinearUpperLimit(Vector3 linearUpper)
		{
			_upperLimit[0] = linearUpper.X;
			_upperLimit[1] = linearUpper.Y;
			_upperLimit[2] = linearUpper.Z;
		}

		public void SetAngularLowerLimit(Vector3 angularLower)
		{
			_lowerLimit[3] = angularLower.X;
			_lowerLimit[4] = angularLower.Y;
			_lowerLimit[5] = angularLower.Z;
		}

		public void SetAngularUpperLimit(Vector3 angularUpper)
		{
			_upperLimit[3] = angularUpper.X;
			_upperLimit[4] = angularUpper.Y;
			_upperLimit[5] = angularUpper.Z;
		}

		//first 3 are linear, next 3 are angular
		public void SetLimit(int axis, float lo, float hi)
		{
			_lowerLimit[axis] = lo;
			_upperLimit[axis] = hi;
		}

		//free means upper < lower, 
		//locked means upper == lower
		//limited means upper > lower
		//limitIndex: first 3 are linear, next 3 are angular
		public bool IsLimited(int limitIndex)
		{
			return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]);
		}

		public override void BuildJacobian()
		{
			Vector3 localNormalInA = new Vector3(0, 0, 0);

			Vector3 pivotInA = _frameInA.Translation;
			Vector3 pivotInB = _frameInB.Translation;

			Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
			Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);

			Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
			Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

			//linear part
			for (int i = 0; i < 3; i++)
			{
				if (IsLimited(i))
				{
					if (i == 0)
						localNormalInA = new Vector3(1, 0, 0);
					else if (i == 1)
						localNormalInA = new Vector3(0, 1, 0);
					else
						localNormalInA = new Vector3(0, 0, 1);

					Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);

					// Create linear atom
					_jacLinear[i] = new JacobianEntry(
						MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
						MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
						MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
						MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
						normalWorld,
						RigidBodyA.InvInertiaDiagLocal,
						RigidBodyA.InverseMass,
						RigidBodyB.InvInertiaDiagLocal,
						RigidBodyB.InverseMass);

					//optionally disable warmstarting
					_accumulatedImpulse[i] = 0f;

					// Apply accumulated impulse
					Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;

					RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
					RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
				}
			}

			// angular part
			for (int i = 0; i < 3; i++)
			{
				if (IsLimited(i + 3))
				{
					Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
					Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);

					Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);

					// Create angular atom
					_jacAng[i] = new JacobianEntry(axis,
						MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
						MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
						RigidBodyA.InvInertiaDiagLocal,
						RigidBodyB.InvInertiaDiagLocal);

					_accumulatedImpulse[i + 3] = 0f;

					// Apply accumulated impulse
					Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;

					RigidBodyA.ApplyTorqueImpulse(impulse_vector);
					RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
				}
			}
		}

		public override void SolveConstraint(float timeStep)
		{
			float tau = 0.1f;
			float damping = 1.0f;

			Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
			Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);

			Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
			Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

			Vector3 localNormalInA = new Vector3();

			// linear
			for (int i = 0; i < 3; i++)
			{
				if (IsLimited(i))
				{
					Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
					Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));

					if (i == 0)
						localNormalInA = new Vector3(1, 0, 0);
					else if (i == 1)
						localNormalInA = new Vector3(0, 1, 0);
					else
						localNormalInA = new Vector3(0, 0, 1);

					Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);

					float jacDiagABInv = 1f / _jacLinear[i].Diagonal;

					//velocity error (first order error)
					float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
																			RigidBodyB.LinearVelocity, angvelB);

					//positional error (zeroth order error)
					float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);
					float lo = -1e30f;
					float hi = 1e30f;

					//handle the limits
					if (_lowerLimit[i] < _upperLimit[i])
					{
						if (depth > _upperLimit[i])
						{
							depth -= _upperLimit[i];
							lo = 0f;
						}
						else
						{
							if (depth < _lowerLimit[i])
							{
								depth -= _lowerLimit[i];
								hi = 0f;
							}
							else
							{
								continue;
							}
						}
					}

					float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;
					float oldNormalImpulse = _accumulatedImpulse[i];
					float sum = oldNormalImpulse + normalImpulse;
					_accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;
					normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse;

					Vector3 impulse_vector = normalWorld * normalImpulse;
					RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
					RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
				}
			}

			Vector3 axis;
			float angle;
			Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;
			Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;

			TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);
			Quaternion diff = new Quaternion(axis, angle);
			Matrix diffMat = Matrix.CreateFromQuaternion(diff);
			Vector3 xyz;
			// this is not perfect, we can first check which axis are limited, and choose a more appropriate order
			MatrixToEulerXYZ(diffMat, out xyz);

			// angular
			for (int i = 0; i < 3; i++)
			{
				if (IsLimited(i + 3))
				{
					Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
					Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));

					float jacDiagABInv = 1f / _jacAng[i].Diagonal;

					//velocity error (first order error)
					float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
																					RigidBodyB.LinearVelocity, angvelB);

					//positional error (zeroth order error)
					Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
					Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);

					float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);

					float lo = -1e30f;
					float hi = 1e30f;

					//handle the twist limit
					if (_lowerLimit[i + 3] < _upperLimit[i + 3])
					{
						//clamp the values
						float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;
						float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;

						float projAngle;

						if (i == 0)
							projAngle = -2f * xyz.Z;
						else if (i == 1)
							projAngle = -2f * xyz.Y;
						else
							projAngle = -2f * xyz.Z;

						if (projAngle < loLimit)
						{
							hi = 0f;
							rel_pos = loLimit - projAngle;
						}
						else
						{
							if (projAngle > hiLimit)
							{
								lo = 0f;
								rel_pos = (hiLimit - projAngle);
							}
							else
							{
								continue;
							}
						}
					}

					//impulse

					float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;
					float oldNormalImpulse = _accumulatedImpulse[i + 3];
					float sum = oldNormalImpulse + normalImpulse;
					_accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;
					normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;

					Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB);
					Vector3 impulse_vector = axis2 * normalImpulse;

					RigidBodyA.ApplyTorqueImpulse(impulse_vector);
					RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
				}
			}
		}

		//MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
		private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz)
		{
			// rot =  cy*cz          -cy*sz           sy
			//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
			//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
			xyz = new Vector3();

			if (MathHelper.GetElement(mat, 2) < 1.0f)
			{
				if (MathHelper.GetElement(mat, 2) > -1.0f)
				{
					xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8));
					xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2));
					xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0));
					return true;
				}
				else
				{
					// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
					xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
					xyz.Y = -(float)Math.PI / 2;
					xyz.Z = 0.0f;
					return false;
				}
			}
			else
			{
				// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
				xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
				xyz.Y = (float)Math.PI / 2;
				xyz.Z = 0.0f;
				return false;
			}
		}
	}
}