godot/servers/physics/joints/generic_6dof_joint_sw.cpp

#include "generic_6dof_joint_sw.h"



#define GENERIC_D6_DISABLE_WARMSTARTING 1

real_t btGetMatrixElem(const Matrix3& mat, int index);
real_t btGetMatrixElem(const Matrix3& mat, int index)
{
	int i = index%3;
	int j = index/3;
	return mat[i][j];
}

///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool	matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz);
bool	matrixToEulerXYZ(const Matrix3& mat,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
//

		if (btGetMatrixElem(mat,2) < real_t(1.0))
		{
			if (btGetMatrixElem(mat,2) > real_t(-1.0))
			{
				xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
				xyz[1] = Math::asin(btGetMatrixElem(mat,2));
				xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
				return true;
			}
			else
			{
				// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
				xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
				xyz[1] = -Math_PI*0.5;
				xyz[2] = real_t(0.0);
				return false;
			}
		}
		else
		{
			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
			xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
			xyz[1] = Math_PI*0.5;
			xyz[2] = 0.0;

		}


	return false;
}



//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////


int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value)
{
	if(m_loLimit>m_hiLimit)
	{
		m_currentLimit = 0;//Free from violation
		return 0;
	}

	if (test_value < m_loLimit)
	{
		m_currentLimit = 1;//low limit violation
		m_currentLimitError =  test_value - m_loLimit;
		return 1;
	}
	else if (test_value> m_hiLimit)
	{
		m_currentLimit = 2;//High limit violation
		m_currentLimitError = test_value - m_hiLimit;
		return 2;
	};

	m_currentLimit = 0;//Free from violation
	return 0;

}


real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
		real_t timeStep,Vector3& axis,real_t jacDiagABInv,
		BodySW * body0, BodySW * body1)
{
    if (needApplyTorques()==false) return 0.0f;

    real_t target_velocity = m_targetVelocity;
    real_t maxMotorForce = m_maxMotorForce;

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

    maxMotorForce *= timeStep;

    // current velocity difference
    Vector3 vel_diff = body0->get_angular_velocity();
    if (body1)
    {
	vel_diff -= body1->get_angular_velocity();
    }



    real_t rel_vel = axis.dot(vel_diff);

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


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


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

	// clip correction impulse
    real_t 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
    real_t	lo = real_t(-1e30);
    real_t	hi = real_t(1e30);

    real_t oldaccumImpulse = m_accumulatedImpulse;
    real_t sum = oldaccumImpulse + clippedMotorImpulse;
    m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;

    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;



    Vector3 motorImp = clippedMotorImpulse * axis;


    body0->apply_torque_impulse(motorImp);
    if (body1) body1->apply_torque_impulse(-motorImp);

    return clippedMotorImpulse;


}

//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////

//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
		real_t timeStep,
	real_t jacDiagABInv,
	BodySW* body1,const Vector3 &pointInA,
	BodySW* body2,const Vector3 &pointInB,
	int limit_index,
	const Vector3 & axis_normal_on_a,
		const Vector3 & anchorPos)
{

///find relative velocity
//    Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
//    Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
    Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
    Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;

    Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
    Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
    Vector3 vel = vel1 - vel2;

    real_t rel_vel = axis_normal_on_a.dot(vel);



/// apply displacement correction

//positional error (zeroth order error)
    real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
    real_t	lo = real_t(-1e30);
    real_t	hi = real_t(1e30);

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

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

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

    real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;




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

    Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
    body1->apply_impulse( rel_pos1, impulse_vector);
    body2->apply_impulse( rel_pos2, -impulse_vector);
    return normalImpulse;
}

//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////


Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
	 :  JointSW(_arr,2)
	, m_frameInA(frameInA)
	, m_frameInB(frameInB),
		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
{
	A=rbA;
	B=rbB;
	A->add_constraint(this,0);
	B->add_constraint(this,1);
}





