TypeIIRMLStep2.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 <TypeIIRMLDecisionTree2.h>
#include <TypeIIRMLStep2WithoutSynchronization.h>
#include <RMLPositionInputParameters.h>
#include <ReflexxesAPI.h>


using namespace TypeIIRMLMath;


//*******************************************************************************************
// Step2

void TypeIIRMLPosition::Step2(void)
{
    unsigned int        i                   =   0;

    if (this->CurrentTrajectoryIsPhaseSynchronized)
    {
        // As we only calculate the trajectory for one DOF, we do not use multiple threads for the
        // Step 2 calculation of the trajectory.

        this->Step2PhaseSynchronization();
    }
    else
    {
        if (this->CurrentTrajectoryIsNotSynchronized)
        {
            for (i = 0; i < this->NumberOfDOFs; i++)
            {
                if((this->ModifiedSelectionVector->VecData)[i])
                {
                    Step2WithoutSynchronization(        (this->CurrentInputParameters->CurrentPositionVector->VecData)      [i]
                                                    ,   (this->CurrentInputParameters->CurrentVelocityVector->VecData)      [i]
                                                    ,   (this->CurrentInputParameters->TargetPositionVector->VecData)       [i]
                                                    ,   (this->CurrentInputParameters->TargetVelocityVector->VecData)       [i]
                                                    ,   (this->CurrentInputParameters->MaxVelocityVector->VecData)          [i]
                                                    ,   (this->CurrentInputParameters->MaxAccelerationVector->VecData)      [i]
                                                    ,   (this->UsedStep1AProfiles->VecData)                                 [i]
                                                    ,   (this->MinimumExecutionTimes->VecData)                              [i]
                                                    ,   &((this->Polynomials)[i])   );
                }
            }
        }
        else
        {
            for (i = 0; i < this->NumberOfDOFs; i++)
            {
                if((this->ModifiedSelectionVector->VecData)[i])
                {
                    TypeIIRMLDecisionTree2(     (this->CurrentInputParameters->CurrentPositionVector->VecData)      [i]
                                            ,   (this->CurrentInputParameters->CurrentVelocityVector->VecData)      [i]
                                            ,   (this->CurrentInputParameters->TargetPositionVector->VecData)       [i]
                                            ,   (this->CurrentInputParameters->TargetVelocityVector->VecData)       [i]
                                            ,   (this->CurrentInputParameters->MaxVelocityVector->VecData)          [i]
                                            ,   (this->CurrentInputParameters->MaxAccelerationVector->VecData)      [i]
                                            ,   this->SynchronizationTime
                                            ,   &((this->Polynomials)[i])   );
                }
            }
        }
    }

    return;
}