How to create a distance constraint?

[pingpongduo]

I would like to know how one would go about creating a distance constraint in newton. The constraint should work as follows:
Given a point on body A and body B, the points must always remain at some specified distance towards to each other regardless of the orientation of the bodies. This means that the bodies are free to rotate about the relative points but the distance must always be maintained.

[Julio Jerez]

isn' t tha a Ball and sockect?

Oh I see it is quite simple, I will post the code when I get home.
Basically it is a ball and socke with the the a relative distance,
I will add it to the standard joints demo.

[pingpongduo]

No it is not. Take a look at the graphics below. [] would be the two bodies, the = signs would be the constraint, and the 0 would be the ball and socket joints.

pic 1:
[]-0========0-[]

[JoeJ]

Meanwhile you could use a virtual body with null collision and two ball sockets in between.

[Body1]-0===[Virtual body]===0-[Body2]

[Julio Jerez]

ok here it is

Code: Select all

class CustomDistance: public CustomJoint 
{
   public:
   CustomDistance (const dVector& pivotInChildInGlobalSpace, const dVector& pivotInParentInGlobalSpace, NewtonBody* const child, NewtonBody* const parent)
      :CustomJoint(3, child, parent)
   {
      dVector dist (pivotInChildInGlobalSpace - pivotInParentInGlobalSpace) ;
      m_distance = dSqrt (dist % dist);

      dMatrix childMatrix (GetIdentityMatrix());
      dMatrix parentMatrix (GetIdentityMatrix());

      childMatrix.m_posit = pivotInChildInGlobalSpace;
      parentMatrix.m_posit = pivotInParentInGlobalSpace;
      childMatrix.m_posit.m_w = 1.0f;
      parentMatrix.m_posit.m_w = 1.0f;

      dMatrix dummy;
      CalculateLocalMatrix (childMatrix, m_localPivot, dummy);
      CalculateLocalMatrix (parentMatrix, dummy, m_parentPivot);
   }

    void 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_localPivot, m_parentPivot, matrix0, matrix1);

      dVector p0 (matrix0.m_posit);
      dVector p1 (matrix1.m_posit);

      dVector dist (p1 - p0);
      dFloat mag2 = dist % dist;
      if (mag2 > 0.0f) {
         dist = dist.Scale (1.0f / dSqrt (mag2));
         p0 += dist.Scale(m_distance);
      }

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

   dMatrix m_localPivot;
   dMatrix m_parentPivot;
   dFloat m_distance;
};

if you sync to svn, you can get the working demo by plain the standard joints in the SDK.
file ..\newton-dynamics\applications\demosSandbox\sdkDemos\demos\StandardJoints.cpp

[pingpongduo]

Meanwhile you could use a virtual body with null collision and two ball sockets in between.

[Body1]-0===[Virtual body]===0-[Body2]

I will eventually try this if I cannot find any other way.

Julio I tried your implementation but it is not working as expected. It does keep the distance between the bodies but not how I want. Picture this, imagine we have two bodies with location as follows:

body1 location 1 <0,0,0>
body2 location 2 <1,1,0>

The problem is that if body 1 is on a ground plane, body 2 is behaving as if it is not affected by gravity. It will just however in the air maintaining the distance that was set. What I want is for body 2 to fall to the ground rather than hover in the air, while the distance is still maintained.

[JoeJ]

Here is what i have.
The problem is it only works if the distance is from com to com of both bodies.
If there is a offset, bodies wobble.
Maybe Julio knows a solution, but i have doupts there is one.

Code: Select all

class DistanceJoint
{
public:

   NewtonBody* body0;
   NewtonBody* body1; // parent
   NewtonJoint* joint;
   NewtonWorld* world;
   sVec3 localPoint0;
   sVec3 localPoint1;
   float distance;

   DistanceJoint (NewtonWorld* world, NewtonBody* body0, NewtonBody* body1,
               sVec3 localPoint0, sVec3 localPoint1, float distance)
   {
      int numRows = 1;//3
      joint = NewtonConstraintCreateUserJoint (world, numRows, DistanceJointCallback, 0, body0, body1);
      NewtonJointSetUserData (joint, this);

      this->body0 = body0;
      this->body1 = body1;
      this->localPoint0 = localPoint0;
      this->localPoint1 = localPoint1;
      this->distance = distance;
      this->world = world;
   }

   ~DistanceJoint () {NewtonDestroyJoint (world, joint);}

