1 /* This file is part of Shapes.
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
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.
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/>.
16 * Copyright 2008 Henrik Tidefelt
19 #include "upsamplers.h"
21 #include "pathtypes.h"
27 using namespace Shapes
;
30 Computation::UpsampleInflections::operator () ( std::vector
< double > * dst
, const Bezier::ControlPoints
< Concrete::Coords2D
> & controls
) const
32 if( controls
.p1_
== controls
.p0_
&&
33 controls
.p2_
== controls
.p3_
)
35 // There are no inflections on a straight segment.
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.
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
;
49 Bezier::PolyCoeffs
< Concrete::Coords2D
> coeffs( controls
);
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
)
56 // There were no inflections.
60 if( *src
== HUGE_VAL
)
62 // There were one inflections.
63 if( t_min
< t
[0] && t
[0] < t_max
)
65 dst
->push_back( t
[0] );
69 // There were two inflections.
72 if( t_min
< t
[0] && t
[0] < t_max
)
74 dst
->push_back( t
[0] );
76 if( t_min
< t
[1] && t
[1] < t_max
)
78 dst
->push_back( t
[1] );
83 if( t_min
< t
[1] && t
[1] < t_max
)
85 dst
->push_back( t
[1] );
87 if( t_min
< t
[0] && t
[0] < t_max
)
89 dst
->push_back( t
[0] );
95 Computation::UpsampleBends::operator () ( std::vector
< double > * dst
, const Bezier::ControlPoints
< Concrete::Coords2D
> & controls
) const
97 /* We begin by constructing inflection-free segments, which can then be upsampled at given angles.
98 See UpsampleInflections.
101 static Computation::UpsampleInflections inflectionSampler
;
103 if( controls
.p1_
== controls
.p0_
&&
104 controls
.p2_
== controls
.p3_
)
106 // There are no bends on a straight segment.
110 Bezier::PolyCoeffs
< Concrete::Coords2D
> coeffs( controls
);
112 std::vector
< double > inflectionSamples
;
113 inflectionSampler( & inflectionSamples
, controls
);
114 inflectionSamples
.push_back( 1 );
116 Concrete::Coords2D p0
= controls
.p0_
;
119 Concrete::UnitFloatPair
d1( 1, 0, bool( ) );
121 Concrete::Length shortLength
= 1.e
-4 * ( controls
.p3_
- controls
.p0_
).norm( );
122 Concrete::Coords2D tmp
= coeffs
.velocity( t1
);
123 if( tmp
.norm( ) < shortLength
)
125 tmp
= coeffs
.acceleration( t1
);
127 d1
= tmp
.direction( );
128 a1
= atan2( d1
.y_
, d1
.x_
);
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
)
140 Bezier::ControlPoints
< Concrete::Coords2D
> subControls( coeffs
.subSection( t1
, t2
) );
141 Concrete::Length shortLength
= 1.e
-4 * ( subControls
.p3_
- subControls
.p0_
).norm( );
143 Concrete::Coords2D tmp
= coeffs
.velocity( t2
);
144 if( tmp
.norm( ) < shortLength
)
146 tmp
= (-1) * coeffs
.acceleration( t2
); // Note the sign!
148 d2
= tmp
.direction( );
149 a2
= atan2( d2
.y_
, d2
.x_
);
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).
157 Concrete::Length tmp
= Concrete::inner( d1
.quarterTurnCounterClockwise( ), subControls
.p2_
- subControls
.p0_
);
158 if( tmp
.abs( ) > shortLength
)
160 ccw
= tmp
> Concrete::ZERO_LENGTH
;
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
;
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
)
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.
193 Concrete::UnitFloatPair
da( cos( a
), sin( a
), bool( ) );
194 Concrete::Coords2D
an( cos( a
+ M_PI_2
), sin( a
+ M_PI_2
) );
197 coeffs
.stationaryPoints( optTimes
, an
); // A HUGE_VAL is used as terminator in the result.
199 for( double * src
= & optTimes
[0]; *src
!= HUGE_VAL
; ++src
)
201 if( *src
<= lastTime
||
206 Concrete::Coords2D v
= coeffs
.velocity( *src
);
207 if( Concrete::inner( da
, v
) > Concrete::ZERO_LENGTH
)
214 dst
->push_back( best_t
);
217 // std::cerr << " (" << ( best_t < t2 ) << ") " << best_t << std::endl ;
221 dst
->push_back( t2
);
222 // std::cerr << " final t2: " << t2 << std::endl ;
229 Computation::UpsampleEvery2D::operator () ( std::vector
< double > * dst
, const Bezier::ControlPoints
< Concrete::Coords2D
> & controls
) const
231 if( controls
.p1_
== controls
.p0_
&&
232 controls
.p2_
== controls
.p3_
)
234 Concrete::Length totLen
= ( controls
.p3_
- controls
.p0_
).norm( );
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
)
241 dst
->push_back( Shapes::straightLineArcTime( nextSample
).offtype
< 0, 1 >( ) );
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 >( );
255 const Concrete::Length segLengthBound
=
256 ( hypotPhysical( x1
-x0
, y1
-y0
) + hypotPhysical( x2
-x1
, y2
-y1
) + hypotPhysical( x3
-x2
, y3
-y2
) ).offtype
< 0, -3 >( );
258 if( segLengthBound
< period_
)
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
)
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
;
275 Concrete::Length dl
= hypotPhysical( vx
, vy
).offtype
< 0, -1 >( );
278 Concrete::Length totlen
= tmpSum_l
* dt
.offtype
< 0, 1 >( );
280 double steps
= ceil( totlen
/ period_
);
281 Concrete::Length sampleInterval
= totlen
/ steps
;
282 Concrete::Length nextSample
= sampleInterval
;
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
)
288 const Concrete::Length breakDiv_dt
= nextSample
/ dt
.offtype
< 0, 1 >( );
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
;
299 Concrete::Length dl
= hypotPhysical( vx
, vy
).offtype
< 0, -1 >( );
301 if( tmpSum_l
>= breakDiv_dt
)
303 dst
->push_back( t
.offtype
< 0, 1 >( ) );
313 Computation::UpsampleEvery3D::operator () ( std::vector
< double > * dst
, const Bezier::ControlPoints
< Concrete::Coords3D
> & controls
) const
315 if( controls
.p1_
== controls
.p0_
&&
316 controls
.p2_
== controls
.p3_
)
318 Concrete::Length totLen
= ( controls
.p3_
- controls
.p0_
).norm( );
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
)
325 dst
->push_back( Shapes::straightLineArcTime( nextSample
).offtype
< 0, 1 >( ) );
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 >( );
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 >( );
346 if( segLengthBound
< period_
)
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
)
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
;
364 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
367 Concrete::Length totlen
= tmpSum_l
* dt
.offtype
< 0, 1 >( );
369 double steps
= ceil( totlen
/ period_
);
370 Concrete::Length sampleInterval
= totlen
/ steps
;
371 Concrete::Length nextSample
= sampleInterval
;
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
)
377 const Concrete::Length breakDiv_dt
= nextSample
/ dt
.offtype
< 0, 1 >( );
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
;
389 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
391 if( tmpSum_l
>= breakDiv_dt
)
393 dst
->push_back( t
.offtype
< 0, 1 >( ) );
403 Computation::UpsampleDifferentiably2D::operator () ( std::vector
< double > * dst
, const Bezier::ControlPoints
< Concrete::Coords2D
> & controls
) const
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.
409 dst
->push_back( 0.5 );
411 // if( controls.p1_ == controls.p0_ &&
412 // controls.p2_ == controls.p3_ )
414 // // By symmetry, we sample the mid point.
415 // dst->push_back( 0.5 );
419 // Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
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 >( );
429 // while( tHigh - tLow > t_tol )
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 )
443 // std::cerr << 0.5 * ( tLow + tHigh ) << std::endl ;
444 // dst->push_back( 0.5 * ( tLow + tHigh ) );
448 Computation::UpsampleDifferentiably3D::operator () ( std::vector
< double > * dst
, const Bezier::ControlPoints
< Concrete::Coords3D
> & controls
) const
450 /* See comments in UpsampleDifferentiably2D.
452 dst
->push_back( 0.5 );