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