   void SubmitConstraints (float timestep, int threadIndex)
   {
      sMat4 matrix0, matrix1;
      BodyGetMatrix ((Body*)body0, *((sMat4*)&matrix0));       
      if (body1) BodyGetMatrix ((Body*)body1, *((sMat4*)&matrix1));
      else matrix1.Identity();

      sVec3 p0 = matrix0.Transform (localPoint0);
      sVec3 p1 = matrix1.Transform (localPoint1);

      sVec3 dir = p0 - p1;
      float len = dir.Length();
      if (len < FP_EPSILON) dir = sVec3 (1,0,0);
      else dir /= len;
      
      //sVec3 tang0 = dir.arbitaryTangent(); // construct a 'random' perpendicular vector, Newton has Gramm Schmidt for that somewhere 
      //sVec3 tang1 = tang0.Cross (dir); // second tangent

      p0 -= dir * distance * 0.5;
      p1 += dir * distance * 0.5;
      
      NewtonUserJointAddLinearRow (joint, (float*)&p0, (float*)&p1, (float*)&dir);
      //NewtonUserJointAddLinearRow (joint, (float*)&p0, (float*)&p1, (float*)&tang0);
      //NewtonUserJointAddLinearRow (joint, (float*)&p0, (float*)&p1, (float*)&tang1);
   }

};

void DistanceJointCallback (const NewtonJoint* joint, float timestep, int threadIndex)
{
   DistanceJoint *custom = (DistanceJoint*) NewtonJointGetUserData(joint);
   custom->SubmitConstraints (timestep, threadIndex);
}


Test-Setup:

Code: Select all

      sVec3 cent (5, 0, 10);
      
      Body *b0 = CreateQuickBox (*this, cent + sVec3(0.5, 0, 0), sVec3(0.4, 0.4, 0.4), 10);
      Body *b1 = CreateQuickBox (*this, cent + sVec3(-0.5, 0, 0), sVec3(0.4, 0.4, 0.4), 10);
      DistanceJoint *j = new DistanceJoint (world, b0, b1, sVec3(0,0,0), sVec3(0,0,0), 1.0); // works
      
      Body *b2 = CreateQuickBox (*this, cent + sVec3(1.5, 0, 2), sVec3(0.4, 0.4, 0.4), 10);
      Body *b3 = CreateQuickBox (*this, cent + sVec3(-1.5, 0, 2), sVec3(0.4, 0.4, 0.4), 10);
      DistanceJoint *j2 = new DistanceJoint (world, b2, b3, sVec3(-1,0,0), sVec3(1,0,0), 1.0); // not perfect stable if offset from center of mass


[Julio Jerez]

The problem is that if body 1 is on a ground plane, body 2 is behaving as if it is not affected by gravity. It will just however in the air maintaining the distance that was set. What I want is for body 2 to fall to the ground rather than hover in the air, while the distance is still maintained.

The joint I poste do just that, the example is connecting one body to the world,
if you want to see both falling and keeping the distance commnet out the code that attach one body to the word, lie this

Code: Select all

