BlazeItFgt2/cutcode/Spline.HC

#ifndef SPLINE_HC
#define SPLINE_HC

#include "Vec"

/*
// Bug
CD2 curveTmp[curveTmpLen] = {
{150,150},
{175,175},
{200,200},
{225,225},
{400,400},
{400,200},
};
*/

//public U0 SplineGetPos(CD2I32 *v, F64 t, CD2 *res) {
public U0 SplineGetPos(CD2 *v, F64 t, CD2 *res) {
  I64 p0 = t;
  I64 p1 = p0 + 1;
  I64 p2 = p1 + 1;
  I64 p3 = p2 + 1;
  t = t - p0;
  F64 tt = t * t;
  F64 ttt = tt * t;
  F64 q1 = -ttt + 2.0*tt - t;
  F64 q2 = 3.0*ttt - 5.0*tt + 2.0;
  F64 q3 = -3.0*ttt + 4.0*tt + t;
  F64 q4 = ttt - tt;
  res->x = 0.5 * (v[p0].x * q1 + v[p1].x * q2 + v[p2].x * q3 + v[p3].x * q4);
  res->y = 0.5 * (v[p0].y * q1 + v[p1].y * q2 + v[p2].y * q3 + v[p3].y * q4);
}

//public U0 SplineGetGradient(CD2I32 *v, F64 t, CD2 *res) {
public U0 SplineGetGradient(CD2 *v, F64 t, CD2 *res) {
  I64 p0 = t;
  I64 p1 = p0 + 1;
  I64 p2 = p1 + 1;
  I64 p3 = p2 + 1;
  t = t - p0;
  F64 tt = t * t;
  F64 q1 = -3.0*tt + 4.0*t - 1.0;
  F64 q2 = 9.0*tt - 10.0*t;
  F64 q3 = -9.0*tt + 8.0*t + 1.0;
  F64 q4 = 3.0*tt - 2.0*t;
  res->x = 0.5 * (v[p0].x * q1 + v[p1].x * q2 + v[p2].x * q3 + v[p3].x * q4);
  res->y = 0.5 * (v[p0].y * q1 + v[p1].y * q2 + v[p2].y * q3 + v[p3].y * q4);
}

//F64 GetSplineLen(CD2I32 *v, I64 node) {
F64 GetSplineLen(CD2 *v, I64 node) {
  F64 len = 0;
  CD2 p0, p1;
  SplineGetPos(v, node, &p0);
  F64 t;
  for (t=0; t<1.0; t += 0.005) {
    SplineGetPos(v, node+t, &p1);
    len += Sqrt((p1.x - p0.x)*(p1.x - p0.x) + (p1.y - p0.y)*(p1.y - p0.y));
    p0.x = p1.x;
    p0.y = p1.y;
  }
  return len;
}

//F64 SplineUpdateLen(CD2I32 *v, I64 len, F64 *res) {
F64 SplineUpdateLen(CD2 *v, I64 len, F64 *res) {
  I64 i;
  F64 total = 0;
  for (i=0; i<len; i++) {
    F64 slen = GetSplineLen(v, i);
    res[i] = slen;
    total += slen;
  }
  return total;
}

F64 SplineGetNormalized(F64 *v, F64 p) {
  I64 i=0;
  while (p > v[i]) {
    p -= v[i];
    i++;
  }
  return i + p/v[i];
}

F64 GetT(F64 t, CD2 *p0, CD2 *p1) {
  CD2 d;
  d.x = p1->x - p0->x;
  d.y = p1->y - p0->y;
  F64 dot = d.x * d.x + d.y * d.y;
  return Pow(dot, 0.5) + t;
}

public U0 SplineGetPos2(CD2 *v, F64 t, CD2 *res) {
  I64 p0 = t;
  I64 p1 = p0 + 1;
  I64 p2 = p1 + 1;
  I64 p3 = p2 + 1;
  t = t - p0;

  F64 t0 = 0;
  F64 t1 = GetT(t0, &v[p0], &v[p1]);
  F64 t2 = GetT(t1, &v[p1], &v[p2]);
  F64 t3 = GetT(t2, &v[p2], &v[p3]);

  // precise lerp
  t = (1-t) * t1 + t * t2;

  CD2 a1, a2, a3, b1, b2;
  F64 w0 = (t1-t)/(t1-t0);
  F64 w1 = (t-t0)/(t1-t0);
  a1.x = w0*v[p0].x + w1*v[p1].x;
  a1.y = w0*v[p0].y + w1*v[p1].y;

  w0 = (t2-t)/(t2-t1);
  w1 = (t-t1)/(t2-t1);
  a2.x = w0*v[p1].x + w1*v[p2].x;
  a2.y = w0*v[p1].y + w1*v[p2].y;

  w0 = (t3-t)/(t3-t2);
  w1 = (t-t2)/(t3-t2);
  a3.x = w0*v[p2].x + w1*v[p3].x;
  a3.y = w0*v[p2].y + w1*v[p3].y;

  w0 = (t2-t)/(t2-t0);
  w1 = (t-t0)/(t2-t0);
  b1.x = w0*a1.x + w1*a2.x;
  b1.y = w0*a1.y + w1*a2.y;

  w0 = (t3-t)/(t3-t1);
  w1 = (t-t1)/(t3-t1);
  b2.x = w0*a2.x + w1*a3.x;
  b2.y = w0*a2.y + w1*a3.y;

  w0 = (t2-t)/(t2-t1);
  w1 = (t-t1)/(t2-t1);
  res->x = w0*b1.x + w1*b2.x;
  res->y = w0*b1.y + w1*b2.y;
}

