How to make an object follow a path with CustomPathFollow

[arkdemon]

Hello everyone. I wanted to make an object follow a path (go to a point taking the shortest path) with the CustomPathFollow but I do not know how to use it.
Does someone know what is pinAndPivotFrame? Is it the matrix of the body or the matrix of the point to go to? and what are lineOrigin and lineSlope?

Thank you for your answers

[AntonSynytsia]

viewtopic.php?f=9&t=4716&p=33835#p33835

[arkdemon]

Hi. I guess I am an idiot but thanks for the link
I still have a problem though, how can we calculate lineSlope? It is written that it is the tangent of the line (in the code) but as I am a complete newbie to physics and maths (still have some years before learning what is used in Newton ), I do not know how to do it.

Thank you for your answers.

[AntonSynytsia]

I digged that link a while ago, just for the future reference on how to make curves, but I did not investigate. I don't understand that stuff myself, lol.

[JoeJ]

This joint constrains a body to keep a fixed distance to an infinite line (i have not used it, just looking at the code.)
The line is hardcoded, slope means simply the direction of that line.

[arkdemon]

Hi. Thank you for your answers.
I tried replacing the lineSlope by the direction where it should go but the object acts strangely; it starts to make huge turns, sometimes disappearing from the screen and it "trembles" all the time (it is like when you move very quickly your hand for example, you see your hand several times; this is what happens). Either it stays fix without mmoving at all until something collides with it or it acts strangely by flying with strange loops until it falls back onto the ground not at the correct point but close to it (generally or it never falls back).

PS: when I took the values of lineSlope and lineOrigin, it acts normally until 2 objects trying to go to the same point collide or touch each other; firstly, they act as if the two were glued together and then they fly away very quickly seperating themselves violently (each go at a different direction).

I think I may not be doing things right :/

[JoeJ]

It's not your fault, the joint seems to assume the line slope is perpendicular to world up axis.
Try this, it should work.

I replaced the tangent generation with Gramm Schmidt.
This gives 2 orthonormal directions the joint needs to keep the body on the line.

Code: Select all

void CustomPathFollow::SubmitConstraints (dFloat timestep, int threadIndex)
{
   dVector lineSlope (1.0f, 2.0f, -3.0f, 0.0f);
   lineSlope = lineSlope.Scale (1.0f / dSqrt (lineSlope % lineSlope));
   dVector lineOrigin (0.0f, 10.0f, 0.0f, 0.0);

   dMatrix matrix0;
      
   // Get the global matrices of each rigid body.
   NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
   matrix0 = m_localMatrix0 * matrix0;

   // Restrict the movement on the pivot along the infine line
   const dVector& p0 = matrix0.m_posit;
   const dVector& p1 = lineOrigin + lineSlope.Scale ((matrix0.m_posit - lineOrigin) % lineSlope);

   dMatrix randomTangents = dgGrammSchmidt (lineSlope);

   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &randomTangents.m_up[0]);
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &randomTangents.m_right[0]);
   
   if (1) // constrain orientation so pivot x axis stays parallel to line slope
   {
      // get a point along the ping axis at some reasonable large distance from the pivot
      dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
      dVector q1 (p1 + lineSlope.Scale(MIN_JOINT_PIN_LENGTH));

      // two more constraints rows to inforce the normal and binormal
       NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
      NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
   }

}


Here also my test setup:

Code: Select all

void CreatePathFollowTest ()
   {
      sMat4 m; m.Identity(); m.Reset4thColumn();

      Shape *shape = world->CreateBoxShape (sVec3(2.0, 0.5, 0.5));
      m[3] = sVec3 (0, 1.0, 0);
      Body *bPathFollow = world->CreateRigidBody (2.0, m, shape);
      world->ReleaseShape (shape);

      //m.Identity(); pivot.Reset4thColumn(); // this makes an offset of 1 unit

      CustomPathFollow *joint = new CustomPathFollow ((dMatrix&)m, bPathFollow);
   }

   void StepPathFollowTest ()
   {
      sVec3 lineOrigin (0.0f, 10.0f, 0.0f);
      sVec3 lineSlope (1.0f, 2.0f, -3.0f);
      lineSlope.Normalize();

      RenderVector (lineOrigin, lineSlope, 1,1,0);
   }


[Julio Jerez]