static void AddDistance (DemoEntityManager* const scene, const dVector& origin)
{
   dVector size (1.0f, 1.0f, 1.0f);
   NewtonBody* const box0 = CreateBox (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
   NewtonBody* const box1 = CreateBox (scene, origin + dVector (size.m_x * 2.0f, 5.0 - size.m_y * 2.0f, size.m_z * 2.0f, 0.0f), size);

   // connect first box to the world
   dMatrix matrix;
   NewtonBodyGetMatrix (box0, & matrix[0][0]);
   matrix.m_posit += dVector (-size.m_x * 0.5f, size.m_y * 0.5f, -size.m_z * 0.5f, 0.0f);

// comment this out to d\see boteh body falling while keeping the distance
//   new CustomBallAndSocketWithFriction (matrix, box0, NULL, 2.0f);

   // link the two boxes with a distance joint
   dMatrix matrix0;
   dMatrix matrix1;
   NewtonBodyGetMatrix (box0, &matrix0[0][0]);
   NewtonBodyGetMatrix (box1, &matrix1[0][0]);
   dVector pivot0 (matrix0.m_posit + dVector ( size.m_x * 0.5f, -size.m_y * 0.5f,  size.m_z * 0.5f, 0.0f));
   dVector pivot1 (matrix1.m_posit + dVector (-size.m_x * 0.5f,  size.m_y * 0.5f, -size.m_z * 0.5f, 0.0f));
   new CustomDistance (pivot0, pivot1, box0, box1);
}

I jus tried and it word, in the demo also the boxes are connected at one corner instead of the center , this is to make more difficult to simulate because the bodies generate very large gyroscopic force that need to be calculated by the engine
when bodie are connected at the center it is much easier simpler since gyro force are negligible.

pleasa try because I believe this is what you described. The bodies do not hover in any case.

[Julio Jerez]

actually I just check it in with wit that like commented put

sync, build and play standard joint demo.
you will see the attached bodies to the left of the screen.

I also make so that they are connect at the center, so that it is more intuitive

[JoeJ]

I tried Julios method too, and remember i did initially the same before.
It works, but the rotation is not totally independent. One body affects the other - not much, but it's there.
That's why i ended up using the midpoint as pin and only a single row.
The gyroscopic motion (what i meant with 'wobble') is similar for both methods.
Depends on usecase, what works better.
I think the virtual body method would give the best simulation quality, but it adds mass and is slower.

[pingpongduo]

actually I just check it in with wit that like commented put

sync, build and play standard joint demo.
you will see the attached bodies to the left of the screen.

I also make so that they are connect at the center, so that it is more intuitive

Just did and checked it out, now I see what you did. From playing with the demo, I realized that depending upon which box is the parent, will determine how the constraint behaves. If you grab one of the boxes with the mouse and cause it to stand on one of its side, it causes the other box to hover in mid air. How can this behavior be removed?

[JoeJ]

Try to modificate Julios joint code this way to quickly test if my method works better for you,
it must be somewhere in Joint Library / CustomDistanceJont.cpp

Code: Select all

dVector dist (p1 - p0);
      dFloat mag2 = dist % dist;
      if (mag2 > 0.0f) {
         dist = dist.Scale (1.0f / dSqrt (mag2));
         p0 += dist.Scale(m_distance * 0.5f); // <--
         p1 -= dist.Scale(m_distance * 0.5f); // <--
      }

     NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0],
&dist[0]);
// remove the other 2 rows below
     


[Julio Jerez]

Just did and checked it out, now I see what you did. From playing with the demo, I realized that depending upon which box is the parent, will determine how the constraint behaves.

when any o fteh tow bodies is not NULL, It should not make any different which body is the parent of the child

If you grab one of the boxes with the mouse and cause it to stand on one of its side, it causes the other box to hover in mid air. How can this behavior be removed?

can you make a youtube video so that I can see what you mean, I do not see any hovering effect.

do you meak that if one biody is on top of the other, teh body on top hover? if thsi si what you mean that is correct, whe that happen the body can not fall,
it will move to one side flowing a pendum property, whi is on teh apet teh veloicity will ve zero, and is will game speed only if is sufficent far from the apex.
please make a youtube video.

also try what joe sugected, it make make a better ditribution of the error violations.

@Joe, you say remove the two rows, I thonk taht doin that will no inforce the distance,
unless he want to only inforce the projection of the disatance along one direction, in which case i misundetood the problem
and it will be a very different joint

[JoeJ]

@Joe, you say remove the two rows, I thonk taht doin that will no inforce the distance,
unless he want to only inforce the projection of the disatance along one direction, in which case i misundetood the problem
and it will be a very different joint

it works, because if you move one body perpendicular to the distance direction, you do not violate the plane but you violate the distance,
so next step the new distance direction will fix the error. Perpendicular rows are not really necessary in that case.
I tried a snake with 10 bodies - both methods have different problems, yours give some energy gaining jumpy linear motion,
mine give gaining angular motion and oszillation due the missing perp. rows.
There's no clear winner, but you should give it a try to see yourself it works.

For your method it makes a small difference who is parent, for mine it does not.

[pingpongduo]

Try to modificate Julios joint code this way to quickly test if my method works better for you,
it must be somewhere in Joint Library / CustomDistanceJont.cpp

Code: Select all

dVector dist (p1 - p0);
      dFloat mag2 = dist % dist;
      if (mag2 > 0.0f) {
         dist = dist.Scale (1.0f / dSqrt (mag2));
         p0 += dist.Scale(m_distance * 0.5f); // <--
         p1 -= dist.Scale(m_distance * 0.5f); // <--
      }

     NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0],
&dist[0]);
// remove the other 2 rows below
     


I tried this method and it works. However, if one of the bodies move fast say around 25 mph, the other becomes unstable and starts bouncing around.

Julio I will make a video to show you how yours is working.