Ragdoll best practice

[zak]

In the post correction i had doubts on orientation correction in HingeJoint::ProjectError() made with CalculatePitchAngle()
Is the explanation related with the small angular approximation technique of 6DOF joint?

In the near future, I will change the code a little, because the solver do not have provision for letting the application to apply the correction,
the solver integrate the velocity and position,
therefore I the correction need to remember the position of joints.
I will make the change so that the solve call the application in between the velocity integration and position integration.
the way the correction will be more correct, because only velocities need to be adjusted.
This should provide zero drift for all joints

A before position integration callback would be the best place in wich operate post correction: best precision, no overhead and even TransformCallback could be used again.

Is expected to brief introduction of before position integration callback?

[Julio Jerez]

I will probably have to revisit the position correct for some of the joints.
you know there are some restriction for that you nee to use an CustomArcticulatedTransformManager for post correct to be applied.
What are you doing? maybe I can help with some ideas of some improvement to the newton code you are usin.

@Joe:
Hey Joe I finally implemented the small angular approximation power Joint.

I used for tow joints: the 6DOF and a new ball and socket CustomControlledBallAndSocket
the CustomControlledBallAndSocket is very cool because you can control the Euler angle independent of each other and they do not have limit, each one can be unbounded and free for Gimbal lock.
I made so that it takes a angular speed, but it can be customized to just use anything, it turn out thet once the correct angular matrix is calculated the joint can take an arbitrarily large step.

[JoeJ]

Hey Joe I finally implemented the small angular approximation power Joint.

Whoooo!!!
Just came back from holiday, will try out now...

[JoeJ]

rotjoint.gif (7.77 KiB) Viewed 4644 times

As far as i understand up to now i'm afraid the method you use is exact the same i've already tried several times, and it never worked as good as expected.
I've added some visualization to your code and describe what i think to ensure we talk about the same thing.

