1 /* This file is part of Shapes.
3 * Shapes is free software: you can redistribute it and/or modify
4 * it under the terms of the GNU General Public License as published by
5 * the Free Software Foundation, either version 3 of the License, or
8 * Shapes is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
13 * You should have received a copy of the GNU General Public License
14 * along with Shapes. If not, see <http://www.gnu.org/licenses/>.
16 * Copyright 2008, 2009 Henrik Tidefelt
21 #include "shapestypes.h"
22 #include "shapesexceptions.h"
26 #include "angleselect.h"
28 #include "trianglefunctions.h"
29 #include "quadratic_programs.h"
35 using namespace Shapes
;
42 ControlPoints
< Concrete::Coords3D
>::get( double * dst
) const
44 *dst
= Concrete::Length::offtype( p0_
.x_
);
46 *dst
= Concrete::Length::offtype( p0_
.y_
);
48 *dst
= Concrete::Length::offtype( p0_
.z_
);
50 *dst
= Concrete::Length::offtype( p1_
.x_
);
52 *dst
= Concrete::Length::offtype( p1_
.y_
);
54 *dst
= Concrete::Length::offtype( p1_
.z_
);
56 *dst
= Concrete::Length::offtype( p2_
.x_
);
58 *dst
= Concrete::Length::offtype( p2_
.y_
);
60 *dst
= Concrete::Length::offtype( p2_
.z_
);
62 *dst
= Concrete::Length::offtype( p3_
.x_
);
64 *dst
= Concrete::Length::offtype( p3_
.y_
);
66 *dst
= Concrete::Length::offtype( p3_
.z_
);
71 ControlPoints
< Concrete::Coords3D
>::getEndpoints( double * dst
) const
73 *dst
= Concrete::Length::offtype( p0_
.x_
);
75 *dst
= Concrete::Length::offtype( p0_
.y_
);
77 *dst
= Concrete::Length::offtype( p0_
.z_
);
79 *dst
= Concrete::Length::offtype( p3_
.x_
);
81 *dst
= Concrete::Length::offtype( p3_
.y_
);
83 *dst
= Concrete::Length::offtype( p3_
.z_
);
87 template< int L
, int T
>
88 Physical
< L
+ L
, T
+ T
>
89 squareSum( Physical
< L
, T
> d1
, Physical
< L
, T
> d2
, Physical
< L
, T
> d3
)
91 return d1
* d1
+ d2
* d2
+ d3
* d3
;
95 tripleInner( Concrete::Speed vx
, Concrete::Speed vy
, Concrete::Speed vz
,
96 Concrete::Acceleration ax
, Concrete::Acceleration ay
, Concrete::Acceleration az
,
97 Concrete::Jerk jx
, Concrete::Jerk jy
, Concrete::Jerk jz
)
100 vx
* ( ay
* jz
- az
* jy
)
101 - vy
* ( ax
* jz
- az
* jx
)
102 + vz
* ( ax
* jy
- ay
* jx
);
105 Lang::ElementaryPath3D::ElementaryPath3D( )
108 DISPATCHIMPL( ElementaryPath3D
);
110 Lang::ElementaryPath3D::~ElementaryPath3D( )
113 Kernel::VariableHandle
114 Lang::ElementaryPath3D::getField( const char * fieldID
, const RefCountPtr
< const Lang::Value
> & selfRef
) const
116 return getField( fieldID
, selfRef
.down_cast
< const Lang::ElementaryPath3D
>( ) );
119 Kernel::VariableHandle
120 Lang::ElementaryPath3D::getField( const char * fieldID
, const RefCountPtr
< const Lang::ElementaryPath3D
> & selfRef
) const
122 if( strcmp( fieldID
, "begin" ) == 0 )
124 return Helpers::newValHandle( new Lang::PathSlider3D( selfRef
, Concrete::ZERO_TIME
) );
126 if( strcmp( fieldID
, "end" ) == 0 )
128 return Helpers::newValHandle( new Lang::PathSlider3D( selfRef
, Concrete::Time( duration( ) ) ) );
130 if( strcmp( fieldID
, "closed?" ) == 0 )
134 return Kernel::THE_TRUE_VARIABLE
;
136 return Kernel::THE_FALSE_VARIABLE
;
138 if( strcmp( fieldID
, "null?" ) == 0 )
142 return Kernel::THE_TRUE_VARIABLE
;
144 return Kernel::THE_FALSE_VARIABLE
;
146 throw Exceptions::NonExistentMember( getTypeName( ), fieldID
);
149 RefCountPtr
< const Lang::ElementaryPath3D
>
150 Lang::ElementaryPath3D::elementaryTransformed( const Lang::Transform3D
& tf
) const
152 Lang::ElementaryPath3D
* res
= new Lang::ElementaryPath3D
;
157 for( const_iterator i
= begin( ); i
!= end( ); ++i
)
159 res
->push_back( (*i
)->transformed( tf
) );
161 return RefCountPtr
< const Lang::ElementaryPath3D
>( res
);
164 RefCountPtr
< const Lang::Path3D
>
165 Lang::ElementaryPath3D::typed_transformed( const Lang::Transform3D
& tf
) const
167 return elementaryTransformed( tf
);
171 Lang::ElementaryPath3D::elementaryJob( std::stack
< const Lang::Path3D
* > * nodeStack
, Lang::ElementaryPath3D
* pth
, Concrete::Coords3D
* basePoint
) const
173 for( const_iterator i
= begin( ); i
!= end( ); ++i
)
175 pth
->push_back( new Concrete::PathPoint3D( **i
) );
179 *basePoint
= *(back( )->mid_
);
184 Lang::ElementaryPath3D::timeCheck( Concrete::Time t
) const
188 if( isinfPhysical( t
) )
190 throw Exceptions::OutOfRange( "The time argument must not be infinite for closed_ paths." );
192 Concrete::Time tMax
= Concrete::Time( duration( ) );
193 t
= modPhysical( t
, tMax
);
203 throw Exceptions::OutOfRange( "The time argument must be non-negative for open paths." );
207 t
= Concrete::Time( duration( ) );
209 else if( t
> Concrete::Time( duration( ) ) )
211 throw Exceptions::OutOfRange( "The time argument must not be greater than the duration of the path." );
218 Lang::ElementaryPath3D::findSegment( Concrete::Time t
, Concrete::Time
* tRes
, const Concrete::PathPoint3D
** p1
, const Concrete::PathPoint3D
** p2
) const
221 const_iterator i
= begin( );
222 for( ; t
>= Concrete::UNIT_TIME
; t
-= Concrete::UNIT_TIME
, ++i
)
240 Lang::ElementaryPath3D::findSegmentReverse( Concrete::Time t
, Concrete::Time
* tRes
, const Concrete::PathPoint3D
** p1
, const Concrete::PathPoint3D
** p2
) const
244 const_iterator i
= begin( );
245 for( ; t
> 0; t
-= Concrete::UNIT_TIME
, ++i
)
261 RefCountPtr
< const Lang::Coords3D
>
262 Lang::ElementaryPath3D::point( Concrete::Time globalTime
) const
266 throw Exceptions::OutOfRange( "The empty path has no points at all." );
269 const Concrete::PathPoint3D
* p1
;
270 const Concrete::PathPoint3D
* p2
;
271 findSegment( globalTime
, &t
, &p1
, &p2
);
275 return RefCountPtr
< const Lang::Coords3D
>( new Lang::Coords3D( * p1
->mid_
) );
279 return RefCountPtr
< const Lang::Coords3D
>( new Lang::Coords3D( * p2
->mid_
) );
282 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
283 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
284 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
285 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
286 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
287 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
288 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
289 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
290 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
291 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
292 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
293 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
295 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
296 Physical
< 0, 3 > k0
= tc
* tc
* tc
;
297 Physical
< 0, 3 > k1
= 3 * tc
* tc
* t
;
298 Physical
< 0, 3 > k2
= 3 * tc
* t
* t
;
299 Physical
< 0, 3 > k3
= t
* t
* t
;
300 Concrete::Length x
= x0
* k0
+ x1
* k1
+ x2
* k2
+ x3
* k3
;
301 Concrete::Length y
= y0
* k0
+ y1
* k1
+ y2
* k2
+ y3
* k3
;
302 Concrete::Length z
= z0
* k0
+ z1
* k1
+ z2
* k2
+ z3
* k3
;
304 return RefCountPtr
< const Lang::Coords3D
>( new Lang::Coords3D( x
, y
, z
) );
307 RefCountPtr
< const Lang::Length
>
308 Lang::ElementaryPath3D::speed( Concrete::Time globalTime
) const
312 throw Exceptions::OutOfRange( "The empty path has no speed at all." );
315 const Concrete::PathPoint3D
* p1
;
316 const Concrete::PathPoint3D
* p2
;
317 findSegment( globalTime
, &t
, &p1
, &p2
);
319 if( t
<= Concrete::ZERO_TIME
)
321 /* here, t = 0, tc == 1, so
323 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
324 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
325 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
326 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
327 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
328 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
329 // Concrete::Speed vx = x0 * (-3) + x1 * 3;
330 // Concrete::Speed vy = y0 * (-3) + y1 * 3;
331 // Concrete::Speed vz = z0 * (-3) + z1 * 3;
332 Concrete::Speed vx
= ( x1
- x0
).offtype
< 0, -2 >( );
333 Concrete::Speed vy
= ( y1
- y0
).offtype
< 0, -2 >( );
334 Concrete::Speed vz
= ( z1
- z0
).offtype
< 0, -2 >( );
335 return RefCountPtr
< const Lang::Length
>( new Lang::Length( hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( ) ) );
337 if( t
>= Concrete::UNIT_TIME
)
339 throw Exceptions::InternalError( "t1 == 1 in speed calculation" );
342 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
343 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
344 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
345 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
346 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
347 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
348 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
349 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
350 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
351 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
352 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
353 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
355 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
356 // kv0 = -3 * tc * tc;
357 // kv1 = 3 * tc * tc - 6 * tc * t;
358 // kv2 = 6 * tc * t - 3 * t * t;
360 Physical
< 0, 2 > kv0
= - tc
* tc
;
361 Physical
< 0, 2 > kv1
= tc
* tc
- 2 * tc
* t
;
362 Physical
< 0, 2 > kv2
= 2 * tc
* t
- t
* t
;
363 Physical
< 0, 2 > kv3
= t
* t
;
364 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
365 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
366 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
368 return RefCountPtr
< const Lang::Length
>( new Lang::Length( hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( ) ) );
371 RefCountPtr
< const Lang::Length
>
372 Lang::ElementaryPath3D::reverse_speed( Concrete::Time globalTime
) const
376 throw Exceptions::OutOfRange( "The empty path has no speed at all." );
379 const Concrete::PathPoint3D
* p1
;
380 const Concrete::PathPoint3D
* p2
;
381 findSegmentReverse( globalTime
, &t
, &p1
, &p2
);
383 if( t
<= Concrete::ZERO_TIME
)
385 throw Exceptions::InternalError( "t1 == 0 in reverse speed calculation" );
387 if( t
>= Concrete::UNIT_TIME
)
389 /* here, t = 1, tc == 0, so
391 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
392 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
393 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
394 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
395 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
396 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
397 // Concrete::Speed vx = x2 * (-3) + x3 * 3;
398 // Concrete::Speed vy = y2 * (-3) + y3 * 3;
399 // Concrete::Speed vz = z2 * (-3) + z3 * 3;
400 Concrete::Speed vx
= ( x3
- x2
).offtype
< 0, -2 >( );
401 Concrete::Speed vy
= ( y3
- y2
).offtype
< 0, -2 >( );
402 Concrete::Speed vz
= ( z3
- z2
).offtype
< 0, -2 >( );
403 return RefCountPtr
< const Lang::Length
>( new Lang::Length( hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( ) ) );
406 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
407 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
408 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
409 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
410 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
411 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
412 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
413 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
414 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
415 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
416 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
417 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
419 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
420 // kv0 = -3 * tc * tc;
421 // kv1 = 3 * tc * tc - 6 * tc * t;
422 // kv2 = 6 * tc * t - 3 * t * t;
424 Physical
< 0, 2 > kv0
= - tc
* tc
;
425 Physical
< 0, 2 > kv1
= tc
* tc
- 2 * tc
* t
;
426 Physical
< 0, 2 > kv2
= 2 * tc
* t
- t
* t
;
427 Physical
< 0, 2 > kv3
= t
* t
;
428 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
429 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
430 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
432 return RefCountPtr
< const Lang::Length
>( new Lang::Length( hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( ) ) );
435 RefCountPtr
< const Lang::FloatTriple
>
436 Lang::ElementaryPath3D::direction( Concrete::Time globalTime
) const
440 throw Exceptions::OutOfRange( "The empty path has no directions at all." );
443 const Concrete::PathPoint3D
* p1
;
444 const Concrete::PathPoint3D
* p2
;
445 findSegment( globalTime
, &t
, &p1
, &p2
);
447 Bezier::ControlPoints
< Concrete::Coords3D
> controls( *(p1
->mid_
), *(p1
->front_
), *(p2
->rear_
), *(p2
->mid_
) );
448 Bezier::PolyCoeffs
< Concrete::Coords3D
> coeffs( controls
);
450 const Concrete::Length segLengthBound
=
451 ( controls
.p1_
- controls
.p0_
).norm( ) + ( controls
.p2_
- controls
.p1_
).norm( ) + ( controls
.p3_
- controls
.p2_
).norm( );
452 const Concrete::Length shortLength
= 1.e
-4 * segLengthBound
;
455 Concrete::Coords3D d
= coeffs
.velocity( t
.offtype
< 0, 1 >( ) );
456 Concrete::Length v
= d
.norm( );
457 if( v
> shortLength
)
459 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( d
.direction( v
) ) );
463 /* We reach here if the velocity more or less was vanishing at t. If we are at the beginning of a segment, the acceleration
464 * will be in the direction of the limiting velocity. If we are at the end, the acceleration will be in the opposite direction
465 * of the limiting velocity.
469 Concrete::Coords3D d
= ( t
< 0.5 ? 1 : -1 ) * coeffs
.acceleration( t
.offtype
< 0, 1 >( ) );
470 Concrete::Length v
= d
.norm( );
471 if( v
> shortLength
)
473 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( d
.direction( v
) ) );
476 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( 0, 0, 0 ) );
479 RefCountPtr
< const Lang::FloatTriple
>
480 Lang::ElementaryPath3D::reverse_direction( Concrete::Time globalTime
) const
484 throw Exceptions::OutOfRange( "The empty path has no directions at all." );
487 const Concrete::PathPoint3D
* p1
;
488 const Concrete::PathPoint3D
* p2
;
489 findSegmentReverse( globalTime
, &t
, &p1
, &p2
);
491 Bezier::ControlPoints
< Concrete::Coords3D
> controls( *(p1
->mid_
), *(p1
->front_
), *(p2
->rear_
), *(p2
->mid_
) );
492 Bezier::PolyCoeffs
< Concrete::Coords3D
> coeffs( controls
);
494 const Concrete::Length segLengthBound
=
495 ( controls
.p1_
- controls
.p0_
).norm( ) + ( controls
.p2_
- controls
.p1_
).norm( ) + ( controls
.p3_
- controls
.p2_
).norm( );
496 const Concrete::Length shortLength
= 1.e
-4 * segLengthBound
;
499 Concrete::Coords3D d
= (-1) * coeffs
.velocity( t
.offtype
< 0, 1 >( ) );
500 Concrete::Length v
= d
.norm( );
501 if( v
> shortLength
)
503 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( d
.direction( v
) ) );
507 /* We reach here if the velocity more or less was vanishing at t. If we are at the beginning of a segment, the acceleration
508 * will be in the direction of the limiting velocity. If we are at the end, the acceleration will be in the opposite direction
509 * of the limiting velocity.
513 Concrete::Coords3D d
= ( t
< 0.5 ? -1 : 1 ) * coeffs
.acceleration( t
.offtype
< 0, 1 >( ) );
514 Concrete::Length v
= d
.norm( );
515 if( v
> shortLength
)
517 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( d
.direction( v
) ) );
520 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( 0, 0, 0 ) );
523 RefCountPtr
< const Lang::FloatTriple
>
524 Lang::ElementaryPath3D::normal( Concrete::Time globalTime
) const
528 throw Exceptions::OutOfRange( "The empty path has no normals at all." );
531 const Concrete::PathPoint3D
* p1
;
532 const Concrete::PathPoint3D
* p2
;
533 findSegment( globalTime
, &t
, &p1
, &p2
);
535 if( p1
->front_
== p1
->mid_
&& p2
->rear_
== p2
->mid_
)
537 throw Exceptions::OutOfRange( "The normal is undefined along straigt segments." );
540 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
541 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
542 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
543 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
544 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
545 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
546 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
547 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
548 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
549 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
550 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
551 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
553 if( t
<= Concrete::ZERO_TIME
)
555 /* here, t = 0, tc == 1, so
557 Concrete::Speed vx
= ( x0
* (-3) + x1
* 3 ).offtype
< 0, -2 >( );
558 Concrete::Speed vy
= ( y0
* (-3) + y1
* 3 ).offtype
< 0, -2 >( );
559 Concrete::Speed vz
= ( z0
* (-3) + z1
* 3 ).offtype
< 0, -2 >( );
560 Concrete::Acceleration ax
= ( x0
* 6 + x1
* (-12) + x2
* 6 ).offtype
< 0, -1 >( );
561 Concrete::Acceleration ay
= ( y0
* 6 + y1
* (-12) + y2
* 6 ).offtype
< 0, -1 >( );
562 Concrete::Acceleration az
= ( z0
* 6 + z1
* (-12) + z2
* 6 ).offtype
< 0, -1 >( );
564 Concrete::Acceleration tmpx
= ax
;
565 Concrete::Acceleration tmpy
= ay
;
566 Concrete::Acceleration tmpz
= az
;
567 if( tmpx
== 0 && tmpy
== 0 && tmpz
== 0 )
569 // We divide by t and let t approach zero.
570 // To see what the limit becomes, the easiest way is to express the position
571 // as a power series in t, differentiate twice, note what is vanishing, and
572 // then divide by t...
573 Physical
< 0, 1 > ka0
= -6;
574 Physical
< 0, 1 > ka1
= 18;
575 Physical
< 0, 1 > ka2
= -18;
576 Physical
< 0, 1 > ka3
= 6;
577 tmpx
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
578 tmpy
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
579 tmpz
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
581 if( tmpx
== 0 && tmpy
== 0 && tmpz
== 0 )
583 throw Exceptions::OutOfRange( "The normal is undefined becaue the acceleration vanishes smoothly." );
585 Physical
< 2, -2 > v2
= squareSum( vx
, vy
, vz
);
586 if( v2
> Physical
< 2, -2 >( 0 ) )
588 Physical
< 0, -1 > f
= ( ax
* vx
+ ay
* vy
+ az
* vz
) / v2
;
594 Concrete::Acceleration tmpNorm
= hypotPhysical( tmpx
, tmpy
, tmpz
);
597 throw Exceptions::OutOfRange( "The normal is undefined where the acceleration is parallel to velocity." );
599 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( tmpx
/ tmpNorm
, tmpy
/ tmpNorm
, tmpz
/ tmpNorm
) );
601 if( t
>= Concrete::UNIT_TIME
)
603 throw Exceptions::InternalError( "t1 == 1 in normal calculation" );
606 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
607 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
608 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
609 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
610 Physical
< 0, 2 > kv3
= 3 * t
* t
;
611 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
612 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
613 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
614 Physical
< 0, 1 > ka0
= 6 * tc
;
615 Physical
< 0, 1 > ka1
= -12 * tc
+ 6 * t
;
616 Physical
< 0, 1 > ka2
= 6 * tc
- 12 * t
;
617 Physical
< 0, 1 > ka3
= 6 * t
;
618 Concrete::Acceleration ax
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
619 Concrete::Acceleration ay
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
620 Concrete::Acceleration az
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
622 Concrete::Acceleration tmpx
= ax
;
623 Concrete::Acceleration tmpy
= ay
;
624 Concrete::Acceleration tmpz
= az
;
625 Physical
< 2, -2 > v2
= squareSum( vx
, vy
, vz
);
626 if( v2
> Physical
< 2, -2 >( 0 ) )
628 Physical
< 0, -1 > f
= ( ax
* vx
+ ay
* vy
+ az
* vz
) / v2
;
634 Physical
< 1, -2 > tmpNorm
= hypotPhysical( tmpx
, tmpy
, tmpz
);
635 if( tmpNorm
== Physical
< 1, -2 >( 0 ) )
637 throw Exceptions::OutOfRange( "The normal is undefined where the acceleration is parallel to velocity." );
639 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( tmpx
/ tmpNorm
, tmpy
/ tmpNorm
, tmpz
/ tmpNorm
) );
642 RefCountPtr
< const Lang::FloatTriple
>
643 Lang::ElementaryPath3D::reverse_normal( Concrete::Time globalTime
) const
647 throw Exceptions::OutOfRange( "The empty path has no normals at all." );
650 const Concrete::PathPoint3D
* p1
;
651 const Concrete::PathPoint3D
* p2
;
652 findSegmentReverse( globalTime
, &t
, &p1
, &p2
);
654 if( p1
->front_
== p1
->mid_
&& p2
->rear_
== p2
->mid_
)
656 throw Exceptions::OutOfRange( "The normal is undefined along straigt segments." );
659 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
660 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
661 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
662 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
663 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
664 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
665 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
666 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
667 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
668 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
669 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
670 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
672 if( t
>= Concrete::UNIT_TIME
)
674 /* here, t = 1, tc == 0, so
676 Concrete::Speed vx
= ( x2
* (-3) + x3
* 3 ).offtype
< 0, -2 >( );
677 Concrete::Speed vy
= ( y2
* (-3) + y3
* 3 ).offtype
< 0, -2 >( );
678 Concrete::Speed vz
= ( z2
* (-3) + z3
* 3 ).offtype
< 0, -2 >( );
679 Concrete::Acceleration ax
= ( x1
* 6 + x2
* (-12) + x3
* 6 ).offtype
< 0, -1 >( );
680 Concrete::Acceleration ay
= ( y1
* 6 + y2
* (-12) + y3
* 6 ).offtype
< 0, -1 >( );
681 Concrete::Acceleration az
= ( z1
* 6 + z2
* (-12) + z3
* 6 ).offtype
< 0, -1 >( );
683 Concrete::Acceleration tmpx
= ax
;
684 Concrete::Acceleration tmpy
= ay
;
685 Concrete::Acceleration tmpz
= az
;
686 if( tmpx
== 0 && tmpy
== 0 && tmpz
== 0 )
688 // We divide by (1-t) and let t approach one.
689 // The coefficients are derived via symmetry arguments this time; compare the non-reverse calculation.
690 Physical
< 0, 1 > ka0
= 6;
691 Physical
< 0, 1 > ka1
= -18;
692 Physical
< 0, 1 > ka2
= 18;
693 Physical
< 0, 1 > ka3
= -6;
694 tmpx
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
695 tmpy
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
696 tmpz
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
698 if( tmpx
== 0 && tmpy
== 0 && tmpz
== 0 )
700 throw Exceptions::OutOfRange( "The normal is undefined becaue the acceleration vanishes smoothly." );
702 Physical
< 2, -2 > v2
= squareSum( vx
, vy
, vz
);
703 if( v2
> Physical
< 2, -2 >( 0 ) )
705 Physical
< 0, -1 > f
= ( ax
* vx
+ ay
* vy
+ az
* vz
) / v2
;
711 Concrete::Acceleration tmpNorm
= hypotPhysical( tmpx
, tmpy
, tmpz
);
714 throw Exceptions::OutOfRange( "The reverse normal is undefined where the acceleration is parallel to velocity." );
716 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( tmpx
/ tmpNorm
, tmpy
/ tmpNorm
, tmpz
/ tmpNorm
) );
718 if( t
<= Concrete::ZERO_TIME
)
720 throw Exceptions::InternalError( "t1 == 0 in reverse normal calculation" );
723 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
724 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
725 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
726 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
727 Physical
< 0, 2 > kv3
= 3 * t
* t
;
728 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
729 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
730 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
731 Physical
< 0, 1 > ka0
= 6 * tc
;
732 Physical
< 0, 1 > ka1
= -12 * tc
+ 6 * t
;
733 Physical
< 0, 1 > ka2
= 6 * tc
- 12 * t
;
734 Physical
< 0, 1 > ka3
= 6 * t
;
735 Concrete::Acceleration ax
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
736 Concrete::Acceleration ay
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
737 Concrete::Acceleration az
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
739 Concrete::Acceleration tmpx
= ax
;
740 Concrete::Acceleration tmpy
= ay
;
741 Concrete::Acceleration tmpz
= az
;
742 Physical
< 2, -2 > v2
= squareSum( vx
, vy
, vz
);
743 if( v2
> Physical
< 2, -2 >( 0 ) )
745 Physical
< 0, -1 > f
= ( ax
* vx
+ ay
* vy
+ az
* vz
) / v2
;
751 Concrete::Acceleration tmpNorm
= hypotPhysical( tmpx
, tmpy
, tmpz
);
754 throw Exceptions::OutOfRange( "The reverse normal is undefined where the acceleration is parallel to velocity." );
756 return RefCountPtr
< const Lang::FloatTriple
>( new Lang::FloatTriple( tmpx
/ tmpNorm
, tmpy
/ tmpNorm
, tmpz
/ tmpNorm
) );
759 RefCountPtr
< const Lang::Length
>
760 Lang::ElementaryPath3D::radiusOfCurvature( Concrete::Time globalTime
) const
764 throw Exceptions::OutOfRange( "The empty path has no curvature at all." );
767 const Concrete::PathPoint3D
* p1
;
768 const Concrete::PathPoint3D
* p2
;
769 findSegment( globalTime
, &t
, &p1
, &p2
);
771 if( t
<= Concrete::ZERO_TIME
)
773 /* here, t = 0, tc == 1, so
775 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
776 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
777 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
778 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
779 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
780 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
781 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
782 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
783 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
784 Concrete::Speed vx
= ( x0
* (-3) + x1
* 3 ).offtype
< 0, -2 >( );
785 Concrete::Speed vy
= ( y0
* (-3) + y1
* 3 ).offtype
< 0, -2 >( );
786 Concrete::Speed vz
= ( z0
* (-3) + z1
* 3 ).offtype
< 0, -2 >( );
787 Concrete::Acceleration ax
= ( x0
* 6 + x2
* 6 ).offtype
< 0, -1 >( );
788 Concrete::Acceleration ay
= ( y0
* 6 + y2
* 6 ).offtype
< 0, -1 >( );
789 Concrete::Acceleration az
= ( z0
* 6 + z2
* 6 ).offtype
< 0, -1 >( );
791 Physical
< 2, -3 > denom
= hypotPhysical( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
794 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
796 Physical
< 2, -2 > tmp
= squareSum( vx
, vy
, vz
);
797 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
* sqrtPhysical( tmp
) / denom
) );
799 if( t
>= Concrete::UNIT_TIME
)
801 throw Exceptions::InternalError( "t1 == 1 in curvature calculation" );
804 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
805 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
806 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
807 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
808 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
809 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
810 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
811 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
812 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
813 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
814 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
815 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
817 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
818 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
819 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
820 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
821 Physical
< 0, 2 > kv3
= 3 * t
* t
;
822 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
823 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
824 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
825 Physical
< 0, 1 > ka0
= 6 * tc
;
826 Physical
< 0, 1 > ka1
= -12 * tc
+ 6 * t
;
827 Physical
< 0, 1 > ka2
= 6 * tc
- 12 * t
;
828 Physical
< 0, 1 > ka3
= 6 * t
;
829 Concrete::Acceleration ax
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
830 Concrete::Acceleration ay
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
831 Concrete::Acceleration az
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
833 Physical
< 2, -3 > denom
= hypotPhysical( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
836 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
838 Physical
< 2, -2 > tmp
= squareSum( vx
, vy
, vz
);
839 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
* sqrtPhysical( tmp
) / denom
) );
842 RefCountPtr
< const Lang::Length
>
843 Lang::ElementaryPath3D::reverse_radiusOfCurvature( Concrete::Time globalTime
) const
847 throw Exceptions::OutOfRange( "The empty path has no curvature at all." );
850 const Concrete::PathPoint3D
* p1
;
851 const Concrete::PathPoint3D
* p2
;
852 findSegmentReverse( globalTime
, &t
, &p1
, &p2
);
854 if( t
<= Concrete::ZERO_TIME
)
856 throw Exceptions::InternalError( "t1 == 0 in reverse curvature calculation" );
858 if( t
>= Concrete::UNIT_TIME
)
860 /* here, t = 1, tc == 0, so
862 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
863 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
864 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
865 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
866 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
867 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
868 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
869 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
870 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
872 Concrete::Speed vx
= ( x2
* (-3) + x3
* 3 ).offtype
< 0, -2 >( );
873 Concrete::Speed vy
= ( y2
* (-3) + y3
* 3 ).offtype
< 0, -2 >( );
874 Concrete::Speed vz
= ( z2
* (-3) + z3
* 3 ).offtype
< 0, -2 >( );
875 Concrete::Acceleration ax
= ( x1
* 6 + x2
* (-12) + x3
* 6 ).offtype
< 0, -1 >( );
876 Concrete::Acceleration ay
= ( y1
* 6 + y2
* (-12) + y3
* 6 ).offtype
< 0, -1 >( );
877 Concrete::Acceleration az
= ( z1
* 6 + z2
* (-12) + z3
* 6 ).offtype
< 0, -1 >( );
879 Physical
< 2, -3 > denom
= hypotPhysical( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
882 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
884 Physical
< 2, -2 > tmp
= squareSum( vx
, vy
, vz
);
885 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
* sqrtPhysical( tmp
) / denom
) );
888 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
889 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
890 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
891 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
892 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
893 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
894 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
895 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
896 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
897 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
898 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
899 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
901 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
902 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
903 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
904 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
905 Physical
< 0, 2 > kv3
= 3 * t
* t
;
906 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
907 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
908 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
909 Physical
< 0, 1 > ka0
= 6 * tc
;
910 Physical
< 0, 1 > ka1
= -12 * tc
+ 6 * t
;
911 Physical
< 0, 1 > ka2
= 6 * tc
- 12 * t
;
912 Physical
< 0, 1 > ka3
= 6 * t
;
913 Concrete::Acceleration ax
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
914 Concrete::Acceleration ay
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
915 Concrete::Acceleration az
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
917 Physical
< 2, -3 > denom
= hypotPhysical( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
920 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
922 Physical
< 2, -2 > tmp
= squareSum( vx
, vy
, vz
);
923 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
* sqrtPhysical( tmp
) / denom
) );
926 RefCountPtr
< const Lang::Length
>
927 Lang::ElementaryPath3D::radiusOfTorsion( Concrete::Time globalTime
) const
931 throw Exceptions::OutOfRange( "The empty path has no torsion at all." );
934 const Concrete::PathPoint3D
* p1
;
935 const Concrete::PathPoint3D
* p2
;
936 findSegment( globalTime
, &t
, &p1
, &p2
);
938 if( t
<= Concrete::ZERO_TIME
)
940 /* here, t = 0, tc == 1, so
942 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
943 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
944 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
945 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
946 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
947 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
948 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
949 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
950 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
951 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
952 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
953 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
955 Concrete::Speed vx
= ( x0
* (-3) + x1
* 3 ).offtype
< 0, -2 >( );
956 Concrete::Speed vy
= ( y0
* (-3) + y1
* 3 ).offtype
< 0, -2 >( );
957 Concrete::Speed vz
= ( z0
* (-3) + z1
* 3 ).offtype
< 0, -2 >( );
958 Concrete::Acceleration ax
= ( x0
* 6 + x2
* 6 ).offtype
< 0, -1 >( );
959 Concrete::Acceleration ay
= ( y0
* 6 + y2
* 6 ).offtype
< 0, -1 >( );
960 Concrete::Acceleration az
= ( z0
* 6 + z2
* 6 ).offtype
< 0, -1 >( );
961 Concrete::Jerk jx
= x0
* (-6) + x1
* 18 + x2
* (-18) + x3
* 6;
962 Concrete::Jerk jy
= y0
* (-6) + y1
* 18 + y2
* (-18) + y3
* 6;
963 Concrete::Jerk jz
= z0
* (-6) + z1
* 18 + z2
* (-18) + z3
* 6;
965 Physical
< 3, -6 > denom
= tripleInner( vx
, vy
, vz
, ax
, ay
, az
, jx
, jy
, jz
);
968 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
970 Physical
< 4, -6 > tmp
= squareSum( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
971 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
/ denom
) );
973 if( t
>= Concrete::UNIT_TIME
)
975 throw Exceptions::InternalError( "t1 == 1 in torsion calculation" );
978 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
979 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
980 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
981 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
982 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
983 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
984 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
985 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
986 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
987 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
988 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
989 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
991 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
992 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
993 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
994 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
995 Physical
< 0, 2 > kv3
= 3 * t
* t
;
996 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
997 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
998 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
999 Physical
< 0, 1 > ka0
= 6 * tc
;
1000 Physical
< 0, 1 > ka1
= -12 * tc
+ 6 * t
;
1001 Physical
< 0, 1 > ka2
= 6 * tc
- 12 * t
;
1002 Physical
< 0, 1 > ka3
= 6 * t
;
1003 Concrete::Acceleration ax
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
1004 Concrete::Acceleration ay
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
1005 Concrete::Acceleration az
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
1006 Physical
< 0, 0 > kj0
= -6;
1007 Physical
< 0, 0 > kj1
= 18;
1008 Physical
< 0, 0 > kj2
= -18;
1009 Physical
< 0, 0 > kj3
= 6;
1010 Concrete::Jerk jx
= x0
* kj0
+ x1
* kj1
+ x2
* kj2
+ x3
* kj3
;
1011 Concrete::Jerk jy
= y0
* kj0
+ y1
* kj1
+ y2
* kj2
+ y3
* kj3
;
1012 Concrete::Jerk jz
= z0
* kj0
+ z1
* kj1
+ z2
* kj2
+ z3
* kj3
;
1014 Physical
< 3, -6 > denom
= tripleInner( vx
, vy
, vz
, ax
, ay
, az
, jx
, jy
, jz
);
1017 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
1019 Physical
< 4, -6 > tmp
= squareSum( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
1020 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
/ denom
) );
1023 RefCountPtr
< const Lang::Length
>
1024 Lang::ElementaryPath3D::reverse_radiusOfTorsion( Concrete::Time globalTime
) const
1028 throw Exceptions::OutOfRange( "The empty path has no torsion at all." );
1031 const Concrete::PathPoint3D
* p1
;
1032 const Concrete::PathPoint3D
* p2
;
1033 findSegmentReverse( globalTime
, &t
, &p1
, &p2
);
1035 if( t
<= Concrete::ZERO_TIME
)
1037 throw Exceptions::InternalError( "t1 == 0 in reverse torsion calculation" );
1039 if( t
>= Concrete::UNIT_TIME
)
1041 /* here, t = 1, tc == 0, so
1043 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
1044 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
1045 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
1046 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
1047 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
1048 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
1049 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
1050 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
1051 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
1052 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
1053 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
1054 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
1056 Concrete::Speed vx
= ( x2
* (-3) + x3
* 3 ).offtype
< 0, -2 >( );
1057 Concrete::Speed vy
= ( y2
* (-3) + y3
* 3 ).offtype
< 0, -2 >( );
1058 Concrete::Speed vz
= ( z2
* (-3) + z3
* 3 ).offtype
< 0, -2 >( );
1059 Concrete::Acceleration ax
= ( x1
* 6 + x2
* (-12) + x3
* 6 ).offtype
< 0, -1 >( );
1060 Concrete::Acceleration ay
= ( y1
* 6 + y2
* (-12) + y3
* 6 ).offtype
< 0, -1 >( );
1061 Concrete::Acceleration az
= ( z1
* 6 + z2
* (-12) + z3
* 6 ).offtype
< 0, -1 >( );
1062 Concrete::Jerk jx
= x0
* (-6) + x1
* 18 + x2
* (-18) + x3
* 6;
1063 Concrete::Jerk jy
= y0
* (-6) + y1
* 18 + y2
* (-18) + y3
* 6;
1064 Concrete::Jerk jz
= z0
* (-6) + z1
* 18 + z2
* (-18) + z3
* 6;
1066 Physical
< 3, -6 > denom
= tripleInner( vx
, vy
, vz
, ax
, ay
, az
, jx
, jy
, jz
);
1069 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
1071 Physical
< 4, -6 > tmp
= squareSum( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
1072 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
/ denom
) );
1075 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
1076 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
1077 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
1078 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
1079 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
1080 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
1081 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
1082 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
1083 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
1084 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
1085 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
1086 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
1088 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
1089 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
1090 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
1091 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
1092 Physical
< 0, 2 > kv3
= 3 * t
* t
;
1093 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
1094 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
1095 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
1096 Physical
< 0, 1 > ka0
= 6 * tc
;
1097 Physical
< 0, 1 > ka1
= -12 * tc
+ 6 * t
;
1098 Physical
< 0, 1 > ka2
= 6 * tc
- 12 * t
;
1099 Physical
< 0, 1 > ka3
= 6 * t
;
1100 Concrete::Acceleration ax
= x0
* ka0
+ x1
* ka1
+ x2
* ka2
+ x3
* ka3
;
1101 Concrete::Acceleration ay
= y0
* ka0
+ y1
* ka1
+ y2
* ka2
+ y3
* ka3
;
1102 Concrete::Acceleration az
= z0
* ka0
+ z1
* ka1
+ z2
* ka2
+ z3
* ka3
;
1103 Physical
< 0, 0 > kj0
= -6;
1104 Physical
< 0, 0 > kj1
= 18;
1105 Physical
< 0, 0 > kj2
= -18;
1106 Physical
< 0, 0 > kj3
= 6;
1107 Concrete::Jerk jx
= x0
* kj0
+ x1
* kj1
+ x2
* kj2
+ x3
* kj3
;
1108 Concrete::Jerk jy
= y0
* kj0
+ y1
* kj1
+ y2
* kj2
+ y3
* kj3
;
1109 Concrete::Jerk jz
= z0
* kj0
+ z1
* kj1
+ z2
* kj2
+ z3
* kj3
;
1111 Physical
< 3, -6 > denom
= tripleInner( vx
, vy
, vz
, ax
, ay
, az
, jx
, jy
, jz
);
1114 return RefCountPtr
< const Lang::Length
>( new Lang::Length( HUGE_VAL
) );
1116 Physical
< 4, -6 > tmp
= squareSum( vy
* az
- vz
* ay
, vz
* ax
- vx
* az
, vx
* ay
- vy
* ax
);
1117 return RefCountPtr
< const Lang::Length
>( new Lang::Length( tmp
/ denom
) );
1121 Lang::ElementaryPath3D::duration( ) const
1131 Lang::ElementaryPath3D::controllingMaximizer( const Lang::FloatTriple
& d
, Lang::Coords3D
* dst
) const
1135 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1137 Concrete::Length opt
= -HUGE_VAL
;
1138 for( const_iterator i
= begin( ); i
!= end( ); ++i
)
1140 if( (*i
)->rear_
!= 0 )
1142 Concrete::Length y
= (*i
)->rear_
->x_
* d
.x_
+ (*i
)->rear_
->y_
* d
.y_
+ (*i
)->rear_
->z_
* d
.z_
;
1146 *dst
= *((*i
)->rear_
);
1149 Concrete::Length y
= (*i
)->mid_
->x_
* d
.x_
+ (*i
)->mid_
->y_
* d
.y_
+ (*i
)->mid_
->z_
* d
.z_
;
1153 *dst
= *((*i
)->mid_
);
1155 if( (*i
)->front_
!= 0 )
1157 Concrete::Length y
= (*i
)->front_
->x_
* d
.x_
+ (*i
)->front_
->y_
* d
.y_
+ (*i
)->front_
->z_
* d
.z_
;
1161 *dst
= *((*i
)->front_
);
1165 if( opt
== -HUGE_VAL
)
1173 RefCountPtr
< const Lang::Coords3D
>
1174 Lang::ElementaryPath3D::discreteMean( ) const
1177 Concrete::Length x
= 0;
1178 Concrete::Length y
= 0;
1179 Concrete::Length z
= 0;
1182 throw Exceptions::OutOfRange( "The empty path cannot be averaged along." );
1184 for( const_iterator i
= begin( ); i
!= end( ); ++i
)
1186 if( (*i
)->rear_
!= 0 )
1189 x
= x
+ (*i
)->rear_
->x_
;
1190 y
= y
+ (*i
)->rear_
->y_
;
1191 z
= z
+ (*i
)->rear_
->z_
;
1194 x
= x
+ (*i
)->mid_
->x_
;
1195 y
= y
+ (*i
)->mid_
->y_
;
1196 z
= z
+ (*i
)->mid_
->z_
;
1197 if( (*i
)->front_
!= 0 )
1200 x
= x
+ (*i
)->front_
->x_
;
1201 y
= y
+ (*i
)->front_
->y_
;
1202 z
= z
+ (*i
)->front_
->z_
;
1205 return RefCountPtr
< const Lang::Coords3D
>( new Lang::Coords3D( x
/ count
, y
/ count
, z
/ count
) );
1208 RefCountPtr
< const Lang::Coords3D
>
1209 Lang::ElementaryPath3D::controllingMaximizer( const Lang::FloatTriple
& d
) const
1211 Lang::Coords3D
* res
= new Lang::Coords3D( Concrete::ZERO_LENGTH
, Concrete::ZERO_LENGTH
, Concrete::ZERO_LENGTH
);
1212 controllingMaximizer( d
, res
);
1213 return RefCountPtr
< const Lang::Coords3D
>( res
);
1216 Concrete::SplineTime
1217 Lang::ElementaryPath3D::discreteMaximizer( const Lang::FloatTriple
& d
) const
1221 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1223 Concrete::Length opt
= -HUGE_VAL
;
1224 const double dx
= d
.x_
;
1225 const double dy
= d
.y_
;
1226 const double dz
= d
.z_
;
1227 Concrete::SplineTime res
= Concrete::ZERO_TIME
;
1228 Concrete::SplineTime t
= Concrete::ZERO_TIME
;
1229 for( const_iterator i
= begin( ); i
!= end( ); ++i
, ++t
)
1231 Concrete::Length y
= (*i
)->mid_
->x_
* dx
+ (*i
)->mid_
->y_
* dy
+ (*i
)->mid_
->z_
* dz
;
1241 Concrete::SplineTime
1242 Lang::ElementaryPath3D::discreteApproximator( const Lang::Coords3D
& p
) const
1246 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1248 Concrete::Length bestDist
= HUGE_VAL
;
1249 const Concrete::Length px
= p
.x_
.get( );
1250 const Concrete::Length py
= p
.y_
.get( );
1251 const Concrete::Length pz
= p
.z_
.get( );
1252 Concrete::SplineTime res
= Concrete::ZERO_TIME
;
1253 Concrete::SplineTime t
= Concrete::ZERO_TIME
;
1254 for( const_iterator i
= begin( ); i
!= end( ); ++i
, ++t
)
1256 Concrete::Length dist
= hypotPhysical( (*i
)->mid_
->x_
- px
, (*i
)->mid_
->y_
- py
, (*i
)->mid_
->z_
- pz
);
1257 if( dist
< bestDist
)
1266 RefCountPtr
< const Lang::Coords3D
>
1267 Lang::ElementaryPath3D::continuousMean( ) const
1271 throw Exceptions::OutOfRange( "The empty path cannot be averaged along." );
1273 Physical
< 2, 0 > intx
= 0;
1274 Physical
< 2, 0 > inty
= 0;
1275 Physical
< 2, 0 > intz
= 0;
1276 Physical
< 1, 0 > totlen
= 0;
1278 const_iterator i1
= begin( );
1279 const_iterator i2
= i1
;
1281 for( ; i1
!= end( ); ++i1
, ++i2
)
1295 if( (*i1
)->front_
== (*i1
)->mid_
&&
1296 (*i2
)->rear_
== (*i2
)->mid_
)
1298 Concrete::Length x0
= (*i1
)->mid_
->x_
;
1299 Concrete::Length y0
= (*i1
)->mid_
->y_
;
1300 Concrete::Length z0
= (*i1
)->mid_
->z_
;
1301 Concrete::Length x3
= (*i2
)->mid_
->x_
;
1302 Concrete::Length y3
= (*i2
)->mid_
->y_
;
1303 Concrete::Length z3
= (*i2
)->mid_
->z_
;
1304 Concrete::Length len
= hypotPhysical( (x3
-x0
), (y3
-y0
), (z3
-z0
) );
1305 intx
+= len
* 0.5 * ( x0
+ x3
);
1306 inty
+= len
* 0.5 * ( y0
+ y3
);
1307 intz
+= len
* 0.5 * ( z0
+ z3
);
1312 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
1313 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
1314 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
1315 Concrete::Bezier x1
= (*i1
)->front_
->x_
.offtype
< 0, 3 >( );
1316 Concrete::Bezier y1
= (*i1
)->front_
->y_
.offtype
< 0, 3 >( );
1317 Concrete::Bezier z1
= (*i1
)->front_
->z_
.offtype
< 0, 3 >( );
1318 Concrete::Bezier x2
= (*i2
)->rear_
->x_
.offtype
< 0, 3 >( );
1319 Concrete::Bezier y2
= (*i2
)->rear_
->y_
.offtype
< 0, 3 >( );
1320 Concrete::Bezier z2
= (*i2
)->rear_
->z_
.offtype
< 0, 3 >( );
1321 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
1322 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
1323 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
1325 Concrete::Time dt
= Shapes::computeDt( hypotPhysical( (x1
-x0
), (y1
-y0
), (z1
-z0
) ).offtype
< 0, -3 >( ) +
1326 hypotPhysical( (x2
-x1
), (y2
-y1
), (z2
-z1
) ).offtype
< 0, -3 >( ) +
1327 hypotPhysical( (x3
-x2
), (y3
-y2
), (z3
-z2
) ).offtype
< 0, -3 >( ) );
1329 Physical
< 2, 0 > tmpSum_x( 0 );
1330 Physical
< 2, 0 > tmpSum_y( 0 );
1331 Physical
< 2, 0 > tmpSum_z( 0 );
1332 Physical
< 1, 0 > tmpSum_l( 0 );
1333 for( Concrete::Time t
= 0; t
< 1; t
+= dt
)
1335 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
1336 Physical
< 0, 3 > k0
= tc
* tc
* tc
;
1337 Physical
< 0, 3 > k1
= 3 * tc
* tc
* t
;
1338 Physical
< 0, 3 > k2
= 3 * tc
* t
* t
;
1339 Physical
< 0, 3 > k3
= t
* t
* t
;
1340 Concrete::Length x
= x0
* k0
+ x1
* k1
+ x2
* k2
+ x3
* k3
;
1341 Concrete::Length y
= y0
* k0
+ y1
* k1
+ y2
* k2
+ y3
* k3
;
1342 Concrete::Length z
= z0
* k0
+ z1
* k1
+ z2
* k2
+ z3
* k3
;
1343 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
1344 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
1345 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
1346 Physical
< 0, 2 > kv3
= 3 * t
* t
;
1347 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
1348 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
1349 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
1351 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
1357 intx
+= tmpSum_x
* dt
.offtype
< 0, 1 >( );
1358 inty
+= tmpSum_y
* dt
.offtype
< 0, 1 >( );
1359 intz
+= tmpSum_z
* dt
.offtype
< 0, 1 >( );
1360 totlen
+= tmpSum_l
* dt
.offtype
< 0, 1 >( );
1363 return RefCountPtr
< const Lang::Coords3D
>( new Lang::Coords3D( intx
/ totlen
, inty
/ totlen
, intz
/ totlen
) );
1366 Concrete::SplineTime
1367 Lang::ElementaryPath3D::continuousMaximizer( const Lang::FloatTriple
& dNonUnit
) const
1371 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1374 Concrete::UnitFloatTriple
d( dNonUnit
.x_
, dNonUnit
.y_
, dNonUnit
.z_
);
1375 Concrete::Coords3D
dBezier( d
.x_
, d
.y_
, d
.z_
); // The Bezier functions requires the direction to be given with the same type as the spline space.
1377 Concrete::Time res
= 0;
1378 Concrete::Length opt
= -Concrete::HUGE_LENGTH
;
1379 Concrete::Time steps
= 0;
1380 const_iterator i1
= begin( );
1381 const_iterator i2
= i1
;
1383 for( ; i1
!= end( ); ++i1
, ++i2
, steps
+= Concrete::UNIT_TIME
)
1393 Concrete::Length v
= Concrete::inner( *(*i1
)->mid_
, d
);
1402 Concrete::Length v
= Concrete::inner( *(*i1
)->mid_
, d
);
1409 /* if the segment is straight, checking its end points is enough.
1411 if( (*i1
)->front_
== (*i1
)->mid_
&&
1412 (*i2
)->rear_
== (*i2
)->mid_
)
1417 /* First check the control points. If none of the four control points improves the optimum,
1418 * searching further is meaningless. Further, since the mid_ points are allways tested anyway,
1419 * we can assume that they already are, and conclude that they cannot improve the optimum further.
1420 * Therefore, there are only the two handles to check.
1423 if( Concrete::inner( *(*i1
)->front_
, d
) <= opt
&&
1424 Concrete::inner( *(*i2
)->rear_
, d
) <= opt
)
1430 Bezier::ControlPoints
< Concrete::Coords3D
> controls( *(*i1
)->mid_
, *(*i1
)->front_
, *(*i2
)->rear_
, *(*i2
)->mid_
);
1431 Bezier::PolyCoeffs
< Concrete::Coords3D
> coeffs( controls
);
1433 coeffs
.stationaryPoints( optTimes
, dBezier
); // A HUGE_VAL is used as terminator in the result.
1435 for( double * src
= & optTimes
[0]; *src
!= HUGE_VAL
; ++src
)
1437 Concrete::Length v
= Concrete::inner( coeffs
.point( *src
), d
);
1451 class ApproximationPoly3D
1453 Concrete::Coords3D p_
;
1454 Bezier::PolyCoeffs
< Concrete::Coords3D
> polyCoeffs_
;
1455 Physical
< 1, 0 > kxD0_0_
;
1456 Physical
< 1, -1 > kxD0_1_
;
1457 Physical
< 1, -2 > kxD0_2_
;
1458 Physical
< 1, -3 > kxD0_3_
;
1459 Physical
< 1, -1 > kxD1_0_
;
1460 Physical
< 1, -2 > kxD1_1_
;
1461 Physical
< 1, -3 > kxD1_2_
;
1462 Physical
< 1, -2 > kxD2_0_
;
1463 Physical
< 1, -3 > kxD2_1_
;
1465 Physical
< 1, 0 > kyD0_0_
;
1466 Physical
< 1, -1 > kyD0_1_
;
1467 Physical
< 1, -2 > kyD0_2_
;
1468 Physical
< 1, -3 > kyD0_3_
;
1469 Physical
< 1, -1 > kyD1_0_
;
1470 Physical
< 1, -2 > kyD1_1_
;
1471 Physical
< 1, -3 > kyD1_2_
;
1472 Physical
< 1, -2 > kyD2_0_
;
1473 Physical
< 1, -3 > kyD2_1_
;
1475 Physical
< 1, 0 > kzD0_0_
;
1476 Physical
< 1, -1 > kzD0_1_
;
1477 Physical
< 1, -2 > kzD0_2_
;
1478 Physical
< 1, -3 > kzD0_3_
;
1479 Physical
< 1, -1 > kzD1_0_
;
1480 Physical
< 1, -2 > kzD1_1_
;
1481 Physical
< 1, -3 > kzD1_2_
;
1482 Physical
< 1, -2 > kzD2_0_
;
1483 Physical
< 1, -3 > kzD2_1_
;
1486 ApproximationPoly3D( const Concrete::Coords3D
& p0
, const Concrete::Coords3D
& p1
, const Concrete::Coords3D
& p2
, const Concrete::Coords3D
& p3
, const Concrete::Coords3D
& _p
);
1487 Concrete::Time
splitTime( const Concrete::Time t_low
, const Concrete::Time t_high
, const Concrete::Time t_tol
) const;
1488 bool isCircular( ) const;
1489 Concrete::Length
distanceAt( Concrete::Time t
) const;
1490 Concrete::LengthSquared
squaredDistanceAt( Concrete::Time t
) const;
1491 Bezier::ControlPoints
< Concrete::Coords3D
> getControls( const Concrete::Time t_low
, const Concrete::Time t_high
) const;
1492 const Concrete::Coords3D
& getPoint( ) const;
1494 class ApproximationSegmentSection3D
1496 Bezier::ControlPoints
< Concrete::Coords3D
> controls_
;
1497 const ApproximationPoly3D
* baseSeg_
;
1498 Concrete::Time steps_
;
1503 ApproximationSegmentSection3D( const Shapes::ApproximationPoly3D
* baseSeg
, Concrete::Time steps
, Concrete::Time t0
, Concrete::Time t1
);
1504 ApproximationSegmentSection3D
* cutAfter( Concrete::Time t
) const;
1505 ApproximationSegmentSection3D
* cutBefore( Concrete::Time t
) const;
1507 Concrete::Length
convexHullDistance( ) const;
1508 Concrete::Time
splitTime( const Concrete::Time t_tol
) const;
1509 Concrete::Time
globalTime( Concrete::Time t
) const;
1510 Concrete::Length
distanceAt( Concrete::Time t
) const;
1511 Concrete::Speed
maxSpeed( ) const;
1512 Concrete::Time
duration( ) const;
1514 /* Argument points must be ordered so that points[0] is the point on the inside side of the plane containing the triangle.
1516 static void triangleDistance( std::vector
< const Concrete::Coords3D
* > & points
, const Concrete::Coords3D
& p
, Concrete::Length
* bestDist
);
1520 Concrete::SplineTime
1521 Lang::ElementaryPath3D::continuousApproximator( const Lang::Coords3D
& _p
) const
1523 const Concrete::Length DISTANCE_TOL
= 0.001 * Computation::the_arcdelta
;
1527 throw Exceptions::OutOfRange( "The empty path cannot be approximated along." );
1530 Concrete::Coords3D
p( _p
.x_
.get( ), _p
.y_
.get( ), _p
.z_
.get( ) );
1531 PtrOwner_back_Access
< std::list
< const Shapes::ApproximationPoly3D
* > > memory
;
1532 typedef std::pair
< Shapes::ApproximationSegmentSection3D
*, Concrete::Length
> WorkItem
;
1533 std::list
< WorkItem
> work
;
1535 Concrete::Time res
= 0;
1536 Concrete::Length bestDist
= HUGE_VAL
;
1537 Concrete::Time steps
= 0;
1538 const_iterator i1
= begin( );
1539 const_iterator i2
= i1
;
1541 for( ; i1
!= end( ); ++i1
, ++i2
, steps
+= Concrete::UNIT_TIME
)
1551 Concrete::Length dist
= hypotPhysical( (*i1
)->mid_
->x_
- p
.x_
, (*i1
)->mid_
->y_
- p
.y_
, (*i1
)->mid_
->z_
- p
.z_
);
1552 if( dist
< bestDist
)
1561 Concrete::Length dist
= hypotPhysical( (*i1
)->mid_
->x_
- p
.x_
, (*i1
)->mid_
->y_
- p
.y_
, (*i1
)->mid_
->z_
- p
.z_
);
1562 if( dist
< bestDist
)
1568 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
1569 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
1570 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
1571 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
1572 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
1573 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
1575 /* if the segment is straight, finding the closest point is a linear least-squares problem.
1577 if( (*i1
)->front_
== (*i1
)->mid_
&&
1578 (*i2
)->rear_
== (*i2
)->mid_
)
1580 const Concrete::Length tx
= ( x3
- x0
).offtype
< 0, -3 >( );
1581 const Concrete::Length ty
= ( y3
- y0
).offtype
< 0, -3 >( );
1582 const Concrete::Length tz
= ( z3
- z0
).offtype
< 0, -3 >( );
1583 double s
= ( tx
* ( p
.x_
- x0
.offtype
< 0, -3 >( ) ) + ty
* ( p
.y_
- y0
.offtype
< 0, -3 >( ) ) + tz
* ( p
.z_
- z0
.offtype
< 0, -3 >( ) ) ) / ( tx
*tx
+ ty
*ty
+ tz
*tz
);
1584 if( 0 < s
&& s
< 1 )
1586 Concrete::Length dist
= hypotPhysical( x0
.offtype
< 0, -3 >( ) + s
* tx
- p
.x_
, y0
.offtype
< 0, -3 >( ) + s
* ty
- p
.y_
, z0
.offtype
< 0, -3 >( ) + s
* tz
- p
.z_
);
1587 if( dist
< bestDist
)
1590 res
= steps
+ Shapes::straightLineArcTime( s
);
1596 Shapes::ApproximationPoly3D
* coeffs
=
1597 new Shapes::ApproximationPoly3D( *((*i1
)->mid_
), *((*i1
)->front_
), *((*i2
)->rear_
), *((*i2
)->mid_
), p
);
1598 Shapes::ApproximationSegmentSection3D
* seg
= new Shapes::ApproximationSegmentSection3D( coeffs
, steps
, 0, 1 );
1599 Concrete::Length lowerBound
= seg
->convexHullDistance( ) + DISTANCE_TOL
;
1600 if( lowerBound
>= bestDist
)
1607 /* Here, we must detect the ill-contditionned case of center-to-circle. This is detected
1608 * by checking the distance at four different points.
1611 if( coeffs
->isCircular( ) )
1613 Concrete::Length dist
= coeffs
->distanceAt( 0 );
1614 if( dist
< bestDist
)
1625 memory
.push_back( coeffs
);
1626 work
.push_back( WorkItem( seg
, lowerBound
) );
1629 while( ! work
.empty( ) )
1631 Shapes::ApproximationSegmentSection3D
* seg
= work
.front( ).first
;
1632 Concrete::Length lowerBound
= work
.front( ).second
;
1634 if( lowerBound
< bestDist
)
1636 const Concrete::Time t_tol
= Computation::the_arcdelta
/ seg
->maxSpeed( );
1637 Concrete::Time split
= seg
->splitTime( 0.001 * t_tol
); // increased precicion seems to have no influence on computation time
1638 Concrete::Length dist
= seg
->distanceAt( split
);
1639 if( dist
< bestDist
)
1642 res
= seg
->globalTime( split
);
1644 if( lowerBound
< bestDist
&&
1645 t_tol
< seg
->duration( ) )
1648 Shapes::ApproximationSegmentSection3D
* part
= seg
->cutAfter( split
);
1649 Concrete::Length lowerBound
= part
->convexHullDistance( ) + DISTANCE_TOL
;
1650 if( lowerBound
>= bestDist
)
1656 work
.push_back( WorkItem( part
, lowerBound
) );
1660 Shapes::ApproximationSegmentSection3D
* part
= seg
->cutBefore( split
);
1661 Concrete::Length lowerBound
= part
->convexHullDistance( ) + DISTANCE_TOL
;
1662 if( lowerBound
>= bestDist
)
1668 work
.push_back( WorkItem( part
, lowerBound
) );
1681 namespace Computation
1684 class SegmentPolynomialPair3D
1686 Bezier::PolyCoeffs
< Concrete::Coords3D
> polyCoeffs_a
;
1687 bool straightLineSeg_a_
;
1689 Physical
< 1, 0 > kxaD0_0
;
1690 Physical
< 1, -1 > kxaD0_1
;
1691 Physical
< 1, -2 > kxaD0_2
;
1692 Physical
< 1, -3 > kxaD0_3
;
1693 Physical
< 1, -1 > kxaD1_0
;
1694 Physical
< 1, -2 > kxaD1_1
;
1695 Physical
< 1, -3 > kxaD1_2
;
1696 Physical
< 1, -2 > kxaD2_0
;
1697 Physical
< 1, -3 > kxaD2_1
;
1699 Physical
< 1, 0 > kyaD0_0
;
1700 Physical
< 1, -1 > kyaD0_1
;
1701 Physical
< 1, -2 > kyaD0_2
;
1702 Physical
< 1, -3 > kyaD0_3
;
1703 Physical
< 1, -1 > kyaD1_0
;
1704 Physical
< 1, -2 > kyaD1_1
;
1705 Physical
< 1, -3 > kyaD1_2
;
1706 Physical
< 1, -2 > kyaD2_0
;
1707 Physical
< 1, -3 > kyaD2_1
;
1709 Physical
< 1, 0 > kzaD0_0
;
1710 Physical
< 1, -1 > kzaD0_1
;
1711 Physical
< 1, -2 > kzaD0_2
;
1712 Physical
< 1, -3 > kzaD0_3
;
1713 Physical
< 1, -1 > kzaD1_0
;
1714 Physical
< 1, -2 > kzaD1_1
;
1715 Physical
< 1, -3 > kzaD1_2
;
1716 Physical
< 1, -2 > kzaD2_0
;
1717 Physical
< 1, -3 > kzaD2_1
;
1719 Bezier::PolyCoeffs
< Concrete::Coords3D
> polyCoeffs_b
;
1720 bool straightLineSeg_b_
;
1722 Physical
< 1, 0 > kxbD0_0
;
1723 Physical
< 1, -1 > kxbD0_1
;
1724 Physical
< 1, -2 > kxbD0_2
;
1725 Physical
< 1, -3 > kxbD0_3
;
1726 Physical
< 1, -1 > kxbD1_0
;
1727 Physical
< 1, -2 > kxbD1_1
;
1728 Physical
< 1, -3 > kxbD1_2
;
1729 Physical
< 1, -2 > kxbD2_0
;
1730 Physical
< 1, -3 > kxbD2_1
;
1732 Physical
< 1, 0 > kybD0_0
;
1733 Physical
< 1, -1 > kybD0_1
;
1734 Physical
< 1, -2 > kybD0_2
;
1735 Physical
< 1, -3 > kybD0_3
;
1736 Physical
< 1, -1 > kybD1_0
;
1737 Physical
< 1, -2 > kybD1_1
;
1738 Physical
< 1, -3 > kybD1_2
;
1739 Physical
< 1, -2 > kybD2_0
;
1740 Physical
< 1, -3 > kybD2_1
;
1742 Physical
< 1, 0 > kzbD0_0
;
1743 Physical
< 1, -1 > kzbD0_1
;
1744 Physical
< 1, -2 > kzbD0_2
;
1745 Physical
< 1, -3 > kzbD0_3
;
1746 Physical
< 1, -1 > kzbD1_0
;
1747 Physical
< 1, -2 > kzbD1_1
;
1748 Physical
< 1, -3 > kzbD1_2
;
1749 Physical
< 1, -2 > kzbD2_0
;
1750 Physical
< 1, -3 > kzbD2_1
;
1752 Concrete::Time steps_a_
;
1753 Concrete::Time steps_b_
;
1755 Concrete::Speed maxSpeed_a_
;
1756 Concrete::Speed maxSpeed_b_
;
1759 SegmentPolynomialPair3D( Concrete::Time steps_a
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_a
, Concrete::Time steps_b
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_b
);
1760 SegmentPolynomialPair3D( Concrete::Time steps_a
, const Concrete::Coords3D
& seg_a_p0
, const Concrete::Coords3D
& seg_a_p3
, Concrete::Time steps_b
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_b
);
1761 SegmentPolynomialPair3D( Concrete::Time steps_a
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_a
, Concrete::Time steps_b
, const Concrete::Coords3D
& seg_b_p0
, const Concrete::Coords3D
& seg_b_p3
);
1762 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. */
1763 Concrete::Length
distanceAt( Concrete::Time ta
, Concrete::Time tb
) const;
1764 Concrete::LengthSquared
squaredDistanceAt( Concrete::Time ta
, Concrete::Time tb
) const;
1765 bool straightLineSeg_a( ) const { return straightLineSeg_a_
; }
1766 bool straightLineSeg_b( ) const { return straightLineSeg_b_
; }
1767 Concrete::Time
steps_a( ) const { return steps_a_
; }
1768 Concrete::Time
steps_b( ) const { return steps_b_
; }
1769 Concrete::Speed
maxSpeed_a( ) const { return maxSpeed_a_
; }
1770 Concrete::Speed
maxSpeed_b( ) const { return maxSpeed_b_
; }
1771 Bezier::ControlPoints
< Concrete::Coords3D
> getControls_a( const Concrete::Time t_low
, const Concrete::Time t_high
) const;
1772 Bezier::ControlPoints
< Concrete::Coords3D
> getControls_b( const Concrete::Time t_low
, const Concrete::Time t_high
) const;
1775 void kInit( ); /* Used by constructors to initialize coefficients. */
1778 class SegmentSectionPair3D
1780 Bezier::ControlPoints
< Concrete::Coords3D
> controls_a
;
1781 Bezier::ControlPoints
< Concrete::Coords3D
> controls_b
;
1782 const SegmentPolynomialPair3D
* baseSegs
;
1783 Concrete::Time t_a0
;
1784 Concrete::Time t_a1
;
1785 Concrete::Time t_b0
;
1786 Concrete::Time t_b1
;
1787 mutable Concrete::LengthSquared hull_dist_
;
1790 SegmentSectionPair3D( const SegmentPolynomialPair3D
* _baseSegs
, Concrete::Time _t_a0
, Concrete::Time _t_a1
, Concrete::Time _t_b0
, Concrete::Time _t_b1
);
1791 SegmentSectionPair3D
* cutAfterAfter( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const;
1792 SegmentSectionPair3D
* cutAfterBefore( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const;
1793 SegmentSectionPair3D
* cutBeforeAfter( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const;
1794 SegmentSectionPair3D
* cutBeforeBefore( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const;
1796 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. */
1797 Concrete::Time
steps_a( ) const { return baseSegs
->steps_a( ); };
1798 Concrete::Time
steps_b( ) const { return baseSegs
->steps_b( ); };
1799 Concrete::Time
globalTime_a( Concrete::Time t
) const;
1800 Concrete::Length
distanceAt( Concrete::Time ta
, Concrete::Time tb
) const;
1801 Concrete::LengthSquared
squaredDistanceAt( Concrete::Time ta
, Concrete::Time tb
) const;
1802 Concrete::Speed
maxSpeed_a( ) const;
1803 Concrete::Speed
maxSpeed_b( ) const;
1804 bool convexHullOverlap( ) const;
1805 bool convexHullSeparatedBy( Concrete::LengthSquared sep
) const; /* If the result is false, the actual distance between the hulls will be stored in hull_dist_. */
1806 Concrete::LengthSquared
precomputedHullDistanceSquared( ) const { return hull_dist_
; }
1807 void update_t_a1( Concrete::Time t
);
1808 bool isEmpty( ) const;
1810 void writeTimes( std::ostream
& os
) const;
1812 static bool convexHullOneWayOverlap( const std::vector
< const Concrete::Coords3D
* > & poly1
, const std::vector
< const Concrete::Coords3D
* > & poly2
);
1822 Kernel::StructureFactory
approximator3D_info_resultFactory( "dist", "other" );
1824 namespace Computation
1826 struct CurvedSegType3D
: public Bezier::ControlPoints
< Concrete::Coords3D
>
1828 Concrete::Time steps
;
1829 CurvedSegType3D( Concrete::Time _steps
, const Bezier::ControlPoints
< Concrete::Coords3D
> & ctrl
)
1830 : Bezier::ControlPoints
< Concrete::Coords3D
>( ctrl
), steps( _steps
)
1833 struct StraightSegType3D
1835 Concrete::Time steps
;
1836 Concrete::Coords3D
* first
;
1837 Concrete::Coords3D
* second
;
1839 StraightSegType3D( Concrete::Time _steps
, Concrete::Coords3D
* _first
, Concrete::Coords3D
* _second
)
1840 : steps( _steps
), first( _first
), second( _second
)
1846 Concrete::SplineTime
1847 Lang::ElementaryPath3D::continuousApproximator( const RefCountPtr
< const Lang::ElementaryPath3D
> & p2
, RefCountPtr
< const Lang::Structure
> * dstInfo
) const
1849 /* The implementation of function is analogous to the 2D case, and in order to avoid duplicate comments in the source,
1850 * we shall generally not make any comments here. Please refer to the corresponding implementation in ElementaryPath2D.
1852 * It could be worth noting, though, that paths in 3D do generally not intersect, and that some of the code below may
1853 * be overly complicated in view of this.
1856 using namespace Computation
;
1858 const Concrete::Length DISTANCE_TOL
= 0.001 * Computation::the_arcdelta
;
1859 const double CUT_LOSS
= 0.01;
1863 throw Exceptions::OutOfRange( "The empty path does not approximate anying." );
1867 throw Exceptions::OutOfRange( "The empty path cannot be the approximation target." );
1870 Concrete::Time resSteps
= Concrete::HUGE_TIME
;
1871 Concrete::Time resFrac
= 0;
1872 Concrete::Time res2
= 0;
1874 Concrete::LengthSquared
bestDist( HUGE_VAL
);
1875 const Concrete::LengthSquared
INTERSECT_DIST( 1e-10 );
1876 const Concrete::LengthSquared
ZERO_DIST( 0 );
1878 std::vector
< CurvedSegType3D
> curvedSegments
;
1879 curvedSegments
.reserve( p2
->size( ) );
1880 std::vector
< StraightSegType3D
> straightSegments
;
1881 straightSegments
.reserve( p2
->size( ) );
1883 Concrete::Time steps2
= Concrete::ZERO_TIME
;
1884 const_iterator i1
= p2
->begin( );
1885 const_iterator i2
= i1
;
1887 for( ; i1
!= p2
->end( ); ++i1
, ++i2
, steps2
+= Concrete::UNIT_TIME
)
1889 Concrete::Time steps
= Concrete::ZERO_TIME
;
1890 for( const_iterator j
= begin( ); j
!= end( ); ++j
, steps
+= Concrete::UNIT_TIME
)
1892 Concrete::LengthSquared d
= ( *((*i1
)->mid_
) - *((*j
)->mid_
) ).normSquared( );
1893 if( d
<= INTERSECT_DIST
)
1895 if( bestDist
> ZERO_DIST
||
1900 bestDist
= ZERO_DIST
;
1903 else if( d
< bestDist
)
1905 resSteps
= steps
; /* At this point, the fraction is still 0. */
1910 if( i2
== p2
->end( ) )
1912 if( p2
->isClosed( ) )
1921 if( (*i1
)->front_
== (*i1
)->mid_
&&
1922 (*i2
)->rear_
== (*i2
)->mid_
)
1924 straightSegments
.push_back( StraightSegType3D( steps2
, (*i1
)->mid_
, (*i2
)->mid_
) );
1928 curvedSegments
.push_back( CurvedSegType3D( steps2
, Bezier::ControlPoints
< Concrete::Coords3D
>( *((*i1
)->mid_
), *((*i1
)->front_
), *((*i2
)->rear_
), *((*i2
)->mid_
) ) ) );
1934 Concrete::Time steps
= Concrete::ZERO_TIME
;
1935 const_iterator i1
= begin( );
1936 const_iterator i2
= i1
;
1938 for( ; i1
!= end( ); ++i1
, ++i2
, steps
+= Concrete::UNIT_TIME
)
1952 if( (*i1
)->front_
== (*i1
)->mid_
&&
1953 (*i2
)->rear_
== (*i2
)->mid_
)
1955 /* Straight line segment. This is the only case we care about at this point.
1957 Concrete::Coords3D l0
= *((*i1
)->mid_
);
1958 Concrete::Coords3D l1
= *((*i2
)->mid_
);
1959 Concrete::Coords3D r
= l1
- l0
;
1960 Concrete::Coords3D rInv
= r
* ( 1. / Concrete::innerScalar( r
, r
) );
1962 const size_t DIM
= 3;
1963 double ga
[ N
* DIM
];
1964 double gb
[ N
* DIM
];
1969 *dstg
= Concrete::Length::offtype( l0
.x_
);
1971 *dstg
= Concrete::Length::offtype( l0
.y_
);
1973 *dstg
= Concrete::Length::offtype( l0
.z_
);
1975 *dstg
= Concrete::Length::offtype( l1
.x_
);
1977 *dstg
= Concrete::Length::offtype( l1
.y_
);
1979 *dstg
= Concrete::Length::offtype( l1
.z_
);
1981 typedef typeof straightSegments ListType
;
1982 for( ListType::const_iterator i
= straightSegments
.begin( ); i
!= straightSegments
.end( ); ++i
)
1986 *dstg
= Concrete::Length::offtype( i
->first
->x_
);
1988 *dstg
= Concrete::Length::offtype( i
->first
->y_
);
1990 *dstg
= Concrete::Length::offtype( i
->first
->z_
);
1992 *dstg
= Concrete::Length::offtype( i
->second
->x_
);
1994 *dstg
= Concrete::Length::offtype( i
->second
->y_
);
1996 *dstg
= Concrete::Length::offtype( i
->second
->z_
);
1998 double double_distSquaredLow
;
1999 double double_distSquaredHigh
;
2000 QPSolverStatus status
;
2001 Computation::polytope_distance_generator_form( DIM
,
2004 & double_distSquaredLow
, & double_distSquaredHigh
,
2005 Concrete::LengthSquared::offtype( bestDist
), -1,
2007 0.5 * Concrete::Length::offtype( Computation::theDistanceTol
), /* Lagrange multiplier tolerance. */
2011 if( ! status
.code
== Computation::QP_OK
)
2013 const char * binaryFilename
= "qp-fail-data.binary";
2014 Computation::polytope_distance_generator_form_write_binary_data( binaryFilename
,
2018 std::ostringstream msg
;
2019 msg
<< "QP solver status: " << status
.code_str( ) << ", with reason: " << status
.sub_str( )
2020 << ", binary data was written to \"" << binaryFilename
<< "\"." ;
2021 throw Exceptions::InternalError( strrefdup( msg
) );
2023 Concrete::LengthSquared
distSquaredLow( double_distSquaredLow
);
2024 Concrete::LengthSquared
distSquaredHigh( double_distSquaredHigh
);
2025 if( distSquaredHigh
<= INTERSECT_DIST
)
2027 /* We interpret this as an intersection. Save only if old optimum was not an intersection,
2028 * or this point is earlier than old optimum.
2030 Concrete::Time t
= Shapes::straightLineArcTime( Concrete::innerScalar( rInv
, Concrete::Coords3D( ya
[0], ya
[1], ya
[2] ) - l0
) );
2031 if( bestDist
> ZERO_DIST
||
2032 steps
+ t
< resSteps
+ resFrac
)
2036 Concrete::Coords3D l0b
= *(i
->first
);
2037 Concrete::Coords3D l1b
= *(i
->second
);
2038 Concrete::Coords3D rb
= l1b
- l0b
;
2039 Concrete::Coords3D rInv_b
= rb
* ( 1. / Concrete::innerScalar( rb
, rb
) );
2040 res2
= i
->steps
+ Shapes::straightLineArcTime( Concrete::innerScalar( rInv_b
, Concrete::Coords3D( yb
[0], yb
[1], yb
[2] ) - l0b
) );
2041 bestDist
= ZERO_DIST
;
2044 else if( distSquaredLow
< bestDist
)
2046 /* The optimum has been improved. Save new optimum. */
2048 resFrac
= Shapes::straightLineArcTime( Concrete::innerScalar( rInv
, Concrete::Coords3D( ya
[0], ya
[1], ya
[2] ) - l0
) );
2049 Concrete::Coords3D l0b
= *(i
->first
);
2050 Concrete::Coords3D l1b
= *(i
->second
);
2051 Concrete::Coords3D rb
= l1b
- l0b
;
2052 Concrete::Coords3D rInv_b
= rb
* ( 1. / Concrete::innerScalar( rb
, rb
) );
2053 res2
= i
->steps
+ Shapes::straightLineArcTime( Concrete::innerScalar( rInv_b
, Concrete::Coords3D( yb
[0], yb
[1], yb
[2] ) - l0b
) );
2054 bestDist
= distSquaredLow
;
2061 std::list
< SegmentSectionPair3D
* > work
;
2062 PtrOwner_back_Access
< std::list
< const SegmentPolynomialPair3D
* > > baseSegsMemory
;
2064 Concrete::Time steps
= Concrete::ZERO_TIME
;
2065 const_iterator i1
= begin( );
2066 const_iterator i2
= i1
;
2068 for( ; i1
!= end( ); ++i1
, ++i2
, steps
+= Concrete::UNIT_TIME
)
2082 if( (*i1
)->front_
== (*i1
)->mid_
&&
2083 (*i2
)->rear_
== (*i2
)->mid_
)
2085 /* Straight line segment. The distance to straight line segments on the other path has already been computed.
2087 typedef typeof curvedSegments ListType
;
2088 for( ListType::const_iterator seg_b
= curvedSegments
.begin( ); seg_b
!= curvedSegments
.end( ); ++seg_b
)
2090 SegmentPolynomialPair3D
* baseSegs
= new SegmentPolynomialPair3D( steps
, *((*i1
)->mid_
), *((*i2
)->mid_
), seg_b
->steps
, *seg_b
);
2091 baseSegsMemory
.push_back( baseSegs
);
2092 pushCloseDeleteOther( & work
, new SegmentSectionPair3D( baseSegs
, Concrete::ZERO_TIME
, Concrete::UNIT_TIME
, Concrete::ZERO_TIME
, Concrete::UNIT_TIME
), bestDist
);
2097 /* General segment. (If it happens to be straight, we don't care.)
2099 Bezier::ControlPoints
< Concrete::Coords3D
> seg_a( *((*i1
)->mid_
), *((*i1
)->front_
), *((*i2
)->rear_
), *((*i2
)->mid_
) );
2102 typedef typeof straightSegments ListType
;
2103 for( ListType::const_iterator i
= straightSegments
.begin( ); i
!= straightSegments
.end( ); ++i
)
2105 SegmentPolynomialPair3D
* baseSegs
= new SegmentPolynomialPair3D( steps
, seg_a
, i
->steps
, *(i
->first
), *(i
->second
) );
2106 baseSegsMemory
.push_back( baseSegs
);
2107 pushCloseDeleteOther( & work
, new SegmentSectionPair3D( baseSegs
, Concrete::ZERO_TIME
, Concrete::UNIT_TIME
, Concrete::ZERO_TIME
, Concrete::UNIT_TIME
), bestDist
);
2112 typedef typeof curvedSegments ListType
;
2113 for( ListType::const_iterator seg_b
= curvedSegments
.begin( ); seg_b
!= curvedSegments
.end( ); ++seg_b
)
2115 SegmentPolynomialPair3D
* baseSegs
= new SegmentPolynomialPair3D( steps
, seg_a
, seg_b
->steps
, *seg_b
);
2116 baseSegsMemory
.push_back( baseSegs
);
2117 pushCloseDeleteOther( & work
, new SegmentSectionPair3D( baseSegs
, Concrete::ZERO_TIME
, Concrete::UNIT_TIME
, Concrete::ZERO_TIME
, Concrete::UNIT_TIME
), bestDist
);
2123 while( ! work
.empty( ) )
2125 SegmentSectionPair3D
* workItem
= work
.front( );
2127 if( workItem
->precomputedHullDistanceSquared( ) > ( ( bestDist
== 0 ) ? INTERSECT_DIST
: bestDist
) )
2132 if( bestDist
<= INTERSECT_DIST
)
2134 if( workItem
->steps_a( ) > resSteps
)
2139 if( workItem
->steps_a( ) == resSteps
)
2141 workItem
->update_t_a1( resFrac
);
2144 if( workItem
->isEmpty( ) )
2150 const Concrete::Time t_tol_1
= Computation::the_arcdelta
/ workItem
->maxSpeed_a( );
2151 const Concrete::Time t_tol_2
= Computation::the_arcdelta
/ workItem
->maxSpeed_b( );
2155 bool intersect
= workItem
->splitTimes( & t1
, & t2
, 0.001 * t_tol_1
, 0.001 * t_tol_2
, DISTANCE_TOL
); // too low precision gives erraneous results!
2156 Concrete::LengthSquared d
= workItem
->squaredDistanceAt( t1
, t2
);
2157 intersect
= intersect
|| d
< INTERSECT_DIST
;
2160 if( bestDist
> ZERO_DIST
2161 || workItem
->globalTime_a( t1
) < resSteps
+ resFrac
)
2163 bestDist
= ZERO_DIST
;
2164 resSteps
= workItem
->steps_a( );
2168 else if( d
< bestDist
)
2171 resSteps
= workItem
->steps_a( );
2176 pushCloseDeleteOther( & work
, workItem
->cutAfterBefore( t1
, t2
, CUT_LOSS
* t_tol_1
, CUT_LOSS
* t_tol_2
), bestDist
);
2177 pushCloseDeleteOther( & work
, workItem
->cutAfterAfter( t1
, t2
, CUT_LOSS
* t_tol_1
, CUT_LOSS
* t_tol_2
), bestDist
);
2181 pushCloseDeleteOther( & work
, workItem
->cutBeforeBefore( t1
, t2
, CUT_LOSS
* t_tol_1
, CUT_LOSS
* t_tol_2
), bestDist
);
2182 pushCloseDeleteOther( & work
, workItem
->cutBeforeAfter( t1
, t2
, CUT_LOSS
* t_tol_1
, CUT_LOSS
* t_tol_2
), bestDist
);
2183 pushCloseDeleteOther( & work
, workItem
->cutAfterBefore( t1
, t2
, CUT_LOSS
* t_tol_1
, CUT_LOSS
* t_tol_2
), bestDist
);
2184 pushCloseDeleteOther( & work
, workItem
->cutAfterAfter( t1
, t2
, CUT_LOSS
* t_tol_1
, CUT_LOSS
* t_tol_2
), bestDist
);
2189 if( resSteps
== Concrete::HUGE_TIME
)
2191 throw Exceptions::InternalError( "Path to path approximation: No result was produced." );
2196 Helpers::approximator3D_info_resultFactory
.set( "dist", Helpers::newValHandle( new Lang::Length( sqrtPhysical( bestDist
) ) ) );
2197 Helpers::approximator3D_info_resultFactory
.set( "other", Helpers::newValHandle( new Lang::PathSlider3D( p2
, res2
) ) );
2198 *dstInfo
= Helpers::approximator3D_info_resultFactory
.build( );
2200 return Concrete::SplineTime( resSteps
+ resFrac
);
2203 Shapes::ApproximationPoly3D::ApproximationPoly3D( const Concrete::Coords3D
& p0
, const Concrete::Coords3D
& p1
, const Concrete::Coords3D
& p2
, const Concrete::Coords3D
& p3
, const Concrete::Coords3D
& p
)
2204 : p_( p
), polyCoeffs_( Bezier::PolyCoeffs
< Concrete::Coords3D
>( Bezier::ControlPoints
< Concrete::Coords3D
>( p0
, p1
, p2
, p3
) ) )
2206 kxD0_0_
= polyCoeffs_
.z0_
.x_
- p_
.x_
;
2207 kxD0_1_
= polyCoeffs_
.z1_
.x_
.offtype
< 0, 1 >( );
2208 kxD0_2_
= polyCoeffs_
.z2_
.x_
.offtype
< 0, 2 >( );
2209 kxD0_3_
= polyCoeffs_
.z3_
.x_
.offtype
< 0, 3 >( );
2212 kxD1_1_
= 2 * kxD0_2_
;
2213 kxD1_2_
= 3 * kxD0_3_
;
2216 kxD2_1_
= 2 * kxD1_2_
;
2218 kyD0_0_
= polyCoeffs_
.z0_
.y_
- p
.y_
;
2219 kyD0_1_
= polyCoeffs_
.z1_
.y_
.offtype
< 0, 1 >( );
2220 kyD0_2_
= polyCoeffs_
.z2_
.y_
.offtype
< 0, 2 >( );
2221 kyD0_3_
= polyCoeffs_
.z3_
.y_
.offtype
< 0, 3 >( );
2224 kyD1_1_
= 2 * kyD0_2_
;
2225 kyD1_2_
= 3 * kyD0_3_
;
2228 kyD2_1_
= 2 * kyD1_2_
;
2230 kzD0_0_
= polyCoeffs_
.z0_
.z_
- p
.z_
;
2231 kzD0_1_
= polyCoeffs_
.z1_
.z_
.offtype
< 0, 1 >( );
2232 kzD0_2_
= polyCoeffs_
.z2_
.z_
.offtype
< 0, 2 >( );
2233 kzD0_3_
= polyCoeffs_
.z3_
.z_
.offtype
< 0, 3 >( );
2236 kzD1_1_
= 2 * kzD0_2_
;
2237 kzD1_2_
= 3 * kzD0_3_
;
2240 kzD2_1_
= 2 * kzD1_2_
;
2244 Shapes::ApproximationPoly3D::splitTime( const Concrete::Time t_low
, const Concrete::Time t_high
, const Concrete::Time t_tol
) const
2247 Concrete::LengthSquared last_f
= HUGE_VAL
;
2249 const double GRID
= 7;
2250 const Concrete::Time STEP
= ( t_high
- t_low
) / GRID
;
2251 for( Concrete::Time tt
= t_low
+ 0.5 * STEP
; tt
< t_high
; tt
+= STEP
)
2253 Concrete::LengthSquared tmp
= squaredDistanceAt( tt
);
2261 Concrete::Time last_t
= -1;
2262 bool lastOffBounds
= false;
2264 while( t
< last_t
- t_tol
|| t
> last_t
+ t_tol
)
2267 Physical
< 2, -1 > objectiveD1( 0 );
2268 Physical
< 2, -2 > objectiveD2( 0 );
2272 Physical
< 1, 0 > fD0
= kxD0_0_
+ t
* ( kxD0_1_
+ t
* ( kxD0_2_
+ t
* kxD0_3_
) );
2273 Physical
< 1, -1 > fD1
= kxD1_0_
+ t
* ( kxD1_1_
+ t
* kxD1_2_
);
2274 Physical
< 1, -2 > fD2
= kxD2_0_
+ t
* kxD2_1_
;
2275 objectiveD1
+= fD0
* fD1
;
2276 objectiveD2
+= fD1
* fD1
+ fD0
* fD2
;
2281 Physical
< 1, 0 > fD0
= kyD0_0_
+ t
* ( kyD0_1_
+ t
* ( kyD0_2_
+ t
* kyD0_3_
) );
2282 Physical
< 1, -1 > fD1
= kyD1_0_
+ t
* ( kyD1_1_
+ t
* kyD1_2_
);
2283 Physical
< 1, -2 > fD2
= kyD2_0_
+ t
* kyD2_1_
;
2284 objectiveD1
+= fD0
* fD1
;
2285 objectiveD2
+= fD1
* fD1
+ fD0
* fD2
;
2290 Physical
< 1, 0 > fD0
= kzD0_0_
+ t
* ( kzD0_1_
+ t
* ( kzD0_2_
+ t
* kzD0_3_
) );
2291 Physical
< 1, -1 > fD1
= kzD1_0_
+ t
* ( kzD1_1_
+ t
* kzD1_2_
);
2292 Physical
< 1, -2 > fD2
= kzD2_0_
+ t
* kzD2_1_
;
2293 objectiveD1
+= fD0
* fD1
;
2294 objectiveD2
+= fD1
* fD1
+ fD0
* fD2
;
2297 Concrete::Time step
;
2298 if( objectiveD2
<= 0 )
2300 /* The minimization problem is not convex, locally.
2301 * Go to one of the extremes.
2303 if( objectiveD1
< 0 )
2305 step
= 1.1 * ( t_high
- t
);
2309 step
= 1.1 * ( t_low
- t
);
2314 step
= -objectiveD1
/ objectiveD2
;
2315 if( t
+ step
< t_low
)
2317 step
= 1.1 * ( t_low
- t
); // 1.1 to make sure that the off-bounds test detects minima outside [ t_low t_high ]
2319 else if( t
+ step
> t_high
)
2321 step
= 1.1 * ( t_high
- t
); // 1.1 to make sure that the off-bounds test detects minima outside [ t_low t_high ]
2325 Concrete::LengthSquared f
= squaredDistanceAt( t
+ step
);
2328 while( f
> last_f
&& step
> t_tol
)
2331 f
= squaredDistanceAt( t
+ step
);
2336 while( f
> last_f
&& step
< -t_tol
)
2339 f
= squaredDistanceAt( t
+ step
);
2354 lastOffBounds
= true;
2357 else if( t
>= t_high
)
2366 lastOffBounds
= true;
2371 lastOffBounds
= false;
2377 if( t
> 0.5 * ( t_low
+ t_high
) )
2387 Bezier::ControlPoints
< Concrete::Coords3D
>
2388 Shapes::ApproximationPoly3D::getControls( const Concrete::Time t_low
, const Concrete::Time t_high
) const
2390 return Bezier::ControlPoints
< Concrete::Coords3D
>( polyCoeffs_
.subSection( t_low
.offtype
< 0, 1 >( ), t_high
.offtype
< 0, 1 >( ) ) );
2394 Shapes::ApproximationPoly3D::isCircular( ) const
2396 Concrete::Length rmax
= -HUGE_VAL
;
2397 Concrete::Length rmin
= HUGE_VAL
;
2398 for( Concrete::Time t
= 0; t
< 1.05; t
+= 0.33333 )
2400 Concrete::Length r
= distanceAt( t
);
2401 rmax
= max( rmax
, r
);
2402 rmin
= min( rmin
, r
);
2404 return rmax
<= rmin
* 1.0001;
2407 const Concrete::Coords3D
&
2408 Shapes::ApproximationPoly3D::getPoint( ) const
2415 Shapes::ApproximationPoly3D::distanceAt( Concrete::Time t
) const
2417 return hypotPhysical( kxD0_0_
+ t
* ( kxD0_1_
+ t
* ( kxD0_2_
+ t
* kxD0_3_
) ),
2418 kyD0_0_
+ t
* ( kyD0_1_
+ t
* ( kyD0_2_
+ t
* kyD0_3_
) ),
2419 kzD0_0_
+ t
* ( kzD0_1_
+ t
* ( kzD0_2_
+ t
* kzD0_3_
) ) );
2422 Concrete::LengthSquared
2423 Shapes::ApproximationPoly3D::squaredDistanceAt( Concrete::Time t
) const
2425 Concrete::Length delta_x
= kxD0_0_
+ t
* ( kxD0_1_
+ t
* ( kxD0_2_
+ t
* kxD0_3_
) );
2426 Concrete::Length delta_y
= kyD0_0_
+ t
* ( kyD0_1_
+ t
* ( kyD0_2_
+ t
* kyD0_3_
) );
2427 Concrete::Length delta_z
= kzD0_0_
+ t
* ( kzD0_1_
+ t
* ( kzD0_2_
+ t
* kzD0_3_
) );
2428 return delta_x
* delta_x
+ delta_y
* delta_y
+ delta_z
* delta_z
;
2431 Shapes::ApproximationSegmentSection3D::ApproximationSegmentSection3D( const Shapes::ApproximationPoly3D
* baseSeg
, Concrete::Time steps
, Concrete::Time t0
, Concrete::Time t1
)
2432 : controls_( baseSeg
->getControls( t0
, t1
) ), baseSeg_( baseSeg
), steps_( steps
), t0_( t0
), t1_( t1
)
2435 Shapes::ApproximationSegmentSection3D
*
2436 Shapes::ApproximationSegmentSection3D::cutAfter( Concrete::Time t
) const
2438 return new ApproximationSegmentSection3D( baseSeg_
, steps_
, t0_
, t
);
2441 Shapes::ApproximationSegmentSection3D
*
2442 Shapes::ApproximationSegmentSection3D::cutBefore( Concrete::Time t
) const
2444 return new ApproximationSegmentSection3D( baseSeg_
, steps_
, t
, t1_
);
2450 class HullSorter3D
: public std::binary_function
< const Concrete::Coords3D
*, const Concrete::Coords3D
*, bool >
2452 const Concrete::Coords3D
& p_
;
2454 HullSorter3D( const Concrete::Coords3D
& p
) : p_( p
) { }
2455 bool operator () ( const Concrete::Coords3D
* p1
, const Concrete::Coords3D
* p2
)
2457 /* The comparison orders the points according to their counter-clockwise
2458 * angle to the positive x-axis.
2459 * Since (by construction) no point can have smaller y-coordinate than p, all angles
2460 * are in the range [ 0, pi ], and it suffices to reason in terms of the argument
2461 * to the monotonic function arccotan.
2462 * The ordinary counter-clockwise-of by means of computing the projection of the
2463 * second on the clockwise normal of the other leads to the same equation.
2465 const Concrete::Length d1x
= p1
->x_
- p_
.x_
;
2466 const Concrete::Length d1y
= p1
->y_
- p_
.y_
;
2467 const Concrete::Length d2x
= p2
->x_
- p_
.x_
;
2468 const Concrete::Length d2y
= p2
->y_
- p_
.y_
;
2469 return d1y
* d2x
< d2y
* d1x
;
2475 Shapes::ApproximationSegmentSection3D::convexHullDistance( ) const
2477 Concrete::Coords3D
p( baseSeg_
->getPoint( ) );
2478 std::list
< const Concrete::Coords3D
* > hullPoints
;
2480 std::vector
< const Concrete::Coords3D
* > pointSet( 0 );
2481 pointSet
.reserve( 4 );
2482 pointSet
.push_back( & controls_
.p0_
);
2483 pointSet
.push_back( & controls_
.p1_
);
2484 pointSet
.push_back( & controls_
.p2_
);
2485 pointSet
.push_back( & controls_
.p3_
);
2487 Concrete::Length res
= HUGE_VAL
;
2489 for( std::vector
< const Concrete::Coords3D
* >::iterator i
= pointSet
.begin( );
2490 i
!= pointSet
.end( );
2493 if( i
!= pointSet
.begin( ) )
2495 const Concrete::Coords3D
* tmp
= pointSet
[0];
2499 triangleDistance( pointSet
, p
, & res
);
2502 if( res
== HUGE_VAL
)
2504 /* This means that p was on the inside of each line, meaning it
2505 * is also inside the convex hull.
2513 Shapes::ApproximationSegmentSection3D::triangleDistance( std::vector
< const Concrete::Coords3D
* > & points
, const Concrete::Coords3D
& p
, Concrete::Length
* bestDist
)
2516 * Note: We will mess around with points, but it will be returned as passed.
2522 * points[0] is the point which is on the inside side of the triangle.
2523 * points[1..3] are the corners of the triangle.
2526 /* This is the call that may throw a NonLocalExit exception, which we should catch! */
2527 const Concrete::UnitFloatTriple n
= Concrete::crossDirection( *points
[2] - *points
[1], *points
[3] - *points
[1] );
2529 const Concrete::Length tn
= Concrete::inner( n
, p
- *points
[1] );
2530 if( ( tn
> Concrete::ZERO_LENGTH
) == ( Concrete::inner( n
, *points
[0] - *points
[1] ) > Concrete::ZERO_LENGTH
) )
2532 /* When p is on the same side of the plane as points[0], we don't update *bestDist.
2533 * If *bestDist is never updated, it means that p is inside all planes and the minimum distance
2534 * is 0. See calling function.
2539 Concrete::Length res
= HUGE_VAL
;
2541 const Concrete::Coords3D pProj
= p
- tn
* n
;
2543 std::vector
< const Concrete::Coords3D
* >::iterator i0
= points
.begin( );
2545 std::vector
< const Concrete::Coords3D
* >::iterator begin
= i0
;
2546 for( ; i0
!= points
.end( ); ++i0
)
2548 const Concrete::Coords3D
* tmp
= *begin
;
2552 std::vector
< const Concrete::Coords3D
* >::iterator j
= begin
;
2553 const Concrete::Coords3D
& pInside( **j
);
2555 const Concrete::Coords3D
& p0( **j
);
2557 const Concrete::Coords3D
& p1( **j
);
2559 /* To find an outward normal, we find the projection of pInside, and then take a vector
2560 * from pInside to the projection. (We work with coordinates less p0.)
2562 const Concrete::Coords3D d1
= p1
- p0
;
2563 const Concrete::Coords3D dInside
= pInside
- p0
;
2564 const Concrete::Coords3D dp
= p
- p0
;
2565 double d1Norm2
= Concrete::innerScalar( d1
, d1
);
2568 res
= min( res
, dp
.norm( ) );
2572 /* Introduce new scope to prevent compiler warnings about goto past initializations.
2575 double sInside
= Concrete::innerScalar( dInside
, d1
) / d1Norm2
;
2576 const Concrete::Coords3D nOut
= d1
* sInside
- dInside
;
2577 const Concrete::Coords3D dpProj
= pProj
- p0
;
2579 if( Concrete::innerScalar( nOut
, nOut
) > 0 &&
2580 Concrete::innerScalar( nOut
, dpProj
) < 0 )
2582 /* pProj is on the same side as pInside
2587 res
= min( res
, ( dp
- d1
* ( Concrete::innerScalar( dpProj
, d1
) / d1Norm2
) ).norm( ) );
2590 /* Swap back so that points remains unchanged
2596 if( res
== HUGE_VAL
)
2598 res
= ( p
- pProj
).norm( );
2601 *bestDist
= min( *bestDist
, res
);
2603 catch( const NonLocalExit::CrossDirectionOfParallel
& ball
)
2605 /* The triangle is collapsed to a line segment (or just a point).
2606 * The point cannot be strictly inside the triangle in this case.
2608 throw Exceptions::InternalError( "Unprepared to handle collapsed triangle here. This algorithm should be re-implemented using the robust polytope distance computation." );
2614 Shapes::ApproximationSegmentSection3D::splitTime( const Concrete::Time t_tol
) const
2616 Concrete::Time d
= 0.2 * ( t1_
- t0_
);
2617 return baseSeg_
->splitTime( t0_
+ d
, t1_
- d
, t_tol
);
2621 Shapes::ApproximationSegmentSection3D::globalTime( Concrete::Time t
) const
2627 Shapes::ApproximationSegmentSection3D::distanceAt( Concrete::Time t
) const
2629 return baseSeg_
->distanceAt( t
);
2633 Shapes::ApproximationSegmentSection3D::maxSpeed( ) const
2635 Concrete::Length tmp
= max( hypotPhysical( controls_
.p1_
.x_
- controls_
.p0_
.x_
, controls_
.p1_
.y_
- controls_
.p0_
.y_
, controls_
.p1_
.z_
- controls_
.p0_
.z_
),
2636 hypotPhysical( controls_
.p2_
.x_
- controls_
.p1_
.x_
, controls_
.p2_
.y_
- controls_
.p1_
.y_
, controls_
.p2_
.z_
- controls_
.p1_
.z_
) );
2637 return max( tmp
, hypotPhysical( controls_
.p3_
.x_
- controls_
.p2_
.x_
, controls_
.p3_
.y_
- controls_
.p2_
.y_
, controls_
.p3_
.z_
- controls_
.p2_
.z_
) ).offtype
< 0, 1 >( );
2641 Shapes::ApproximationSegmentSection3D::duration( ) const
2648 Lang::ElementaryPath3D::arcLength( ) const
2652 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2654 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2656 Concrete::Length totlen
= 0;
2658 const Concrete::Length arcdelta
= Computation::the_arcdelta
;
2660 const_iterator i1
= begin( );
2661 const_iterator i2
= i1
;
2663 for( ; i1
!= end( ); ++i1
, ++i2
)
2676 if( (*i1
)->front_
== (*i1
)->mid_
&&
2677 (*i2
)->rear_
== (*i2
)->mid_
)
2679 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
2680 totlen
+= segLength
;
2684 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
2685 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
2686 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
2687 Concrete::Bezier x1
= (*i1
)->front_
->x_
.offtype
< 0, 3 >( );
2688 Concrete::Bezier y1
= (*i1
)->front_
->y_
.offtype
< 0, 3 >( );
2689 Concrete::Bezier z1
= (*i1
)->front_
->z_
.offtype
< 0, 3 >( );
2690 Concrete::Bezier x2
= (*i2
)->rear_
->x_
.offtype
< 0, 3 >( );
2691 Concrete::Bezier y2
= (*i2
)->rear_
->y_
.offtype
< 0, 3 >( );
2692 Concrete::Bezier z2
= (*i2
)->rear_
->z_
.offtype
< 0, 3 >( );
2693 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
2694 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
2695 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
2697 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
2699 if( segLengthBound
< arcdelta
)
2701 totlen
+= segLengthBound
;
2705 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
2706 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
2707 for( Concrete::Time t
= 0; t
< 1; t
+= dt
)
2709 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
2710 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
2711 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
2712 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
2713 Physical
< 0, 2 > kv3
= 3 * t
* t
;
2714 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
2715 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
2716 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
2718 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
2721 totlen
+= tmpSum_l
* dt
.offtype
< 0, 1 >( );
2729 Lang::ElementaryPath3D::arcLength( Concrete::Time tRemaining
) const
2733 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2735 if( tRemaining
< 0 )
2737 return negative_arcLength( -tRemaining
);
2739 if( isinfPhysical( tRemaining
) )
2741 throw Exceptions::OutOfRange( "The arctime to infinity is not defined." );
2743 if( tRemaining
> duration( ) && ! closed_
)
2745 throw Exceptions::OutOfRange( "Too big arctime for open path." );
2747 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2749 Concrete::Length totlen
= 0;
2751 const Concrete::Length arcdelta
= Computation::the_arcdelta
;
2754 const double revolutions
= floor( tRemaining
/ Concrete::Time( duration( ) ) );
2755 if( revolutions
> 0 )
2757 totlen
+= revolutions
* arcLength( );
2758 tRemaining
-= revolutions
* Concrete::Time( duration( ) );
2762 const_iterator i1
= begin( );
2763 const_iterator i2
= i1
;
2765 for( ; tRemaining
> 0; ++i1
, ++i2
, tRemaining
-= Concrete::UNIT_TIME
)
2769 /* This could be considered an error situation, but it may be due to numeric
2770 * errors, so we take action accordingly.
2776 /* This could also be an error situation...
2780 if( (*i1
)->front_
== (*i1
)->mid_
&&
2781 (*i2
)->rear_
== (*i2
)->mid_
)
2783 if( tRemaining
<= 1 )
2785 Concrete::Time t1
= tRemaining
;
2786 Concrete::Time tc
= Concrete::UNIT_TIME
- t1
; /* complement to t1 */
2787 Concrete::Coords3D tmp
=
2788 ( tc
* tc
* ( tc
+ 3 * t1
) ).offtype
< 0, 3 >( ) * (*((*i1
)->mid_
)) +
2789 ( t1
* t1
* ( t1
+ 3 * tc
) ).offtype
< 0, 3 >( ) * (*((*i2
)->mid_
));
2790 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- tmp
.x_
, (*i1
)->mid_
->y_
- tmp
.y_
, (*i1
)->mid_
->z_
- tmp
.z_
);
2791 totlen
+= segLength
;
2794 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
2795 totlen
+= segLength
;
2799 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
2800 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
2801 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
2802 Concrete::Bezier x1
= (*i1
)->front_
->x_
.offtype
< 0, 3 >( );
2803 Concrete::Bezier y1
= (*i1
)->front_
->y_
.offtype
< 0, 3 >( );
2804 Concrete::Bezier z1
= (*i1
)->front_
->z_
.offtype
< 0, 3 >( );
2805 Concrete::Bezier x2
= (*i2
)->rear_
->x_
.offtype
< 0, 3 >( );
2806 Concrete::Bezier y2
= (*i2
)->rear_
->y_
.offtype
< 0, 3 >( );
2807 Concrete::Bezier z2
= (*i2
)->rear_
->z_
.offtype
< 0, 3 >( );
2808 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
2809 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
2810 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
2812 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
2814 if( segLengthBound
< arcdelta
)
2816 if( tRemaining
<= 1 )
2818 totlen
+= ( tRemaining
/ Concrete::UNIT_TIME
) * segLengthBound
;
2821 totlen
+= segLengthBound
;
2825 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
2826 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
2827 const Concrete::Time tend
= min( Concrete::UNIT_TIME
, tRemaining
);
2828 for( Concrete::Time t
= 0; t
< tend
; t
+= dt
)
2830 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
2831 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
2832 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
2833 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
2834 Physical
< 0, 2 > kv3
= 3 * t
* t
;
2835 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
2836 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
2837 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
2839 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
2842 totlen
+= tmpSum_l
* dt
.offtype
< 0, 1 >( );
2850 Lang::ElementaryPath3D::negative_arcLength( Concrete::Time tRemaining
) const
2854 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2856 if( tRemaining
< 0 )
2858 throw Exceptions::InternalError( "Negative tRemaining in negative_arcLength." );
2862 throw Exceptions::OutOfRange( "The arctime at negative lengths is not defined for open paths." );
2864 if( isinfPhysical( tRemaining
) )
2866 throw Exceptions::OutOfRange( "The arctime to infinity is not defined." );
2869 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2871 Concrete::Length totlen
= 0;
2873 const Concrete::Length arcdelta
= Computation::the_arcdelta
;
2876 const double revolutions
= floor( tRemaining
/ Concrete::Time( duration( ) ) );
2877 if( revolutions
> 0 )
2879 totlen
-= revolutions
* arcLength( );
2880 tRemaining
-= revolutions
* Concrete::Time( duration( ) );
2884 const_reverse_iterator i1
= rbegin( );
2885 const_reverse_iterator i2
= i1
;
2896 for( ; tRemaining
> 0; ++i1
, ++i2
, tRemaining
-= Concrete::UNIT_TIME
)
2900 /* This could be considered an error situation, but it may be due to numeric
2901 * errors, so we take action accordingly.
2909 if( (*i1
)->rear_
== (*i1
)->mid_
&&
2910 (*i2
)->front_
== (*i2
)->mid_
)
2912 if( tRemaining
<= 1 )
2914 Concrete::Time t1
= tRemaining
;
2915 Concrete::Time tc
= Concrete::UNIT_TIME
- t1
; /* complement to t1 */
2916 Concrete::Coords3D tmp
=
2917 ( tc
* tc
* ( tc
+ 3 * t1
) ).offtype
< 0, 3 >( ) * (*((*i1
)->mid_
)) +
2918 ( t1
* t1
* ( t1
+ 3 * tc
) ).offtype
< 0, 3 >( ) * (*((*i2
)->mid_
));
2919 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- tmp
.x_
, (*i1
)->mid_
->y_
- tmp
.y_
, (*i1
)->mid_
->z_
- tmp
.z_
);
2920 totlen
-= segLength
;
2923 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
2924 totlen
-= segLength
;
2928 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
2929 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
2930 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
2931 Concrete::Bezier x1
= (*i1
)->rear_
->x_
.offtype
< 0, 3 >( );
2932 Concrete::Bezier y1
= (*i1
)->rear_
->y_
.offtype
< 0, 3 >( );
2933 Concrete::Bezier z1
= (*i1
)->rear_
->z_
.offtype
< 0, 3 >( );
2934 Concrete::Bezier x2
= (*i2
)->front_
->x_
.offtype
< 0, 3 >( );
2935 Concrete::Bezier y2
= (*i2
)->front_
->y_
.offtype
< 0, 3 >( );
2936 Concrete::Bezier z2
= (*i2
)->front_
->z_
.offtype
< 0, 3 >( );
2937 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
2938 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
2939 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
2941 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
2943 if( segLengthBound
< arcdelta
)
2945 if( tRemaining
<= 1 )
2947 totlen
-= ( tRemaining
/ Concrete::UNIT_TIME
) * segLengthBound
;
2950 totlen
-= segLengthBound
;
2954 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
2955 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
2956 const Concrete::Time tend
= min( Concrete::UNIT_TIME
, tRemaining
);
2957 for( Concrete::Time t
= 0; t
< tend
; t
+= dt
)
2959 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
2960 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
2961 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
2962 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
2963 Physical
< 0, 2 > kv3
= 3 * t
* t
;
2964 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
2965 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
2966 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
2968 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
2971 totlen
-= tmpSum_l
* dt
.offtype
< 0, 1 >( );
2978 Concrete::SplineTime
2979 Lang::ElementaryPath3D::arcTime( const Concrete::Length
& t
, Concrete::Time t0
) const
2983 throw Exceptions::OutOfRange( "The empty path has no arctimes defined." );
2985 if( duration( ) == 0 )
2987 throw Exceptions::OutOfRange( "The singleton path has no arctimes defined." );
2991 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2995 return negative_arcTime( - t
, t0
);
2998 Concrete::Time splineTime
= t0
;
2999 t0
= modPhysical( t0
, Concrete::Time( duration( ) ) );
3002 t0
+= Concrete::Time( duration( ) );
3005 const_iterator i1
= begin( );
3008 t0
-= Concrete::UNIT_TIME
;
3014 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3019 const_iterator i2
= i1
;
3022 const Concrete::Length arcdelta
= Computation::the_arcdelta
;
3023 Concrete::Length remainingLength
= t
;
3031 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3035 if( (*i1
)->front_
== (*i1
)->mid_
&&
3036 (*i2
)->rear_
== (*i2
)->mid_
)
3038 Concrete::Time tc
= Concrete::UNIT_TIME
- t0
; /* complement to t0 */
3039 Concrete::Coords3D tmp
=
3040 ( tc
* tc
* ( tc
+ 3 * t0
) ).offtype
< 0, 3 >( ) * (*((*i1
)->mid_
)) +
3041 ( t0
* t0
* ( t0
+ 3 * tc
) ).offtype
< 0, 3 >( ) * (*((*i2
)->mid_
));
3042 const Concrete::Length segRestLength
= hypotPhysical( tmp
.x_
- (*i2
)->mid_
->x_
, tmp
.y_
- (*i2
)->mid_
->y_
, tmp
.z_
- (*i2
)->mid_
->z_
);
3043 if( segRestLength
< remainingLength
)
3045 remainingLength
-= segRestLength
;
3048 const Concrete::Length segPastLength
= hypotPhysical( tmp
.x_
- (*i1
)->mid_
->x_
, tmp
.y_
- (*i1
)->mid_
->y_
, tmp
.z_
- (*i1
)->mid_
->z_
);
3049 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
3050 splineTime
+= Shapes::straightLineArcTime( ( segPastLength
+ remainingLength
) / segLength
) - t0
;
3055 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
3056 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
3057 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
3058 Concrete::Bezier x1
= (*i1
)->front_
->x_
.offtype
< 0, 3 >( );
3059 Concrete::Bezier y1
= (*i1
)->front_
->y_
.offtype
< 0, 3 >( );
3060 Concrete::Bezier z1
= (*i1
)->front_
->z_
.offtype
< 0, 3 >( );
3061 Concrete::Bezier x2
= (*i2
)->rear_
->x_
.offtype
< 0, 3 >( );
3062 Concrete::Bezier y2
= (*i2
)->rear_
->y_
.offtype
< 0, 3 >( );
3063 Concrete::Bezier z2
= (*i2
)->rear_
->z_
.offtype
< 0, 3 >( );
3064 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
3065 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
3066 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
3068 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
3070 if( segLengthBound
< arcdelta
)
3072 remainingLength
-= segLengthBound
;
3073 if( remainingLength
<= 0 )
3081 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
3083 const Concrete::Length remainingDiv_dt
= remainingLength
/ dt
.offtype
< 0, 1 >( );
3084 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
3085 for( Concrete::Time t
= t0
; t
< 1; t
+= dt
)
3087 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
3088 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
3089 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
3090 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
3091 Physical
< 0, 2 > kv3
= 3 * t
* t
;
3092 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
3093 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
3094 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
3096 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
3098 if( tmpSum_l
>= remainingDiv_dt
)
3100 splineTime
+= t
- t0
;
3104 remainingLength
-= tmpSum_l
* dt
.offtype
< 0, 1 >( );
3109 splineTime
= Concrete::Time( ceil( splineTime
.offtype
< 0, 1 >( ) ) );
3115 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3122 for( ; ; ++i1
, ++i2
, splineTime
+= Concrete::UNIT_TIME
)
3126 /* If not closed, then the break occurs when i2 reaches end
3134 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3138 if( (*i1
)->front_
== (*i1
)->mid_
&&
3139 (*i2
)->rear_
== (*i2
)->mid_
)
3141 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
3142 if( segLength
< remainingLength
)
3144 remainingLength
-= segLength
;
3147 splineTime
+= Shapes::straightLineArcTime( remainingLength
/ segLength
);
3151 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
3152 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
3153 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
3154 Concrete::Bezier x1
= (*i1
)->front_
->x_
.offtype
< 0, 3 >( );
3155 Concrete::Bezier y1
= (*i1
)->front_
->y_
.offtype
< 0, 3 >( );
3156 Concrete::Bezier z1
= (*i1
)->front_
->z_
.offtype
< 0, 3 >( );
3157 Concrete::Bezier x2
= (*i2
)->rear_
->x_
.offtype
< 0, 3 >( );
3158 Concrete::Bezier y2
= (*i2
)->rear_
->y_
.offtype
< 0, 3 >( );
3159 Concrete::Bezier z2
= (*i2
)->rear_
->z_
.offtype
< 0, 3 >( );
3160 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
3161 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
3162 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
3164 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
3166 if( segLengthBound
< arcdelta
)
3168 remainingLength
-= segLengthBound
;
3169 if( remainingLength
<= 0 )
3177 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
3179 const Concrete::Length remainingDiv_dt
= remainingLength
/ dt
.offtype
< 0, 1 >( );
3180 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
3181 for( Concrete::Time t
= 0; t
< 1; t
+= dt
)
3183 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
3184 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
3185 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
3186 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
3187 Physical
< 0, 2 > kv3
= 3 * t
* t
;
3188 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
3189 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
3190 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
3192 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
3194 if( tmpSum_l
>= remainingDiv_dt
)
3200 remainingLength
-= tmpSum_l
* dt
.offtype
< 0, 1 >( );
3208 Concrete::SplineTime
3209 Lang::ElementaryPath3D::negative_arcTime( const Concrete::Length deltaLen
, Concrete::Time t0
) const
3213 throw Exceptions::InternalError( "Negative t in negative_arcTime." );
3217 throw Exceptions::OutOfRange( "The empty path has no arctimes defined." );
3219 if( duration( ) == 0 )
3221 throw Exceptions::OutOfRange( "The singleton path has no arctimes defined." );
3224 if( deltaLen
>= HUGE_VAL
)
3226 return Concrete::SplineTime( 0, true );
3229 Concrete::Time splineTime
= t0
;
3232 t0
= modPhysical( t0
, Concrete::Time( duration( ) ) );
3235 t0
+= Concrete::Time( duration( ) );
3244 else if( t0
> Concrete::Time( duration( ) ) )
3246 t0
= Concrete::Time( duration( ) );
3249 t0
= Concrete::Time( duration( ) ) - t0
;
3250 // From here on, t0 is the (positive) backwards-time
3252 const_reverse_iterator i1
= rbegin( );
3260 t0
-= Concrete::UNIT_TIME
;
3266 return Concrete::SplineTime( 0, true );
3271 const_reverse_iterator i2
= i1
;
3274 const Concrete::Length arcdelta
= Computation::the_arcdelta
;
3275 Concrete::Length remainingLength
= deltaLen
;
3283 return Concrete::SplineTime( 0, true );
3287 if( (*i1
)->rear_
== (*i1
)->mid_
&&
3288 (*i2
)->front_
== (*i2
)->mid_
)
3290 Concrete::Time tc
= Concrete::UNIT_TIME
- t0
; /* complement to t0 */
3291 Concrete::Coords3D tmp
=
3292 ( tc
* tc
* ( tc
+ 3 * t0
) ).offtype
< 0, 3 >( ) * (*((*i1
)->mid_
)) +
3293 ( t0
* t0
* ( t0
+ 3 * tc
) ).offtype
< 0, 3 >( ) * (*((*i2
)->mid_
));
3294 const Concrete::Length segRestLength
= hypotPhysical( tmp
.x_
- (*i2
)->mid_
->x_
, tmp
.y_
- (*i2
)->mid_
->y_
, tmp
.z_
- (*i2
)->mid_
->z_
);
3295 if( segRestLength
< remainingLength
)
3297 remainingLength
-= segRestLength
;
3300 const Concrete::Length segPastLength
= hypotPhysical( tmp
.x_
- (*i1
)->mid_
->x_
, tmp
.y_
- (*i1
)->mid_
->y_
, tmp
.z_
- (*i1
)->mid_
->z_
);
3301 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
3302 splineTime
-= Shapes::straightLineArcTime( ( segPastLength
+ remainingLength
) / segLength
) - t0
;
3307 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
3308 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
3309 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
3310 Concrete::Bezier x1
= (*i1
)->rear_
->x_
.offtype
< 0, 3 >( );
3311 Concrete::Bezier y1
= (*i1
)->rear_
->y_
.offtype
< 0, 3 >( );
3312 Concrete::Bezier z1
= (*i1
)->rear_
->z_
.offtype
< 0, 3 >( );
3313 Concrete::Bezier x2
= (*i2
)->front_
->x_
.offtype
< 0, 3 >( );
3314 Concrete::Bezier y2
= (*i2
)->front_
->y_
.offtype
< 0, 3 >( );
3315 Concrete::Bezier z2
= (*i2
)->front_
->z_
.offtype
< 0, 3 >( );
3316 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
3317 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
3318 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
3320 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
3322 if( segLengthBound
< arcdelta
)
3324 remainingLength
-= segLengthBound
;
3325 if( remainingLength
<= 0 )
3333 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
3335 const Concrete::Length remainingDiv_dt
= remainingLength
/ dt
.offtype
< 0, 1 >( );
3336 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
3337 for( Concrete::Time t
= t0
; t
< 1; t
+= dt
)
3339 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
3340 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
3341 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
3342 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
3343 Physical
< 0, 2 > kv3
= 3 * t
* t
;
3344 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
3345 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
3346 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
3348 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
3350 if( tmpSum_l
>= remainingDiv_dt
)
3352 splineTime
-= t
- t0
;
3356 remainingLength
-= tmpSum_l
* dt
.offtype
< 0, 1 >( );
3361 splineTime
= Concrete::Time( floor( splineTime
.offtype
< 0, 1 >( ) ) );
3367 return Concrete::SplineTime( 0, true );
3374 for( ; ; ++i1
, ++i2
, splineTime
-= Concrete::UNIT_TIME
)
3378 /* If not closed, then the break occurs when i2 reaches end
3386 return Concrete::SplineTime( 0, true );
3390 if( (*i1
)->rear_
== (*i1
)->mid_
&&
3391 (*i2
)->front_
== (*i2
)->mid_
)
3393 const Concrete::Length segLength
= hypotPhysical( (*i1
)->mid_
->x_
- (*i2
)->mid_
->x_
, (*i1
)->mid_
->y_
- (*i2
)->mid_
->y_
, (*i1
)->mid_
->z_
- (*i2
)->mid_
->z_
);
3394 if( segLength
< remainingLength
)
3396 remainingLength
-= segLength
;
3399 splineTime
-= Shapes::straightLineArcTime( remainingLength
/ segLength
);
3403 Concrete::Bezier x0
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
3404 Concrete::Bezier y0
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
3405 Concrete::Bezier z0
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
3406 Concrete::Bezier x1
= (*i1
)->rear_
->x_
.offtype
< 0, 3 >( );
3407 Concrete::Bezier y1
= (*i1
)->rear_
->y_
.offtype
< 0, 3 >( );
3408 Concrete::Bezier z1
= (*i1
)->rear_
->z_
.offtype
< 0, 3 >( );
3409 Concrete::Bezier x2
= (*i2
)->front_
->x_
.offtype
< 0, 3 >( );
3410 Concrete::Bezier y2
= (*i2
)->front_
->y_
.offtype
< 0, 3 >( );
3411 Concrete::Bezier z2
= (*i2
)->front_
->z_
.offtype
< 0, 3 >( );
3412 Concrete::Bezier x3
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
3413 Concrete::Bezier y3
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
3414 Concrete::Bezier z3
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
3416 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
3418 if( segLengthBound
< arcdelta
)
3420 remainingLength
-= segLengthBound
;
3421 if( remainingLength
<= 0 )
3429 Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
3431 const Concrete::Length remainingDiv_dt
= remainingLength
/ dt
.offtype
< 0, 1 >( );
3432 Concrete::Length tmpSum_l
= Concrete::ZERO_LENGTH
;
3433 for( Concrete::Time t
= 0; t
< 1; t
+= dt
)
3435 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
3436 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
3437 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
3438 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
3439 Physical
< 0, 2 > kv3
= 3 * t
* t
;
3440 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
3441 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
3442 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
3444 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
3446 if( tmpSum_l
>= remainingDiv_dt
)
3452 remainingLength
-= tmpSum_l
* dt
.offtype
< 0, 1 >( );
3460 RefCountPtr
< const Lang::ElementaryPath3D
>
3461 Lang::ElementaryPath3D::subpath( const Concrete::SplineTime splt1
, const Concrete::SplineTime splt2
) const
3465 throw Exceptions::OutOfRange( "The empty path has no subpaths." );
3467 Lang::ElementaryPath3D
* res
= new Lang::ElementaryPath3D
;
3469 if( splt2
.t( ) < splt1
.t( ) )
3471 throw Exceptions::OutOfRange( "The path-times must come in ascending order." );
3473 if( splt2
.t( ) != HUGE_VAL
&& splt2
.t( ) > splt1
.t( ) + size( ) )
3475 throw Exceptions::OutOfRange( "It is forbidden (although not logically necessary) to cycle more than one revolution." );
3478 Concrete::Time gt1
= timeCheck( splt1
.t( ) ); /* "g" as in global. t1 refers to a time on an elementary segment */
3479 Concrete::Time gt2
= timeCheck( splt2
.t( ) );
3480 if( gt2
<= gt1
&& splt2
.t( ) > splt1
.t( ) )
3485 /* First, we treat the case where both cuts are on the same elementary segment.
3486 * The reason is twofold. First, it is more efficient, as it is almost only half
3487 * the amount of computation this way. Secondly, cutting sequentially on the same
3488 * segment requires a bit of extra manipulation of the time arguments.
3490 if( fmod( floor( gt1
.offtype
< 0, 1 >( ) ) + 1, size( ) ) == ceil( gt2
.offtype
< 0, 1 >( ) ) )
3493 const Concrete::PathPoint3D
* p1
;
3494 const Concrete::PathPoint3D
* p2
;
3495 findSegment( gt1
, &t1
, &p1
, &p2
); /* note that the stored pointers must not be deleted */
3496 Concrete::Time t2
= gt2
- ceil( gt2
.offtype
< 0, 1 >( ) ) + 1;
3497 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
3498 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
3499 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
3500 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
3501 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
3502 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
3503 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
3504 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
3505 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
3506 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
3507 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
3508 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
3510 Concrete::Time tc
= t1
- Concrete::UNIT_TIME
; /* Note the sign here! */
3511 Concrete::Time td
= t1
- t2
;
3513 /* Compute new x polynomial using variable change t = t1 - td * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
3515 Concrete::Length k0x
= - tc
* tc
* tc
* x0
+ t1
* ( 3 * tc
* tc
* x1
+ t1
* ( 3 * Concrete::UNIT_TIME
* x2
- 3 * t1
* x2
+ t1
* x3
) );
3516 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
) );
3517 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
);
3518 Concrete::Length k3x
= td
* td
* td
* ( x0
- 3 * x1
+ 3 * x2
- x3
);
3520 /* Compute new coefficients in the standard polynomial base */
3521 Concrete::Length x0new
= k0x
;
3522 Concrete::Length x1new
= k0x
+ k1x
/ 3;
3523 Concrete::Length x2new
= k0x
+ ( 2 * k1x
+ k2x
) / 3;
3524 Concrete::Length x3new
= k0x
+ k1x
+ k2x
+ k3x
;
3526 /* Same thing for y */
3528 Concrete::Length k0y
= - tc
* tc
* tc
* y0
+ t1
* ( 3 * tc
* tc
* y1
+ t1
* ( 3 * Concrete::UNIT_TIME
* y2
- 3 * t1
* y2
+ t1
* y3
) );
3529 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
) );
3530 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
);
3531 Concrete::Length k3y
= td
* td
* td
* ( y0
- 3 * y1
+ 3 * y2
- y3
);
3533 Concrete::Length y0new
= k0y
;
3534 Concrete::Length y1new
= k0y
+ k1y
/ 3;
3535 Concrete::Length y2new
= k0y
+ ( 2 * k1y
+ k2y
) / 3;
3536 Concrete::Length y3new
= k0y
+ k1y
+ k2y
+ k3y
;
3538 /* Same thing for z */
3540 Concrete::Length k0z
= - tc
* tc
* tc
* z0
+ t1
* ( 3 * tc
* tc
* z1
+ t1
* ( 3 * Concrete::UNIT_TIME
* z2
- 3 * t1
* z2
+ t1
* z3
) );
3541 Concrete::Length k1z
= 3 * td
* ( tc
* tc
* z0
+ ( -Concrete::UNIT_TIME
*Concrete::UNIT_TIME
+ 4 * Concrete::UNIT_TIME
* t1
- 3 * t1
* t1
) * z1
+ t1
* ( -2 * Concrete::UNIT_TIME
* z2
+ 3 * t1
* z2
- t1
* z3
) );
3542 Concrete::Length k2z
= -3 * td
* td
* ( tc
* z0
+ 2 * Concrete::UNIT_TIME
* z1
- 3 * t1
* z1
- Concrete::UNIT_TIME
* z2
+ 3 * t1
* z2
- t1
* z3
);
3543 Concrete::Length k3z
= td
* td
* td
* ( z0
- 3 * z1
+ 3 * z2
- z3
);
3545 Concrete::Length z0new
= k0z
;
3546 Concrete::Length z1new
= k0z
+ k1z
/ 3;
3547 Concrete::Length z2new
= k0z
+ ( 2 * k1z
+ k2z
) / 3;
3548 Concrete::Length z3new
= k0z
+ k1z
+ k2z
+ k3z
;
3551 /* In case the path was logically straight, we don't want to produce a path which is not
3552 * logically straight. Of course, it is a bit inefficient to take this into account now,
3553 * but the code becomes very intuitive.
3556 Concrete::PathPoint3D
* ptmp
= new Concrete::PathPoint3D( x0new
, y0new
, z0new
);
3557 if( p1
->front_
!= p1
->mid_
|| p2
->rear_
!= p2
->mid_
)
3559 ptmp
->front_
= new Concrete::Coords3D( x1new
, y1new
, z1new
);
3561 res
->push_back( ptmp
);
3562 ptmp
= new Concrete::PathPoint3D( x3new
, y3new
, z3new
);
3563 if( p1
->front_
!= p1
->mid_
|| p2
->rear_
!= p2
->mid_
)
3565 ptmp
->rear_
= new Concrete::Coords3D( x2new
, y2new
, z2new
);
3567 res
->push_back( ptmp
);
3569 return RefCountPtr
< const Lang::ElementaryPath3D
>( res
);
3573 /* Now, we treat the other case, where the cuts are on different segments. This results in a three phase
3575 * Since "cutting away after" requires much simpler formulas than straight-forward "cutting away before",
3576 * the latter is treated as the former, but with time reversed.
3579 Concrete::Time t
= 0;
3580 const_iterator i1
= begin( );
3581 for( ; t
+ 1 <= gt1
; t
+= Concrete::UNIT_TIME
, ++i1
)
3583 const_iterator i2
= i1
;
3590 Concrete::Time t1
= gt1
- t
;
3591 Concrete::PathPoint3D
* p1
;
3592 Concrete::PathPoint3D
* p2
;
3595 p1
= new Concrete::PathPoint3D( **i1
);
3596 p2
= new Concrete::PathPoint3D( **i2
);
3601 Concrete::Bezier x3
= (*i1
)->mid_
->x_
.offtype
< 0, 3 >( );
3602 Concrete::Bezier y3
= (*i1
)->mid_
->y_
.offtype
< 0, 3 >( );
3603 Concrete::Bezier z3
= (*i1
)->mid_
->z_
.offtype
< 0, 3 >( );
3604 Concrete::Bezier x2
= (*i1
)->front_
->x_
.offtype
< 0, 3 >( );
3605 Concrete::Bezier y2
= (*i1
)->front_
->y_
.offtype
< 0, 3 >( );
3606 Concrete::Bezier z2
= (*i1
)->front_
->z_
.offtype
< 0, 3 >( );
3607 Concrete::Bezier x1
= (*i2
)->rear_
->x_
.offtype
< 0, 3 >( );
3608 Concrete::Bezier y1
= (*i2
)->rear_
->y_
.offtype
< 0, 3 >( );
3609 Concrete::Bezier z1
= (*i2
)->rear_
->z_
.offtype
< 0, 3 >( );
3610 Concrete::Bezier x0
= (*i2
)->mid_
->x_
.offtype
< 0, 3 >( );
3611 Concrete::Bezier y0
= (*i2
)->mid_
->y_
.offtype
< 0, 3 >( );
3612 Concrete::Bezier z0
= (*i2
)->mid_
->z_
.offtype
< 0, 3 >( );
3614 t1
= Concrete::UNIT_TIME
- t1
;
3616 /* Compute new x polynomial using variable change t = t1 * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
3618 Concrete::Bezier k0x
= x0
;
3619 Concrete::Bezier k1x
= 3 * t1
* ( x1
- x0
).offtype
< 0, 1 >( );
3620 Concrete::Bezier k2x
= 3 * t1
* t1
* ( x0
- 2 * x1
+ x2
).offtype
< 0, 2 >( );
3621 Concrete::Bezier k3x
= t1
* t1
* t1
* ( -x0
+ 3 * x1
- 3 * x2
+ x3
).offtype
< 0, 3 >( );
3622 /* Compute new coefficients in the standard polynomial base */
3623 Concrete::Bezier x0new
= k0x
;
3624 Concrete::Bezier x1new
= k0x
+ k1x
/ 3;
3625 Concrete::Bezier x2new
= k0x
+ ( 2 * k1x
+ k2x
) / 3;
3626 Concrete::Bezier x3new
= k0x
+ k1x
+ k2x
+ k3x
;
3628 Concrete::Bezier k0y
= y0
;
3629 Concrete::Bezier k1y
= 3 * t1
* ( y1
- y0
).offtype
< 0, 1 >( );
3630 Concrete::Bezier k2y
= 3 * t1
* t1
* ( y0
- 2 * y1
+ y2
).offtype
< 0, 2 >( );
3631 Concrete::Bezier k3y
= t1
* t1
* t1
* ( -y0
+ 3 * y1
- 3 * y2
+ y3
).offtype
< 0, 3 >( );
3632 /* Compute new coefficients in the standard polynomial base */
3633 Concrete::Bezier y0new
= k0y
;
3634 Concrete::Bezier y1new
= k0y
+ k1y
/ 3;
3635 Concrete::Bezier y2new
= k0y
+ ( 2 * k1y
+ k2y
) / 3;
3636 Concrete::Bezier y3new
= k0y
+ k1y
+ k2y
+ k3y
;
3638 Concrete::Bezier k0z
= z0
;
3639 Concrete::Bezier k1z
= 3 * t1
* ( z1
- z0
).offtype
< 0, 1 >( );
3640 Concrete::Bezier k2z
= 3 * t1
* t1
* ( z0
- 2 * z1
+ z2
).offtype
< 0, 2 >( );
3641 Concrete::Bezier k3z
= t1
* t1
* t1
* ( -z0
+ 3 * z1
- 3 * z2
+ z3
).offtype
< 0, 3 >( );
3642 /* Compute new coefficients in the standard polynomial base */
3643 Concrete::Bezier z0new
= k0z
;
3644 Concrete::Bezier z1new
= k0z
+ k1z
/ 3;
3645 Concrete::Bezier z2new
= k0z
+ ( 2 * k1z
+ k2z
) / 3;
3646 Concrete::Bezier z3new
= k0z
+ k1z
+ k2z
+ k3z
;
3648 /* Now, reverse time again! */
3650 /* In case the path was logically straight, we don't want to produce a path which is not
3651 * logically straight. Of course, it is a bit inefficient to take this into account now,
3652 * but the code becomes very intuitive.
3655 p1
= new Concrete::PathPoint3D( x3new
.offtype
< 0, -3 >( ), y3new
.offtype
< 0, -3 >( ), z3new
.offtype
< 0, -3 >( ) );
3656 if( (*i1
)->front_
!= (*i1
)->mid_
|| (*i2
)->rear_
!= (*i2
)->mid_
)
3658 p1
->front_
= new Concrete::Coords3D( x2new
.offtype
< 0, -3 >( ), y2new
.offtype
< 0, -3 >( ), z2new
.offtype
< 0, -3 >( ) );
3661 p2
= new Concrete::PathPoint3D( x0new
.offtype
< 0, -3 >( ), y0new
.offtype
< 0, -3 >( ), z0new
.offtype
< 0, -3 >( ) ); /* This point must be the same as before. */
3662 if( (*i1
)->front_
!= (*i1
)->mid_
|| (*i2
)->rear_
!= (*i2
)->mid_
)
3664 p2
->rear_
= new Concrete::Coords3D( x1new
.offtype
< 0, -3 >( ), y1new
.offtype
< 0, -3 >( ), z1new
.offtype
< 0, -3 >( ) );
3666 if( (*i2
)->front_
!= (*i2
)->mid_
)
3668 p2
->front_
= new Concrete::Coords3D( *(*i2
)->front_
);
3672 /* At this point we know that the rest of this segment shall be included, since the cuts are not on the
3676 for( ; t
+ 1 < gt2
; t
+= Concrete::UNIT_TIME
)
3678 res
->push_back( p1
);
3686 p2
= new Concrete::PathPoint3D( **i2
);
3689 /* Remember to delete p1 and p2 if unused! */
3691 Concrete::Time t2
= gt2
- t
;
3694 res
->push_back( p1
);
3695 res
->push_back( p2
);
3699 Concrete::Length xn1
= p1
->rear_
->x_
; /* "n" for negative index. We save these values now, so that we can delete p1 and p2 soon. */
3700 Concrete::Length yn1
= p1
->rear_
->y_
;
3701 Concrete::Length zn1
= p1
->rear_
->z_
;
3702 bool doBackHandle
= p1
->rear_
!= p1
->mid_
;
3704 Concrete::Bezier x0
= p1
->mid_
->x_
.offtype
< 0, 3 >( );
3705 Concrete::Bezier y0
= p1
->mid_
->y_
.offtype
< 0, 3 >( );
3706 Concrete::Bezier z0
= p1
->mid_
->z_
.offtype
< 0, 3 >( );
3707 Concrete::Bezier x1
= p1
->front_
->x_
.offtype
< 0, 3 >( );
3708 Concrete::Bezier y1
= p1
->front_
->y_
.offtype
< 0, 3 >( );
3709 Concrete::Bezier z1
= p1
->front_
->z_
.offtype
< 0, 3 >( );
3710 Concrete::Bezier x2
= p2
->rear_
->x_
.offtype
< 0, 3 >( );
3711 Concrete::Bezier y2
= p2
->rear_
->y_
.offtype
< 0, 3 >( );
3712 Concrete::Bezier z2
= p2
->rear_
->z_
.offtype
< 0, 3 >( );
3713 Concrete::Bezier x3
= p2
->mid_
->x_
.offtype
< 0, 3 >( );
3714 Concrete::Bezier y3
= p2
->mid_
->y_
.offtype
< 0, 3 >( );
3715 Concrete::Bezier z3
= p2
->mid_
->z_
.offtype
< 0, 3 >( );
3716 bool doInHandles
= p1
->front_
!= p1
->mid_
|| p2
->rear_
!= p2
->mid_
;
3721 /* Compute new x polynomial using variable change t = t2 * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
3723 Concrete::Bezier k0x
= x0
;
3724 Concrete::Bezier k1x
= 3 * t2
* ( x1
- x0
).offtype
< 0, 1 >( );
3725 Concrete::Bezier k2x
= 3 * t2
* t2
* ( x0
- 2 * x1
+ x2
).offtype
< 0, 2 >( );
3726 Concrete::Bezier k3x
= t2
* t2
* t2
* ( -x0
+ 3 * x1
- 3 * x2
+ x3
).offtype
< 0, 3 >( );
3727 /* Compute new coefficients in the standard polynomial base */
3728 Concrete::Bezier x0new
= k0x
;
3729 Concrete::Bezier x1new
= k0x
+ k1x
/ 3;
3730 Concrete::Bezier x2new
= k0x
+ ( 2 * k1x
+ k2x
) / 3;
3731 Concrete::Bezier x3new
= k0x
+ k1x
+ k2x
+ k3x
;
3733 Concrete::Bezier k0y
= y0
;
3734 Concrete::Bezier k1y
= 3 * t2
* ( y1
- y0
).offtype
< 0, 1 >( );
3735 Concrete::Bezier k2y
= 3 * t2
* t2
* ( y0
- 2 * y1
+ y2
).offtype
< 0, 2 >( );
3736 Concrete::Bezier k3y
= t2
* t2
* t2
* ( -y0
+ 3 * y1
- 3 * y2
+ y3
).offtype
< 0, 3 >( );
3738 Concrete::Bezier y0new
= k0y
;
3739 Concrete::Bezier y1new
= k0y
+ k1y
/ 3;
3740 Concrete::Bezier y2new
= k0y
+ ( 2 * k1y
+ k2y
) / 3;
3741 Concrete::Bezier y3new
= k0y
+ k1y
+ k2y
+ k3y
;
3743 Concrete::Bezier k0z
= z0
;
3744 Concrete::Bezier k1z
= 3 * t2
* ( z1
- z0
).offtype
< 0, 1 >( );
3745 Concrete::Bezier k2z
= 3 * t2
* t2
* ( z0
- 2 * z1
+ z2
).offtype
< 0, 2 >( );
3746 Concrete::Bezier k3z
= t2
* t2
* t2
* ( -z0
+ 3 * z1
- 3 * z2
+ z3
).offtype
< 0, 3 >( );
3748 Concrete::Bezier z0new
= k0z
;
3749 Concrete::Bezier z1new
= k0z
+ k1z
/ 3;
3750 Concrete::Bezier z2new
= k0z
+ ( 2 * k1z
+ k2z
) / 3;
3751 Concrete::Bezier z3new
= k0z
+ k1z
+ k2z
+ k3z
;
3753 p1
= new Concrete::PathPoint3D( x0new
.offtype
< 0, -3 >( ), y0new
.offtype
< 0, -3 >( ), z0new
.offtype
< 0, -3 >( ) );
3756 p1
->front_
= new Concrete::Coords3D( x1new
.offtype
< 0, -3 >( ), y1new
.offtype
< 0, -3 >( ), z1new
.offtype
< 0, -3 >( ) );
3760 p1
->rear_
= new Concrete::Coords3D( xn1
, yn1
, zn1
);
3762 p2
= new Concrete::PathPoint3D( x3new
.offtype
< 0, -3 >( ), y3new
.offtype
< 0, -3 >( ), z3new
.offtype
< 0, -3 >( ) );
3765 p2
->rear_
= new Concrete::Coords3D( x2new
.offtype
< 0, -3 >( ), y2new
.offtype
< 0, -3 >( ), z2new
.offtype
< 0, -3 >( ) );
3768 res
->push_back( p1
);
3769 res
->push_back( p2
);
3773 return RefCountPtr
< const Lang::ElementaryPath3D
>( res
);
3777 RefCountPtr
< const Lang::ElementaryPath3D
>
3778 Lang::ElementaryPath3D::reverse( ) const
3780 /* Note that reversing a closed path is not quite as straight-forward as one might first think!
3783 Lang::ElementaryPath3D
* res
= new Lang::ElementaryPath3D
;
3789 const_iterator i
= begin( );
3792 return RefCountPtr
< const Lang::ElementaryPath3D
>( res
);
3797 // The first path point remains first!
3800 for( ; i
!= end( ); ++i
)
3802 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( *((*i
)->mid_
) ) );
3803 if( (*i
)->rear_
!= (*i
)->mid_
)
3805 newPoint
->front_
= new Concrete::Coords3D( *((*i
)->rear_
) );
3807 if( (*i
)->front_
!= (*i
)->mid_
)
3809 newPoint
->rear_
= new Concrete::Coords3D( *((*i
)->front_
) );
3811 res
->push_front( newPoint
);
3816 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( *((*i
)->mid_
) ) );
3817 if( (*i
)->rear_
!= (*i
)->mid_
)
3819 newPoint
->front_
= new Concrete::Coords3D( *((*i
)->rear_
) );
3821 if( (*i
)->front_
!= (*i
)->mid_
)
3823 newPoint
->rear_
= new Concrete::Coords3D( *((*i
)->front_
) );
3825 res
->push_front( newPoint
);
3828 return RefCountPtr
< const Lang::ElementaryPath3D
>( res
);
3831 RefCountPtr
< const Lang::ElementaryPath3D
>
3832 Lang::ElementaryPath3D::upsample( const Computation::Upsampler3D
& sampler
) const
3834 throw Exceptions::NotImplemented( "ElementaryPath3D::upsample" );
3838 Lang::ElementaryPath3D::show( std::ostream
& os
) const
3840 os
<< "Elementary 3D subpath with " << size( ) << " path points" ;
3844 Lang::ElementaryPath3D::getRepresentativePoints( const Lang::Transform3D
& tf
, Concrete::Coords3D
* p0
, Concrete::Coords3D
* p1
, Concrete::Coords3D
* p2
) const
3846 // We do this by selecting three points that spans a large triangle
3847 // This procedure may be overly expensive...
3848 // ... perhaps because it is hard to make it smart.
3850 Concrete::Area
bestArea( 0 );
3852 for( const_iterator i0
= begin( ); ; )
3854 Concrete::Coords3D
tmp0( (*i0
)->mid_
->transformed( tf
) );
3855 const_iterator i1
= i0
;
3859 Concrete::Coords3D
tmp1( (*i1
)->mid_
->transformed( tf
) );
3860 const_iterator i2
= i1
;
3862 for( ; i2
!= end( ); ++i2
)
3864 Concrete::Coords3D
tmp2( (*i2
)->mid_
->transformed( tf
) );
3865 Concrete::Area a
= Computation::triangleArea( tmp0
, tmp1
, tmp2
);
3874 --i2
; // now i2 points to the element before end
3881 --i1
; // now i1 points to the second last element
3891 Computation::SegmentPolynomialPair3D::kInit( )
3893 kxaD0_0
= polyCoeffs_a
.z0_
.x_
;
3894 kxaD0_1
= polyCoeffs_a
.z1_
.x_
.offtype
< 0, 1 >( );
3895 kxaD0_2
= polyCoeffs_a
.z2_
.x_
.offtype
< 0, 2 >( );
3896 kxaD0_3
= polyCoeffs_a
.z3_
.x_
.offtype
< 0, 3 >( );
3899 kxaD1_1
= 2 * kxaD0_2
;
3900 kxaD1_2
= 3 * kxaD0_3
;
3903 kxaD2_1
= 2 * kxaD1_2
;
3905 kyaD0_0
= polyCoeffs_a
.z0_
.y_
;
3906 kyaD0_1
= polyCoeffs_a
.z1_
.y_
.offtype
< 0, 1 >( );
3907 kyaD0_2
= polyCoeffs_a
.z2_
.y_
.offtype
< 0, 2 >( );
3908 kyaD0_3
= polyCoeffs_a
.z3_
.y_
.offtype
< 0, 3 >( );
3911 kyaD1_1
= 2 * kyaD0_2
;
3912 kyaD1_2
= 3 * kyaD0_3
;
3915 kyaD2_1
= 2 * kyaD1_2
;
3917 kzaD0_0
= polyCoeffs_a
.z0_
.z_
;
3918 kzaD0_1
= polyCoeffs_a
.z1_
.z_
.offtype
< 0, 1 >( );
3919 kzaD0_2
= polyCoeffs_a
.z2_
.z_
.offtype
< 0, 2 >( );
3920 kzaD0_3
= polyCoeffs_a
.z3_
.z_
.offtype
< 0, 3 >( );
3923 kzaD1_1
= 2 * kzaD0_2
;
3924 kzaD1_2
= 3 * kzaD0_3
;
3927 kzaD2_1
= 2 * kzaD1_2
;
3929 kxbD0_0
= polyCoeffs_b
.z0_
.x_
;
3930 kxbD0_1
= polyCoeffs_b
.z1_
.x_
.offtype
< 0, 1 >( );
3931 kxbD0_2
= polyCoeffs_b
.z2_
.x_
.offtype
< 0, 2 >( );
3932 kxbD0_3
= polyCoeffs_b
.z3_
.x_
.offtype
< 0, 3 >( );
3935 kxbD1_1
= 2 * kxbD0_2
;
3936 kxbD1_2
= 3 * kxbD0_3
;
3939 kxbD2_1
= 2 * kxbD1_2
;
3941 kybD0_0
= polyCoeffs_b
.z0_
.y_
;
3942 kybD0_1
= polyCoeffs_b
.z1_
.y_
.offtype
< 0, 1 >( );
3943 kybD0_2
= polyCoeffs_b
.z2_
.y_
.offtype
< 0, 2 >( );
3944 kybD0_3
= polyCoeffs_b
.z3_
.y_
.offtype
< 0, 3 >( );
3947 kybD1_1
= 2 * kybD0_2
;
3948 kybD1_2
= 3 * kybD0_3
;
3951 kybD2_1
= 2 * kybD1_2
;
3953 kzbD0_0
= polyCoeffs_b
.z0_
.z_
;
3954 kzbD0_1
= polyCoeffs_b
.z1_
.z_
.offtype
< 0, 1 >( );
3955 kzbD0_2
= polyCoeffs_b
.z2_
.z_
.offtype
< 0, 2 >( );
3956 kzbD0_3
= polyCoeffs_b
.z3_
.z_
.offtype
< 0, 3 >( );
3959 kzbD1_1
= 2 * kzbD0_2
;
3960 kzbD1_2
= 3 * kzbD0_3
;
3963 kzbD2_1
= 2 * kzbD1_2
;
3966 Computation::SegmentPolynomialPair3D::SegmentPolynomialPair3D( Concrete::Time steps_a
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_a
, Concrete::Time steps_b
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_b
)
3967 : polyCoeffs_a( seg_a
), straightLineSeg_a_( false ), polyCoeffs_b( seg_b
), straightLineSeg_b_( false ),
3968 steps_a_( steps_a
), steps_b_( steps_b
)
3972 maxSpeed_a_
= max( max( hypotPhysical( seg_a
.p1_
.x_
- seg_a
.p0_
.x_
, seg_a
.p1_
.y_
- seg_a
.p0_
.y_
, seg_a
.p1_
.z_
- seg_a
.p0_
.z_
),
3973 hypotPhysical( seg_a
.p2_
.x_
- seg_a
.p1_
.x_
, seg_a
.p2_
.y_
- seg_a
.p1_
.y_
, seg_a
.p2_
.z_
- seg_a
.p1_
.z_
) ),
3974 hypotPhysical( seg_a
.p3_
.x_
- seg_a
.p2_
.x_
, seg_a
.p3_
.y_
- seg_a
.p2_
.y_
, seg_a
.p3_
.z_
- seg_a
.p2_
.z_
) ).offtype
< 0, 1 >( );
3975 maxSpeed_b_
= max( max( hypotPhysical( seg_b
.p1_
.x_
- seg_b
.p0_
.x_
, seg_b
.p1_
.y_
- seg_b
.p0_
.y_
, seg_b
.p1_
.z_
- seg_b
.p0_
.z_
),
3976 hypotPhysical( seg_b
.p2_
.x_
- seg_b
.p1_
.x_
, seg_b
.p2_
.y_
- seg_b
.p1_
.y_
, seg_b
.p2_
.z_
- seg_b
.p1_
.z_
) ),
3977 hypotPhysical( seg_b
.p3_
.x_
- seg_b
.p2_
.x_
, seg_b
.p3_
.y_
- seg_b
.p2_
.y_
, seg_b
.p3_
.z_
- seg_b
.p2_
.z_
) ).offtype
< 0, 1 >( );
3980 Computation::SegmentPolynomialPair3D::SegmentPolynomialPair3D( Concrete::Time steps_a
, const Concrete::Coords3D
& seg_a_p0
, const Concrete::Coords3D
& seg_a_p3
, Concrete::Time steps_b
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_b
)
3981 : polyCoeffs_a( Bezier::ControlPoints
< Concrete::Coords3D
>( seg_a_p0
, seg_a_p0
, seg_a_p3
, seg_a_p3
) ), straightLineSeg_a_( true ), polyCoeffs_b( seg_b
), straightLineSeg_b_( false ),
3982 steps_a_( steps_a
), steps_b_( steps_b
)
3986 maxSpeed_a_
= hypotPhysical( seg_a_p3
.x_
- seg_a_p0
.x_
, seg_a_p3
.y_
- seg_a_p0
.y_
, seg_a_p3
.z_
- seg_a_p0
.z_
).offtype
< 0, 1 >( );
3987 maxSpeed_b_
= max( max( hypotPhysical( seg_b
.p1_
.x_
- seg_b
.p0_
.x_
, seg_b
.p1_
.y_
- seg_b
.p0_
.y_
, seg_b
.p1_
.z_
- seg_b
.p0_
.z_
),
3988 hypotPhysical( seg_b
.p2_
.x_
- seg_b
.p1_
.x_
, seg_b
.p2_
.y_
- seg_b
.p1_
.y_
, seg_b
.p2_
.z_
- seg_b
.p1_
.z_
) ),
3989 hypotPhysical( seg_b
.p3_
.x_
- seg_b
.p2_
.x_
, seg_b
.p3_
.y_
- seg_b
.p2_
.y_
, seg_b
.p3_
.z_
- seg_b
.p2_
.z_
) ).offtype
< 0, 1 >( );
3992 Computation::SegmentPolynomialPair3D::SegmentPolynomialPair3D( Concrete::Time steps_a
, const Bezier::ControlPoints
< Concrete::Coords3D
> & seg_a
, Concrete::Time steps_b
, const Concrete::Coords3D
& seg_b_p0
, const Concrete::Coords3D
& seg_b_p3
)
3993 : polyCoeffs_a( seg_a
), straightLineSeg_a_( false ), polyCoeffs_b( Bezier::ControlPoints
< Concrete::Coords3D
>( seg_b_p0
, seg_b_p0
, seg_b_p3
, seg_b_p3
) ), straightLineSeg_b_( true ),
3994 steps_a_( steps_a
), steps_b_( steps_b
)
3998 maxSpeed_a_
= max( max( hypotPhysical( seg_a
.p1_
.x_
- seg_a
.p0_
.x_
, seg_a
.p1_
.y_
- seg_a
.p0_
.y_
, seg_a
.p1_
.z_
- seg_a
.p0_
.z_
),
3999 hypotPhysical( seg_a
.p2_
.x_
- seg_a
.p1_
.x_
, seg_a
.p2_
.y_
- seg_a
.p1_
.y_
, seg_a
.p2_
.z_
- seg_a
.p1_
.z_
) ),
4000 hypotPhysical( seg_a
.p3_
.x_
- seg_a
.p2_
.x_
, seg_a
.p3_
.y_
- seg_a
.p2_
.y_
, seg_a
.p3_
.z_
- seg_a
.p2_
.z_
) ).offtype
< 0, 1 >( );
4001 maxSpeed_b_
= hypotPhysical( seg_b_p3
.x_
- seg_b_p0
.x_
, seg_b_p3
.y_
- seg_b_p0
.y_
, seg_b_p3
.z_
- seg_b_p0
.z_
).offtype
< 0, 1 >( );
4005 Computation::SegmentPolynomialPair3D::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
4009 Concrete::LengthSquared
last_f( HUGE_VAL
);
4011 const double GRID
= 7;
4012 const Concrete::Time STEP_A
= ( t_ahigh
- t_alow
) / GRID
;
4013 const Concrete::Time STEP_B
= ( t_bhigh
- t_blow
) / GRID
;
4014 for( Concrete::Time tta
= t_alow
+ 0.5 * STEP_A
; tta
< t_ahigh
; tta
+= STEP_A
)
4016 for( Concrete::Time ttb
= t_blow
+ 0.5 * STEP_B
; ttb
< t_bhigh
; ttb
+= STEP_B
)
4018 Concrete::LengthSquared tmp
= squaredDistanceAt( tta
, ttb
);
4028 Concrete::Time last_ta
= - Concrete::UNIT_TIME
;
4029 Concrete::Time last_tb
= - Concrete::UNIT_TIME
;
4031 while( ta
< last_ta
- t_atol
|| ta
> last_ta
+ t_atol
||
4032 tb
< last_tb
- t_btol
|| tb
> last_tb
+ t_btol
)
4036 /* The derivatives below are actually computed for the objective divided by two
4037 * but since we will divide one by the other, it doesn't matter.
4039 Physical
< 2, -1 > objectiveD1a( 0 );
4040 Physical
< 2, -1 > objectiveD1b( 0 );
4041 Physical
< 2, -2 > objectiveD2aa( 0 );
4042 Physical
< 2, -2 > objectiveD2ab( 0 );
4043 Physical
< 2, -2 > objectiveD2bb( 0 );
4047 Physical
< 1, 0 > faD0
= kxaD0_0
+ ta
* ( kxaD0_1
+ ta
* ( kxaD0_2
+ ta
* kxaD0_3
) );
4048 Physical
< 1, 0 > fbD0
= kxbD0_0
+ tb
* ( kxbD0_1
+ tb
* ( kxbD0_2
+ tb
* kxbD0_3
) );
4049 Physical
< 1, -1 > faD1
= kxaD1_0
+ ta
* ( kxaD1_1
+ ta
* kxaD1_2
);
4050 Physical
< 1, -1 > fbD1
= kxbD1_0
+ tb
* ( kxbD1_1
+ tb
* kxbD1_2
);
4051 Physical
< 1, -2 > faD2
= kxaD2_0
+ ta
* kxaD2_1
;
4052 Physical
< 1, -2 > fbD2
= kxbD2_0
+ tb
* kxbD2_1
;
4054 Physical
< 1, 0 > deltaD0
= faD0
- fbD0
;
4056 objectiveD1a
+= deltaD0
* faD1
;
4057 objectiveD1b
+= - deltaD0
* fbD1
;
4058 objectiveD2aa
+= faD1
* faD1
+ deltaD0
* faD2
;
4059 objectiveD2bb
+= fbD1
* fbD1
- deltaD0
* fbD2
;
4060 objectiveD2ab
+= - faD1
* fbD1
;
4065 Physical
< 1, 0 > faD0
= kyaD0_0
+ ta
* ( kyaD0_1
+ ta
* ( kyaD0_2
+ ta
* kyaD0_3
) );
4066 Physical
< 1, 0 > fbD0
= kybD0_0
+ tb
* ( kybD0_1
+ tb
* ( kybD0_2
+ tb
* kybD0_3
) );
4067 Physical
< 1, -1 > faD1
= kyaD1_0
+ ta
* ( kyaD1_1
+ ta
* kyaD1_2
);
4068 Physical
< 1, -1 > fbD1
= kybD1_0
+ tb
* ( kybD1_1
+ tb
* kybD1_2
);
4069 Physical
< 1, -2 > faD2
= kyaD2_0
+ ta
* kyaD2_1
;
4070 Physical
< 1, -2 > fbD2
= kybD2_0
+ tb
* kybD2_1
;
4072 Physical
< 1, 0 > deltaD0
= faD0
- fbD0
;
4074 objectiveD1a
+= deltaD0
* faD1
;
4075 objectiveD1b
+= - deltaD0
* fbD1
;
4076 objectiveD2aa
+= faD1
* faD1
+ deltaD0
* faD2
;
4077 objectiveD2bb
+= fbD1
* fbD1
- deltaD0
* fbD2
;
4078 objectiveD2ab
+= - faD1
* fbD1
;
4083 Physical
< 1, 0 > faD0
= kzaD0_0
+ ta
* ( kzaD0_1
+ ta
* ( kzaD0_2
+ ta
* kzaD0_3
) );
4084 Physical
< 1, 0 > fbD0
= kzbD0_0
+ tb
* ( kzbD0_1
+ tb
* ( kzbD0_2
+ tb
* kzbD0_3
) );
4085 Physical
< 1, -1 > faD1
= kzaD1_0
+ ta
* ( kzaD1_1
+ ta
* kzaD1_2
);
4086 Physical
< 1, -1 > fbD1
= kzbD1_0
+ tb
* ( kzbD1_1
+ tb
* kzbD1_2
);
4087 Physical
< 1, -2 > faD2
= kzaD2_0
+ ta
* kzaD2_1
;
4088 Physical
< 1, -2 > fbD2
= kzbD2_0
+ tb
* kzbD2_1
;
4090 Physical
< 1, 0 > deltaD0
= faD0
- fbD0
;
4092 objectiveD1a
+= deltaD0
* faD1
;
4093 objectiveD1b
+= - deltaD0
* fbD1
;
4094 objectiveD2aa
+= faD1
* faD1
+ deltaD0
* faD2
;
4095 objectiveD2bb
+= fbD1
* fbD1
- deltaD0
* fbD2
;
4096 objectiveD2ab
+= - faD1
* fbD1
;
4099 Concrete::Time step_a
;
4100 Concrete::Time step_b
;
4101 if( objectiveD2aa
<= Physical
< 2, -2 >( 0 ) || objectiveD2bb
<= Physical
< 2, -2 >( 0 ) || objectiveD2ab
* objectiveD2ab
>= objectiveD2aa
* objectiveD2bb
)
4103 /* The minimization problem is not convex, locally.
4104 * Knowing that we have examined a grid of candidate points, we stop the minimization here.
4110 Physical
< -4, 4 > invDet
= 1. / ( objectiveD2aa
* objectiveD2bb
- objectiveD2ab
* objectiveD2ab
);
4111 // here's the division where the missing factors of 2 cancel.
4112 step_a
= - invDet
* ( objectiveD2bb
* objectiveD1a
- objectiveD2ab
* objectiveD1b
);
4113 step_b
= - invDet
* ( - objectiveD2ab
* objectiveD1a
+ objectiveD2aa
* objectiveD1b
);
4116 bool onBound
= false;
4120 if( ta
+ alpha
* step_a
< t_alow
)
4122 alpha
= ( t_alow
- ta
) / step_a
;
4125 else if( ta
+ alpha
* step_a
> t_ahigh
)
4127 alpha
= ( t_ahigh
- ta
) / step_a
;
4131 if( tb
+ alpha
* step_b
< t_blow
)
4133 alpha
= ( t_blow
- tb
) / step_b
;
4136 else if( tb
+ alpha
* step_b
> t_bhigh
)
4138 alpha
= ( t_bhigh
- tb
) / step_b
;
4146 Concrete::LengthSquared f
= squaredDistanceAt( ta
+ step_a
, tb
+ step_b
);
4147 while( f
> last_f
&& ( step_a
.abs( ) > t_atol
|| step_b
.abs( ) > t_btol
) )
4149 step_a
= step_a
* 0.5;
4150 step_b
= step_b
* 0.5;
4151 f
= squaredDistanceAt( ta
+ step_a
, tb
+ step_b
);
4166 return distanceAt( ta
, tb
) < distance_tol
;
4169 Concrete::LengthSquared
4170 Computation::SegmentPolynomialPair3D::squaredDistanceAt( Concrete::Time ta
, Concrete::Time tb
) const
4172 Concrete::Length fxaD0
= kxaD0_0
+ ta
* ( kxaD0_1
+ ta
* ( kxaD0_2
+ ta
* kxaD0_3
) );
4173 Concrete::Length fyaD0
= kyaD0_0
+ ta
* ( kyaD0_1
+ ta
* ( kyaD0_2
+ ta
* kyaD0_3
) );
4174 Concrete::Length fzaD0
= kzaD0_0
+ ta
* ( kzaD0_1
+ ta
* ( kzaD0_2
+ ta
* kzaD0_3
) );
4175 Concrete::Length fxbD0
= kxbD0_0
+ tb
* ( kxbD0_1
+ tb
* ( kxbD0_2
+ tb
* kxbD0_3
) );
4176 Concrete::Length fybD0
= kybD0_0
+ tb
* ( kybD0_1
+ tb
* ( kybD0_2
+ tb
* kybD0_3
) );
4177 Concrete::Length fzbD0
= kzbD0_0
+ tb
* ( kzbD0_1
+ tb
* ( kzbD0_2
+ tb
* kzbD0_3
) );
4178 Concrete::Length deltax
= fxaD0
- fxbD0
;
4179 Concrete::Length deltay
= fyaD0
- fybD0
;
4180 Concrete::Length deltaz
= fzaD0
- fzbD0
;
4181 return deltax
* deltax
+ deltay
* deltay
+ deltaz
* deltaz
;
4185 Computation::SegmentPolynomialPair3D::distanceAt( Concrete::Time ta
, Concrete::Time tb
) const
4187 Concrete::Length fxaD0
= kxaD0_0
+ ta
* ( kxaD0_1
+ ta
* ( kxaD0_2
+ ta
* kxaD0_3
) );
4188 Concrete::Length fyaD0
= kyaD0_0
+ ta
* ( kyaD0_1
+ ta
* ( kyaD0_2
+ ta
* kyaD0_3
) );
4189 Concrete::Length fzaD0
= kzaD0_0
+ ta
* ( kzaD0_1
+ ta
* ( kzaD0_2
+ ta
* kzaD0_3
) );
4190 Concrete::Length fxbD0
= kxbD0_0
+ tb
* ( kxbD0_1
+ tb
* ( kxbD0_2
+ tb
* kxbD0_3
) );
4191 Concrete::Length fybD0
= kybD0_0
+ tb
* ( kybD0_1
+ tb
* ( kybD0_2
+ tb
* kybD0_3
) );
4192 Concrete::Length fzbD0
= kzbD0_0
+ tb
* ( kzbD0_1
+ tb
* ( kzbD0_2
+ tb
* kzbD0_3
) );
4193 Concrete::Length deltax
= fxaD0
- fxbD0
;
4194 Concrete::Length deltay
= fyaD0
- fybD0
;
4195 Concrete::Length deltaz
= fzaD0
- fzbD0
;
4196 return hypotPhysical( deltax
, deltay
, deltaz
);
4199 Bezier::ControlPoints
< Concrete::Coords3D
>
4200 Computation::SegmentPolynomialPair3D::getControls_a( const Concrete::Time t_low
, const Concrete::Time t_high
) const
4202 return Bezier::ControlPoints
< Concrete::Coords3D
>( polyCoeffs_a
.subSection( t_low
.offtype
< 0, 1 >( ), t_high
.offtype
< 0, 1 >( ) ) );
4205 Bezier::ControlPoints
< Concrete::Coords3D
>
4206 Computation::SegmentPolynomialPair3D::getControls_b( const Concrete::Time t_low
, const Concrete::Time t_high
) const
4208 return Bezier::ControlPoints
< Concrete::Coords3D
>( polyCoeffs_b
.subSection( t_low
.offtype
< 0, 1 >( ), t_high
.offtype
< 0, 1 >( ) ) );
4211 Computation::SegmentSectionPair3D::SegmentSectionPair3D( const SegmentPolynomialPair3D
* _baseSegs
, Concrete::Time _t_a0
, Concrete::Time _t_a1
, Concrete::Time _t_b0
, Concrete::Time _t_b1
)
4212 : controls_a( _baseSegs
->getControls_a( _t_a0
, _t_a1
) ), controls_b( _baseSegs
->getControls_b( _t_b0
, _t_b1
) ),
4213 baseSegs( _baseSegs
), t_a0( _t_a0
), t_a1( _t_a1
), t_b0( _t_b0
), t_b1( _t_b1
)
4216 Computation::SegmentSectionPair3D
*
4217 Computation::SegmentSectionPair3D::cutAfterAfter( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const
4219 return new SegmentSectionPair3D( baseSegs
, t_a0
, ta
- tol_a
, t_b0
, tb
- tol_b
);
4222 Computation::SegmentSectionPair3D
*
4223 Computation::SegmentSectionPair3D::cutAfterBefore( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const
4225 return new SegmentSectionPair3D( baseSegs
, t_a0
, ta
- tol_a
, tb
+ tol_b
, t_b1
);
4228 Computation::SegmentSectionPair3D
*
4229 Computation::SegmentSectionPair3D::cutBeforeAfter( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const
4231 return new SegmentSectionPair3D( baseSegs
, ta
+ tol_a
, t_a1
, t_b0
, tb
- tol_b
);
4234 Computation::SegmentSectionPair3D
*
4235 Computation::SegmentSectionPair3D::cutBeforeBefore( Concrete::Time ta
, Concrete::Time tb
, Concrete::Time tol_a
, Concrete::Time tol_b
) const
4237 return new SegmentSectionPair3D( baseSegs
, ta
+ tol_a
, t_a1
, tb
+ tol_b
, t_b1
);
4241 Computation::SegmentSectionPair3D::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
4243 Concrete::Time da
= 0.01 * ( t_a1
- t_a0
);
4244 Concrete::Time db
= 0.01 * ( t_b1
- t_b0
);
4245 if( baseSegs
->splitTimes( dst_a
, dst_b
,
4246 t_a0
, t_a1
, t_tol_a
,
4247 t_b0
, t_b1
, t_tol_b
,
4250 /* An intersection was found to withing tolerances. Do not adjust for robust convergence! */
4254 /* For robust convergence, make sure we don't return split times that are too close to the corners of these sections. */
4255 if( *dst_a
< t_a0
+ da
&& *dst_b
< t_b0
+ db
)
4260 else if( *dst_a
< t_a0
+ da
&& *dst_b
> t_b1
- db
)
4265 else if( *dst_a
> t_a1
- da
&& *dst_b
< t_b0
+ db
)
4270 else if( *dst_a
> t_a1
- da
&& *dst_b
> t_b1
- db
)
4279 Computation::SegmentSectionPair3D::globalTime_a( Concrete::Time t
) const
4281 return steps_a( ) + t
;
4285 Computation::SegmentSectionPair3D::distanceAt( Concrete::Time ta
, Concrete::Time tb
) const
4287 return baseSegs
->distanceAt( ta
, tb
);
4290 Concrete::LengthSquared
4291 Computation::SegmentSectionPair3D::squaredDistanceAt( Concrete::Time ta
, Concrete::Time tb
) const
4293 return baseSegs
->squaredDistanceAt( ta
, tb
);
4297 Computation::SegmentSectionPair3D::convexHullSeparatedBy( Concrete::LengthSquared sep
) const
4299 hull_dist_
= Concrete::LengthSquared( 0 );
4300 /* Even though there is a chance that the eight doubles that describe the generators are
4301 * stored densly, starting at & controls_a, the copying we do here takes no time compared
4302 * to the distance computation itself.
4304 const size_t N
= 4; /* Maximum number of generators per polytope. */
4305 const size_t DIM
= 3; /* Dimension of each generator. */
4306 double ga
[ N
* DIM
]; /* Allocate data on stack. */
4307 double gb
[ N
* DIM
];
4308 size_t na
; /* Number of generators used. */
4310 /* Initialize generators for each polytope. */
4311 if( baseSegs
->straightLineSeg_a( ) )
4314 controls_a
.getEndpoints( & ga
[0] );
4319 controls_a
.get( & ga
[0] );
4321 if( baseSegs
->straightLineSeg_b( ) )
4324 controls_b
.getEndpoints( & gb
[0] );
4329 controls_b
.get( & gb
[0] );
4332 double double_distSquaredLow
;
4333 double double_distSquaredHigh
;
4334 QPSolverStatus status
;
4335 Computation::polytope_distance_generator_form( DIM
,
4338 & double_distSquaredLow
, & double_distSquaredHigh
,
4339 Concrete::LengthSquared::offtype( sep
), -1,
4341 0.5 * Concrete::Length::offtype( Computation::theDistanceTol
), /* Lagrange multiplier tolerance. */
4343 0, 0, /* We don't care about where the optimum is */
4345 if( ! status
.code
== Computation::QP_OK
)
4347 const char * binaryFilename
= "qp-fail-data.binary";
4348 Computation::polytope_distance_generator_form_write_binary_data( binaryFilename
,
4352 std::ostringstream msg
;
4353 msg
<< "QP solver status: " << status
.code_str( ) << ", with reason: " << status
.sub_str( )
4354 << ", binary data was written to \"" << binaryFilename
<< "\"." ;
4355 throw Exceptions::InternalError( strrefdup( msg
) );
4357 Concrete::LengthSquared
distSquaredLow( double_distSquaredLow
);
4358 Concrete::LengthSquared
distSquaredHigh( double_distSquaredHigh
);
4359 if( distSquaredLow
>= sep
)
4363 hull_dist_
= distSquaredHigh
;
4368 Computation::SegmentSectionPair3D::update_t_a1( Concrete::Time t
)
4370 t_a1
= min( t_a1
, t
);
4374 Computation::SegmentSectionPair3D::isEmpty( ) const
4376 return t_a1
< t_a0
|| t_b1
< t_b0
;
4380 Computation::SegmentSectionPair3D::writeTimes( std::ostream
& os
) const
4382 os
<< Concrete::Time::offtype( t_a0
) << "--" << Concrete::Time::offtype( t_a1
) << ", "
4383 << Concrete::Time::offtype( t_b0
) << "--" << Concrete::Time::offtype( t_b1
) << std::endl
;
4388 Computation::SegmentSectionPair3D::maxSpeed_a( ) const
4390 return baseSegs
->maxSpeed_a( );
4394 Computation::SegmentSectionPair3D::maxSpeed_b( ) const
4396 return baseSegs
->maxSpeed_b( );
4401 Lang::ElementaryPath3D::gcMark( Kernel::GCMarkedSet
& marked
)
4404 RefCountPtr
< const Lang::ElementaryPath2D
>
4405 Lang::ElementaryPath3D::make2D( Concrete::Length eyez
) const
4407 Lang::ElementaryPath2D
* res
= new Lang::ElementaryPath2D
;
4413 if( eyez
== HUGE_VAL
)
4415 for( const_iterator i
= begin( ); i
!= end( ); ++i
)
4417 Concrete::PathPoint2D
* newPoint
= new Concrete::PathPoint2D( (*i
)->mid_
->make2D( eyez
) );
4418 if( (*i
)->rear_
!= (*i
)->mid_
)
4420 newPoint
->rear_
= (*i
)->rear_
->make2D( eyez
);
4422 if( (*i
)->front_
!= (*i
)->mid_
)
4424 newPoint
->front_
= (*i
)->front_
->make2D( eyez
);
4426 res
->push_back( newPoint
);
4431 const_iterator i1
= begin( );
4432 const_iterator i2
= i1
;
4435 Lang::ElementaryPath2D cycleSegment
;
4436 Concrete::Coords2D
* passedRear_
= 0;
4440 const_iterator last
= end( );
4442 makeSegment2D( & cycleSegment
, & passedRear_
, (*last
)->mid_
, (*last
)->front_
, (*i1
)->rear_
, (*i1
)->mid_
, eyez
);
4445 for( ; i2
!= end( ); ++i1
, ++i2
)
4447 makeSegment2D( res
, & passedRear_
, (*i1
)->mid_
, (*i1
)->front_
, (*i2
)->rear_
, (*i2
)->mid_
, eyez
);
4452 if( passedRear_
!= 0 )
4454 cycleSegment
.front( )->rear_
= passedRear_
;
4456 while( ! cycleSegment
.empty( ) )
4458 res
->push_back( cycleSegment
.front( ) );
4459 cycleSegment
.pop_front( );
4464 Concrete::PathPoint2D
* newPoint
= new Concrete::PathPoint2D( (*i1
)->mid_
->make2D( eyez
) );
4465 if( passedRear_
!= 0 )
4467 newPoint
->rear_
= passedRear_
;
4469 if( (*i1
)->front_
!= (*i1
)->mid_
)
4471 newPoint
->front_
= (*i1
)->front_
->make2D( eyez
);
4473 res
->push_back( newPoint
);
4477 return RefCountPtr
< const Lang::ElementaryPath2D
>( res
);
4481 Lang::ElementaryPath3D::makeSegment2D( Lang::ElementaryPath2D
* dst
, Concrete::Coords2D
** passedRear_
, const Concrete::Coords3D
* _p0
, const Concrete::Coords3D
* _p1
, const Concrete::Coords3D
* _p2
, const Concrete::Coords3D
* _p3
, Concrete::Length eyez
)
4483 if( _p1
== _p0
&& _p2
== _p3
)
4485 Concrete::PathPoint2D
* newPoint
= new Concrete::PathPoint2D( _p0
->make2D( eyez
) );
4486 if( *passedRear_
!= 0 )
4488 newPoint
->rear_
= *passedRear_
;
4491 dst
->push_back( newPoint
);
4495 Concrete::Coords3D p0
= *_p0
;
4496 std::stack
< Concrete::Coords3D
> pointStack
;
4497 pointStack
.push( *_p3
);
4498 pointStack
.push( *_p2
);
4499 pointStack
.push( *_p1
);
4501 while( pointStack
.size( ) > 2 )
4503 Concrete::Coords3D p1
= pointStack
.top( );
4505 Concrete::Coords3D p2
= pointStack
.top( );
4507 Concrete::Coords3D p3
= pointStack
.top( );
4508 // Note that p3 is not popped!
4510 if( viewDistortionTooBig( p0
, p1
, p2
, p3
, eyez
, 0.003 ) )
4512 Bezier::PolyCoeffs
< Concrete::Coords3D
> poly( Bezier::ControlPoints
< Concrete::Coords3D
>( p0
, p1
, p2
, p3
) );
4513 Bezier::ControlPoints
< Concrete::Coords3D
> sub1( poly
.subSection( 0, 0.5 ) );
4514 Bezier::ControlPoints
< Concrete::Coords3D
> sub2( poly
.subSection( 0.5, 1 ) );
4515 pointStack
.push( sub2
.p2_
);
4516 pointStack
.push( sub2
.p1_
);
4517 pointStack
.push( sub2
.p0_
);
4518 pointStack
.push( sub1
.p2_
);
4519 pointStack
.push( sub1
.p1_
);
4523 Concrete::PathPoint2D
* newPoint
= new Concrete::PathPoint2D( p0
.make2D( eyez
) );
4524 if( *passedRear_
!= 0 )
4526 newPoint
->rear_
= *passedRear_
;
4528 newPoint
->front_
= p1
.make2D( eyez
);
4529 dst
->push_back( newPoint
);
4531 *passedRear_
= p2
.make2D( eyez
);
4539 Lang::ElementaryPath3D::viewDistortionTooBig( const Concrete::Coords3D
& p0
, const Concrete::Coords3D
& p1
, const Concrete::Coords3D
& p2
, const Concrete::Coords3D
& p3
, Concrete::Length eyez
, double _threshold
)
4541 Bezier::ControlPoints
< Concrete::Coords3D
> bezier3D( p0
, p1
, p2
, p3
);
4543 Concrete::Coords2D p02D
= p0
.make2DAutomatic( eyez
);
4544 Concrete::Coords2D p12D
= p1
.make2DAutomatic( eyez
);
4545 Concrete::Coords2D p22D
= p2
.make2DAutomatic( eyez
);
4546 Concrete::Coords2D p32D
= p3
.make2DAutomatic( eyez
);
4548 Bezier::ControlPoints
< Concrete::Coords2D
> bezier2D( p02D
, p12D
, p22D
, p32D
);
4550 Concrete::Length threshold
= 0;
4551 threshold
+= hypotPhysical( p12D
.x_
- p02D
.x_
, p12D
.y_
- p02D
.y_
);
4552 threshold
+= hypotPhysical( p22D
.x_
- p12D
.x_
, p22D
.y_
- p12D
.y_
);
4553 threshold
+= hypotPhysical( p32D
.x_
- p22D
.x_
, p32D
.y_
- p22D
.y_
);
4554 if( threshold
== Concrete::ZERO_LENGTH
)
4556 // The path is degenerate, so it isn't distorted at all.
4559 threshold
*= _threshold
;
4561 for( double t
= 0.25; t
< 0.9; t
+= 0.25 )
4563 Concrete::Coords2D p3D
= bezier3D
.point( t
).make2DAutomatic( eyez
);
4564 Concrete::Coords2D p2D
= bezier2D
.point( t
);
4565 if( hypotPhysical( p3D
.x_
- p2D
.x_
, p3D
.y_
- p2D
.y_
) > threshold
)
4574 Lang::ElementaryPath3D::dashifyIn2D( RefCountPtr
< const Lang::Group2D
> * res
, Concrete::Length eyez
, const RefCountPtr
< const Kernel::GraphicsState
> & metaState
) const
4581 Kernel::GraphicsState solidValue
;
4582 solidValue
.dash_
= Lang::THE_SOLID_DASH
;
4583 RefCountPtr
< const Kernel::GraphicsState
> solidState( new Kernel::GraphicsState( solidValue
, *metaState
) );
4585 Lang::Dash::Iterator dashi
= metaState
->dash_
->begin( );
4587 Lang::ElementaryPath3D newPath
;
4589 Concrete::Length remainingLength
= dashi
.getLength( );
4591 const_iterator i1
= begin( );
4593 Concrete::Coords3D
* passedRear_
= 0;
4594 if( (*i1
)->rear_
!= (*i1
)->mid_
)
4596 passedRear_
= new Concrete::Coords3D( *( (*i1
)->rear_
) );
4599 Concrete::Coords3D
* lastMid_
= 0;
4601 const_iterator i2
= i1
;
4603 for( ; i1
!= end( ); ++i1
, ++i2
)
4610 lastMid_
= new Concrete::Coords3D( *( (*i2
)->mid_
) );
4614 lastMid_
= new Concrete::Coords3D( *( (*i1
)->mid_
) );
4618 dashifySegment( res
, newPath
, remainingLength
, & passedRear_
,
4619 (*i1
)->mid_
, (*i1
)->front_
, (*i2
)->rear_
, (*i2
)->mid_
,
4627 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( lastMid_
);
4628 if( passedRear_
!= 0 )
4630 newPoint
->rear_
= passedRear_
;
4633 newPath
.push_back( newPoint
);
4634 *res
= RefCountPtr
< const Lang::Group2D
>( new Lang::GroupPair2D( RefCountPtr
< Lang::Drawable2D
>( new Lang::PaintedPath2D( solidState
,
4635 newPath
.make2D( eyez
),
4644 if( passedRear_
!= 0 )
4646 throw Exceptions::InternalError( "passedRear_ != 0, but the dash iterator is not on." );
4647 // If this wasn't an error situation, passedRear_ should be deleted here.
4649 if( ! newPath
.empty( ) )
4651 throw Exceptions::InternalError( "There are points in the newPath, but the dash iterator is not on." );
4657 Lang::ElementaryPath3D::dashifySegment( RefCountPtr
< const Lang::Group2D
> * res
,
4658 Lang::ElementaryPath3D
& newPath
,
4659 Concrete::Length remainingLength
,
4660 Concrete::Coords3D
** passedRear_
,
4661 const Concrete::Coords3D
* p0
, const Concrete::Coords3D
* p1
,
4662 const Concrete::Coords3D
* p2
, const Concrete::Coords3D
* p3
,
4663 Concrete::Length eyez
,
4664 const RefCountPtr
< const Kernel::GraphicsState
> & solidState
,
4665 Lang::Dash::Iterator
& dashi
)
4672 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( *p0
) );
4673 if( *passedRear_
!= 0 )
4675 newPoint
->rear_
= *passedRear_
;
4678 newPath
.push_back( newPoint
);
4679 // Note that on a straight segment, we don't set **passedRear_ to *p2
4682 const Concrete::Length segLength
= hypotPhysical( p0
->x_
- p3
->x_
, p0
->y_
- p3
->y_
, p0
->z_
- p3
->z_
);
4683 if( segLength
< remainingLength
)
4685 remainingLength
-= segLength
;
4689 Concrete::Length l
= remainingLength
;
4693 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( p0
->x_
+ ( p3
->x_
- p0
->x_
) * l
/ segLength
,
4694 p0
->y_
+ ( p3
->y_
- p0
->y_
) * l
/ segLength
,
4695 p0
->z_
+ ( p3
->z_
- p0
->z_
) * l
/ segLength
) );
4696 // there can be no passedRear_ here
4697 newPath
.push_back( newPoint
);
4698 *res
= RefCountPtr
< const Lang::Group2D
>( new Lang::GroupPair2D( RefCountPtr
< Lang::Drawable2D
>( new Lang::PaintedPath2D( solidState
,
4699 newPath
.make2D( eyez
),
4707 while( l
<= segLength
)
4710 if( ! newPath
.empty( ) )
4712 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( p0
->x_
+ ( p3
->x_
- p0
->x_
) * l
/ segLength
,
4713 p0
->y_
+ ( p3
->y_
- p0
->y_
) * l
/ segLength
,
4714 p0
->z_
+ ( p3
->z_
- p0
->z_
) * l
/ segLength
) );
4715 // there can be no passedRear_ here
4716 newPath
.push_back( newPoint
);
4717 *res
= RefCountPtr
< const Lang::Group2D
>( new Lang::GroupPair2D( RefCountPtr
< Lang::Drawable2D
>( new Lang::PaintedPath2D( solidState
,
4718 newPath
.make2D( eyez
),
4728 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( p0
->x_
+ ( p3
->x_
- p0
->x_
) * l
/ segLength
,
4729 p0
->y_
+ ( p3
->y_
- p0
->y_
) * l
/ segLength
,
4730 p0
->z_
+ ( p3
->z_
- p0
->z_
) * l
/ segLength
) );
4731 // there can be no passedRear_ here
4732 newPath
.push_back( newPoint
);
4735 l
+= dashi
.getLength( );
4738 remainingLength
= l
- segLength
;
4742 Bezier::ControlPoints
< Concrete::Coords3D
> controls( *p0
, *p1
, *p2
, *p3
);
4743 Bezier::PolyCoeffs
< Concrete::Coords3D
> poly( controls
);
4745 Concrete::Bezier x0
= p0
->x_
.offtype
< 0, 3 >( );
4746 Concrete::Bezier y0
= p0
->y_
.offtype
< 0, 3 >( );
4747 Concrete::Bezier z0
= p0
->z_
.offtype
< 0, 3 >( );
4748 Concrete::Bezier x1
= p1
->x_
.offtype
< 0, 3 >( );
4749 Concrete::Bezier y1
= p1
->y_
.offtype
< 0, 3 >( );
4750 Concrete::Bezier z1
= p1
->z_
.offtype
< 0, 3 >( );
4751 Concrete::Bezier x2
= p2
->x_
.offtype
< 0, 3 >( );
4752 Concrete::Bezier y2
= p2
->y_
.offtype
< 0, 3 >( );
4753 Concrete::Bezier z2
= p2
->z_
.offtype
< 0, 3 >( );
4754 Concrete::Bezier x3
= p3
->x_
.offtype
< 0, 3 >( );
4755 Concrete::Bezier y3
= p3
->y_
.offtype
< 0, 3 >( );
4756 Concrete::Bezier z3
= p3
->z_
.offtype
< 0, 3 >( );
4758 const Concrete::Length segLengthBound
= ( hypotPhysical( x1
-x0
, y1
-y0
, z1
-z0
) + hypotPhysical( x2
-x1
, y2
-y1
, z2
-z1
) + hypotPhysical( x3
-x2
, y3
-y2
, z3
-z2
) ).offtype
< 0, -3 >( );
4759 const Concrete::Time dt
= Shapes::computeDt( segLengthBound
);
4761 Concrete::Time t1
= 0;
4762 laterArcTime( x0
, y0
, z0
, x1
, y1
, z1
, x2
, y2
, z2
, x3
, y3
, z3
, & t1
, & remainingLength
, dt
);
4767 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( *p0
) );
4768 if( *passedRear_
!= 0 )
4770 newPoint
->rear_
= *passedRear_
;
4773 newPoint
->front_
= new Concrete::Coords3D( *p1
);
4774 newPath
.push_back( newPoint
);
4775 *passedRear_
= new Concrete::Coords3D( *p2
);
4782 Bezier::ControlPoints
< Concrete::Coords3D
> stroke( poly
.subSection( 0, t1
.offtype
< 0, 1 >( ) ) );
4784 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( *p0
) );
4785 if( *passedRear_
!= 0 )
4787 newPoint
->rear_
= *passedRear_
;
4790 newPoint
->front_
= new Concrete::Coords3D( stroke
.p1_
);
4791 newPath
.push_back( newPoint
);
4794 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( stroke
.p3_
) );
4795 newPoint
->rear_
= new Concrete::Coords3D( stroke
.p2_
);
4796 newPath
.push_back( newPoint
);
4798 *res
= RefCountPtr
< const Lang::Group2D
>( new Lang::GroupPair2D( RefCountPtr
< Lang::Drawable2D
>( new Lang::PaintedPath2D( solidState
,
4799 newPath
.make2D( eyez
),
4809 remainingLength
= dashi
.getLength( );
4810 Concrete::Time t0
= t1
;
4811 laterArcTime( x0
, y0
, z0
, x1
, y1
, z1
, x2
, y2
, z2
, x3
, y3
, z3
, & t1
, & remainingLength
, dt
);
4816 Bezier::ControlPoints
< Concrete::Coords3D
> stroke( poly
.subSection( t0
.offtype
< 0, 1 >( ), 1 ) );
4817 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( stroke
.p0_
) );
4818 newPoint
->front_
= new Concrete::Coords3D( stroke
.p1_
);
4819 newPath
.push_back( newPoint
);
4820 *passedRear_
= new Concrete::Coords3D( stroke
.p2_
);
4827 Bezier::ControlPoints
< Concrete::Coords3D
> stroke( poly
.subSection( t0
.offtype
< 0, 1 >( ), t1
.offtype
< 0, 1 >( ) ) );
4829 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( stroke
.p0_
) );
4830 // there can be no *passedRear_ here
4831 newPoint
->front_
= new Concrete::Coords3D( stroke
.p1_
);
4832 newPath
.push_back( newPoint
);
4835 Concrete::PathPoint3D
* newPoint
= new Concrete::PathPoint3D( new Concrete::Coords3D( stroke
.p3_
) );
4836 newPoint
->rear_
= new Concrete::Coords3D( stroke
.p2_
);
4837 newPath
.push_back( newPoint
);
4839 *res
= RefCountPtr
< const Lang::Group2D
>( new Lang::GroupPair2D( RefCountPtr
< Lang::Drawable2D
>( new Lang::PaintedPath2D( solidState
,
4840 newPath
.make2D( eyez
),
4850 Lang::ElementaryPath3D::laterArcTime( const Concrete::Bezier
& x0
, const Concrete::Bezier
& y0
, const Concrete::Bezier
& z0
, const Concrete::Bezier
& x1
, const Concrete::Bezier
& y1
, const Concrete::Bezier
& z1
, const Concrete::Bezier
& x2
, const Concrete::Bezier
& y2
, const Concrete::Bezier
& z2
, const Concrete::Bezier
& x3
, const Concrete::Bezier
& y3
, const Concrete::Bezier
& z3
, Concrete::Time
* _t
, Concrete::Length
* l
, const Concrete::Time
& dt
)
4852 Physical
< 1, -1 > tmpSum_l
= *l
/ dt
;
4853 for( Concrete::Time t
= *_t
; t
< 1; t
+= dt
)
4855 Concrete::Time tc
= Concrete::UNIT_TIME
- t
; /* complement to t */
4856 Physical
< 0, 2 > kv0
= -3 * tc
* tc
;
4857 Physical
< 0, 2 > kv1
= 3 * tc
* tc
- 6 * tc
* t
;
4858 Physical
< 0, 2 > kv2
= 6 * tc
* t
- 3 * t
* t
;
4859 Physical
< 0, 2 > kv3
= 3 * t
* t
;
4860 Concrete::Speed vx
= x0
* kv0
+ x1
* kv1
+ x2
* kv2
+ x3
* kv3
;
4861 Concrete::Speed vy
= y0
* kv0
+ y1
* kv1
+ y2
* kv2
+ y3
* kv3
;
4862 Concrete::Speed vz
= z0
* kv0
+ z1
* kv1
+ z2
* kv2
+ z3
* kv3
;
4864 Concrete::Length dl
= hypotPhysical( vx
, vy
, vz
).offtype
< 0, -1 >( );
4865 tmpSum_l
-= dl
.offtype
< 0, 1 >( );
4868 *_t
= t
- ( tmpSum_l
/ dl
.offtype
< 0, 1 >( ) ) * dt
;
4878 Lang::ElementaryPath3D::pushCloseDeleteOther( std::list
< Computation::SegmentSectionPair3D
* > * work
, Computation::SegmentSectionPair3D
* item
, Concrete::LengthSquared closeDist
)
4880 if( item
->isEmpty( )
4881 || item
->convexHullSeparatedBy( closeDist
) )
4887 work
->push_back( item
);