void Generic6DOFJointSW::calculateAngleInfo()
{
	Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;

	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);



	// in euler angle mode we do not actually constrain the angular velocity
  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
  //
  //    to get			constrain w2-w1 along		...not
  //    ------			---------------------		------
  //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
  //    d(angle[1])/dt = 0	ax[1]
  //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
  //
  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
  // to prove the result for angle[0], write the expression for angle[0] from
  // GetInfo1 then take the derivative. to prove this for angle[2] it is
  // easier to take the euler rate expression for d(angle[2])/dt with respect
  // to the components of w and set that to 0.

	Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
	Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);

	m_calculatedAxis[1] = axis2.cross(axis0);
	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);


//    if(m_debugDrawer)
//    {
//
//    	char buff[300];
//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
//		m_calculatedAxisAngleDiff[0],
//		m_calculatedAxisAngleDiff[1],
//		m_calculatedAxisAngleDiff[2]);
//    	m_debugDrawer->reportErrorWarning(buff);
//    }

}

void Generic6DOFJointSW::calculateTransforms()
{
    m_calculatedTransformA = A->get_transform() * m_frameInA;
    m_calculatedTransformB = B->get_transform() * m_frameInB;

    calculateAngleInfo();
}


void Generic6DOFJointSW::buildLinearJacobian(
    JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
    const Vector3 & pivotAInW,const Vector3 & pivotBInW)
{
   memnew_placement(&jacLinear, JacobianEntrySW(
	A->get_transform().basis.transposed(),
	B->get_transform().basis.transposed(),
	pivotAInW - A->get_transform().origin,
	pivotBInW - B->get_transform().origin,
	normalWorld,
	A->get_inv_inertia(),
	A->get_inv_mass(),
	B->get_inv_inertia(),
	B->get_inv_mass()));

}

void Generic6DOFJointSW::buildAngularJacobian(
    JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
{
    memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
				      A->get_transform().basis.transposed(),
				      B->get_transform().basis.transposed(),
				      A->get_inv_inertia(),
				      B->get_inv_inertia()));

}

bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
{
    real_t angle = m_calculatedAxisAngleDiff[axis_index];

    //test limits
    m_angularLimits[axis_index].testLimitValue(angle);
    return m_angularLimits[axis_index].needApplyTorques();
}

bool Generic6DOFJointSW::setup(float p_step) {

	// Clear accumulated impulses for the next simulation step
    m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
    int i;
    for(i = 0; i < 3; i++)
    {
	m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
    }
    //calculates transform
    calculateTransforms();

//  const Vector3& pivotAInW = m_calculatedTransformA.origin;
//  const Vector3& pivotBInW = m_calculatedTransformB.origin;
	calcAnchorPos();
	Vector3 pivotAInW = m_AnchorPos;
	Vector3 pivotBInW = m_AnchorPos;

// not used here
//    Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
//    Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;

    Vector3 normalWorld;
    //linear part
    for (i=0;i<3;i++)
    {
	if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
	{
			if (m_useLinearReferenceFrameA)
		    normalWorld = m_calculatedTransformA.basis.get_axis(i);
			else
		    normalWorld = m_calculatedTransformB.basis.get_axis(i);

	    buildLinearJacobian(
		m_jacLinear[i],normalWorld ,
		pivotAInW,pivotBInW);

	}
    }

    // angular part
    for (i=0;i<3;i++)
    {
	//calculates error angle
	if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
	{
	    normalWorld = this->getAxis(i);
	    // Create angular atom
	    buildAngularJacobian(m_jacAng[i],normalWorld);
	}
    }

	return true;
}


void Generic6DOFJointSW::solve(real_t	timeStep)
{
    m_timeStep = timeStep;

    //calculateTransforms();

    int i;

    // linear

    Vector3 pointInA = m_calculatedTransformA.origin;
	Vector3 pointInB = m_calculatedTransformB.origin;

	real_t jacDiagABInv;
	Vector3 linear_axis;
    for (i=0;i<3;i++)
    {
	if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
	{
	    jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();

			if (m_useLinearReferenceFrameA)
		    linear_axis = m_calculatedTransformA.basis.get_axis(i);
			else
		    linear_axis = m_calculatedTransformB.basis.get_axis(i);

	    m_linearLimits.solveLinearAxis(
		m_timeStep,
		jacDiagABInv,
		A,pointInA,
		B,pointInB,
		i,linear_axis, m_AnchorPos);

	}
    }

    // angular
    Vector3 angular_axis;
    real_t angularJacDiagABInv;
    for (i=0;i<3;i++)
    {
	if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
	{

			// get axis
			angular_axis = getAxis(i);

			angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();

			m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B);
	}
    }
}