The way i did it was this:
1. Take the target relative rotation between the two bodies and convert to axis and angle.
2. stabilize the axis of rotation with two perpendicular linear rows (the small green vectors in the screenshot)
3. to make it move, bring the two red dots, both lying on the plane of rotation, together (i've tried this already using either angular or linear row).

I still believe this is the right and simplest way to do it, and planned to try it once more, but up to now the result was always unstable when using in a many bodies system.
Do you think your method is different in any way? I'm unsure and teed to spend more time to fully anderstand the details...
There is at least one problem which prevents me from using it for my ragdoll at the moment:
It seems you can not control speed precisely - it varies from very fast to very slow using constant m_angulaSpeed.
To fix that we would need to set acceleration for the last row (step 3) - actually i think newton error correction does most of the work, so speed is unpredictable.
Do you agree?
I can do that, but because i've already done in the past i'd like to hear your thoughts first...

[JoeJ]

Ok, i've found a math bug in my old code. It works now, stable powered ragdoll joint with only one angular row.
If i wouldn't be that tired i'd jump around like three happy little kids.

Method is like described above. I suggest you make the last row with acceleration, and using an angular row works perfect for me.

Thanks a lot!

[Julio Jerez]

you can always emit constrains that will move one frame to the other but moving along the sort angle, what you can not do is to emit a
serious of constrains that will do the same and at the same time have control of the euler angle. The only time this possible is when eh step are small angles.
so the trick is no to get there in one step, but doing in s series of steps, keeping track of the current angles.

The method I am using is this:
you have two bodies that you one to constraint to rotate around a pivot.
let us call body1 the reference frame and body2 to the child frame.

you can express the write the relative rotation matrix that take frame1 to frame to as

F2 = L * F1

this Frame2 is equal to ration matrix L time rotation matrix Frame1
we want Matrix L to be expressed as three independent angular rotations Picth, Yaw, Roll, so we get
F2 = P * Y * R * F1
The problem that we have is that successive rotation do no yield Matrix L to be the composition of three angular rotations, successive Rotations yield the result.
F2 = Pn * Yn * Rn * .... P1 * Y1 * R1 * P0 * Y0 * R0 * F1
and we want is,
F2 = (Pn * ... * P1 * P0) * (Yn * ... * Y2 * Y0) * (Rn * ... * R1 * R0) * F1
the is the only way that matrix L can me decompose into impendent euler angles that can be added or substrate, is if we keep tack on the previous rotation PYR in the class
so if you consider two consecutive rotations, the fist one is

F2 = L0 * F1 = P0 * Y0 * R0 * F1

you can decompose

L0 = P0 * Y0 * R0 and save the angle with the matrix. and save wi the class

The next rotation is

F2 = L1 * F1 = P1 * Y1 * R1 * F1 = Px * Yx * Rx * P0 * Y0 * R0 * F1

where are the tree small angle the frame have o rotate Px * Yx * Rx that can be calculate as

Px * Yx * Rx = P1 * Y1 * R1 * inv (P0 * Y0 * R0)

so the joint code doe a fallows

1-build the Euler matrix of the previous rotation
2-calcualte relative matrix for frame 1 to frame2.
3-decompose the new matrix in the three eulers angles and save fo next update.
4- calculate the relative small angular matirx Lsmall = Px * Yx * Rx = P1 * Y1 * R1 * inv (P0 * Y0 * R0)

5-convert matrix Lsmall to a quaternion to get the axis and angle that frame1 have to rotation and add the constrains rows to do that.
Now this stable because either frame will follow the same arch, basically the will be doing just one small rotation around the shortest path that eliminate matrix Lsmall


this is similar to doing three linear translation, but instead to emit three linear row along any three perpendicular axis, you emit one linear row that tru to eliminate the Euclidian distance between eth two pivot and the other two row are just to constraints perpendicular to the path for stability.

[Julio Jerez]

The implementation is in file
../newton-dynamics\packages\dCustomJoints\CustomBallAndSocket.h

class CustomControlledBallAndSocket

[JoeJ]

I use your implementation.
The problem is the non constant speed, so i was unable to plug it into ragdoll, but i noticed the joint is stable.
In parctice i don't care if i can set a target offset for the next frame (like i do), or if i set a target and a speed (like you do) - both is the same.
My interface allows me to drag a number with the mouse, and i mapped 3 numbers to the target euler angles.
By doing so you can see that the speed varies in an unpredictable way, seeming not only related to the size of the error angle.
I added 2 setups to the joints demo to illustrate, one moves 50 degrees and the other 5, but both take approx the same time.
How can i calculate how long it takes to reach the target?

The problem that i had with my old 3 angles joint was this:
I have a constraint space and get the rotation axis directly from there, and the angles by dot products of those axis with the actual error.
To keep it stable i try to rotate this constraint space as less as possible from step to step when error is small, and if error is large i align one constraint space axis more closely with error.
Problem is: Movement is slower when error is distributed equally over all axis, and faster when error is the most on one axis.
By tweaking i can choose between limiting unpredictable speed or increasing stability, but a can't get the best of both.
My expectations on your new joint are to fix that. I guess it is more stable, but i don't know yet how to control speed.
For the ragdoll i don't need to control or track euler angles, so my new single axis joint seems to solve all my problems.

Anyways, i need to get animation mapping back working again.
Then i can play a running animation and measure which method can follow the animation most exactly...

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
*/

#include <toolbox_stdafx.h>
#include "SkyBox.h"
#include "DemoEntityManager.h"
#include "DemoCamera.h"
#include "PhysicsUtils.h"
#include "DemoMesh.h"
#include "../toolBox/OpenGlUtil.h"
#include <CustomGear.h>
#include <Custom6DOF.h>
#include <CustomHinge.h>
#include <CustomSlider.h>
#include <CustomPulley.h>
#include <CustomCorkScrew.h>
#include <CustomBallAndSocket.h>
#include <CustomRackAndPinion.h>

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 (dGetIdentityMatrix());
      dMatrix parentMatrix (dGetIdentityMatrix());

      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));
         p1 -= 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;
};

class CustomBallAndSocketWithFriction: public CustomBallAndSocket
{
   public:
    CustomBallAndSocketWithFriction (const dMatrix& pinAndPivotFrame, NewtonBody* const child, NewtonBody* const parent, dFloat dryFriction)
        :CustomBallAndSocket (pinAndPivotFrame, child, parent)
        ,m_dryFriction (dryFriction)
    {
    }

    void SubmitConstraints (dFloat timestep, int threadIndex)
    {
        CustomBallAndSocket::SubmitConstraints (timestep, threadIndex);

        dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
        dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);

