newton中旋转大于180度的解决办法

以前用都没有对joint旋转大于180度的操作，因做挖掘机程序时，旋转180度时，挖掘机飞掉了。

准备去newton论坛上去问的，但那段时间不知怎么的，newton不能注册，所有一直都没有机会去问。某一天闲着无事，就又上论坛去注册，竟然可以注册了。注册完后可以访问FAQ模块了，进去看了看，原来作者已经将这个问题提出来，并给出了解决方案。

 

原文如下：

 

Yes definitely but the functionality is not supported directly since it is a more complex operation than just adding and substrating angles in a Cartesian space.
To add angular limit large than +=180 degree that application only need to consider that the error angle need to be calculate in the circular space using the functions sin (a - b) and cos (a - b)
Here is an example of a custom hinge modified to handle any arbitrary angular limit

 

class CustomHinge: public NewtonCustomJoint 
{
   public:
   CustomHinge(const dVector& pivot, const dVector& pin, NewtonBody* child, NewtonBody* parent = NULL);
   virtual ~CustomHinge();

   void EnableLimits(bool state);
   void SetLimis(dFloat minAngle, dFloat maxAngle);

   protected:
   virtual void SubmitConstrainst ();

   float CalculateJointAngle (const dMatrix& matrix0, const dMatrix& matrix1) const;

   dMatrix m_localMatrix0;
   dMatrix m_localMatrix1;

   bool m_limitsOn;
   
   dFloat m_minAngle;
   dFloat m_maxAngle;
   dFloat m_curJointAngle;
};


#define MIN_JOINT_PIN_LENGTH   50.0f

CustomHinge::CustomHinge(const dVector& pivot, const dVector& pin, NewtonBody* child, NewtonBody* parent)
   :NewtonCustomJoint(6, child, parent)
{
   m_limitsOn = false;
   m_minAngle = -45.0f * 3.1416f / 180.0f;
   m_maxAngle =  45.0f * 3.1416f / 180.0f;

      // the joint current angle is zero at joint creation time
   m_curJointAngle = 0.0f;


   // calculate the two local matrix of the pivot point
   CalculateLocalMatrix (pivot, pin, m_localMatrix0, m_localMatrix1);
}

CustomHinge::~CustomHinge()
{
}


void CustomHinge::EnableLimits(bool state)
{
   m_limitsOn = state;
}

void CustomHinge::SetLimis(dFloat minAngle, dFloat maxAngle)
{
   //_ASSERTE (minAngle < 0.0f);
   //_ASSERTE (maxAngle > 0.0f);
   m_minAngle = minAngle;
   m_maxAngle = maxAngle;
}

// the funtion calculate the error angular considering angula wraping aroung += 180 degree
float CustomHinge::CalculateJointAngle (const dMatrix& matrix0, const dMatrix& matrix1) const
{
   dFloat sinAngle;
   dFloat cosAngle;
   dFloat sinJointAngle;
   dFloat cosJointAngle;

   dFloat sin_da;
   dFloat cos_da;


   // the joint angle can be determine by getting the angle between any two non parallel vectors
   sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
   cosAngle = matrix0.m_up % matrix1.m_up;

   sinJointAngle = sinf (m_curJointAngle);
   cosJointAngle = cosf (m_curJointAngle);

   sin_da = sinAngle * cosJointAngle - cosAngle * sinJointAngle;
   cos_da = cosAngle * cosJointAngle + sinAngle * sinJointAngle;

   return  dAtan2 (sin_da, cos_da) + m_curJointAngle;
}


void CustomHinge::SubmitConstrainst ()
{
//   dFloat angle;
   dMatrix matrix0;
   dMatrix matrix1;

   // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
   CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);

   // Restrict the movement on the pivot point along all tree orthonormal direction
   NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_front[0]);
   NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
   NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
   
   // get a point along the pin axis at some reasonable large distance from the pivot
   dVector q0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
   dVector q1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));

   // two constraints row perpendicular to the pin vector
   NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
   NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);


   // if limit are enable ...
   if (m_limitsOn) {
      // the joint angle can be determine by getting the angle between any two non parallel vectors
//      sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
//      cosAngle = matrix0.m_up % matrix1.m_up;
//      angle = dAtan2 (sinAngle, cosAngle);

      m_curJointAngle = CalculateJointAngle (matrix0, matrix1);
      if (m_curJointAngle < m_minAngle) {
         dFloat relAngle;

         relAngle = m_curJointAngle - m_minAngle;
         // the angle was clipped save the new clipp limit
         m_curJointAngle = m_minAngle;

         // tell joint error will minimize the exceeded angle error
         NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

         // need high stiffness here
         NewtonUserJointSetRowStiffness (m_joint, 1.0f);

         // allow the joint to move back freely
         NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);


      } else if (m_curJointAngle > m_maxAngle) {
         dFloat relAngle;

         relAngle = m_curJointAngle - m_maxAngle;

         // the angle was clipped save the new clipp limit
         m_curJointAngle = m_maxAngle;
         
         // tell joint error will minimize the exceeded angle error
         NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

         // need high stiffness here
         NewtonUserJointSetRowStiffness (m_joint, 1.0f);

         // allow the joint to move back freely
         NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
      }
   }
}