61 lines
1.4 KiB
C++
61 lines
1.4 KiB
C++
#include "trajectory.h"
|
|
|
|
Trajectory::Trajectory( )
|
|
{
|
|
accumulatedTime = 0.0f;
|
|
}
|
|
|
|
|
|
Trajectory::~Trajectory( )
|
|
{
|
|
for( auto& p : ControlPoints )
|
|
delete( p );
|
|
|
|
ControlPoints.clear();
|
|
}
|
|
|
|
|
|
void Trajectory::AddPoint( Point2D *p )
|
|
{
|
|
ControlPoints.push_back( p );
|
|
}
|
|
|
|
|
|
void Trajectory::CalculatePosition( float time, uint16_t speed, bool looped, int *xreturn, int *yreturn )
|
|
{
|
|
accumulatedTime += speed * time / 2000000.0f;
|
|
if (accumulatedTime>ControlPoints.size()) 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 = (int) tx;
|
|
*yreturn = (int) ty;
|
|
} |