home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 1997 March / VPR9703A.ISO / VPR_DATA / DOGA / SOURCES / MEDIT.LZH / BEZIER.CPP < prev    next >
C/C++ Source or Header  |  1995-09-26  |  11KB  |  471 lines
  1. #include <stdio.h>
  2. #include "matrix.h"
  3. #include "bezier.h"
  4.  
  5. #include "log.h"
  6.  
  7. static const int AllocUnit = 32;
  8. static const int BezierPoints = 64;
  9.  
  10. static const double minimumlength = 100.0;
  11.  
  12. class BezierPoint {
  13. public:
  14.     double (*bezierrate)[4];
  15.  
  16.     BezierPoint();
  17.     ~BezierPoint() {delete bezierrate;}
  18. };
  19.  
  20. BezierPoint::BezierPoint()
  21. {
  22.     bezierrate = new double[BezierPoints+1][4];
  23.     double t = 0.0;
  24.     for (int i = 0; i < BezierPoints+1; ++i) {
  25.         bezierrate[i][0] =   (1-t)*(1-t)*(1-t);
  26.         bezierrate[i][1] = 3*   t *(1-t)*(1-t);
  27.         bezierrate[i][2] = 3*   t *   t *(1-t);
  28.         bezierrate[i][3] =      t *   t *   t ;
  29.         t += (1.0 / BezierPoints);
  30.     }
  31. }
  32.  
  33.  
  34.  
  35. BezierPoint bp;
  36.  
  37. #if 0
  38. double bezierrate[17][4] = {
  39.     {4096.0 / 4096.0,    0.0 / 4096.0,    0.0 / 4096.0,    0.0 / 4096.0},
  40.     {3375.0 / 4096.0,  675.0 / 4096.0,   45.0 / 4096.0,    1.0 / 4096.0},
  41.     {2744.0 / 4096.0, 1176.0 / 4096.0,  168.0 / 4096.0,    8.0 / 4096.0},
  42.     {2197.0 / 4096.0, 1521.0 / 4096.0,  351.0 / 4096.0,   27.0 / 4096.0},
  43.     {1728.0 / 4096.0, 1728.0 / 4096.0,  576.0 / 4096.0,   64.0 / 4096.0},
  44.     {1331.0 / 4096.0, 1815.0 / 4096.0,  750.0 / 4096.0,  125.0 / 4096.0},
  45.     {1000.0 / 4096.0, 1800.0 / 4096.0, 1080.0 / 4096.0,  216.0 / 4096.0},
  46.     { 729.0 / 4096.0, 1701.0 / 4096.0, 1323.0 / 4096.0,  343.0 / 4096.0},
  47.     { 512.0 / 4096.0, 1536.0 / 4096.0, 1536.0 / 4096.0,  512.0 / 4096.0},
  48.     { 343.0 / 4096.0, 1323.0 / 4096.0, 1701.0 / 4096.0,  729.0 / 4096.0},
  49.     { 216.0 / 4096.0, 1080.0 / 4096.0, 1800.0 / 4096.0, 1000.0 / 4096.0},
  50.     { 125.0 / 4096.0,  750.0 / 4096.0, 1815.0 / 4096.0, 1331.0 / 4096.0},
  51.     {  64.0 / 4096.0,  576.0 / 4096.0, 1728.0 / 4096.0, 1728.0 / 4096.0},
  52.     {  27.0 / 4096.0,  351.0 / 4096.0, 1521.0 / 4096.0, 2197.0 / 4096.0},
  53.     {   8.0 / 4096.0,  168.0 / 4096.0, 1176.0 / 4096.0, 2744.0 / 4096.0},
  54.     {   1.0 / 4096.0,   45.0 / 4096.0,  675.0 / 4096.0, 3375.0 / 4096.0},
  55.     {   0.0 / 4096.0,    0.0 / 4096.0,    0.0 / 4096.0, 4096.0 / 4096.0},
  56. };
  57. #endif
  58.  
  59. Bezier::Bezier()
  60. {
  61.     points = -1;
  62.     allocpoints = AllocUnit;
  63.     point = new Vector[allocpoints*3+2];
  64.     length = new double[allocpoints];
  65.  
  66.        point[0] = Vector(0,0,0);
  67.     lengthvalid = FALSE;
  68. }
  69.  
  70. Bezier::Bezier(Bezier& b)
  71. {
  72.     memcpy(this, &b, sizeof(Bezier));
  73.     point = new Vector[allocpoints*3+2];
  74.     length = new double[allocpoints];
  75.     memcpy(point, b.point, sizeof(Vector) * (points*3+2));
  76.     memcpy(length, b.length, sizeof(double) * points);
  77. }
  78.  
  79. Bezier::Bezier(Vector &v1)
  80. {
  81.     points = 0;
  82.     allocpoints = AllocUnit;
  83.     point = new Vector[allocpoints*3+2];
  84.     length = new double[allocpoints];
  85.  
  86.     point[0] = point[1] = v1;
  87.  
  88.     lengthvalid = FALSE;
  89. }
  90.  
  91. Bezier::Bezier(Vector &v1, Vector &v2)
  92. {
  93.     points = 0;
  94.     allocpoints = AllocUnit;
  95.     point = new Vector[allocpoints*3+2];
  96.     length = new double[allocpoints];
  97.  
  98.     point[0] = v1;
  99.     point[1] = v2;
  100.  
  101.     lengthvalid = FALSE;
  102. }
  103.  
  104. Bezier::~Bezier()
  105. {
  106.     delete point;
  107.     delete length;
  108. }
  109.  
  110. Vector Bezier::GetPoint(double t)
  111. {
  112.     if (points == 0) {
  113.         return point[0];
  114.     } else if (t <= 0) {
  115.         return point[0];
  116.     } else if (t >= (double)points) {
  117.         return point[3 * points];
  118.     } else {
  119.         int begin = (int)t;
  120.         t -= begin;
  121.         Vector *p = point + 3 * begin;
  122.         Vector v = (  (1-t)*(1-t)*(1-t))*p[0]
  123.                  + (3*   t *(1-t)*(1-t))*p[1]
  124.                  + (3*   t *   t *(1-t))*p[2]
  125.                  + (     t *   t *   t )*p[3];
  126.         return v;
  127.     }
  128. }
  129.  
  130. Vector Bezier::GetVector(double t)
  131. {
  132.     if (points == 0) {
  133.         return Vector(0,0,0);
  134.     }
  135.     if (t <= 0) {
  136.         if (point[1] == point[0]) {
  137.             return 3.0 * (point[2] - point[0]);
  138.         } else {
  139.             return 3.0 * (point[1] - point[0]);
  140.         }
  141.     } else if (t >= (double)points) {
  142.         if (point[3*points] == point[3*points-1]) {
  143.             return 3.0 * (point[3*points] - point[3*points-2]);
  144.         } else {
  145.             return 3.0 * (point[3*points] - point[3*points-1]);
  146.         }
  147.     } else {
  148.         int begin = (int)t;
  149.         t -= begin;
  150.         Vector *p = point + 3 * begin;
  151.         if (t == 0.0 && p[0] == p[1]) {
  152.             return p[2] - p[0];
  153.         } else {
  154.             return (-3 * (1 -   t) * (1-t)) * p[0]
  155.                  + ( 3 * (1 - 3*t) * (1-t)) * p[1]
  156.                  + ( 3 * (2 - 3*t) *    t ) * p[2]
  157.                  + ( 3 *        t  *    t ) * p[3];
  158.         }
  159.     }
  160. }
  161.  
  162. double Bezier::GetRate(double l)
  163. {
  164.     if (l <= 0.0) {
  165.         return 0.0;
  166.     }
  167.     for (int i = 0; i < points; ++i) {
  168.         if (l < length[i]) {
  169.             break;
  170.         }
  171.         l -= length[i];
  172.     }
  173.     if (i == points) {
  174.         return (double)points;
  175.     }
  176.     Vector *p = point + i * 3;
  177.     for (int j = 0; j < BezierPoints; ++j) {
  178.         Vector v = (bp.bezierrate[j+1][0]-bp.bezierrate[j][0]) * p[0]
  179.                  + (bp.bezierrate[j+1][1]-bp.bezierrate[j][1]) * p[1]
  180.                  + (bp.bezierrate[j+1][2]-bp.bezierrate[j][2]) * p[2]
  181.                  + (bp.bezierrate[j+1][3]-bp.bezierrate[j][3]) * p[3];
  182.         double vl = v.length();
  183.         if (l < vl) {
  184.             return (double)i + ((double)j + l / vl) / (double)BezierPoints;
  185.         }
  186.         l -= vl;
  187.     }
  188.     return (double)(i+1);
  189. }
  190.  
  191. double Bezier::TotalLength(void)
  192. {
  193.     if (points <= 0) {
  194.         return 0.0;
  195.     }
  196.     if (lengthvalid == FALSE) {
  197.         UpdateLength();
  198.     }
  199.     double l = 0;
  200.     for (int i = 0; i < points; ++i) {
  201.         l += length[i];
  202.     }
  203.     return l;
  204. }
  205.  
  206. void Bezier::UpdateLength(int i)
  207. {
  208.     Vector *p = point + 3 * i;
  209.     double l = 0;
  210.     for (int j = 0; j < BezierPoints; ++j) {
  211.         Vector v = (bp.bezierrate[j+1][0]-bp.bezierrate[j][0]) * p[0]
  212.                  + (bp.bezierrate[j+1][1]-bp.bezierrate[j][1]) * p[1]
  213.                  + (bp.bezierrate[j+1][2]-bp.bezierrate[j][2]) * p[2]
  214.                  + (bp.bezierrate[j+1][3]-bp.bezierrate[j][3]) * p[3];
  215.         l += v.length();
  216.     }
  217.     if (l < minimumlength) {
  218.         l = minimumlength;
  219.     }
  220.     length[i] = l;
  221. }
  222.  
  223. void Bezier::UpdateLength(void)
  224. {
  225.     for (int i = 0; i < points; ++i) {
  226.         UpdateLength(i);
  227.     }
  228.     lengthvalid = TRUE;
  229. }
  230.  
  231. void Bezier::AddPoint(Vector& v1, Vector& v2, Vector& v3)
  232. {
  233.     if (points < 0) {
  234.         point[0] = point[1] = v3;
  235.         points = 0;
  236.     } else {
  237.         AllocPoint(points+1);
  238.         point[points*3+1] = v1;
  239.         point[points*3+2] = v2;
  240.         point[points*3+3] = v3;
  241.         points++;
  242.     }
  243.     lengthvalid = FALSE;
  244. }
  245.  
  246. void Bezier::AddPoint(Vector& p1, Vector& p2)
  247. {
  248.     if (points < 0) {
  249.         point[0] = p1;
  250.         point[1] = p2;
  251.         points = 0;
  252.     } else {
  253.         AllocPoint(points+1);
  254.         point[points*3+2] = 2 * p1 - p2;
  255.         point[points*3+3] = p1;
  256.         point[points*3+4] = p2;
  257.         points++;
  258.     }
  259.     lengthvalid = FALSE;
  260. }
  261.  
  262. void Bezier::MovePoint(int pos, Vector& p, BezierControl flag)
  263. {
  264.     if (pos < 0 || pos > points * 3 + 1) {
  265.         return;
  266.     }
  267.     if (flag == BCdep) {
  268.         if (pos == 0) {
  269.             point[1] += p - point[0];
  270.         } else if (pos > 1) {
  271.             Vector v;
  272.             double l1, l2;
  273.             switch (pos % 3) {
  274.             case 0:
  275.                 point[pos-1] += p - point[pos];
  276.                 point[pos+1] += p - point[pos];
  277.                 break;
  278.             case 1:
  279.                 v = p - point[pos-1];
  280.                 l1 = (point[pos-1] - point[pos-2]).length();
  281.                 l2 = v.length();
  282.                 if (l2 > 0.0) {
  283.                     point[pos-2] = point[pos-1] - (l1/l2) * v;
  284.                 }
  285.                 break;
  286.             case 2:
  287.                 v = p - point[pos+1];
  288.                 l1 = (point[pos+1] - point[pos+2]).length();
  289.                 l2 = v.length();
  290.                 if (l2 > 0.0) {
  291.                     point[pos+2] = point[pos+1] - (l1/l2) * v;
  292.                 }
  293.                 break;
  294.             }
  295.         }
  296.     } else if (flag == BCsame) {
  297.         if (pos == 0) {
  298.             point[1] += p - point[0];
  299.         } else if (pos > 1) {
  300.             switch (pos % 3) {
  301.             case 0:
  302.                 point[pos-1] += p - point[pos];
  303.                 point[pos+1] += p - point[pos];
  304.                 break;
  305.             case 1:
  306.                 point[pos-2] = 2.0 * point[pos-1] - p;
  307.                 break;
  308.             case 2:
  309.                 point[pos+2] = 2.0 * point[pos+1] - p;
  310.                 break;
  311.             }
  312.         }
  313.     }
  314.     point[pos] = p;
  315.     lengthvalid = FALSE;
  316. }
  317.  
  318. void Bezier::InsertPoint(int pos)
  319. {
  320.     if (pos < 0 || pos >= points) {
  321.         return;
  322.     }
  323.     AllocPoint(points+1);
  324.     for (int i = points*3+1; i >= pos*3+2; --i) {
  325.         point[i+3] = point[i];
  326.     }
  327.     for (i = points; i > pos; --i) {
  328.         length[i] = length[i-1];
  329.     }
  330.     points++;
  331.     i = pos*3;
  332. //    Vector& v0 = point[i];
  333. //    Vector& v3 = point[i+6];
  334.     Vector v1 = point[i+1];
  335.     Vector v2 = point[i+2];
  336.  
  337. //    point[i+1] = 0.5 * (v0 + v1);
  338. //    point[i+5] = 0.5 * (v2 + v3);
  339.     Vector v12 = 0.5 * (v1 + v2);
  340.     point[i+2] = 0.5 * (point[i+1] + v12);
  341.     point[i+4] = 0.5 * (v12 + point[i+5]);
  342.     point[i+3] = 0.5 * (point[i+2] + point[i+4]);
  343.  
  344.     length[pos] = length[pos+1] = length[pos]/2;
  345.     lengthvalid = FALSE;
  346. }
  347.  
  348. void Bezier::InsertPoint(int pos, Vector& v1, Vector& v2)
  349. {
  350.     if (pos < 0 || pos > points+1) {
  351.         return;
  352.     }
  353.     AllocPoint(points+1);
  354.  
  355.     for (int i = points*3+1; i >= pos*3-1 && i >= 0; --i) {
  356.         point[i+3] = point[i];
  357.     }
  358.     for (i = points; i > pos; --i) {
  359.         length[i] = length[i-1];
  360.     }
  361.     points++;
  362.     i = pos*3;
  363.     if (pos == 0) {
  364.         point[i  ] = v1;
  365.         point[i+1] = v2;
  366.         point[i+2] = 2.0 * point[i+3] - point[i+4];
  367.         length[0] = 0.0;
  368.     } else {
  369.         point[i-1] = 2.0 * v1 - v2;
  370.         point[i  ] = v1;
  371.         point[i+1] = v2;
  372.         double l0 = length[pos-1];
  373.         UpdateLength(pos-1);
  374.         UpdateLength(pos);
  375.         if (length[pos-1] == 0.0) {
  376.             length[pos] = l0;
  377.         } else if (length[pos] == 0.0) {
  378.             length[pos-1] = l0;
  379.         } else {
  380.             double l1 = length[pos-1] + length[pos];
  381.             length[pos-1] *= l0/l1;
  382.             length[pos  ] *= l0/l1;
  383.             }
  384.     }
  385.     lengthvalid = FALSE;
  386. }
  387.  
  388. void Bezier::DeletePoint(int pos)
  389. {
  390.     if (pos < 0 || pos > points) {
  391.         return;
  392.     }
  393.     int i;
  394.     if (pos == 0) {
  395.         for (i = 0; i < points*3-1; ++i) {
  396.             point[i] = point[i+3];
  397.         }
  398.     } else {
  399.         length[pos-1] += length[pos];
  400.         if (pos < points){
  401.             for (i = pos*3-1; i < points*3-1; ++i) {
  402.                 point[i] = point[i+3];
  403.             }
  404.         }
  405.     }
  406.     for (i = pos; i < points; ++i) {
  407.         length[i] = length[i+1];
  408.     }
  409.     points--;
  410.  
  411.     lengthvalid = FALSE;
  412. }
  413.  
  414. void Bezier::AllocPoint(int num)
  415. {
  416.     if (allocpoints < num) {
  417.         for (int nalloc = allocpoints; nalloc < num; nalloc += AllocUnit)
  418.             ;
  419.         Vector *v = new Vector[nalloc*3+2];
  420.         double *d = new double[nalloc];
  421.         int i;
  422.         for (i = 0; i < allocpoints * 3 + 2; ++i) {
  423.             v[i] = point[i];
  424.         }
  425. //        for (i = 0; i < allocpoints; ++i) {
  426. //            d[i] = length[i];
  427. //        }
  428.         allocpoints = nalloc;
  429.         delete[] point;
  430.         delete[] length;
  431.         point = v;
  432.         length = d;
  433.         lengthvalid = FALSE;
  434.     }
  435. }
  436.  
  437. void Bezier::AddSpline(Vector* p, int num)
  438. {
  439.     AllocPoint(points+num);
  440.  
  441.     for (int i = 0; i < num; ++i) {
  442.         AddPoint(p[i], p[i]);
  443.     }
  444. #if 0
  445.     Vector d[num], z[num];
  446.     double dd[num];
  447.     if (points > 0) {
  448.         z[0] = point[3*points-2] - 2 * point[3*points-1] + point[3*points];
  449.     } else {
  450.         z[0] = Vector(0,0,0);
  451.     }
  452.     z[num-1] = Vector(0,0,0);
  453.     int i;
  454.     for (i = 0; i < num-1; i++) {
  455.         d[i+1] = p[i+1] - p[i];
  456.     }
  457.     z[1] = d[2] - d[1] - z[0];
  458.     dd[1] = 4;
  459.     for (i = 1; i < num-2; i++) {
  460.         double t = 1.0 / dd[i];
  461.         z[i+1] = d[i+2] - d[i+1] - t * z[i];
  462.         dd[i+1] = 4 - t;
  463.     }
  464.     z[num-2] -= z[num-1];
  465.     for (i = num-2; i > 0; i--) {
  466.         z[i] = (z[i] - z[i+1]) / dd[i];
  467.     }
  468. #endif
  469. }
  470.  
  471.