src/bezier.cpp

   1 #include "bezier.h"
   2
   3 #include <unordered_map>
   4 #include <cmath>
   5 #include <algorithm>
   6
   7 using namespace std;
   8
   9 namespace IPDF
  10 {
  11
  12 vector<Real> SolveQuadratic(const Real & a, const Real & b, const Real & c, const Real & min, const Real & max)
  13 {
  14         vector<Real> roots; roots.reserve(2);
  15         if (a == 0 && b != 0)
  16         {
  17                 roots.push_back(-c/b);
  18                 return roots;
  19         }
  20         Real disc(b*b - Real(4)*a*c);
  21         if (disc < 0)
  22         {
  23                 return roots;
  24         }
  25         else if (disc == 0)
  26         {
  27                 Real x(-b/Real(2)*a);
  28                 if (x >= min && x <= max)
  29                         roots.push_back(x);
  30                 return roots;
  31         }
  32
  33         Real x0((-b - Sqrt(b*b - Real(4)*a*c))/(Real(2)*a));
  34         Real x1((-b + Sqrt(b*b - Real(4)*a*c))/(Real(2)*a));
  35         if (x0 > x1)
  36         {
  37                 Real tmp(x0);
  38                 x0 = x1;
  39                 x1 = tmp;
  40         }
  41         if (x0 >= min && x0 <= max)
  42                 roots.push_back(x0);
  43         if (x1 >= min && x1 <= max)
  44                 roots.push_back(x1);
  45         return roots;
  46 }
  47
  48 /**
  49  * Finds the root (if it exists) in a monotonicly in(de)creasing segment of a Cubic
  50  */
  51
  52 static void CubicSolveSegment(vector<Real> & roots, const Real & a, const Real & b, const Real & c, const Real & d, Real & tl, Real & tu, const Real & delta)
  53 {
  54         Real l = a*tl*tl*tl + b*tl*tl + c*tl + d;
  55         Real u = a*tu*tu*tu + b*tu*tu + c*tu + d;
  56         if ((l < 0 && u < 0) || (l > 0 && u > 0))
  57                 return;
  58
  59         bool negative = (u < l); // lower point > 0, upper point < 0
  60         while (tu - tl > delta)
  61         {
  62                 Real t(tu+tl);
  63                 t /= 2;
  64                 Real m = a*t*t*t + b*t*t + c*t + d;
  65                 if (m > 0)
  66                 {
  67                         if (negative)
  68                                 tl = t;
  69                         else
  70                                 tu = t;
  71                 }
  72                 else if (negative)
  73                 {
  74                         tu = t;
  75                 }
  76                 else
  77                 {
  78                         tl = t;
  79                 }
  80                 //Debug("Delta is %f (%f - %f -> %f)", tu-tl, tu, tl, t);
  81         }
  82         roots.push_back(tl);
  83 }
  84 vector<Real> SolveCubic(const Real & a, const Real & b, const Real & c, const Real & d, const Real & min, const Real & max, const Real & delta)
  85 {
  86         vector<Real> roots; roots.reserve(3);
  87         Real tu(max);
  88         Real tl(min);
  89         vector<Real> turns(SolveQuadratic(a*3, b*2, c));
  90         //Debug("%u turning points", turns.size());
  91         for (unsigned i = 1; i < turns.size(); ++i)
  92         {
  93                 tu = turns[i];
  94                 CubicSolveSegment(roots, a, b, c, d, tl, tu,delta);
  95                 tl = turns[i];
  96         }
  97         tu = max;
  98         CubicSolveSegment(roots, a, b, c, d, tl, tu,delta);
  99         return roots;
 100         /*
 101                 Real maxi(100);
 102                 Real prevRes(d);
 103                 for(int i = 0; i <= 100; ++i)
 104                 {
 105                         Real x(i);
 106                         x /= maxi;
 107                         Real y = a*(x*x*x) + b*(x*x) + c*x + d;
 108                         if (((y < Real(0)) && (prevRes > Real(0))) || ((y > Real(0)) && (prevRes < Real(0))))
 109                         {
 110                                 //Debug("Found root of %fx^3 + %fx^2 + %fx + %f at %f (%f)", a, b, c, d, x, y);
 111                                 roots.push_back(x);
 112                         }
 113                         prevRes = y;
 114                 }
 115                 return roots;
 116         */
 117 }
 118
 119 /**
 120  * Factorial
 121  * Use dynamic programming / recursion
 122  */
 123 int Factorial(int n)
 124 {
 125         static unordered_map<int, int> dp;
 126         static bool init = false;
 127         if (!init)
 128         {
 129                 init = true;
 130                 dp[0] = 1;
 131         }
 132         auto it = dp.find(n);
 133         if (it != dp.end())
 134                 return it->second;
 135         int result = n*Factorial(n-1);
 136         dp[n] = result;
 137         return result;
 138 }
 139
 140 /**
 141  * Binomial coefficients
 142  */
 143 int BinomialCoeff(int n, int k)
 144 {
 145         return Factorial(n) / (Factorial(k) * Factorial(n-k));
 146 }
 147
 148 /**
 149  * Bernstein Basis Polynomial
 150  */
 151 Real Bernstein(int k, int n, const Real & u)
 152 {
 153         return Real(BinomialCoeff(n, k)) * Power(u, k) * Power(Real(1.0) - u, n-k);
 154 }
 155
 156
 157 /**
 158  * Returns the parametric parameter at the turning point(s)
 159  * In one coordinate direction
 160  */
 161
 162 pair<Real, Real> BezierTurningPoints(const Real & p0, const Real & p1, const Real & p2, const Real & p3)
 163 {
 164         // straight line
 165         if (p1 == p2 && p2 == p3)
 166         {
 167                 return pair<Real,Real>(0, 1);
 168         }
 169         Real a = ((p1-p2)*3 + p3 - p0);
 170         Real b = (p2 - p1*2 + p0)*2;
 171         Real c = (p1-p0);
 172         if (a == 0)
 173         {
 174                 if (b == 0)
 175                         return pair<Real, Real>(0,1);
 176                 Real t = -c/b;
 177                 if (t > 1) t = 1;
 178                 if (t < 0) t = 0;
 179                 return pair<Real, Real>(t, t);
 180         }
 181         //Debug("a, b, c are %f, %f, %f", Float(a), Float(b), Float(c));
 182         if (b*b - a*c*4 < 0)
 183         {
 184                 //Debug("No real roots");
 185                 return pair<Real, Real>(0,1);
 186         }
 187         vector<Real> tsols = SolveQuadratic(a, b, c);
 188         if (tsols.size() == 1)
 189                 return pair<Real,Real>(tsols[0], tsols[0]);
 190         else if (tsols.size() == 0)
 191                 return pair<Real, Real>(0,1);
 192
 193         return pair<Real,Real>(tsols[0], tsols[1]);
 194
 195 }
 196
 197 inline bool CompRealByPtr(const Real * a, const Real * b)
 198 {
 199         return (*a) < (*b);
 200 }
 201
 202 /**
 203  * Get top most *point* on Bezier curve
 204  */
 205 pair<Real,Real> Bezier::GetTop() const
 206 {
 207         pair<Real, Real> tsols = BezierTurningPoints(y0,y1,y2,y3);
 208         Real tx0; Real ty0;
 209         Real tx1; Real ty1;
 210         Evaluate(tx0, ty0, tsols.first);
 211         Evaluate(tx1, ty1, tsols.second);
 212         vector<const Real*> v(4);
 213         v[0] = &y0;
 214         v[1] = &y3;
 215         v[2] = &ty0;
 216         v[3] = &ty1;
 217         sort(v.begin(), v.end(), CompRealByPtr);
 218         pair<Real,Real> result;
 219         result.second = *v[0];
 220         if (v[0] == &y0)
 221         {
 222                 result.first = x0;
 223         }
 224         else if (v[0] == &y3)
 225         {
 226                 result.first = x3;
 227         }
 228         else if (v[0] == &ty0)
 229         {
 230                 result.first = tx0;
 231         }
 232         else if (v[0] == &ty1)
 233         {
 234                 result.first = tx1;
 235         }
 236         return result;
 237 }
 238
 239 /**
 240  * Get bottom most *point* on Bezier curve
 241  */
 242 pair<Real,Real> Bezier::GetBottom() const
 243 {
 244         pair<Real, Real> tsols = BezierTurningPoints(y0,y1,y2,y3);
 245         Real tx0; Real ty0;
 246         Real tx1; Real ty1;
 247         Evaluate(tx0, ty0, tsols.first);
 248         Evaluate(tx1, ty1, tsols.second);
 249         vector<const Real*> v(4);
 250         v[0] = &y0;
 251         v[1] = &y3;
 252         v[2] = &ty0;
 253         v[3] = &ty1;
 254         sort(v.begin(), v.end(), CompRealByPtr);
 255         pair<Real,Real> result;
 256         result.second = *v[3];
 257         if (v[3] == &y0)
 258         {
 259                 result.first = x0;
 260         }
 261         else if (v[3] == &y3)
 262         {
 263                 result.first = x3;
 264         }
 265         else if (v[3] == &ty0)
 266         {
 267                 result.first = tx0;
 268         }
 269         else if (v[3] == &ty1)
 270         {
 271                 result.first = tx1;
 272         }
 273         return result;
 274 }
 275
 276 /**
 277  * Get left most *point* on Bezier curve
 278  */
 279 pair<Real,Real> Bezier::GetLeft() const
 280 {
 281         pair<Real, Real> tsols = BezierTurningPoints(x0,x1,x2,x3);
 282         Real tx0; Real ty0;
 283         Real tx1; Real ty1;
 284         Evaluate(tx0, ty0, tsols.first);
 285         Evaluate(tx1, ty1, tsols.second);
 286         vector<const Real*> v(4);
 287         v[0] = &x0;
 288         v[1] = &x3;
 289         v[2] = &tx0;
 290         v[3] = &tx1;
 291         sort(v.begin(), v.end(), CompRealByPtr);
 292         pair<Real,Real> result;
 293         result.first = *v[0];
 294         if (v[0] == &x0)
 295         {
 296                 result.second = y0;
 297         }
 298         else if (v[0] == &x3)
 299         {
 300                 result.second = y3;
 301         }
 302         else if (v[0] == &tx0)
 303         {
 304                 result.second = ty0;
 305         }
 306         else if (v[0] == &tx1)
 307         {
 308                 result.second = ty1;
 309         }
 310         return result;
 311 }
 312
 313
 314 /**
 315  * Get left most *point* on Bezier curve
 316  */
 317 pair<Real,Real> Bezier::GetRight() const
 318 {
 319         pair<Real, Real> tsols = BezierTurningPoints(x0,x1,x2,x3);
 320         Real tx0; Real ty0;
 321         Real tx1; Real ty1;
 322         Evaluate(tx0, ty0, tsols.first);
 323         Evaluate(tx1, ty1, tsols.second);
 324         vector<const Real*> v(4);
 325         v[0] = &x0;
 326         v[1] = &x3;
 327         v[2] = &tx0;
 328         v[3] = &tx1;
 329         sort(v.begin(), v.end(), CompRealByPtr);
 330         pair<Real,Real> result;
 331         result.first = *v[3];
 332         if (v[3] == &x0)
 333         {
 334                 result.second = y0;
 335         }
 336         else if (v[3] == &x3)
 337         {
 338                 result.second = y3;
 339         }
 340         else if (v[3] == &tx0)
 341         {
 342                 result.second = ty0;
 343         }
 344         else if (v[3] == &tx1)
 345         {
 346                 result.second = ty1;
 347         }
 348         return result;
 349 }
 350
 351 vector<Real> Bezier::SolveXParam(const Real & x) const
 352 {
 353         Real d(x0 - x);
 354         Real c((x1 - x0)*Real(3));
 355         Real b((x2 - x1)*Real(3) - c);
 356         Real a(x3 -x0 - c - b);
 357         vector<Real> results(SolveCubic(a, b, c, d));
 358         for (unsigned i = 0; i < results.size(); ++i)
 359         {
 360                 Vec2 p;
 361                 Evaluate(p.x, p.y, results[i]);
 362         }
 363         return results;
 364 }
 365
 366
 367 vector<Real> Bezier::SolveYParam(const Real & y) const
 368 {
 369         Real d(y0 - y);
 370         Real c((y1 - y0)*Real(3));
 371         Real b((y2 - y1)*Real(3) - c);
 372         Real a(y3 -y0 - c - b);
 373         vector<Real> results(SolveCubic(a, b, c, d));
 374         for (unsigned i = 0; i < results.size(); ++i)
 375         {
 376                 Vec2 p;
 377                 Evaluate(p.x, p.y, results[i]);
 378         }
 379         return results;
 380 }
 381
 382 vector<Vec2> Bezier::Evaluate(const vector<Real> & u) const
 383 {
 384         vector<Vec2> result(u.size());
 385         for (unsigned i = 0; i < u.size(); ++i)
 386         {
 387                 Evaluate(result[i].x, result[i].y, u[i]);
 388         }
 389         return result;
 390 }
 391
 392 /**
 393  * Get Bounds Rectangle of Bezier
 394  */
 395 Rect Bezier::SolveBounds() const
 396 {
 397         Rect result;
 398         pair<Real, Real> tsols = BezierTurningPoints(x0, x1, x2, x3);
 399
 400         Real tp0; Real tp1; Real o;
 401         Evaluate(tp0, o, tsols.first);
 402         Evaluate(tp1, o, tsols.second);
 403
 404         //Debug("x: tp0 is %f tp1 is %f", Float(tp0), Float(tp1));
 405
 406         vector<const Real*> v(4);
 407         v[0] = &x0;
 408         v[1] = &x3;
 409         v[2] = &tp0;
 410         v[3] = &tp1;
 411
 412         // Not using a lambda to keep this compiling on cabellera
 413         sort(v.begin(), v.end(), CompRealByPtr);
 414
 415         result.x = *(v[0]);
 416         result.w = *(v[3]) - result.x;
 417
 418         // Do the same thing for y component (wow this is a mess)
 419         tsols = BezierTurningPoints(y0, y1, y2, y3);
 420         Evaluate(o, tp0, tsols.first);
 421         Evaluate(o, tp1, tsols.second);
 422
 423
 424         //Debug("y: tp0 is %f tp1 is %f", Float(tp0), Float(tp1));
 425
 426         v[0] = &y0;
 427         v[1] = &y3;
 428         v[2] = &tp0;
 429         v[3] = &tp1;
 430         sort(v.begin(), v.end(), CompRealByPtr);
 431
 432         result.y = *(v[0]);
 433         result.h = *(v[3]) - result.y;
 434
 435         //Debug("Solved Bezier %s bounds as %s", Str().c_str(), result.Str().c_str());
 436         return result;
 437 }
 438
 439 } // end namespace