fit.cpp

Go to the documentation of this file.
00001 /*
00002  * fit.cpp
00003  *
00004  * Copyright (C) 2007  Thomas A. Vaughan
00005  * All rights reserved.
00006  *
00007  *
00008  * Redistribution and use in source and binary forms, with or without
00009  * modification, are permitted provided that the following conditions are met:
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *     * Neither the name of the <organization> nor the
00016  *       names of its contributors may be used to endorse or promote products
00017  *       derived from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THOMAS A. VAUGHAN ''AS IS'' AND ANY
00020  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  * DISCLAIMED. IN NO EVENT SHALL THOMAS A. VAUGHAN BE LIABLE FOR ANY
00023  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *
00030  *
00031  * Implementation of fitting structure fit_t.  See fit.h
00032  */
00033 
00034 // includes --------------------------------------------------------------------
00035 #include "fit.h"                // always include our own header first!
00036 
00037 #include <math.h>
00038 
00039 #include "perf/perf.h"
00040 
00041 
00042 namespace bezier {
00043 
00044 typedef std::vector<point_t> vec_point_t;
00045 
00046 ////////////////////////////////////////////////////////////////////////////////
00047 //
00048 //      quadratic fitting
00049 //
00050 ////////////////////////////////////////////////////////////////////////////////
00051 
00052 static point_t
00053 getTangent
00054 (
00055 IN const point_t * p,
00056 IN int nPoints,
00057 IN int i
00058 )
00059 throw()
00060 {
00061         ASSERT(p, "null");
00062         ASSERT(nPoints > 1, "Bad array size: %d", nPoints);
00063         ASSERT(i >= 0 && i < nPoints, "Bad index: %d", i);
00064 
00065         // edge cases
00066         if (0 == i) {
00067                 return p[1] - p[0];
00068         } else if (nPoints - 1 == i) {
00069                 return p[nPoints - 1] - p[nPoints - 2];
00070         }
00071 
00072         // use average
00073         point_t delta = p[i + 1] - p[i - 1];
00074         return point_t(0.5 * delta.x, 0.5 * delta.y);
00075 }
00076 
00077 
00078 
00079 void
00080 getQuadPathFromRawPoints
00081 (
00082 IN const point_t * in_p,
00083 IN int nPoints,
00084 IN float ds,
00085 OUT quad_path_t& path
00086 )
00087 {
00088         perf::Timer timer("bezier::getQuadPathsFromRawPoints");
00089         ASSERT(in_p, "null");
00090         ASSERT(nPoints > 0, "Bad nPoints: %d", nPoints);
00091         ASSERT(ds > 0.0, "Bad line step increment: %f cm", ds);
00092         path.clear();
00093 
00094         if (nPoints < 2) {
00095                 // DPRINTF("Not enough points!  Ignoring...");
00096                 return;
00097         }
00098 
00099         // respace
00100 /*
00101         point_t * q = NULL;
00102         int newPoints = 0;
00103         respacePath(in_p, nPoints, ds, &q, newPoints);
00104         ASSERT(q, "null respaced points");
00105         ASSERT(newPoints > 1, "Should have at least 2 points: %d", newPoints);
00106 */
00107         const point_t * q = in_p;
00108         int newPoints = nPoints;
00109 
00110         // find any cusps
00111         int * cusps = NULL;
00112         int nCusps = getCuspIndices(q, newPoints, &cusps);
00113         ASSERT(cusps, "should have cusp array, even if empty");
00114         ASSERT(nCusps >= 1, "should have at least 1 cusp (end): %d", nCusps);
00115         // DPRINTF("nCusps = %d", nCusps);
00116 
00117         // now walk through set of respaced points and calculate quadratic fits
00118         int start = 0;
00119         int end;
00120         for (int c = 0; c < nCusps; ++c) {
00121                 end = cusps[c];
00122                 ASSERT(end > start && end < newPoints, "Bad end: %d", end);
00123                 // DPRINTF("Segment from %d to %d", start, end);
00124 
00125                 // starting tangent?
00126                 point_t p0 = q[start];
00127                 for (int i = start + 1; i <= end; ++i) {
00128                         // DPRINTF("  point %d...", i);
00129                         point_t p1 = q[i];
00130                         point_t t0 = getTangent(q, newPoints, i - 1);
00131                         quad_bezier_t qb;
00132                         qb.setFromPointsAndSlope(p0, p1, t0);
00133                         path.push_back(qb);
00134 
00135                         // set up for next quadratic
00136                         p0 = p1;
00137                 }
00138 
00139                 // next set
00140                 start = end;
00141         }
00142 
00143         // DPRINTF("Finished quad fitting, %d total beziers", path.size());
00144 }
00145 
00146 
00147 
00148 void
00149 getCubicPathFromRawPoints
00150 (
00151 IN const point_t * in_p,
00152 IN int nPoints,
00153 IN float ds,
00154 OUT cubic_path_t& path
00155 )
00156 {
00157         perf::Timer timer("bezier::getCubicPathsFromRawPoints");
00158         ASSERT(in_p, "null");
00159         ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
00160         ASSERT(ds > 0.0, "Bad line step increment: %f cm", ds);
00161         path.clear();
00162 
00163         // respace
00164         point_t * q = NULL;
00165         int newPoints = 0;
00166         respacePath(in_p, nPoints, ds, &q, newPoints);
00167         ASSERT(q, "null respaced points");
00168         ASSERT(newPoints > 1, "Should have at least 2 points: %d", newPoints);
00169 
00170         // find any cusps
00171         int * cusps = NULL;
00172         int nCusps = getCuspIndices(q, newPoints, &cusps);
00173         ASSERT(cusps, "should have cusp array, even if empty");
00174         ASSERT(nCusps >= 1, "should have at least 1 cusp (end): %d", nCusps);
00175         // DPRINTF("nCusps = %d", nCusps);
00176 
00177         // now walk through set of respaced points and calculate cubic fits
00178         int start = 0;
00179         int end;
00180         for (int c = 0; c < nCusps; ++c) {
00181                 end = cusps[c];
00182                 ASSERT(end > start && end < newPoints, "Bad end: %d", end);
00183                 // DPRINTF("Segment from %d to %d", start, end);
00184 
00185                 // starting tangent
00186                 point_t t0 = getTangent(q, newPoints, start);
00187                 point_t p0 = q[start];
00188                 for (int i = start + 1; i <= end; ++i) {
00189                         // DPRINTF("  point %d...", i);
00190                         point_t p1 = q[i];
00191                         point_t t1 = getTangent(q, newPoints, i);
00192                         control_points_t cp;
00193                         cp.setFromPointsAndTangents(p0, t0, p1, t1);
00194                         curve_t b;
00195                         getCurveFromControlPoints(cp, b);
00196                         path.push_back(b);
00197 
00198                         // set up for next cubic
00199                         p0 = p1;
00200                         t0 = t1;
00201                 }
00202 
00203                 // next set
00204                 start = end;
00205         }
00206 
00207         // DPRINTF("Finished cubic fitting, %d total beziers", path.size());
00208 }
00209 
00210 
00211 ////////////////////////////////////////////////////////////////////////////////
00212 //
00213 //      static helper methods
00214 //
00215 ////////////////////////////////////////////////////////////////////////////////
00216 
00217 static void
00218 getCubic
00219 (
00220 IN float x1,
00221 IN float x2,
00222 IN float x3,
00223 OUT float& a,
00224 OUT float& b,
00225 OUT float& c
00226 )
00227 throw()
00228 {
00229         // assumes a fit of the form
00230         //  x(t) = x0 + c t + b t^2 + a t ^3
00231         //
00232         // Furthermore, assumes x0 = 0 and x1, x2, x3 are relative to x0
00233         //
00234         // See 8 March 07 journal entry
00235 
00236         a = (9.0 / 2.0) * (3.0 * x1 - 3.0 * x2 + x3);
00237         b = (9.0 / 2.0) * (x2 - 2.0 * x1 - (2.0 / 9.0) * a);
00238         c = 3.0 * (x1 - (b / 9.0) - (a / 27.0));
00239 }
00240 
00241 
00242 
00243 static void
00244 leastSquaresFit
00245 (
00246 IN const point_t * p,   // array of input points
00247 IN int nPoints,         // size of array
00248 IN const point_t& q,    // initial slope/direction
00249 IN const point_t& r,    // ending slope/direction
00250 OUT control_points_t& cp // bezier control points
00251 )
00252 throw()
00253 {
00254         ASSERT(nPoints > 0, "Bad nPoints: %d", nPoints);
00255 
00256         if (1 == nPoints) {
00257                 // dot!
00258                 cp.p0 = cp.p1 = cp.p2 = cp.p3 = p[0];
00259                 return;
00260         } else if (2 == nPoints) {
00261                 // simple line!
00262                 curve_t c;
00263                 c.clear();
00264                 c.x0 = p[0].x;
00265                 c.y0 = p[0].y;
00266                 c.cx = p[1].x - p[0].x;
00267                 c.cy = p[1].y - p[0].y;
00268                 getControlPointsFromCurve(c, cp);
00269                 return;
00270         } else if (3 == nPoints) {
00271                 // simple quadratic
00272                 curve_t c;
00273                 c.clear();
00274                 c.x0 = p[0].x;
00275                 c.y0 = p[0].y;
00276                 c.bx = 2.0 * (p[2].x - p[0].x) - 4.0 * (p[1].x - p[0].x);
00277                 c.by = 2.0 * (p[2].y - p[0].y) - 4.0 * (p[1].y - p[0].y);
00278                 c.cx = p[2].x - p[0].x - c.bx;
00279                 c.cy = p[2].y - p[0].y - c.by;
00280                 getControlPointsFromCurve(c, cp);
00281                 return;
00282         } else if (4 == nPoints) {
00283                 // simple cubic (yuck)
00284                 curve_t c;
00285                 c.x0 = p[0].x;
00286                 c.y0 = p[0].y;
00287 
00288                 getCubic(p[1].x - p[0].x, p[2].x - p[0].x, p[3].x - p[0].x,
00289                     c.ax, c.bx, c.cx);
00290                 getCubic(p[1].y - p[0].y, p[2].y - p[0].y, p[3].y - p[0].y,
00291                     c.ay, c.by, c.cy);
00292                 getControlPointsFromCurve(c, cp);
00293                 return;
00294         }
00295 
00296         //
00297         // This is *complicated*  See my 6 march 07 entry.  Basically, we
00298         // have 8 variables (4 control points, 2 variables each).  We know
00299         // the start and end point (4 variables) and the initial and ending
00300         // slopes (2 variables).  That leaves 2 remaining variables, which
00301         // we compute via least-squares fitting.
00302         //
00303 
00304         // DPRINTF("leastSquaresFit(N = %d)", nPoints);
00305         // q.dump("q");
00306         // r.dump("r");
00307 
00308         float q2 = (q.x * q.x) + (q.y * q.y);
00309         float r2 = (r.x * r.x) + (r.y * r.y);
00310 
00311         float w, x, y, z, v;
00312         w = x = y = z = v = 0.0;        // summation variables
00313 
00314         // initial and final control points
00315         cp.p0 = p[0];
00316         cp.p3 = p[nPoints - 1];
00317 
00318         // assume all input points are evenly spaced (parameter-wise)
00319         float dt = 1.0 / (nPoints - 1);
00320         float t = dt;
00321         for (int i = 1; i < nPoints - 1; ++i, t += dt) {
00322                 float alpha = 1.0 - t;
00323                 float h = 3.0 * t * alpha * alpha;
00324                 float k = -3.0 * t * t * alpha;
00325 
00326                 point_t g;
00327                 g.x = alpha * alpha * alpha * cp.p0.x + h * cp.p0.x - k * cp.p3.x + t * t * t * cp.p3.x;
00328                 g.y = alpha * alpha * alpha * cp.p0.y + h * cp.p0.y - k * cp.p3.y + t * t * t * cp.p3.y;
00329 
00330                 point_t gp;     // g-p
00331                 gp.x = g.x - p[i].x;
00332                 gp.y = g.y - p[i].y;
00333 
00334                 w += h * (q.x * gp.x + q.y * gp.y);
00335                 x += h * h;
00336                 y += h * k;
00337                 v += k * k;
00338                 z += k * (r.x * gp.x + r.y * gp.y);
00339         }
00340 
00341         // scale any sums
00342         x *= q2;
00343         v *= r2;
00344         y *= (q.x * r.x + q.y * r.y);
00345 
00346         // DPRINTF("  w = %5.2f", w);
00347         // DPRINTF("  x = %5.2f", x);
00348         // DPRINTF("  y = %5.2f", y);
00349         // DPRINTF("  z = %5.2f", z);
00350         // DPRINTF("  v = %5.2f", v);
00351 
00352         // okay, now have our interesting scalars w, x, y, z, v
00353         float denom = v * x - y * y;
00354         ASSERT(denom, "null denominator--need to handle this!");
00355 
00356         float a = (y * z - v * w) / denom;
00357         float b = (w * y - z * x) / denom;
00358 
00359         // DPRINTF("a = %5.2f,   b = %5.2f", a, b);
00360 
00361         // construct middle control points
00362         cp.p1.x = cp.p0.x + a * q.x;
00363         cp.p1.y = cp.p0.y + a * q.y;
00364 
00365         cp.p2.x = cp.p3.x - b * r.x;
00366         cp.p2.y = cp.p3.y - b * r.y;
00367 
00368         // that's it!  p0, p1, p2, p3 are your best fit bezier control points
00369 }
00370 
00371 
00372 
00373 static void
00374 getQR
00375 (
00376 IN const point_t * p,
00377 IN int nPoints,
00378 OUT point_t& q,         // initial direction (slope)
00379 OUT point_t& r          // final direction (slope) -- actually the negative
00380 )
00381 throw()
00382 {
00383         ASSERT(p, "null");
00384         ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
00385 
00386         // q is initial direction
00387         q = p[1] - p[0];
00388 
00389         // r is final direction
00390         r = p[nPoints - 1] - p[nPoints - 2];
00391 }
00392 
00393 
00394 
00395 void
00396 addPathSegments
00397 (
00398 IN const point_t * p,
00399 IN const point_t& q,
00400 IN const point_t& r,
00401 IN int nPoints,
00402 IN float tolerance,
00403 IO path_t& path
00404 )
00405 {
00406         ASSERT(p, "null array");
00407         ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
00408         ASSERT(tolerance > 0.0, "Bad tolerance: %f", tolerance);
00409 
00410         //DPRINTF("  addPathSegments(n=%d)", nPoints);
00411 
00412         // fit
00413         control_points_t cp;
00414         leastSquaresFit(p, nPoints, q, r, cp);
00415         // cp.dump("least squares fit");
00416 
00417         // compute curve
00418         curve_t c;
00419         getCurveFromControlPoints(cp, c);
00420 
00421         // does this match to within the desired tolerance?
00422         float fit = getFitQuality(p, nPoints, c);
00423         if (fit < tolerance) {
00424                 path.push_back(c);      // yes, success!
00425                 return;
00426         }
00427 
00428         // didn't fit to within tolerance.  Subdivide
00429         // DPRINTF("  Subdividing!");
00430         ASSERT(nPoints > 4, "Bad fit with %d points!?", nPoints);
00431         int mid = nPoints / 2;
00432         ASSERT(mid > 0 && mid < nPoints - 1, "Bad midpoint: %d", mid);
00433 
00434         // fit first half
00435         int newN = mid + 1;
00436         // DPRINTF("First half: %d - %d", 0, mid);
00437         point_t q2, r2;
00438         getQR(p, newN, q2, r2);
00439         addPathSegments(p, q2, r2, newN, tolerance, path);
00440 
00441         // fit second half
00442         newN = nPoints - mid;
00443         // DPRINTF("Second half: %d - %d", mid, nPoints - 1);
00444         getQR(p + mid, newN, q2, r2);
00445         addPathSegments(p + mid, q2, r2, newN, tolerance, path);
00446 }
00447 
00448 
00449 
00450 ////////////////////////////////////////////////////////////////////////////////
00451 //
00452 //      public API
00453 //
00454 ////////////////////////////////////////////////////////////////////////////////
00455 
00456 void
00457 fitPoints
00458 (
00459 IN const float * x,
00460 IN int nPoints,
00461 IN float slope,
00462 OUT fit_t& fit
00463 )
00464 throw()
00465 {
00466         ASSERT(x, "null x-array");
00467         ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
00468 
00469         fit.clear();
00470         //DPRINTF("  slope = %5.2f", slope);
00471 
00472         if (2 == nPoints) {
00473                 // just a straight line!
00474                 fit.q0 = x[0];
00475                 fit.c = x[1] - x[0];
00476                 return;
00477         }
00478         ASSERT(nPoints > 2, "Bad nPoints: %d", nPoints);
00479 
00480         // use control points
00481         float x0 = x[0];
00482         float x1 = x0 + (slope / 3.0);
00483         float x3 = x[nPoints - 1];
00484 
00485         // least-squares fit for x2
00486         float sum_riti = 0.0;
00487         float sum_ti2 = 0.0;
00488         float dt = 1.0 / (nPoints - 1);
00489         float t = dt;
00490 
00491         for (int i = 1; i < nPoints - 1; ++i) {
00492                 float s = 1.0 - t;
00493                 float ti = t * t * s;
00494                 float ri = (x0 * s * s * s) + (3.0 * x1 * t * s * s) + (x3 * t * t * t) - x[i];
00495                 sum_riti += ri * ti;
00496                 sum_ti2 += ti * ti;
00497                 t += dt;
00498         }
00499 
00500         float x2 = (-1.0/3.0) * (sum_riti / sum_ti2);
00501 
00502         //DPRINTF("    Control points: x0=%5.2f  x1=%5.2f  x2=%5.2f  x3=%5.2f",
00503         //    x0, x1, x2, x3);
00504 
00505         // transform from control points back to curve params
00506         fit.q0 = x0;
00507         fit.c = 3.0 * (x1 - x0);
00508         fit.b = 3.0 * (x2 - x1) - fit.c;
00509         fit.a = x3 - x0 - fit.b - fit.c;
00510 }
00511 
00512 
00513 
00514 int
00515 getCuspIndices
00516 (
00517 IN const point_t * p,
00518 IN int nPoints,
00519 OUT int ** cusps
00520 )
00521 {
00522         ASSERT(p, "null array");
00523         ASSERT(nPoints > 0, "Bad array");
00524         ASSERT(cusps, "null array");
00525 
00526         // cusp arrays
00527         static int * s_cusp = NULL;
00528         static int s_nPoints = 0;
00529 
00530         if (nPoints > s_nPoints) {
00531                 if (s_cusp)
00532                         delete[] s_cusp;
00533                 s_nPoints = nPoints + 32;
00534                 s_cusp = new int[s_nPoints];
00535         }
00536         ASSERT(s_cusp, "out of memory");
00537 
00538         // degenerate case
00539         if (nPoints < 3) {
00540                 s_cusp[0] = nPoints - 1;
00541                 *cusps = s_cusp;
00542                 return 1;
00543         }
00544 
00545         float s_threshold = 0.5;
00546         int nCusps = 0;
00547 
00548         // we run through and calculate the dot product of sequential rays.
00549         // A low result means the path has radically changed direction
00550         point_t p0 = p[1] - p[0];
00551         p0.normalize();
00552         for (int i = 1; i < nPoints - 1; ++i) {
00553                 point_t p1 = p[i + 1] - p[i];
00554                 p1.normalize();
00555 
00556                 float dot = point_t::dotProduct(p0, p1);
00557                 // DPRINTF("dot = %f", dot);
00558                 if (dot < s_threshold) {
00559                         // DPRINTF("  cusp!");
00560                         s_cusp[nCusps] = i;
00561                         nCusps++;
00562                 }
00563 
00564                 p0 = p1;
00565         }
00566 
00567         // add final point as a cusp (I think this helps clients...)
00568         s_cusp[nCusps] = nPoints - 1;
00569         ++nCusps;
00570 
00571         // return
00572         *cusps = s_cusp;
00573         return nCusps;
00574 }
00575 
00576 
00577 
00578 float
00579 getFitQuality
00580 (
00581 IN const point_t * p,
00582 IN int nPoints,
00583 IN const curve_t& curve
00584 )
00585 throw()
00586 {
00587         ASSERT(p, "null array");
00588         ASSERT(nPoints > 1, "bad npoints");
00589 
00590         float dt = 1.0 / (nPoints - 1);
00591         float sum2 = 0.0;
00592         float t = 0.0;
00593         point_t q;
00594         for (int i = 1; i < nPoints - 1; ++i) {
00595                 t += dt;
00596                 curve.getPointAtT(t, q);
00597                 // DPRINTF("  Actual (%5.2f, %5.2f)  vs. calculated (%5.2f, %5.2f)",
00598                 //     p[i].x, p[i].y, q.x, q.y);
00599                 float dx = p[i].x - q.x;
00600                 float dy = p[i].y - q.y;
00601                 float diff = (dx * dx) + (dy * dy);
00602                 sum2 += diff; 
00603         }
00604 
00605         float rms = sqrt(sum2) / nPoints;
00606         // DPRINTF("RMS of fit: %5.2f", rms);
00607 
00608         return rms;
00609 }
00610 
00611 
00612 
00613 void
00614 fitPath
00615 (
00616 IN const point_t * p,
00617 IN int nPoints,
00618 IN float tolerance,
00619 OUT path_t& path
00620 )
00621 {
00622         perf::Timer timer("bezier::fitPath");
00623 
00624         ASSERT(p, "null array");
00625         ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
00626         ASSERT(tolerance > 0.0, "Bad tolerance: %f", tolerance);
00627         path.clear();
00628 
00629         // get all cusps
00630         int * cusps = NULL;
00631         int nCusp = getCuspIndices(p, nPoints, &cusps);
00632         ASSERT(cusps, "should have non-null cusp index array");
00633         ASSERT(nCusp > 0, "should have at least 1 cusp (end): %d", nCusp);
00634 
00635         // loop through all cusps
00636         int start = 0;          // starting index
00637         int end = 0;
00638         for (int i = 0; i < nCusp; ++i) {
00639                 // determine endpoint for this segment
00640                 end = cusps[i];
00641                 // was the final point already a cusp?
00642                 if (start == end)
00643                         break;
00644                 ASSERT(end > start, "Bad start=%d, end=%d", start, end);
00645                 ASSERT(end < nPoints, "bad end=%d", end);
00646 
00647                 // have an endpoint
00648                 // DPRINTF("start=%02d  end=%02d", start, end);
00649 
00650                 // Number of points?
00651                 int N = end - start + 1;
00652                 ASSERT(N > 1, "Bad N:%d", N);
00653 
00654                 // get starting/ending directions
00655                 point_t q,r;
00656                 getQR(p + start, N, q, r);
00657 
00658                 // get this segment's path elements
00659                 addPathSegments(p + start, q, r, N, tolerance, path);
00660 
00661                 // next segment
00662                 start = end;
00663         }
00664 }
00665 
00666 
00667 
00668 void
00669 respacePath
00670 (
00671 IN const point_t * p,
00672 IN int nPoints,
00673 IN float dt,
00674 OUT point_t ** out_p,
00675 OUT int& newPoints
00676 )
00677 {
00678         ASSERT(nPoints > 0, "not enough points!");
00679         ASSERT(p, "null");
00680         ASSERT(out_p, "null");
00681         ASSERT(dt > 0, "bad path step size");
00682 
00683         static vec_point_t s_p;
00684         s_p.clear();
00685 
00686         // first point must match
00687         s_p.push_back(p[0]);
00688 
00689         // special case n=1
00690         if (1 == nPoints) {
00691                 point_t d(0.005, 0.005);
00692                 s_p.push_back(p[0] + d);
00693 //              *out_p = s_p.data();    // not supported in Shrike
00694                 *out_p = &s_p[0];
00695                 newPoints = 2;
00696                 return;
00697         }
00698 
00699         // longer paths
00700         int src = 0;    // starting at source point 0
00701         float s = 0.0;          // total path length
00702         point_t p0 = p[0];
00703         for (float t = dt; src < nPoints - 1; t += dt) {
00704                 // step source until we are at good start point
00705                 float ds;
00706                 point_t dp;
00707                 while (true) {
00708                         point_t p1 = p[src + 1];
00709                         dp = p1 - p0;
00710                         ds = sqrt(point_t::dotProduct(dp, dp));
00711                         if (t < s + ds || src >= nPoints - 1)
00712                                 break;
00713                         src++;
00714                         p0 = p1;
00715                         s += ds;
00716                 }
00717 
00718                 // interpolate between src and src + 1
00719                 float d0 = t - s;
00720                 float q = d0 / ds;
00721                 if (q > 1.0) {
00722                         q = 1.0;
00723                 }
00724                 ASSERT(q >= 0 && q <= 1.0, "Bad q: %f", q);
00725 
00726                 point_t p1(p0.x + q * dp.x, p0.y + q * dp.y);
00727                 point_t d = p1 - s_p.back();
00728                 if (sqrt(point_t::dotProduct(d, d)) > 0.2 * ds) {
00729                         s_p.push_back(p1);
00730                 }
00731         }
00732 
00733         // DPRINTF("Total length s=%5.2f  dt=%5.2f  in_n=%d  out_n=%d",
00734         //    s, dt, nPoints, (int) s_p.size());
00735 
00736         // all done!  Return data
00737 //      *out_p = s_p.data();    // not supported in Shrike!
00738         *out_p = &s_p[0];
00739         newPoints = (int) s_p.size();
00740 
00741         if (1 == newPoints) {
00742                 // add a point so we always deal with two...
00743                 point_t p1 = s_p.back();
00744                 p1.x += 0.001;
00745                 p1.y += 0.001;
00746                 s_p.push_back(p1);
00747                 newPoints = 2;
00748         }
00749 
00750         /*
00751         // DEBUG
00752         {
00753                 int i = 0;
00754                 while (i < nPoints || i < newPoints) {
00755                         point_t old(-1.0, -1.0);
00756                         if (i < nPoints)
00757                                 old = p[i];
00758                         point_t newp(-1.0, -1.0);
00759                         if (i < newPoints)
00760                                 newp = s_p[i];
00761 
00762                         DPRINTF("%03d: old (%5.2f, %5.2f)   new (%5.2f, %5.2f)",
00763                             i, old.x, old.y, newp.x, newp.y);
00764                         ++i;
00765                 }
00766         }
00767         */
00768 }
00769 
00770 
00771 
00772 };      // bezier namespace
00773