//! apply the correction impulses for two bodies
		public float SolveAngularLimits(float timeStep, ref IndexedVector3 axis, float jacDiagABInv, RigidBody body0, RigidBody body1)
		{
			if (NeedApplyTorques() == false)
			{
				return 0.0f;
			}

			float target_velocity = m_targetVelocity;
			float maxMotorForce = m_maxMotorForce;

			//current error correction
			if (m_currentLimit != 0)
			{
				target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
				maxMotorForce = m_maxLimitForce;
			}

			maxMotorForce *= timeStep;

			// current velocity difference

			IndexedVector3 angVelA = IndexedVector3.Zero;
			body0.InternalGetAngularVelocity(ref angVelA);
			IndexedVector3 angVelB = IndexedVector3.Zero;
			body1.InternalGetAngularVelocity(ref angVelB);

			IndexedVector3 vel_diff = angVelA - angVelB;

			float rel_vel = IndexedVector3.Dot(axis, vel_diff);

			// correction velocity
			float motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);

			if (motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON)
			{
				return 0.0f;//no need for applying force
			}


			// correction impulse
			float unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;

			// clip correction impulse
			float clippedMotorImpulse;

			///@todo: should clip against accumulated impulse
			if (unclippedMotorImpulse > 0.0f)
			{
				clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
			}
			else
			{
				clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
			}


			// sort with accumulated impulses
			float lo = float.MinValue;
			float hi = float.MaxValue;

			float oldaccumImpulse = m_accumulatedImpulse;
			float sum = oldaccumImpulse + clippedMotorImpulse;
			m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;

			clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;

			IndexedVector3 motorImp = clippedMotorImpulse * axis;

			//body0.applyTorqueImpulse(motorImp);
			//body1.applyTorqueImpulse(-motorImp);

            body0.InternalApplyImpulse(IndexedVector3.Zero, body0.GetInvInertiaTensorWorld() * axis, clippedMotorImpulse, "Generic6DoF body0");
            body1.InternalApplyImpulse(IndexedVector3.Zero, body1.GetInvInertiaTensorWorld() * axis, -clippedMotorImpulse, "Generic6DoF body1");

			return clippedMotorImpulse;
		}
		public float SolveLinearAxis(
			float timeStep,
			float jacDiagABInv,
			RigidBody body1, ref IndexedVector3 pointInA,
			RigidBody body2, ref IndexedVector3 pointInB,
			int limit_index,
			ref IndexedVector3 axis_normal_on_a,
			ref IndexedVector3 anchorPos)
		{
			///find relative velocity
			//    IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
			//    IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
			IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
			IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();

			IndexedVector3 vel1 = IndexedVector3.Zero;
			body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1);
			IndexedVector3 vel2 = IndexedVector3.Zero; ;
			body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2);
			IndexedVector3 vel = vel1 - vel2;

			float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel);

			/// apply displacement correction

			//positional error (zeroth order error)
			float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a);
			float lo = float.MinValue;
			float hi = float.MaxValue;

			float minLimit = m_lowerLimit[limit_index];
			float maxLimit = m_upperLimit[limit_index];

			//handle the limits
			if (minLimit < maxLimit)
			{
				{
					if (depth > maxLimit)
					{
						depth -= maxLimit;
						lo = 0f;

					}
					else
					{
						if (depth < minLimit)
						{
							depth -= minLimit;
							hi = 0f;
						}
						else
						{
							return 0.0f;
						}
					}
				}
			}

			float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;

			float oldNormalImpulse = m_accumulatedImpulse[limit_index];
			float sum = oldNormalImpulse + normalImpulse;
			m_accumulatedImpulse[limit_index] =  (sum > hi ? 0f : sum < lo ? 0f : sum);
			normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;

			IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse;
			//body1.applyImpulse( impulse_vector, rel_pos1);
			//body2.applyImpulse(-impulse_vector, rel_pos2);

			IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a);
			IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a);
            body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1");
            body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2");

			return normalImpulse;

		}