Sorfware for Lexy ( Hexapode )

Dependencies:   mbed BLE_API X_NUCLEO_IDB0XA1 MODSERIAL

LIB/Bezier.cpp

Committer:
Essenceia
Date:
2016-08-20
Revision:
5:58acbceb4f9e
Parent:
1:8bab9152933e

File content as of revision 5:58acbceb4f9e:

#include "Bezier.h"

Bezier::Bezier()
{;}
Bezier::~Bezier()
{
    ;
    }
void Bezier::add_point(float x,float y)
{
        xpos.push_back(x);
        ypos.push_back(y);
    }
    
void Bezier::get_pos(float t, float *xret, float *yret)
{
        int ii, ij, npoints;
        float *x, *y;

        npoints = xpos.size();
        x = new float [npoints];
        y = new float [npoints];
        // load with current points
        for (ii=0; ii<npoints; ii++)
        {
            x[ii] = xpos[ii];
            y[ii] = ypos[ii];
        }

        // iterate over levels
        for (ii=0; ii<npoints-1; ii++)
        {
            for (ij=0; ij<npoints-ii-1; ij++)
            {
                x[ij] = (1.0-t)*x[ij] + t*x[ij+1];
                y[ij] = (1.0-t)*y[ij] + t*y[ij+1];
            }
        }

        *xret = x[0];
        *yret = y[0];

        delete [] x;
        delete [] y;
    }