source/upsamplers.cc

   1 /* This file is part of Shapes.
   2  *
   3  * Shapes is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * any later version.
   7  *
   8  * Shapes is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with Shapes.  If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * Copyright 2008 Henrik Tidefelt
  17  */
  18
  19 #include "upsamplers.h"
  20 #include "consts.h"
  21 #include "pathtypes.h"
  22 #include "globals.h"
  23
  24 #include <cmath>
  25 #include <algorithm>
  26
  27 using namespace Shapes;
  28
  29 void
  30 Computation::UpsampleInflections::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords2D > & controls ) const
  31 {
  32         if( controls.p1_ == controls.p0_ &&
  33                         controls.p2_ == controls.p3_ )
  34                 {
  35                         // There are no inflections on a straight segment.
  36                         return;
  37                 }
  38
  39         /* It is sometimes a limiting case that the acceleration and velocity are parallel at one or both of the endpoints.
  40          * However, it is no good to upsample at the endpoints, so we must work with a tolarance here.
  41          */
  42         Concrete::Speed maxSpeed = std::max( std::max( ( controls.p1_ - controls.p0_ ).norm( ),
  43                                                                                                                                                                                                         ( controls.p2_ - controls.p1_ ).norm( ) ),
  44                                                                                                                                                                 ( controls.p3_ - controls.p2_ ).norm( ) ).offtype< 0, 1 >( );
  45         const double t_tol = ( Computation::the_arcdelta / maxSpeed ).offtype< 0, 1 >( );
  46         const double t_min = t_tol;
  47         const double t_max = 1 - t_tol;
  48
  49         Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
  50         double t[3];
  51         coeffs.inflections( t );
  52         // Now, we must just ensure that the times are sorted.
  53         double * src = & t[0];
  54         if( *src == HUGE_VAL )
  55                 {
  56                         // There were no inflections.
  57                         return;
  58                 }
  59         ++src;
  60         if( *src == HUGE_VAL )
  61                 {
  62                         // There were one inflections.
  63                         if( t_min < t[0] && t[0] < t_max )
  64                                 {
  65                                         dst->push_back( t[0] );
  66                                 }
  67                         return;
  68                 }
  69         // There were two inflections.
  70         if( t[0] <= t[1] )
  71                 {
  72                         if( t_min < t[0] && t[0] < t_max )
  73                                 {
  74                                         dst->push_back( t[0] );
  75                                 }
  76                         if( t_min < t[1] && t[1] < t_max )
  77                                 {
  78                                         dst->push_back( t[1] );
  79                                 }
  80                 }
  81         else
  82                 {
  83                         if( t_min < t[1] && t[1] < t_max )
  84                                 {
  85                                         dst->push_back( t[1] );
  86                                 }
  87                         if( t_min < t[0] && t[0] < t_max )
  88                                 {
  89                                         dst->push_back( t[0] );
  90                                 }
  91                 }
  92 }
  93
  94 void
  95 Computation::UpsampleBends::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords2D > & controls ) const
  96 {
  97         /* We begin by constructing inflection-free segments, which can then be upsampled at given angles.
  98                  See UpsampleInflections.
  99          */
 100
 101         static Computation::UpsampleInflections inflectionSampler;
 102
 103         if( controls.p1_ == controls.p0_ &&
 104                         controls.p2_ == controls.p3_ )
 105                 {
 106                         // There are no bends on a straight segment.
 107                         return;
 108                 }
 109
 110         Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
 111
 112         std::vector< double > inflectionSamples;
 113         inflectionSampler( & inflectionSamples, controls );
 114         inflectionSamples.push_back( 1 );
 115
 116         Concrete::Coords2D p0 = controls.p0_;
 117         double t1 = 0;
 118         double a1;
 119         Concrete::UnitFloatPair d1( 1, 0, bool( ) );
 120         {
 121                 Concrete::Length shortLength = 1.e-4 * ( controls.p3_ - controls.p0_ ).norm( );
 122                 Concrete::Coords2D tmp = coeffs.velocity( t1 );
 123                 if( tmp.norm( ) < shortLength )
 124                         {
 125                                 tmp = coeffs.acceleration( t1 );
 126                         }
 127                 d1 = tmp.direction( );
 128                 a1 = atan2( d1.y_, d1.x_ );
 129         }
 130
 131         double a2;
 132         double t2;
 133         Concrete::UnitFloatPair d2( 1, 0, bool( ) );
 134         typedef typeof inflectionSamples ListType;
 135         for( ListType::const_iterator inflection_i = inflectionSamples.begin( );
 136                          inflection_i != inflectionSamples.end( );
 137                          ++inflection_i, a1 = a2, t1 = t2, d1 = d2 )
 138                 {
 139                         t2 = *inflection_i;
 140                         Bezier::ControlPoints< Concrete::Coords2D > subControls( coeffs.subSection( t1, t2 ) );
 141                         Concrete::Length shortLength = 1.e-4 * ( subControls.p3_ - subControls.p0_ ).norm( );
 142                         {
 143                                 Concrete::Coords2D tmp = coeffs.velocity( t2 );
 144                                 if( tmp.norm( ) < shortLength )
 145                                         {
 146                                                 tmp = (-1) * coeffs.acceleration( t2 ); // Note the sign!
 147                                         }
 148                                 d2 = tmp.direction( );
 149                                 a2 = atan2( d2.y_, d2.x_ );
 150                         }
 151                         /* Check if the turn is counter clockwise or not.
 152                          * Note that it is tempting to use the acceleration at t1, but this is a bad idea since it is parallel with the
 153                          * velocity at the points of inflection (and t1 will often be such a point).
 154                          */
 155                         bool ccw;
 156                         {
 157                                 Concrete::Length tmp = Concrete::inner( d1.quarterTurnCounterClockwise( ), subControls.p2_ - subControls.p0_ );
 158                                 if( tmp.abs( ) > shortLength )
 159                                         {
 160                                                 ccw = tmp > Concrete::ZERO_LENGTH;
 161                                         }
 162                                 else
 163                                         {
 164                                                 // If p2_ was in line with d1, then we use p3_ instead. As this is our last resort, we use it unconditionally.
 165                                                 ccw = Concrete::inner( d1.quarterTurnCounterClockwise( ), subControls.p3_ - subControls.p0_ ) > Concrete::ZERO_LENGTH;
 166                                         }
 167                         }
 168                         if( ccw )
 169                                 {
 170                                         if( a2 < a1 )
 171                                                 {
 172                                                         a2 += 2 * M_PI;
 173                                                 }
 174                                 }
 175                         else
 176                                 {
 177                                         if( a2 > a1 )
 178                                                 {
 179                                                         a2 -= 2 * M_PI;
 180                                                 }
 181                                 }
 182
 183                         double steps = ceil( fabs( a2 - a1 ) / maxAngle_ );
 184                         double aStep = ( a2 - a1 ) / steps;
 185                         double a = a1 + aStep;
 186                         double lastTime = t1;
 187                         //                      std::cerr << "a1: " << a1*(180/M_PI) << "        a2: " << a2*(180/M_PI) << std::endl ;
 188                         for( double i = 1; i < steps; ++i, a += aStep )
 189                                 {
 190                                         //                                      std::cerr << "  " << a*(180/M_PI) ;
 191                                         /* Locate the angle a by finding a point where the velocity is orthogonal to a vector orthogonal to a.
 192                                          */
 193                                         Concrete::UnitFloatPair da( cos( a ), sin( a ), bool( ) );
 194                                         Concrete::Coords2D an( cos( a + M_PI_2 ), sin( a + M_PI_2 ) );
 195
 196                                         double optTimes[3];
 197                                         coeffs.stationaryPoints( optTimes, an ); // A HUGE_VAL is used as terminator in the result.
 198                                         double best_t = t2;
 199                                         for( double * src = & optTimes[0]; *src != HUGE_VAL; ++src )
 200                                                 {
 201                                                         if( *src <= lastTime ||
 202                                                                         *src >= best_t )
 203                                                                 {
 204                                                                         continue;
 205                                                                 }
 206                                                         Concrete::Coords2D v = coeffs.velocity( *src );
 207                                                         if( Concrete::inner( da, v ) > Concrete::ZERO_LENGTH )
 208                                                                 {
 209                                                                         best_t = *src;
 210                                                                 }
 211                                                 }
 212                                         if( best_t < t2 )
 213                                                 {
 214                                                         dst->push_back( best_t );
 215                                                         lastTime = best_t;
 216                                                 }
 217                                         //                                      std::cerr << "  (" << ( best_t < t2 ) << ")     " << best_t << std::endl ;
 218                                 }
 219                         if( t2 < 1 )
 220                                 {
 221                                         dst->push_back( t2 );
 222                                         //                                      std::cerr << "  final t2: " << t2 << std::endl ;
 223                                 }
 224                 }
 225
 226 }
 227
 228 void
 229 Computation::UpsampleEvery2D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords2D > & controls ) const
 230 {
 231         if( controls.p1_ == controls.p0_ &&
 232                         controls.p2_ == controls.p3_ )
 233                 {
 234                         Concrete::Length totLen = ( controls.p3_ - controls.p0_ ).norm( );
 235
 236                         double steps = ceil( totLen / period_ );
 237                         double sampleInterval = 1 / steps;
 238                         double nextSample = sampleInterval;
 239                         for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 240                                 {
 241                                         dst->push_back( Shapes::straightLineArcTime( nextSample ).offtype< 0, 1 >( ) );
 242                                 }
 243                 }
 244         else
 245                 {
 246                         Concrete::Bezier x0 = controls.p0_.x_.offtype< 0, 3 >( );
 247                         Concrete::Bezier y0 = controls.p0_.y_.offtype< 0, 3 >( );
 248                         Concrete::Bezier x1 = controls.p1_.x_.offtype< 0, 3 >( );
 249                         Concrete::Bezier y1 = controls.p1_.y_.offtype< 0, 3 >( );
 250                         Concrete::Bezier x2 = controls.p2_.x_.offtype< 0, 3 >( );
 251                         Concrete::Bezier y2 = controls.p2_.y_.offtype< 0, 3 >( );
 252                         Concrete::Bezier x3 = controls.p3_.x_.offtype< 0, 3 >( );
 253                         Concrete::Bezier y3 = controls.p3_.y_.offtype< 0, 3 >( );
 254
 255                         const Concrete::Length segLengthBound =
 256                                 ( hypotPhysical( x1-x0, y1-y0 ) + hypotPhysical( x2-x1, y2-y1 ) + hypotPhysical( x3-x2, y3-y2 ) ).offtype< 0, -3 >( );
 257
 258                         if( segLengthBound < period_ )
 259                                 {
 260                                         return;
 261                                 }
 262
 263                         Concrete::Time dt = Shapes::computeDt( segLengthBound );
 264                         Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 265                         for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
 266                                 {
 267                                         Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 268                                         Physical< 0, 2 > kv0 = -3 * tc * tc;
 269                                         Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 270                                         Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 271                                         Physical< 0, 2 > kv3 = 3 * t * t;
 272                                         Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 273                                         Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 274
 275                                         Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
 276                                         tmpSum_l += dl;
 277                                 }
 278                         Concrete::Length totlen = tmpSum_l * dt.offtype< 0, 1 >( );
 279
 280                         double steps = ceil( totlen / period_ );
 281                         Concrete::Length sampleInterval = totlen / steps;
 282                         Concrete::Length nextSample = sampleInterval;
 283                         {
 284                                 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 285                                 Concrete::Time t = Concrete::ZERO_TIME;
 286                                 for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 287                                         {
 288                                                 const Concrete::Length breakDiv_dt = nextSample / dt.offtype< 0, 1 >( );
 289                                                 for( ; ; t += dt )
 290                                                         {
 291                                                                 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 292                                                                 Physical< 0, 2 > kv0 = -3 * tc * tc;
 293                                                                 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 294                                                                 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 295                                                                 Physical< 0, 2 > kv3 = 3 * t * t;
 296                                                                 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 297                                                                 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 298
 299                                                                 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
 300                                                                 tmpSum_l += dl;
 301                                                                 if( tmpSum_l >= breakDiv_dt )
 302                                                                         {
 303                                                                                 dst->push_back( t.offtype< 0, 1 >( ) );
 304                                                                                 break;
 305                                                                         }
 306                                                         }
 307                                         }
 308                                 }
 309                 }
 310 }
 311
 312 void
 313 Computation::UpsampleEvery3D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords3D > & controls ) const
 314 {
 315         if( controls.p1_ == controls.p0_ &&
 316                         controls.p2_ == controls.p3_ )
 317                 {
 318                         Concrete::Length totLen = ( controls.p3_ - controls.p0_ ).norm( );
 319
 320                         double steps = ceil( totLen / period_ );
 321                         double sampleInterval = 1 / steps;
 322                         double nextSample = sampleInterval;
 323                         for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 324                                 {
 325                                         dst->push_back( Shapes::straightLineArcTime( nextSample ).offtype< 0, 1 >( ) );
 326                                 }
 327                 }
 328         else
 329                 {
 330                         Concrete::Bezier x0 = controls.p0_.x_.offtype< 0, 3 >( );
 331                         Concrete::Bezier y0 = controls.p0_.y_.offtype< 0, 3 >( );
 332                         Concrete::Bezier z0 = controls.p0_.z_.offtype< 0, 3 >( );
 333                         Concrete::Bezier x1 = controls.p1_.x_.offtype< 0, 3 >( );
 334                         Concrete::Bezier y1 = controls.p1_.y_.offtype< 0, 3 >( );
 335                         Concrete::Bezier z1 = controls.p1_.z_.offtype< 0, 3 >( );
 336                         Concrete::Bezier x2 = controls.p2_.x_.offtype< 0, 3 >( );
 337                         Concrete::Bezier y2 = controls.p2_.y_.offtype< 0, 3 >( );
 338                         Concrete::Bezier z2 = controls.p2_.z_.offtype< 0, 3 >( );
 339                         Concrete::Bezier x3 = controls.p3_.x_.offtype< 0, 3 >( );
 340                         Concrete::Bezier y3 = controls.p3_.y_.offtype< 0, 3 >( );
 341                         Concrete::Bezier z3 = controls.p3_.z_.offtype< 0, 3 >( );
 342
 343                         const Concrete::Length segLengthBound =
 344                                 ( hypotPhysical( x1-x0, y1-y0, z1-z0 ) + hypotPhysical( x2-x1, y2-y1, z2-z1 ) + hypotPhysical( x3-x2, y3-y2, z3-z2 ) ).offtype< 0, -3 >( );
 345
 346                         if( segLengthBound < period_ )
 347                                 {
 348                                         return;
 349                                 }
 350
 351                         Concrete::Time dt = Shapes::computeDt( segLengthBound );
 352                         Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 353                         for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
 354                                 {
 355                                         Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 356                                         Physical< 0, 2 > kv0 = -3 * tc * tc;
 357                                         Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 358                                         Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 359                                         Physical< 0, 2 > kv3 = 3 * t * t;
 360                                         Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 361                                         Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 362                                         Concrete::Speed vz = z0 * kv0 + z1 * kv1 + z2 * kv2 + z3 * kv3;
 363
 364                                         Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
 365                                         tmpSum_l += dl;
 366                                 }
 367                         Concrete::Length totlen = tmpSum_l * dt.offtype< 0, 1 >( );
 368
 369                         double steps = ceil( totlen / period_ );
 370                         Concrete::Length sampleInterval = totlen / steps;
 371                         Concrete::Length nextSample = sampleInterval;
 372                         {
 373                                 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 374                                 Concrete::Time t = Concrete::ZERO_TIME;
 375                                 for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 376                                         {
 377                                                 const Concrete::Length breakDiv_dt = nextSample / dt.offtype< 0, 1 >( );
 378                                                 for( ; ; t += dt )
 379                                                         {
 380                                                                 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 381                                                                 Physical< 0, 2 > kv0 = -3 * tc * tc;
 382                                                                 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 383                                                                 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 384                                                                 Physical< 0, 2 > kv3 = 3 * t * t;
 385                                                                 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 386                                                                 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 387                                                                 Concrete::Speed vz = z0 * kv0 + z1 * kv1 + z2 * kv2 + z3 * kv3;
 388
 389                                                                 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
 390                                                                 tmpSum_l += dl;
 391                                                                 if( tmpSum_l >= breakDiv_dt )
 392                                                                         {
 393                                                                                 dst->push_back( t.offtype< 0, 1 >( ) );
 394                                                                                 break;
 395                                                                         }
 396                                                         }
 397                                         }
 398                                 }
 399                 }
 400 }
 401
 402 void
 403 Computation::UpsampleDifferentiably2D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords2D > & controls ) const
 404 {
 405         /* I might have shown this some time before, but as I write this I have forgotten if there was a simple argument.       Today, I just
 406          * note that the differentiable sample point is always at 0.5.  I leave the code to reproduce in case someone is curious about this.
 407          */
 408
 409         dst->push_back( 0.5 );
 410
 411 //       if( controls.p1_ == controls.p0_ &&
 412 //                       controls.p2_ == controls.p3_ )
 413 //               {
 414 //                       // By symmetry, we sample the mid point.
 415 //                       dst->push_back( 0.5 );
 416 //                       return;
 417 //               }
 418
 419 //       Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
 420
 421 //       Concrete::Speed maxSpeed = std::max( std::max( ( controls.p1_ - controls.p0_ ).norm( ),
 422 //                                                                                                                                                                                                      ( controls.p2_ - controls.p1_ ).norm( ) ),
 423 //                                                                                                                                                              ( controls.p3_ - controls.p2_ ).norm( ) ).offtype< 0, 1 >( );
 424 //       double t_tol = ( Computation::the_arcdelta / maxSpeed ).offtype< 0, 1 >( );
 425
 426 //       double tLow = 0;
 427 //       double tHigh = 1;
 428
 429 //       while( tHigh - tLow > t_tol )
 430 //               {
 431 //                       double tMid = 0.5 * ( tLow + tHigh );
 432 //                       Concrete::Length speedLow ( coeffs.subSection( 0, tMid ).velocity( 1 ).norm( ) );
 433 //                       Concrete::Length speedHigh( coeffs.subSection( tMid, 1 ).velocity( 0 ).norm( ) );
 434 //                       if( speedLow < speedHigh )
 435 //                               {
 436 //                                       tLow = tMid;
 437 //                               }
 438 //                       else
 439 //                               {
 440 //                                       tHigh = tMid;
 441 //                               }
 442 //               }
 443 //       std::cerr << 0.5 * ( tLow + tHigh ) << std::endl ;
 444 //       dst->push_back( 0.5 * ( tLow + tHigh ) );
 445 }
 446
 447 void
 448 Computation::UpsampleDifferentiably3D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords3D > & controls ) const
 449 {
 450         /* See comments in UpsampleDifferentiably2D.
 451          */
 452         dst->push_back( 0.5 );
 453 }