        // get the omega vector
        NewtonBodyGetOmega(m_body0, &omega0[0]);
        if (m_body1) {
            NewtonBodyGetOmega(m_body1, &omega1[0]);
        }

        dVector relOmega (omega0 - omega1);
        dFloat omegaMag = dSqrt (relOmega % relOmega);
        if (omegaMag > 0.1f) {
            // tell newton to used this the friction of the omega vector to apply the rolling friction
            dMatrix basis (dGrammSchmidt (relOmega));
            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]);
            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]);
            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[0][0]);

            // calculate the acceleration to stop the ball in one time step
            dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;

            // override the desired acceleration, with the desired acceleration for full stop.
            NewtonUserJointSetRowAcceleration (m_joint, -omegaMag * invTimestep);

            // set the friction limit proportional the sphere Inertia
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_dryFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_dryFriction);
        } else {
            // when omega is too low this is correct but the small angle approximation theorem.
            dMatrix basis (dGetIdentityMatrix());
            for (int i = 0; i < 3; i ++) {
                NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[i][0]);
                NewtonUserJointSetRowMinimumFriction (m_joint, -m_dryFriction);
                NewtonUserJointSetRowMaximumFriction (m_joint,  m_dryFriction);
            }
        }
    }

    dFloat m_dryFriction;
};

static NewtonBody* CreateBox (DemoEntityManager* const scene, const dVector& location, const dVector& size)
{
    NewtonWorld* const world = scene->GetNewton();
    int materialID =  NewtonMaterialGetDefaultGroupID (world);
    NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
      DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

    dFloat mass = 1.0f;
    dMatrix matrix (dGetIdentityMatrix());
    matrix.m_posit = location;
    matrix.m_posit.m_w = 1.0f;
    NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

    geometry->Release();
    NewtonDestroyCollision(collision);
    return body;
}

static NewtonBody* CreateCapule (DemoEntityManager* const scene, const dVector& location, const dVector& size)
{
   NewtonWorld* const world = scene->GetNewton();
   int materialID =  NewtonMaterialGetDefaultGroupID (world);
   dMatrix uprightAligment (dRollMatrix(3.141592f * 90.0f / 180.0f));
   NewtonCollision* const collision = CreateConvexCollision (world, &uprightAligment[0][0], size, _CAPSULE_PRIMITIVE, 0);
   DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

   dFloat mass = 1.0f;
   dMatrix matrix (dGetIdentityMatrix());
   matrix.m_posit = location;
   matrix.m_posit.m_w = 1.0f;
   NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

   geometry->Release();
   NewtonDestroyCollision(collision);
   return body;
}



static NewtonBody* CreateWheel (DemoEntityManager* const scene, const dVector& location, dFloat radius, dFloat height)
{
    NewtonWorld* const world = scene->GetNewton();
    int materialID =  NewtonMaterialGetDefaultGroupID (world);
    dVector size (radius, height, 0.0f, 0.0f);
    NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _CHAMFER_CYLINDER_PRIMITIVE, 0);
    DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

    dFloat mass = 1.0f;
    dMatrix matrix (dGetIdentityMatrix());
    matrix.m_posit = location;
    matrix.m_posit.m_w = 1.0f;
    NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

    geometry->Release();
    NewtonDestroyCollision(collision);
    return body;
}

