Fix using wrong quadratic solution for timestep

[Puttichai]

Background

In each iteration of the Check function, we solve for the next time step to check for constraints by solving a quadratic equation. The next time step to do constraint checking is the time step such that the joint nLargestStepIndex moves for an amount of its resolution.

Relevant codes are as follows

            dReal fMinNextTimeStep = prevtimestep; // when computing the new timestep, have to make sure it is greater than this value
            if( bComputeNewStep ) {
                if( prevtimestep >= fLargestInflectionTime ) {
                    fBestNewStep = fStep-fLargestStepDelta;
                }
                else {
                    // could be straddling the inflection so have to compensate. note that this will skip checking collision at the inflection
                    if( (fLargestStepDelta > 0 && fStep+fLargestStepDelta > fLargestInflection) || (fLargestStepDelta < 0 && fStep+fLargestStepDelta < fLargestInflection) ) {
                        fBestNewStep = fLargestInflection - (fStep+fLargestStepDelta - fLargestInflection);
                        fMinNextTimeStep = fLargestInflectionTime-1e-7; // in order to force to choose a time after the inflection
                    }
                    else {
                        fBestNewStep = fStep+fLargestStepDelta;
                    }
                }
            }

When the current time step is near the inflection time and we expect to the next time step to be after the inflection time, fMinNextTimeStep should be set to fLargestInflectionTime. The value fMinNextTimeStep will be used later on to choose an appropriate solution of the following quadratic equation

int numroots = mathextra::solvequad(fLargestStepAccel*0.5, fLargestStepInitialVelocity, -fNewStep, timesteproots[0], timesteproots[1]);

The Issue

fMinNextTimeStep should stay the same for the same istep. However, the following situation happens

• Iteration j
  • bComputeNewStep is true so we proceed to compute fBestNewStep as usual.
  • It happens that fMinNextTimeStep is also modified to be the inflection time fLargestInflectionTime.
  • The next time step for this iteration is computed to be t_next > fLargestInflectionTime.
  • At the end, it happens that istep does not get incremented because the reached time step is t_reached < fLargestInflectionTime. bComputeNewStep is now false because we should be using the same fBestNewStep in the following iteration.
• Iteration j + 1
  • istep remains the same as in iteration j. But fMinNextTimeStep gets reset to prevtimestep (which is now t_reached).
  • The next time step for this iteration is computed to be t_next < fLargestInflectionTime (due to the wrongly set fMinNextTimeStep.
  • At the end of the iteration, istep gets incremented while not having reached fBestNewStep yet. This messes up the counting of istep.
• In the end, istep can reach numSteps while the function does not yet reach the end point of the ramp.

This causes an error in parabolic smoother (function SegmentFeasible2) when it checks if the returned configurations from the Check function actually reached the desired values.

The Fix

Make sure that fMinNextTimeStep gets computed correctly even when bComputeNewStep = false.

[rdiankov]

@snozawa always scary to touch this function.. please review . thanks

[snozawa]

@snozawa always scary to touch this function.. please review . thanks

sure, i'll check!
(it might take time since the change is not trivial though)