Custom joint with quaternion twist for ragdoll etc.

[JoeJ]

This is an alternate option for the LimitBallSocket

Differences:
* LimitBallSocket has friction behaviour when touching limit cone, this one slides freely at the cone
* LimitBallSocket has flipping problems at poles perpendicular to pin axis, this one has not, cone angle can be larger than 90 degrees
* otherwise pretty compatible, for different twist limit options see cpp

I'll do some more testing tomorrow...
Feel free to add it to the Joint Library.





CustomConeTwist.h

Code: Select all

/* Copyright (c) <2009> <Newton Game Dynamics>
*
* 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
*/


#ifndef _CUSTOMCONETWIST_H_
#define _CUSTOMCONETWIST_H_

#include "CustomBallAndSocket.h"



class JOINTLIBRARY_API CustomConeTwist: public CustomBallAndSocket 
{
   public:
   dAddRtti(NewtonCustomJoint);
   CustomConeTwist(const dMatrix& pinsAndPivoFrame, NewtonBody* const child, NewtonBody* const parent = NULL);
   virtual ~CustomConeTwist();

   void SetConeAngle (dFloat angle);
   void SetTwistAngle (dFloat minAngle, dFloat maxAngle);

   protected:
   virtual void SubmitConstraints (dFloat timestep, int threadIndex);
   virtual void GetInfo (NewtonJointRecord* info) const;

   dFloat m_minTwistAngle;
   dFloat m_maxTwistAngle;

   dFloat m_coneAngle;
};


#endif


CustomConeTwist.cpp

Code: Select all

/* Copyright (c) <2009> <Newton Game Dynamics>
*
* 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
*/


// Cone twist joint with quaternion twist by Joe Joechler, 2012
//
//////////////////////////////////////////////////////////////////////
#include "CustomJointLibraryStdAfx.h"
#include "CustomConeTwist.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

dInitRtti(CustomConeTwist);




CustomConeTwist::CustomConeTwist(const dMatrix& pinAndPivotFrame, NewtonBody* const child, NewtonBody* const parent)
   :CustomBallAndSocket(pinAndPivotFrame, child, parent)
{
   dMatrix matrix0;
   dMatrix matrix1;

   SetTwistAngle (0.0f, 0.0f);
   SetConeAngle (0.0f);

   // calculate the twist handle vector
   CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);
}


CustomConeTwist::~CustomConeTwist()
{
}


void CustomConeTwist::SetConeAngle (dFloat angle)
{
   if (angle > 2.9) angle = 2.9;
   if (angle < 0.05) angle = 0.05;
   m_coneAngle = angle;
}


void CustomConeTwist::SetTwistAngle (dFloat minAngle, dFloat maxAngle)
{
   m_minTwistAngle = minAngle;
   m_maxTwistAngle = maxAngle;
}


void CustomConeTwist::GetInfo (NewtonJointRecord* info) const
{
   CustomBallAndSocket::GetInfo (info);

   strcpy (info->m_descriptionType, "limitconetwist");


}

void CustomConeTwist::SubmitConstraints (dFloat timestep, int threadIndex)
{
   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);

   const dVector& p0 = matrix0.m_posit;
   const dVector& p1 = matrix1.m_posit;

   // Restrict the movement on the pivot point along all tree orthonormal direction
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);


   const dVector& coneDir0 = matrix0.m_front;
   const dVector& coneDir1 = matrix1.m_front;
   float dot = coneDir0 % coneDir1;
   if (dot < -0.999) return; // should never happen

   // do the twist

   if (m_maxTwistAngle >= m_minTwistAngle) // twist restricted?
   {
      dQuaternion quat0(matrix0), quat1(matrix1);
      
      // factor rotation about x axis: qt = q0.Inversed() * q1; halfTwistAngle = atan (qt[0] / qt[3]);
      float *q0 = (float*)&quat0;
      float *q1 = (float*)&quat1;
      float twistAngle = 2.0 * atan (
         ( ( ( (q0[0] * q1[1]) + (-q0[1] * q1[0]) ) + (-q0[2] * q1[3]) ) - (-q0[3] * q1[2]) ) /
         ( ( ( (q0[0] * q1[0]) - (-q0[1] * q1[1]) ) - (-q0[2] * q1[2]) ) - (-q0[3] * q1[3]) ) );

      //dVector twistAxis = coneDir0;
      dVector twistAxis = coneDir0 + coneDir1; // @ Julio: using any interpolant works here - average seemed best after some tests
      twistAxis.Scale (1.0 / sqrt (twistAxis % twistAxis));

      if (m_maxTwistAngle == m_minTwistAngle) // no freedom for any twist
      {
         NewtonUserJointAddAngularRow (m_joint, twistAngle - m_maxTwistAngle, (float*)&twistAxis);
      }
      else if (twistAngle > m_maxTwistAngle)
      {
         NewtonUserJointAddAngularRow (m_joint, twistAngle - m_maxTwistAngle, (float*)&twistAxis);
         NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
      }
      else if (twistAngle < m_minTwistAngle)
      {
         NewtonUserJointAddAngularRow (m_joint, twistAngle - m_minTwistAngle, (float*)&twistAxis);
         NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
      }
   }

   // do the swing

   float angle = acos (dot) - m_coneAngle;
   if (angle > 0)
   {
      dVector swingAxis = (coneDir0 * coneDir1);
      swingAxis.Scale (1.0 / sqrt (swingAxis % swingAxis));
      NewtonUserJointAddAngularRow (m_joint, angle, (float*)&swingAxis);
      NewtonUserJointSetRowMinimumFriction (m_joint, 0.0);
   }
}





