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                         for( double i = 1; i < steps; ++i, a += aStep )
 188                                 {
 189                                         /* Locate the angle a by finding a point where the velocity is orthogonal to a vector orthogonal to a.
 190                                          */
 191                                         Concrete::UnitFloatPair da( cos( a ), sin( a ), bool( ) );
 192                                         Concrete::Coords2D an( cos( a + M_PI_2 ), sin( a + M_PI_2 ) );
 193
 194                                         double optTimes[3];
 195                                         coeffs.stationaryPoints( optTimes, an ); // A HUGE_VAL is used as terminator in the result.
 196                                         double best_t = t2;
 197                                         for( double * src = & optTimes[0]; *src != HUGE_VAL; ++src )
 198                                                 {
 199                                                         if( *src <= lastTime ||
 200                                                                         *src >= best_t )
 201                                                                 {
 202                                                                         continue;
 203                                                                 }
 204                                                         Concrete::Coords2D v = coeffs.velocity( *src );
 205                                                         if( Concrete::inner( da, v ) > Concrete::ZERO_LENGTH )
 206                                                                 {
 207                                                                         best_t = *src;
 208                                                                 }
 209                                                 }
 210                                         if( best_t < t2 )
 211                                                 {
 212                                                         dst->push_back( best_t );
 213                                                         lastTime = best_t;
 214                                                 }
 215                                 }
 216                         if( t2 < 1 )
 217                                 {
 218                                         dst->push_back( t2 );
 219                                 }
 220                 }
 221
 222 }
 223
 224 void
 225 Computation::UpsampleEvery2D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords2D > & controls ) const
 226 {
 227         if( controls.p1_ == controls.p0_ &&
 228                         controls.p2_ == controls.p3_ )
 229                 {
 230                         Concrete::Length totLen = ( controls.p3_ - controls.p0_ ).norm( );
 231
 232                         double steps = ceil( totLen / period_ );
 233                         double sampleInterval = 1 / steps;
 234                         double nextSample = sampleInterval;
 235                         for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 236                                 {
 237                                         dst->push_back( Shapes::straightLineArcTime( nextSample ).offtype< 0, 1 >( ) );
 238                                 }
 239                 }
 240         else
 241                 {
 242                         Concrete::Bezier x0 = controls.p0_.x_.offtype< 0, 3 >( );
 243                         Concrete::Bezier y0 = controls.p0_.y_.offtype< 0, 3 >( );
 244                         Concrete::Bezier x1 = controls.p1_.x_.offtype< 0, 3 >( );
 245                         Concrete::Bezier y1 = controls.p1_.y_.offtype< 0, 3 >( );
 246                         Concrete::Bezier x2 = controls.p2_.x_.offtype< 0, 3 >( );
 247                         Concrete::Bezier y2 = controls.p2_.y_.offtype< 0, 3 >( );
 248                         Concrete::Bezier x3 = controls.p3_.x_.offtype< 0, 3 >( );
 249                         Concrete::Bezier y3 = controls.p3_.y_.offtype< 0, 3 >( );
 250
 251                         const Concrete::Length segLengthBound =
 252                                 ( hypotPhysical( x1-x0, y1-y0 ) + hypotPhysical( x2-x1, y2-y1 ) + hypotPhysical( x3-x2, y3-y2 ) ).offtype< 0, -3 >( );
 253
 254                         if( segLengthBound < period_ )
 255                                 {
 256                                         return;
 257                                 }
 258
 259                         Concrete::Time dt = Shapes::computeDt( segLengthBound );
 260                         Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 261                         for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
 262                                 {
 263                                         Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 264                                         Physical< 0, 2 > kv0 = -3 * tc * tc;
 265                                         Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 266                                         Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 267                                         Physical< 0, 2 > kv3 = 3 * t * t;
 268                                         Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 269                                         Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 270
 271                                         Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
 272                                         tmpSum_l += dl;
 273                                 }
 274                         Concrete::Length totlen = tmpSum_l * dt.offtype< 0, 1 >( );
 275
 276                         double steps = ceil( totlen / period_ );
 277                         Concrete::Length sampleInterval = totlen / steps;
 278                         Concrete::Length nextSample = sampleInterval;
 279                         {
 280                                 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 281                                 Concrete::Time t = Concrete::ZERO_TIME;
 282                                 for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 283                                         {
 284                                                 const Concrete::Length breakDiv_dt = nextSample / dt.offtype< 0, 1 >( );
 285                                                 for( ; ; t += dt )
 286                                                         {
 287                                                                 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 288                                                                 Physical< 0, 2 > kv0 = -3 * tc * tc;
 289                                                                 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 290                                                                 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 291                                                                 Physical< 0, 2 > kv3 = 3 * t * t;
 292                                                                 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 293                                                                 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 294
 295                                                                 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
 296                                                                 tmpSum_l += dl;
 297                                                                 if( tmpSum_l >= breakDiv_dt )
 298                                                                         {
 299                                                                                 dst->push_back( t.offtype< 0, 1 >( ) );
 300                                                                                 break;
 301                                                                         }
 302                                                         }
 303                                         }
 304                                 }
 305                 }
 306 }
 307
 308 void
 309 Computation::UpsampleEvery3D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords3D > & controls ) const
 310 {
 311         if( controls.p1_ == controls.p0_ &&
 312                         controls.p2_ == controls.p3_ )
 313                 {
 314                         Concrete::Length totLen = ( controls.p3_ - controls.p0_ ).norm( );
 315
 316                         double steps = ceil( totLen / period_ );
 317                         double sampleInterval = 1 / steps;
 318                         double nextSample = sampleInterval;
 319                         for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 320                                 {
 321                                         dst->push_back( Shapes::straightLineArcTime( nextSample ).offtype< 0, 1 >( ) );
 322                                 }
 323                 }
 324         else
 325                 {
 326                         Concrete::Bezier x0 = controls.p0_.x_.offtype< 0, 3 >( );
 327                         Concrete::Bezier y0 = controls.p0_.y_.offtype< 0, 3 >( );
 328                         Concrete::Bezier z0 = controls.p0_.z_.offtype< 0, 3 >( );
 329                         Concrete::Bezier x1 = controls.p1_.x_.offtype< 0, 3 >( );
 330                         Concrete::Bezier y1 = controls.p1_.y_.offtype< 0, 3 >( );
 331                         Concrete::Bezier z1 = controls.p1_.z_.offtype< 0, 3 >( );
 332                         Concrete::Bezier x2 = controls.p2_.x_.offtype< 0, 3 >( );
 333                         Concrete::Bezier y2 = controls.p2_.y_.offtype< 0, 3 >( );
 334                         Concrete::Bezier z2 = controls.p2_.z_.offtype< 0, 3 >( );
 335                         Concrete::Bezier x3 = controls.p3_.x_.offtype< 0, 3 >( );
 336                         Concrete::Bezier y3 = controls.p3_.y_.offtype< 0, 3 >( );
 337                         Concrete::Bezier z3 = controls.p3_.z_.offtype< 0, 3 >( );
 338
 339                         const Concrete::Length segLengthBound =
 340                                 ( hypotPhysical( x1-x0, y1-y0, z1-z0 ) + hypotPhysical( x2-x1, y2-y1, z2-z1 ) + hypotPhysical( x3-x2, y3-y2, z3-z2 ) ).offtype< 0, -3 >( );
 341
 342                         if( segLengthBound < period_ )
 343                                 {
 344                                         return;
 345                                 }
 346
 347                         Concrete::Time dt = Shapes::computeDt( segLengthBound );
 348                         Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 349                         for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
 350                                 {
 351                                         Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 352                                         Physical< 0, 2 > kv0 = -3 * tc * tc;
 353                                         Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 354                                         Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 355                                         Physical< 0, 2 > kv3 = 3 * t * t;
 356                                         Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 357                                         Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 358                                         Concrete::Speed vz = z0 * kv0 + z1 * kv1 + z2 * kv2 + z3 * kv3;
 359
 360                                         Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
 361                                         tmpSum_l += dl;
 362                                 }
 363                         Concrete::Length totlen = tmpSum_l * dt.offtype< 0, 1 >( );
 364
 365                         double steps = ceil( totlen / period_ );
 366                         Concrete::Length sampleInterval = totlen / steps;
 367                         Concrete::Length nextSample = sampleInterval;
 368                         {
 369                                 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
 370                                 Concrete::Time t = Concrete::ZERO_TIME;
 371                                 for( double i = 1; i < steps; ++i, nextSample += sampleInterval )
 372                                         {
 373                                                 const Concrete::Length breakDiv_dt = nextSample / dt.offtype< 0, 1 >( );
 374                                                 for( ; ; t += dt )
 375                                                         {
 376                                                                 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
 377                                                                 Physical< 0, 2 > kv0 = -3 * tc * tc;
 378                                                                 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
 379                                                                 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
 380                                                                 Physical< 0, 2 > kv3 = 3 * t * t;
 381                                                                 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
 382                                                                 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
 383                                                                 Concrete::Speed vz = z0 * kv0 + z1 * kv1 + z2 * kv2 + z3 * kv3;
 384
 385                                                                 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
 386                                                                 tmpSum_l += dl;
 387                                                                 if( tmpSum_l >= breakDiv_dt )
 388                                                                         {
 389                                                                                 dst->push_back( t.offtype< 0, 1 >( ) );
 390                                                                                 break;
 391                                                                         }
 392                                                         }
 393                                         }
 394                                 }
 395                 }
 396 }
 397
 398 void
 399 Computation::UpsampleDifferentiably2D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords2D > & controls ) const
 400 {
 401         /* 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
 402          * note that the differentiable sample point is always at 0.5.  I leave the code to reproduce in case someone is curious about this.
 403          */
 404
 405         dst->push_back( 0.5 );
 406
 407 //       if( controls.p1_ == controls.p0_ &&
 408 //                       controls.p2_ == controls.p3_ )
 409 //               {
 410 //                       // By symmetry, we sample the mid point.
 411 //                       dst->push_back( 0.5 );
 412 //                       return;
 413 //               }
 414
 415 //       Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
 416
 417 //       Concrete::Speed maxSpeed = std::max( std::max( ( controls.p1_ - controls.p0_ ).norm( ),
 418 //                                                                                                                                                                                                      ( controls.p2_ - controls.p1_ ).norm( ) ),
 419 //                                                                                                                                                              ( controls.p3_ - controls.p2_ ).norm( ) ).offtype< 0, 1 >( );
 420 //       double t_tol = ( Computation::the_arcdelta / maxSpeed ).offtype< 0, 1 >( );
 421
 422 //       double tLow = 0;
 423 //       double tHigh = 1;
 424
 425 //       while( tHigh - tLow > t_tol )
 426 //               {
 427 //                       double tMid = 0.5 * ( tLow + tHigh );
 428 //                       Concrete::Length speedLow ( coeffs.subSection( 0, tMid ).velocity( 1 ).norm( ) );
 429 //                       Concrete::Length speedHigh( coeffs.subSection( tMid, 1 ).velocity( 0 ).norm( ) );
 430 //                       if( speedLow < speedHigh )
 431 //                               {
 432 //                                       tLow = tMid;
 433 //                               }
 434 //                       else
 435 //                               {
 436 //                                       tHigh = tMid;
 437 //                               }
 438 //               }
 439 //       dst->push_back( 0.5 * ( tLow + tHigh ) );
 440 }
 441
 442 void
 443 Computation::UpsampleDifferentiably3D::operator () ( std::vector< double > * dst, const Bezier::ControlPoints< Concrete::Coords3D > & controls ) const
 444 {
 445         /* See comments in UpsampleDifferentiably2D.
 446          */
 447         dst->push_back( 0.5 );
 448 }