src/bezier.cpp

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