void	Generic6DOFJointSW::updateRHS(real_t	timeStep)
{
    (void)timeStep;

}

Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
{
    return m_calculatedAxis[axis_index];
}

real_t Generic6DOFJointSW::getAngle(int axis_index) const
{
    return m_calculatedAxisAngleDiff[axis_index];
}

void Generic6DOFJointSW::calcAnchorPos(void)
{
	real_t imA = A->get_inv_mass();
	real_t imB = B->get_inv_mass();
	real_t weight;
	if(imB == real_t(0.0))
	{
		weight = real_t(1.0);
	}
	else
	{
		weight = imA / (imA + imB);
	}
	const Vector3& pA = m_calculatedTransformA.origin;
	const Vector3& pB = m_calculatedTransformB.origin;
	m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
	return;
} // Generic6DOFJointSW::calcAnchorPos()


void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {

	ERR_FAIL_INDEX(p_axis,3);
	switch(p_param) {
		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {

			m_linearLimits.m_lowerLimit[p_axis]=p_value;
		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {

			m_linearLimits.m_upperLimit[p_axis]=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {

			m_linearLimits.m_limitSoftness[p_axis]=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {

			m_linearLimits.m_restitution[p_axis]=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {

			m_linearLimits.m_damping[p_axis]=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {

			m_angularLimits[p_axis].m_loLimit=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {

			m_angularLimits[p_axis].m_hiLimit=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {

			m_angularLimits[p_axis].m_limitSoftness=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {

			m_angularLimits[p_axis].m_damping=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {

			m_angularLimits[p_axis].m_bounce=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {

			m_angularLimits[p_axis].m_maxLimitForce=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {

			m_angularLimits[p_axis].m_ERP=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {

			m_angularLimits[p_axis].m_targetVelocity=p_value;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {

			m_angularLimits[p_axis].m_maxLimitForce=p_value;

		} break;
	}
}

float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
	ERR_FAIL_INDEX_V(p_axis,3,0);
	switch(p_param) {
		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {

			return m_linearLimits.m_lowerLimit[p_axis];
		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {

			return m_linearLimits.m_upperLimit[p_axis];

		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {

			return m_linearLimits.m_limitSoftness[p_axis];

		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {

			return m_linearLimits.m_restitution[p_axis];

		} break;
		case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {

			return m_linearLimits.m_damping[p_axis];

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {

			return m_angularLimits[p_axis].m_loLimit;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {

			return m_angularLimits[p_axis].m_hiLimit;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {

			return m_angularLimits[p_axis].m_limitSoftness;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {

			return m_angularLimits[p_axis].m_damping;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {

			return m_angularLimits[p_axis].m_bounce;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {

			return m_angularLimits[p_axis].m_maxLimitForce;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {

			return m_angularLimits[p_axis].m_ERP;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {

			return m_angularLimits[p_axis].m_targetVelocity;

		} break;
		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {

			return m_angularLimits[p_axis].m_maxLimitForce;

		} break;
	}
	return 0;
}

void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){

	ERR_FAIL_INDEX(p_axis,3);

	switch(p_flag) {
		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {

			m_linearLimits.enable_limit[p_axis]=p_value;
		} break;
		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {

			m_angularLimits[p_axis].m_enableLimit=p_value;
		} break;
		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {

			m_angularLimits[p_axis].m_enableMotor=p_value;
		} break;
	}


}
bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{

	ERR_FAIL_INDEX_V(p_axis,3,0);
	switch(p_flag) {
		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {

			return m_linearLimits.enable_limit[p_axis];
		} break;
		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {

			return m_angularLimits[p_axis].m_enableLimit;
		} break;
		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {

			return m_angularLimits[p_axis].m_enableMotor;
		} break;
	}

	return 0;
}