75 lines
1.7 KiB
C++
75 lines
1.7 KiB
C++
#include "../config.h"
|
|
|
|
#include "trajectory.h"
|
|
|
|
Trajectory::Trajectory( )
|
|
{
|
|
registration = 0;
|
|
//accumulatedTime = 0.0f;
|
|
}
|
|
|
|
|
|
Trajectory::~Trajectory( )
|
|
{
|
|
for( auto& p : ControlPoints )
|
|
delete( p );
|
|
|
|
ControlPoints.clear();
|
|
}
|
|
|
|
|
|
void Trajectory::AddPoint( Vector2D *p )
|
|
{
|
|
ControlPoints.push_back( p );
|
|
}
|
|
|
|
|
|
void Trajectory::AddRegistry( void )
|
|
{
|
|
registration++;
|
|
}
|
|
|
|
void Trajectory::DeleteRegistry( void )
|
|
{
|
|
registration--;
|
|
}
|
|
|
|
void Trajectory::CalculatePosition( float *accumulatedTime, float time, uint16_t speed, bool looped, libnum::num *xreturn, libnum::num *yreturn )
|
|
{
|
|
*accumulatedTime += speed * time / 2000000.0f;
|
|
if (*accumulatedTime>ControlPoints.size()) *accumulatedTime-=ControlPoints.size();
|
|
if (*accumulatedTime<0) *accumulatedTime+=ControlPoints.size();
|
|
|
|
libnum::num t = libnum::num( *accumulatedTime - (int) *accumulatedTime );
|
|
|
|
int p0, p1, p2, p3;
|
|
|
|
if (!looped)
|
|
{
|
|
p1 = (int) *accumulatedTime + 1;
|
|
p2 = p1 + 1;
|
|
p3 = p2 + 1;
|
|
p0 = p1 - 1;
|
|
}
|
|
else
|
|
{
|
|
p1 = (int) *accumulatedTime;
|
|
p2 = (p1 + 1) % ControlPoints.size();
|
|
p3 = (p2 + 1) % ControlPoints.size();
|
|
p0 = p1 >= 1 ? p1 - 1 : ControlPoints.size() - 1;
|
|
}
|
|
|
|
libnum::num tt = t * t;
|
|
libnum::num ttt = tt * t;
|
|
|
|
libnum::num q1 = -ttt + 2*tt - t;
|
|
libnum::num q2 = 3*ttt - 5*tt + libnum::num( 2 );
|
|
libnum::num q3 = -3*ttt + 4*tt + t;
|
|
libnum::num q4 = ttt - tt;
|
|
|
|
libnum::num tx = libnum::num( 0.5f ) * (ControlPoints[p0]->x * q1 + ControlPoints[p1]->x * q2 + ControlPoints[p2]->x * q3 + ControlPoints[p3]->x * q4);
|
|
libnum::num ty = libnum::num( 0.5f ) * (ControlPoints[p0]->y * q1 + ControlPoints[p1]->y * q2 + ControlPoints[p2]->y * q3 + ControlPoints[p3]->y * q4);
|
|
|
|
*xreturn = tx;
|
|
*yreturn = ty;
|
|
} |