src/2geom/sbasis.cpp

   1 /*
   2  *  sbasis.cpp - S-power basis function class + supporting classes
   3  *
   4  *  Authors:
   5  *   Nathan Hurst <njh@mail.csse.monash.edu.au>
   6  *   Michael Sloan <mgsloan@gmail.com>
   7  *
   8  * Copyright (C) 2006-2007 authors
   9  *
  10  * This library is free software; you can redistribute it and/or
  11  * modify it either under the terms of the GNU Lesser General Public
  12  * License version 2.1 as published by the Free Software Foundation
  13  * (the "LGPL") or, at your option, under the terms of the Mozilla
  14  * Public License Version 1.1 (the "MPL"). If you do not alter this
  15  * notice, a recipient may use your version of this file under either
  16  * the MPL or the LGPL.
  17  *
  18  * You should have received a copy of the LGPL along with this library
  19  * in the file COPYING-LGPL-2.1; if not, write to the Free Software
  20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  21  * You should have received a copy of the MPL along with this library
  22  * in the file COPYING-MPL-1.1
  23  *
  24  * The contents of this file are subject to the Mozilla Public License
  25  * Version 1.1 (the "License"); you may not use this file except in
  26  * compliance with the License. You may obtain a copy of the License at
  27  * http://www.mozilla.org/MPL/
  28  *
  29  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
  30  * OF ANY KIND, either express or implied. See the LGPL or the MPL for
  31  * the specific language governing rights and limitations.
  32  */
  33
  34 #include <cmath>
  35
  36 #include "sbasis.h"
  37 #include "isnan.h"
  38
  39 namespace Geom{
  40
  41 /*** At some point we should work on tighter bounds for the error.  It is clear that the error is
  42  * bounded by the L1 norm over the tail of the series, but this is very loose, leading to far too
  43  * many cubic beziers.  I've changed this to be \sum _i=tail ^\infty |hat a_i| 2^-i but I have no
  44  * evidence that this is correct.
  45  */
  46
  47 /*
  48 double SBasis::tail_error(unsigned tail) const {
  49     double err = 0, s = 1./(1<<(2*tail)); // rough
  50     for(unsigned i = tail; i < size(); i++) {
  51         err += (fabs((*this)[i][0]) + fabs((*this)[i][1]))*s;
  52         s /= 4;
  53     }
  54     return err;
  55 }
  56 */
  57
  58 double SBasis::tailError(unsigned tail) const {
  59   Interval bs = bounds_fast(*this, tail);
  60   return std::max(fabs(bs.min()),fabs(bs.max()));
  61 }
  62
  63 bool SBasis::isFinite() const {
  64     for(unsigned i = 0; i < size(); i++) {
  65         if(!(*this)[i].isFinite())
  66             return false;
  67     }
  68     return true;
  69 }
  70
  71 std::vector<double> SBasis::valueAndDerivatives(double t, unsigned n) const {
  72     std::vector<double> ret(n+1);
  73     ret.push_back(valueAt(t));
  74     SBasis tmp = *this;
  75     for(unsigned i = 0; i < n; i++) {
  76         tmp.derive();
  77         ret[i] = tmp.valueAt(t);
  78     }
  79     return ret;
  80 }
  81
  82
  83 SBasis operator+(const SBasis& a, const SBasis& b) {
  84     SBasis result;
  85     const unsigned out_size = std::max(a.size(), b.size());
  86     const unsigned min_size = std::min(a.size(), b.size());
  87     result.reserve(out_size);
  88
  89     for(unsigned i = 0; i < min_size; i++) {
  90         result.push_back(a[i] + b[i]);
  91     }
  92     for(unsigned i = min_size; i < a.size(); i++)
  93         result.push_back(a[i]);
  94     for(unsigned i = min_size; i < b.size(); i++)
  95         result.push_back(b[i]);
  96
  97     assert(result.size() == out_size);
  98     return result;
  99 }
 100
 101 SBasis operator-(const SBasis& a, const SBasis& b) {
 102     SBasis result;
 103     const unsigned out_size = std::max(a.size(), b.size());
 104     const unsigned min_size = std::min(a.size(), b.size());
 105     result.reserve(out_size);
 106
 107     for(unsigned i = 0; i < min_size; i++) {
 108         result.push_back(a[i] - b[i]);
 109     }
 110     for(unsigned i = min_size; i < a.size(); i++)
 111         result.push_back(a[i]);
 112     for(unsigned i = min_size; i < b.size(); i++)
 113         result.push_back(-b[i]);
 114
 115     assert(result.size() == out_size);
 116     return result;
 117 }
 118
 119 SBasis& operator+=(SBasis& a, const SBasis& b) {
 120     const unsigned out_size = std::max(a.size(), b.size());
 121     const unsigned min_size = std::min(a.size(), b.size());
 122     a.reserve(out_size);
 123
 124     for(unsigned i = 0; i < min_size; i++)
 125         a[i] += b[i];
 126     for(unsigned i = min_size; i < b.size(); i++)
 127         a.push_back(b[i]);
 128
 129     assert(a.size() == out_size);
 130     return a;
 131 }
 132
 133 SBasis& operator-=(SBasis& a, const SBasis& b) {
 134     const unsigned out_size = std::max(a.size(), b.size());
 135     const unsigned min_size = std::min(a.size(), b.size());
 136     a.reserve(out_size);
 137
 138     for(unsigned i = 0; i < min_size; i++)
 139         a[i] -= b[i];
 140     for(unsigned i = min_size; i < b.size(); i++)
 141         a.push_back(-b[i]);
 142
 143     assert(a.size() == out_size);
 144     return a;
 145 }
 146
 147 SBasis operator*(SBasis const &a, double k) {
 148     SBasis c;
 149     c.reserve(a.size());
 150     for(unsigned i = 0; i < a.size(); i++)
 151         c.push_back(a[i] * k);
 152     return c;
 153 }
 154
 155 SBasis& operator*=(SBasis& a, double b) {
 156     if (a.isZero()) return a;
 157     if (b == 0)
 158         a.clear();
 159     else
 160         for(unsigned i = 0; i < a.size(); i++)
 161             a[i] *= b;
 162     return a;
 163 }
 164
 165 SBasis shift(SBasis const &a, int sh) {
 166     SBasis c = a;
 167     if(sh > 0) {
 168         c.insert(c.begin(), sh, Linear(0,0));
 169     } else {
 170         //TODO: truncate
 171     }
 172     return c;
 173 }
 174
 175 SBasis shift(Linear const &a, int sh) {
 176     SBasis c;
 177     if(sh > 0) {
 178         c.insert(c.begin(), sh, Linear(0,0));
 179         c.push_back(a);
 180     }
 181     return c;
 182 }
 183
 184 #if 0
 185 SBasis multiply(SBasis const &a, SBasis const &b) {
 186     // c = {a0*b0 - shift(1, a.Tri*b.Tri), a1*b1 - shift(1, a.Tri*b.Tri)}
 187
 188     // shift(1, a.Tri*b.Tri)
 189     SBasis c;
 190     if(a.isZero() || b.isZero())
 191         return c;
 192     c.resize(a.size() + b.size(), Linear(0,0));
 193     for(unsigned j = 0; j < b.size(); j++) {
 194         for(unsigned i = j; i < a.size()+j; i++) {
 195             double tri = Tri(b[j])*Tri(a[i-j]);
 196             c[i+1/*shift*/] += Linear(Hat(-tri));
 197         }
 198     }
 199     for(unsigned j = 0; j < b.size(); j++) {
 200         for(unsigned i = j; i < a.size()+j; i++) {
 201             for(unsigned dim = 0; dim < 2; dim++)
 202                 c[i][dim] += b[j][dim]*a[i-j][dim];
 203         }
 204     }
 205     c.normalize();
 206     //assert(!(0 == c.back()[0] && 0 == c.back()[1]));
 207     return c;
 208 }
 209 #else
 210
 211 SBasis multiply_add(SBasis const &a, SBasis const &b, SBasis c) {
 212     if(a.isZero() || b.isZero())
 213         return c;
 214     c.resize(a.size() + b.size(), Linear(0,0));
 215     for(unsigned j = 0; j < b.size(); j++) {
 216         for(unsigned i = j; i < a.size()+j; i++) {
 217             double tri = Tri(b[j])*Tri(a[i-j]);
 218             c[i+1/*shift*/] += Linear(Hat(-tri));
 219         }
 220     }
 221     for(unsigned j = 0; j < b.size(); j++) {
 222         for(unsigned i = j; i < a.size()+j; i++) {
 223             for(unsigned dim = 0; dim < 2; dim++)
 224                 c[i][dim] += b[j][dim]*a[i-j][dim];
 225         }
 226     }
 227     c.normalize();
 228     //assert(!(0 == c.back()[0] && 0 == c.back()[1]));
 229     return c;
 230 }
 231
 232 SBasis multiply(SBasis const &a, SBasis const &b) {
 233     SBasis c;
 234     if(a.isZero() || b.isZero())
 235         return c;
 236     return multiply_add(a, b, c);
 237 }
 238 #endif
 239 SBasis integral(SBasis const &c) {
 240     SBasis a;
 241     a.resize(c.size() + 1, Linear(0,0));
 242     a[0] = Linear(0,0);
 243
 244     for(unsigned k = 1; k < c.size() + 1; k++) {
 245         double ahat = -Tri(c[k-1])/(2*k);
 246         a[k] = Hat(ahat);
 247     }
 248     double aTri = 0;
 249     for(int k = c.size()-1; k >= 0; k--) {
 250         aTri = (Hat(c[k]).d + (k+1)*aTri/2)/(2*k+1);
 251         a[k][0] -= aTri/2;
 252         a[k][1] += aTri/2;
 253     }
 254     a.normalize();
 255     return a;
 256 }
 257
 258 SBasis derivative(SBasis const &a) {
 259     SBasis c;
 260     c.resize(a.size(), Linear(0,0));
 261
 262     for(unsigned k = 0; k < a.size()-1; k++) {
 263         double d = (2*k+1)*(a[k][1] - a[k][0]);
 264
 265         c[k][0] = d + (k+1)*a[k+1][0];
 266         c[k][1] = d - (k+1)*a[k+1][1];
 267     }
 268     int k = a.size()-1;
 269     double d = (2*k+1)*(a[k][1] - a[k][0]);
 270     if(d == 0)
 271         c.pop_back();
 272     else {
 273         c[k][0] = d;
 274         c[k][1] = d;
 275     }
 276
 277     return c;
 278 }
 279
 280 void SBasis::derive() { // in place version
 281     for(unsigned k = 0; k < size()-1; k++) {
 282         double d = (2*k+1)*((*this)[k][1] - (*this)[k][0]);
 283
 284         (*this)[k][0] = d + (k+1)*(*this)[k+1][0];
 285         (*this)[k][1] = d - (k+1)*(*this)[k+1][1];
 286     }
 287     int k = size()-1;
 288     double d = (2*k+1)*((*this)[k][1] - (*this)[k][0]);
 289     if(d == 0)
 290         pop_back();
 291     else {
 292         (*this)[k][0] = d;
 293         (*this)[k][1] = d;
 294     }
 295 }
 296
 297 //TODO: convert int k to unsigned k, and remove cast
 298 SBasis sqrt(SBasis const &a, int k) {
 299     SBasis c;
 300     if(a.isZero() || k == 0)
 301         return c;
 302     c.resize(k, Linear(0,0));
 303     c[0] = Linear(std::sqrt(a[0][0]), std::sqrt(a[0][1]));
 304     SBasis r = a - multiply(c, c); // remainder
 305
 306     for(unsigned i = 1; i <= (unsigned)k and i<r.size(); i++) {
 307         Linear ci(r[i][0]/(2*c[0][0]), r[i][1]/(2*c[0][1]));
 308         SBasis cisi = shift(ci, i);
 309         r -= multiply(shift((c*2 + cisi), i), SBasis(ci));
 310         r.truncate(k+1);
 311         c += cisi;
 312         if(r.tailError(i) == 0) // if exact
 313             break;
 314     }
 315
 316     return c;
 317 }
 318
 319 // return a kth order approx to 1/a)
 320 SBasis reciprocal(Linear const &a, int k) {
 321     SBasis c;
 322     assert(!a.isZero());
 323     c.resize(k, Linear(0,0));
 324     double r_s0 = (Tri(a)*Tri(a))/(-a[0]*a[1]);
 325     double r_s0k = 1;
 326     for(unsigned i = 0; i < (unsigned)k; i++) {
 327         c[i] = Linear(r_s0k/a[0], r_s0k/a[1]);
 328         r_s0k *= r_s0;
 329     }
 330     return c;
 331 }
 332
 333 SBasis divide(SBasis const &a, SBasis const &b, int k) {
 334     SBasis c;
 335     assert(!a.isZero());
 336     SBasis r = a; // remainder
 337
 338     k++;
 339     r.resize(k, Linear(0,0));
 340     c.resize(k, Linear(0,0));
 341
 342     for(unsigned i = 0; i < (unsigned)k; i++) {
 343         Linear ci(r[i][0]/b[0][0], r[i][1]/b[0][1]); //H0
 344         c[i] += ci;
 345         r -= shift(multiply(ci,b), i);
 346         r.truncate(k+1);
 347         if(r.tailError(i) == 0) // if exact
 348             break;
 349     }
 350
 351     return c;
 352 }
 353
 354 // a(b)
 355 // return a0 + s(a1 + s(a2 +...  where s = (1-u)u; ak =(1 - u)a^0_k + ua^1_k
 356 SBasis compose(SBasis const &a, SBasis const &b) {
 357     SBasis s = multiply((SBasis(Linear(1,1))-b), b);
 358     SBasis r;
 359
 360     for(int i = a.size()-1; i >= 0; i--) {
 361         r = multiply_add(r, s, SBasis(Linear(Hat(a[i][0]))) - b*a[i][0] + b*a[i][1]);
 362     }
 363     return r;
 364 }
 365
 366 // a(b)
 367 // return a0 + s(a1 + s(a2 +...  where s = (1-u)u; ak =(1 - u)a^0_k + ua^1_k
 368 SBasis compose(SBasis const &a, SBasis const &b, unsigned k) {
 369     SBasis s = multiply((SBasis(Linear(1,1))-b), b);
 370     SBasis r;
 371
 372     for(int i = a.size()-1; i >= 0; i--) {
 373         r = multiply_add(r, s, SBasis(Linear(Hat(a[i][0]))) - b*a[i][0] + b*a[i][1]);
 374     }
 375     r.truncate(k);
 376     return r;
 377 }
 378
 379 /*
 380 Inversion algorithm. The notation is certainly very misleading. The
 381 pseudocode should say:
 382
 383 c(v) := 0
 384 r(u) := r_0(u) := u
 385 for i:=0 to k do
 386   c_i(v) := H_0(r_i(u)/(t_1)^i; u)
 387   c(v) := c(v) + c_i(v)*t^i
 388   r(u) := r(u) ? c_i(u)*(t(u))^i
 389 endfor
 390 */
 391
 392 //#define DEBUG_INVERSION 1
 393
 394 SBasis inverse(SBasis a, int k) {
 395     assert(a.size() > 0);
 396 // the function should have 'unit range'("a00 = 0 and a01 = 1") and be monotonic.
 397     double a0 = a[0][0];
 398     if(a0 != 0) {
 399         a -= a0;
 400     }
 401     double a1 = a[0][1];
 402     assert(a1 != 0); // not invertable.
 403
 404     if(a1 != 1) {
 405         a /= a1;
 406     }
 407     SBasis c;                           // c(v) := 0
 408     if(a.size() >= 2 && k == 2) {
 409         c.push_back(Linear(0,1));
 410         Linear t1(1+a[1][0], 1-a[1][1]);    // t_1
 411         c.push_back(Linear(-a[1][0]/t1[0], -a[1][1]/t1[1]));
 412     } else if(a.size() >= 2) {                      // non linear
 413         SBasis r = Linear(0,1);             // r(u) := r_0(u) := u
 414         Linear t1(1./(1+a[1][0]), 1./(1-a[1][1]));    // 1./t_1
 415         Linear one(1,1);
 416         Linear t1i = one;                   // t_1^0
 417         SBasis one_minus_a = SBasis(one) - a;
 418         SBasis t = multiply(one_minus_a, a); // t(u)
 419         SBasis ti(one);                     // t(u)^0
 420 #ifdef DEBUG_INVERSION
 421         std::cout << "a=" << a << std::endl;
 422         std::cout << "1-a=" << one_minus_a << std::endl;
 423         std::cout << "t1=" << t1 << std::endl;
 424         //assert(t1 == t[1]);
 425 #endif
 426
 427         c.resize(k+1, Linear(0,0));
 428         for(unsigned i = 0; i < (unsigned)k; i++) {   // for i:=0 to k do
 429 #ifdef DEBUG_INVERSION
 430             std::cout << "-------" << i << ": ---------" <<std::endl;
 431             std::cout << "r=" << r << std::endl
 432                       << "c=" << c << std::endl
 433                       << "ti=" << ti << std::endl
 434                       << std::endl;
 435 #endif
 436             if(r.size() <= i)                // ensure enough space in the remainder, probably not needed
 437                 r.resize(i+1, Linear(0,0));
 438             Linear ci(r[i][0]*t1i[0], r[i][1]*t1i[1]); // c_i(v) := H_0(r_i(u)/(t_1)^i; u)
 439 #ifdef DEBUG_INVERSION
 440             std::cout << "t1i=" << t1i << std::endl;
 441             std::cout << "ci=" << ci << std::endl;
 442 #endif
 443             for(int dim = 0; dim < 2; dim++) // t1^-i *= 1./t1
 444                 t1i[dim] *= t1[dim];
 445             c[i] = ci; // c(v) := c(v) + c_i(v)*t^i
 446             // change from v to u parameterisation
 447             SBasis civ = one_minus_a*ci[0] + a*ci[1];
 448             // r(u) := r(u) - c_i(u)*(t(u))^i
 449             // We can truncate this to the number of final terms, as no following terms can
 450             // contribute to the result.
 451             r -= multiply(civ,ti);
 452             r.truncate(k);
 453             if(r.tailError(i) == 0)
 454                 break; // yay!
 455             ti = multiply(ti,t);
 456         }
 457 #ifdef DEBUG_INVERSION
 458         std::cout << "##########################" << std::endl;
 459 #endif
 460     } else
 461         c = Linear(0,1); // linear
 462     c -= a0; // invert the offset
 463     c /= a1; // invert the slope
 464     return c;
 465 }
 466
 467 SBasis sin(Linear b, int k) {
 468     SBasis s = Linear(std::sin(b[0]), std::sin(b[1]));
 469     Tri tr(s[0]);
 470     double t2 = Tri(b);
 471     s.push_back(Linear(std::cos(b[0])*t2 - tr, -std::cos(b[1])*t2 + tr));
 472
 473     t2 *= t2;
 474     for(int i = 0; i < k; i++) {
 475         Linear bo(4*(i+1)*s[i+1][0] - 2*s[i+1][1],
 476                   -2*s[i+1][0] + 4*(i+1)*s[i+1][1]);
 477         bo -= s[i]*(t2/(i+1));
 478
 479
 480         s.push_back(bo/double(i+2));
 481     }
 482
 483     return s;
 484 }
 485
 486 SBasis cos(Linear bo, int k) {
 487     return sin(Linear(bo[0] + M_PI/2,
 488                       bo[1] + M_PI/2),
 489                k);
 490 }
 491
 492 //compute fog^-1. ("zero" = double comparison threshold. *!*we might divide by "zero"*!*)
 493 //TODO: compute order according to tol?
 494 //TODO: requires g(0)=0 & g(1)=1 atm... adaptation to other cases should be obvious!
 495 SBasis compose_inverse(SBasis const &f, SBasis const &g, unsigned order, double zero){
 496     SBasis result; //result
 497     SBasis r=f; //remainder
 498     SBasis Pk=Linear(1)-g,Qk=g,sg=Pk*Qk;
 499     Pk.truncate(order);
 500     Qk.truncate(order);
 501     Pk.resize(order,Linear(0.));
 502     Qk.resize(order,Linear(0.));
 503     r.resize(order,Linear(0.));
 504
 505     int vs= valuation(sg,zero);
 506
 507     for (unsigned k=0; k<order; k+=vs){
 508         double p10 = Pk.at(k)[0];// we have to solve the linear system:
 509         double p01 = Pk.at(k)[1];//
 510         double q10 = Qk.at(k)[0];//   p10*a + q10*b = r10
 511         double q01 = Qk.at(k)[1];// &
 512         double r10 =  r.at(k)[0];//   p01*a + q01*b = r01
 513         double r01 =  r.at(k)[1];//
 514         double a,b;
 515         double det = p10*q01-p01*q10;
 516
 517         //TODO: handle det~0!!
 518         if (fabs(det)<zero){
 519             det = zero;
 520             a=b=0;
 521         }else{
 522             a=( q01*r10-q10*r01)/det;
 523             b=(-p01*r10+p10*r01)/det;
 524         }
 525         result.push_back(Linear(a,b));
 526         r=r-Pk*a-Qk*b;
 527
 528         Pk=Pk*sg;
 529         Qk=Qk*sg;
 530         Pk.truncate(order);
 531         Qk.truncate(order);
 532         r.truncate(order);
 533     }
 534     result.normalize();
 535     return result;
 536 }
 537
 538 }
 539
 540 /*
 541   Local Variables:
 542   mode:c++
 543   c-file-style:"stroustrup"
 544   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 545   indent-tabs-mode:nil
 546   fill-column:99
 547   End:
 548 */
 549 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :