interpolate.cpp

Source code for cnc controlling three dimentioal machine

// interpolate.cpp: implementation of the interpolate class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "mastercontrol.h"
#include "interpolate.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

static Vect work[4];
static Vect ctlPts[] = { {0,0,0}, {0,0,0}, {0,0,0} , {0,0,0}};
static float *knots;
static float bezKts[] = {0, 0, 0, 1, 1, 1, BIG};
static float lagKts[] = {0.00, 0.25, 0.75, 1.00, BIG};
static float catKts[] = {-1, 0, 1, 2, BIG};
static float bspKts[] = {-2, -1, 0, 1, 2, 3, BIG};
static int m = MAXDIM;
static int n = 2;
static int Cn = 1;
static Bool interp = FALSE;
static Vect val = {0.0, 0.0, 0.0};	  // this is the result vector
static float t = 0;
static int eh = 0;
enum Flavor{PLY, LAG, BEZ, CAT, BSP, NFLAVORS};
char fnames[][4] = {"PLY", "LAG", "BEZ", "CAT", "BSP"};

interpolate::interpolate()
{
  flavor = CAT;
}

interpolate::~interpolate()
{

}

void interpolate::lerp(Knot t, Knot a0, Knot a1, Vect p0, Vect p1, int m, Vect p)
{
    register Knot t0=(a1-t)/(a1-a0), t1=1-t0;
    register int i;
    for (i=m-1; i>=0; i--) p[i] = t0*p0[i] + t1*p1[i];
}

int interpolate::DialASpline(Knot t, Knot a[], Vect p[], int m, int n, Vect work[],
                    int Cn, Bool interp, Vect val)
{
    register int i, j, k, h, lo, hi;	
    if (Cn>n-1) Cn = n-1;       /* Anything greater gives one polynomial */
    for (k=0; t> a[k]; k++);    /* Find enclosing knot interval */
    for (h=k; t==a[k]; k++);    /* May want to use fewer legs */
    if (k>n) {k = n; if (h>k) h = k;}
    h = 1+Cn - (k-h); k--;
    lo = k-Cn; hi = k+1+Cn;

    if (interp) {               /* Lagrange interpolation steps */
        int drop=0;
        if (lo<0) {lo = 0; drop += Cn-k;
                   if (hi-lo<Cn) {drop += Cn-hi; hi = Cn;}}
        if (hi>n) {hi = n; drop += k+1+Cn-n;
                   if (hi-lo<Cn) {drop += lo-(n-Cn); lo = n-Cn;}}
        for (i=lo; i<=hi; i++) V_Op(work[i],=,,p[i],m);
        for (j=1; j<=Cn; j++) {
            for (i=lo; i<=hi-j; i++) {
                lerp(t,a[i],a[i+j],work[i],work[i+1],m,work[i]);
            }
        }
        h = 1+Cn-drop;
    } else {                    /* Prepare for B-spline steps */
        if (lo<0) {h += lo; lo = 0;}
        for (i=lo; i<=lo+h; i++) V_Op(work[i],=,,p[i],m);
        if (h<0) h = 0;
    }
    for (j=0; j<h; j++) {
        int tmp = 1+Cn-j;
        for (i=h-1; i>=j; i--) {
            lerp(t,a[lo+i],a[lo+i+tmp],work[lo+i],work[lo+i+1],m,work[lo+i+1]);
        }
    }
    V_Op(val,=,,work[lo+h],m);
    return (k);
}

void interpolate::DoInterp( float t )
{
   
   switch (flavor) {
	  case PLY:   knots = lagKts; interp = TRUE;  Cn = 0; break;
	  case LAG:   knots = lagKts; interp = TRUE;  Cn = 2; break;
	  case BEZ:   knots = bezKts; interp = FALSE; Cn = 2; break;
	  case BSP:   knots = bspKts; interp = FALSE; Cn = 2; break;
	//case CAT:   knots = catKts; interp = TRUE;  Cn = 1; break;
          case CAT:   knots = catKts; interp = TRUE;  Cn = 2; break;
	//case CAT:   knots = catKts; interp = TRUE;  Cn = 3; break; same as Cn=2
          default:    knots = bspKts; interp = FALSE; Cn = 0; break;
        }

	 eh = DialASpline(t, knots, ctlPts, m, n, work, Cn, interp, val);
   	 newx = (double) val[0];
	 newy = (double) val[1];
	 newz = (double) val[2];

   
}

void interpolate::SetInterp(  int pos, double x, double y, double z)
{
	float x1,y1,z1;
 
	x1 = (float) x;
	y1 = (float) y;
	z1 = (float) z;
	ctlPts[pos][0] = x1;
	ctlPts[pos][1] = y1;
	ctlPts[pos][2] = z1;

}