TypeIIRMLVelocityFallBackStrategy.cpp

Go to the documentation of this file.

//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
//   For a convenient reading of this file's source code,
//   please use a tab width of four characters.
//  ----------------------------------------------------------


#include <TypeIIRMLVelocity.h>
#include <RMLVelocityInputParameters.h>
#include <RMLVelocityOutputParameters.h>


using namespace TypeIIRMLMath;


//****************************************************************************
// FallBackStrategy()

void TypeIIRMLVelocity::FallBackStrategy(       const RMLVelocityInputParameters    &InputValues
                                            ,   RMLVelocityOutputParameters         *OutputValues   )
{
    unsigned int        i   =   0;

    for (i = 0; i < this->NumberOfDOFs; i++)
    {
        if (InputValues.SelectionVector->VecData[i])
        {
            (OutputValues->NewPositionVector->VecData)[i]
                =   InputValues.CurrentPositionVector->VecData[i]
                    +   this->CycleTime
                    *   (InputValues.CurrentVelocityVector->VecData)[i];

            (OutputValues->NewVelocityVector->VecData)[i]
                =   InputValues.CurrentVelocityVector->VecData[i];
        }
        else
        {
            (OutputValues->NewPositionVector->VecData)[i]
                =   InputValues.CurrentPositionVector->VecData[i];

            (OutputValues->NewVelocityVector->VecData)[i]
                =   InputValues.CurrentVelocityVector->VecData[i];
        }

        OutputValues->ExecutionTimes->VecData[i]    =   0.0;

        OutputValues->PositionValuesAtTargetVelocity->VecData[i]
                =   InputValues.CurrentPositionVector->VecData[i];
    }

    this->SetPositionalExtremsToZero(OutputValues);

    OutputValues->TrajectoryIsPhaseSynchronized     =   false;
    OutputValues->SynchronizationTime               =   0.0;
    OutputValues->DOFWithTheGreatestExecutionTime   =   0;

    return;
}