TypeIIRMLStep1IntermediateProfiles.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 <TypeIIRMLStep1IntermediateProfiles.h>


using namespace TypeIIRMLMath;


//****************************************************************************
// NegateStep1()

void TypeIIRMLMath::NegateStep1(        double          *ThisCurrentPosition
                                    ,   double          *ThisCurrentVelocity
                                    ,   double          *ThisTargetPosition
                                    ,   double          *ThisTargetVelocity)
{
    *ThisCurrentPosition    =   -(*ThisCurrentPosition  )   ;
    *ThisCurrentVelocity    =   -(*ThisCurrentVelocity  )   ;
    *ThisTargetPosition     =   -(*ThisTargetPosition   )   ;
    *ThisTargetVelocity     =   -(*ThisTargetVelocity   )   ;

    return;
}


//****************************************************************************
// VToVMaxStep1()

void TypeIIRMLMath::VToVMaxStep1(       double          *TotalTime
                                    ,   double          *ThisCurrentPosition
                                    ,   double          *ThisCurrentVelocity
                                    ,   const double    &MaxVelocity
                                    ,   const double    &MaxAcceleration)
{
    double  TimeForCurrentStep  =   (*ThisCurrentVelocity - MaxVelocity)
                                    / MaxAcceleration;

    *TotalTime              +=  TimeForCurrentStep;
    *ThisCurrentPosition    +=  0.5 * (*ThisCurrentVelocity + MaxVelocity)
                                * TimeForCurrentStep;
    *ThisCurrentVelocity    =   MaxVelocity;

    return;
}


//****************************************************************************
// VToZeroStep1()

void TypeIIRMLMath::VToZeroStep1(       double          *TotalTime
                                    ,   double          *ThisCurrentPosition
                                    ,   double          *ThisCurrentVelocity
                                    ,   const double    &MaxAcceleration)
{
    double  TimeForCurrentStep  =   *ThisCurrentVelocity / MaxAcceleration;

    *TotalTime              +=  TimeForCurrentStep;
    *ThisCurrentPosition    +=  0.5 * (*ThisCurrentVelocity) * TimeForCurrentStep;
    *ThisCurrentVelocity    =   0.0;

    return;
}