static NewtonBody* CreateCylinder (DemoEntityManager* const scene, const dVector& location, dFloat radius, dFloat height)
{
    NewtonWorld* const world = scene->GetNewton();
    int materialID =  NewtonMaterialGetDefaultGroupID (world);
    dVector size (radius, height, 0.0f, 0.0f);
    NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _CYLINDER_PRIMITIVE, 0);
    DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

    dFloat mass = 1.0f;
    dMatrix matrix (dGetIdentityMatrix());
    matrix.m_posit = location;
    matrix.m_posit.m_w = 1.0f;
    NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

    geometry->Release();
    NewtonDestroyCollision(collision);
    return body;
}


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

   dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));

   // connect first box to the world
   dMatrix matrix0;
   NewtonBodyGetMatrix (box0, &matrix0[0][0]);
   pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
   new CustomBallAndSocket (pinMatrix, box0, NULL);

   // link the two boxes with a distance joint
   dMatrix matrix1;
   NewtonBodyGetMatrix (box1, &matrix1[0][0]);

   // get the origins
   dVector pivot0 (matrix0.m_posit);
   dVector pivot1 (matrix1.m_posit);

   // connect bodies at a corner
   new CustomDistance (pivot0, pivot1, box0, box1);
}


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

   dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));

   // connect first box to the world
   dMatrix matrix0;
   NewtonBodyGetMatrix (box0, &matrix0[0][0]);
   pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
   new CustomBallAndSocketWithFriction (pinMatrix, box0, NULL, 2.0f);

   // link the two boxes
   dMatrix matrix1;
   NewtonBodyGetMatrix (box1, & matrix1[0][0]);
   pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
   new CustomBallAndSocket (pinMatrix, box0, box1);
}


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

   const dFloat angle = 60.0f * 3.1415592f / 180.0f;
   dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));

   // connect first box to the world
   dMatrix matrix0;
   NewtonBodyGetMatrix (box0, & matrix0[0][0]);
   pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
   Custom6DOF* const joint0 = new Custom6DOF (pinMatrix, pinMatrix, box0, NULL);
   joint0->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));

   // link the two boxes
   dMatrix matrix1;
   NewtonBodyGetMatrix (box1, &matrix1[0][0]);
   pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
   Custom6DOF* const joint1 = new Custom6DOF (pinMatrix, pinMatrix, box0, box1);
   joint1->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
}

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

   dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));

   // connect first box to the world
   dMatrix matrix0;
   NewtonBodyGetMatrix (box0, & matrix0[0][0]);
   pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
   CustomControlledBallAndSocket* const joint0 = new CustomControlledBallAndSocket (pinMatrix, box0, NULL);
   joint0->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
   joint0->SetPitchAngle (-45.0f * 3.141592f / 180.0f);
   joint0->SetYawAngle (-85.0f * 3.141592f / 180.0f);
   joint0->SetRollAngle (120.0f * 3.141592f / 180.0f);

   // link the two boxes
   dMatrix matrix1;
   NewtonBodyGetMatrix (box1, &matrix1[0][0]);
   pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
   CustomControlledBallAndSocket* const joint1 = new CustomControlledBallAndSocket (pinMatrix, box0, box1);
   joint1->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
   joint1->SetPitchAngle (45.0f * 3.141592f / 180.0f);
   joint1->SetYawAngle ( 30.0f * 3.141592f / 180.0f);
   joint1->SetRollAngle (25.0f * 3.141592f / 180.0f);
}

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

   dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));

   // connect first box to the world
   dMatrix matrix0;
   NewtonBodyGetMatrix (box0, & matrix0[0][0]);
   pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
   CustomControlledBallAndSocket* const joint0 = new CustomControlledBallAndSocket (pinMatrix, box0, NULL);
   joint0->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
   joint0->SetPitchAngle (0);
   joint0->SetYawAngle (0);
   joint0->SetRollAngle (0);

   // link the two boxes
   dMatrix matrix1;
   NewtonBodyGetMatrix (box1, &matrix1[0][0]);
   pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
   CustomControlledBallAndSocket* const joint1 = new CustomControlledBallAndSocket (pinMatrix, box0, box1);
   joint1->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
   joint1->SetPitchAngle (0);
   joint1->SetYawAngle (5.0f * 3.141592f / 180.0f);
   joint1->SetRollAngle (0);
}

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

   dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));

   // connect first box to the world
   dMatrix matrix0;
   NewtonBodyGetMatrix (box0, & matrix0[0][0]);
   pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
   CustomControlledBallAndSocket* const joint0 = new CustomControlledBallAndSocket (pinMatrix, box0, NULL);
   joint0->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
   joint0->SetPitchAngle (0);
   joint0->SetYawAngle (0);
   joint0->SetRollAngle (0);

   // link the two boxes
   dMatrix matrix1;
   NewtonBodyGetMatrix (box1, &matrix1[0][0]);
   pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
   CustomControlledBallAndSocket* const joint1 = new CustomControlledBallAndSocket (pinMatrix, box0, box1);
   joint1->SetAngularVelocity (1000.0f * 3.141592f / 180.0f);
   joint1->SetPitchAngle (0);
   joint1->SetYawAngle (50.0f * 3.141592f / 180.0f);
   joint1->SetRollAngle (0);
}




