Enabling Bodies

[Sweenie]

Hi.

I'm glad that you found my demo useful for debugging.
I have always been a big fan of this engine so it's nice to be able to help out.

Regarding the keys for the forklift, it should be like this...
W,A,S,D = Steering & driving
Space = Brakes
Q, E = Lift the fork up and down
R, F = Tilt the fork up and down(realised though that this is not the way a real forklift works, but hey, it's just for fun)

The collisions works perfectly now(unless I set solvermode to 0, see below).

Some things that are different though compared to ver 2.33/2.34.
In 3.0 the bodies move slower, like they are under water or something.
I'm using a gravity force of -11.0f but it behaves like it's only 6 or 7(compared to 2.34)
I have set angular and linear damping to 0 but it doesn't make any difference.

Also the wheels on the forklift as well as the fork itself turns faster in 2.34 and doesn't have that "extra" inertia as you mention, in 2.34 the forkjoint stops immediately when releasing the key.
I've tried changing the joint stiffness but that doesn't help.

[EDIT] Try replacing newton.dll in the demo folder with the 2.33(2.34) dll and you will see the difference. [/EDIT]

Just remembered though that I get the same "low gravity" and joint issues in 2.34 as well, if I change the Solvermode to anything other than 0.
In 3.0 if I try to set the SolverMode to 0, the forklift won't move at all, in world/test the bodies fall through each other and world/test2 just crasch the demo.
What is the default solvermode in 3.0?

And the joints.
I have three custom joints.
One raycast joint, one wheeljoint that is almost exactly like the MultiBodyVehicle joint and one "ForkLift joint".

The code below is a bit messy.

ForkLift joint

Code: Select all

void NewtonLiftJoint::SubmitConstraints(const NewtonJoint* joint, dFloat timestep, int threadIndex)
{
   const float PIN_DIST = 50.0f;

   NewtonLiftJoint* lj = static_cast<NewtonLiftJoint*>(NewtonJointGetUserData(joint));

   Transform4D childTransform,parentTransform;
   NewtonBodyGetMatrix(lj->bodyA, &childTransform(0,0));

   if(lj->bodyB)
      NewtonBodyGetMatrix(lj->bodyB, &parentTransform(0,0));

   Transform4D childPivot,parentPivot;

   Matrix3D rot;
   rot.SetIdentity();
   rot.SetRotationAboutX(lj->angle * (3.14f/180.0f));

   childPivot = childTransform * ( lj->localTransformA * rot );

   if(lj->bodyB)
      parentPivot = parentTransform * lj->localTransformB;
   else
      parentPivot = lj->locatorWorldTransform;
   
   Vector3D childFront = childPivot[0];// Pin dir(red axle)
   Vector3D childRight = childPivot[1];
   Vector3D childUp   = childPivot[2];
   Point3D childPos = childPivot.GetTranslation();

   Vector3D parentFront = parentPivot[0];// Pin dir(red axle)
   Vector3D parentRight = parentPivot[1];
   Vector3D parentUp   = parentPivot[2];
   Point3D parentPos = parentPivot * lj->relPos;

   // Restrict linear movement
   NewtonUserJointAddLinearRow (joint, &childPos.x, &parentPos.x, &childFront.x);
   NewtonUserJointAddLinearRow (joint, &childPos.x, &parentPos.x, &childRight.x);
   NewtonUserJointAddLinearRow (joint, &childPos.x, &parentPos.x, &childUp.x);

   // Restrict angular movement
   // Using Linear rows instead which gives a more robust joint, Angular Rows has a tendency to break/give in
   Vector3D pointInPinChild = parentPos + parentFront * PIN_DIST;
   Vector3D pointInPinParent = childPos + childFront * PIN_DIST;
   Vector3D pointInPinChild2 = parentPos + parentRight * PIN_DIST;
   Vector3D pointInPinParent2 = childPos + childRight * PIN_DIST;
   NewtonUserJointAddLinearRow(joint, &pointInPinParent.x, &pointInPinChild.x, &childRight.x);
   NewtonUserJointAddLinearRow(joint, &pointInPinParent.x, &pointInPinChild.x, &childUp.x);
   NewtonUserJointAddLinearRow(joint, &pointInPinParent2.x, &pointInPinChild2.x, &childUp.x);

}

RayCast joint

Code: Select all

void RCVWheel::SubmitConstraints(const NewtonJoint* joint, dFloat timestep, int threadIndex)
{
   const float PIN_DIST = 50.0f;

   RCVWheel* w = static_cast<RCVWheel*>(NewtonJointGetUserData(joint));
   RCV* r = w->chassi;

   Transform4D chassiTransform;
   NewtonBodyGetMatrix(r->GetBody(), &chassiTransform(0,0));

   Point3D suspPos; //Hard point there the suspension connects with the chassi
   suspPos = chassiTransform * w->GetTargetNode()->GetNodePosition();

   Matrix3D rot;
   rot.SetIdentity();
   rot.SetRotationAboutZ((w->steering * w->maxSteerAngle) * (3.14f/180.0f));

   Vector3D chassiFront = chassiTransform[0]; //X
   Vector3D chassiRight = chassiTransform[1]; //Y
   Vector3D chassiUp    = chassiTransform[2]; //Z

   Vector3D wheelRight = chassiTransform.GetMatrix3D() * (rot * Vector3D(0, -1, 0));
   wheelRight.Normalize();

   NewtonWorld* nw = TheNewtonMgr->GetWorld();

   // Cast Ray
   Point3D p0 = suspPos;
   Point3D p1 = suspPos + chassiTransform * Vector3D(0.0f, 0.0f, -(w->suspRestLength + w->radius) );
   w->hitDist = 99.0f;
   w->onGround = false;
   w->prevsuspLen = w->suspLen;
   NewtonWorldRayCast(nw, &p0.x, &p1.x, RayCallBack, w, 0);
   bool wheelLocked = false;

   if(w->onGround)
   {
      w->suspLen = ((w->suspRestLength + w->radius) * w->hitDist) - w->radius;
      Vector3D contactForward = Cross(w->hitNormal, wheelRight);
      //TheEngine->Report(String<>(Magnitude(contactForward)));
      
      Vector3D contactRight = Cross(contactForward, w->hitNormal);

      Point3D chassiCOM;
      NewtonBodyGetCentreOfMass(r->GetBody(), &chassiCOM.x);
      chassiCOM = chassiTransform * chassiCOM; //Local COM to Global
      float chassiMass, Ixx, Iyy, Izz;
      NewtonBodyGetMassMatrix(r->GetBody(), &chassiMass, &Ixx, &Iyy, &Izz);

      w->hitPos = suspPos + chassiTransform * Vector3D(0.0f, 0.0f, -(w->suspRestLength + w->radius) * w->hitDist);
      float compression = 1.0f - w->hitDist;

      float nDotUp = Dot(w->hitNormal, chassiUp);
      float springForce = w->suspStiffness * chassiMass *  compression * nDotUp;
      float comprVel = ( w->prevsuspLen - w->suspLen ) / timestep;
      float dampingForce;
      if(comprVel > 0)
         dampingForce = w->suspDamping * chassiMass * comprVel;
      else
         dampingForce = w->suspDamping * 0.5f * chassiMass * comprVel;   //TODO: Add Relaxation Damping as Wheel Parameter

      float normalForce = (springForce + dampingForce);

      //if(compression > 0.01f )
      //{
      //   NewtonUserJointAddLinearRow(joint, &suspPos.x, &suspPos.x, &chassiUp.x);
      //   //NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
      //   NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
      //}
   
      //Vector3D chassiForce = normalForce * chassiUp;
      Vector3D chassiForce = normalForce * w->hitNormal;
      Vector3D chassiTorque = Cross(w->hitPos - chassiCOM, chassiForce);
      NewtonBodyAddForce(r->GetBody(), &chassiForce.x);
      NewtonBodyAddTorque(r->GetBody(), &chassiTorque.x);

      // TODO: Implement minimum suspension length to prevent wheel from going into chassi.
      // Linear Row along suspension direction

      float maxForce = w->suspStiffness * chassiMass * 0.3f * nDotUp;
      normalForce = Min(normalForce, maxForce);
      float maxFriction = normalForce * 2.8f;   // TODO: Add Slip Coefficient as Wheel Parameter
      Vector3D forcePos = w->hitPos + chassiTransform * Vector3D(0.0f, 0.0f, 0.4f);  //Roll Reduction   TODO: Add Roll Reduction as Wheel Parameter
      Vector3D brakePos = w->hitPos + chassiTransform * Vector3D(0.0f, 0.0f, 0.2f);  //Roll Reduction   TODO: Add Roll Reduction as Wheel Parameter

      float brakeForce = w->brakeTorque * w->brake;
      if(brakeForce > 0.0f)
      {
         maxFriction -= brakeForce;
         NewtonUserJointAddLinearRow(joint, &brakePos.x, &brakePos.x, &contactForward.x);
         NewtonUserJointSetRowMinimumFriction(joint, -maxFriction);
         NewtonUserJointSetRowMaximumFriction(joint, maxFriction);
         wheelLocked = true;
      }

      // Lateral Friction
      NewtonUserJointAddLinearRow(joint, &forcePos.x, &forcePos.x, &contactRight.x);
      NewtonUserJointSetRowMinimumFriction(joint, -maxFriction);
      NewtonUserJointSetRowMaximumFriction(joint, maxFriction);

      // Longitudinal Friction
      // TODO: Brake force / Combine with Drive Force & Lateral Friction Force and scale them to fit within a friction cone.
      // Must be done before adding constraint rows

      // Drive force
      Vector3D chassiVelocity(0,0,0);
      Vector3D chassiOmega(0,0,0);
      NewtonBodyGetVelocity(r->GetBody(), &chassiVelocity.x);
      NewtonBodyGetOmega(r->GetBody(), &chassiOmega.x);
      chassiVelocity += Cross(chassiOmega, suspPos - chassiCOM);
      w->WheelForwardVel = Dot(chassiVelocity, contactForward);
      float wheelRPM = Fabs(w->WheelForwardVel / w->radius);
      float torqueDamping = 0.02f;   //Uhm, this makes drive force decline as the wheel rotates faster... Higher value means lower top speed
      wheelRPM = Max(1.0f / torqueDamping, wheelRPM);

      float latVel = Fabs(Dot(chassiVelocity, chassiRight));

      float driveForce = ( w->driveTorque * w->throttle ) / (wheelRPM * torqueDamping);

      if(brakeForce == 0.0f)
      {
         Vector3D forwardForce = contactForward * (driveForce);  // TODO: Fix proper engine force and stuff :)
         Vector3D forwardTorque = Cross(forcePos - chassiCOM, forwardForce);
         NewtonBodyAddForce(r->GetBody(), &forwardForce.x);
         NewtonBodyAddTorque(r->GetBody(), &forwardTorque.x);

         if(w->hitBody)
         {
            Vector3D backwardForce = contactForward * (-driveForce);  // TODO: Fix proper engine force and stuff :)
            //Vector3D backwardTorque = Cross(forcePos - chassiCOM, forwardForce);
            NewtonBodyAddForce(w->hitBody, &backwardForce.x);
         }

      }

   }
   else
   {
      w->suspLen = w->suspRestLength;
      w->skidding = false;

      if(w->driveTorque * w->throttle == 0.0f)
         w->WheelForwardVel *= 0.99f; // Slow down the wheels so they don't spin forever...
      else
         w->WheelForwardVel = 15.0f * w->throttle; // Faking a bit here. ;)
   }

   // Update subnode(Wheel visuals)
   if(w->subNode && wheelLocked == false)
   {
      Matrix3D spinRot;
      spinRot.SetIdentity();
      w->spinAngle += w->WheelForwardVel / (w->radius * 2.0f);
      spinRot.SetRotationAboutY(w->spinAngle * (3.14f / 180.0f));
      spinRot = rot * spinRot;

      Point3D wheelpos(0.0f, 0.0f, -w->suspLen);
      Transform4D wheeltransform(spinRot, wheelpos);
      w->subNode->SetNodeTransform(wheeltransform);
      w->subNode->Invalidate();
   }



[Julio Jerez]

The collisions works perfectly now(unless I set solvermode to 0, see below).

Ha that explained, solver mode zero make feel very snappy.
For core 300 I have not impelmenetd teh exact complemnetary solver yet.
The defualt mode is mode 1.

what we see is that thr iterative solver does not find the exact reaction forces in eh give number of passes and it terminate with an aproximation,
but that is the compromize we have to make if we want fast vs acurate simulation.
I guess it is fine untill I get to implement the exact solver. That will come later since there are still many more critical areas to work with.

as a partical remedy you can increase the solver passes say maybe mode 4 or 6 an that will make more snappy. No as good as mode zeor but much more responsive than mode 1.


keep the demos comming, the more we ge the more I can polish the engine.

One feature I am palnning ot add to cpore 300 is teh ability to select the teh solve on per join bases.
for example you will wna teh fok lift to be very responsive by maybe so they nee solver mode zero, but all other object in teh scene can do just find with solve mode 1.

[Sweenie]

One feature I am palnning ot add to cpore 300 is teh ability to select the teh solve on per join bases.

oh, that would be awesome.

By the way, just tried changing the solver mode to 4, but it didn't make any noticable difference at all. The forkjoint still moves for a short time
after I released the key. Then I changed the solver mode to 6, the same thing.
I even tried setting the solvermode to 64(64 is the max nr of passes, right?), and yet it feels just the same as solvermode 1.
Maybe there is something I'm not doing right in the joint.

[Sweenie]

I've found something interesting with custom joints that happens in both 2.33 and 3.0.

As mentioned earlier my vehicle falls slower when using SolverMode 1.
I did some testing and found that this happens when my body has more than one joint.

I created a simple body with a box collider and attached a custom joint that does nothing(just to keep it simple)...

Code: Select all

void CustomJoint::SubmitConstraints(const NewtonJoint* joint, dFloat timestep, int threadIndex)
{
// Do nothing
   return;
}

First with only 1 joint.
After running the simulation several times I find that it takes about 1450 milliseconds for the body to hit the ground.

Then I add another joint, so the body now has 2 joints.
Now it takes bout 1800 milliseconds for the body to hit the ground.

Note that this only happens in Solvermode 1 to n....
In solvermode 0(exact solver) this doesn't happen.

Put together a little demo to show the problem.
http://www.svenberra.net/testjoint.zip
Just do the same as the other demo, open the console and type load world/test
You will see two green boxes.
One box have 1 custom joint and the other box has 2 custom joints. The joint does nothing(as described above).

[Julio Jerez]

Ha, this is a problem I found about two years ago, but that it never put time to work on it.
first I will explain the theroty behind the iteratove solver so that you understand why this happens, There are two complementarity solvers in the engine.
one is Linear Search Conjugate Gradient (i call this exact because it alway sove the system of equation untill the error is bellow some small values, or when there is no solution becaus the syetm is over or under determined
it fin the best least sqaure aproximattion to the set of forces, so the solve nev fail to find a solition even if teh matrix is ill formed),
the other based on Gauss Sidlle iterations and this is you get what the solver can find ina fix nomber olf steps, so teh force are no alway teh eaxct value to prevent constraint violations.

When a body has only one joint the engine uses the exact solver. for two or more joints it uses the iterative solver when the mode is set to iterative.

The iterative solver uses a my own version of Runge Kutta order 4 with a twist. The original RK4 integration method works like this:
-Evaluate the derivative four times at sub intevale, an advance the state variables by a weighed average of all the derivatives.
say the y is the state variable (position and orientaion of a body). say that dh is the time step and h is the current time, in general then the next state is given by:

y(h + dh) = y + Y'(dh)
Y' = is the partial derivatives of y relative to all components of Y and all the degree of freedom. In newton Y' is what the function call JacobianDerivative does

the RK4 method states that instead of calculation the derivative at time h, it is better to calculating 4 derivatives at for differnt times

y1 = h * f'(h, y)
y2 = h * f'(h + dh/2, y + y1/2)
y3 = h * f'(h + dh/2, y + y2/2)
y3 = h * f'(h + dh, y + y3)

the new position is given by
y(h + dh) = y(h) + y1/6 + 2 *y2 / 6 + 2 * y3 / 6 + y4 /6

to implement this in a rigid body physics solver it is not very easy. There are many, many hard problems that have to be addressed first.
The first one is that applying a naive RK4 like I see many people do is not any better than simply dividing the step by 4 and take 4 steps,
it is just marginally better, it is exelen for integration smooth functions, but it can even diverge for highly non linear functions or indicent funtions.
(a smooth function is one with continues derivatives, an indecent funtion is one with discontinues derivatives)
the reason for the devergence si that all RK methods the most importnt condition is that the the must have fist and secund continue derivatives. and contact are not continue.

normally the error in an integration function is given by the series expansion of that function so is you take a function
y(h) = f (h)

then

Y(h + dh) = y(h) + f'(h) * dh + k * f''(h) * dh^2 + ....

a simple Euler integration will take only the first derivative an advance by n, this will generate an integration that can be written as:

Y(h + dh) = y(h) + f'(h) * dh + O (dh^2)

where the error is
O (dh^2) = k * f''(h) * dh^2 + ....

now if you divide the time by 4 what you get is
RK4 evaluate the same 4 derivative but instead of doing a normal accumulation what it does is that is average the derivatives using a weigh

Y(h + h/4) = y(h) + y'(n/4) * dh + O ((dh/4)^2)
Y(h + h/4) = y(h) + y'(n/4) * dh + O (dh/16)

the accumulated error is 4 time O (dh^2/16)

RK4 evaluates the same 4 derivatives but instead of doing a normal accumulation, what it does is that it averages the derivatives using weighted coefficients,
empirically RK4 itegration test demostate that it produces an accumulated error of order O(dh^4)

The problem is that for some non lineal functions and for non smooth functions O(dh^4) can be larger than 4 * O(dh^2/16)
It just happens that In rigid body physics, Joints are extremely nonlinear, and contact are non smooth, so it appears that the normal RK4 is useless for rigid body physics.

The second problem is that taking the 4 steps will make the method way more expensive than simply subdividing the time step.
This is because taking four steps, runs the collision system and the new system matrix for that island can be complelly different.

So the problem is how to get the best of RK4 by advancing the simulation at a faster rate without taking a full step, and at the same time rejecting the worse of RK4
like it nastines in the present of indecent functions like rigibody derivatives but that can be trivially solved by taking a full step?

what I do is that I advance the simulation 4 times in a temporary variable but instead of calculating the full derivatives,
I calculate the approximation of the derivative using partial derivatives in an extra callback called JointAccelerations.
in the cases where there is an explicit junction like joints this is eassy, in the case of contacs I use linear extrapolation.
then instead of using the original RK4 coefficient (1/6, 2/6, 2/6, 1/6) and I use (1/4, 1/4, 1/4, 1/4)

I believe that the discrepancies comes from the fact that I use different set of Coefficient plus that I evaluate the partial derivatives in different order.
RK4 evaluate the derivative at times [h, (h + dh/2), (h + dh/4), (h + 3 * dh/4)] and I do it at times [h, (h + dh/4), (h + dh/2), (h + 3 * dh/4)]
but that twist allows me to have only one partial derivative function or one with a parameter that indicates if it is a full derivation of a Partial extrapolation.

when I first spotted that problem my intention was to fix it but using a full RK4, but I realized it is not that simple since it almosts double the amount of code in the callback,
because I will have to have JointAccelerations1, JointAccelerations2, JointAccelerations3 for each joint, plus the inner loop of the solver will get more complex.
Since for most people, this was not a real issue, I decided not to fix it at the time.

Maybe I am over simplificating it, but it really is a hard problem to solve, and I am not really sure if my suspicions are the real reason for the problem.
It adds to the problem the fact that in the literature of numerical analysis, I had never found if the weighting coefficients in (1/6, 2/6, 2/6, 1/6) in RK4, are empirical or analytical.
if they are analytical, then my suspicion is valid and what it means is that I may be losing energy. If they are empirical, then it means I may have a bug somewhere.

Since you are the first person to find that problem, and you produce a demo to clearrly expose it, I guess this is a good time to investigate more and see what I can do about.
Worse comes to worse it is not really a big deal, because we have many other solutions to work apound problem using heurstics. we can:

-Add the exact solver, and use only one derivative.
-and add the feature that island with bilateral joints are solved using the exact solver, the same way that islans with only one joint are doing now in core 200

[Sweenie]

Wow, that was a detailed explanation.

Well, this isn't a problem for me right now since I prefer to use the exact solver anyway.
So I'll just stay with version 2.33 until you find a solution for the "multiple joints problem" or implement the exact solver in 3.0

I also suppose I could change my raycast car to use only 1 joint instead of 4 by adding all the constraint rows inside the same joint.

For the forklift this would be trickier though, since it's chassi is connected to 5 different physical bodies(4 wheel bodies and the fork).

[Julio Jerez]

I am interested in people to start switching to core 300, I can make demos muselft but that is very time consuming and usully I hit a rowblock with GUIs, file format, assets,
and other nonsense that are very time cosuming for me, and I ussually end up working on a diffrent problem.

The first thing I will do is that I will add the secund solver to core 300. and then teh featire I was tlaking about.

Then I will take a secund look at that problem, I will have ot start by refreshing myself in the derivation of where the weighting coefiecent of RK4 come from before I start making any changes.
I need to make sure that the theory is correct before I move on. But liek I say we do have a solution, it is just that is not in yet.

The good thing abut the exact solvers for core 300, is that it can very eassyly be implemented using muticores, as opposed to how it was in core 200. so all in all it iwkl no be a big slow down.

[Sweenie]

Sounds great.

Just let me know if you need any specific scenarios to test.

@Yezide.
Sorry for hijacking your thread.

[Julio Jerez]

Ok Sweenie, I have good news, some bad news, and good news again. all in all it is all good news. and it is all thanks to you finding that nasty bug that was there for years.

The first good news, is that my RK4 twist is correct, in fact is very valid. in numerical analysist there are few versions that do use 1/4 coefficients.
the choice of 1/6, 2/6, 2/6, 1/6 is a matter of arbitratry choices solving a matrix of coefficients, here is a paper that explain the process, of how to get to those numbers
http://pathfinder.scar.utoronto.ca/~dye ... ode50.html

the w1, w2, w2, w3 for a RK4 method are in fact arbitrary, you can actually force then to be anything you want. what is special about [1/6, 2/6, 2/6, 1/6] is that they produce higher accuracy, so they say.
(my opinion is that very much like everything else, in the bigotry world of the self appointed academia expertrts that control ACM, IEEE, etc, no one had ever put that claim to the test.
and what happen is that one person published a paper with those numbers and the next one took it then as the bible. whent in fact those numbers depend of what values you use solve 9 linear equations)

I found that using my those numbers force you to have an evaluation that is gratiltuslly more complex that evaluating the equation at 1/4 * h. but the pay off is not really that different.
I still have to evaluate the derivatives 4 times, but the tradiotional RK4 requires a lot more extra space and for the engie that space will almost double the size of each body.
I have run many integrations of equation comparing the difference, between traditional RK4, EULER with h/4 and my version of RK4 and my result shows
that the accuracy of RK4 is only marginally better using the 1/6 numbers. The good news is that my method does not violates any law of Algebra an yeld similar results.

That was good news, the bad news is that if that was right then, then I most have a bug somewhere in the code.
and that could be really, really bad news because it would mean that the method could be unstable once I find the bug, and if that was the case I will be at square one whne I fix it,
or I can just leave the bug and leave with it. I was ready to leave with the bug if it turned out that the solution would make the solver unstable.
The problem with that is that I could not leave with myself knowing that the bug was there, and not telling the people that there was an error there.
I want the accuracy but I did not want to lose the stability, so I was ready to add an option if it was necessary.

I started revisiting the code and guess what, I found the bug. it is here. if you look at the solver function
void CalculateForcesGameMode(dgInt32 iterations, dgFloat32 maxAccNorm) const

After the loop that does that calculates the derivatives at each step. teh velocity is integrated on teh body velocity in this loop

Code: Select all

    for (dgInt32 i = 1; i < m_bodyCount; i++)
    {
      dgBody* const body = bodyArray[i].m_body;
      dgVector force(body->m_accel + internalForces[i].m_linear);
      dgVector accel(force.Scale(body->m_invMass.m_w));
      body->m_veloc += accel.Scale(timeStep);
      internalVeloc[i].m_linear += body->m_veloc;
    }

After the loop that does that calculates the derivatives at each step. the velocity is integrated on the
the vector internalVeloc is the RK4 intemediate derivative that I was mention I would he to have 4 of those for traditinal RK4
time step, is already weithed by teh 1/4 coeficeint before enterin the loop.

then after the loop is completed I still need to calculate the final state of the body, that is, the net body acceleration, because Netwon is not like all the comercial impulse based engine where to forces are converted
to impulse and you integrate the velocity, in newton the forces do persidt so there is another loop that add the effect of all the reaction forces togher with the external forces. that loop is this

Code: Select all

  dgFloat32 maxAccNorm2 = maxAccNorm * maxAccNorm;
  for (dgInt32 i = 1; i < m_bodyCount; i++)
  {
    dgBody* const body = bodyArray[i].m_body;

#ifdef DG_WIGHT_FINAL_RK4_DERIVATIVES
    body->m_veloc = internalVeloc[i].m_linear.Scale(invStep);
#endif

    dgVector accel = (body->m_veloc - body->m_netForce).Scale(m_invTimeStep);

    body->m_accel = accel;
    body->m_netForce = accel.Scale(body->m_mass[3]);
  }

note: in case you are confused, on initialization the body current velocity is copied to the variable m_netForce. then the net force is updated with the actual force acting on teh body body.
any way my mistake was that in the secund loop, I weighted the velocity again!!!

Code: Select all

    body->m_veloc = internalVeloc[i].m_linear.Scale(invStep);

I wrapped on the conditional just in case.
after debugging an revisiting the math I found that that was a mistake, in fact I was lossing energy and could be the bad news. because more energy might bring the instatbility.
so very, very scared I comment out the line and run all the test, and guess what, they are as stable as they were with the line, but they are more acurate. No longer the slow motion behavior!!.
I run all the test many time to make sure It was not a fluke, but just in case I put the line inside a conditional.

I am very happy with the result, so far they are just as stable as before, just as fast or marginally faster, and they are more acurate. what else could we ask?
I added the change and all of the stacking test are very stable, I run then with auto sleep off, that way I do not get false sence of acuracy and stability.

I tested with your jorklift demo, and I did not see anything extraodinary. It is diffcult for me to see if it is moving faster or slower, but I do not see any instability.
This fix does no adress the incapacity of the iterative solver to calculate exact reaction forces in one step,
Also I have not seeng why increasion the number of iteration is not yielding better solution, my guess that's a bug too and I will investigate.

Anyway please see if this is better now.

Now I feel so much better, because I can add the exact solver, not to hide a bug, but to provide a better functionality.

[Sweenie]

Excellent.

The raycast car now behaves just as it did with the exact solver in 2.33.
The forklift works fine too except for the "non snappy" joints though.

[Julio Jerez]

Excellent.
The forklift works fine too except for the "non snappy" joints though.

yes but that is expected, the solution to that problem will be the feature we talked about, a smart selection of exact vs iterative whn bilateral joints are in an island
for that I have do complete the exact solver for core 300. I am doing that now.

then later I will see why increasing the iterations in teh iterative solve is not increasin teh acurasy as we expect.
I am afraid there will be nothing I can do about since that is the nature of Guass Siddle, but keeps the hopes up, maybe there is a bug.

I will bring back a feature I had in core 100, which was a singular value decomposition to analyse the jacobian matrix but not for remove bad rows as I was doing in core 100.
That is usefull to determine the rate of convergence we can expect in each Jacobi or sidlle iteration, if the Condition number is high then that will explain the reason for poor converge and there is nothing we can do.
if it is low then it means there is a bug some where and we can fix it.
what I think is that your Forklift, is made of rigid bodies with very different mass ratios, and that lead to a matrix with high condition number.
therefore a Gausss siddle, Jacobi or any stationary method for liniar system of equations, will be limited by that factor and after few iterations it goes in an asymtotic rate of converge.
Only a non stationary method can deal with matrices with higher condition number and that is the exact solver.

[Sweenie]

The chassi has a mass of 1500, each wheel a mass of 100 and the fork a mass of 250.
Just to see what would happen I changed the mass to 10 for the chassi, wheels and fork. But it behaved just the same anyway.

[Julio Jerez]

the ideal condition numbe of a matrix is 1.0
there are many factor the contribuet to that value, teh mass valeu is teh most dominant an dteh oen we can control, how ever teh inreta distrubution is also a big factor
therfore even if you set all masses to the same value, it will be extamally har to get a conditin numeb of 1.0
only a diagonal matrix can gave that.

do this. Set all the masses o fteh forklift to the same value. That will not make the solver better but is should make it better by increasing the number of iterations.
if that test shows an improvement as you increase the number of passes it means there is nothing we can do.

[Sweenie]

Just tried it.
All bodies having the same mass and started with solvermode 1, then 10, 20, 30, 40, 50 but I couldn't see any difference at all.

[Julio Jerez]

ok, that might be good news, because then maybe there is a bug,
let me complete the convetion of the exact solver an dhen I take a look at that.