public U0 SplineGetGradient2(CD2 *v, F64 t, CD2 *res) {
  I64 p0 = t;
  I64 p1 = p0 + 1;
  I64 p2 = p1 + 1;
  I64 p3 = p2 + 1;
  t = t - p0;

  F64 t0 = 0;
  F64 t1 = GetT(t0, &v[p0], &v[p1]);
  F64 t2 = GetT(t1, &v[p1], &v[p2]);
  F64 t3 = GetT(t2, &v[p2], &v[p3]);

  // precise lerp
  t = (1-t) * t1 + t * t2;
  //t = t1 + t * (t2 - t1);

  // TODO inline, optimize
  CD2 a1, a2, a3, b1, b2;
  a1.x = (v[p1].x-v[p0].x)/(t1-t0);
  a1.y = (v[p1].y-v[p0].y)/(t1-t0);

  a2.x = (v[p2].x-v[p1].x)/(t2-t1);
  a2.y = (v[p2].y-v[p1].y)/(t2-t1);

  a3.x = (v[p3].x-v[p2].x)/(t3-t2);
  a3.y = (v[p3].y-v[p2].y)/(t3-t2);

  F64 w0 = (t2-t)/(t2-t0);
  F64 w1 = (t-t0)/(t2-t0);
  b1.x = (a2.x-a1.x)/(t2-t0) + w0*a1.x + w1*a2.x;
  b1.y = (a2.y-a1.y)/(t2-t0) + w0*a1.y + w1*a2.y;

  w0 = (t3-t)/(t3-t1);
  w1 = (t-t1)/(t3-t1);
  b2.x = (a3.x-a2.x)/(t3-t1) + w0*a2.x + w1*a3.x;
  b2.y = (a3.y-a2.y)/(t3-t1) + w0*a2.y + w1*a3.y;

  w0 = (t2-t)/(t2-t1);
  w1 = (t-t1)/(t2-t1);
  res->x = (b2.x-b1.x)/(t2-t1) + w0*b1.x + w1*b2.x;
  res->y = (b2.y-b1.y)/(t2-t1) + w0*b1.y + w1*b2.y;
}

F64 GetSplineLen2(CD2 *v, I64 node) {
  F64 len = 0;
  CD2 p0, p1;
  SplineGetPos2(v, node, &p0);
  F64 t;
  for (t=0; t<1.0; t += 0.005) {
    SplineGetPos2(v, node+t, &p1);
    len += Sqrt((p1.x - p0.x)*(p1.x - p0.x) + (p1.y - p0.y)*(p1.y - p0.y));
    p0.x = p1.x;
    p0.y = p1.y;
  }
  return len;
}

F64 SplineUpdateLen2(CD2 *v, I64 len, F64 *res) {
  I64 i;
  F64 total = 0;
  for (i=0; i<len; i++) {
    F64 slen = GetSplineLen2(v, i);
    res[i] = slen;
    total += slen;
  }
  return total;
}

class MoveSpline {
  I64 cnt;
  F64 len;
  CD2 *path;
  F64 *pathLens;
};

U0 InitMoveSpline(MoveSpline *dst, I64 cnt) {
  dst->cnt = cnt;
  dst->len = 0;
  dst->path = MAlloc(cnt*sizeof(CD2));
  dst->pathLens = MAlloc(cnt*sizeof(F64));
}

U0 DrawMoveSpline(CDC *dc, MoveSpline *v) {
  CD2 res;
  F64 fi;
  dc->color = WHITE;
  for (fi=0; fi<v->cnt-3; fi += 0.005) {
    SplineGetPos(v->path, fi, &res);
    GrPlot1(dc, res.x, res.y);
  }
  I64 i;
  dc->color = RED;
  for (i=0; i<v->cnt; i++) {
    GrCircle(dc, v->path[i].x, v->path[i].y, 16);
  }
}

U0 DrawMoveSpline2(CDC *dc, MoveSpline *v) {
  CD2 res;
  F64 fi;
  dc->color = YELLOW;
  for (fi=0; fi<v->cnt-3; fi += 0.005) {
    SplineGetPos2(v->path, fi, &res);
    GrPlot1(dc, res.x, res.y);
  }
  I64 i;
  dc->color = PURPLE;
  for (i=0; i<v->cnt; i++) {
    GrCircle(dc, v->path[i].x, v->path[i].y, 10);
  }
}

U0 DrawSplineAgent(CDC *dc, MoveSpline *v, F64 t) {
  t = t%v->len;
  F64 tn = SplineGetNormalized(v->pathLens, t);
  CD2 p1, g1;
  SplineGetPos2(v->path, tn, &p1);
  SplineGetGradient2(v->path, tn, &g1);
  GrLine(dc, p1.x, p1.y, p1.x+g1.x*64, p1.y+g1.y*64);
  F64 sR = Arg(g1.x, -g1.y);
  dc->color = BLUE;
  GrLine(dc, 5.0*Sin(sR)+p1.x, 5.0*Cos(sR)+p1.y, -5.0*Sin(sR)+p1.x, -5.0*Cos(sR)+p1.y);
  dc->color = YELLOW;
  GrCircle(dc, p1.x, p1.y, 16);
}

#endif