void AddHinge (DemoEntityManager* const scene, const dVector& origin)
{
    dVector size (1.5f, 4.0f, 0.125f);
    NewtonBody* const box0 = CreateBox (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), size);
    NewtonBody* const box1 = CreateBox (scene, origin + dVector (1.5f, 4.0f, 0.0f, 0.0f), size);
    NewtonBody* const box2 = CreateBox (scene, origin + dVector (3.0f, 4.0f, 0.0f, 0.0f), size);

    // the joint pin is the first row of the matrix, to make a upright pin we
    // take the x axis and rotate by 90 degree around the y axis
    dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));

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

    // add hinge with limit and friction
    CustomHinge* const hinge0 = new CustomHinge (matrix, box0, NULL);
    hinge0->EnableLimits (true);
    hinge0->SetLimis(-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
    hinge0->SetFriction (20.0f);

    // link the two boxes
    NewtonBodyGetMatrix (box1, &matrix[0][0]);
    matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
    matrix = localPin * matrix;
    CustomHinge* const hinge1 = new CustomHinge (matrix, box0, box1);
    hinge1->EnableLimits (true);
    hinge1->SetLimis (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
    hinge1->SetFriction (20.0f);

    // link the two boxes
    NewtonBodyGetMatrix (box2, &matrix[0][0]);
    matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
    matrix = localPin * matrix;
    CustomHinge* const hinge2 = new CustomHinge (matrix, box1, box2);
    hinge2->EnableLimits (true);
    hinge2->SetLimis (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
    //hinge2->SetFriction (20.0f);
}

static void AddSlider (DemoEntityManager* const scene, const dVector& origin)
{
    // make a reel static
    NewtonBody* const reel = CreateBox (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), dVector (8.0f, 0.25f, 0.25f, 0.0f));
    NewtonBodySetMassMatrix (reel, 0.0f, 0.0f, 0.0f, 0.0f);

    NewtonBody* const wheel = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);

    dMatrix matrix;
    NewtonBodyGetMatrix (wheel, &matrix[0][0]);

    CustomSlider* const slider = new CustomSlider (matrix, wheel, reel);

    // enable limit of first axis
    slider->EnableLimits(true);

    // set limit on second axis
    slider->SetLimis (-4.0f, 4.0f);
}

static void AddCylindrical (DemoEntityManager* const scene, const dVector& origin)
{
    // make a reel static
    NewtonBody* const reel = CreateCylinder (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 0.25f, 8.0f);
    NewtonBodySetMassMatrix (reel, 0.0f, 0.0f, 0.0f, 0.0f);

    NewtonBody* const wheel = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);

    dMatrix matrix;
    NewtonBodyGetMatrix (wheel, &matrix[0][0]);

    CustomCorkScrew* const cylinder = new CustomCorkScrew (matrix, wheel, reel);

    // enable limit of first axis
    cylinder->EnableLinearLimits(true);

    // set limit on second axis
    cylinder->SetLinearLimis (-4.0f, 4.0f);
}


static CustomHinge* AddHingeWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
{
    NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);

    // the joint pin is the first row of the matrix
    //dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
    dMatrix localPin (dGetIdentityMatrix());
    dMatrix matrix;
    NewtonBodyGetMatrix (wheel, & matrix[0][0]);
    matrix = localPin * matrix;

    // connect first box to the world
    return new CustomHinge (matrix, wheel, parent);
}


static void AddGear (DemoEntityManager* const scene, const dVector& origin)
{
    NewtonBody* const reel = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
    NewtonBodySetMassMatrix (reel, 0.0f, 0.0f, 0.0f, 0.0f);

    CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 1.0f, reel);
    CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 1.0f, reel);

    NewtonBody* const body0 = hinge0->GetBody0();
    NewtonBody* const body1 = hinge1->GetBody0();

    dMatrix matrix0;
    dMatrix matrix1;
    NewtonBodyGetMatrix (body0, &matrix0[0][0]);
    NewtonBodyGetMatrix (body1, &matrix1[0][0]);

    dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
    dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
    new CustomGear (4.0f, pin0, pin1, body0, body1);
}