yes the default behaviors is that the path is a fix tangent horizontal line alone the x axis, the idea was that the joint is to be subclasses and function
dMatrix CustomPathFollow::EvalueCurve (const dVector& posit)
should be overloaded with one the return a matrix the aligned the pivot to the point on the path and the front axis is aligned to the path tangent.
these hard coded values are causing is what make the behavior you see
dVector lineSlope (1.0f, 0.0f, 0.0f, 0.0f);
dVector lineOrigin (0.0f, 10.0f, 0.0f, 0.0);

I see tha it can be difficult to write function EvalueCurve in a sub class an I agree it should be easier to use, so I change the class a little.
I added dVector lineSlope and dVector lineOrigin as members of the class, and I added functions
void CustomPathFollow::GetPathTarget (dVector& posit, dVector& tangent) const
void CustomPathFollow::SetPathTarget (const dVector& posit, const dVector& tangent)

so now you only need to call those functions when you want the object to move along the path.
Sync to GitHub so you get that change.

[arkdemon]

@JeoJ (was writing this before you wrote your post Julio )
Hi Thank you for your answer but I still get some objects flying all over the place, here is an example (.wmv) : https://dl.dropboxusercontent.com/u/949 ... .17.54.wmv

I put the background color to grey so that you can better see the objects (red) and the ground to greeen for the same reason.
If ever, you can not read the file, here is another link: (.avi) https://dl.dropboxusercontent.com/u/949 ... .17.54.avi
but the quality is much lower than the previous one.

This is the code I use to initialize the joint:

Code: Select all

fst::mat4 m = fst::translate(fst::mat4(1.f), fst::vec3(0.f,1.f,0.f));
            joint = new CustomPathFollow(dgMatrix(&m[0].x), b, dgVector(&point.x));//point is the point to go to, eg lineOrigin

and the custompathfollow:
PS: I did not compile dMath so I replaced every dxxxx function by dgxxx leading to this:

Code: Select all

#define dMatrix dgMatrix
#define dVector dgVector
#define GetIdentityMatrix dgGetIdentityMatrix
#define dAtan2 atan2f
#define dAbs abs
#define dCos cosf
#define dSin sinf
#define dAssert assert
#define dSqrt sqrtf

Code: Select all

#define MIN_JOINT_PIN_LENGTH   50.0f

//dInitRtti(CustomPathFollow);

CustomPathFollow::CustomPathFollow (const dMatrix& pinAndPivotFrame, NewtonBody* const child, dgVector _point)
    :CustomJoint(6, child, NULL)
{
    // calculate the two local matrix of the pivot point
    dMatrix tmp;
    point = _point;
    CalculateLocalMatrix (pinAndPivotFrame, m_localMatrix0, tmp);
}

CustomPathFollow::~CustomPathFollow()
{
}

void CustomPathFollow::GetInfo (NewtonJointRecord* const info) const
{
/*
    strcpy (info->m_descriptionType, "slider");

    info->m_attachBody_0 = m_body0;
    info->m_attachBody_1 = m_body1;

    if (m_limitsOn) {
        dFloat dist;
        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);
        dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;

        info->m_minLinearDof[0] = m_minDist - dist;
        info->m_maxLinearDof[0] = m_maxDist - dist;
    } else {
        info->m_minLinearDof[0] = -FLT_MAX ;
        info->m_maxLinearDof[0] =  FLT_MAX ;
    }


    info->m_minLinearDof[1] = 0.0f;
    info->m_maxLinearDof[1] = 0.0f;;

    info->m_minLinearDof[2] = 0.0f;
    info->m_maxLinearDof[2] = 0.0f;

    info->m_minAngularDof[0] = 0.0f;
    info->m_maxAngularDof[0] = 0.0f;

    info->m_minAngularDof[1] = 0.0f;
    info->m_maxAngularDof[1] = 0.0f;

    info->m_minAngularDof[2] = 0.0f;
    info->m_maxAngularDof[2] = 0.0f;

    memcpy (info->m_attachmenMatrix_0, &m_localMatrix0, sizeof (dMatrix));
    memcpy (info->m_attachmenMatrix_1, &m_localMatrix1, sizeof (dMatrix));
*/
}

