BezierCurve.cpp

00001 #include "BezierCurve.h"
00002 #include "math.h"
00003 #include "mbed.h"
00004 
00005 extern Serial pc;
00006 int factorial(int k) {
00007     int f = 1;
00008     for (int j = 2 ; j<= k ; j++)
00009         f*=j; 
00010     return f;   
00011 }
00012 
00013 BezierCurve::BezierCurve(int dim, int order) :_dim(dim), _order(order) {
00014     _pts = new float*[_order+1];
00015     _nck = new int[_order+1];
00016     _nck_deriv = new int[_order];
00017     int facn = factorial(_order);
00018     int facn2= factorial(_order-1);
00019     for(int i = 0 ; i <= _order ; i++) {
00020         _pts[i] = new float[_dim];
00021         _nck[i] = facn / factorial(i) / factorial(_order-i);
00022         if (i< _order) {
00023             _nck_deriv[i] = facn2 / factorial(i) / factorial(_order-1-i);
00024         }
00025     }
00026 }
00027 
00028 BezierCurve::~BezierCurve() {
00029     for(int i = 0 ; i < _dim ; i++) {
00030         delete _pts[i];    
00031     }    
00032     delete _pts;
00033 }
00034 
00035 void BezierCurve::setPoints(float pts[] ) {
00036     pc.printf("Setting Points\n");
00037     float * p = pts;
00038     for(int i = 0 ; i<=_order ; i++) {
00039         pc.printf("\n\r\tPt. %d:",i);
00040         for( int j = 0 ; j < _dim ; j++) {
00041             _pts[i][j] = *p;
00042             p++;
00043             pc.printf("\t\t%f",_pts[i][j]);
00044         }    
00045     }
00046 }
00047 
00048 void BezierCurve::evaluate(float time, float point[]) {
00049     //float *_point = new float[_dim];
00050     
00051     for(int i=0; i< _dim ; i++) {
00052         point[i] = 0;
00053     }        
00054     for(int i=0; i<=_order ; i++) {
00055         float mi = pow(time,i)*pow(1-time,_order-i) * _nck[i];
00056         for(int j=0 ; j < _dim ; j++) {
00057             point[j] += _pts[i][j] * mi;
00058         }    
00059     }
00060     //for(int i=0; i< _dim ; i++) {
00061     //    point[i] = _point[i];
00062     //} 
00063     //delete _point;
00064 }
00065 
00066 void BezierCurve::evaluateDerivative(float time, float point[]) {
00067     //double *_point = new double[_dim];
00068     for(int i=0; i< _dim ; i++) {
00069         point[i] = 0;
00070     }    
00071     //double dtime = time;    
00072     for(int i=0; i<=_order-1 ; i++) {
00073         float mi = pow(time,i)*pow(1-time,_order-1-i) * _nck_deriv[i] * _order;
00074         for(int j=0 ; j < _dim ; j++) {
00075             point[j] += (_pts[i+1][j] - _pts[i][j] ) * mi;
00076         }    
00077     }
00078     //for(int i=0; i< _dim ; i++) {
00079     //    point[i] = _point[i];
00080     //} 
00081     //delete _point;
00082 }