static CustomSlider* AddSliderWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
{
    NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);

    // the joint pin is the first row of the matrix
    //dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
    dMatrix localPin (dGetIdentityMatrix());
    dMatrix matrix;
    NewtonBodyGetMatrix (wheel, & matrix[0][0]);
    matrix = localPin * matrix;

    // connect first box to the world
    return new CustomSlider (matrix, wheel, parent);
}

void AddPulley (DemoEntityManager* const scene, const dVector& origin)
{
    NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
    NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f));
    NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
    NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);

    CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
    CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel1);

    slider0->EnableLimits(true);
    slider0->SetLimis (-2.0f, 2.0f);

    NewtonBody* const body0 = slider0->GetBody0();
    NewtonBody* const body1 = slider1->GetBody0();

    dMatrix matrix0;
    dMatrix matrix1;
    NewtonBodyGetMatrix (body0, &matrix0[0][0]);
    NewtonBodyGetMatrix (body1, &matrix1[0][0]);

    dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
    dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
    new CustomPulley (4.0f, pin0, pin1, body0, body1);
}


static CustomCorkScrew* AddCylindricalWheel (DemoEntityManager* const scene, const dVector& origin, dFloat radius, dFloat height, NewtonBody* const parent)
{
    NewtonBody* const wheel = CreateWheel (scene, origin, height, radius);

    // the joint pin is the first row of the matrix
    dMatrix matrix;
    NewtonBodyGetMatrix (wheel, &matrix[0][0]);

    return new CustomCorkScrew (matrix, wheel, parent);
}


static void AddGearAndRack (DemoEntityManager* const scene, const dVector& origin)
{
    NewtonBody* const reel0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
    NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
    NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
    NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);

    CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
    CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
    CustomCorkScrew* const cylinder = AddCylindricalWheel(scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel1);

    cylinder->EnableLinearLimits(true);
    cylinder->SetLinearLimis(-2.0f, 2.0f);

    NewtonBody* const body0 = hinge0->GetBody0();
    NewtonBody* const body1 = hinge1->GetBody0();
    NewtonBody* const body2 = cylinder->GetBody0();

    dMatrix matrix0;
    dMatrix matrix1;
    dMatrix matrix2;

    NewtonBodyGetMatrix (body0, &matrix0[0][0]);
    NewtonBodyGetMatrix (body1, &matrix1[0][0]);
    NewtonBodyGetMatrix (body2, &matrix2[0][0]);

    dVector pin0 (matrix0.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
    dVector pin1 (matrix1.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
    dVector pin2 (matrix2.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));

    new CustomGear (1.0f, pin0, pin2, body0, body2);
    new CustomRackAndPinion (1.0f, pin1, pin2, body1, body2);
}




void StandardJoints (DemoEntityManager* const scene)
{
    scene->CreateSkyBox();

    // customize the scene after loading
    // set a user friction variable in the body for variable friction demos
    // later this will be done using LUA script
    dMatrix offsetMatrix (dGetIdentityMatrix());

    CreateLevelMesh (scene, "flatPlane.ngd", 1);

    dVector location (0.0f, 0.0f, 0.0f, 0.0f);
    dVector size (1.5f, 2.0f, 2.0f, 0.0f);

   AddDistance (scene, dVector (-20.0f, 0.0f, -20.0f));
   AddBallAndSockectWithFriction (scene, dVector (-20.0f, 0.0f, -15.0f));
   Add6DOF (scene, dVector (-20.0f, 0.0f, -10.0f));
   AddPoweredRagDoll (scene, dVector (-20.0f, 0.0f, -5.0f));

AddPoweredRagDoll2 (scene, dVector (-25.0f, 0.0f, -2.0f));
AddPoweredRagDoll3 (scene, dVector (-25.0f, 0.0f, -7.0f));

   AddHinge (scene, dVector (-20.0f, 0.0f, -0.0f));
   AddSlider (scene, dVector (-20.0f, 0.0f, 5.0f));
   AddCylindrical (scene, dVector (-20.0f, 0.0f, 10.0f));

    //add relational joints example
   AddGear (scene, dVector (-20.0f, 0.0f, 15.0f));
   AddPulley (scene, dVector (-20.0f, 0.0f, 20.0f));
   AddGearAndRack (scene, dVector (-20.0f, 0.0f, 25.0f));

   // this joint is not very stable when using non rotational inertia, like these examples
   // AddUniversal (mSceneMgr, m_physicsWorld, Vector3 (2.0f, 0.0f, 30.0f));

    // place camera into position
    dMatrix camMatrix (dGetIdentityMatrix());
    dQuaternion rot (camMatrix);
    dVector origin (-40.0f, 5.0f, 0.0f, 0.0f);
    scene->SetCameraMatrix(rot, origin);
}