EDIT: Changed max cone angle from 1.5 to 2.9 radians

[JoeJ]

I did some tests.


First i connected 2 very long boxes with the joints with natural limits.
By playing around with them there's not much difference, but the LimitBallSocket is more stable at the twist.


Then i made a rope of 10 Capsules, radius 2, height 8.
LimitBallSocket Limits: Cone 80deg, Twist 2deg:
Both bend- and twist-behaviour is unnatural and jumpy.
It's not possible to transfer rotation around the length from the first to the last body on a bended rope.

ConeTwist Limits: Cone 120deg, Twist 0deg:
Behaviour natural, comes nicely to rest.
Transfering rotation around the length works well independent from bending.


Other engines have quaternion twist build in, but they don't have a stable solver like newton.
I use similar joint in ragdoll for a long time and never saw that quality in any actual games.
Dead bodies never do some unexpected motion, recommended!

[PJani]

Nice work JoeJ this will come to good use

[Julio Jerez]

Ha ok, after I complete the car feature, I will turn to make teh joind demo, and I will add that joint.
I will problably raplace the existing one with this, or combine teh two to make the best.

The one thing I also want to add in that joint that I was talking to you a while back, bascially a joint that will controll all thre angules in an independent order or rotation

[PJani]

Huh car feature when do we expect you will finish car? cant wait

[JoeJ]

No, no - rotation joint is the real cool thing... can't wait for that!

[Julio Jerez]

when do we expect you will finish car

I am hooping to have some that can be drive this weekend.

[JoeJ]

I observed some things about the joint that i want to add:

There is unwanted motion on dead bodies.
This happens f.ex. when the hand touches the cone limit, but no collisions stop its movement.
There happens some swinging motion that never stops, which is physical correct as there is no friction in the joint.
In worst case such motion can flow through the spine and keeps the whole ragdoll swinging.
For me that's no issue as i use powered ragdoll to simulate some friction, which stops that.
Playing Games i assume they mostly use high autosleep threshold to avoid that,
but it might be a better option to use angular damping - how can that be done with newton?
Adding additional row(s) to the joint for friction sounds like waste of CPU power to me.


Another thing i want to note is the use of uniform inertia for ragdoll bodies.
That means setting equal inerta values for each axis,
which can improve stability and is not visible, because the joints have most impact on rotational behaviour.
Worth trying out! (Havok did that internally, when i used it years ago)
I've used the mass value for inertia values and recommended that somewhere in the forum, but that's real bullshit.
Actually i compute a sphere with equal mass as the original body and use that, which seems the most correct to me now:

float radSph = pow (volumeOfOriginalBody / (4/3*PI), 1.0 / 3.0); // radius of a sphere with equal volume
float I = (2.0 * massOfOriginalBody * radSph*radSph) / 5.0;
BodySetMassMatrix (body, massOfOriginalBody, I,I,I);

[PJani]

@Julio: What type of car you are actually working on(body, convex, raycast)?

I think i have just made ball and socket joint with extra linear rows instead of using angular rows for twist. I looks more rigid

[Julio Jerez]

neither,
It is the vehicle mopdel of core 1.5 which wa sno a raycast car, but with some improvemnets.
The tires are convex cast, an it will has it own solver just like core 1.53
This is more expensive than pure ray cast car, but much cheaper that mutibopdy car.

Ray cast car are not worth the effort, because you can not really implement car behaviors as an emerging physics behaviors, instead you have to code each stuff specially.
stuff like drifting, power slide, circle of friction, are very eassy to add in mutibody model, while in a ray cast car they are always unstable an dhard to implement.
plus you can implement stuff like axel, (althought I am not doing that on the first pass)

[PJani]

I havent time to check your repo. Is vehicle drivable?

[Julio Jerez]

not is not ready yet, maybe one more week.