void CustomPathFollow::SubmitConstraints (dFloat timestep, int threadIndex)
{
   dVector lineSlope (1.0f, 2.0f, -3.0f, 0.0f);
   lineSlope = lineSlope.Scale4(1.0f / dSqrt (lineSlope % lineSlope));
   dVector lineOrigin (0.0f, 10.0f, 0.0f, 0.0);

   dMatrix matrix0;

   // Get the global matrices of each rigid body.
   NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
   matrix0 = m_localMatrix0 * matrix0;

   // Restrict the movement on the pivot along the infine line
   const dVector& p0 = matrix0.m_posit;
   const dVector& p1 = lineOrigin + lineSlope.Scale4((matrix0.m_posit - lineOrigin) % lineSlope);

   dMatrix randomTangents = dgGrammSchmidt (lineSlope);

   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &randomTangents.m_up[0]);
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &randomTangents.m_right[0]);

   if (1) // constrain orientation so pivot x axis stays parallel to line slope
   {
      // get a point along the ping axis at some reasonable large distance from the pivot
      dVector q0 (p0 + matrix0.m_front.Scale4(MIN_JOINT_PIN_LENGTH));
      dVector q1 (p1 + lineSlope.Scale4(MIN_JOINT_PIN_LENGTH));

      // two more constraints rows to inforce the normal and binormal
      NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
      NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
   }

}

and the code for dgGrammSchmidt:

Code: Select all

dMatrix dgGrammSchmidt(const dVector& dir)
{
    dVector up;
    dVector right;
    dVector front (dir);

    front = front.Scale4(1.0f / dSqrt (front % front));
    if (dAbs (front.m_z) > 0.577f) {
        right = front * dVector (-front.m_y, front.m_z, 0.0f,0.f);
    } else {
        right = front * dVector (-front.m_y, front.m_x, 0.0f,0.f);
    }
    right = right.Scale4(1.0f / dSqrt (right % right));
    up = right * front;

    front.m_w = 0.0f;
    up.m_w = 0.0f;
    right.m_w = 0.0f;
    return dMatrix (front, up, right, dVector (0.0f, 0.0f, 0.0f, 1.0f));
}

Can we intialize lineSlope depending of lineOrigin and the position of the body? Can we do like this: lineSlope = normalize(lineOrigin - bodyPosition) ?

Thank you

[arkdemon]

@ Julio, OK I will do that and post my results Thanks

[arkdemon]

Hello. Is it normal that the body makes loops when I take the demonstration's values after it got to the point (I am not sure) and gets away from it?
and how can we calculate lineSLope?
Thank you

[Julio Jerez]

make a video.

the slope is the tangent to the path at the point that the body is at.

[JoeJ]

Is it normal that the body makes loops

You mean the same behaviour as in your video? No, this did not happen to me. Using the code example i gave the box flys slowly to the line and then does not leave the line. If the pivot matrix has an offset, the box rotates around the line, but it keeps its distance fixed - no loops.
I' sure it's the same for julios update. But i don't have an idea what's wrong. Check your matrices (last column 0,0,0,1 ?), ant try using body matrix as pivot, so there is no offset.

how can we calculate lineSLope?

Not sure what you mean because it's that simple:

vec slope = (linePointA - linePointB);
slope.Normalize();

I assume Julio renamed the "slope" term to "Tangent" in his new get and set functions, but it's still the same.

[arkdemon]

@ Julio, here is a video with all the default values:

Code: Select all

fst::mat4 m = fst::mat4(1.f);
m[POS] = fst::vec4(0.f,1.f,0.f,1.f);
joint = new CustomPathFollow(dgMatrix(&m[0].x), b);

https://dl.dropboxusercontent.com/u/949 ... .31.46.wmv

@ JoeJ Here is another video when I initialize it like this (I take the body's matrix):

Code: Select all

fst::mat4 m = pl::BodyMatrix(b);
m[POS] = fst::vec4(0.f,1.f,0.f,1.f);
joint = new CustomPathFollow(dgMatrix(&m[0].x), b);

PS: for the second video, normally it does better loops when I do not record it.

https://dl.dropboxusercontent.com/u/949 ... .47.51.wmv

[arkdemon]

And this is what happens when I initialize the joint like this:

Code: Select all

fst::mat4 m = pl::BodyMatrix(b);
m[POS].w = 1.f;
joint = new CustomPathFollow(dgMatrix(&m[0].x), b);
joint->SetPathTarget(dgVector(0.f,10.f,0.f,0.f), dgVector(1.f,0.f,0.f,0.f));

https://dl.dropboxusercontent.com/u/949 ... 2.4.44.wmv

It works fine for one body at a time but if there are several bodies at once, there is some strange behavior However, you can see in the video that they start to drift away slowly.

Edit: the body rotates quickly while going up and going away

Edit Edit: Here is a closeup view: https://dl.dropboxusercontent.com/u/949 ... 2.16.4.wmv
Normally it rotates at least twice as fast
It also happens that the body drifts away in the opposite direction.