?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Update suitable examples and tests to use blank mode
[shapes.git] / source / elementarypath2D.cc
blob10462d880aac5bbf4b8e52e3ee50928cc7cace76
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, 2009 Henrik Tidefelt
17 */
18
19 #include <cmath>
20
21 #include "shapestypes.h"
22 #include "shapesexceptions.h"
23 #include "astexpr.h"
24 #include "consts.h"
25 #include "globals.h"
26 #include "angleselect.h"
27 #include "bezier.h"
28 #include "upsamplers.h"
29 #include "quadratic_programs.h"
30 #include "constructorrepresentation.h"
31
32 #include <ctype.h>
33 #include <stack>
34 #include <algorithm>
35
36 using namespace Shapes;
37 using namespace std;
38
39 namespace Bezier
40 {
41 template< >
42 void
43 ControlPoints< Concrete::Coords2D >::get( double * dst ) const
44 {
45 *dst = Concrete::Length::offtype( p0_.x_ );
46 ++dst;
47 *dst = Concrete::Length::offtype( p0_.y_ );
48 ++dst;
49 *dst = Concrete::Length::offtype( p1_.x_ );
50 ++dst;
51 *dst = Concrete::Length::offtype( p1_.y_ );
52 ++dst;
53 *dst = Concrete::Length::offtype( p2_.x_ );
54 ++dst;
55 *dst = Concrete::Length::offtype( p2_.y_ );
56 ++dst;
57 *dst = Concrete::Length::offtype( p3_.x_ );
58 ++dst;
59 *dst = Concrete::Length::offtype( p3_.y_ );
60 }
61
62 template< >
63 void
64 ControlPoints< Concrete::Coords2D >::getEndpoints( double * dst ) const
65 {
66 *dst = Concrete::Length::offtype( p0_.x_ );
67 ++dst;
68 *dst = Concrete::Length::offtype( p0_.y_ );
69 ++dst;
70 *dst = Concrete::Length::offtype( p3_.x_ );
71 ++dst;
72 *dst = Concrete::Length::offtype( p3_.y_ );
73 }
74 }
75
76 inline
77 bool
78 isinfPhysical( const double & p1 )
79 {
80 return isinf( p1 );
81 }
82
83 inline
84 double
85 modPhysical( const double & p1, const double & p2 )
86 {
87 return fmod( p1, p2 );
88 }
89
90 Lang::ElementaryPath2D::ElementaryPath2D( )
91 : allComplete_( true )
92 { }
93
94 DISPATCHIMPL( ElementaryPath2D );
95
96 Lang::ElementaryPath2D::~ElementaryPath2D( )
97 { }
98
99 Kernel::VariableHandle
100 Lang::ElementaryPath2D::getField( const char * fieldID, const RefCountPtr< const Lang::Value > & selfRef ) const
101 {
102 return getField( fieldID, selfRef.down_cast< const Lang::ElementaryPath2D >( ) );
103 }
104
105 Kernel::VariableHandle
106 Lang::ElementaryPath2D::getField( const char * fieldID, const RefCountPtr< const Lang::ElementaryPath2D > & selfRef ) const
107 {
108 if( strcmp( fieldID, "begin" ) == 0 )
109 {
110 return Helpers::newValHandle( new Lang::PathSlider2D( selfRef, Concrete::ZERO_TIME ) );
111 }
112 if( strcmp( fieldID, "end" ) == 0 )
113 {
114 return Helpers::newValHandle( new Lang::PathSlider2D( selfRef, Concrete::Time( duration( ) ) ) );
115 }
116 if( strcmp( fieldID, "closed?" ) == 0 )
117 {
118 if( closed_ )
119 {
120 return Kernel::THE_TRUE_VARIABLE;
121 }
122 return Kernel::THE_FALSE_VARIABLE;
123 }
124 if( strcmp( fieldID, "null?" ) == 0 )
125 {
126 if( empty( ) )
127 {
128 return Kernel::THE_TRUE_VARIABLE;
129 }
130 return Kernel::THE_FALSE_VARIABLE;
131 }
132 throw Exceptions::NonExistentMember( getTypeName( ), fieldID );
133 }
134
135 void
136 Lang::ElementaryPath2D::writePath( ostream & os ) const
137 {
138 const_iterator i = begin( );
139 if( i == end( ) )
140 {
141 throw Exceptions::InternalError( "Invoking writePath on the empty path." );
142 }
143 const Concrete::Coords2D * p0 = (*i)->mid_;
144 os << *p0 << " m " ;
145 const Concrete::Coords2D * p1 = (*i)->front_;
146 ++i;
147 for( ; i != end( ); ++i )
148 {
149 const Concrete::Coords2D * p2 = (*i)->rear_;
150 const Concrete::Coords2D * p3 = (*i)->mid_;
151 if( p1 != p0 && p2 != p3 )
152 {
153 os << *p1 << " " << *p2 << " " << *p3 << " c " ;
154 }
155 else if( p1 != p0 )
156 {
157 os << *p1 << " " << *p3 << " y " ;
158 }
159 else if( p2 != p3 )
160 {
161 os << *p2 << " " << *p3 << " v " ;
162 }
163 else
164 {
165 os << *p3 << " l " ;
166 }
167 p0 = p3;
168 p1 = (*i)->front_;
169 }
170 if( closed_ )
171 {
172 i = begin( );
173 const Concrete::Coords2D * p2 = (*i)->rear_;
174 const Concrete::Coords2D * p3 = (*i)->mid_;
175 if( p1 != p0 && p2 != p3 )
176 {
177 os << *p1 << " " << *p2 << " " << *p3 << " c " ;
178 }
179 else if( p1 != p0 )
180 {
181 os << *p1 << " " << *p3 << " y " ;
182 }
183 else if( p2 != p3 )
184 {
185 os << *p2 << " " << *p3 << " v " ;
186 }
187 /* In case there is no handle along this segment, it is drawn by the h operator itself.
188 */
189 os << "h " ;
190 }
191 }
192
193 void
194 Lang::ElementaryPath2D::writeInputForm( ostream & os ) const
195 {
196 for( const_iterator i = begin( ); i != end( ); ++i )
197 {
198 if( i != begin( ) )
199 {
200 os << "--" ;
201 }
202 if( (*i)->rear_ != (*i)->mid_ )
203 {
204 os << Helpers::shapesFormat( *(*i)->rear_ ) << "<" ;
205 }
206 os << Helpers::shapesFormat( *(*i)->mid_ );
207 if( (*i)->front_ != (*i)->mid_ )
208 {
209 os << ">" << Helpers::shapesFormat( *(*i)->front_ );
210 }
211 }
212 if( closed_ )
213 {
214 os << "--cycle" ;
215 }
216 }
217
218 RefCountPtr< const Lang::ElementaryPath2D >
219 Lang::ElementaryPath2D::elementaryTransformed( const Lang::Transform2D & tf ) const
220 {
221 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D;
222 if( closed_ )
223 {
224 res->close( );
225 }
226 for( const_iterator i = begin( ); i != end( ); ++i )
227 {
228 res->push_back( (*i)->transformed( tf ) );
229 }
230 return RefCountPtr< const Lang::ElementaryPath2D >( res );
231 }
232
233 RefCountPtr< const Lang::ElementaryPath3D >
234 Lang::ElementaryPath2D::elementaryTransformed( const Lang::Transform3D & tf ) const
235 {
236 Lang::ElementaryPath3D * res = new Lang::ElementaryPath3D;
237 if( closed_ )
238 {
239 res->close( );
240 }
241 for( const_iterator i = begin( ); i != end( ); ++i )
242 {
243 res->push_back( (*i)->transformed( tf ) );
244 }
245 return RefCountPtr< const Lang::ElementaryPath3D >( res );
246 }
247
248 RefCountPtr< const Lang::Path2D >
249 Lang::ElementaryPath2D::typed_transformed( const Lang::Transform2D & tf ) const
250 {
251 return elementaryTransformed( tf );
252 }
253
254 void
255 Lang::ElementaryPath2D::elementaryJob( std::stack< const Lang::Value * > * nodeStack, Lang::ElementaryPath2D * pth, Concrete::Coords2D * basePoint ) const
256 {
257 for( const_iterator i = begin( ); i != end( ); ++i )
258 {
259 pth->push_back( new Concrete::PathPoint2D( **i ) );
260 }
261 if( ! empty( ) )
262 {
263 *basePoint = *(back( )->mid_);
264 }
265 }
266
267 Concrete::Time
268 Lang::ElementaryPath2D::timeCheck( Concrete::Time t ) const
269 {
270 if( closed_ )
271 {
272 if( isinfPhysical( t ) )
273 {
274 throw Exceptions::OutOfRange( "The time argument must not be infinite for closed_ paths." );
275 }
276 Concrete::Time tMax( duration( ) );
277 t = modPhysical( t, tMax );
278 if( t < Concrete::ZERO_TIME )
279 {
280 t += tMax;
281 }
282 }
283 else
284 {
285 if( t < Concrete::ZERO_TIME )
286 {
287 throw Exceptions::OutOfRange( "The time argument must be non-negative for open paths." );
288 }
289 if( t >= Concrete::HUGE_TIME )
290 {
291 t = Concrete::Time( duration( ) );
292 }
293 else if( t > Concrete::Time( duration( ) ) )
294 {
295 std::ostringstream msg;
296 msg << "The time argument must not be greater than the duration of the path: "
297 << Concrete::Time::offtype( t ) << " > " << duration( ) << "." ;
298 throw Exceptions::OutOfRange( strrefdup( msg ) );
299 }
300 }
301 return t;
302 }
303
304 void
305 Lang::ElementaryPath2D::findSegment( Concrete::Time t, Concrete::Time * tRes, const Concrete::PathPoint2D ** p1, const Concrete::PathPoint2D ** p2 ) const
306 {
307 t = timeCheck( t );
308 const_iterator i = begin( );
309 for( ; t >= Concrete::UNIT_TIME; t -= Concrete::UNIT_TIME, ++i )
310 { }
311 *tRes = t;
312 if( i == end( ) )
313 {
314 i = begin( );
315 }
316 *p1 = *i;
317
318 ++i;
319 if( i == end( ) )
320 {
321 i = begin( );
322 }
323 *p2 = *i;
324 }
325
326 void
327 Lang::ElementaryPath2D::findSegmentReverse( Concrete::Time t, Concrete::Time * tRes, const Concrete::PathPoint2D ** p1, const Concrete::PathPoint2D ** p2 ) const
328 {
329 t = timeCheck( t );
330
331 const_iterator i = begin( );
332 for( ; t > Concrete::ZERO_TIME; t -= Concrete::UNIT_TIME, ++i )
333 { }
334 *tRes = t + Concrete::UNIT_TIME;
335 if( i == end( ) )
336 {
337 i = begin( );
338 }
339 *p2 = *i;
340 if( i == begin( ) )
341 {
342 i = end( );
343 }
344 --i;
345 *p1 = *i;
346 }
347
348 RefCountPtr< const Lang::Coords2D >
349 Lang::ElementaryPath2D::point( Concrete::Time globalTime ) const
350 {
351 if( empty( ) )
352 {
353 throw Exceptions::OutOfRange( "The empty path has no points at all." );
354 }
355 Concrete::Time t;
356 const Concrete::PathPoint2D * p1;
357 const Concrete::PathPoint2D * p2;
358 findSegment( globalTime, &t, &p1, &p2 );
359
360 if( t == Concrete::ZERO_TIME )
361 {
362 return RefCountPtr< const Lang::Coords2D >( new Lang::Coords2D( * p1->mid_ ) );
363 }
364 if( t == Concrete::UNIT_TIME )
365 {
366 return RefCountPtr< const Lang::Coords2D >( new Lang::Coords2D( * p2->mid_ ) );
367 }
368
369 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
370 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
371 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
372 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
373 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
374 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
375 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
376 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
377
378 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
379 Physical< 0, 3 > k0 = tc * tc * tc;
380 Physical< 0, 3 > k1 = Concrete::Scalar( 3 ) * tc * tc * t;
381 Physical< 0, 3 > k2 = Concrete::Scalar( 3 ) * tc * t * t;
382 Physical< 0, 3 > k3 = t * t * t;
383 Concrete::Length x = x0 * k0 + x1 * k1 + x2 * k2 + x3 * k3;
384 Concrete::Length y = y0 * k0 + y1 * k1 + y2 * k2 + y3 * k3;
385
386 return RefCountPtr< const Lang::Coords2D >( new Lang::Coords2D( x, y ) );
387 }
388
389 RefCountPtr< const Lang::Length >
390 Lang::ElementaryPath2D::speed( Concrete::Time globalTime ) const
391 {
392 if( empty( ) )
393 {
394 throw Exceptions::OutOfRange( "The empty path has no speed at all." );
395 }
396 Concrete::Time t;
397 const Concrete::PathPoint2D * p1;
398 const Concrete::PathPoint2D * p2;
399 findSegment( globalTime, &t, &p1, &p2 );
400
401 if( t <= Concrete::ZERO_TIME )
402 {
403 /* here, t = 0, tc == 1, so
404 */
405 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
406 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
407 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
408 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
409 Concrete::Speed vx = ( x0 * (-3) + x1 * 3 ).offtype< 0, -2 >( );
410 Concrete::Speed vy = ( y0 * (-3) + y1 * 3 ).offtype< 0, -2 >( );
411 // Concrete::Speed vx = x1 - x0;
412 // Concrete::Speed vy = y1 - y0;
413 return RefCountPtr< const Lang::Length >( new Lang::Length( hypotPhysical( vx, vy ).offtype< 0, -1 >( ) ) );
414 }
415 if( t >= Concrete::UNIT_TIME )
416 {
417 throw Exceptions::InternalError( "t1 == 1 in speed calculation" );
418 }
419
420 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
421 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
422 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
423 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
424 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
425 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
426 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
427 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
428
429 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
430 Physical< 0, 2 > kv0 = -3 * tc * tc;
431 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
432 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
433 Physical< 0, 2 > kv3 = 3 * t * t;
434 // Physical< 0, 2 > kv0 = - tc * tc;
435 // Physical< 0, 2 > kv1 = tc * tc - 2 * tc * t;
436 // Physical< 0, 2 > kv2 = 2 * tc * t - t * t;
437 // Physical< 0, 2 > kv3 = t * t;
438 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
439 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
440
441 return RefCountPtr< const Lang::Length >( new Lang::Length( hypotPhysical( vx, vy ).offtype< 0, -1 >( ) ) );
442 }
443
444 RefCountPtr< const Lang::Length >
445 Lang::ElementaryPath2D::reverse_speed( Concrete::Time globalTime ) const
446 {
447 if( empty( ) )
448 {
449 throw Exceptions::OutOfRange( "The empty path has no speed at all." );
450 }
451 Concrete::Time t;
452 const Concrete::PathPoint2D * p1;
453 const Concrete::PathPoint2D * p2;
454 findSegmentReverse( globalTime, &t, &p1, &p2 );
455
456 if( t <= Concrete::ZERO_TIME )
457 {
458 throw Exceptions::InternalError( "t1 == 0 in reverse speed calculation" );
459 }
460 if( t >= Concrete::UNIT_TIME )
461 {
462 /* here, t = 1, tc == 0, so
463 */
464 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
465 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
466 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
467 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
468 Concrete::Speed vx = ( x2 * (-3) + x3 * 3 ).offtype< 0, -2 >( );
469 Concrete::Speed vy = ( y2 * (-3) + y3 * 3 ).offtype< 0, -2 >( );
470 // Concrete::Speed vx = x3 - x2;
471 // Concrete::Speed vy = y3 - y2;
472 return RefCountPtr< const Lang::Length >( new Lang::Length( hypotPhysical( vx, vy ).offtype< 0, -1 >( ) ) );
473 }
474
475 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
476 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
477 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
478 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
479 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
480 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
481 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
482 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
483
484 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
485 Physical< 0, 2 > kv0 = -3 * tc * tc;
486 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
487 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
488 Physical< 0, 2 > kv3 = 3 * t * t;
489 // Physical< 0, 2 > kv0 = - tc * tc;
490 // Physical< 0, 2 > kv1 = tc * tc - 2 * tc * t;
491 // Physical< 0, 2 > kv2 = 2 * tc * t - t * t;
492 // Physical< 0, 2 > kv3 = t * t;
493 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
494 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
495
496 return RefCountPtr< const Lang::Length >( new Lang::Length( hypotPhysical( vx, vy ).offtype< 0, -1 >( ) ) );
497 }
498
499 RefCountPtr< const Lang::FloatPair >
500 Lang::ElementaryPath2D::direction( Concrete::Time globalTime ) const
501 {
502 if( empty( ) )
503 {
504 throw Exceptions::OutOfRange( "The empty path has no directions at all." );
505 }
506 Concrete::Time t;
507 const Concrete::PathPoint2D * p1;
508 const Concrete::PathPoint2D * p2;
509 findSegment( globalTime, &t, &p1, &p2 );
510
511 Bezier::ControlPoints< Concrete::Coords2D > controls( *(p1->mid_), *(p1->front_), *(p2->rear_), *(p2->mid_) );
512 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
513
514 const Concrete::Length segLengthBound =
515 ( controls.p1_ - controls.p0_ ).norm( ) + ( controls.p2_ - controls.p1_ ).norm( ) + ( controls.p3_ - controls.p2_ ).norm( );
516 const Concrete::Length shortLength = 1.e-4 * segLengthBound;
517
518 {
519 Concrete::Coords2D d = coeffs.velocity( t.offtype< 0, 1 >( ) );
520 Concrete::Length v = d.norm( );
521 if( v > shortLength )
522 {
523 return RefCountPtr< const Lang::FloatPair >( new Lang::FloatPair( d.direction( v ) ) );
524 }
525 }
526
527 /* We reach here if the velocity more or less was vanishing at t. If we are at the beginning of a segment, the acceleration
528 * will be in the direction of the limiting velocity. If we are at the end, the acceleration will be in the opposite direction
529 * of the limiting velocity.
530 */
531
532 {
533 Concrete::Coords2D d = ( t < 0.5 ? 1 : -1 ) * coeffs.acceleration( t.offtype< 0, 1 >( ) );
534 Concrete::Length v = d.norm( );
535 if( v > shortLength )
536 {
537 return RefCountPtr< const Lang::FloatPair >( new Lang::FloatPair( d.direction( v ) ) );
538 }
539 }
540 return RefCountPtr< const Lang::FloatPair >( new Lang::FloatPair( 0, 0 ) );
541 }
542
543 RefCountPtr< const Lang::FloatPair >
544 Lang::ElementaryPath2D::reverse_direction( Concrete::Time globalTime ) const
545 {
546 if( empty( ) )
547 {
548 throw Exceptions::OutOfRange( "The empty path has no directions at all." );
549 }
550 Concrete::Time t;
551 const Concrete::PathPoint2D * p1;
552 const Concrete::PathPoint2D * p2;
553 findSegmentReverse( globalTime, &t, &p1, &p2 );
554
555 Bezier::ControlPoints< Concrete::Coords2D > controls( *(p1->mid_), *(p1->front_), *(p2->rear_), *(p2->mid_) );
556 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
557
558 const Concrete::Length segLengthBound =
559 ( controls.p1_ - controls.p0_ ).norm( ) + ( controls.p2_ - controls.p1_ ).norm( ) + ( controls.p3_ - controls.p2_ ).norm( );
560 const Concrete::Length shortLength = 1.e-4 * segLengthBound;
561
562 {
563 Concrete::Coords2D d = (-1) * coeffs.velocity( t.offtype< 0, 1 >( ) );
564 Concrete::Length v = d.norm( );
565 if( v > shortLength )
566 {
567 return RefCountPtr< const Lang::FloatPair >( new Lang::FloatPair( d.direction( v ) ) );
568 }
569 }
570
571 /* We reach here if the velocity more or less was vanishing at t. If we are at the beginning of a segment, the acceleration
572 * will be in the direction of the limiting velocity. If we are at the end, the acceleration will be in the opposite direction
573 * of the limiting velocity.
574 */
575
576 {
577 Concrete::Coords2D d = ( t < 0.5 ? -1 : 1 ) * coeffs.acceleration( t.offtype< 0, 1 >( ) );
578 Concrete::Length v = d.norm( );
579 if( v > shortLength )
580 {
581 return RefCountPtr< const Lang::FloatPair >( new Lang::FloatPair( d.direction( v ) ) );
582 }
583 }
584 return RefCountPtr< const Lang::FloatPair >( new Lang::FloatPair( 0, 0 ) );
585 }
586
587 RefCountPtr< const Lang::Length >
588 Lang::ElementaryPath2D::radiusOfCurvature( Concrete::Time globalTime ) const
589 {
590 if( empty( ) )
591 {
592 throw Exceptions::OutOfRange( "The empty path has no curvature at all." );
593 }
594 Concrete::Time t;
595 const Concrete::PathPoint2D * p1;
596 const Concrete::PathPoint2D * p2;
597 findSegment( globalTime, &t, &p1, &p2 );
598
599 if( t <= Concrete::ZERO_TIME )
600 {
601 /* here, t = 0, tc == 1, so
602 */
603 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
604 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
605 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
606 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
607 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
608 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
609 Concrete::Speed vx = ( x0 * (-3) + x1 * 3 ).offtype< 0, -2 >( );
610 Concrete::Speed vy = ( y0 * (-3) + y1 * 3 ).offtype< 0, -2 >( );
611 Concrete::Acceleration ax = ( x0 * 6 + x2 * 6 ).offtype< 0, -1 >( );
612 Concrete::Acceleration ay = ( y0 * 6 + y2 * 6 ).offtype< 0, -1 >( );
613
614 Physical< 2, -3 > denom = vx * ay - vy * ax;
615 if( denom == Physical< 2, -3 >( 0 ) )
616 {
617 return RefCountPtr< const Lang::Length >( new Lang::Length( Concrete::HUGE_LENGTH ) );
618 }
619 Concrete::Speed tmp = hypotPhysical( vx, vy );
620 return RefCountPtr< const Lang::Length >( new Lang::Length( Concrete::Length( tmp * tmp * tmp / denom ) ) );
621 }
622 if( t >= Concrete::UNIT_TIME )
623 {
624 throw Exceptions::InternalError( "t1 == 1 in curvature calculation" );
625 }
626
627 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
628 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
629 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
630 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
631 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
632 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
633 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
634 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
635
636 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
637 Physical< 0, 2 > kv0 = -3 * tc * tc;
638 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
639 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
640 Physical< 0, 2 > kv3 = 3 * t * t;
641 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
642 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
643 Physical< 0, 1 > ka0 = 6 * tc;
644 Physical< 0, 1 > ka1 = -12 * tc + 6 * t;
645 Physical< 0, 1 > ka2 = 6 * tc - 12 * t;
646 Physical< 0, 1 > ka3 = 6 * t;
647 Concrete::Acceleration ax = x0 * ka0 + x1 * ka1 + x2 * ka2 + x3 * ka3;
648 Concrete::Acceleration ay = y0 * ka0 + y1 * ka1 + y2 * ka2 + y3 * ka3;
649
650 Physical< 2, -3 > denom = vx * ay - vy * ax;
651 if( denom == Physical< 2, -3 >( 0 ) )
652 {
653 return RefCountPtr< const Lang::Length >( new Lang::Length( Concrete::HUGE_LENGTH ) );
654 }
655 Concrete::Speed tmp = hypotPhysical( vx, vy );
656 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * tmp * tmp / denom ) );
657 }
658
659 RefCountPtr< const Lang::Length >
660 Lang::ElementaryPath2D::reverse_radiusOfCurvature( Concrete::Time globalTime ) const
661 {
662 if( empty( ) )
663 {
664 throw Exceptions::OutOfRange( "The empty path has no curvature at all." );
665 }
666 Concrete::Time t;
667 const Concrete::PathPoint2D * p1;
668 const Concrete::PathPoint2D * p2;
669 findSegmentReverse( globalTime, &t, &p1, &p2 );
670
671 if( t <= Concrete::ZERO_TIME )
672 {
673 throw Exceptions::InternalError( "t1 == 0 in reverse curvature calculation" );
674 }
675 if( t >= Concrete::UNIT_TIME )
676 {
677 /* here, t = 1, tc == 0, so
678 */
679 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
680 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
681 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
682 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
683 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
684 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
685
686 Concrete::Speed vx = ( x2 * (-3) + x3 * 3 ).offtype< 0, -2 >( );
687 Concrete::Speed vy = ( y2 * (-3) + y3 * 3 ).offtype< 0, -2 >( );
688 Concrete::Acceleration ax = ( x1 * 6 + x2 * (-12) + x3 * 6 ).offtype< 0, -1 >( );
689 Concrete::Acceleration ay = ( y1 * 6 + y2 * (-12) + y3 * 6 ).offtype< 0, -1 >( );
690
691 Physical< 2, -3 > denom = vx * ay - vy * ax;
692 if( denom == Physical< 2, -3 >( 0 ) )
693 {
694 return RefCountPtr< const Lang::Length >( new Lang::Length( Concrete::HUGE_LENGTH ) );
695 }
696 Concrete::Speed tmp = hypotPhysical( vx, vy );
697 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * tmp * tmp / denom ) );
698 }
699
700 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
701 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
702 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
703 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
704 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
705 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
706 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
707 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
708
709 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
710 Physical< 0, 2 > kv0 = -3 * tc * tc;
711 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
712 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
713 Physical< 0, 2 > kv3 = 3 * t * t;
714 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
715 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
716 Physical< 0, 1 > ka0 = 6 * tc;
717 Physical< 0, 1 > ka1 = -12 * tc + 6 * t;
718 Physical< 0, 1 > ka2 = 6 * tc - 12 * t;
719 Physical< 0, 1 > ka3 = 6 * t;
720 Concrete::Acceleration ax = x0 * ka0 + x1 * ka1 + x2 * ka2 + x3 * ka3;
721 Concrete::Acceleration ay = y0 * ka0 + y1 * ka1 + y2 * ka2 + y3 * ka3;
722
723 Physical< 2, -3 > denom = vx * ay - vy * ax;
724 if( denom == Physical< 2, -3 >( 0 ) )
725 {
726 return RefCountPtr< const Lang::Length >( new Lang::Length( Concrete::HUGE_LENGTH ) );
727 }
728 Concrete::Speed tmp = hypotPhysical( vx, vy );
729 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * tmp * tmp / denom ) );
730 }
731
732 size_t
733 Lang::ElementaryPath2D::duration( ) const
734 {
735 if( closed_ )
736 {
737 return size( );
738 }
739 return size( ) - 1;
740 }
741
742 bool
743 Lang::ElementaryPath2D::controllingMaximizer( const Lang::FloatPair & d, Lang::Coords2D * dst ) const
744 {
745 if( empty( ) )
746 {
747 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
748 }
749 Concrete::Length opt = -Concrete::HUGE_LENGTH;
750 const double dx = d.x_;
751 const double dy = d.y_;
752 for( const_iterator i = begin( ); i != end( ); ++i )
753 {
754 if( (*i)->rear_ != 0 )
755 {
756 Concrete::Length y = (*i)->rear_->x_ * dx + (*i)->rear_->y_ * dy;
757 if( y > opt )
758 {
759 opt = y;
760 *dst = *((*i)->rear_);
761 }
762 }
763 Concrete::Length y = (*i)->mid_->x_ * dx + (*i)->mid_->y_ * dy;
764 if( y > opt )
765 {
766 opt = y;
767 *dst = *((*i)->mid_);
768 }
769 if( (*i)->front_ != 0 )
770 {
771 Concrete::Length y = (*i)->front_->x_ * dx + (*i)->front_->y_ * dy;
772 if( y > opt )
773 {
774 opt = y;
775 *dst = *((*i)->front_);
776 }
777 }
778 }
779 if( opt == -Concrete::HUGE_LENGTH )
780 {
781 return false;
782 }
783 return true;
784 }
785
786 bool
787 Lang::ElementaryPath2D::boundingRectangle( Concrete::Coords2D * dstll, Concrete::Coords2D * dstur ) const
788 {
789 if( empty( ) )
790 {
791 throw Exceptions::OutOfRange( "The empty path has no bounding rectangle." );
792 }
793 Concrete::Length xmin = Concrete::HUGE_LENGTH;
794 Concrete::Length ymin = Concrete::HUGE_LENGTH;
795 Concrete::Length xmax = -Concrete::HUGE_LENGTH;
796 Concrete::Length ymax = -Concrete::HUGE_LENGTH;
797 for( const_iterator i = begin( ); i != end( ); ++i )
798 {
799 if( (*i)->rear_ != 0 )
800 {
801 Concrete::Length x = (*i)->rear_->x_;
802 Concrete::Length y = (*i)->rear_->y_;
803 xmin = min( xmin, x );
804 ymin = min( ymin, y );
805 xmax = max( xmax, x );
806 ymax = max( ymax, y );
807 }
808 Concrete::Length x = (*i)->mid_->x_;
809 Concrete::Length y = (*i)->mid_->y_;
810 xmin = min( xmin, x );
811 ymin = min( ymin, y );
812 xmax = max( xmax, x );
813 ymax = max( ymax, y );
814 if( (*i)->front_ != 0 )
815 {
816 Concrete::Length x = (*i)->front_->x_;
817 Concrete::Length y = (*i)->front_->y_;
818 xmin = min( xmin, x );
819 ymin = min( ymin, y );
820 xmax = max( xmax, x );
821 ymax = max( ymax, y );
822 }
823 }
824 dstll->x_ = xmin;
825 dstll->y_ = ymin;
826 dstur->x_ = xmax;
827 dstur->y_ = ymax;
828
829 if( xmin == Concrete::HUGE_LENGTH )
830 {
831 return false;
832 }
833 return true;
834 }
835
836 bool
837 Lang::ElementaryPath2D::lineSegmentIntersection( Concrete::Time * dstSelf, Concrete::Time * dstLine, const Concrete::Coords2D & l0, const Concrete::Coords2D & l1 ) const
838 {
839 Concrete::Coords2D r = l1 - l0;
840 Concrete::Coords2D rInv = r * ( 1. / Concrete::innerScalar( r, r ) );
841 Concrete::Coords2D n( l0.y_ - l1.y_, l1.x_ - l0.x_ );
842 double offset = Concrete::innerScalar( n, l0 );
843 Concrete::Time steps = Concrete::ZERO_TIME;
844 const_iterator i1 = begin( );
845 const_iterator i2 = i1;
846 ++i2;
847 Concrete::Time res = Concrete::HUGE_TIME;
848 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
849 {
850 if( i2 == end( ) )
851 {
852 if( closed_ )
853 {
854 i2 = begin( );
855 }
856 else
857 {
858 break;
859 }
860 }
861
862 if( (*i1)->front_ == (*i1)->mid_ &&
863 (*i2)->rear_ == (*i2)->mid_ )
864 {
865 Concrete::Coords2D r2 = *((*i2)->mid_) - *((*i1)->mid_);
866 Concrete::Coords2D d0 = *((*i1)->mid_) - l0;
867 Concrete::LengthSquared det = r.y_ * r2.x_ - r.x_ * r2.y_;
868 if( fabs( Concrete::LengthSquared::offtype( det ) ) < 1e-14 )
869 {
870 /* Parallel lines */
871 if( Concrete::inner( d0, n.direction( ) ).abs( ) > Computation::theDistanceTol )
872 {
873 /* The lines are separated in the normal direction */
874 continue;
875 }
876 double ta = Concrete::innerScalar( rInv, *((*i1)->mid_) - l0 );
877 double tb = Concrete::innerScalar( rInv, *((*i2)->mid_) - l0 );
878 bool swapped = false;
879 if( ta > tb ) /* Sort points ta <= tb */
880 {
881 double tmp = ta;
882 ta = tb;
883 tb = tmp;
884 swapped = true;
885 }
886 if( ta < 0 )
887 {
888 if( tb < 0 )
889 {
890 continue;
891 }
892 *dstLine = Concrete::Time( 0 );
893 *dstSelf = steps + Shapes::straightLineArcTime( ( l0 - *((*i1)->mid_) ).norm( ) / ( *((*i2)->mid_) - *((*i1)->mid_) ).norm( ) );
894 return true;
895 }
896 if( ta <= 1 )
897 {
898 Concrete::Time tmp = Shapes::straightLineArcTime( ta );
899 if( tmp < res )
900 {
901 res = tmp;
902 /* If ta and tb were swapped previously, ta corresponds to *i2, where the time is 1 past <steps>.
903 * Otherwise, ta corresponds to *i1, where the time is <steps>.
904 */
905 *dstSelf = swapped ? ( steps + Concrete::UNIT_TIME ) : steps;
906 }
907 }
908 continue;
909 }
910
911 double t1 = ( d0.y_ * r2.x_ - d0.x_ * r2.y_ ) / det;
912 if( 0 <= t1 && t1 <= 1 )
913 {
914 double t2 = ( d0.y_ * r.x_ - d0.x_ * r.y_ ) / det;
915 if( 0 <= t2 && t2 <= 1 )
916 {
917 Concrete::Time tmp = Shapes::straightLineArcTime( t1 );
918 if( tmp < res )
919 {
920 res = tmp;
921 *dstSelf = steps + Shapes::straightLineArcTime( t2 );
922 }
923 }
924 }
925 continue;
926 }
927
928 Bezier::ControlPoints< Concrete::Coords2D > controls( *(*i1)->mid_, *(*i1)->front_, *(*i2)->rear_, *(*i2)->mid_ );
929 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
930 double tmp2[4];
931 coeffs.hyperplaneIntersections( tmp2, n, offset );
932 for( const double * it2 = tmp2; *it2 != HUGE_VAL; ++it2 )
933 {
934 Concrete::Time t = Shapes::straightLineArcTime( Concrete::innerScalar( rInv, coeffs.point( *it2 ) - l0 ) );
935 if( Concrete::ZERO_TIME <= t && t <= Concrete::UNIT_TIME )
936 {
937 if( t < res )
938 {
939 res = t;
940 *dstSelf = steps + *it2;
941 }
942 }
943 }
944 }
945
946 if( res < Concrete::HUGE_TIME )
947 {
948 *dstLine = res;
949 return true;
950 }
951 return false;
952 }
953
954 RefCountPtr< const Lang::Coords2D >
955 Lang::ElementaryPath2D::controllingMaximizer( const Lang::FloatPair & d ) const
956 {
957 Lang::Coords2D * res = new Lang::Coords2D( Concrete::ZERO_LENGTH, Concrete::ZERO_LENGTH );
958 controllingMaximizer( d, res );
959 return RefCountPtr< const Lang::Coords2D >( res );
960 }
961
962 RefCountPtr< const Lang::Coords2D >
963 Lang::ElementaryPath2D::discreteMean( ) const
964 {
965 double count = 0;
966 Concrete::Length x( 0 );
967 Concrete::Length y( 0 );
968 if( empty( ) )
969 {
970 throw Exceptions::OutOfRange( "The empty path cannot be averaged along." );
971 }
972 for( const_iterator i = begin( ); i != end( ); ++i )
973 {
974 if( (*i)->rear_ != 0 )
975 {
976 ++count;
977 x = x + (*i)->rear_->x_;
978 y = y + (*i)->rear_->y_;
979 }
980 ++count;
981 x = x + (*i)->mid_->x_;
982 y = y + (*i)->mid_->y_;
983 if( (*i)->front_ != 0 )
984 {
985 ++count;
986 x = x + (*i)->front_->x_;
987 y = y + (*i)->front_->y_;
988 }
989 }
990 return RefCountPtr< const Lang::Coords2D >( new Lang::Coords2D( x / count, y / count ) );
991 }
992
993 Concrete::SplineTime
994 Lang::ElementaryPath2D::discreteMaximizer( const Lang::FloatPair & d ) const
995 {
996 if( empty( ) )
997 {
998 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
999 }
1000 Concrete::Length opt = -Concrete::HUGE_LENGTH;
1001 const double dx = d.x_;
1002 const double dy = d.y_;
1003 Concrete::SplineTime res = Concrete::ZERO_TIME;
1004 Concrete::SplineTime t = Concrete::ZERO_TIME;
1005 for( const_iterator i = begin( ); i != end( ); ++i, ++t )
1006 {
1007 Concrete::Length y = (*i)->mid_->x_ * dx + (*i)->mid_->y_ * dy;
1008 if( y > opt )
1009 {
1010 opt = y;
1011 res = t;
1012 }
1013 }
1014 return res;
1015 }
1016
1017 Concrete::SplineTime
1018 Lang::ElementaryPath2D::discreteApproximator( const Lang::Coords2D & p ) const
1019 {
1020 if( empty( ) )
1021 {
1022 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1023 }
1024 Concrete::Length bestDist = Concrete::HUGE_LENGTH;
1025 const Concrete::Length px = p.x_.get( );
1026 const Concrete::Length py = p.y_.get( );
1027 Concrete::SplineTime res = Concrete::ZERO_TIME;
1028 Concrete::SplineTime t = Concrete::ZERO_TIME;
1029 for( const_iterator i = begin( ); i != end( ); ++i, ++t )
1030 {
1031 Concrete::Length dist = hypotPhysical( (*i)->mid_->x_ - px, (*i)->mid_->y_ - py );
1032 if( dist < bestDist )
1033 {
1034 bestDist = dist;
1035 res = t;
1036 }
1037 }
1038 return res;
1039 }
1040
1041 RefCountPtr< const Lang::Coords2D >
1042 Lang::ElementaryPath2D::continuousMean( ) const
1043 {
1044 if( empty( ) )
1045 {
1046 throw Exceptions::OutOfRange( "The empty path cannot be averaged along." );
1047 }
1048 Physical< 2, 0 > intx( 0 );
1049 Physical< 2, 0 > inty( 0 );
1050 Physical< 1, 0 > totlen( 0 );
1051
1052 const_iterator i1 = begin( );
1053 const_iterator i2 = i1;
1054 ++i2;
1055 for( ; i1 != end( ); ++i1, ++i2 )
1056 {
1057 if( i2 == end( ) )
1058 {
1059 if( closed_ )
1060 {
1061 i2 = begin( );
1062 }
1063 else
1064 {
1065 break;
1066 }
1067 }
1068
1069 if( (*i1)->front_ == (*i1)->mid_ &&
1070 (*i2)->rear_ == (*i2)->mid_ )
1071 {
1072 Concrete::Length x0 = (*i1)->mid_->x_;
1073 Concrete::Length y0 = (*i1)->mid_->y_;
1074 Concrete::Length x3 = (*i2)->mid_->x_;
1075 Concrete::Length y3 = (*i2)->mid_->y_;
1076 Concrete::Length len = hypotPhysical( (x3-x0), (y3-y0) );
1077 intx += len * 0.5 * ( x0 + x3 );
1078 inty += len * 0.5 * ( y0 + y3 );
1079 totlen += len;
1080 continue;
1081 }
1082
1083 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
1084 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
1085 Concrete::Bezier x1 = (*i1)->front_->x_.offtype< 0, 3 >( );
1086 Concrete::Bezier y1 = (*i1)->front_->y_.offtype< 0, 3 >( );
1087 Concrete::Bezier x2 = (*i2)->rear_->x_.offtype< 0, 3 >( );
1088 Concrete::Bezier y2 = (*i2)->rear_->y_.offtype< 0, 3 >( );
1089 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
1090 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
1091
1092 Concrete::Time dt = Shapes::computeDt( hypotPhysical( (x1-x0), (y1-y0) ).offtype< 0, -3 >( ) +
1093 hypotPhysical( (x2-x1), (y2-y1) ).offtype< 0, -3 >( ) +
1094 hypotPhysical( (x3-x2), (y3-y2) ).offtype< 0, -3 >( ) );
1095
1096 Physical< 2, 0 > tmpSum_x( 0 );
1097 Physical< 2, 0 > tmpSum_y( 0 );
1098 Physical< 1, 0 > tmpSum_l( 0 );
1099 for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
1100 {
1101 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
1102 Physical< 0, 3 > k0 = tc * tc * tc;
1103 Physical< 0, 3 > k1 = 3 * tc * tc * t;
1104 Physical< 0, 3 > k2 = 3 * tc * t * t;
1105 Physical< 0, 3 > k3 = t * t * t;
1106 Concrete::Length x = x0 * k0 + x1 * k1 + x2 * k2 + x3 * k3;
1107 Concrete::Length y = y0 * k0 + y1 * k1 + y2 * k2 + y3 * k3;
1108 Physical< 0, 2 > kv0 = -3 * tc * tc;
1109 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
1110 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
1111 Physical< 0, 2 > kv3 = 3 * t * t;
1112 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
1113 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
1114
1115 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
1116 tmpSum_x += x * dl;
1117 tmpSum_y += y * dl;
1118 tmpSum_l += dl;
1119 }
1120 intx += tmpSum_x * dt.offtype< 0, 1 >( );
1121 inty += tmpSum_y * dt.offtype< 0, 1 >( );
1122 totlen += tmpSum_l * dt.offtype< 0, 1 >( );
1123 }
1124
1125 return RefCountPtr< const Lang::Coords2D >( new Lang::Coords2D( intx / totlen, inty / totlen ) );
1126 }
1127
1128 Concrete::SplineTime
1129 Lang::ElementaryPath2D::continuousMaximizer( const Lang::FloatPair & dNonUnit ) const
1130 {
1131 if( empty( ) )
1132 {
1133 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1134 }
1135
1136 Concrete::UnitFloatPair d( dNonUnit.x_, dNonUnit.y_ );
1137 Concrete::Coords2D dBezier( d.x_, d.y_ ); // The Bezier functions requires the direction to be given with the same type as the spline space.
1138
1139 Concrete::SplineTime res = Concrete::ZERO_TIME;
1140 Concrete::Length opt = -Concrete::HUGE_LENGTH;
1141 Concrete::Time steps = Concrete::ZERO_TIME;
1142 const_iterator i1 = begin( );
1143 const_iterator i2 = i1;
1144 ++i2;
1145 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
1146 {
1147 if( i2 == end( ) )
1148 {
1149 if( closed_ )
1150 {
1151 i2 = begin( );
1152 }
1153 else
1154 {
1155 Concrete::Length v = Concrete::inner( *(*i1)->mid_, d );
1156 if( v > opt )
1157 {
1158 opt = v;
1159 res = steps;
1160 }
1161 break;
1162 }
1163 }
1164 Concrete::Length v = Concrete::inner( *(*i1)->mid_, d );
1165 if( v > opt )
1166 {
1167 opt = v;
1168 res = steps;
1169 }
1170
1171 /* if the segment is straight, checking its end points is enough.
1172 */
1173 if( (*i1)->front_ == (*i1)->mid_ &&
1174 (*i2)->rear_ == (*i2)->mid_ )
1175 {
1176 continue;
1177 }
1178
1179 /* First check the control points. If none of the four control points improves the optimum,
1180 * searching further is meaningless. Further, since the mid_ points are allways tested anyway,
1181 * we can assume that they already are, and conclude that they cannot improve the optimum further.
1182 * Therefore, there are only the two handles to check.
1183 */
1184 {
1185 if( Concrete::inner( *(*i1)->front_, d ) <= opt &&
1186 Concrete::inner( *(*i2)->rear_, d ) <= opt )
1187 {
1188 continue;
1189 }
1190 }
1191
1192 Bezier::ControlPoints< Concrete::Coords2D > controls( *(*i1)->mid_, *(*i1)->front_, *(*i2)->rear_, *(*i2)->mid_ );
1193 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
1194 double optTimes[3];
1195 coeffs.stationaryPoints( optTimes, dBezier ); // A HUGE_VAL is used as terminator in the result.
1196
1197 for( double * src = & optTimes[0]; *src != HUGE_VAL; ++src )
1198 {
1199 Concrete::Length v = Concrete::inner( coeffs.point( *src ), d );
1200 if( v > opt )
1201 {
1202 opt = v;
1203 res = steps + *src;
1204 }
1205 }
1206 }
1207
1208 return res;
1209 }
1210
1211 namespace Shapes
1212 {
1213 class ApproximationPoly2D
1214 {
1215 Concrete::Coords2D p_;
1216 Bezier::PolyCoeffs< Concrete::Coords2D > polyCoeffs_;
1217 Physical< 1, 0 > kxD0_0_;
1218 Physical< 1, -1 > kxD0_1_;
1219 Physical< 1, -2 > kxD0_2_;
1220 Physical< 1, -3 > kxD0_3_;
1221 Physical< 1, -1 > kxD1_0_;
1222 Physical< 1, -2 > kxD1_1_;
1223 Physical< 1, -3 > kxD1_2_;
1224 Physical< 1, -2 > kxD2_0_;
1225 Physical< 1, -3 > kxD2_1_;
1226
1227 Physical< 1, 0 > kyD0_0_;
1228 Physical< 1, -1 > kyD0_1_;
1229 Physical< 1, -2 > kyD0_2_;
1230 Physical< 1, -3 > kyD0_3_;
1231 Physical< 1, -1 > kyD1_0_;
1232 Physical< 1, -2 > kyD1_1_;
1233 Physical< 1, -3 > kyD1_2_;
1234 Physical< 1, -2 > kyD2_0_;
1235 Physical< 1, -3 > kyD2_1_;
1236
1237 Concrete::Speed maxSpeed_;
1238 public:
1239 ApproximationPoly2D( const Concrete::Coords2D & p0, const Concrete::Coords2D & p1, const Concrete::Coords2D & p2, const Concrete::Coords2D & p3, const Concrete::Coords2D & p );
1240 Concrete::Time splitTime( const Concrete::Time t_low, const Concrete::Time t_high, const Concrete::Time t_tol ) const;
1241 bool isCircular( ) const;
1242 Concrete::Length distanceAt( Concrete::Time t ) const;
1243 Concrete::LengthSquared squaredDistanceAt( Concrete::Time t ) const;
1244 Concrete::Speed maxSpeed( ) const { return maxSpeed_; }
1245 Bezier::ControlPoints< Concrete::Coords2D > getControls( const Concrete::Time t_low, const Concrete::Time t_high ) const;
1246 const Concrete::Coords2D & getPoint( ) const;
1247 };
1248 class ApproximationSegmentSection2D
1249 {
1250 Bezier::ControlPoints< Concrete::Coords2D > controls_;
1251 const ApproximationPoly2D * baseSeg_;
1252 Concrete::Time steps_;
1253 Concrete::Time t0_;
1254 Concrete::Time t1_;
1255 public:
1256
1257 ApproximationSegmentSection2D( const Shapes::ApproximationPoly2D * baseSeg, Concrete::Time steps, Concrete::Time t0, Concrete::Time t1 );
1258 ApproximationSegmentSection2D * cutAfter( Concrete::Time t ) const;
1259 ApproximationSegmentSection2D * cutBefore( Concrete::Time t ) const;
1260
1261 Concrete::Length convexHullDistance( ) const;
1262 Concrete::Time splitTime( const Concrete::Time t_tol ) const;
1263 Concrete::Time globalTime( Concrete::Time t ) const;
1264 Concrete::Length distanceAt( Concrete::Time t ) const;
1265 Concrete::Speed maxSpeed( ) const;
1266 Concrete::Time duration( ) const;
1267 };
1268 }
1269
1270 Concrete::SplineTime
1271 Lang::ElementaryPath2D::continuousApproximator( const Lang::Coords2D & coordPoint ) const
1272 {
1273 const Concrete::Length DISTANCE_TOL = 0.001 * Computation::the_arcdelta;
1274
1275 if( empty( ) )
1276 {
1277 throw Exceptions::OutOfRange( "The empty path cannot be approximated along." );
1278 }
1279
1280 Concrete::Coords2D p( coordPoint.x_.get( ), coordPoint.y_.get( ) );
1281 PtrOwner_back_Access< std::list< const Shapes::ApproximationPoly2D * > > memory;
1282 typedef std::pair< Shapes::ApproximationSegmentSection2D *, Concrete::Length > WorkItem;
1283 std::list< WorkItem > work;
1284
1285 Concrete::SplineTime res = Concrete::ZERO_TIME;
1286 Concrete::Length bestDist = Concrete::HUGE_LENGTH;
1287 Concrete::Time steps = Concrete::ZERO_TIME;
1288 const_iterator i1 = begin( );
1289 const_iterator i2 = i1;
1290 ++i2;
1291 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
1292 {
1293 if( i2 == end( ) )
1294 {
1295 if( closed_ )
1296 {
1297 i2 = begin( );
1298 }
1299 else
1300 {
1301 Concrete::Length dist = hypotPhysical( (*i1)->mid_->x_ - p.x_, (*i1)->mid_->y_ - p.y_ );
1302 if( dist < bestDist )
1303 {
1304 bestDist = dist;
1305 res = steps;
1306 }
1307 break;
1308 }
1309 }
1310
1311 Concrete::Length dist = hypotPhysical( (*i1)->mid_->x_ - p.x_, (*i1)->mid_->y_ - p.y_ );
1312 if( dist < bestDist )
1313 {
1314 bestDist = dist;
1315 res = steps;
1316 }
1317
1318 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
1319 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
1320 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
1321 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
1322
1323 /* if the segment is straight, finding the closest point is a linear least-squares problem.
1324 */
1325 if( (*i1)->front_ == (*i1)->mid_ &&
1326 (*i2)->rear_ == (*i2)->mid_ )
1327 {
1328 const Concrete::Length tx = ( x3 - x0 ).offtype< 0, -3 >( );
1329 const Concrete::Length ty = ( y3 - y0 ).offtype< 0, -3 >( );
1330 double s = ( tx * ( p.x_ - x0.offtype< 0, -3 >( ) ) + ty * ( p.y_ - y0.offtype< 0, -3 >( ) ) ) / ( tx*tx + ty*ty );
1331 if( 0 < s && s < 1 )
1332 {
1333 Concrete::Length dist = hypotPhysical( x0.offtype< 0, -3 >( ) + s * tx - p.x_, y0.offtype< 0, -3 >( ) + s * ty - p.y_ );
1334 if( dist < bestDist )
1335 {
1336 bestDist = dist;
1337 res = steps + Shapes::straightLineArcTime( s );
1338 }
1339 }
1340 continue;
1341 }
1342
1343 Shapes::ApproximationPoly2D * coeffs =
1344 new Shapes::ApproximationPoly2D( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_), p );
1345 Shapes::ApproximationSegmentSection2D * seg = new Shapes::ApproximationSegmentSection2D( coeffs, steps, Concrete::ZERO_TIME, Concrete::UNIT_TIME );
1346 Concrete::Length lowerBound = seg->convexHullDistance( ) + DISTANCE_TOL;
1347 if( lowerBound >= bestDist )
1348 {
1349 delete seg;
1350 delete coeffs;
1351 continue;
1352 }
1353
1354 /* Here, we must detect the ill-contditionned case of center-to-circle. This is detected
1355 * by checking the distance at four different points.
1356 */
1357 {
1358 if( coeffs->isCircular( ) )
1359 {
1360 Concrete::Length dist = coeffs->distanceAt( Concrete::ZERO_TIME );
1361 if( dist < bestDist )
1362 {
1363 bestDist = dist;
1364 res = steps;
1365 }
1366 delete seg;
1367 delete coeffs;
1368 continue;
1369 }
1370 }
1371
1372 memory.push_back( coeffs );
1373 work.push_back( WorkItem( seg, lowerBound ) );
1374 }
1375
1376 while( ! work.empty( ) )
1377 {
1378 Shapes::ApproximationSegmentSection2D * seg = work.front( ).first;
1379 Concrete::Length lowerBound = work.front( ).second;
1380 work.pop_front( );
1381 if( lowerBound < bestDist )
1382 {
1383 const Concrete::Time t_tol = Computation::the_arcdelta / seg->maxSpeed( );
1384 Concrete::Time split = seg->splitTime( 0.001 * t_tol ); // increased precicion seems to have no influence on computation time
1385 Concrete::Length dist = seg->distanceAt( split );
1386 if( dist < bestDist )
1387 {
1388 bestDist = dist;
1389 res = seg->globalTime( split );
1390 }
1391 if( lowerBound < bestDist &&
1392 t_tol < seg->duration( ) )
1393 {
1394 {
1395 Shapes::ApproximationSegmentSection2D * part = seg->cutAfter( split );
1396 Concrete::Length lowerBound = part->convexHullDistance( ) + DISTANCE_TOL;
1397 if( lowerBound >= bestDist )
1398 {
1399 delete part;
1400 }
1401 else
1402 {
1403 work.push_back( WorkItem( part, lowerBound ) );
1404 }
1405 }
1406 {
1407 Shapes::ApproximationSegmentSection2D * part = seg->cutBefore( split );
1408 Concrete::Length lowerBound = part->convexHullDistance( ) + DISTANCE_TOL;
1409 if( lowerBound >= bestDist )
1410 {
1411 delete part;
1412 }
1413 else
1414 {
1415 work.push_back( WorkItem( part, lowerBound ) );
1416 }
1417 }
1418 }
1419 }
1420 delete seg;
1421 }
1422
1423 return res;
1424 }
1425
1426 Shapes::ApproximationPoly2D::ApproximationPoly2D( const Concrete::Coords2D & p0, const Concrete::Coords2D & p1, const Concrete::Coords2D & p2, const Concrete::Coords2D & p3, const Concrete::Coords2D & p )
1427 : p_( p ), polyCoeffs_( Bezier::PolyCoeffs< Concrete::Coords2D >( Bezier::ControlPoints< Concrete::Coords2D >( p0, p1, p2, p3 ) ) )
1428 {
1429 kxD0_0_ = polyCoeffs_.z0_.x_ - p.x_;
1430 kxD0_1_ = polyCoeffs_.z1_.x_.offtype< 0, 1 >( );
1431 kxD0_2_ = polyCoeffs_.z2_.x_.offtype< 0, 2 >( );
1432 kxD0_3_ = polyCoeffs_.z3_.x_.offtype< 0, 3 >( );
1433
1434 kxD1_0_ = kxD0_1_;
1435 kxD1_1_ = 2 * kxD0_2_;
1436 kxD1_2_ = 3 * kxD0_3_;
1437
1438 kxD2_0_ = kxD1_1_;
1439 kxD2_1_ = 2 * kxD1_2_;
1440
1441 kyD0_0_ = polyCoeffs_.z0_.y_ - p.y_;
1442 kyD0_1_ = polyCoeffs_.z1_.y_.offtype< 0, 1 >( );
1443 kyD0_2_ = polyCoeffs_.z2_.y_.offtype< 0, 2 >( );
1444 kyD0_3_ = polyCoeffs_.z3_.y_.offtype< 0, 3 >( );
1445
1446 kyD1_0_ = kyD0_1_;
1447 kyD1_1_ = 2 * kyD0_2_;
1448 kyD1_2_ = 3 * kyD0_3_;
1449
1450 kyD2_0_ = kyD1_1_;
1451 kyD2_1_ = 2 * kyD1_2_;
1452
1453 Concrete::Length tmp = max( hypotPhysical( p1.x_ - p0.x_, p1.y_ - p0.y_ ),
1454 hypotPhysical( p2.x_ - p1.x_, p2.y_ - p1.y_ ) );
1455 maxSpeed_ = max( tmp, hypotPhysical( p3.x_ - p2.x_, p3.y_ - p2.y_ ) ).offtype< 0, 1 >( );
1456 }
1457
1458 Concrete::Time
1459 Shapes::ApproximationPoly2D::splitTime( const Concrete::Time t_low, const Concrete::Time t_high, const Concrete::Time t_tol ) const
1460 {
1461 Concrete::Time t;
1462 Concrete::LengthSquared last_f( HUGE_VAL );
1463 {
1464 const double GRID = 7;
1465 const Concrete::Time STEP = ( t_high - t_low ) / GRID;
1466 for( Concrete::Time tt = t_low + 0.5 * STEP; tt < t_high; tt += STEP )
1467 {
1468 Concrete::LengthSquared tmp = squaredDistanceAt( tt );
1469 if( tmp < last_f )
1470 {
1471 last_f = tmp;
1472 t = tt;
1473 }
1474 }
1475 }
1476 Concrete::Time last_t = - Concrete::UNIT_TIME;
1477 bool lastOffBounds = false;
1478
1479 while( t < last_t - t_tol || t > last_t + t_tol )
1480 {
1481 last_t = t;
1482 Physical< 2, -1 > objectiveD1( 0 ); // these are actually computed for the objective divided by two
1483 Physical< 2, -2 > objectiveD2( 0 ); // but since we will divide one by the other, it doesn't matter.
1484
1485 {
1486 /* x-components */
1487 Physical< 1, 0 > fD0 = kxD0_0_ + t * ( kxD0_1_ + t * ( kxD0_2_ + t * kxD0_3_ ) );
1488 Physical< 1, -1 > fD1 = kxD1_0_ + t * ( kxD1_1_ + t * kxD1_2_ );
1489 Physical< 1, -2 > fD2 = kxD2_0_ + t * kxD2_1_;
1490 objectiveD1 += fD0 * fD1;
1491 objectiveD2 += fD1 * fD1 + fD0 * fD2;
1492 }
1493
1494 {
1495 /* y-components */
1496 Physical< 1, 0 > fD0 = kyD0_0_ + t * ( kyD0_1_ + t * ( kyD0_2_ + t * kyD0_3_ ) );
1497 Physical< 1, -1 > fD1 = kyD1_0_ + t * ( kyD1_1_ + t * kyD1_2_ );
1498 Physical< 1, -2 > fD2 = kyD2_0_ + t * kyD2_1_;
1499 objectiveD1 += fD0 * fD1;
1500 objectiveD2 += fD1 * fD1 + fD0 * fD2;
1501 }
1502
1503 Concrete::Time step;
1504 if( objectiveD2 <= Physical< 2, -2 >( 0 ) )
1505 {
1506 /* The minimization problem is not convex, locally.
1507 * Go to one of the extremes.
1508 */
1509 if( objectiveD1 < Physical< 2, -1 >( 0 ) )
1510 {
1511 step = 1.1 * ( t_high - t );
1512 }
1513 else
1514 {
1515 step = 1.1 * ( t_low - t );
1516 }
1517 }
1518 else
1519 {
1520 step = -objectiveD1 / objectiveD2; // here's the division where the missing factors of 2 cancel.
1521 if( t + step < t_low )
1522 {
1523 step = 1.1 * ( t_low - t ); // 1.1 to make sure that the off-bounds test detects minima outside [ t_low t_high ]
1524 }
1525 else if( t + step > t_high )
1526 {
1527 step = 1.1 * ( t_high - t ); // 1.1 to make sure that the off-bounds test detects minima outside [ t_low t_high ]
1528 }
1529 }
1530
1531 Concrete::LengthSquared f = squaredDistanceAt( t + step );
1532 if( step > Concrete::ZERO_TIME )
1533 {
1534 while( f > last_f && step > t_tol )
1535 {
1536 step = step * 0.5;
1537 f = squaredDistanceAt( t + step );
1538 }
1539 }
1540 else
1541 {
1542 while( f > last_f && step < -t_tol )
1543 {
1544 step = step * 0.5;
1545 f = squaredDistanceAt( t + step );
1546 }
1547 }
1548 t += step;
1549 last_f = f;
1550
1551 if( t < t_low )
1552 {
1553 if( lastOffBounds )
1554 {
1555 return t_low;
1556 }
1557 else
1558 {
1559 t = t_low;
1560 lastOffBounds = true;
1561 }
1562 }
1563 else if( t > t_high )
1564 {
1565 if( lastOffBounds )
1566 {
1567 return t_high;
1568 }
1569 else
1570 {
1571 t = t_high;
1572 lastOffBounds = true;
1573 }
1574 }
1575 else
1576 {
1577 lastOffBounds = false;
1578 }
1579 }
1580
1581 if( lastOffBounds )
1582 {
1583 if( t > 0.5 * ( t_low + t_high ) )
1584 {
1585 return t_high;
1586 }
1587 return t_low;
1588 }
1589
1590 return t;
1591 }
1592
1593 Bezier::ControlPoints< Concrete::Coords2D >
1594 Shapes::ApproximationPoly2D::getControls( const Concrete::Time t_low, const Concrete::Time t_high ) const
1595 {
1596 return Bezier::ControlPoints< Concrete::Coords2D >( polyCoeffs_.subSection( t_low.offtype< 0, 1 >( ), t_high.offtype< 0, 1 >( ) ) );
1597 }
1598
1599 bool
1600 Shapes::ApproximationPoly2D::isCircular( ) const
1601 {
1602 Concrete::Length rmax = -Concrete::HUGE_LENGTH;
1603 Concrete::Length rmin = Concrete::HUGE_LENGTH;
1604 for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::Time( 1.05 ); t += Concrete::Time( 0.33333 ) )
1605 {
1606 Concrete::Length r = distanceAt( t );
1607 rmax = max( rmax, r );
1608 rmin = min( rmin, r );
1609 }
1610 return rmax <= rmin * 1.0001;
1611 }
1612
1613 const Concrete::Coords2D &
1614 Shapes::ApproximationPoly2D::getPoint( ) const
1615 {
1616 return p_;
1617 }
1618
1619
1620 Concrete::Length
1621 Shapes::ApproximationPoly2D::distanceAt( Concrete::Time t ) const
1622 {
1623 return hypotPhysical( kxD0_0_ + t * ( kxD0_1_ + t * ( kxD0_2_ + t * kxD0_3_ ) ),
1624 kyD0_0_ + t * ( kyD0_1_ + t * ( kyD0_2_ + t * kyD0_3_ ) ) );
1625 }
1626
1627 Concrete::LengthSquared
1628 Shapes::ApproximationPoly2D::squaredDistanceAt( Concrete::Time t ) const
1629 {
1630 Concrete::Length deltax = kxD0_0_ + t * ( kxD0_1_ + t * ( kxD0_2_ + t * kxD0_3_ ) );
1631 Concrete::Length deltay = kyD0_0_ + t * ( kyD0_1_ + t * ( kyD0_2_ + t * kyD0_3_ ) );
1632 return deltax * deltax + deltay * deltay;
1633 }
1634
1635 Shapes::ApproximationSegmentSection2D::ApproximationSegmentSection2D( const Shapes::ApproximationPoly2D * baseSeg, Concrete::Time steps, Concrete::Time t0, Concrete::Time t1 )
1636 : controls_( baseSeg->getControls( t0, t1 ) ), baseSeg_( baseSeg ), steps_( steps ), t0_( t0 ), t1_( t1 )
1637 { }
1638
1639 Shapes::ApproximationSegmentSection2D *
1640 Shapes::ApproximationSegmentSection2D::cutAfter( Concrete::Time t ) const
1641 {
1642 return new ApproximationSegmentSection2D( baseSeg_, steps_, t0_, t );
1643 }
1644
1645 Shapes::ApproximationSegmentSection2D *
1646 Shapes::ApproximationSegmentSection2D::cutBefore( Concrete::Time t ) const
1647 {
1648 return new ApproximationSegmentSection2D( baseSeg_, steps_, t, t1_ );
1649 }
1650
1651 Concrete::Length
1652 Shapes::ApproximationSegmentSection2D::convexHullDistance( ) const
1653 {
1654 Concrete::Coords2D p( baseSeg_->getPoint( ) );
1655
1656 std::vector< const Concrete::Coords2D * > pointSet( 0 );
1657 pointSet.reserve( 4 );
1658 pointSet.push_back( & controls_.p0_ );
1659 pointSet.push_back( & controls_.p1_ );
1660 pointSet.push_back( & controls_.p2_ );
1661 pointSet.push_back( & controls_.p3_ );
1662
1663 Concrete::Length res = Concrete::HUGE_LENGTH;
1664
1665 for( std::vector< const Concrete::Coords2D * >::const_iterator i0 = pointSet.begin( ); i0 != pointSet.end( ); ++i0 )
1666 {
1667 std::vector< const Concrete::Coords2D * >::const_iterator i1 = i0;
1668 ++i1;
1669 for( ; i1 != pointSet.end( ); ++i1 )
1670 {
1671 const Concrete::Coords2D & p0( **i0 );
1672 const Concrete::Coords2D & p1( **i1 );
1673
1674 /* The two points bound the convex hull iff the other points are on the same side of the segment.
1675 * If they are on the same side and p is on the other side, then the distance to p is of interest.
1676 */
1677 const Concrete::Length tx = p1.x_ - p0.x_;
1678 const Concrete::Length ty = p1.y_ - p0.y_;
1679 bool counterClockwise; // true when ( p0, p1 ) are ordered counter-clockwise around the interior.
1680 {
1681 std::vector< const Concrete::Coords2D * >::const_iterator i2 = pointSet.begin( );
1682 for( ; ; ++i2 ) // we don't have to check against pointSet.end( )
1683 {
1684 if( i2 == i0 || i2 == i1 )
1685 {
1686 continue;
1687 }
1688 const Concrete::Length dx = (*i2)->x_ - p0.x_;
1689 const Concrete::Length dy = (*i2)->y_ - p0.y_;
1690 counterClockwise = ( ty * dx - tx * dy < Concrete::LengthSquared( 0 ) );
1691 break;
1692 }
1693 ++i2;
1694 for( ; ; ++i2 ) // we don't have to check against pointSet.end( )
1695 {
1696 if( i2 == i0 || i2 == i1 )
1697 {
1698 continue;
1699 }
1700 const Concrete::Length dx = (*i2)->x_ - p0.x_;
1701 const Concrete::Length dy = (*i2)->y_ - p0.y_;
1702 if( counterClockwise == ( ty * dx - tx * dy < Concrete::LengthSquared( 0 ) ) )
1703 {
1704 goto checkDistance;
1705 }
1706 break;
1707 }
1708 continue; // the points where on different sides.
1709 checkDistance:
1710
1711 const Concrete::Length dx = p.x_ - p0.x_;
1712 const Concrete::Length dy = p.y_ - p0.y_;
1713 if( ( ty * dx - tx * dy > Concrete::LengthSquared( 0 ) ) == counterClockwise )
1714 {
1715 double s = ( tx * dx + ty * dy ) / ( tx*tx + ty*ty );
1716 Concrete::Length dist;
1717 if( s <= 0 )
1718 {
1719 dist = hypotPhysical( p0.x_ - p.x_, p0.y_ - p.y_ );
1720 }
1721 else if( s >= 1 )
1722 {
1723 dist = hypotPhysical( p1.x_ - p.x_, p1.y_ - p.y_ );
1724 }
1725 else
1726 {
1727 dist = hypotPhysical( s * tx - dx, s * ty - dy );
1728 }
1729 res = min( res, dist );
1730 }
1731 }
1732 }
1733 }
1734
1735 if( res == Concrete::HUGE_LENGTH )
1736 {
1737 /* This means that p was on the inside of each line, meaning it
1738 * is also inside the convex hull.
1739 */
1740 return Concrete::ZERO_LENGTH;
1741 }
1742 return res;
1743 }
1744
1745 Concrete::Time
1746 Shapes::ApproximationSegmentSection2D::splitTime( const Concrete::Time t_tol ) const
1747 {
1748 Concrete::Time d = 0.2 * ( t1_ - t0_ );
1749 return baseSeg_->splitTime( t0_ + d, t1_ - d, t_tol );
1750 }
1751
1752 Concrete::Time
1753 Shapes::ApproximationSegmentSection2D::globalTime( Concrete::Time t ) const
1754 {
1755 return steps_ + t;
1756 }
1757
1758 Concrete::Length
1759 Shapes::ApproximationSegmentSection2D::distanceAt( Concrete::Time t ) const
1760 {
1761 return baseSeg_->distanceAt( t );
1762 }
1763
1764 Concrete::Speed
1765 Shapes::ApproximationSegmentSection2D::maxSpeed( ) const
1766 {
1767 return baseSeg_->maxSpeed( );
1768 }
1769
1770 Concrete::Time
1771 Shapes::ApproximationSegmentSection2D::duration( ) const
1772 {
1773 return t1_ - t0_;
1774 }
1775
1776 namespace Shapes
1777 {
1778 class HullSorter2D : public std::binary_function< const Concrete::Coords2D *, const Concrete::Coords2D *, bool >
1779 {
1780 const Concrete::Coords2D & p_;
1781 public:
1782 HullSorter2D( const Concrete::Coords2D & p ) : p_( p ) { }
1783 bool operator () ( const Concrete::Coords2D * p1, const Concrete::Coords2D * p2 )
1784 {
1785 /* The comparison orders the points according to their counter-clockwise
1786 * angle to the positive x-axis.
1787 * Since (by construction) no point can have smaller y-coordinate than p, all angles
1788 * are in the range [ 0, pi ], and it suffices to reason in terms of the argument
1789 * to the monotonic function arccotan.
1790 * The ordinary counter-clockwise-of by means of computing the projection of the
1791 * second on the clockwise normal of the other leads to the same equation.
1792 */
1793 const Concrete::Length d1x = p1->x_ - p_.x_;
1794 const Concrete::Length d1y = p1->y_ - p_.y_;
1795 const Concrete::Length d2x = p2->x_ - p_.x_;
1796 const Concrete::Length d2y = p2->y_ - p_.y_;
1797 return d1y * d2x < d2y * d1x;
1798 }
1799 };
1800 }
1801
1802 RefCountPtr< const Lang::ElementaryPath2D >
1803 Lang::ElementaryPath2D::controlling_hull( ) const
1804 {
1805 if( empty( ) )
1806 {
1807 return Lang::THE_EMPTYPATH2D;
1808 }
1809
1810 if( size( ) == 1 )
1811 {
1812 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D( );
1813 const_iterator i = begin( );
1814 res->push_back( new Concrete::PathPoint2D( new Concrete::Coords2D( *((*i)->mid_) ) ) );
1815 if( closed_ )
1816 {
1817 if( (*i)->front_ != (*i)->mid_ )
1818 {
1819 res->push_back( new Concrete::PathPoint2D( new Concrete::Coords2D( *((*i)->front_) ) ) );
1820 }
1821 if( (*i)->rear_ != (*i)->mid_ )
1822 {
1823 res->push_back( new Concrete::PathPoint2D( new Concrete::Coords2D( *((*i)->rear_) ) ) );
1824 }
1825 }
1826 res->close( );
1827 return RefCountPtr< const Lang::ElementaryPath2D >( res );
1828 }
1829
1830 /* Compute convex hull using Graham scan algorithm.
1831 * The stack of points in the hull is split into the last point and the rest.
1832 */
1833
1834 std::vector< const Concrete::Coords2D * > sortedPoints( 0 );
1835 sortedPoints.reserve( 3 * size( ) );
1836
1837 /* Step 0: Place all distinct points in sortedPoints. The sort will take place soon...
1838 */
1839 {
1840 const_iterator i = begin( );
1841 if( closed_ )
1842 {
1843 if( (*i)->rear_ != (*i)->mid_ )
1844 {
1845 sortedPoints.push_back( (*i)->rear_ );
1846 }
1847 }
1848 sortedPoints.push_back( (*i)->mid_ );
1849 if( (*i)->front_ != (*i)->mid_ )
1850 {
1851 sortedPoints.push_back( (*i)->front_ );
1852 }
1853 ++i;
1854
1855 const_iterator theEnd = end( );
1856 --theEnd;
1857 for( ; i != theEnd; ++i )
1858 {
1859 if( (*i)->rear_ != (*i)->mid_ )
1860 {
1861 sortedPoints.push_back( (*i)->rear_ );
1862 }
1863 sortedPoints.push_back( (*i)->mid_ );
1864 if( (*i)->front_ != (*i)->mid_ )
1865 {
1866 sortedPoints.push_back( (*i)->front_ );
1867 }
1868 }
1869 if( (*i)->rear_ != (*i)->mid_ )
1870 {
1871 sortedPoints.push_back( (*i)->rear_ );
1872 }
1873 sortedPoints.push_back( (*i)->mid_ );
1874 if( closed_ )
1875 {
1876 if( (*i)->front_ != (*i)->mid_ )
1877 {
1878 sortedPoints.push_back( (*i)->front_ );
1879 }
1880 }
1881 }
1882
1883 /* Step 1: Find leftmost of the lowest vertexes. The sort will take place very soon...
1884 */
1885 const Concrete::Coords2D * last;
1886 const Concrete::Coords2D * start;
1887 {
1888 std::vector< const Concrete::Coords2D * >::iterator i = sortedPoints.begin( );
1889 std::vector< const Concrete::Coords2D * >::iterator start_i = i;
1890 start = *start_i;
1891 ++i;
1892 for( ; i != sortedPoints.end( ); ++i )
1893 {
1894 if( (*i)->y_ > start->y_ )
1895 {
1896 continue;
1897 }
1898 if( (*i)->y_ < start->y_ )
1899 {
1900 start_i = i;
1901 start = *start_i;
1902 continue;
1903 }
1904 if( (*i)->x_ >= start->x_ )
1905 {
1906 continue;
1907 }
1908 start_i = i;
1909 start = *start_i;
1910 }
1911 last = start;
1912 // Remove the starting point from the set by moving the last point to its position.
1913 *start_i = sortedPoints.back( );
1914 sortedPoints.pop_back( );
1915 }
1916
1917 /* Step 2: Sort remaining points.
1918 */
1919 std::sort( sortedPoints.begin( ), sortedPoints.end( ), Shapes::HullSorter2D( *start ) );
1920
1921 /* Step 3: Construct hull. The pointers are first placed in a list, and only copied to the new path once we know
1922 * what points belong to the hull.
1923 */
1924 const Concrete::LengthSquared TOL( 1e-20 );
1925 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D( );
1926 {
1927 std::list< const Concrete::Coords2D * > hullPoints;
1928 for( std::vector< const Concrete::Coords2D * >::const_iterator i = sortedPoints.begin( );
1929 i != sortedPoints.end( );
1930 ++i )
1931 {
1932 /* Don't trust points that are too close to the starting point, since
1933 * the angle sort is not reliable for these.
1934 */
1935 if( ( **i - *start ).normSquared( ) < TOL )
1936 {
1937 continue;
1938 }
1939 while( ! hullPoints.empty( ) )
1940 {
1941 /* Avoid including points that are too close to each other, as this makes
1942 * the direction between the points ill-defined.
1943 */
1944 if( ( **i - *last ).normSquared( ) < TOL )
1945 {
1946 last = hullPoints.back( );
1947 hullPoints.pop_back( );
1948 continue;
1949 }
1950
1951 /* In case the two points are on a line from the starting point, always keep the point
1952 * with the greatest distance to the starting point, and never keep the point
1953 * with the smaller distance -- this takes care of the degenerate
1954 * situation when the ordering in the previous sort was unclear.
1955 */
1956 Concrete::Coords2D dLast = *last - *start;
1957 Concrete::Coords2D di = **i - *start;
1958 if( dLast.angleCosineSignSquared( di ) > 1 - 1e-8 ) /* Test if angle is smaller approximately than approimately 1e-4. */
1959 {
1960 if( dLast.normSquared( ) <= di.normSquared( ) )
1961 {
1962 last = hullPoints.back( );
1963 hullPoints.pop_back( );
1964 }
1965 else
1966 {
1967 break;
1968 }
1969 }
1970
1971 /* If new point is to the right of the line from second last to last point,
1972 * then the last point is removed.
1973 * Let n be the outward pointing normal.
1974 */
1975 const Concrete::Coords2D * secondLast = hullPoints.back( );
1976 const Concrete::Length nx = last->y_ - secondLast->y_;
1977 const Concrete::Length ny = secondLast->x_ - last->x_;
1978 const Concrete::Length dx = (*i)->x_ - last->x_;
1979 const Concrete::Length dy = (*i)->y_ - last->y_;
1980 if( nx * dx + ny * dy >= Concrete::LengthSquared( 0 ) )
1981 {
1982 last = hullPoints.back( );
1983 hullPoints.pop_back( );
1984 }
1985 else
1986 {
1987 break;
1988 }
1989 }
1990 hullPoints.push_back( last );
1991 last = *i;
1992 }
1993 hullPoints.push_back( last );
1994
1995 for( std::list< const Concrete::Coords2D * >::const_iterator i = hullPoints.begin( );
1996 i != hullPoints.end( );
1997 ++i )
1998 {
1999 res->push_back( new Concrete::PathPoint2D( new Concrete::Coords2D( *(*i) ) ) );
2000 }
2001 }
2002 res->close( );
2003 return RefCountPtr< const Lang::ElementaryPath2D >( res );
2004 }
2005
2006 RefCountPtr< const Lang::ElementaryPath2D >
2007 Lang::ElementaryPath2D::upsample( const Computation::Upsampler2D & sampler ) const
2008 {
2009 if( empty( ) )
2010 {
2011 return Lang::THE_EMPTYPATH2D;
2012 }
2013
2014 std::vector< double > sampleTimes;
2015 Concrete::Coords2D rearHandle( 0, 0 );
2016
2017 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D( );
2018 if( closed_ )
2019 {
2020 res->close( );
2021 }
2022
2023 if( size( ) == 1 )
2024 {
2025 if( ! closed_ )
2026 {
2027 res->push_back( new Concrete::PathPoint2D( *front( ) ) );
2028 return RefCountPtr< const Lang::ElementaryPath2D >( res );
2029 }
2030
2031 const_iterator i1 = begin( );
2032 Bezier::ControlPoints< Concrete::Coords2D > controls( *(*i1)->mid_, *(*i1)->front_, *(*i1)->rear_, *(*i1)->mid_ );
2033 sampler( & sampleTimes, controls );
2034 if( sampleTimes.empty( ) )
2035 {
2036 res->push_back( new Concrete::PathPoint2D( *front( ) ) );
2037 return RefCountPtr< const Lang::ElementaryPath2D >( res );
2038 }
2039
2040 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
2041 {
2042 // We will compute the same sub section again later, but doing it twice makes the code cleaner and more similar to the general case below.
2043 Bezier::ControlPoints< Concrete::Coords2D > lastSeg( coeffs.subSection( sampleTimes.back( ), 1 ) );
2044 rearHandle = lastSeg.p2_;
2045 }
2046 sampleTimes.push_back( 1 );
2047 typedef typeof sampleTimes ListType;
2048 double t0 = 0;
2049 ListType::const_iterator ti = sampleTimes.begin( );
2050 double t1;
2051 for( ; ti != sampleTimes.end( ); t0 = t1, ++ti )
2052 {
2053 t1 = *ti;
2054 Bezier::ControlPoints< Concrete::Coords2D > seg( coeffs.subSection( t0, t1 ) );
2055 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( new Concrete::Coords2D( seg.p0_ ) );
2056 newPoint->front_ = new Concrete::Coords2D( seg.p1_ );
2057 newPoint->rear_ = new Concrete::Coords2D( rearHandle );
2058 res->push_back( newPoint );
2059 rearHandle = seg.p2_;
2060 }
2061
2062 return RefCountPtr< const Lang::ElementaryPath2D >( res );
2063 }
2064
2065 const_iterator i1 = begin( );
2066 const_iterator i2 = i1;
2067 ++i2;
2068
2069 // Determine the initial rear handle.
2070 if( closed_ )
2071 {
2072 const_iterator i0 = end( );
2073 --i0;
2074 Bezier::ControlPoints< Concrete::Coords2D > controls( *(*i0)->mid_, *(*i0)->front_, *(*i1)->rear_, *(*i1)->mid_ );
2075 sampleTimes.clear( ); // Not needed here, but included for symmetry with the general case below.
2076 sampler( & sampleTimes, controls );
2077 if( sampleTimes.empty( ) )
2078 {
2079 rearHandle = *(*i1)->rear_;
2080 }
2081 else
2082 {
2083 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
2084 Bezier::ControlPoints< Concrete::Coords2D > lastSeg( coeffs.subSection( sampleTimes.back( ), 1 ) );
2085 rearHandle = lastSeg.p2_;
2086 }
2087 }
2088 else
2089 {
2090 rearHandle = *(*i1)->rear_;
2091 }
2092
2093 for( ; i1 != end( ); ++i1, ++i2 )
2094 {
2095 if( i2 == end( ) )
2096 {
2097 if( closed_ )
2098 {
2099 i2 = begin( );
2100 }
2101 else
2102 {
2103 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( new Concrete::Coords2D( *(*i1)->mid_ ) );
2104 newPoint->front_ = new Concrete::Coords2D( *(*i1)->front_ );
2105 newPoint->rear_ = new Concrete::Coords2D( rearHandle );
2106 res->push_back( newPoint );
2107 break;
2108 }
2109 }
2110
2111 Bezier::ControlPoints< Concrete::Coords2D > controls( *(*i1)->mid_, *(*i1)->front_, *(*i2)->rear_, *(*i2)->mid_ );
2112 sampleTimes.clear( );
2113 sampler( & sampleTimes, controls );
2114 if( sampleTimes.empty( ) )
2115 {
2116 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( new Concrete::Coords2D( controls.p0_ ) );
2117 newPoint->front_ = new Concrete::Coords2D( controls.p1_ );
2118 newPoint->rear_ = new Concrete::Coords2D( rearHandle );
2119 res->push_back( newPoint );
2120 rearHandle = controls.p2_;
2121 }
2122 else
2123 {
2124 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
2125 sampleTimes.push_back( 1 );
2126 typedef typeof sampleTimes ListType;
2127 double t0 = 0;
2128 ListType::const_iterator ti = sampleTimes.begin( );
2129 double t1;
2130 for( ; ti != sampleTimes.end( ); t0 = t1, ++ti )
2131 {
2132 t1 = *ti;
2133 Bezier::ControlPoints< Concrete::Coords2D > seg( coeffs.subSection( t0, t1 ) );
2134 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( new Concrete::Coords2D( seg.p0_ ) );
2135 newPoint->front_ = new Concrete::Coords2D( seg.p1_ );
2136 newPoint->rear_ = new Concrete::Coords2D( rearHandle );
2137 res->push_back( newPoint );
2138 rearHandle = seg.p2_;
2139 }
2140 }
2141
2142 }
2143
2144 return RefCountPtr< const Lang::ElementaryPath2D >( res );
2145 }
2146
2147
2148 bool
2149 Lang::ElementaryPath2D::isConvexPoly( Concrete::Length tol ) const
2150 {
2151 for( const_iterator i = begin( ); i != end( ); ++i )
2152 {
2153 if( (*i)->front_ != (*i)->mid_ || (*i)->rear_ != (*i)->mid_ )
2154 {
2155 return false;
2156 }
2157 }
2158 if( size( ) <= 3 )
2159 {
2160 return true;
2161 }
2162 for( const_iterator i = begin( ); i != end( ); ++i )
2163 {
2164 const_iterator i1 = i;
2165 ++i1;
2166 if( i1 == end( ) )
2167 {
2168 i1 = begin( );
2169 }
2170 const_iterator i2 = i1;
2171 ++i2;
2172 if( i2 == end( ) )
2173 {
2174 i2 = begin( );
2175 }
2176 const_iterator i3 = i2;
2177 ++i3;
2178 if( i3 == end( ) )
2179 {
2180 i3 = begin( );
2181 }
2182
2183 // The following test is a local convexity test which does not need an orientation, but checks that the two points neighboring an edge are on the same side of the edge.
2184 Concrete::UnitFloatPair n = (*i)->mid_->normalizedOrthogonal( *((*i1)->mid_) );
2185 Concrete::Length tmp = Concrete::inner( n, *((*i2)->mid_) - *((*i)->mid_) );
2186 if( tmp < - tol )
2187 {
2188 if( Concrete::inner( n, *((*i3)->mid_) - *((*i)->mid_) ) > tol )
2189 {
2190 return false;
2191 }
2192 }
2193 else if( tmp > tol )
2194 {
2195 if( Concrete::inner( n, *((*i3)->mid_) - *((*i)->mid_) ) < - tol )
2196 {
2197 return false;
2198 }
2199 }
2200 }
2201 return true;
2202 }
2203
2204 bool
2205 Lang::ElementaryPath2D::convexPolyContains( const Concrete::Coords2D & p, Concrete::Length tol ) const
2206 {
2207 if( size( ) < 3 )
2208 {
2209 return false;
2210 }
2211
2212 // Since we know that we are a convex poly, the same-side test used in isConvexPoly can now be used with *((*i3)->mid_) replaced by p
2213 for( const_iterator i = begin( ); i != end( ); ++i )
2214 {
2215 const_iterator i1 = i;
2216 ++i1;
2217 if( i1 == end( ) )
2218 {
2219 i1 = begin( );
2220 }
2221 const_iterator i2 = i1;
2222 ++i2;
2223 if( i2 == end( ) )
2224 {
2225 i2 = begin( );
2226 }
2227
2228 Concrete::UnitFloatPair n = (*i)->mid_->normalizedOrthogonal( *((*i1)->mid_) );
2229 Concrete::Length tmp = Concrete::inner( n, *((*i2)->mid_) - *((*i)->mid_) );
2230 if( tmp < - tol )
2231 {
2232 if( Concrete::inner( n, p - *((*i)->mid_) ) > tol )
2233 {
2234 return false;
2235 }
2236 }
2237 else if( tmp > tol )
2238 {
2239 if( Concrete::inner( n, p - *((*i)->mid_) ) < - tol )
2240 {
2241 return false;
2242 }
2243 }
2244 }
2245 return true;
2246 }
2247
2248
2249 Concrete::Length
2250 Lang::ElementaryPath2D::arcLength( ) const
2251 {
2252 if( empty( ) )
2253 {
2254 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2255 }
2256 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2257 */
2258 Concrete::Length totlen = Concrete::ZERO_LENGTH;
2259
2260 const Concrete::Length arcdelta = Computation::the_arcdelta;
2261
2262 const_iterator i1 = begin( );
2263 const_iterator i2 = i1;
2264 ++i2;
2265 for( ; i1 != end( ); ++i1, ++i2 )
2266 {
2267 if( i2 == end( ) )
2268 {
2269 if( closed_ )
2270 {
2271 i2 = begin( );
2272 }
2273 else
2274 {
2275 break;
2276 }
2277 }
2278 if( (*i1)->front_ == (*i1)->mid_ &&
2279 (*i2)->rear_ == (*i2)->mid_ )
2280 {
2281 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2282 totlen += segLength;
2283 continue;
2284 }
2285
2286 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2287 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2288 Concrete::Bezier x1 = (*i1)->front_->x_.offtype< 0, 3 >( );
2289 Concrete::Bezier y1 = (*i1)->front_->y_.offtype< 0, 3 >( );
2290 Concrete::Bezier x2 = (*i2)->rear_->x_.offtype< 0, 3 >( );
2291 Concrete::Bezier y2 = (*i2)->rear_->y_.offtype< 0, 3 >( );
2292 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2293 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2294
2295 const Concrete::Length segLengthBound =
2296 ( hypotPhysical( x1-x0, y1-y0 ) + hypotPhysical( x2-x1, y2-y1 ) + hypotPhysical( x3-x2, y3-y2 ) ).offtype< 0, -3 >( );
2297
2298 if( segLengthBound < arcdelta )
2299 {
2300 totlen += segLengthBound;
2301 }
2302 else
2303 {
2304 Concrete::Time dt = Shapes::computeDt( segLengthBound );
2305 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
2306 for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
2307 {
2308 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
2309 Physical< 0, 2 > kv0 = -3 * tc * tc;
2310 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
2311 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
2312 Physical< 0, 2 > kv3 = 3 * t * t;
2313 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
2314 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
2315
2316 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
2317 tmpSum_l += dl;
2318 }
2319 totlen += tmpSum_l * dt.offtype< 0, 1 >( );
2320 }
2321 }
2322
2323 return totlen;
2324 }
2325
2326 Concrete::Length
2327 Lang::ElementaryPath2D::arcLength( Concrete::Time tRemaining ) const
2328 {
2329 if( empty( ) )
2330 {
2331 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2332 }
2333 if( tRemaining < Concrete::ZERO_TIME )
2334 {
2335 return negative_arcLength( -tRemaining );
2336 }
2337 if( isinfPhysical( tRemaining ) )
2338 {
2339 throw Exceptions::OutOfRange( "The arctime to infinity is not defined." );
2340 }
2341 if( tRemaining > Concrete::Time( duration( ) ) && ! closed_ )
2342 {
2343 throw Exceptions::OutOfRange( "Too big arctime for open path." );
2344 }
2345 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2346 */
2347 Concrete::Length totlen = Concrete::ZERO_LENGTH;
2348
2349 const Concrete::Length arcdelta = Computation::the_arcdelta;
2350
2351 {
2352 const double revolutions = floor( tRemaining / Concrete::Time( duration( ) ) );
2353 if( revolutions > 0 )
2354 {
2355 totlen += revolutions * arcLength( );
2356 tRemaining -= revolutions * Concrete::Time( duration( ) );
2357 }
2358 }
2359
2360 const_iterator i1 = begin( );
2361 const_iterator i2 = i1;
2362 ++i2;
2363 for( ; tRemaining > Concrete::ZERO_TIME; ++i1, ++i2, tRemaining -= Concrete::UNIT_TIME )
2364 {
2365 if( i1 == end( ) )
2366 {
2367 /* This could be considered an error situation, but it may be due to numeric
2368 * errors, so we take action accordingly.
2369 */
2370 break;
2371 }
2372 if( i2 == end( ) )
2373 {
2374 /* This could also be an error situation...
2375 */
2376 i2 = begin( );
2377 }
2378 if( (*i1)->front_ == (*i1)->mid_ &&
2379 (*i2)->rear_ == (*i2)->mid_ )
2380 {
2381 if( tRemaining <= Concrete::UNIT_TIME )
2382 {
2383 Concrete::Time t1 = tRemaining;
2384 Concrete::Time tc = Concrete::UNIT_TIME - t1; /* complement to t1 */
2385 Concrete::Coords2D tmp =
2386 ( tc * tc * ( tc + 3 * t1 ) ).offtype< 0, 3 >( ) * (*((*i1)->mid_)) +
2387 ( t1 * t1 * ( t1 + 3 * tc ) ).offtype< 0, 3 >( ) * (*((*i2)->mid_));
2388 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - tmp.x_, (*i1)->mid_->y_ - tmp.y_ );
2389 totlen += segLength;
2390 break;
2391 }
2392 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2393 totlen += segLength;
2394 continue;
2395 }
2396
2397 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2398 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2399 Concrete::Bezier x1 = (*i1)->front_->x_.offtype< 0, 3 >( );
2400 Concrete::Bezier y1 = (*i1)->front_->y_.offtype< 0, 3 >( );
2401 Concrete::Bezier x2 = (*i2)->rear_->x_.offtype< 0, 3 >( );
2402 Concrete::Bezier y2 = (*i2)->rear_->y_.offtype< 0, 3 >( );
2403 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2404 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2405
2406 const Concrete::Length segLengthBound =
2407 ( hypotPhysical( x1-x0, y1-y0 ) + hypotPhysical( x2-x1, y2-y1 ) + hypotPhysical( x3-x2, y3-y2 ) ).offtype< 0, -3 >( );
2408
2409 if( segLengthBound < arcdelta )
2410 {
2411 if( tRemaining <= Concrete::UNIT_TIME )
2412 {
2413 totlen += double(tRemaining / Concrete::UNIT_TIME ) * segLengthBound;
2414 break;
2415 }
2416 totlen += segLengthBound;
2417 }
2418 else
2419 {
2420 Concrete::Time dt = Shapes::computeDt( segLengthBound );
2421 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
2422 const Concrete::Time tend = min( Concrete::UNIT_TIME, tRemaining );
2423 for( Concrete::Time t = Concrete::ZERO_TIME; t < tend; t += dt )
2424 {
2425 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
2426 Physical< 0, 2 > kv0 = -3 * tc * tc;
2427 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
2428 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
2429 Physical< 0, 2 > kv3 = 3 * t * t;
2430 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
2431 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
2432
2433 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
2434 tmpSum_l += dl;
2435 }
2436 totlen += tmpSum_l * dt.offtype< 0, 1 >( );
2437 }
2438 }
2439
2440 return totlen;
2441 }
2442
2443 Concrete::Length
2444 Lang::ElementaryPath2D::negative_arcLength( Concrete::Time tRemaining ) const
2445 {
2446 if( empty( ) )
2447 {
2448 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2449 }
2450 if( tRemaining < Concrete::ZERO_TIME )
2451 {
2452 throw Exceptions::InternalError( "Negative tRemaining in negative_arcLength." );
2453 }
2454 if( ! closed_ )
2455 {
2456 throw Exceptions::OutOfRange( "The arctime at negative lengths is not defined for open paths." );
2457 }
2458 if( isinfPhysical( tRemaining ) )
2459 {
2460 throw Exceptions::OutOfRange( "The arctime to infinity is not defined." );
2461 }
2462
2463 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2464 */
2465 Concrete::Length totlen = Concrete::ZERO_LENGTH;
2466
2467 const Concrete::Length arcdelta = Computation::the_arcdelta;
2468
2469 {
2470 const double revolutions = floor( tRemaining / Concrete::Time( duration( ) ) );
2471 if( revolutions > 0 )
2472 {
2473 totlen -= revolutions * arcLength( );
2474 tRemaining -= revolutions * Concrete::Time( duration( ) );
2475 }
2476 }
2477
2478 const_reverse_iterator i1 = rbegin( );
2479 const_reverse_iterator i2 = i1;
2480 if( closed_ )
2481 {
2482 i1 = rend( );
2483 --i1;
2484 i2 = rbegin( );
2485 }
2486 else
2487 {
2488 ++i2;
2489 }
2490 for( ; tRemaining > Concrete::ZERO_TIME; ++i1, ++i2, tRemaining -= Concrete::UNIT_TIME )
2491 {
2492 if( i2 == rend( ) )
2493 {
2494 /* This could be considered an error situation, but it may be due to numeric
2495 * errors, so we take action accordingly.
2496 */
2497 break;
2498 }
2499 if( i1 == rend( ) )
2500 {
2501 i1 = rbegin( );
2502 }
2503 if( (*i1)->rear_ == (*i1)->mid_ &&
2504 (*i2)->front_ == (*i2)->mid_ )
2505 {
2506 if( tRemaining <= Concrete::UNIT_TIME )
2507 {
2508 Concrete::Time t1 = tRemaining;
2509 Concrete::Time tc = Concrete::UNIT_TIME - t1; /* complement to t1 */
2510 Concrete::Coords2D tmp =
2511 ( tc * tc * ( tc + 3 * t1 ) ).offtype< 0, 3 >( ) * (*((*i1)->mid_)) +
2512 ( t1 * t1 * ( t1 + 3 * tc ) ).offtype< 0, 3 >( ) * (*((*i2)->mid_));
2513 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - tmp.x_, (*i1)->mid_->y_ - tmp.y_ );
2514 totlen -= segLength;
2515 break;
2516 }
2517 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2518 totlen -= segLength;
2519 continue;
2520 }
2521
2522 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2523 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2524 Concrete::Bezier x1 = (*i1)->rear_->x_.offtype< 0, 3 >( );
2525 Concrete::Bezier y1 = (*i1)->rear_->y_.offtype< 0, 3 >( );
2526 Concrete::Bezier x2 = (*i2)->front_->x_.offtype< 0, 3 >( );
2527 Concrete::Bezier y2 = (*i2)->front_->y_.offtype< 0, 3 >( );
2528 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2529 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2530
2531 const Concrete::Length segLengthBound =
2532 ( hypotPhysical( x1-x0, y1-y0 ) + hypotPhysical( x2-x1, y2-y1 ) + hypotPhysical( x3-x2, y3-y2 ) ).offtype< 0, -3 >( );
2533
2534 if( segLengthBound < arcdelta )
2535 {
2536 if( tRemaining <= Concrete::UNIT_TIME )
2537 {
2538 totlen -= ( tRemaining / Concrete::UNIT_TIME ) * segLengthBound;
2539 break;
2540 }
2541 totlen -= segLengthBound;
2542 }
2543 else
2544 {
2545 Concrete::Time dt = Shapes::computeDt( segLengthBound );
2546 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
2547 const Concrete::Time tend = min( Concrete::UNIT_TIME, tRemaining );
2548 for( Concrete::Time t = Concrete::ZERO_TIME; t < tend; t += dt )
2549 {
2550 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
2551 Physical< 0, 2 > kv0 = -3 * tc * tc;
2552 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
2553 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
2554 Physical< 0, 2 > kv3 = 3 * t * t;
2555 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
2556 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
2557
2558 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
2559 tmpSum_l += dl;
2560 }
2561 totlen -= tmpSum_l * dt.offtype< 0, 1 >( );
2562 }
2563 }
2564
2565 return totlen;
2566 }
2567
2568 Concrete::SplineTime
2569 Lang::ElementaryPath2D::arcTime( const Concrete::Length & t, Concrete::Time t0 ) const
2570 {
2571 if( empty( ) )
2572 {
2573 throw Exceptions::OutOfRange( "The empty path has no arctimes defined." );
2574 }
2575 if( duration( ) == 0 )
2576 {
2577 throw Exceptions::OutOfRange( "The singleton path has no arctimes defined." );
2578 }
2579 if( t >= Concrete::HUGE_LENGTH )
2580 {
2581 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2582 }
2583 if( t < Concrete::ZERO_LENGTH )
2584 {
2585 return negative_arcTime( - t, t0 );
2586 }
2587
2588 Concrete::Time splineTime = t0;
2589 t0 = modPhysical( t0, Concrete::Time( duration( ) ) );
2590 if( t0 < Concrete::ZERO_TIME )
2591 {
2592 t0 += Concrete::Time( duration( ) );
2593 }
2594
2595 const_iterator i1 = begin( );
2596 while( t0 >= Concrete::UNIT_TIME )
2597 {
2598 t0 -= Concrete::UNIT_TIME;
2599 ++i1;
2600 if( i1 == end( ) )
2601 {
2602 if( ! closed_ )
2603 {
2604 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2605 }
2606 i1 = begin( );
2607 }
2608 }
2609 const_iterator i2 = i1;
2610 ++i2;
2611
2612 const Concrete::Length arcdelta = Computation::the_arcdelta;
2613 Concrete::Length remainingLength = t;
2614
2615 if( t0 > Concrete::ZERO_TIME )
2616 {
2617 if( i2 == end( ) )
2618 {
2619 if( ! closed_ )
2620 {
2621 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2622 }
2623 i2 = begin( );
2624 }
2625 if( (*i1)->front_ == (*i1)->mid_ &&
2626 (*i2)->rear_ == (*i2)->mid_ )
2627 {
2628 Concrete::Time tc = Concrete::UNIT_TIME - t0; /* complement to t0 */
2629 Concrete::Coords2D tmp =
2630 ( tc * tc * ( tc + 3 * t0 ) ).offtype< 0, 3 >( ) * (*((*i1)->mid_)) +
2631 ( t0 * t0 * ( t0 + 3 * tc ) ).offtype< 0, 3 >( ) * (*((*i2)->mid_));
2632 const Concrete::Length segRestLength = hypotPhysical( tmp.x_ - (*i2)->mid_->x_, tmp.y_ - (*i2)->mid_->y_ );
2633 if( segRestLength < remainingLength )
2634 {
2635 remainingLength -= segRestLength;
2636 goto beginLoop;
2637 }
2638 const Concrete::Length segPastLength = hypotPhysical( tmp.x_ - (*i1)->mid_->x_, tmp.y_ - (*i1)->mid_->y_ );
2639 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2640 splineTime += Shapes::straightLineArcTime( ( segPastLength + remainingLength ) / segLength ) - t0;
2641 goto done;
2642 }
2643
2644 {
2645 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2646 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2647 Concrete::Bezier x1 = (*i1)->front_->x_.offtype< 0, 3 >( );
2648 Concrete::Bezier y1 = (*i1)->front_->y_.offtype< 0, 3 >( );
2649 Concrete::Bezier x2 = (*i2)->rear_->x_.offtype< 0, 3 >( );
2650 Concrete::Bezier y2 = (*i2)->rear_->y_.offtype< 0, 3 >( );
2651 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2652 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2653
2654 const Concrete::Length segLengthBound =
2655 ( hypotPhysical( (x1-x0), (y1-y0) ) + hypotPhysical( (x2-x1), (y2-y1) ) + hypotPhysical( (x3-x2), (y3-y2) ) ).offtype< 0, -3 >( );
2656
2657 if( segLengthBound < arcdelta )
2658 {
2659 remainingLength -= segLengthBound;
2660 if( remainingLength <= Concrete::ZERO_LENGTH )
2661 {
2662 splineTime += Concrete::Time( 0.5 );
2663 goto done;
2664 }
2665 }
2666 else
2667 {
2668 Concrete::Time dt = Shapes::computeDt( segLengthBound );
2669
2670 const Concrete::Length remainingDiv_dt = remainingLength / dt.offtype< 0, 1 >( );
2671 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
2672 for( Concrete::Time t = t0; t < Concrete::UNIT_TIME; t += dt )
2673 {
2674 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
2675 Physical< 0, 2 > kv0 = -3 * tc * tc;
2676 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
2677 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
2678 Physical< 0, 2 > kv3 = 3 * t * t;
2679 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
2680 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
2681
2682 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
2683 tmpSum_l += dl;
2684 if( tmpSum_l >= remainingDiv_dt )
2685 {
2686 splineTime += t - t0;
2687 goto done;
2688 }
2689 }
2690 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
2691 }
2692 }
2693
2694 beginLoop:
2695 splineTime = Concrete::Time( ceil( splineTime.offtype< 0, 1 >( ) ) );
2696 ++i1;
2697 if( i1 == end( ) )
2698 {
2699 if( ! closed_ )
2700 {
2701 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2702 }
2703 i1 = begin( );
2704 }
2705 ++i2;
2706 }
2707
2708 for( ; ; ++i1, ++i2, splineTime += Concrete::UNIT_TIME )
2709 {
2710 if( i1 == end( ) )
2711 {
2712 /* If not closed, then the break occurs when i2 reaches end
2713 */
2714 i1 = begin( );
2715 }
2716 if( i2 == end( ) )
2717 {
2718 if( ! closed_ )
2719 {
2720 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2721 }
2722 i2 = begin( );
2723 }
2724 if( (*i1)->front_ == (*i1)->mid_ &&
2725 (*i2)->rear_ == (*i2)->mid_ )
2726 {
2727 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2728 if( segLength < remainingLength )
2729 {
2730 remainingLength -= segLength;
2731 continue;
2732 }
2733 splineTime += Shapes::straightLineArcTime( remainingLength / segLength );
2734 goto done;
2735 }
2736
2737 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2738 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2739 Concrete::Bezier x1 = (*i1)->front_->x_.offtype< 0, 3 >( );
2740 Concrete::Bezier y1 = (*i1)->front_->y_.offtype< 0, 3 >( );
2741 Concrete::Bezier x2 = (*i2)->rear_->x_.offtype< 0, 3 >( );
2742 Concrete::Bezier y2 = (*i2)->rear_->y_.offtype< 0, 3 >( );
2743 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2744 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2745
2746 const Concrete::Length segLengthBound =
2747 ( hypotPhysical( (x1-x0), (y1-y0) ) + hypotPhysical( (x2-x1), (y2-y1) ) + hypotPhysical( (x3-x2), (y3-y2) ) ).offtype< 0, -3 >( );
2748
2749 if( segLengthBound < arcdelta )
2750 {
2751 remainingLength -= segLengthBound;
2752 if( remainingLength <= Concrete::ZERO_LENGTH )
2753 {
2754 splineTime += Concrete::Time( 0.5 );
2755 goto done;
2756 }
2757 }
2758 else
2759 {
2760 Concrete::Time dt = Shapes::computeDt( segLengthBound );
2761
2762 const Concrete::Length remainingDiv_dt = remainingLength / dt.offtype< 0, 1 >( );
2763 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
2764 for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
2765 {
2766 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
2767 Physical< 0, 2 > kv0 = -3 * tc * tc;
2768 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
2769 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
2770 Physical< 0, 2 > kv3 = 3 * t * t;
2771 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
2772 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
2773
2774 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
2775 tmpSum_l += dl;
2776 if( tmpSum_l >= remainingDiv_dt )
2777 {
2778 splineTime += t;
2779 goto done;
2780 }
2781 }
2782 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
2783 }
2784 }
2785
2786 done:
2787 return splineTime;
2788 }
2789
2790 Concrete::SplineTime
2791 Lang::ElementaryPath2D::negative_arcTime( const Concrete::Length deltaLen, Concrete::Time t0 ) const
2792 {
2793 if( deltaLen < Concrete::ZERO_LENGTH )
2794 {
2795 throw Exceptions::InternalError( "Negative t in negative_arcTime." );
2796 }
2797 if( empty( ) )
2798 {
2799 throw Exceptions::OutOfRange( "The empty path has no arctimes defined." );
2800 }
2801 if( duration( ) == 0 )
2802 {
2803 throw Exceptions::OutOfRange( "The singleton path has no arctimes defined." );
2804 }
2805
2806 if( deltaLen >= Concrete::HUGE_LENGTH )
2807 {
2808 return Concrete::SplineTime( Concrete::ZERO_TIME, true );
2809 }
2810
2811 Concrete::Time splineTime = t0;
2812 if( isClosed( ) )
2813 {
2814 t0 = modPhysical( t0, Concrete::Time( duration( ) ) );
2815 if( t0 < 0 )
2816 {
2817 t0 += Concrete::Time( duration( ) );
2818 }
2819 }
2820 else
2821 {
2822 if( t0 < 0 )
2823 {
2824 t0 = 0;
2825 }
2826 else if( t0 > Concrete::Time( duration( ) ) )
2827 {
2828 t0 = Concrete::Time( duration( ) );
2829 }
2830 }
2831 t0 = Concrete::Time( duration( ) ) - t0;
2832 // From here on, t0 is the (positive) backwards-time
2833
2834 const_reverse_iterator i1 = rbegin( );
2835 if( closed_ )
2836 {
2837 i1 = rend( );
2838 --i1;
2839 }
2840 while( t0 >= Concrete::UNIT_TIME )
2841 {
2842 t0 -= Concrete::UNIT_TIME;
2843 ++i1;
2844 if( i1 == rend( ) )
2845 {
2846 if( ! closed_ )
2847 {
2848 return Concrete::SplineTime( Concrete::ZERO_TIME, true );
2849 }
2850 i1 = rbegin( );
2851 }
2852 }
2853 const_reverse_iterator i2 = i1;
2854 ++i2;
2855
2856 const Concrete::Length arcdelta = Computation::the_arcdelta;
2857 Concrete::Length remainingLength = deltaLen;
2858
2859 if( t0 > Concrete::ZERO_TIME )
2860 {
2861 if( i2 == rend( ) )
2862 {
2863 if( ! closed_ )
2864 {
2865 return Concrete::SplineTime( Concrete::ZERO_TIME, true );
2866 }
2867 i2 = rbegin( );
2868 }
2869 if( (*i1)->rear_ == (*i1)->mid_ &&
2870 (*i2)->front_ == (*i2)->mid_ )
2871 {
2872 Concrete::Time tc = Concrete::UNIT_TIME - t0; /* complement to t0 */
2873 Concrete::Coords2D tmp =
2874 ( tc * tc * ( tc + 3 * t0 ) ).offtype< 0, 3 >( ) * (*((*i1)->mid_)) +
2875 ( t0 * t0 * ( t0 + 3 * tc ) ).offtype< 0, 3 >( ) * (*((*i2)->mid_));
2876 const Concrete::Length segRestLength = hypotPhysical( tmp.x_ - (*i2)->mid_->x_, tmp.y_ - (*i2)->mid_->y_ );
2877 if( segRestLength < remainingLength )
2878 {
2879 remainingLength -= segRestLength;
2880 goto beginLoop;
2881 }
2882 const Concrete::Length segPastLength = hypotPhysical( tmp.x_ - (*i1)->mid_->x_, tmp.y_ - (*i1)->mid_->y_ );
2883 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2884 splineTime -= Shapes::straightLineArcTime( ( segPastLength + remainingLength ) / segLength ) - t0;
2885 goto done;
2886 }
2887
2888 {
2889 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2890 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2891 Concrete::Bezier x1 = (*i1)->rear_->x_.offtype< 0, 3 >( );
2892 Concrete::Bezier y1 = (*i1)->rear_->y_.offtype< 0, 3 >( );
2893 Concrete::Bezier x2 = (*i2)->front_->x_.offtype< 0, 3 >( );
2894 Concrete::Bezier y2 = (*i2)->front_->y_.offtype< 0, 3 >( );
2895 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2896 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2897
2898 const Concrete::Length segLengthBound =
2899 ( hypotPhysical( (x1-x0), (y1-y0) ) + hypotPhysical( (x2-x1), (y2-y1) ) + hypotPhysical( (x3-x2), (y3-y2) ) ).offtype< 0, -3 >( );
2900
2901 if( segLengthBound < arcdelta )
2902 {
2903 remainingLength -= segLengthBound;
2904 if( remainingLength <= Concrete::ZERO_LENGTH )
2905 {
2906 splineTime -= Concrete::Time( 0.5 );
2907 goto done;
2908 }
2909 }
2910 else
2911 {
2912 Concrete::Time dt = Shapes::computeDt( segLengthBound );
2913
2914 const Concrete::Length remainingDiv_dt = remainingLength / dt.offtype< 0, 1 >( );
2915 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
2916 for( Concrete::Time t = t0; t < Concrete::UNIT_TIME; t += dt )
2917 {
2918 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
2919 Physical< 0, 2 > kv0 = -3 * tc * tc;
2920 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
2921 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
2922 Physical< 0, 2 > kv3 = 3 * t * t;
2923 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
2924 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
2925
2926 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
2927 tmpSum_l += dl;
2928 if( tmpSum_l >= remainingDiv_dt )
2929 {
2930 splineTime -= t - t0;
2931 goto done;
2932 }
2933 }
2934 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
2935 }
2936 }
2937
2938 beginLoop:
2939 splineTime = Concrete::Time( floor( splineTime.offtype< 0, 1 >( ) ) );
2940 ++i1;
2941 if( i1 == rend( ) )
2942 {
2943 if( ! closed_ )
2944 {
2945 return Concrete::SplineTime( Concrete::ZERO_TIME, true );
2946 }
2947 i1 = rbegin( );
2948 }
2949 ++i2;
2950 }
2951
2952 for( ; ; ++i1, ++i2, splineTime -= Concrete::UNIT_TIME )
2953 {
2954 if( i1 == rend( ) )
2955 {
2956 /* If not closed, then the break occurs when i2 reaches end
2957 */
2958 i1 = rbegin( );
2959 }
2960 if( i2 == rend( ) )
2961 {
2962 if( ! closed_ )
2963 {
2964 return Concrete::SplineTime( Concrete::ZERO_TIME, true );
2965 }
2966 i2 = rbegin( );
2967 }
2968 if( (*i1)->rear_ == (*i1)->mid_ &&
2969 (*i2)->front_ == (*i2)->mid_ )
2970 {
2971 const Concrete::Length segLength = hypotPhysical( (*i1)->mid_->x_ - (*i2)->mid_->x_, (*i1)->mid_->y_ - (*i2)->mid_->y_ );
2972 if( segLength < remainingLength )
2973 {
2974 remainingLength -= segLength;
2975 continue;
2976 }
2977 splineTime -= Shapes::straightLineArcTime( remainingLength / segLength );
2978 goto done;
2979 }
2980
2981 Concrete::Bezier x0 = (*i1)->mid_->x_.offtype< 0, 3 >( );
2982 Concrete::Bezier y0 = (*i1)->mid_->y_.offtype< 0, 3 >( );
2983 Concrete::Bezier x1 = (*i1)->rear_->x_.offtype< 0, 3 >( );
2984 Concrete::Bezier y1 = (*i1)->rear_->y_.offtype< 0, 3 >( );
2985 Concrete::Bezier x2 = (*i2)->front_->x_.offtype< 0, 3 >( );
2986 Concrete::Bezier y2 = (*i2)->front_->y_.offtype< 0, 3 >( );
2987 Concrete::Bezier x3 = (*i2)->mid_->x_.offtype< 0, 3 >( );
2988 Concrete::Bezier y3 = (*i2)->mid_->y_.offtype< 0, 3 >( );
2989
2990 const Concrete::Length segLengthBound =
2991 ( hypotPhysical( (x1-x0), (y1-y0) ) + hypotPhysical( (x2-x1), (y2-y1) ) + hypotPhysical( (x3-x2), (y3-y2) ) ).offtype< 0, -3 >( );
2992
2993 if( segLengthBound < arcdelta )
2994 {
2995 remainingLength -= segLengthBound;
2996 if( remainingLength <= Concrete::ZERO_LENGTH )
2997 {
2998 splineTime -= Concrete::Time( 0.5 );
2999 goto done;
3000 }
3001 }
3002 else
3003 {
3004 Concrete::Time dt = Shapes::computeDt( segLengthBound );
3005
3006 const Concrete::Length remainingDiv_dt = remainingLength / dt.offtype< 0, 1 >( );
3007 Concrete::Length tmpSum_l = Concrete::ZERO_LENGTH;
3008 for( Concrete::Time t = Concrete::ZERO_TIME; t < Concrete::UNIT_TIME; t += dt )
3009 {
3010 Concrete::Time tc = Concrete::UNIT_TIME - t; /* complement to t */
3011 Physical< 0, 2 > kv0 = -3 * tc * tc;
3012 Physical< 0, 2 > kv1 = 3 * tc * tc - 6 * tc * t;
3013 Physical< 0, 2 > kv2 = 6 * tc * t - 3 * t * t;
3014 Physical< 0, 2 > kv3 = 3 * t * t;
3015 Concrete::Speed vx = x0 * kv0 + x1 * kv1 + x2 * kv2 + x3 * kv3;
3016 Concrete::Speed vy = y0 * kv0 + y1 * kv1 + y2 * kv2 + y3 * kv3;
3017
3018 Concrete::Length dl = hypotPhysical( vx, vy ).offtype< 0, -1 >( );
3019 tmpSum_l += dl;
3020 if( tmpSum_l >= remainingDiv_dt )
3021 {
3022 splineTime -= t;
3023 goto done;
3024 }
3025 }
3026 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
3027 }
3028 }
3029
3030 done:
3031 return splineTime;
3032 }
3033
3034 namespace Shapes
3035 {
3036 namespace Computation
3037 {
3038 typedef unsigned char WindingSigns;
3039
3040 const WindingSigns NO_SIGN = 0x00;
3041 const WindingSigns POS_X = 0x01;
3042 const WindingSigns POS_Y = 0x02;
3043
3044 void
3045 windingNumberHelper( const Concrete::Coords2D & p, const Computation::WindingSigns QUAD_1, const Computation::WindingSigns QUAD_2,
3046 const Concrete::Coords2D & p1, const Concrete::Coords2D & p2,
3047 int * count, WindingSigns * lastSigns )
3048 {
3049 Concrete::Coords2D d = p2 - p;
3050 WindingSigns signs = ( ( d.x_ > 0 ) ? POS_X : NO_SIGN ) | ( ( d.y_ > 0 ) ? POS_Y : NO_SIGN );
3051 switch( signs ^ *lastSigns )
3052 {
3053 case NO_SIGN:
3054 break;
3055 case POS_X:
3056 case POS_Y:
3057 if( *lastSigns == QUAD_1 && signs == QUAD_2 )
3058 {
3059 ++*count;
3060 }
3061 else if( *lastSigns == QUAD_2 && signs == QUAD_1 )
3062 {
3063 --*count;
3064 }
3065 break;
3066 case POS_X | POS_Y:
3067 /* There is a switch from one quadrand to the opposite quadrant,
3068 * and we must determine an intermediate quadrant so that we know the direction of rotation.
3069 * The intermediate point is selected as the point on the segment which is closest to the origin.
3070 */
3071 {
3072 Concrete::UnitFloatPair n = d.normalizedOrthogonalNoFail( p1 - p );
3073 Concrete::Coords2D dMid = n * Concrete::inner( d, n );
3074 WindingSigns signsMid = ( ( dMid.x_ > 0 ) ? POS_X : NO_SIGN ) | ( ( dMid.y_ > 0 ) ? POS_Y : NO_SIGN );
3075 if( *lastSigns == QUAD_1 && signsMid == QUAD_2 )
3076 {
3077 ++*count;
3078 }
3079 else if( *lastSigns == QUAD_2 && signsMid == QUAD_1 )
3080 {
3081 --*count;
3082 }
3083 if( signsMid == QUAD_1 && signs == QUAD_2 )
3084 {
3085 ++*count;
3086 }
3087 else if( signsMid == QUAD_2 && signs == QUAD_1 )
3088 {
3089 --*count;
3090 }
3091 }
3092 break;
3093 default:
3094 throw Exceptions::InternalError( "windingNumberHelper: Bitwise computation went astray." );
3095 }
3096 *lastSigns = signs;
3097 }
3098
3099 bool
3100 convexHullMember( const Concrete::Coords2D & p, const Concrete::Coords2D & g0, const Concrete::Coords2D & g1, const Concrete::Coords2D & g2, const Concrete::Coords2D & g3 )
3101 {
3102 /* If and only if p is not a member of the convex hull of the generators, there is a normal direction that proves it, and this normal direction
3103 * is orthogonal to one pair of generators.
3104 * ... except for the deflated case, when we must also test one more direction. See below.
3105 * When this function is used by the winding number algorith, a true result will cause the segment to be sub-divided, and if in doubt,
3106 * sub-dividing is the safer option. Hence, we shall prefer to return true when in doubt.
3107 */
3108 std::vector< const Concrete::Coords2D * > pointSet( 0 );
3109 pointSet.reserve( 4 );
3110 pointSet.push_back( & g0 );
3111 pointSet.push_back( & g1 );
3112 pointSet.push_back( & g2 );
3113 pointSet.push_back( & g3 );
3114
3115 const Concrete::Length SEP_TOL = 1e-10 * std::max( ( g1 - g0 ).norm( ),
3116 std::max( ( g2 - g1 ).norm( ),
3117 ( g3 - g2 ).norm( ) ) );
3118
3119 for( std::vector< const Concrete::Coords2D * >::const_iterator i0 = pointSet.begin( ); i0 != pointSet.end( ); ++i0 )
3120 {
3121 const Concrete::Coords2D & p0( **i0 );
3122 std::vector< const Concrete::Coords2D * >::const_iterator i1 = i0;
3123 ++i1;
3124 for( ; i1 != pointSet.end( ); ++i1 )
3125 {
3126 const Concrete::Coords2D & p1( **i1 );
3127 Concrete::UnitFloatPair n = p0.normalizedOrthogonalNoFail( p1 ); /* In order to make sense of SEP_TOL, this needs to be normalized. */
3128 Concrete::Length dp = Concrete::inner( n, p - p0 );
3129 if( dp < - SEP_TOL )
3130 {
3131 n = n.reverse( );
3132 dp = -dp;
3133 }
3134 else if( dp < SEP_TOL )
3135 {
3136 /* p is on the line from p0 to p1 */
3137 return true;
3138 }
3139 dp -= SEP_TOL;
3140 bool separated = true;
3141 for( std::vector< const Concrete::Coords2D * >::const_iterator i2 = pointSet.begin( ); i2 != pointSet.end( ); ++i2 )
3142 {
3143 if( i2 == i0 || i2 == i1 )
3144 {
3145 continue;
3146 }
3147 if( Concrete::inner( n, **i2 - p0 ) >= dp )
3148 {
3149 separated = false;
3150 break;
3151 }
3152 }
3153 if( separated )
3154 {
3155 return false;
3156 }
3157 }
3158 }
3159
3160 {
3161 /* Take care of the degenerate case.
3162 * The normal direction may be taken as the direction from any vertex to the query point, and need not be normalized.
3163 * The convex hull is separated if the query point is further away in the normal direction than any of the vertices.
3164 */
3165 std::vector< const Concrete::Coords2D * >::const_iterator i2 = pointSet.begin( );
3166 const Concrete::Coords2D & p0( **i2 );
3167 ++i2;
3168 Concrete::UnitFloatPair n( Concrete::Length::offtype( p.x_ - p0.x_ ), Concrete::Length::offtype( p.y_ - p0.y_ ), bool( ) );
3169 Concrete::Length dp = Concrete::inner( n, p - p0 ); /* Will be positive by construction. */
3170 bool separated = true;
3171 for( ; i2 != pointSet.end( ); ++i2 )
3172 {
3173 if( Concrete::inner( n, **i2 - p0 ) >= dp )
3174 {
3175 separated = false;
3176 break;
3177 }
3178 }
3179 if( separated )
3180 {
3181 return false;
3182 }
3183 }
3184
3185 /* No direction proving separation was found. */
3186 return true;
3187 }
3188 }
3189 }
3190
3191 int
3192 Lang::ElementaryPath2D::windingNumber( const Concrete::Coords2D & p ) const
3193 {
3194 if( ! closed_ )
3195 {
3196 throw Exceptions::InternalError( "ElementaryPath2D::windingNumber called with non-closed path." );
3197 }
3198 if( empty( ) )
3199 {
3200 return 0;
3201 }
3202
3203 int count = 0;
3204 const_iterator i1 = begin( );
3205 const_iterator i2 = i1;
3206 ++i2;
3207
3208 Concrete::Coords2D d = *((*i1)->mid_) - p;
3209 Computation::WindingSigns lastSigns = ( ( d.x_ > 0 ) ? Computation::POS_X : Computation::NO_SIGN ) | ( ( d.y_ > 0 ) ? Computation::POS_Y : Computation::NO_SIGN );
3210 /* Select two quadrants, between which crossings shall be counted. Avoid the quadrant where the path starts and ends. */
3211 const Computation::WindingSigns QUAD_1 = lastSigns ^ ( Computation::POS_X | Computation::POS_Y ); /* Invert both signs. */
3212 const Computation::WindingSigns QUAD_2 = ( ( d.y_ > 0 ) ? Computation::POS_X : Computation::NO_SIGN ) | ( ( d.x_ <= 0 ) ? Computation::POS_Y : Computation::NO_SIGN ); /* Next quadrant in clock-wise direction. */
3213
3214 std::stack< Concrete::Coords2D > pointStack; /* End point at bottom of stack. */
3215
3216 const Concrete::LengthSquared SEG_TOL = Computation::the_arcdelta * Computation::the_arcdelta;
3217
3218 Concrete::Time res = Concrete::HUGE_TIME;
3219 for( ; i1 != end( ); ++i1, ++i2 )
3220 {
3221 if( i2 == end( ) )
3222 {
3223 i2 = begin( );
3224 }
3225 if( (*i1)->front_ == (*i1)->mid_ &&
3226 (*i2)->rear_ == (*i2)->mid_ )
3227 {
3228 Computation::windingNumberHelper( p, QUAD_1, QUAD_2, *((*i1)->mid_), *((*i2)->mid_), & count, & lastSigns );
3229 continue;
3230 }
3231
3232 pointStack.push( *((*i2)->mid_) );
3233 pointStack.push( *((*i2)->rear_) );
3234 pointStack.push( *((*i1)->front_) );
3235 Concrete::Coords2D p0 = *((*i1)->mid_);
3236 while( ! pointStack.empty( ) )
3237 {
3238 Concrete::Coords2D p1 = pointStack.top( );
3239 pointStack.pop( );
3240 Concrete::Coords2D p2 = pointStack.top( );
3241 pointStack.pop( );
3242 Concrete::Coords2D p3 = pointStack.top( );
3243 /* Leave the last point at the stack for now... */
3244
3245 /* Only subdivide unless the segment is not tiny (in relation to the_arcdelta). */
3246 if( ( ( p1 - p0 ).normSquared( ) >= SEG_TOL || ( p2 - p1 ).normSquared( ) >= SEG_TOL || ( p3 - p2 ).normSquared( ) >= SEG_TOL )
3247 && Computation::convexHullMember( p, p0, p1, p2, p3 ) )
3248 {
3249 Bezier::ControlPoints< Concrete::Coords2D > controls( p0, p1, p2, p3 );
3250 Bezier::PolyCoeffs< Concrete::Coords2D > coeffs( controls );
3251 Bezier::ControlPoints< Concrete::Coords2D > sub1( coeffs.subSection( 0, 0.5 ) );
3252 Bezier::ControlPoints< Concrete::Coords2D > sub2( coeffs.subSection( 0.5, 1 ) );
3253 pointStack.push( sub2.p2_ );
3254 pointStack.push( sub2.p1_ );
3255 pointStack.push( sub2.p0_ );
3256 pointStack.push( sub1.p2_ );
3257 pointStack.push( sub1.p1_ );
3258 }
3259 else
3260 {
3261 pointStack.pop( );
3262 Computation::windingNumberHelper( p, QUAD_1, QUAD_2, p0, p3, & count, & lastSigns );
3263 p0 = p3;
3264 }
3265 }
3266
3267 }
3268 return count;
3269 }
3270
3271
3272 namespace Shapes
3273 {
3274 namespace Computation
3275 {
3276
3277 class SegmentPolynomialPair2D
3278 {
3279 Bezier::PolyCoeffs< Concrete::Coords2D > polyCoeffs_a;
3280 bool straightLineSeg_a_;
3281
3282 Physical< 1, 0 > kxaD0_0;
3283 Physical< 1, -1 > kxaD0_1;
3284 Physical< 1, -2 > kxaD0_2;
3285 Physical< 1, -3 > kxaD0_3;
3286 Physical< 1, -1 > kxaD1_0;
3287 Physical< 1, -2 > kxaD1_1;
3288 Physical< 1, -3 > kxaD1_2;
3289 Physical< 1, -2 > kxaD2_0;
3290 Physical< 1, -3 > kxaD2_1;
3291
3292 Physical< 1, 0 > kyaD0_0;
3293 Physical< 1, -1 > kyaD0_1;
3294 Physical< 1, -2 > kyaD0_2;
3295 Physical< 1, -3 > kyaD0_3;
3296 Physical< 1, -1 > kyaD1_0;
3297 Physical< 1, -2 > kyaD1_1;
3298 Physical< 1, -3 > kyaD1_2;
3299 Physical< 1, -2 > kyaD2_0;
3300 Physical< 1, -3 > kyaD2_1;
3301
3302 Bezier::PolyCoeffs< Concrete::Coords2D > polyCoeffs_b;
3303 bool straightLineSeg_b_;
3304
3305 Physical< 1, 0 > kxbD0_0;
3306 Physical< 1, -1 > kxbD0_1;
3307 Physical< 1, -2 > kxbD0_2;
3308 Physical< 1, -3 > kxbD0_3;
3309 Physical< 1, -1 > kxbD1_0;
3310 Physical< 1, -2 > kxbD1_1;
3311 Physical< 1, -3 > kxbD1_2;
3312 Physical< 1, -2 > kxbD2_0;
3313 Physical< 1, -3 > kxbD2_1;
3314
3315 Physical< 1, 0 > kybD0_0;
3316 Physical< 1, -1 > kybD0_1;
3317 Physical< 1, -2 > kybD0_2;
3318 Physical< 1, -3 > kybD0_3;
3319 Physical< 1, -1 > kybD1_0;
3320 Physical< 1, -2 > kybD1_1;
3321 Physical< 1, -3 > kybD1_2;
3322 Physical< 1, -2 > kybD2_0;
3323 Physical< 1, -3 > kybD2_1;
3324
3325 Concrete::Time steps_a_;
3326 Concrete::Time steps_b_;
3327
3328 Concrete::Speed maxSpeed_a_;
3329 Concrete::Speed maxSpeed_b_;
3330
3331 public:
3332 SegmentPolynomialPair2D( Concrete::Time steps_a, const Bezier::ControlPoints< Concrete::Coords2D > & seg_a, Concrete::Time steps_b, const Bezier::ControlPoints< Concrete::Coords2D > & seg_b );
3333 SegmentPolynomialPair2D( Concrete::Time steps_a, const Concrete::Coords2D & seg_a_p0, const Concrete::Coords2D & seg_a_p3, Concrete::Time steps_b, const Bezier::ControlPoints< Concrete::Coords2D > & seg_b );
3334 SegmentPolynomialPair2D( Concrete::Time steps_a, const Bezier::ControlPoints< Concrete::Coords2D > & seg_a, Concrete::Time steps_b, const Concrete::Coords2D & seg_b_p0, const Concrete::Coords2D & seg_b_p3 );
3335 bool splitTimes( Concrete::Time * dst_a, Concrete::Time * dst_b, const Concrete::Time t_alow, const Concrete::Time t_ahigh, const Concrete::Time t_atol, const Concrete::Time t_blow, const Concrete::Time t_bhigh, const Concrete::Time t_btol, Concrete::Length distance_tol ) const; /* Return true if intersection is found. */
3336 Concrete::Length distanceAt( Concrete::Time ta, Concrete::Time tb ) const;
3337 Concrete::LengthSquared squaredDistanceAt( Concrete::Time ta, Concrete::Time tb ) const;
3338 bool straightLineSeg_a( ) const { return straightLineSeg_a_; }
3339 bool straightLineSeg_b( ) const { return straightLineSeg_b_; }
3340 Concrete::Speed maxSpeed_a( ) const { return maxSpeed_a_; }
3341 Concrete::Speed maxSpeed_b( ) const { return maxSpeed_b_; }
3342 Concrete::Time steps_a( ) const { return steps_a_; }
3343 Concrete::Time steps_b( ) const { return steps_b_; }
3344 Bezier::ControlPoints< Concrete::Coords2D > getControls_a( const Concrete::Time t_low, const Concrete::Time t_high ) const;
3345 Bezier::ControlPoints< Concrete::Coords2D > getControls_b( const Concrete::Time t_low, const Concrete::Time t_high ) const;
3346
3347 private:
3348 void kInit( ); /* Used by constructors to initialize coefficients. */
3349 };
3350
3351 class SegmentSectionPair2D
3352 {
3353 Bezier::ControlPoints< Concrete::Coords2D > controls_a;
3354 Bezier::ControlPoints< Concrete::Coords2D > controls_b;
3355 const SegmentPolynomialPair2D * baseSegs;
3356 Concrete::Time t_a0;
3357 Concrete::Time t_a1;
3358 Concrete::Time t_b0;
3359 Concrete::Time t_b1;
3360 mutable Concrete::LengthSquared hull_dist_;
3361
3362 public:
3363 SegmentSectionPair2D( const SegmentPolynomialPair2D * _baseSegs, Concrete::Time _t_a0, Concrete::Time _t_a1, Concrete::Time _t_b0, Concrete::Time _t_b1 );
3364 SegmentSectionPair2D * cutAfterAfter( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const;
3365 SegmentSectionPair2D * cutAfterBefore( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const;
3366 SegmentSectionPair2D * cutBeforeAfter( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const;
3367 SegmentSectionPair2D * cutBeforeBefore( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const;
3368
3369 bool splitTimes( Concrete::Time * dst_a, Concrete::Time * dst_b, const Concrete::Time t_tol_a, const Concrete::Time t_tol_b, Concrete::Length distance_tol ) const; /* True if intersection was found. */
3370 Concrete::Time steps_a( ) const { return baseSegs->steps_a( ); };
3371 Concrete::Time steps_b( ) const { return baseSegs->steps_b( ); };
3372 Concrete::Time globalTime_a( Concrete::Time t ) const;
3373 Concrete::Length distanceAt( Concrete::Time ta, Concrete::Time tb ) const;
3374 Concrete::LengthSquared squaredDistanceAt( Concrete::Time ta, Concrete::Time tb ) const;
3375 Concrete::Speed maxSpeed_a( ) const;
3376 Concrete::Speed maxSpeed_b( ) const;
3377 bool convexHullOverlap( ) const;
3378 bool convexHullSeparatedBy( Concrete::LengthSquared sep ) const; /* If the result is false, the actual distance between the hulls will be stored in hull_dist_. */
3379 Concrete::LengthSquared precomputedHullDistanceSquared( ) const { return hull_dist_; }
3380 void update_t_a1( Concrete::Time t );
3381 bool isEmpty( ) const;
3382
3383 void writeTimes( std::ostream & os ) const;
3384 private:
3385 static bool convexHullOneWayOverlap( const std::vector< const Concrete::Coords2D * > & poly1, const std::vector< const Concrete::Coords2D * > & poly2 );
3386 };
3387
3388 }
3389 }
3390
3391 namespace Shapes
3392 {
3393 namespace Helpers
3394 {
3395 Kernel::StructureFactory intersection_info_resultFactory( "other" );
3396 Kernel::StructureFactory approximator_info_resultFactory( "dist", "other" );
3397 }
3398 namespace Computation
3399 {
3400 struct CurvedSegType : public Bezier::ControlPoints< Concrete::Coords2D >
3401 {
3402 Concrete::Time steps;
3403 CurvedSegType( Concrete::Time _steps, const Bezier::ControlPoints< Concrete::Coords2D > & ctrl )
3404 : Bezier::ControlPoints< Concrete::Coords2D >( ctrl ), steps( _steps )
3405 { }
3406 };
3407 struct StraightSegType
3408 {
3409 Concrete::Time steps;
3410 Concrete::Coords2D * first;
3411 Concrete::Coords2D * second;
3412
3413 StraightSegType( Concrete::Time _steps, Concrete::Coords2D * _first, Concrete::Coords2D * _second )
3414 : steps( _steps ), first( _first ), second( _second )
3415 { }
3416 };
3417 }
3418 }
3419
3420 Concrete::SplineTime
3421 Lang::ElementaryPath2D::intersection( const RefCountPtr< const Lang::ElementaryPath2D > & p2, RefCountPtr< const Lang::Structure > * dstInfo ) const
3422 {
3423 using namespace Computation;
3424
3425 const Concrete::Length DISTANCE_TOL = 0.001 * Computation::the_arcdelta;
3426 const double CUT_LOSS = 0.01;
3427
3428 if( empty( ) )
3429 {
3430 throw Exceptions::OutOfRange( "The empty path does not intersect with anying." );
3431 }
3432
3433 std::vector< CurvedSegType > curvedSegments;
3434 curvedSegments.reserve( p2->size( ) );
3435 std::vector< StraightSegType > straightSegments;
3436 straightSegments.reserve( p2->size( ) );
3437 {
3438 Concrete::Time steps2 = Concrete::ZERO_TIME;
3439 const_iterator i1 = p2->begin( );
3440 const_iterator i2 = i1;
3441 ++i2;
3442 for( ; i1 != p2->end( ); ++i1, ++i2, steps2 += Concrete::UNIT_TIME )
3443 {
3444 if( i2 == p2->end( ) )
3445 {
3446 if( p2->isClosed( ) )
3447 {
3448 i2 = p2->begin( );
3449 }
3450 else
3451 {
3452 break;
3453 }
3454 }
3455 if( (*i1)->front_ == (*i1)->mid_ &&
3456 (*i2)->rear_ == (*i2)->mid_ )
3457 {
3458 straightSegments.push_back( StraightSegType( steps2, (*i1)->mid_, (*i2)->mid_ ) );
3459 }
3460 else
3461 {
3462 curvedSegments.push_back( CurvedSegType( steps2, Bezier::ControlPoints< Concrete::Coords2D >( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_) ) ) );
3463 }
3464 }
3465 }
3466
3467 Concrete::Time steps = Concrete::ZERO_TIME;
3468 const_iterator i1 = begin( );
3469 const_iterator i2 = i1;
3470 ++i2;
3471 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
3472 {
3473 if( i2 == end( ) )
3474 {
3475 if( closed_ )
3476 {
3477 i2 = begin( );
3478 }
3479 else
3480 {
3481 break;
3482 }
3483 }
3484
3485 /* if the segment is straight a dedicated intersection-finder is invoked
3486 */
3487 if( (*i1)->front_ == (*i1)->mid_ &&
3488 (*i2)->rear_ == (*i2)->mid_ )
3489 {
3490 Concrete::Time t;
3491 Concrete::Time res2;
3492 if( p2->lineSegmentIntersection( & res2, & t, *(*i1)->mid_, *(*i2)->mid_ ) )
3493 {
3494 if( dstInfo != 0 )
3495 {
3496 Helpers::intersection_info_resultFactory.set( "other", Helpers::newValHandle( new Lang::PathSlider2D( p2, res2 ) ) );
3497 *dstInfo = Helpers::intersection_info_resultFactory.build( );
3498 }
3499 return steps + t;
3500 }
3501 continue;
3502 }
3503
3504 Bezier::ControlPoints< Concrete::Coords2D > seg_a( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_) );
3505 Bezier::PolyCoeffs< Concrete::Coords2D > seg_a_coeffs( seg_a );
3506
3507 /* A segment may intersect with the other path at several places, but only the first shall be returned.
3508 * This makes it useful to track the earliest intersection time we've seen so far. Then segments that are
3509 * after this point doesn't have to be searched at all, and segments before this point need only be
3510 * searched up to this point.
3511 */
3512 Concrete::Time t = Concrete::HUGE_TIME;
3513 Concrete::Time res2 = Concrete::HUGE_TIME;
3514
3515 /* Otherwise, I first scan the straight parts of the other path in order to find the easy intersections
3516 * as soon as possible.
3517 */
3518 {
3519 typedef typeof straightSegments ListType;
3520 for( ListType::const_iterator i = straightSegments.begin( ); i != straightSegments.end( ); ++i )
3521 {
3522 Concrete::Coords2D rInv = ( *(i->second) - *(i->first) ) * ( 1. / Concrete::innerScalar( *(i->second) - *(i->first), *(i->second) - *(i->first) ) );
3523 Concrete::Coords2D n( i->first->y_ - i->second->y_, i->second->x_ - i->first->x_ );
3524 double offset = Concrete::innerScalar( n, *i->first );
3525 double tmp[4];
3526 seg_a_coeffs.hyperplaneIntersections( tmp, n, offset );
3527 for( double * tmpi = tmp; *tmpi != HUGE_VAL; ++tmpi )
3528 {
3529 double t2( Concrete::innerScalar( rInv, seg_a_coeffs.point( *tmpi ) - *(i->first) ) );
3530 if( 0 <= t2 && t2 <= 1 )
3531 {
3532 if( Concrete::Time( *tmpi ) < t )
3533 {
3534 t = Concrete::Time( *tmpi );
3535 res2 = i->steps + Shapes::straightLineArcTime( t2 );
3536 }
3537 }
3538 }
3539 }
3540 }
3541
3542 /* The remaining segments are pushed onto a queue
3543 */
3544 std::list< SegmentSectionPair2D * > work;
3545
3546 /* Keep track of the memory we use:
3547 */
3548 PtrOwner_back_Access< std::list< const SegmentPolynomialPair2D * > > baseSegsMemory;
3549
3550 {
3551 typedef typeof curvedSegments ListType;
3552 for( ListType::const_iterator seg_b = curvedSegments.begin( ); seg_b != curvedSegments.end( ); ++seg_b )
3553 {
3554 SegmentSectionPair2D * workItem;
3555 SegmentPolynomialPair2D * baseSegs = new SegmentPolynomialPair2D( steps, seg_a, seg_b->steps, *seg_b );
3556 baseSegsMemory.push_back( baseSegs );
3557 if( t < Concrete::HUGE_TIME )
3558 {
3559 workItem = new SegmentSectionPair2D( baseSegs, Concrete::ZERO_TIME, t, Concrete::ZERO_TIME, Concrete::UNIT_TIME );
3560 }
3561 else
3562 {
3563 workItem = new SegmentSectionPair2D( baseSegs, Concrete::ZERO_TIME, Concrete::UNIT_TIME, Concrete::ZERO_TIME, Concrete::UNIT_TIME );
3564 }
3565
3566 pushOverlappingDeleteOther( work, workItem );
3567 }
3568 }
3569
3570 /* Then we do the work...
3571 */
3572 while( ! work.empty( ) )
3573 {
3574 SegmentSectionPair2D * workItem = work.front( );
3575 work.pop_front( );
3576 workItem->update_t_a1( t );
3577 if( workItem->isEmpty( ) )
3578 {
3579 delete workItem;
3580 continue;
3581 }
3582
3583 const Concrete::Time t_tol_1 = Computation::the_arcdelta / workItem->maxSpeed_a( );
3584 const Concrete::Time t_tol_2 = Computation::the_arcdelta / workItem->maxSpeed_b( );
3585
3586 Concrete::Time t1;
3587 Concrete::Time t2;
3588 if( workItem->splitTimes( & t1, & t2, 0.001 * t_tol_1, 0.001 * t_tol_2, DISTANCE_TOL ) // too low precision gives erraneous results!
3589 || workItem->distanceAt( t1, t2 ) < DISTANCE_TOL )
3590 {
3591 if( t1 < t )
3592 {
3593 t = t1;
3594 res2 = workItem->steps_b( ) + t2;
3595 }
3596 // We have an intersection; only two children to consider
3597 /* When splitTimes returned true, DO WE HAVE TO convergence by ourselves? */
3598 pushOverlappingDeleteOther( work, workItem->cutAfterBefore( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ) );
3599 pushOverlappingDeleteOther( work, workItem->cutAfterAfter( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ) );
3600 }
3601 else
3602 {
3603 // No intersection; four children to consider
3604 pushOverlappingDeleteOther( work, workItem->cutBeforeBefore( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ) );
3605 pushOverlappingDeleteOther( work, workItem->cutBeforeAfter( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ) );
3606 pushOverlappingDeleteOther( work, workItem->cutAfterBefore( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ) );
3607 pushOverlappingDeleteOther( work, workItem->cutAfterAfter( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ) );
3608 }
3609 delete workItem;
3610 }
3611
3612 if( t < Concrete::HUGE_TIME ) // HUGE_VAL means that no intersection was found along the current segment.
3613 {
3614 if( dstInfo != 0 )
3615 {
3616 Helpers::intersection_info_resultFactory.set( "other", Helpers::newValHandle( new Lang::PathSlider2D( p2, res2 ) ) );
3617 *dstInfo = Helpers::intersection_info_resultFactory.build( );
3618 }
3619 return steps + t;
3620 }
3621 }
3622
3623 throw Exceptions::OutOfRange( "Failed to find intersection. Probably because there was no intersection." );
3624 }
3625
3626 Concrete::SplineTime
3627 Lang::ElementaryPath2D::continuousApproximator( const RefCountPtr< const Lang::ElementaryPath2D > & p2, RefCountPtr< const Lang::Structure > * dstInfo ) const
3628 {
3629 using namespace Computation;
3630
3631 /* Note that this algorithm will try to detect intsersections.
3632 * When an intersection is found, it should be possible to switch to the algorithm that is specialized for intersection computations.
3633 * However, as I am not sure that the tolerances are set correctly, this is one reason why calling that algorithm may be dangerous.
3634 * Additionally, calling the intersection algorithm does not generalize to the 3D case, and it could be difficult to take advantage of all
3635 * the work that has already been done in this algorithm.
3636 * What we still should to is to switch to an overlap-detection algorithm instead of the distance algorithm when an intersection has been
3637 * encountered, but this optimization is not implemented at the moment.
3638 */
3639
3640 const Concrete::Length DISTANCE_TOL = 0.001 * Computation::the_arcdelta;
3641 const double CUT_LOSS = 0.01;
3642
3643 if( empty( ) )
3644 {
3645 throw Exceptions::OutOfRange( "The empty path does not approximate anying." );
3646 }
3647 if( p2->empty( ) )
3648 {
3649 throw Exceptions::OutOfRange( "The empty path cannot be the approximation target." );
3650 }
3651
3652 Concrete::Time resSteps = Concrete::HUGE_TIME;
3653 Concrete::Time resFrac = 0;
3654 Concrete::Time res2 = 0;
3655
3656 /* In this algorithm, we always work with the distance between the paths squared.
3657 */
3658 Concrete::LengthSquared bestDist( HUGE_VAL );
3659 const Concrete::LengthSquared INTERSECT_DIST( 1e-10 );
3660 const Concrete::LengthSquared ZERO_DIST( 0 );
3661
3662 /* First, the second path is divided up into its straight and (potentially) curved segments.
3663 * During the scan, we also compute a first crude estimate of the distance between the paths by
3664 * computing the distance (squared) between all path points.
3665 */
3666 std::vector< CurvedSegType > curvedSegments;
3667 curvedSegments.reserve( p2->size( ) );
3668 std::vector< StraightSegType > straightSegments;
3669 straightSegments.reserve( p2->size( ) );
3670 {
3671 Concrete::Time steps2 = Concrete::ZERO_TIME;
3672 const_iterator i1 = p2->begin( );
3673 const_iterator i2 = i1;
3674 ++i2;
3675 for( ; i1 != p2->end( ); ++i1, ++i2, steps2 += Concrete::UNIT_TIME )
3676 {
3677 Concrete::Time steps = Concrete::ZERO_TIME;
3678 for( const_iterator j = begin( ); j != end( ); ++j, steps += Concrete::UNIT_TIME )
3679 {
3680 Concrete::LengthSquared d = ( *((*i1)->mid_) - *((*j)->mid_) ).normSquared( );
3681 if( d <= INTERSECT_DIST )
3682 {
3683 /* We interpret this as an intersection. Save only if old optimum was not an intersection,
3684 * or this point is earlier than old optimum.
3685 *
3686 * At this point, the fraction is still 0.
3687 */
3688 if( bestDist > ZERO_DIST ||
3689 steps < resSteps )
3690 {
3691 resSteps = steps;
3692 res2 = steps2;
3693 bestDist = ZERO_DIST;
3694 }
3695 }
3696 else if( d < bestDist )
3697 {
3698 resSteps = steps; /* At this point, the fraction is still 0. */
3699 res2 = steps2;
3700 bestDist = d;
3701 }
3702 }
3703 if( i2 == p2->end( ) )
3704 {
3705 if( p2->isClosed( ) )
3706 {
3707 i2 = p2->begin( );
3708 }
3709 else
3710 {
3711 break;
3712 }
3713 }
3714 if( (*i1)->front_ == (*i1)->mid_ &&
3715 (*i2)->rear_ == (*i2)->mid_ )
3716 {
3717 straightSegments.push_back( StraightSegType( steps2, (*i1)->mid_, (*i2)->mid_ ) );
3718 }
3719 else
3720 {
3721 curvedSegments.push_back( CurvedSegType( steps2, Bezier::ControlPoints< Concrete::Coords2D >( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_) ) ) );
3722 }
3723 }
3724 }
3725
3726 /* Next, the crude estimate is refined by comparing all straight line segments.
3727 */
3728 {
3729 Concrete::Time steps = Concrete::ZERO_TIME;
3730 const_iterator i1 = begin( );
3731 const_iterator i2 = i1;
3732 ++i2;
3733 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
3734 {
3735 if( i2 == end( ) )
3736 {
3737 if( closed_ )
3738 {
3739 i2 = begin( );
3740 }
3741 else
3742 {
3743 break;
3744 }
3745 }
3746
3747 if( (*i1)->front_ == (*i1)->mid_ &&
3748 (*i2)->rear_ == (*i2)->mid_ )
3749 {
3750 /* Straight line segment. This is the only case we care about at this point.
3751 */
3752 Concrete::Coords2D l0 = *((*i1)->mid_);
3753 Concrete::Coords2D l1 = *((*i2)->mid_);
3754 Concrete::Coords2D r = l1 - l0;
3755 Concrete::Coords2D rInv = r * ( 1. / Concrete::innerScalar( r, r ) );
3756 /* Distance between straight line segments is solved as a polytope distance problem. */
3757 const size_t N = 2;
3758 const size_t DIM = 2;
3759 double ga[ N * DIM ];
3760 double gb[ N * DIM ];
3761 double ya[ DIM ];
3762 double yb[ DIM ];
3763 {
3764 double * dstg = ga;
3765 *dstg = Concrete::Length::offtype( l0.x_ );
3766 ++dstg;
3767 *dstg = Concrete::Length::offtype( l0.y_ );
3768 ++dstg;
3769 *dstg = Concrete::Length::offtype( l1.x_ );
3770 ++dstg;
3771 *dstg = Concrete::Length::offtype( l1.y_ );
3772 }
3773 typedef typeof straightSegments ListType;
3774 for( ListType::const_iterator i = straightSegments.begin( ); i != straightSegments.end( ); ++i )
3775 {
3776 {
3777 double * dstg = gb;
3778 *dstg = Concrete::Length::offtype( i->first->x_ );
3779 ++dstg;
3780 *dstg = Concrete::Length::offtype( i->first->y_ );
3781 ++dstg;
3782 *dstg = Concrete::Length::offtype( i->second->x_ );
3783 ++dstg;
3784 *dstg = Concrete::Length::offtype( i->second->y_ );
3785 }
3786 double double_distSquaredLow;
3787 double double_distSquaredHigh;
3788 QPSolverStatus status;
3789 Computation::polytope_distance_generator_form( DIM,
3790 N, & ga[0],
3791 N, & gb[0],
3792 & double_distSquaredLow, & double_distSquaredHigh,
3793 Concrete::LengthSquared::offtype( bestDist ), -1,
3794 0, 0,
3795 0.5 * Concrete::Length::offtype( Computation::theDistanceTol ), /* Lagrange multiplier tolerance. */
3796 0, 0,
3797 & ya[0], & yb[0],
3798 & status );
3799 if( status.code != Computation::QP_OK )
3800 {
3801 const char * binaryFilename = "qp-fail-data.binary";
3802 Computation::polytope_distance_generator_form_write_binary_data( binaryFilename,
3803 DIM,
3804 N, & ga[0],
3805 N, & gb[0] );
3806 std::ostringstream msg;
3807 msg << "QP solver status: " << status.code_str( ) << ", with reason: " << status.sub_str( )
3808 << ", binary data was written to \"" << binaryFilename << "\"." ;
3809 throw Exceptions::InternalError( strrefdup( msg ) );
3810 }
3811 Concrete::LengthSquared distSquaredLow( double_distSquaredLow );
3812 Concrete::LengthSquared distSquaredHigh( double_distSquaredHigh );
3813 if( distSquaredHigh <= INTERSECT_DIST )
3814 {
3815 /* We interpret this as an intersection. Save only if old optimum was not an intersection,
3816 * or this point is earlier than old optimum.
3817 */
3818 Concrete::Time t = Shapes::straightLineArcTime( Concrete::innerScalar( rInv, Concrete::Coords2D( ya[0], ya[1] ) - l0 ) );
3819 if( bestDist > ZERO_DIST ||
3820 steps + t < resSteps + resFrac )
3821 {
3822 resSteps = steps;
3823 resFrac = t;
3824 Concrete::Coords2D l0b = *(i->first);
3825 Concrete::Coords2D l1b = *(i->second);
3826 Concrete::Coords2D rb = l1b - l0b;
3827 Concrete::Coords2D rInv_b = rb * ( 1. / Concrete::innerScalar( rb, rb ) );
3828 res2 = i->steps + Shapes::straightLineArcTime( Concrete::innerScalar( rInv_b, Concrete::Coords2D( yb[0], yb[1] ) - l0b ) );
3829 bestDist = ZERO_DIST;
3830 }
3831 }
3832 else if( distSquaredLow < bestDist )
3833 {
3834 /* The optimum has been improved. Save new optimum. */
3835 resSteps = steps;
3836 resFrac = Shapes::straightLineArcTime( Concrete::innerScalar( rInv, Concrete::Coords2D( ya[0], ya[1] ) - l0 ) );
3837 Concrete::Coords2D l0b = *(i->first);
3838 Concrete::Coords2D l1b = *(i->second);
3839 Concrete::Coords2D rb = l1b - l0b;
3840 Concrete::Coords2D rInv_b = rb * ( 1. / Concrete::innerScalar( rb, rb ) );
3841 res2 = i->steps + Shapes::straightLineArcTime( Concrete::innerScalar( rInv_b, Concrete::Coords2D( yb[0], yb[1] ) - l0b ) );
3842 bestDist = distSquaredLow;
3843 }
3844 }
3845 }
3846 }
3847 }
3848
3849 /* Next, we scan *this path, and for each segment we pair it up with all segments on p2.
3850 * In the case that two straight segments are paired, their distance has already been computed, but any other
3851 * combination of segments is pushed to a queue of work to be done.
3852 */
3853
3854 /* The remaining segments are pushed onto a queue
3855 */
3856 std::list< SegmentSectionPair2D * > work;
3857
3858 /* Keep track of the memory we use:
3859 */
3860 PtrOwner_back_Access< std::list< const SegmentPolynomialPair2D * > > baseSegsMemory;
3861
3862 Concrete::Time steps = Concrete::ZERO_TIME;
3863 const_iterator i1 = begin( );
3864 const_iterator i2 = i1;
3865 ++i2;
3866 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
3867 {
3868 if( i2 == end( ) )
3869 {
3870 if( closed_ )
3871 {
3872 i2 = begin( );
3873 }
3874 else
3875 {
3876 break;
3877 }
3878 }
3879
3880 if( (*i1)->front_ == (*i1)->mid_ &&
3881 (*i2)->rear_ == (*i2)->mid_ )
3882 {
3883 /* Straight line segment. The distance to straight line segments on the other path has already been computed.
3884 */
3885 typedef typeof curvedSegments ListType;
3886 for( ListType::const_iterator seg_b = curvedSegments.begin( ); seg_b != curvedSegments.end( ); ++seg_b )
3887 {
3888 SegmentPolynomialPair2D * baseSegs = new SegmentPolynomialPair2D( steps, *((*i1)->mid_), *((*i2)->mid_), seg_b->steps, *seg_b );
3889 baseSegsMemory.push_back( baseSegs );
3890 pushCloseDeleteOther( & work, new SegmentSectionPair2D( baseSegs, Concrete::ZERO_TIME, Concrete::UNIT_TIME, Concrete::ZERO_TIME, Concrete::UNIT_TIME ), bestDist );
3891 }
3892 }
3893 else
3894 {
3895 /* General segment. (If it happens to be straight, we don't care.)
3896 */
3897 Bezier::ControlPoints< Concrete::Coords2D > seg_a( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_) );
3898
3899 {
3900 typedef typeof straightSegments ListType;
3901 for( ListType::const_iterator i = straightSegments.begin( ); i != straightSegments.end( ); ++i )
3902 {
3903 SegmentPolynomialPair2D * baseSegs = new SegmentPolynomialPair2D( steps, seg_a, i->steps, *(i->first), *(i->second) );
3904 baseSegsMemory.push_back( baseSegs );
3905 pushCloseDeleteOther( & work, new SegmentSectionPair2D( baseSegs, Concrete::ZERO_TIME, Concrete::UNIT_TIME, Concrete::ZERO_TIME, Concrete::UNIT_TIME ), bestDist );
3906 }
3907 }
3908
3909 {
3910 typedef typeof curvedSegments ListType;
3911 for( ListType::const_iterator seg_b = curvedSegments.begin( ); seg_b != curvedSegments.end( ); ++seg_b )
3912 {
3913 SegmentPolynomialPair2D * baseSegs = new SegmentPolynomialPair2D( steps, seg_a, seg_b->steps, *seg_b );
3914 baseSegsMemory.push_back( baseSegs );
3915 pushCloseDeleteOther( & work, new SegmentSectionPair2D( baseSegs, Concrete::ZERO_TIME, Concrete::UNIT_TIME, Concrete::ZERO_TIME, Concrete::UNIT_TIME ), bestDist );
3916 }
3917 }
3918 }
3919 }
3920
3921 /* Now we reach the main part of the algorithm...
3922 */
3923 while( ! work.empty( ) )
3924 {
3925 SegmentSectionPair2D * workItem = work.front( );
3926 work.pop_front( );
3927 if( workItem->precomputedHullDistanceSquared( ) > ( ( bestDist == 0 ) ? INTERSECT_DIST : bestDist ) )
3928 {
3929 /* A better solution has been found since this piece of work was created.
3930 */
3931 delete workItem;
3932 continue;
3933 }
3934 if( bestDist <= INTERSECT_DIST )
3935 {
3936 /* If we have detected an intersection, we can skip any piece of work that cannot yield an earlier optimum. */
3937 if( workItem->steps_a( ) > resSteps )
3938 {
3939 delete workItem;
3940 continue;
3941 }
3942 if( workItem->steps_a( ) == resSteps )
3943 {
3944 workItem->update_t_a1( resFrac );
3945 }
3946 }
3947 if( workItem->isEmpty( ) )
3948 {
3949 delete workItem;
3950 continue;
3951 }
3952
3953 const Concrete::Time t_tol_1 = Computation::the_arcdelta / workItem->maxSpeed_a( );
3954 const Concrete::Time t_tol_2 = Computation::the_arcdelta / workItem->maxSpeed_b( );
3955
3956 Concrete::Time t1;
3957 Concrete::Time t2;
3958 bool intersect = workItem->splitTimes( & t1, & t2, 0.001 * t_tol_1, 0.001 * t_tol_2, DISTANCE_TOL ); // too low precision gives erraneous results!
3959 Concrete::LengthSquared d = workItem->squaredDistanceAt( t1, t2 );
3960 intersect = intersect || d < INTERSECT_DIST;
3961 if( intersect )
3962 {
3963 if( bestDist > ZERO_DIST
3964 || workItem->globalTime_a( t1 ) < resSteps + resFrac )
3965 {
3966 bestDist = ZERO_DIST;
3967 resSteps = workItem->steps_a( );
3968 resFrac = t1;
3969 res2 = workItem->steps_b( ) + t2;
3970 }
3971 }
3972 else if( d < bestDist )
3973 {
3974 bestDist = d;
3975 resSteps = workItem->steps_a( );
3976 resFrac = t1;
3977 res2 = workItem->steps_b( ) + t2;
3978 }
3979 if( intersect )
3980 {
3981 // We have an intersection; only two children to consider
3982 pushCloseDeleteOther( & work, workItem->cutAfterBefore( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ), bestDist );
3983 pushCloseDeleteOther( & work, workItem->cutAfterAfter( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ), bestDist );
3984 }
3985 else
3986 {
3987 // No intersection; four children to consider
3988 pushCloseDeleteOther( & work, workItem->cutBeforeBefore( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ), bestDist );
3989 pushCloseDeleteOther( & work, workItem->cutBeforeAfter( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ), bestDist );
3990 pushCloseDeleteOther( & work, workItem->cutAfterBefore( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ), bestDist );
3991 pushCloseDeleteOther( & work, workItem->cutAfterAfter( t1, t2, CUT_LOSS * t_tol_1, CUT_LOSS * t_tol_2 ), bestDist);
3992 }
3993 delete workItem;
3994 }
3995
3996 if( resSteps == Concrete::HUGE_TIME )
3997 {
3998 throw Exceptions::InternalError( "Path to path approximation: No result was produced." );
3999 }
4000
4001 if( dstInfo != 0 )
4002 {
4003 Helpers::approximator_info_resultFactory.set( "dist", Helpers::newValHandle( new Lang::Length( sqrtPhysical( bestDist ) ) ) );
4004 Helpers::approximator_info_resultFactory.set( "other", Helpers::newValHandle( new Lang::PathSlider2D( p2, res2 ) ) );
4005 *dstInfo = Helpers::approximator_info_resultFactory.build( );
4006 }
4007 return Concrete::SplineTime( resSteps + resFrac );
4008 }
4009
4010 void
4011 Lang::ElementaryPath2D::pushOverlappingDeleteOther( std::list< Computation::SegmentSectionPair2D * > & work, Computation::SegmentSectionPair2D * item )
4012 {
4013 if( item->convexHullOverlap( ) )
4014 {
4015 work.push_back( item );
4016 }
4017 else
4018 {
4019 delete item;
4020 }
4021 }
4022
4023 void
4024 Lang::ElementaryPath2D::pushCloseDeleteOther( std::list< Computation::SegmentSectionPair2D * > * work, Computation::SegmentSectionPair2D * item, Concrete::LengthSquared closeDist )
4025 {
4026 if( item->isEmpty( )
4027 || item->convexHullSeparatedBy( closeDist ) )
4028 {
4029 delete item;
4030 }
4031 else
4032 {
4033 work->push_back( item );
4034 }
4035 }
4036
4037 void
4038 Computation::SegmentPolynomialPair2D::kInit( )
4039 {
4040 kxaD0_0 = polyCoeffs_a.z0_.x_;
4041 kxaD0_1 = polyCoeffs_a.z1_.x_.offtype< 0, 1 >( );
4042 kxaD0_2 = polyCoeffs_a.z2_.x_.offtype< 0, 2 >( );
4043 kxaD0_3 = polyCoeffs_a.z3_.x_.offtype< 0, 3 >( );
4044
4045 kxaD1_0 = kxaD0_1;
4046 kxaD1_1 = 2 * kxaD0_2;
4047 kxaD1_2 = 3 * kxaD0_3;
4048
4049 kxaD2_0 = kxaD1_1;
4050 kxaD2_1 = 2 * kxaD1_2;
4051
4052 kyaD0_0 = polyCoeffs_a.z0_.y_;
4053 kyaD0_1 = polyCoeffs_a.z1_.y_.offtype< 0, 1 >( );
4054 kyaD0_2 = polyCoeffs_a.z2_.y_.offtype< 0, 2 >( );
4055 kyaD0_3 = polyCoeffs_a.z3_.y_.offtype< 0, 3 >( );
4056
4057 kyaD1_0 = kyaD0_1;
4058 kyaD1_1 = 2 * kyaD0_2;
4059 kyaD1_2 = 3 * kyaD0_3;
4060
4061 kyaD2_0 = kyaD1_1;
4062 kyaD2_1 = 2 * kyaD1_2;
4063
4064 kxbD0_0 = polyCoeffs_b.z0_.x_;
4065 kxbD0_1 = polyCoeffs_b.z1_.x_.offtype< 0, 1 >( );
4066 kxbD0_2 = polyCoeffs_b.z2_.x_.offtype< 0, 2 >( );
4067 kxbD0_3 = polyCoeffs_b.z3_.x_.offtype< 0, 3 >( );
4068
4069 kxbD1_0 = kxbD0_1;
4070 kxbD1_1 = 2 * kxbD0_2;
4071 kxbD1_2 = 3 * kxbD0_3;
4072
4073 kxbD2_0 = kxbD1_1;
4074 kxbD2_1 = 2 * kxbD1_2;
4075
4076 kybD0_0 = polyCoeffs_b.z0_.y_;
4077 kybD0_1 = polyCoeffs_b.z1_.y_.offtype< 0, 1 >( );
4078 kybD0_2 = polyCoeffs_b.z2_.y_.offtype< 0, 2 >( );
4079 kybD0_3 = polyCoeffs_b.z3_.y_.offtype< 0, 3 >( );
4080
4081 kybD1_0 = kybD0_1;
4082 kybD1_1 = 2 * kybD0_2;
4083 kybD1_2 = 3 * kybD0_3;
4084
4085 kybD2_0 = kybD1_1;
4086 kybD2_1 = 2 * kybD1_2;
4087 }
4088
4089 Computation::SegmentPolynomialPair2D::SegmentPolynomialPair2D( Concrete::Time steps_a, const Bezier::ControlPoints< Concrete::Coords2D > & seg_a, Concrete::Time steps_b, const Bezier::ControlPoints< Concrete::Coords2D > & seg_b )
4090 : polyCoeffs_a( seg_a ), straightLineSeg_a_( false ), polyCoeffs_b( seg_b ), straightLineSeg_b_( false ),
4091 steps_a_( steps_a ), steps_b_( steps_b )
4092 {
4093 kInit( );
4094
4095 maxSpeed_a_ = max( max( hypotPhysical( seg_a.p1_.x_ - seg_a.p0_.x_, seg_a.p1_.y_ - seg_a.p0_.y_ ),
4096 hypotPhysical( seg_a.p2_.x_ - seg_a.p1_.x_, seg_a.p2_.y_ - seg_a.p1_.y_ ) ),
4097 hypotPhysical( seg_a.p3_.x_ - seg_a.p2_.x_, seg_a.p3_.y_ - seg_a.p2_.y_ ) ).offtype< 0, 1 >( );
4098 maxSpeed_b_ = max( max( hypotPhysical( seg_b.p1_.x_ - seg_b.p0_.x_, seg_b.p1_.y_ - seg_b.p0_.y_ ),
4099 hypotPhysical( seg_b.p2_.x_ - seg_b.p1_.x_, seg_b.p2_.y_ - seg_b.p1_.y_ ) ),
4100 hypotPhysical( seg_b.p3_.x_ - seg_b.p2_.x_, seg_b.p3_.y_ - seg_b.p2_.y_ ) ).offtype< 0, 1 >( );
4101 }
4102
4103 Computation::SegmentPolynomialPair2D::SegmentPolynomialPair2D( Concrete::Time steps_a, const Concrete::Coords2D & seg_a_p0, const Concrete::Coords2D & seg_a_p3, Concrete::Time steps_b, const Bezier::ControlPoints< Concrete::Coords2D > & seg_b )
4104 : polyCoeffs_a( Bezier::ControlPoints< Concrete::Coords2D >( seg_a_p0, seg_a_p0, seg_a_p3, seg_a_p3 ) ), straightLineSeg_a_( true ), polyCoeffs_b( seg_b ), straightLineSeg_b_( false ),
4105 steps_a_( steps_a ), steps_b_( steps_b )
4106 {
4107 kInit( );
4108
4109 maxSpeed_a_ = hypotPhysical( seg_a_p3.x_ - seg_a_p0.x_, seg_a_p3.y_ - seg_a_p0.y_ ).offtype< 0, 1 >( );
4110 maxSpeed_b_ = max( max( hypotPhysical( seg_b.p1_.x_ - seg_b.p0_.x_, seg_b.p1_.y_ - seg_b.p0_.y_ ),
4111 hypotPhysical( seg_b.p2_.x_ - seg_b.p1_.x_, seg_b.p2_.y_ - seg_b.p1_.y_ ) ),
4112 hypotPhysical( seg_b.p3_.x_ - seg_b.p2_.x_, seg_b.p3_.y_ - seg_b.p2_.y_ ) ).offtype< 0, 1 >( );
4113 }
4114
4115 Computation::SegmentPolynomialPair2D::SegmentPolynomialPair2D( Concrete::Time steps_a, const Bezier::ControlPoints< Concrete::Coords2D > & seg_a, Concrete::Time steps_b, const Concrete::Coords2D & seg_b_p0, const Concrete::Coords2D & seg_b_p3 )
4116 : polyCoeffs_a( seg_a ), straightLineSeg_a_( false ), polyCoeffs_b( Bezier::ControlPoints< Concrete::Coords2D >( seg_b_p0, seg_b_p0, seg_b_p3, seg_b_p3 ) ), straightLineSeg_b_( true ),
4117 steps_a_( steps_a ), steps_b_( steps_b )
4118 {
4119 kInit( );
4120
4121 maxSpeed_a_ = max( max( hypotPhysical( seg_a.p1_.x_ - seg_a.p0_.x_, seg_a.p1_.y_ - seg_a.p0_.y_ ),
4122 hypotPhysical( seg_a.p2_.x_ - seg_a.p1_.x_, seg_a.p2_.y_ - seg_a.p1_.y_ ) ),
4123 hypotPhysical( seg_a.p3_.x_ - seg_a.p2_.x_, seg_a.p3_.y_ - seg_a.p2_.y_ ) ).offtype< 0, 1 >( );
4124 maxSpeed_b_ = hypotPhysical( seg_b_p3.x_ - seg_b_p0.x_, seg_b_p3.y_ - seg_b_p0.y_ ).offtype< 0, 1 >( );
4125 }
4126
4127 bool
4128 Computation::SegmentPolynomialPair2D::splitTimes( Concrete::Time * dst_a, Concrete::Time * dst_b, const Concrete::Time t_alow, const Concrete::Time t_ahigh, const Concrete::Time t_atol, const Concrete::Time t_blow, const Concrete::Time t_bhigh, const Concrete::Time t_btol, Concrete::Length distance_tol ) const
4129 {
4130 Concrete::Time ta;
4131 Concrete::Time tb;
4132 Concrete::LengthSquared last_f( HUGE_VAL );
4133 {
4134 const double GRID = 7;
4135 const Concrete::Time STEP_A = ( t_ahigh - t_alow ) / GRID;
4136 const Concrete::Time STEP_B = ( t_bhigh - t_blow ) / GRID;
4137 for( Concrete::Time tta = t_alow + 0.5 * STEP_A; tta < t_ahigh; tta += STEP_A )
4138 {
4139 for( Concrete::Time ttb = t_blow + 0.5 * STEP_B; ttb < t_bhigh; ttb += STEP_B )
4140 {
4141 Concrete::LengthSquared tmp = squaredDistanceAt( tta, ttb );
4142 if( tmp < last_f )
4143 {
4144 last_f = tmp;
4145 ta = tta;
4146 tb = ttb;
4147 }
4148 }
4149 }
4150 }
4151 Concrete::Time last_ta = - Concrete::UNIT_TIME;
4152 Concrete::Time last_tb = - Concrete::UNIT_TIME;
4153
4154 while( ta < last_ta - t_atol || ta > last_ta + t_atol ||
4155 tb < last_tb - t_btol || tb > last_tb + t_btol )
4156 {
4157 last_ta = ta;
4158 last_tb = tb;
4159 /* The derivatives below are actually computed for the objective divided by two
4160 * but since we will divide one by the other, it doesn't matter.
4161 */
4162 Physical< 2, -1 > objectiveD1a( 0 );
4163 Physical< 2, -1 > objectiveD1b( 0 );
4164 Physical< 2, -2 > objectiveD2aa( 0 );
4165 Physical< 2, -2 > objectiveD2ab( 0 );
4166 Physical< 2, -2 > objectiveD2bb( 0 );
4167
4168 {
4169 /* x-components */
4170 Physical< 1, 0 > faD0 = kxaD0_0 + ta * ( kxaD0_1 + ta * ( kxaD0_2 + ta * kxaD0_3 ) );
4171 Physical< 1, 0 > fbD0 = kxbD0_0 + tb * ( kxbD0_1 + tb * ( kxbD0_2 + tb * kxbD0_3 ) );
4172 Physical< 1, -1 > faD1 = kxaD1_0 + ta * ( kxaD1_1 + ta * kxaD1_2 );
4173 Physical< 1, -1 > fbD1 = kxbD1_0 + tb * ( kxbD1_1 + tb * kxbD1_2 );
4174 Physical< 1, -2 > faD2 = kxaD2_0 + ta * kxaD2_1;
4175 Physical< 1, -2 > fbD2 = kxbD2_0 + tb * kxbD2_1;
4176
4177 Physical< 1, 0 > deltaD0 = faD0 - fbD0;
4178
4179 objectiveD1a += deltaD0 * faD1;
4180 objectiveD1b += - deltaD0 * fbD1;
4181 objectiveD2aa += faD1 * faD1 + deltaD0 * faD2;
4182 objectiveD2bb += fbD1 * fbD1 - deltaD0 * fbD2;
4183 objectiveD2ab += - faD1 * fbD1;
4184 }
4185
4186 {
4187 /* y-components */
4188 Physical< 1, 0 > faD0 = kyaD0_0 + ta * ( kyaD0_1 + ta * ( kyaD0_2 + ta * kyaD0_3 ) );
4189 Physical< 1, 0 > fbD0 = kybD0_0 + tb * ( kybD0_1 + tb * ( kybD0_2 + tb * kybD0_3 ) );
4190 Physical< 1, -1 > faD1 = kyaD1_0 + ta * ( kyaD1_1 + ta * kyaD1_2 );
4191 Physical< 1, -1 > fbD1 = kybD1_0 + tb * ( kybD1_1 + tb * kybD1_2 );
4192 Physical< 1, -2 > faD2 = kyaD2_0 + ta * kyaD2_1;
4193 Physical< 1, -2 > fbD2 = kybD2_0 + tb * kybD2_1;
4194
4195 Physical< 1, 0 > deltaD0 = faD0 - fbD0;
4196
4197 objectiveD1a += deltaD0 * faD1;
4198 objectiveD1b += - deltaD0 * fbD1;
4199 objectiveD2aa += faD1 * faD1 + deltaD0 * faD2;
4200 objectiveD2bb += fbD1 * fbD1 - deltaD0 * fbD2;
4201 objectiveD2ab += - faD1 * fbD1;
4202 }
4203
4204 Concrete::Time step_a;
4205 Concrete::Time step_b;
4206 if( objectiveD2aa <= Physical< 2, -2 >( 0 ) || objectiveD2bb <= Physical< 2, -2 >( 0 ) || objectiveD2ab * objectiveD2ab >= objectiveD2aa * objectiveD2bb )
4207 {
4208 /* The minimization problem is not convex, locally.
4209 * Knowing that we have examined a grid of candidate points, we stop the minimization here.
4210 */
4211
4212 break;
4213
4214 // double D1norm = hypotPhysical( objecitveD1a, objectiveD1b );
4215 // double boxDiameter = max( t_ahigh - t_alow, t_bhigh - t_blow );
4216 // step_a = - objectiveD1a * ( boxDiameter / D1norm );
4217 // step_b = - objectiveD1b * ( boxDiameter / D1norm );
4218 }
4219 else
4220 {
4221 Physical< -4, 4 > invDet = 1. / ( objectiveD2aa * objectiveD2bb - objectiveD2ab * objectiveD2ab );
4222 // here's the division where the missing factors of 2 cancel.
4223 step_a = - invDet * ( objectiveD2bb * objectiveD1a - objectiveD2ab * objectiveD1b );
4224 step_b = - invDet * ( - objectiveD2ab * objectiveD1a + objectiveD2aa * objectiveD1b );
4225 }
4226
4227 bool onBound = false;
4228 {
4229 double alpha = 1;
4230
4231 if( ta + alpha * step_a < t_alow )
4232 {
4233 alpha = ( t_alow - ta ) / step_a;
4234 onBound = true;
4235 }
4236 else if( ta + alpha * step_a > t_ahigh )
4237 {
4238 alpha = ( t_ahigh - ta ) / step_a;
4239 onBound = true;
4240 }
4241
4242 if( tb + alpha * step_b < t_blow )
4243 {
4244 alpha = ( t_blow - tb ) / step_b;
4245 onBound = true;
4246 }
4247 else if( tb + alpha * step_b > t_bhigh )
4248 {
4249 alpha = ( t_bhigh - tb ) / step_b;
4250 onBound = true;
4251 }
4252
4253 step_a *= alpha;
4254 step_b *= alpha;
4255 }
4256
4257 Concrete::LengthSquared f = squaredDistanceAt( ta + step_a, tb + step_b );
4258 while( f > last_f && ( step_a.abs( ) > t_atol || step_b.abs( ) > t_btol ) )
4259 {
4260 step_a = step_a * 0.5;
4261 step_b = step_b * 0.5;
4262 f = squaredDistanceAt( ta + step_a, tb + step_b );
4263 onBound = false;
4264 }
4265 ta += step_a;
4266 tb += step_b;
4267 last_f = f;
4268
4269 if( onBound )
4270 {
4271 break;
4272 }
4273 }
4274
4275 *dst_a = ta;
4276 *dst_b = tb;
4277 return distanceAt( ta, tb ) < distance_tol;
4278 }
4279
4280 Concrete::LengthSquared
4281 Computation::SegmentPolynomialPair2D::squaredDistanceAt( Concrete::Time ta, Concrete::Time tb ) const
4282 {
4283 Concrete::Length fxaD0 = kxaD0_0 + ta * ( kxaD0_1 + ta * ( kxaD0_2 + ta * kxaD0_3 ) );
4284 Concrete::Length fyaD0 = kyaD0_0 + ta * ( kyaD0_1 + ta * ( kyaD0_2 + ta * kyaD0_3 ) );
4285 Concrete::Length fxbD0 = kxbD0_0 + tb * ( kxbD0_1 + tb * ( kxbD0_2 + tb * kxbD0_3 ) );
4286 Concrete::Length fybD0 = kybD0_0 + tb * ( kybD0_1 + tb * ( kybD0_2 + tb * kybD0_3 ) );
4287 Concrete::Length deltax = fxaD0 - fxbD0;
4288 Concrete::Length deltay = fyaD0 - fybD0;
4289 return deltax * deltax + deltay * deltay;
4290 }
4291
4292 Concrete::Length
4293 Computation::SegmentPolynomialPair2D::distanceAt( Concrete::Time ta, Concrete::Time tb ) const
4294 {
4295 Concrete::Length fxaD0 = kxaD0_0 + ta * ( kxaD0_1 + ta * ( kxaD0_2 + ta * kxaD0_3 ) );
4296 Concrete::Length fyaD0 = kyaD0_0 + ta * ( kyaD0_1 + ta * ( kyaD0_2 + ta * kyaD0_3 ) );
4297 Concrete::Length fxbD0 = kxbD0_0 + tb * ( kxbD0_1 + tb * ( kxbD0_2 + tb * kxbD0_3 ) );
4298 Concrete::Length fybD0 = kybD0_0 + tb * ( kybD0_1 + tb * ( kybD0_2 + tb * kybD0_3 ) );
4299 Concrete::Length deltax = fxaD0 - fxbD0;
4300 Concrete::Length deltay = fyaD0 - fybD0;
4301 return hypotPhysical( deltax, deltay );
4302 }
4303
4304 Bezier::ControlPoints< Concrete::Coords2D >
4305 Computation::SegmentPolynomialPair2D::getControls_a( const Concrete::Time t_low, const Concrete::Time t_high ) const
4306 {
4307 return Bezier::ControlPoints< Concrete::Coords2D >( polyCoeffs_a.subSection( t_low.offtype< 0, 1 >( ), t_high.offtype< 0, 1 >( ) ) );
4308 }
4309
4310 Bezier::ControlPoints< Concrete::Coords2D >
4311 Computation::SegmentPolynomialPair2D::getControls_b( const Concrete::Time t_low, const Concrete::Time t_high ) const
4312 {
4313 return Bezier::ControlPoints< Concrete::Coords2D >( polyCoeffs_b.subSection( t_low.offtype< 0, 1 >( ), t_high.offtype< 0, 1 >( ) ) );
4314 }
4315
4316 Computation::SegmentSectionPair2D::SegmentSectionPair2D( const SegmentPolynomialPair2D * _baseSegs, Concrete::Time _t_a0, Concrete::Time _t_a1, Concrete::Time _t_b0, Concrete::Time _t_b1 )
4317 : controls_a( _baseSegs->getControls_a( _t_a0, _t_a1 ) ), controls_b( _baseSegs->getControls_b( _t_b0, _t_b1 ) ),
4318 baseSegs( _baseSegs ), t_a0( _t_a0 ), t_a1( _t_a1 ), t_b0( _t_b0 ), t_b1( _t_b1 )
4319 { }
4320
4321 Computation::SegmentSectionPair2D *
4322 Computation::SegmentSectionPair2D::cutAfterAfter( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4323 {
4324 return new SegmentSectionPair2D( baseSegs, t_a0, ta - tol_a, t_b0, tb - tol_b );
4325 }
4326
4327 Computation::SegmentSectionPair2D *
4328 Computation::SegmentSectionPair2D::cutAfterBefore( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4329 {
4330 return new SegmentSectionPair2D( baseSegs, t_a0, ta - tol_a, tb + tol_b, t_b1 );
4331 }
4332
4333 Computation::SegmentSectionPair2D *
4334 Computation::SegmentSectionPair2D::cutBeforeAfter( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4335 {
4336 return new SegmentSectionPair2D( baseSegs, ta + tol_a, t_a1, t_b0, tb - tol_b );
4337 }
4338
4339 Computation::SegmentSectionPair2D *
4340 Computation::SegmentSectionPair2D::cutBeforeBefore( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4341 {
4342 return new SegmentSectionPair2D( baseSegs, ta + tol_a, t_a1, tb + tol_b, t_b1 );
4343 }
4344
4345 bool
4346 Computation::SegmentSectionPair2D::splitTimes( Concrete::Time * dst_a, Concrete::Time * dst_b, const Concrete::Time t_tol_a, const Concrete::Time t_tol_b, Concrete::Length distance_tol ) const
4347 {
4348 Concrete::Time da = 0.01 * ( t_a1 - t_a0 );
4349 Concrete::Time db = 0.01 * ( t_b1 - t_b0 );
4350 if( baseSegs->splitTimes( dst_a, dst_b,
4351 t_a0, t_a1, t_tol_a,
4352 t_b0, t_b1, t_tol_b,
4353 distance_tol ) )
4354 {
4355 /* An intersection was found to withing tolerances. Do not adjust for robust convergence! */
4356 return true;
4357 }
4358
4359 /* For robust convergence, make sure we don't return split times that are too close to the corners of these sections. */
4360 if( *dst_a < t_a0 + da && *dst_b < t_b0 + db )
4361 {
4362 *dst_a = t_a0 + da;
4363 *dst_b = t_b0 + db;
4364 }
4365 else if( *dst_a < t_a0 + da && *dst_b > t_b1 - db )
4366 {
4367 *dst_a = t_a0 + da;
4368 *dst_b = t_b1 - db;
4369 }
4370 else if( *dst_a > t_a1 - da && *dst_b < t_b0 + db )
4371 {
4372 *dst_a = t_a1 - da;
4373 *dst_b = t_b0 + db;
4374 }
4375 else if( *dst_a > t_a1 - da && *dst_b > t_b1 - db )
4376 {
4377 *dst_a = t_a1 - da;
4378 *dst_b = t_b1 - db;
4379 }
4380 return false;
4381 }
4382
4383 Concrete::Time
4384 Computation::SegmentSectionPair2D::globalTime_a( Concrete::Time t ) const
4385 {
4386 return steps_a( ) + t;
4387 }
4388
4389 Concrete::Length
4390 Computation::SegmentSectionPair2D::distanceAt( Concrete::Time ta, Concrete::Time tb ) const
4391 {
4392 return baseSegs->distanceAt( ta, tb );
4393 }
4394
4395 Concrete::LengthSquared
4396 Computation::SegmentSectionPair2D::squaredDistanceAt( Concrete::Time ta, Concrete::Time tb ) const
4397 {
4398 return baseSegs->squaredDistanceAt( ta, tb );
4399 }
4400
4401 bool
4402 Computation::SegmentSectionPair2D::convexHullOverlap( ) const
4403 {
4404 /* The implementation is based around the idea that if there is a separating line
4405 * (which there is exactly when the polygons don't overlap), there has to be
4406 * a direction of such a line that is also the direction from one point to another
4407 * within a polygon. This gives some symmetry, which is implemented by
4408 * calling a function that searches such directions in only one of the polygons.
4409 * A quick check using enclosing circles is performed first to detect the simples cases.
4410 */
4411 std::vector< const Concrete::Coords2D * > pointSet_a( 0 );
4412 pointSet_a.reserve( 4 );
4413 pointSet_a.push_back( & controls_a.p0_ );
4414 pointSet_a.push_back( & controls_a.p1_ );
4415 pointSet_a.push_back( & controls_a.p2_ );
4416 pointSet_a.push_back( & controls_a.p3_ );
4417 std::vector< const Concrete::Coords2D * > pointSet_b( 0 );
4418 pointSet_b.reserve( 4 );
4419 pointSet_b.push_back( & controls_b.p0_ );
4420 pointSet_b.push_back( & controls_b.p1_ );
4421 pointSet_b.push_back( & controls_b.p2_ );
4422 pointSet_b.push_back( & controls_b.p3_ );
4423
4424 /* Do the circle-circle check
4425 */
4426 {
4427 Concrete::Coords2D ca( 0, 0 );
4428 for( std::vector< const Concrete::Coords2D * >::const_iterator i = pointSet_a.begin( );
4429 i != pointSet_a.end( );
4430 ++i )
4431 {
4432 ca = ca + **i;
4433 }
4434 ca = ca * 0.25;
4435
4436 Concrete::Coords2D cb( 0, 0 );
4437 for( std::vector< const Concrete::Coords2D * >::const_iterator i = pointSet_b.begin( );
4438 i != pointSet_b.end( );
4439 ++i )
4440 {
4441 cb = cb + **i;
4442 }
4443 cb = cb * 0.25;
4444
4445 Concrete::Length ra = 0;
4446 for( std::vector< const Concrete::Coords2D * >::const_iterator i = pointSet_a.begin( );
4447 i != pointSet_a.end( );
4448 ++i )
4449 {
4450 ra = max( ra, hypotPhysical( ca.x_ - (*i)->x_, ca.y_ - (*i)->y_ ) );
4451 }
4452
4453 Concrete::Length rb = 0;
4454 for( std::vector< const Concrete::Coords2D * >::const_iterator i = pointSet_b.begin( );
4455 i != pointSet_b.end( );
4456 ++i )
4457 {
4458 rb = max( rb, hypotPhysical( cb.x_ - (*i)->x_, cb.y_ - (*i)->y_ ) );
4459 }
4460
4461 if( ( ra + rb ) * ( ra + rb ) < ( ca.x_ - cb.x_ ) * ( ca.x_ - cb.x_ ) + ( ca.y_ - cb.y_ ) * ( ca.y_ - cb.y_ ) )
4462 {
4463 return false;
4464 }
4465 }
4466
4467 /* Now to the search for a separating line.
4468 */
4469 if( ! convexHullOneWayOverlap( pointSet_a, pointSet_b ) )
4470 {
4471 return false;
4472 }
4473 if( ! convexHullOneWayOverlap( pointSet_b, pointSet_a ) )
4474 {
4475 return false;
4476 }
4477 return true;
4478 }
4479
4480 bool
4481 Computation::SegmentSectionPair2D::convexHullOneWayOverlap( const std::vector< const Concrete::Coords2D * > & pointSet1, const std::vector< const Concrete::Coords2D * > & pointSet2 )
4482 {
4483 Concrete::LengthSquared coincide_tol = 0.0001 * Computation::the_arcdelta * Computation::the_arcdelta;
4484 for( std::vector< const Concrete::Coords2D * >::const_iterator i0 = pointSet1.begin( ); i0 != pointSet1.end( ); ++i0 )
4485 {
4486 std::vector< const Concrete::Coords2D * >::const_iterator i1 = i0;
4487 ++i1;
4488 for( ; i1 != pointSet1.end( ); ++i1 )
4489 {
4490 const Concrete::Coords2D & p0( **i0 );
4491 const Concrete::Coords2D & p1( **i1 );
4492
4493 /* The two points bound the convex hull iff the other points are on the same side of the segment.
4494 * If they are on the same side and p is on the other side, then the distance to p is of interest.
4495 */
4496 const Concrete::Length tx = p1.x_ - p0.x_;
4497 const Concrete::Length ty = p1.y_ - p0.y_;
4498 if( tx * tx + ty * ty < coincide_tol )
4499 {
4500 continue;
4501 }
4502 bool counterClockwise; // true when ( p0, p1 ) are ordered counter-clockwise around the interior.
4503 {
4504 std::vector< const Concrete::Coords2D * >::const_iterator i2 = pointSet1.begin( );
4505 for( ; ; ++i2 ) // we don't have to check against pointSet1.end( )
4506 {
4507 if( i2 == i0 || i2 == i1 )
4508 {
4509 continue;
4510 }
4511 const Concrete::Length dx = (*i2)->x_ - p0.x_;
4512 const Concrete::Length dy = (*i2)->y_ - p0.y_;
4513 counterClockwise = ( ty * dx - tx * dy < 0 );
4514 break;
4515 }
4516 ++i2;
4517 for( ; ; ++i2 ) // we don't have to check against pointSet1.end( )
4518 {
4519 if( i2 == i0 || i2 == i1 )
4520 {
4521 continue;
4522 }
4523 const Concrete::Length dx = (*i2)->x_ - p0.x_;
4524 const Concrete::Length dy = (*i2)->y_ - p0.y_;
4525 if( counterClockwise == ( ty * dx - tx * dy < 0 ) )
4526 {
4527 goto checkDistance;
4528 }
4529 break;
4530 }
4531 continue; // the points were on different sides.
4532 checkDistance:
4533
4534 bool allOutside = true;
4535 for( std::vector< const Concrete::Coords2D * >::const_iterator p = pointSet2.begin( ); p != pointSet2.end( ); ++p )
4536 {
4537 const Concrete::Length dx = (*p)->x_ - p0.x_;
4538 const Concrete::Length dy = (*p)->y_ - p0.y_;
4539 if( ( ty * dx - tx * dy > 0 ) != counterClockwise )
4540 {
4541 allOutside = false;
4542 break;
4543 }
4544 }
4545 if( allOutside )
4546 {
4547 return false;
4548 }
4549 }
4550 }
4551 }
4552
4553 /* Note that this is only one part of the test, only if both symmetric tests return true do the
4554 * polygons overlap.
4555 */
4556 return true;
4557 }
4558
4559 bool
4560 Computation::SegmentSectionPair2D::convexHullSeparatedBy( Concrete::LengthSquared sep ) const
4561 {
4562 hull_dist_ = Concrete::LengthSquared( 0 );
4563 /* Even though there is a chance that the eight doubles that describe the generators are
4564 * stored densly, starting at & controls_a, the copying we do here takes no time compared
4565 * to the distance computation itself.
4566 */
4567 const size_t N = 4; /* Maximum number of generators per polytope. */
4568 const size_t DIM = 2; /* Dimension of each generator. */
4569 double ga[ N * DIM ]; /* Allocate data on stack. */
4570 double gb[ N * DIM ];
4571 size_t na; /* Number of generators used. */
4572 size_t nb;
4573 /* Initialize generators for each polytope. */
4574 if( baseSegs->straightLineSeg_a( ) )
4575 {
4576 na = 2;
4577 controls_a.getEndpoints( & ga[0] );
4578 }
4579 else
4580 {
4581 na = 4;
4582 controls_a.get( & ga[0] );
4583 }
4584 if( baseSegs->straightLineSeg_b( ) )
4585 {
4586 nb = 2;
4587 controls_b.getEndpoints( & gb[0] );
4588 }
4589 else
4590 {
4591 nb = 4;
4592 controls_b.get( & gb[0] );
4593 }
4594
4595 double double_distSquaredLow;
4596 double double_distSquaredHigh;
4597 QPSolverStatus status;
4598 Computation::polytope_distance_generator_form( DIM,
4599 na, & ga[0],
4600 nb, & gb[0],
4601 & double_distSquaredLow, & double_distSquaredHigh,
4602 Concrete::LengthSquared::offtype( sep ), -1,
4603 0, 0,
4604 0.5 * Concrete::Length::offtype( Computation::theDistanceTol ), /* Lagrange multiplier tolerance. */
4605 0, 0,
4606 0, 0, /* We don't care about where the optimum is */
4607 & status );
4608 if( status.code != Computation::QP_OK )
4609 {
4610 const char * binaryFilename = "qp-fail-data.binary";
4611 Computation::polytope_distance_generator_form_write_binary_data( binaryFilename,
4612 DIM,
4613 na, & ga[0],
4614 nb, & gb[0] );
4615 std::ostringstream msg;
4616 msg << "QP solver status: " << status.code_str( ) << ", with reason: " << status.sub_str( )
4617 << ", binary data was written to \"" << binaryFilename << "\"." ;
4618 throw Exceptions::InternalError( strrefdup( msg ) );
4619 }
4620 Concrete::LengthSquared distSquaredLow( double_distSquaredLow );
4621 Concrete::LengthSquared distSquaredHigh( double_distSquaredHigh );
4622 if( distSquaredLow >= sep )
4623 {
4624 return true;
4625 }
4626 hull_dist_ = distSquaredHigh;
4627 return false;
4628 }
4629
4630 void
4631 Computation::SegmentSectionPair2D::update_t_a1( Concrete::Time t )
4632 {
4633 t_a1 = min( t_a1, t );
4634 }
4635
4636 bool
4637 Computation::SegmentSectionPair2D::isEmpty( ) const
4638 {
4639 return t_a1 < t_a0 || t_b1 < t_b0;
4640 }
4641
4642 void
4643 Computation::SegmentSectionPair2D::writeTimes( std::ostream & os ) const
4644 {
4645 os << Concrete::Time::offtype( t_a0 ) << "--" << Concrete::Time::offtype( t_a1 ) << ", "
4646 << Concrete::Time::offtype( t_b0 ) << "--" << Concrete::Time::offtype( t_b1 ) << std::endl ;
4647 }
4648
4649
4650 Concrete::Speed
4651 Computation::SegmentSectionPair2D::maxSpeed_a( ) const
4652 {
4653 return baseSegs->maxSpeed_a( );
4654 }
4655
4656 Concrete::Speed
4657 Computation::SegmentSectionPair2D::maxSpeed_b( ) const
4658 {
4659 return baseSegs->maxSpeed_b( );
4660 // Concrete::Length lengthBound =
4661 // hypotPhysical( controls_b.p1_.x_ - controls_b.p0_.x_, controls_b.p1_.y_ - controls_b.p0_.y_ ) +
4662 // hypotPhysical( controls_b.p2_.x_ - controls_b.p1_.x_, controls_b.p2_.y_ - controls_b.p1_.y_ ) +
4663 // hypotPhysical( controls_b.p3_.x_ - controls_b.p2_.x_, controls_b.p3_.y_ - controls_b.p2_.y_ );
4664
4665 // return arclen_tol * ( t_b1 - t_b0 ) / *lengthBound;
4666 }
4667
4668 RefCountPtr< const Lang::ElementaryPath2D >
4669 Lang::ElementaryPath2D::subpath( const Concrete::SplineTime splt1, const Concrete::SplineTime splt2 ) const
4670 {
4671 if( empty( ) )
4672 {
4673 throw Exceptions::OutOfRange( "The empty path has no subpaths." );
4674 }
4675 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D;
4676
4677 if( splt2.t( ) < splt1.t( ) )
4678 {
4679 throw Exceptions::OutOfRange( "The path-times must come in ascending order." );
4680 }
4681 if( splt2.t( ) != HUGE_VAL && splt2.t( ) > splt1.t( ) + size( ) )
4682 {
4683 throw Exceptions::OutOfRange( "It is forbidden (although not logically necessary) to cycle more than one revolution." );
4684 }
4685
4686 Concrete::Time gt1 = timeCheck( splt1.t( ) ); /* "g" as in global. t1 refers to a time on an elementary segment */
4687 Concrete::Time gt2 = timeCheck( splt2.t( ) );
4688 if( gt2 <= gt1 && splt2.t( ) > splt1.t( ) )
4689 {
4690 gt2 += size( );
4691 }
4692
4693 /* First, we treat the case where both cuts are on the same elementary segment.
4694 * The reason is twofold. First, it is more efficient, as it is almost only half
4695 * the amount of computation this way. Secondly, cutting sequentially on the same
4696 * segment requires a bit of extra manipulation of the time arguments.
4697 */
4698 if( fmod( floor( gt1.offtype< 0, 1 >( ) ) + 1, duration( ) + 1 ) == ceil( gt2.offtype< 0, 1 >( ) ) )
4699 {
4700 Concrete::Time t1;
4701 const Concrete::PathPoint2D * p1;
4702 const Concrete::PathPoint2D * p2;
4703 findSegment( gt1, &t1, &p1, &p2 ); /* note that the stored pointers must not be deleted */
4704 Concrete::Time t2 = gt2 - ceil( gt2.offtype< 0, 1 >( ) ) + 1;
4705 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
4706 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
4707 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
4708 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
4709 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
4710 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
4711 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
4712 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
4713
4714 Concrete::Time tc = t1 - 1; /* Note the sign here! */
4715 Concrete::Time td = t1 - t2;
4716
4717 /* Compute new x polynomial using variable change t = t1 - td * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
4718 */
4719 Concrete::Length k0x = - tc * tc * tc * x0 + t1 * ( 3 * tc * tc * x1 + t1 * ( 3 * Concrete::UNIT_TIME * x2 - 3 * t1 * x2 + t1 * x3 ) );
4720 Concrete::Length k1x = 3 * td * ( tc * tc * x0 + ( -Concrete::UNIT_TIME*Concrete::UNIT_TIME + 4 * Concrete::UNIT_TIME * t1 - 3 * t1 * t1 ) * x1 + t1 * ( -2 * Concrete::UNIT_TIME * x2 + 3 * t1 * x2 - t1 * x3 ) );
4721 Concrete::Length k2x = -3 * td * td * ( tc * x0 + 2 * Concrete::UNIT_TIME * x1 - 3 * t1 * x1 - Concrete::UNIT_TIME * x2 + 3 * t1 * x2 - t1 * x3 );
4722 Concrete::Length k3x = td * td * td * ( x0 - 3 * x1 + 3 * x2 - x3 );
4723 /* Compute new coefficients in the standard polynomial base */
4724 Concrete::Length x0new = k0x;
4725 Concrete::Length x1new = k0x + k1x / 3;
4726 Concrete::Length x2new = k0x + ( 2 * k1x + k2x ) / 3;
4727 Concrete::Length x3new = k0x + k1x + k2x + k3x;
4728
4729 /* Same thing for y */
4730
4731 Concrete::Length k0y = - tc * tc * tc * y0 + t1 * ( 3 * tc * tc * y1 + t1 * ( 3 * Concrete::UNIT_TIME * y2 - 3 * t1 * y2 + t1 * y3 ) );
4732 Concrete::Length k1y = 3 * td * ( tc * tc * y0 + ( -Concrete::UNIT_TIME*Concrete::UNIT_TIME + 4 * Concrete::UNIT_TIME * t1 - 3 * t1 * t1 ) * y1 + t1 * ( -2 * Concrete::UNIT_TIME * y2 + 3 * t1 * y2 - t1 * y3 ) );
4733 Concrete::Length k2y = -3 * td * td * ( tc * y0 + 2 * Concrete::UNIT_TIME * y1 - 3 * t1 * y1 - Concrete::UNIT_TIME * y2 + 3 * t1 * y2 - t1 * y3 );
4734 Concrete::Length k3y = td * td * td * ( y0 - 3 * y1 + 3 * y2 - y3 );
4735
4736 Concrete::Length y0new = k0y;
4737 Concrete::Length y1new = k0y + k1y / 3;
4738 Concrete::Length y2new = k0y + ( 2 * k1y + k2y ) / 3;
4739 Concrete::Length y3new = k0y + k1y + k2y + k3y;
4740
4741 Concrete::PathPoint2D * ptmp = new Concrete::PathPoint2D( x0new, y0new );
4742 ptmp->front_ = new Concrete::Coords2D( x1new, y1new );
4743 res->push_back( ptmp );
4744 ptmp = new Concrete::PathPoint2D( x3new, y3new );
4745 ptmp->rear_ = new Concrete::Coords2D( x2new, y2new );
4746 res->push_back( ptmp );
4747
4748 return RefCountPtr< const Lang::ElementaryPath2D >( res );
4749 }
4750
4751
4752 /* Now, we treat the other case, where the cuts are on different segments. This results in a three phase
4753 * algorithm.
4754 * Since "cutting away after" requires much simpler formulas than straight-forward "cutting away before",
4755 * the latter is treated as the former, but with time reversed.
4756 */
4757
4758 Concrete::Time t = Concrete::ZERO_TIME;
4759 const_iterator i1 = begin( );
4760 for( ; t + 1 <= gt1; t += Concrete::UNIT_TIME, ++i1 )
4761 ;
4762 const_iterator i2 = i1;
4763 ++i2;
4764 if( i2 == end( ) )
4765 {
4766 i2 = begin( );
4767 }
4768
4769 Concrete::Time t1 = gt1 - t;
4770 Concrete::PathPoint2D * p1;
4771 Concrete::PathPoint2D * p2;
4772 if( t1 == 0 )
4773 {
4774 p1 = new Concrete::PathPoint2D( **i1 );
4775 p2 = new Concrete::PathPoint2D( **i2 );
4776 }
4777 else
4778 {
4779 /* Reverse time! */
4780 Concrete::Bezier x3 = (*i1)->mid_->x_.offtype< 0, 3 >( );
4781 Concrete::Bezier y3 = (*i1)->mid_->y_.offtype< 0, 3 >( );
4782 Concrete::Bezier x2 = (*i1)->front_->x_.offtype< 0, 3 >( );
4783 Concrete::Bezier y2 = (*i1)->front_->y_.offtype< 0, 3 >( );
4784 Concrete::Bezier x1 = (*i2)->rear_->x_.offtype< 0, 3 >( );
4785 Concrete::Bezier y1 = (*i2)->rear_->y_.offtype< 0, 3 >( );
4786 Concrete::Bezier x0 = (*i2)->mid_->x_.offtype< 0, 3 >( );
4787 Concrete::Bezier y0 = (*i2)->mid_->y_.offtype< 0, 3 >( );
4788 t1 = Concrete::UNIT_TIME - t1;
4789
4790 /* Compute new x polynomial using variable change t = t1 * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
4791 */
4792 Concrete::Bezier k0x = x0;
4793 Concrete::Bezier k1x = 3 * t1 * ( x1 - x0 ).offtype< 0, 1 >( );
4794 Concrete::Bezier k2x = 3 * t1 * t1 * ( x0 - 2 * x1 + x2 ).offtype< 0, 2 >( );
4795 Concrete::Bezier k3x = t1 * t1 * t1 * ( -x0 + 3 * x1 - 3 * x2 + x3 ).offtype< 0, 3 >( );
4796 /* Compute new coefficients in the standard polynomial base */
4797 Concrete::Bezier x0new = k0x;
4798 Concrete::Bezier x1new = k0x + k1x / 3;
4799 Concrete::Bezier x2new = k0x + ( 2 * k1x + k2x ) / 3;
4800 Concrete::Bezier x3new = k0x + k1x + k2x + k3x;
4801
4802 Concrete::Bezier k0y = y0;
4803 Concrete::Bezier k1y = 3 * t1 * ( y1 - y0 ).offtype< 0, 1 >( );
4804 Concrete::Bezier k2y = 3 * t1 * t1 * ( y0 - 2 * y1 + y2 ).offtype< 0, 2 >( );
4805 Concrete::Bezier k3y = t1 * t1 * t1 * ( -y0 + 3 * y1 - 3 * y2 + y3 ).offtype< 0, 3 >( );
4806
4807 Concrete::Bezier y0new = k0y;
4808 Concrete::Bezier y1new = k0y + k1y / 3;
4809 Concrete::Bezier y2new = k0y + ( 2 * k1y + k2y ) / 3;
4810 Concrete::Bezier y3new = k0y + k1y + k2y + k3y;
4811
4812 /* Now, reverse time again! */
4813
4814 p1 = new Concrete::PathPoint2D( x3new.offtype< 0, -3 >( ), y3new.offtype< 0, -3 >( ) );
4815 p1->front_ = new Concrete::Coords2D( x2new.offtype< 0, -3 >( ), y2new.offtype< 0, -3 >( ) );
4816 p2 = new Concrete::PathPoint2D( x0new.offtype< 0, -3 >( ), y0new.offtype< 0, -3 >( ) ); /* This point must be the same as before. */
4817 p2->rear_ = new Concrete::Coords2D( x1new.offtype< 0, -3 >( ), y1new.offtype< 0, -3 >( ) );
4818 p2->front_ = new Concrete::Coords2D( *(*i2)->front_ );
4819 }
4820
4821 /* At this point we know that the rest of this segment shall be included, since the cuts are not on the
4822 * same segment.
4823 */
4824
4825 for( ; t + 1 < gt2; t += Concrete::UNIT_TIME )
4826 {
4827 res->push_back( p1 );
4828 p1 = p2;
4829 i1 = i2;
4830 ++i2;
4831 if( i2 == end( ) )
4832 {
4833 i2 = begin( );
4834 }
4835 p2 = new Concrete::PathPoint2D( **i2 );
4836 }
4837
4838 /* Remember to delete p1 and p2 if unused! */
4839
4840 Concrete::Time t2 = gt2 - t;
4841 if( t2 == 1 )
4842 {
4843 res->push_back( p1 );
4844 res->push_back( p2 );
4845 }
4846 else
4847 {
4848 Concrete::Length xn1 = p1->rear_->x_; /* "n" for negative index. We save these values now, so that we can delete p1 and p2 soon. */
4849 Concrete::Length yn1 = p1->rear_->y_;
4850
4851 Concrete::Bezier x0 = p1->mid_->x_.offtype< 0, 3 >( );
4852 Concrete::Bezier y0 = p1->mid_->y_.offtype< 0, 3 >( );
4853 Concrete::Bezier x1 = p1->front_->x_.offtype< 0, 3 >( );
4854 Concrete::Bezier y1 = p1->front_->y_.offtype< 0, 3 >( );
4855 Concrete::Bezier x2 = p2->rear_->x_.offtype< 0, 3 >( );
4856 Concrete::Bezier y2 = p2->rear_->y_.offtype< 0, 3 >( );
4857 Concrete::Bezier x3 = p2->mid_->x_.offtype< 0, 3 >( );
4858 Concrete::Bezier y3 = p2->mid_->y_.offtype< 0, 3 >( );
4859
4860 delete p1;
4861 delete p2;
4862
4863 /* Compute new x polynomial using variable change t = t2 * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
4864 */
4865 Concrete::Bezier k0x = x0;
4866 Concrete::Bezier k1x = 3 * t2 * ( x1 - x0 ).offtype< 0, 1 >( );
4867 Concrete::Bezier k2x = 3 * t2 * t2 * ( x0 - 2 * x1 + x2 ).offtype< 0, 2 >( );
4868 Concrete::Bezier k3x = t2 * t2 * t2 * ( -x0 + 3 * x1 - 3 * x2 + x3 ).offtype< 0, 3 >( );
4869 /* Compute new coefficients in the standard polynomial base */
4870 Concrete::Bezier x0new = k0x;
4871 Concrete::Bezier x1new = k0x + k1x / 3;
4872 Concrete::Bezier x2new = k0x + ( 2 * k1x + k2x ) / 3;
4873 Concrete::Bezier x3new = k0x + k1x + k2x + k3x;
4874
4875 Concrete::Bezier k0y = y0;
4876 Concrete::Bezier k1y = 3 * t2 * ( y1 - y0 ).offtype< 0, 1 >( );
4877 Concrete::Bezier k2y = 3 * t2 * t2 * ( y0 - 2 * y1 + y2 ).offtype< 0, 2 >( );
4878 Concrete::Bezier k3y = t2 * t2 * t2 * ( -y0 + 3 * y1 - 3 * y2 + y3 ).offtype< 0, 3 >( );
4879
4880 Concrete::Bezier y0new = k0y;
4881 Concrete::Bezier y1new = k0y + k1y / 3;
4882 Concrete::Bezier y2new = k0y + ( 2 * k1y + k2y ) / 3;
4883 Concrete::Bezier y3new = k0y + k1y + k2y + k3y;
4884
4885 p1 = new Concrete::PathPoint2D( x0new.offtype< 0, -3 >( ), y0new.offtype< 0, -3 >( ) );
4886 p1->front_ = new Concrete::Coords2D( x1new.offtype< 0, -3 >( ), y1new.offtype< 0, -3 >( ) );
4887 p1->rear_ = new Concrete::Coords2D( xn1, yn1 );
4888 p2 = new Concrete::PathPoint2D( x3new.offtype< 0, -3 >( ), y3new.offtype< 0, -3 >( ) );
4889 p2->rear_ = new Concrete::Coords2D( x2new.offtype< 0, -3 >( ), y2new.offtype< 0, -3 >( ) );
4890
4891 res->push_back( p1 );
4892 res->push_back( p2 );
4893 }
4894
4895
4896 return RefCountPtr< const Lang::ElementaryPath2D >( res );
4897 }
4898
4899 RefCountPtr< const Lang::ElementaryPath2D >
4900 Lang::ElementaryPath2D::reverse( ) const
4901 {
4902 /* Note that reversing a closed path is not quite as straight-forward as one might first think!
4903 */
4904
4905 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D;
4906 if( closed_ )
4907 {
4908 res->close( );
4909 }
4910
4911 const_iterator i = begin( );
4912 if( i == end( ) )
4913 {
4914 return RefCountPtr< const Lang::ElementaryPath2D >( res );
4915 }
4916
4917 if( closed_ )
4918 {
4919 // The first path point remains first!
4920 ++i;
4921 }
4922 for( ; i != end( ); ++i )
4923 {
4924 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( new Concrete::Coords2D( *((*i)->mid_) ) );
4925 if( (*i)->rear_ != (*i)->mid_ )
4926 {
4927 newPoint->front_ = new Concrete::Coords2D( *((*i)->rear_) );
4928 }
4929 if( (*i)->front_ != (*i)->mid_ )
4930 {
4931 newPoint->rear_ = new Concrete::Coords2D( *((*i)->front_) );
4932 }
4933 res->push_front( newPoint );
4934 }
4935 if( closed_ )
4936 {
4937 i = begin( );
4938 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( new Concrete::Coords2D( *((*i)->mid_) ) );
4939 if( (*i)->rear_ != (*i)->mid_ )
4940 {
4941 newPoint->front_ = new Concrete::Coords2D( *((*i)->rear_) );
4942 }
4943 if( (*i)->front_ != (*i)->mid_ )
4944 {
4945 newPoint->rear_ = new Concrete::Coords2D( *((*i)->front_) );
4946 }
4947 res->push_front( newPoint );
4948 }
4949
4950 return RefCountPtr< const Lang::ElementaryPath2D >( res );
4951 }
4952
4953 void
4954 Lang::ElementaryPath2D::show( std::ostream & os ) const
4955 {
4956 os << "Elementary subpath with " << size( ) << " path points" ;
4957 }
4958
4959 void
4960 Lang::ElementaryPath2D::gcMark( Kernel::GCMarkedSet & marked )
4961 { }
4962
4963 RefCountPtr< const Lang::Path3D >
4964 Lang::ElementaryPath2D::typed_to3D( const RefCountPtr< const Lang::Path2D > & self ) const
4965 {
4966 typedef const Lang::ElementaryPath2D ArgType;
4967 RefCountPtr< ArgType > selfTyped = self.down_cast< ArgType >( );
4968 if( selfTyped == NullPtr< ArgType >( ) )
4969 {
4970 throw Exceptions::InternalError( "ElementaryPath2D::typed_to3D: self was of unexpected type." );
4971 }
4972
4973 return RefCountPtr< const Lang::Path3D >( new Lang::Path2Din3D( selfTyped ) );
4974 }
4975
4976
4977 RefCountPtr< const Lang::Geometric3D >
4978 Lang::ElementaryPath2D::to3D( const RefCountPtr< const Lang::Geometric2D > & self ) const
4979 {
4980 typedef const Lang::ElementaryPath2D ArgType;
4981 RefCountPtr< ArgType > selfTyped = self.down_cast< ArgType >( );
4982 if( selfTyped == NullPtr< ArgType >( ) )
4983 {
4984 throw Exceptions::InternalError( "ElementaryPath2D::to3D: self was of unexpected type." );
4985 }
4986
4987 return RefCountPtr< const Lang::Geometric3D >( new Lang::Path2Din3D( selfTyped ) );
4988 }
Graphics language