TypeIIRMLStep3.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 <TypeIIRMLPosition.h>
#include <TypeIIRMLMath.h>
#include <RMLPositionInputParameters.h>
#include <RMLPositionOutputParameters.h>
#include <ReflexxesAPI.h>


//*******************************************************************************************
// Step3

int TypeIIRMLPosition::Step3(       const double                    &TimeValueInSeconds
                                ,   RMLPositionOutputParameters     *OP                 ) const
{
    unsigned int            i                       =   0;

    int                     j                       =   0
                        ,   ReturnValueForThisMethod = ReflexxesAPI::RML_FINAL_STATE_REACHED;

    for (i = 0; i < this->NumberOfDOFs; i++)
    {
        if ( (this->ModifiedSelectionVector->VecData)[i] )
        {
            j = 0;

            while ( (TimeValueInSeconds > (this->Polynomials)[i].PolynomialTimes[j]) && (j < MAXIMAL_NO_OF_POLYNOMIALS - 1))
            {
                j++;
            }

            (OP->NewPositionVector->VecData)    [i]
                =   (this->Polynomials)[i].PositionPolynomial[j].CalculateValue(TimeValueInSeconds);
            (OP->NewVelocityVector->VecData)    [i]
                =   (this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(TimeValueInSeconds);
            (OP->NewAccelerationVector->VecData)[i]
                =   (this->Polynomials)[i].AccelerationPolynomial[j].CalculateValue(TimeValueInSeconds);

            if ( j < ((this->Polynomials)[i].ValidPolynomials) - 1)
            {
                ReturnValueForThisMethod = ReflexxesAPI::RML_WORKING;
            }
        }
        else
        {
            (OP->NewPositionVector->VecData)    [i]
                =   (this->CurrentInputParameters->CurrentPositionVector->VecData)[i];
            (OP->NewVelocityVector->VecData)    [i]
                =   (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i];
            (OP->NewAccelerationVector->VecData)[i]
                =   (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[i];
        }
    }

    return(ReturnValueForThisMethod);
}