Diff: BezierCurve.cpp

Revision:

16:f9ea2b2d410f

Child:

17:1bb5aa45826e

diff -r 495333b2ccf1 -r f9ea2b2d410f BezierCurve.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BezierCurve.cpp	Thu Sep 24 20:16:05 2020 +0000
@@ -0,0 +1,82 @@
+#include "BezierCurve.h"
+#include "math.h"
+#include "mbed.h"
+
+extern Serial pc;
+int factorial(int k) {
+    int f = 1;
+    for (int j = 2 ; j<= k ; j++)
+        f*=j; 
+    return f;   
+}
+
+BezierCurve::BezierCurve(int dim, int order) :_dim(dim), _order(order) {
+    _pts = new float*[_order+1];
+    _nck = new int[_order+1];
+    _nck_deriv = new int[_order];
+    int facn = factorial(_order);
+    int facn2= factorial(_order-1);
+    for(int i = 0 ; i <= _order ; i++) {
+        _pts[i] = new float[_dim];
+        _nck[i] = facn / factorial(i) / factorial(_order-i);
+        if (i< _order) {
+            _nck_deriv[i] = facn2 / factorial(i) / factorial(_order-1-i);
+        }
+    }
+}
+
+BezierCurve::~BezierCurve() {
+    for(int i = 0 ; i < _dim ; i++) {
+        delete _pts[i];    
+    }    
+    delete _pts;
+}
+
+void BezierCurve::setPoints(float pts[] ) {
+    pc.printf("Setting Points\n");
+    float * p = pts;
+    for(int i = 0 ; i<=_order ; i++) {
+        pc.printf("\tPt. %d:",i);
+        for( int j = 0 ; j < _dim ; j++) {
+            _pts[i][j] = *p;
+            p++;
+            pc.printf("\t\t%f\n",_pts[i][j]);
+        }    
+    }
+}
+
+void BezierCurve::evaluate(float time, float point[]) {
+    //float *_point = new float[_dim];
+    
+    for(int i=0; i< _dim ; i++) {
+        point[i] = 0;
+    }        
+    for(int i=0; i<=_order ; i++) {
+        float mi = pow(time,i)*pow(1-time,_order-i) * _nck[i];
+        for(int j=0 ; j < _dim ; j++) {
+            point[j] += _pts[i][j] * mi;
+        }    
+    }
+    //for(int i=0; i< _dim ; i++) {
+    //    point[i] = _point[i];
+    //} 
+    //delete _point;
+}
+
+void BezierCurve::evaluateDerivative(float time, float point[]) {
+    //double *_point = new double[_dim];
+    for(int i=0; i< _dim ; i++) {
+        point[i] = 0;
+    }    
+    //double dtime = time;    
+    for(int i=0; i<=_order-1 ; i++) {
+        float mi = pow(time,i)*pow(1-time,_order-1-i) * _nck_deriv[i] * _order;
+        for(int j=0 ; j < _dim ; j++) {
+            point[j] += (_pts[i+1][j] - _pts[i][j] ) * mi;
+        }    
+    }
+    //for(int i=0; i< _dim ; i++) {
+    //    point[i] = _point[i];
+    //} 
+    //delete _point;
+}
\ No newline at end of file