[Julio Jerez]

Maybe it was not you, but I remember that you for a long time were looking for a joint when you can control the ordination of a child bone but specifying three target angles.
I said that using the angle as parameter is no to have three independent angles with one coordinate system, because each time you angle changes then the third angle because is a function of the first two.
however if the movement in each step small then the, then you can implement a joint that can actually do it.
the reason is that when angel are small, the product products the matrices commute and the error is also small.

when the code calculate the small matrix, I do not think they is guarantee the the angle speed is constant. what is contact is the angular speed along the shortest path from the origin to the target.

The joint guarantee that is reach the target, but it does not guarantee the Path to reach that target.

esencially you use like this.

set the desirer pitch you and roll and the joint will move there. as long as new angle are small changes for the previous orientation, the movement is almost linear in all tree angles.

say for example you have a joint setup, and you want to move to
pitch = 30.0
yaw = 15.0
Roll = 75.0
you can do something like this

Code: Select all

vector angle0 (joint->SetPitchAngle (), joint->SetYawAngle (),joint->SetRollAngle ());
vector angle1 (pitch, yaw, Roll);

vector error (angle1 - angle0);
if (error.dot(error) > someValue) {
    angle1 = someValue.Scale (error.normalized);
}
joint->SetPitchAngle (angle1.x);
joint->SetYawAngle (angle1.y);
joint->SetRollAngle (angle1.z);

and the joint will move to that new target following the shortest arch path as close as possible, however the concatenation of those arch moment will not be a straight geodesic arch,
It can only be a straight geodesic arch when the steps are infinitesimally small and the solver was infinitely strong, however for reasonable size angular steps it should do a very good job.
and you are treating the angles as if they were is a Cartesian space.

[JoeJ]

Maybe it was not you, but I remember that you for a long time were looking for a joint when you can control the ordination of a child bone but specifying three target angles.

It's me, but now i realize we've had a big misunderstanding for a long time, i'm sorry for that

I'd like to try out the solver bugfix.
I've a situation where i need 3 angular constraints for powered ragdoll joints.
It shows up that while this is stable, it produces slower motion than using a single constraint.
Maybe the fix helps here too.
...
I want to do a single relative rotation between the 2 bodies, so i try a single angular constraint.
That works, but if the joint is nearly satisfied (angle nearly zero), the axis of that rotation jumps around wildely, so it gets instable - damn, damn and once again: Damn!
To fix that, i distribute the rotation on three fixed axis, in a way that at least guaranties that the extraction of the angles is order indipendent.
Now everything is stable, but less exact, so less responsive.

http://newtondynamics.com/forum/viewtopic.php?f=9&t=6894&start=60

I always wanted just a shortest arc rotation from current state to a single target orientation - the 3 independent angles where just a necessary evil for me.
Because the single angular row idea initially did not work i thought using 3 angular rows like in kinematic joint is the only way to keep stable at rest.
It worked, but i was not happy and knew there should be a better way.
Then when you introduced your euler angles idea, i did not understand competely but thought that must be the correct solution to the problem.

Ironically you helped me to solve it. I saw in your code you stabilize the rotation axis the same way i've already tried, and it works for you.
So i tried again, realized a bug and now it works like it should.


Finally i think you should add the shortest arc rotation controller instead the euler angle method
It's difficult for users to do that themselves, and this feature out of the box should be very attractive.
Physics need to become more active than passive - moving robots instead box stacks...
Your engine is by far the best for that, but researchers still use ODE with super small timesteps for their work.
If you want i can implement that joint for you.

[Julio Jerez]

Ok I will add a arch path ball and sockect