?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Update procedures
[shapes.git] / source / elementarypath3D.cc
blob63c1d55dc74ba335e42e946edf4845b160f21d3c
1 /* This file is part of Shapes.
2 *
3 * Shapes is free software: you can redistribute it and/or modify
4 * it under the terms of the GNU General Public License as published by
5 * the Free Software Foundation, either version 3 of the License, or
6 * any later version.
7 *
8 * Shapes is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with Shapes. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * Copyright 2008, 2009 Henrik Tidefelt
17 */
18
19 #include <cmath>
20
21 #include "shapestypes.h"
22 #include "shapesexceptions.h"
23 #include "astexpr.h"
24 #include "consts.h"
25 #include "globals.h"
26 #include "angleselect.h"
27 #include "bezier.h"
28 #include "trianglefunctions.h"
29 #include "quadratic_programs.h"
30
31 #include <complex>
32 #include <ctype.h>
33 #include <stack>
34
35 using namespace Shapes;
36 using namespace std;
37
38 namespace Bezier
39 {
40 template< >
41 void
42 ControlPoints< Concrete::Coords3D >::get( double * dst ) const
43 {
44 *dst = Concrete::Length::offtype( p0_.x_ );
45 ++dst;
46 *dst = Concrete::Length::offtype( p0_.y_ );
47 ++dst;
48 *dst = Concrete::Length::offtype( p0_.z_ );
49 ++dst;
50 *dst = Concrete::Length::offtype( p1_.x_ );
51 ++dst;
52 *dst = Concrete::Length::offtype( p1_.y_ );
53 ++dst;
54 *dst = Concrete::Length::offtype( p1_.z_ );
55 ++dst;
56 *dst = Concrete::Length::offtype( p2_.x_ );
57 ++dst;
58 *dst = Concrete::Length::offtype( p2_.y_ );
59 ++dst;
60 *dst = Concrete::Length::offtype( p2_.z_ );
61 ++dst;
62 *dst = Concrete::Length::offtype( p3_.x_ );
63 ++dst;
64 *dst = Concrete::Length::offtype( p3_.y_ );
65 ++dst;
66 *dst = Concrete::Length::offtype( p3_.z_ );
67 }
68
69 template< >
70 void
71 ControlPoints< Concrete::Coords3D >::getEndpoints( double * dst ) const
72 {
73 *dst = Concrete::Length::offtype( p0_.x_ );
74 ++dst;
75 *dst = Concrete::Length::offtype( p0_.y_ );
76 ++dst;
77 *dst = Concrete::Length::offtype( p0_.z_ );
78 ++dst;
79 *dst = Concrete::Length::offtype( p3_.x_ );
80 ++dst;
81 *dst = Concrete::Length::offtype( p3_.y_ );
82 ++dst;
83 *dst = Concrete::Length::offtype( p3_.z_ );
84 }
85 }
86
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 )
90 {
91 return d1 * d1 + d2 * d2 + d3 * d3;
92 }
93
94 Physical< 3, -6 >
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 )
98 {
99 return
100 vx * ( ay * jz - az * jy )
101 - vy * ( ax * jz - az * jx )
102 + vz * ( ax * jy - ay * jx );
103 }
104
105 Lang::ElementaryPath3D::ElementaryPath3D( )
106 { }
107
108 DISPATCHIMPL( ElementaryPath3D );
109
110 Lang::ElementaryPath3D::~ElementaryPath3D( )
111 { }
112
113 Kernel::VariableHandle
114 Lang::ElementaryPath3D::getField( const char * fieldID, const RefCountPtr< const Lang::Value > & selfRef ) const
115 {
116 return getField( fieldID, selfRef.down_cast< const Lang::ElementaryPath3D >( ) );
117 }
118
119 Kernel::VariableHandle
120 Lang::ElementaryPath3D::getField( const char * fieldID, const RefCountPtr< const Lang::ElementaryPath3D > & selfRef ) const
121 {
122 if( strcmp( fieldID, "begin" ) == 0 )
123 {
124 return Helpers::newValHandle( new Lang::PathSlider3D( selfRef, Concrete::ZERO_TIME ) );
125 }
126 if( strcmp( fieldID, "end" ) == 0 )
127 {
128 return Helpers::newValHandle( new Lang::PathSlider3D( selfRef, Concrete::Time( duration( ) ) ) );
129 }
130 if( strcmp( fieldID, "closed?" ) == 0 )
131 {
132 if( closed_ )
133 {
134 return Kernel::THE_TRUE_VARIABLE;
135 }
136 return Kernel::THE_FALSE_VARIABLE;
137 }
138 if( strcmp( fieldID, "null?" ) == 0 )
139 {
140 if( empty( ) )
141 {
142 return Kernel::THE_TRUE_VARIABLE;
143 }
144 return Kernel::THE_FALSE_VARIABLE;
145 }
146 throw Exceptions::NonExistentMember( getTypeName( ), fieldID );
147 }
148
149 RefCountPtr< const Lang::ElementaryPath3D >
150 Lang::ElementaryPath3D::elementaryTransformed( const Lang::Transform3D & tf ) const
151 {
152 Lang::ElementaryPath3D * res = new Lang::ElementaryPath3D;
153 if( closed_ )
154 {
155 res->close( );
156 }
157 for( const_iterator i = begin( ); i != end( ); ++i )
158 {
159 res->push_back( (*i)->transformed( tf ) );
160 }
161 return RefCountPtr< const Lang::ElementaryPath3D >( res );
162 }
163
164 RefCountPtr< const Lang::Path3D >
165 Lang::ElementaryPath3D::typed_transformed( const Lang::Transform3D & tf ) const
166 {
167 return elementaryTransformed( tf );
168 }
169
170 void
171 Lang::ElementaryPath3D::elementaryJob( std::stack< const Lang::Path3D * > * nodeStack, Lang::ElementaryPath3D * pth, Concrete::Coords3D * basePoint ) const
172 {
173 for( const_iterator i = begin( ); i != end( ); ++i )
174 {
175 pth->push_back( new Concrete::PathPoint3D( **i ) );
176 }
177 if( ! empty( ) )
178 {
179 *basePoint = *(back( )->mid_);
180 }
181 }
182
183 Concrete::Time
184 Lang::ElementaryPath3D::timeCheck( Concrete::Time t ) const
185 {
186 if( closed_ )
187 {
188 if( isinfPhysical( t ) )
189 {
190 throw Exceptions::OutOfRange( "The time argument must not be infinite for closed_ paths." );
191 }
192 Concrete::Time tMax = Concrete::Time( duration( ) );
193 t = modPhysical( t, tMax );
194 if( t < 0 )
195 {
196 t += tMax;
197 }
198 }
199 else
200 {
201 if( t < 0 )
202 {
203 throw Exceptions::OutOfRange( "The time argument must be non-negative for open paths." );
204 }
205 if( t >= HUGE_VAL )
206 {
207 t = Concrete::Time( duration( ) );
208 }
209 else if( t > Concrete::Time( duration( ) ) )
210 {
211 throw Exceptions::OutOfRange( "The time argument must not be greater than the duration of the path." );
212 }
213 }
214 return t;
215 }
216
217 void
218 Lang::ElementaryPath3D::findSegment( Concrete::Time t, Concrete::Time * tRes, const Concrete::PathPoint3D ** p1, const Concrete::PathPoint3D ** p2 ) const
219 {
220 t = timeCheck( t );
221 const_iterator i = begin( );
222 for( ; t >= Concrete::UNIT_TIME; t -= Concrete::UNIT_TIME, ++i )
223 { }
224 *tRes = t;
225 if( i == end( ) )
226 {
227 i = begin( );
228 }
229 *p1 = *i;
230
231 ++i;
232 if( i == end( ) )
233 {
234 i = begin( );
235 }
236 *p2 = *i;
237 }
238
239 void
240 Lang::ElementaryPath3D::findSegmentReverse( Concrete::Time t, Concrete::Time * tRes, const Concrete::PathPoint3D ** p1, const Concrete::PathPoint3D ** p2 ) const
241 {
242 t = timeCheck( t );
243
244 const_iterator i = begin( );
245 for( ; t > 0; t -= Concrete::UNIT_TIME, ++i )
246 { }
247 *tRes = t + 1;
248 if( i == end( ) )
249 {
250 i = begin( );
251 }
252 *p2 = *i;
253 if( i == begin( ) )
254 {
255 i = end( );
256 }
257 --i;
258 *p1 = *i;
259 }
260
261 RefCountPtr< const Lang::Coords3D >
262 Lang::ElementaryPath3D::point( Concrete::Time globalTime ) const
263 {
264 if( empty( ) )
265 {
266 throw Exceptions::OutOfRange( "The empty path has no points at all." );
267 }
268 Concrete::Time t;
269 const Concrete::PathPoint3D * p1;
270 const Concrete::PathPoint3D * p2;
271 findSegment( globalTime, &t, &p1, &p2 );
272
273 if( t == 0 )
274 {
275 return RefCountPtr< const Lang::Coords3D >( new Lang::Coords3D( * p1->mid_ ) );
276 }
277 if( t == 1 )
278 {
279 return RefCountPtr< const Lang::Coords3D >( new Lang::Coords3D( * p2->mid_ ) );
280 }
281
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 >( );
294
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;
303
304 return RefCountPtr< const Lang::Coords3D >( new Lang::Coords3D( x, y, z ) );
305 }
306
307 RefCountPtr< const Lang::Length >
308 Lang::ElementaryPath3D::speed( Concrete::Time globalTime ) const
309 {
310 if( empty( ) )
311 {
312 throw Exceptions::OutOfRange( "The empty path has no speed at all." );
313 }
314 Concrete::Time t;
315 const Concrete::PathPoint3D * p1;
316 const Concrete::PathPoint3D * p2;
317 findSegment( globalTime, &t, &p1, &p2 );
318
319 if( t <= Concrete::ZERO_TIME )
320 {
321 /* here, t = 0, tc == 1, so
322 */
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 >( ) ) );
336 }
337 if( t >= Concrete::UNIT_TIME )
338 {
339 throw Exceptions::InternalError( "t1 == 1 in speed calculation" );
340 }
341
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 >( );
354
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;
359 // kv3 = 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;
367
368 return RefCountPtr< const Lang::Length >( new Lang::Length( hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( ) ) );
369 }
370
371 RefCountPtr< const Lang::Length >
372 Lang::ElementaryPath3D::reverse_speed( Concrete::Time globalTime ) const
373 {
374 if( empty( ) )
375 {
376 throw Exceptions::OutOfRange( "The empty path has no speed at all." );
377 }
378 Concrete::Time t;
379 const Concrete::PathPoint3D * p1;
380 const Concrete::PathPoint3D * p2;
381 findSegmentReverse( globalTime, &t, &p1, &p2 );
382
383 if( t <= Concrete::ZERO_TIME )
384 {
385 throw Exceptions::InternalError( "t1 == 0 in reverse speed calculation" );
386 }
387 if( t >= Concrete::UNIT_TIME )
388 {
389 /* here, t = 1, tc == 0, so
390 */
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 >( ) ) );
404 }
405
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 >( );
418
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;
423 // kv3 = 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;
431
432 return RefCountPtr< const Lang::Length >( new Lang::Length( hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( ) ) );
433 }
434
435 RefCountPtr< const Lang::FloatTriple >
436 Lang::ElementaryPath3D::direction( Concrete::Time globalTime ) const
437 {
438 if( empty( ) )
439 {
440 throw Exceptions::OutOfRange( "The empty path has no directions at all." );
441 }
442 Concrete::Time t;
443 const Concrete::PathPoint3D * p1;
444 const Concrete::PathPoint3D * p2;
445 findSegment( globalTime, &t, &p1, &p2 );
446
447 Bezier::ControlPoints< Concrete::Coords3D > controls( *(p1->mid_), *(p1->front_), *(p2->rear_), *(p2->mid_) );
448 Bezier::PolyCoeffs< Concrete::Coords3D > coeffs( controls );
449
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;
453
454 {
455 Concrete::Coords3D d = coeffs.velocity( t.offtype< 0, 1 >( ) );
456 Concrete::Length v = d.norm( );
457 if( v > shortLength )
458 {
459 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( d.direction( v ) ) );
460 }
461 }
462
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.
466 */
467
468 {
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 )
472 {
473 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( d.direction( v ) ) );
474 }
475 }
476 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( 0, 0, 0 ) );
477 }
478
479 RefCountPtr< const Lang::FloatTriple >
480 Lang::ElementaryPath3D::reverse_direction( Concrete::Time globalTime ) const
481 {
482 if( empty( ) )
483 {
484 throw Exceptions::OutOfRange( "The empty path has no directions at all." );
485 }
486 Concrete::Time t;
487 const Concrete::PathPoint3D * p1;
488 const Concrete::PathPoint3D * p2;
489 findSegmentReverse( globalTime, &t, &p1, &p2 );
490
491 Bezier::ControlPoints< Concrete::Coords3D > controls( *(p1->mid_), *(p1->front_), *(p2->rear_), *(p2->mid_) );
492 Bezier::PolyCoeffs< Concrete::Coords3D > coeffs( controls );
493
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;
497
498 {
499 Concrete::Coords3D d = (-1) * coeffs.velocity( t.offtype< 0, 1 >( ) );
500 Concrete::Length v = d.norm( );
501 if( v > shortLength )
502 {
503 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( d.direction( v ) ) );
504 }
505 }
506
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.
510 */
511
512 {
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 )
516 {
517 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( d.direction( v ) ) );
518 }
519 }
520 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( 0, 0, 0 ) );
521 }
522
523 RefCountPtr< const Lang::FloatTriple >
524 Lang::ElementaryPath3D::normal( Concrete::Time globalTime ) const
525 {
526 if( empty( ) )
527 {
528 throw Exceptions::OutOfRange( "The empty path has no normals at all." );
529 }
530 Concrete::Time t;
531 const Concrete::PathPoint3D * p1;
532 const Concrete::PathPoint3D * p2;
533 findSegment( globalTime, &t, &p1, &p2 );
534
535 if( p1->front_ == p1->mid_ && p2->rear_ == p2->mid_ )
536 {
537 throw Exceptions::OutOfRange( "The normal is undefined along straigt segments." );
538 }
539
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 >( );
552
553 if( t <= Concrete::ZERO_TIME )
554 {
555 /* here, t = 0, tc == 1, so
556 */
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 >( );
563
564 Concrete::Acceleration tmpx = ax;
565 Concrete::Acceleration tmpy = ay;
566 Concrete::Acceleration tmpz = az;
567 if( tmpx == 0 && tmpy == 0 && tmpz == 0 )
568 {
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;
580 }
581 if( tmpx == 0 && tmpy == 0 && tmpz == 0 )
582 {
583 throw Exceptions::OutOfRange( "The normal is undefined becaue the acceleration vanishes smoothly." );
584 }
585 Physical< 2, -2 > v2 = squareSum( vx, vy, vz );
586 if( v2 > Physical< 2, -2 >( 0 ) )
587 {
588 Physical< 0, -1 > f = ( ax * vx + ay * vy + az * vz ) / v2;
589 tmpx -= vx * f;
590 tmpy -= vy * f;
591 tmpz -= vz * f;
592 }
593
594 Concrete::Acceleration tmpNorm = hypotPhysical( tmpx, tmpy, tmpz );
595 if( tmpNorm == 0 )
596 {
597 throw Exceptions::OutOfRange( "The normal is undefined where the acceleration is parallel to velocity." );
598 }
599 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( tmpx / tmpNorm, tmpy / tmpNorm, tmpz / tmpNorm ) );
600 }
601 if( t >= Concrete::UNIT_TIME )
602 {
603 throw Exceptions::InternalError( "t1 == 1 in normal calculation" );
604 }
605
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;
621
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 ) )
627 {
628 Physical< 0, -1 > f = ( ax * vx + ay * vy + az * vz ) / v2;
629 tmpx -= vx * f;
630 tmpy -= vy * f;
631 tmpz -= vz * f;
632 }
633
634 Physical< 1, -2 > tmpNorm = hypotPhysical( tmpx, tmpy, tmpz );
635 if( tmpNorm == Physical< 1, -2 >( 0 ) )
636 {
637 throw Exceptions::OutOfRange( "The normal is undefined where the acceleration is parallel to velocity." );
638 }
639 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( tmpx / tmpNorm, tmpy / tmpNorm, tmpz / tmpNorm ) );
640 }
641
642 RefCountPtr< const Lang::FloatTriple >
643 Lang::ElementaryPath3D::reverse_normal( Concrete::Time globalTime ) const
644 {
645 if( empty( ) )
646 {
647 throw Exceptions::OutOfRange( "The empty path has no normals at all." );
648 }
649 Concrete::Time t;
650 const Concrete::PathPoint3D * p1;
651 const Concrete::PathPoint3D * p2;
652 findSegmentReverse( globalTime, &t, &p1, &p2 );
653
654 if( p1->front_ == p1->mid_ && p2->rear_ == p2->mid_ )
655 {
656 throw Exceptions::OutOfRange( "The normal is undefined along straigt segments." );
657 }
658
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 >( );
671
672 if( t >= Concrete::UNIT_TIME )
673 {
674 /* here, t = 1, tc == 0, so
675 */
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 >( );
682
683 Concrete::Acceleration tmpx = ax;
684 Concrete::Acceleration tmpy = ay;
685 Concrete::Acceleration tmpz = az;
686 if( tmpx == 0 && tmpy == 0 && tmpz == 0 )
687 {
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;
697 }
698 if( tmpx == 0 && tmpy == 0 && tmpz == 0 )
699 {
700 throw Exceptions::OutOfRange( "The normal is undefined becaue the acceleration vanishes smoothly." );
701 }
702 Physical< 2, -2 > v2 = squareSum( vx, vy, vz );
703 if( v2 > Physical< 2, -2 >( 0 ) )
704 {
705 Physical< 0, -1 > f = ( ax * vx + ay * vy + az * vz ) / v2;
706 tmpx -= vx * f;
707 tmpy -= vy * f;
708 tmpz -= vz * f;
709 }
710
711 Concrete::Acceleration tmpNorm = hypotPhysical( tmpx, tmpy, tmpz );
712 if( tmpNorm == 0 )
713 {
714 throw Exceptions::OutOfRange( "The reverse normal is undefined where the acceleration is parallel to velocity." );
715 }
716 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( tmpx / tmpNorm, tmpy / tmpNorm, tmpz / tmpNorm ) );
717 }
718 if( t <= Concrete::ZERO_TIME )
719 {
720 throw Exceptions::InternalError( "t1 == 0 in reverse normal calculation" );
721 }
722
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;
738
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 ) )
744 {
745 Physical< 0, -1 > f = ( ax * vx + ay * vy + az * vz ) / v2;
746 tmpx -= vx * f;
747 tmpy -= vy * f;
748 tmpz -= vz * f;
749 }
750
751 Concrete::Acceleration tmpNorm = hypotPhysical( tmpx, tmpy, tmpz );
752 if( tmpNorm == 0 )
753 {
754 throw Exceptions::OutOfRange( "The reverse normal is undefined where the acceleration is parallel to velocity." );
755 }
756 return RefCountPtr< const Lang::FloatTriple >( new Lang::FloatTriple( tmpx / tmpNorm, tmpy / tmpNorm, tmpz / tmpNorm ) );
757 }
758
759 RefCountPtr< const Lang::Length >
760 Lang::ElementaryPath3D::radiusOfCurvature( Concrete::Time globalTime ) const
761 {
762 if( empty( ) )
763 {
764 throw Exceptions::OutOfRange( "The empty path has no curvature at all." );
765 }
766 Concrete::Time t;
767 const Concrete::PathPoint3D * p1;
768 const Concrete::PathPoint3D * p2;
769 findSegment( globalTime, &t, &p1, &p2 );
770
771 if( t <= Concrete::ZERO_TIME )
772 {
773 /* here, t = 0, tc == 1, so
774 */
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 >( );
790
791 Physical< 2, -3 > denom = hypotPhysical( vy * az - vz * ay, vz * ax - vx * az, vx * ay - vy * ax );
792 if( denom == 0 )
793 {
794 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
795 }
796 Physical< 2, -2 > tmp = squareSum( vx, vy, vz );
797 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * sqrtPhysical( tmp ) / denom ) );
798 }
799 if( t >= Concrete::UNIT_TIME )
800 {
801 throw Exceptions::InternalError( "t1 == 1 in curvature calculation" );
802 }
803
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 >( );
816
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;
832
833 Physical< 2, -3 > denom = hypotPhysical( vy * az - vz * ay, vz * ax - vx * az, vx * ay - vy * ax );
834 if( denom == 0 )
835 {
836 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
837 }
838 Physical< 2, -2 > tmp = squareSum( vx, vy, vz );
839 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * sqrtPhysical( tmp ) / denom ) );
840 }
841
842 RefCountPtr< const Lang::Length >
843 Lang::ElementaryPath3D::reverse_radiusOfCurvature( Concrete::Time globalTime ) const
844 {
845 if( empty( ) )
846 {
847 throw Exceptions::OutOfRange( "The empty path has no curvature at all." );
848 }
849 Concrete::Time t;
850 const Concrete::PathPoint3D * p1;
851 const Concrete::PathPoint3D * p2;
852 findSegmentReverse( globalTime, &t, &p1, &p2 );
853
854 if( t <= Concrete::ZERO_TIME )
855 {
856 throw Exceptions::InternalError( "t1 == 0 in reverse curvature calculation" );
857 }
858 if( t >= Concrete::UNIT_TIME )
859 {
860 /* here, t = 1, tc == 0, so
861 */
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 >( );
871
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 >( );
878
879 Physical< 2, -3 > denom = hypotPhysical( vy * az - vz * ay, vz * ax - vx * az, vx * ay - vy * ax );
880 if( denom == 0 )
881 {
882 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
883 }
884 Physical< 2, -2 > tmp = squareSum( vx, vy, vz );
885 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * sqrtPhysical( tmp ) / denom ) );
886 }
887
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 >( );
900
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;
916
917 Physical< 2, -3 > denom = hypotPhysical( vy * az - vz * ay, vz * ax - vx * az, vx * ay - vy * ax );
918 if( denom == 0 )
919 {
920 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
921 }
922 Physical< 2, -2 > tmp = squareSum( vx, vy, vz );
923 return RefCountPtr< const Lang::Length >( new Lang::Length( tmp * sqrtPhysical( tmp ) / denom ) );
924 }
925
926 RefCountPtr< const Lang::Length >
927 Lang::ElementaryPath3D::radiusOfTorsion( Concrete::Time globalTime ) const
928 {
929 if( empty( ) )
930 {
931 throw Exceptions::OutOfRange( "The empty path has no torsion at all." );
932 }
933 Concrete::Time t;
934 const Concrete::PathPoint3D * p1;
935 const Concrete::PathPoint3D * p2;
936 findSegment( globalTime, &t, &p1, &p2 );
937
938 if( t <= Concrete::ZERO_TIME )
939 {
940 /* here, t = 0, tc == 1, so
941 */
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 >( );
954
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;
964
965 Physical< 3, -6 > denom = tripleInner( vx, vy, vz, ax, ay, az, jx, jy, jz );
966 if( denom == 0 )
967 {
968 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
969 }
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 ) );
972 }
973 if( t >= Concrete::UNIT_TIME )
974 {
975 throw Exceptions::InternalError( "t1 == 1 in torsion calculation" );
976 }
977
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 >( );
990
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;
1013
1014 Physical< 3, -6 > denom = tripleInner( vx, vy, vz, ax, ay, az, jx, jy, jz );
1015 if( denom == 0 )
1016 {
1017 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
1018 }
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 ) );
1021 }
1022
1023 RefCountPtr< const Lang::Length >
1024 Lang::ElementaryPath3D::reverse_radiusOfTorsion( Concrete::Time globalTime ) const
1025 {
1026 if( empty( ) )
1027 {
1028 throw Exceptions::OutOfRange( "The empty path has no torsion at all." );
1029 }
1030 Concrete::Time t;
1031 const Concrete::PathPoint3D * p1;
1032 const Concrete::PathPoint3D * p2;
1033 findSegmentReverse( globalTime, &t, &p1, &p2 );
1034
1035 if( t <= Concrete::ZERO_TIME )
1036 {
1037 throw Exceptions::InternalError( "t1 == 0 in reverse torsion calculation" );
1038 }
1039 if( t >= Concrete::UNIT_TIME )
1040 {
1041 /* here, t = 1, tc == 0, so
1042 */
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 >( );
1055
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;
1065
1066 Physical< 3, -6 > denom = tripleInner( vx, vy, vz, ax, ay, az, jx, jy, jz );
1067 if( denom == 0 )
1068 {
1069 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
1070 }
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 ) );
1073 }
1074
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 >( );
1087
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;
1110
1111 Physical< 3, -6 > denom = tripleInner( vx, vy, vz, ax, ay, az, jx, jy, jz );
1112 if( denom == 0 )
1113 {
1114 return RefCountPtr< const Lang::Length >( new Lang::Length( HUGE_VAL ) );
1115 }
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 ) );
1118 }
1119
1120 size_t
1121 Lang::ElementaryPath3D::duration( ) const
1122 {
1123 if( isClosed( ) )
1124 {
1125 return size( );
1126 }
1127 return size( ) - 1;
1128 }
1129
1130 bool
1131 Lang::ElementaryPath3D::controllingMaximizer( const Lang::FloatTriple & d, Lang::Coords3D * dst ) const
1132 {
1133 if( empty( ) )
1134 {
1135 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1136 }
1137 Concrete::Length opt = -HUGE_VAL;
1138 for( const_iterator i = begin( ); i != end( ); ++i )
1139 {
1140 if( (*i)->rear_ != 0 )
1141 {
1142 Concrete::Length y = (*i)->rear_->x_ * d.x_ + (*i)->rear_->y_ * d.y_ + (*i)->rear_->z_ * d.z_;
1143 if( y > opt )
1144 {
1145 opt = y;
1146 *dst = *((*i)->rear_);
1147 }
1148 }
1149 Concrete::Length y = (*i)->mid_->x_ * d.x_ + (*i)->mid_->y_ * d.y_ + (*i)->mid_->z_ * d.z_;
1150 if( y > opt )
1151 {
1152 opt = y;
1153 *dst = *((*i)->mid_);
1154 }
1155 if( (*i)->front_ != 0 )
1156 {
1157 Concrete::Length y = (*i)->front_->x_ * d.x_ + (*i)->front_->y_ * d.y_ + (*i)->front_->z_ * d.z_;
1158 if( y > opt )
1159 {
1160 opt = y;
1161 *dst = *((*i)->front_);
1162 }
1163 }
1164 }
1165 if( opt == -HUGE_VAL )
1166 {
1167 return false;
1168 }
1169 return true;
1170 }
1171
1172
1173 RefCountPtr< const Lang::Coords3D >
1174 Lang::ElementaryPath3D::discreteMean( ) const
1175 {
1176 double count = 0;
1177 Concrete::Length x = 0;
1178 Concrete::Length y = 0;
1179 Concrete::Length z = 0;
1180 if( empty( ) )
1181 {
1182 throw Exceptions::OutOfRange( "The empty path cannot be averaged along." );
1183 }
1184 for( const_iterator i = begin( ); i != end( ); ++i )
1185 {
1186 if( (*i)->rear_ != 0 )
1187 {
1188 ++count;
1189 x = x + (*i)->rear_->x_;
1190 y = y + (*i)->rear_->y_;
1191 z = z + (*i)->rear_->z_;
1192 }
1193 ++count;
1194 x = x + (*i)->mid_->x_;
1195 y = y + (*i)->mid_->y_;
1196 z = z + (*i)->mid_->z_;
1197 if( (*i)->front_ != 0 )
1198 {
1199 ++count;
1200 x = x + (*i)->front_->x_;
1201 y = y + (*i)->front_->y_;
1202 z = z + (*i)->front_->z_;
1203 }
1204 }
1205 return RefCountPtr< const Lang::Coords3D >( new Lang::Coords3D( x / count, y / count, z / count ) );
1206 }
1207
1208 RefCountPtr< const Lang::Coords3D >
1209 Lang::ElementaryPath3D::controllingMaximizer( const Lang::FloatTriple & d ) const
1210 {
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 );
1214 }
1215
1216 Concrete::SplineTime
1217 Lang::ElementaryPath3D::discreteMaximizer( const Lang::FloatTriple & d ) const
1218 {
1219 if( empty( ) )
1220 {
1221 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1222 }
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 )
1230 {
1231 Concrete::Length y = (*i)->mid_->x_ * dx + (*i)->mid_->y_ * dy + (*i)->mid_->z_ * dz;
1232 if( y > opt )
1233 {
1234 opt = y;
1235 res = t;
1236 }
1237 }
1238 return res;
1239 }
1240
1241 Concrete::SplineTime
1242 Lang::ElementaryPath3D::discreteApproximator( const Lang::Coords3D & p ) const
1243 {
1244 if( empty( ) )
1245 {
1246 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1247 }
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 )
1255 {
1256 Concrete::Length dist = hypotPhysical( (*i)->mid_->x_ - px, (*i)->mid_->y_ - py, (*i)->mid_->z_ - pz );
1257 if( dist < bestDist )
1258 {
1259 bestDist = dist;
1260 res = t;
1261 }
1262 }
1263 return res;
1264 }
1265
1266 RefCountPtr< const Lang::Coords3D >
1267 Lang::ElementaryPath3D::continuousMean( ) const
1268 {
1269 if( empty( ) )
1270 {
1271 throw Exceptions::OutOfRange( "The empty path cannot be averaged along." );
1272 }
1273 Physical< 2, 0 > intx = 0;
1274 Physical< 2, 0 > inty = 0;
1275 Physical< 2, 0 > intz = 0;
1276 Physical< 1, 0 > totlen = 0;
1277
1278 const_iterator i1 = begin( );
1279 const_iterator i2 = i1;
1280 ++i2;
1281 for( ; i1 != end( ); ++i1, ++i2 )
1282 {
1283 if( i2 == end( ) )
1284 {
1285 if( closed_ )
1286 {
1287 i2 = begin( );
1288 }
1289 else
1290 {
1291 break;
1292 }
1293 }
1294
1295 if( (*i1)->front_ == (*i1)->mid_ &&
1296 (*i2)->rear_ == (*i2)->mid_ )
1297 {
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 );
1308 totlen += len;
1309 continue;
1310 }
1311
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 >( );
1324
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 >( ) );
1328
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 )
1334 {
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;
1350
1351 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
1352 tmpSum_x += x * dl;
1353 tmpSum_y += y * dl;
1354 tmpSum_z += z * dl;
1355 tmpSum_l += dl;
1356 }
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 >( );
1361 }
1362
1363 return RefCountPtr< const Lang::Coords3D >( new Lang::Coords3D( intx / totlen, inty / totlen, intz / totlen ) );
1364 }
1365
1366 Concrete::SplineTime
1367 Lang::ElementaryPath3D::continuousMaximizer( const Lang::FloatTriple & dNonUnit ) const
1368 {
1369 if( empty( ) )
1370 {
1371 throw Exceptions::OutOfRange( "The empty path cannot be maximized along." );
1372 }
1373
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.
1376
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;
1382 ++i2;
1383 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
1384 {
1385 if( i2 == end( ) )
1386 {
1387 if( closed_ )
1388 {
1389 i2 = begin( );
1390 }
1391 else
1392 {
1393 Concrete::Length v = Concrete::inner( *(*i1)->mid_, d );
1394 if( v > opt )
1395 {
1396 opt = v;
1397 res = steps;
1398 }
1399 break;
1400 }
1401 }
1402 Concrete::Length v = Concrete::inner( *(*i1)->mid_, d );
1403 if( v > opt )
1404 {
1405 opt = v;
1406 res = steps;
1407 }
1408
1409 /* if the segment is straight, checking its end points is enough.
1410 */
1411 if( (*i1)->front_ == (*i1)->mid_ &&
1412 (*i2)->rear_ == (*i2)->mid_ )
1413 {
1414 continue;
1415 }
1416
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.
1421 */
1422 {
1423 if( Concrete::inner( *(*i1)->front_, d ) <= opt &&
1424 Concrete::inner( *(*i2)->rear_, d ) <= opt )
1425 {
1426 continue;
1427 }
1428 }
1429
1430 Bezier::ControlPoints< Concrete::Coords3D > controls( *(*i1)->mid_, *(*i1)->front_, *(*i2)->rear_, *(*i2)->mid_ );
1431 Bezier::PolyCoeffs< Concrete::Coords3D > coeffs( controls );
1432 double optTimes[3];
1433 coeffs.stationaryPoints( optTimes, dBezier ); // A HUGE_VAL is used as terminator in the result.
1434
1435 for( double * src = & optTimes[0]; *src != HUGE_VAL; ++src )
1436 {
1437 Concrete::Length v = Concrete::inner( coeffs.point( *src ), d );
1438 if( v > opt )
1439 {
1440 opt = v;
1441 res = steps + *src;
1442 }
1443 }
1444 }
1445
1446 return res;
1447 }
1448
1449 namespace Shapes
1450 {
1451 class ApproximationPoly3D
1452 {
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_;
1464
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_;
1474
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_;
1484
1485 public:
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;
1493 };
1494 class ApproximationSegmentSection3D
1495 {
1496 Bezier::ControlPoints< Concrete::Coords3D > controls_;
1497 const ApproximationPoly3D * baseSeg_;
1498 Concrete::Time steps_;
1499 Concrete::Time t0_;
1500 Concrete::Time t1_;
1501 public:
1502
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;
1506
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;
1513 private:
1514 /* Argument points must be ordered so that points[0] is the point on the inside side of the plane containing the triangle.
1515 */
1516 static void triangleDistance( std::vector< const Concrete::Coords3D * > & points, const Concrete::Coords3D & p, Concrete::Length * bestDist );
1517 };
1518 }
1519
1520 Concrete::SplineTime
1521 Lang::ElementaryPath3D::continuousApproximator( const Lang::Coords3D & _p ) const
1522 {
1523 const Concrete::Length DISTANCE_TOL = 0.001 * Computation::the_arcdelta;
1524
1525 if( empty( ) )
1526 {
1527 throw Exceptions::OutOfRange( "The empty path cannot be approximated along." );
1528 }
1529
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;
1534
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;
1540 ++i2;
1541 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
1542 {
1543 if( i2 == end( ) )
1544 {
1545 if( closed_ )
1546 {
1547 i2 = begin( );
1548 }
1549 else
1550 {
1551 Concrete::Length dist = hypotPhysical( (*i1)->mid_->x_ - p.x_, (*i1)->mid_->y_ - p.y_, (*i1)->mid_->z_ - p.z_ );
1552 if( dist < bestDist )
1553 {
1554 bestDist = dist;
1555 res = steps;
1556 }
1557 break;
1558 }
1559 }
1560
1561 Concrete::Length dist = hypotPhysical( (*i1)->mid_->x_ - p.x_, (*i1)->mid_->y_ - p.y_, (*i1)->mid_->z_ - p.z_ );
1562 if( dist < bestDist )
1563 {
1564 bestDist = dist;
1565 res = steps;
1566 }
1567
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 >( );
1574
1575 /* if the segment is straight, finding the closest point is a linear least-squares problem.
1576 */
1577 if( (*i1)->front_ == (*i1)->mid_ &&
1578 (*i2)->rear_ == (*i2)->mid_ )
1579 {
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 )
1585 {
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 )
1588 {
1589 bestDist = dist;
1590 res = steps + Shapes::straightLineArcTime( s );
1591 }
1592 }
1593 continue;
1594 }
1595
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 )
1601 {
1602 delete seg;
1603 delete coeffs;
1604 continue;
1605 }
1606
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.
1609 */
1610 {
1611 if( coeffs->isCircular( ) )
1612 {
1613 Concrete::Length dist = coeffs->distanceAt( 0 );
1614 if( dist < bestDist )
1615 {
1616 bestDist = dist;
1617 res = steps;
1618 }
1619 delete seg;
1620 delete coeffs;
1621 continue;
1622 }
1623 }
1624
1625 memory.push_back( coeffs );
1626 work.push_back( WorkItem( seg, lowerBound ) );
1627 }
1628
1629 while( ! work.empty( ) )
1630 {
1631 Shapes::ApproximationSegmentSection3D * seg = work.front( ).first;
1632 Concrete::Length lowerBound = work.front( ).second;
1633 work.pop_front( );
1634 if( lowerBound < bestDist )
1635 {
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 )
1640 {
1641 bestDist = dist;
1642 res = seg->globalTime( split );
1643 }
1644 if( lowerBound < bestDist &&
1645 t_tol < seg->duration( ) )
1646 {
1647 {
1648 Shapes::ApproximationSegmentSection3D * part = seg->cutAfter( split );
1649 Concrete::Length lowerBound = part->convexHullDistance( ) + DISTANCE_TOL;
1650 if( lowerBound >= bestDist )
1651 {
1652 delete part;
1653 }
1654 else
1655 {
1656 work.push_back( WorkItem( part, lowerBound ) );
1657 }
1658 }
1659 {
1660 Shapes::ApproximationSegmentSection3D * part = seg->cutBefore( split );
1661 Concrete::Length lowerBound = part->convexHullDistance( ) + DISTANCE_TOL;
1662 if( lowerBound >= bestDist )
1663 {
1664 delete part;
1665 }
1666 else
1667 {
1668 work.push_back( WorkItem( part, lowerBound ) );
1669 }
1670 }
1671 }
1672 }
1673 delete seg;
1674 }
1675
1676 return res;
1677 }
1678
1679 namespace Shapes
1680 {
1681 namespace Computation
1682 {
1683
1684 class SegmentPolynomialPair3D
1685 {
1686 Bezier::PolyCoeffs< Concrete::Coords3D > polyCoeffs_a;
1687 bool straightLineSeg_a_;
1688
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;
1698
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;
1708
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;
1718
1719 Bezier::PolyCoeffs< Concrete::Coords3D > polyCoeffs_b;
1720 bool straightLineSeg_b_;
1721
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;
1731
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;
1741
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;
1751
1752 Concrete::Time steps_a_;
1753 Concrete::Time steps_b_;
1754
1755 Concrete::Speed maxSpeed_a_;
1756 Concrete::Speed maxSpeed_b_;
1757
1758 public:
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;
1773
1774 private:
1775 void kInit( ); /* Used by constructors to initialize coefficients. */
1776 };
1777
1778 class SegmentSectionPair3D
1779 {
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_;
1788
1789 public:
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;
1795
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;
1809
1810 void writeTimes( std::ostream & os ) const;
1811 private:
1812 static bool convexHullOneWayOverlap( const std::vector< const Concrete::Coords3D * > & poly1, const std::vector< const Concrete::Coords3D * > & poly2 );
1813 };
1814
1815 }
1816 }
1817
1818 namespace Shapes
1819 {
1820 namespace Helpers
1821 {
1822 Kernel::StructureFactory approximator3D_info_resultFactory( "dist", "other" );
1823 }
1824 namespace Computation
1825 {
1826 struct CurvedSegType3D : public Bezier::ControlPoints< Concrete::Coords3D >
1827 {
1828 Concrete::Time steps;
1829 CurvedSegType3D( Concrete::Time _steps, const Bezier::ControlPoints< Concrete::Coords3D > & ctrl )
1830 : Bezier::ControlPoints< Concrete::Coords3D >( ctrl ), steps( _steps )
1831 { }
1832 };
1833 struct StraightSegType3D
1834 {
1835 Concrete::Time steps;
1836 Concrete::Coords3D * first;
1837 Concrete::Coords3D * second;
1838
1839 StraightSegType3D( Concrete::Time _steps, Concrete::Coords3D * _first, Concrete::Coords3D * _second )
1840 : steps( _steps ), first( _first ), second( _second )
1841 { }
1842 };
1843 }
1844 }
1845
1846 Concrete::SplineTime
1847 Lang::ElementaryPath3D::continuousApproximator( const RefCountPtr< const Lang::ElementaryPath3D > & p2, RefCountPtr< const Lang::Structure > * dstInfo ) const
1848 {
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.
1851 *
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.
1854 */
1855
1856 using namespace Computation;
1857
1858 const Concrete::Length DISTANCE_TOL = 0.001 * Computation::the_arcdelta;
1859 const double CUT_LOSS = 0.01;
1860
1861 if( empty( ) )
1862 {
1863 throw Exceptions::OutOfRange( "The empty path does not approximate anying." );
1864 }
1865 if( p2->empty( ) )
1866 {
1867 throw Exceptions::OutOfRange( "The empty path cannot be the approximation target." );
1868 }
1869
1870 Concrete::Time resSteps = Concrete::HUGE_TIME;
1871 Concrete::Time resFrac = 0;
1872 Concrete::Time res2 = 0;
1873
1874 Concrete::LengthSquared bestDist( HUGE_VAL );
1875 const Concrete::LengthSquared INTERSECT_DIST( 1e-10 );
1876 const Concrete::LengthSquared ZERO_DIST( 0 );
1877
1878 std::vector< CurvedSegType3D > curvedSegments;
1879 curvedSegments.reserve( p2->size( ) );
1880 std::vector< StraightSegType3D > straightSegments;
1881 straightSegments.reserve( p2->size( ) );
1882 {
1883 Concrete::Time steps2 = Concrete::ZERO_TIME;
1884 const_iterator i1 = p2->begin( );
1885 const_iterator i2 = i1;
1886 ++i2;
1887 for( ; i1 != p2->end( ); ++i1, ++i2, steps2 += Concrete::UNIT_TIME )
1888 {
1889 Concrete::Time steps = Concrete::ZERO_TIME;
1890 for( const_iterator j = begin( ); j != end( ); ++j, steps += Concrete::UNIT_TIME )
1891 {
1892 Concrete::LengthSquared d = ( *((*i1)->mid_) - *((*j)->mid_) ).normSquared( );
1893 if( d <= INTERSECT_DIST )
1894 {
1895 if( bestDist > ZERO_DIST ||
1896 steps < resSteps )
1897 {
1898 resSteps = steps;
1899 res2 = steps2;
1900 bestDist = ZERO_DIST;
1901 }
1902 }
1903 else if( d < bestDist )
1904 {
1905 resSteps = steps; /* At this point, the fraction is still 0. */
1906 res2 = steps2;
1907 bestDist = d;
1908 }
1909 }
1910 if( i2 == p2->end( ) )
1911 {
1912 if( p2->isClosed( ) )
1913 {
1914 i2 = p2->begin( );
1915 }
1916 else
1917 {
1918 break;
1919 }
1920 }
1921 if( (*i1)->front_ == (*i1)->mid_ &&
1922 (*i2)->rear_ == (*i2)->mid_ )
1923 {
1924 straightSegments.push_back( StraightSegType3D( steps2, (*i1)->mid_, (*i2)->mid_ ) );
1925 }
1926 else
1927 {
1928 curvedSegments.push_back( CurvedSegType3D( steps2, Bezier::ControlPoints< Concrete::Coords3D >( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_) ) ) );
1929 }
1930 }
1931 }
1932
1933 {
1934 Concrete::Time steps = Concrete::ZERO_TIME;
1935 const_iterator i1 = begin( );
1936 const_iterator i2 = i1;
1937 ++i2;
1938 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
1939 {
1940 if( i2 == end( ) )
1941 {
1942 if( closed_ )
1943 {
1944 i2 = begin( );
1945 }
1946 else
1947 {
1948 break;
1949 }
1950 }
1951
1952 if( (*i1)->front_ == (*i1)->mid_ &&
1953 (*i2)->rear_ == (*i2)->mid_ )
1954 {
1955 /* Straight line segment. This is the only case we care about at this point.
1956 */
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 ) );
1961 const size_t N = 2;
1962 const size_t DIM = 3;
1963 double ga[ N * DIM ];
1964 double gb[ N * DIM ];
1965 double ya[ DIM ];
1966 double yb[ DIM ];
1967 {
1968 double * dstg = ga;
1969 *dstg = Concrete::Length::offtype( l0.x_ );
1970 ++dstg;
1971 *dstg = Concrete::Length::offtype( l0.y_ );
1972 ++dstg;
1973 *dstg = Concrete::Length::offtype( l0.z_ );
1974 ++dstg;
1975 *dstg = Concrete::Length::offtype( l1.x_ );
1976 ++dstg;
1977 *dstg = Concrete::Length::offtype( l1.y_ );
1978 ++dstg;
1979 *dstg = Concrete::Length::offtype( l1.z_ );
1980 }
1981 typedef typeof straightSegments ListType;
1982 for( ListType::const_iterator i = straightSegments.begin( ); i != straightSegments.end( ); ++i )
1983 {
1984 {
1985 double * dstg = gb;
1986 *dstg = Concrete::Length::offtype( i->first->x_ );
1987 ++dstg;
1988 *dstg = Concrete::Length::offtype( i->first->y_ );
1989 ++dstg;
1990 *dstg = Concrete::Length::offtype( i->first->z_ );
1991 ++dstg;
1992 *dstg = Concrete::Length::offtype( i->second->x_ );
1993 ++dstg;
1994 *dstg = Concrete::Length::offtype( i->second->y_ );
1995 ++dstg;
1996 *dstg = Concrete::Length::offtype( i->second->z_ );
1997 }
1998 double double_distSquaredLow;
1999 double double_distSquaredHigh;
2000 QPSolverStatus status;
2001 Computation::polytope_distance_generator_form( DIM,
2002 N, & ga[0],
2003 N, & gb[0],
2004 & double_distSquaredLow, & double_distSquaredHigh,
2005 Concrete::LengthSquared::offtype( bestDist ), -1,
2006 0, 0,
2007 0.5 * Concrete::Length::offtype( Computation::theDistanceTol ), /* Lagrange multiplier tolerance. */
2008 0, 0,
2009 & ya[0], & yb[0],
2010 & status );
2011 if( status.code != Computation::QP_OK )
2012 {
2013 const char * binaryFilename = "qp-fail-data.binary";
2014 Computation::polytope_distance_generator_form_write_binary_data( binaryFilename,
2015 DIM,
2016 N, & ga[0],
2017 N, & gb[0] );
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 ) );
2022 }
2023 Concrete::LengthSquared distSquaredLow( double_distSquaredLow );
2024 Concrete::LengthSquared distSquaredHigh( double_distSquaredHigh );
2025 if( distSquaredHigh <= INTERSECT_DIST )
2026 {
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.
2029 */
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 )
2033 {
2034 resSteps = steps;
2035 resFrac = t;
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;
2042 }
2043 }
2044 else if( distSquaredLow < bestDist )
2045 {
2046 /* The optimum has been improved. Save new optimum. */
2047 resSteps = steps;
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;
2055 }
2056 }
2057 }
2058 }
2059 }
2060
2061 std::list< SegmentSectionPair3D * > work;
2062 PtrOwner_back_Access< std::list< const SegmentPolynomialPair3D * > > baseSegsMemory;
2063
2064 Concrete::Time steps = Concrete::ZERO_TIME;
2065 const_iterator i1 = begin( );
2066 const_iterator i2 = i1;
2067 ++i2;
2068 for( ; i1 != end( ); ++i1, ++i2, steps += Concrete::UNIT_TIME )
2069 {
2070 if( i2 == end( ) )
2071 {
2072 if( closed_ )
2073 {
2074 i2 = begin( );
2075 }
2076 else
2077 {
2078 break;
2079 }
2080 }
2081
2082 if( (*i1)->front_ == (*i1)->mid_ &&
2083 (*i2)->rear_ == (*i2)->mid_ )
2084 {
2085 /* Straight line segment. The distance to straight line segments on the other path has already been computed.
2086 */
2087 typedef typeof curvedSegments ListType;
2088 for( ListType::const_iterator seg_b = curvedSegments.begin( ); seg_b != curvedSegments.end( ); ++seg_b )
2089 {
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 );
2093 }
2094 }
2095 else
2096 {
2097 /* General segment. (If it happens to be straight, we don't care.)
2098 */
2099 Bezier::ControlPoints< Concrete::Coords3D > seg_a( *((*i1)->mid_), *((*i1)->front_), *((*i2)->rear_), *((*i2)->mid_) );
2100
2101 {
2102 typedef typeof straightSegments ListType;
2103 for( ListType::const_iterator i = straightSegments.begin( ); i != straightSegments.end( ); ++i )
2104 {
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 );
2108 }
2109 }
2110
2111 {
2112 typedef typeof curvedSegments ListType;
2113 for( ListType::const_iterator seg_b = curvedSegments.begin( ); seg_b != curvedSegments.end( ); ++seg_b )
2114 {
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 );
2118 }
2119 }
2120 }
2121 }
2122
2123 while( ! work.empty( ) )
2124 {
2125 SegmentSectionPair3D * workItem = work.front( );
2126 work.pop_front( );
2127 if( workItem->precomputedHullDistanceSquared( ) > ( ( bestDist == 0 ) ? INTERSECT_DIST : bestDist ) )
2128 {
2129 delete workItem;
2130 continue;
2131 }
2132 if( bestDist <= INTERSECT_DIST )
2133 {
2134 if( workItem->steps_a( ) > resSteps )
2135 {
2136 delete workItem;
2137 continue;
2138 }
2139 if( workItem->steps_a( ) == resSteps )
2140 {
2141 workItem->update_t_a1( resFrac );
2142 }
2143 }
2144 if( workItem->isEmpty( ) )
2145 {
2146 delete workItem;
2147 continue;
2148 }
2149
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( );
2152
2153 Concrete::Time t1;
2154 Concrete::Time t2;
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;
2158 if( intersect )
2159 {
2160 if( bestDist > ZERO_DIST
2161 || workItem->globalTime_a( t1 ) < resSteps + resFrac )
2162 {
2163 bestDist = ZERO_DIST;
2164 resSteps = workItem->steps_a( );
2165 resFrac = t1;
2166 }
2167 }
2168 else if( d < bestDist )
2169 {
2170 bestDist = d;
2171 resSteps = workItem->steps_a( );
2172 resFrac = t1;
2173 }
2174 if( intersect )
2175 {
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 );
2178 }
2179 else
2180 {
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);
2185 }
2186 delete workItem;
2187 }
2188
2189 if( resSteps == Concrete::HUGE_TIME )
2190 {
2191 throw Exceptions::InternalError( "Path to path approximation: No result was produced." );
2192 }
2193
2194 if( dstInfo != 0 )
2195 {
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( );
2199 }
2200 return Concrete::SplineTime( resSteps + resFrac );
2201 }
2202
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 ) ) )
2205 {
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 >( );
2210
2211 kxD1_0_ = kxD0_1_;
2212 kxD1_1_ = 2 * kxD0_2_;
2213 kxD1_2_ = 3 * kxD0_3_;
2214
2215 kxD2_0_ = kxD1_1_;
2216 kxD2_1_ = 2 * kxD1_2_;
2217
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 >( );
2222
2223 kyD1_0_ = kyD0_1_;
2224 kyD1_1_ = 2 * kyD0_2_;
2225 kyD1_2_ = 3 * kyD0_3_;
2226
2227 kyD2_0_ = kyD1_1_;
2228 kyD2_1_ = 2 * kyD1_2_;
2229
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 >( );
2234
2235 kzD1_0_ = kzD0_1_;
2236 kzD1_1_ = 2 * kzD0_2_;
2237 kzD1_2_ = 3 * kzD0_3_;
2238
2239 kzD2_0_ = kzD1_1_;
2240 kzD2_1_ = 2 * kzD1_2_;
2241 }
2242
2243 Concrete::Time
2244 Shapes::ApproximationPoly3D::splitTime( const Concrete::Time t_low, const Concrete::Time t_high, const Concrete::Time t_tol ) const
2245 {
2246 Concrete::Time t;
2247 Concrete::LengthSquared last_f = HUGE_VAL;
2248 {
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 )
2252 {
2253 Concrete::LengthSquared tmp = squaredDistanceAt( tt );
2254 if( tmp < last_f )
2255 {
2256 last_f = tmp;
2257 t = tt;
2258 }
2259 }
2260 }
2261 Concrete::Time last_t = -1;
2262 bool lastOffBounds = false;
2263
2264 while( t < last_t - t_tol || t > last_t + t_tol )
2265 {
2266 last_t = t;
2267 Physical< 2, -1 > objectiveD1( 0 );
2268 Physical< 2, -2 > objectiveD2( 0 );
2269
2270 {
2271 /* x-components */
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;
2277 }
2278
2279 {
2280 /* y-components */
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;
2286 }
2287
2288 {
2289 /* z-components */
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;
2295 }
2296
2297 Concrete::Time step;
2298 if( objectiveD2 <= 0 )
2299 {
2300 /* The minimization problem is not convex, locally.
2301 * Go to one of the extremes.
2302 */
2303 if( objectiveD1 < 0 )
2304 {
2305 step = 1.1 * ( t_high - t );
2306 }
2307 else
2308 {
2309 step = 1.1 * ( t_low - t );
2310 }
2311 }
2312 else
2313 {
2314 step = -objectiveD1 / objectiveD2;
2315 if( t + step < t_low )
2316 {
2317 step = 1.1 * ( t_low - t ); // 1.1 to make sure that the off-bounds test detects minima outside [ t_low t_high ]
2318 }
2319 else if( t + step > t_high )
2320 {
2321 step = 1.1 * ( t_high - t ); // 1.1 to make sure that the off-bounds test detects minima outside [ t_low t_high ]
2322 }
2323 }
2324
2325 Concrete::LengthSquared f = squaredDistanceAt( t + step );
2326 if( step > 0 )
2327 {
2328 while( f > last_f && step > t_tol )
2329 {
2330 step = step * 0.5;
2331 f = squaredDistanceAt( t + step );
2332 }
2333 }
2334 else
2335 {
2336 while( f > last_f && step < -t_tol )
2337 {
2338 step = step * 0.5;
2339 f = squaredDistanceAt( t + step );
2340 }
2341 }
2342 t += step;
2343 last_f = f;
2344
2345 if( t <= t_low )
2346 {
2347 if( lastOffBounds )
2348 {
2349 return t_low;
2350 }
2351 else
2352 {
2353 t = t_low;
2354 lastOffBounds = true;
2355 }
2356 }
2357 else if( t >= t_high )
2358 {
2359 if( lastOffBounds )
2360 {
2361 return t_high;
2362 }
2363 else
2364 {
2365 t = t_high;
2366 lastOffBounds = true;
2367 }
2368 }
2369 else
2370 {
2371 lastOffBounds = false;
2372 }
2373 }
2374
2375 if( lastOffBounds )
2376 {
2377 if( t > 0.5 * ( t_low + t_high ) )
2378 {
2379 return t_high;
2380 }
2381 return t_low;
2382 }
2383
2384 return t;
2385 }
2386
2387 Bezier::ControlPoints< Concrete::Coords3D >
2388 Shapes::ApproximationPoly3D::getControls( const Concrete::Time t_low, const Concrete::Time t_high ) const
2389 {
2390 return Bezier::ControlPoints< Concrete::Coords3D >( polyCoeffs_.subSection( t_low.offtype< 0, 1 >( ), t_high.offtype< 0, 1 >( ) ) );
2391 }
2392
2393 bool
2394 Shapes::ApproximationPoly3D::isCircular( ) const
2395 {
2396 Concrete::Length rmax = -HUGE_VAL;
2397 Concrete::Length rmin = HUGE_VAL;
2398 for( Concrete::Time t = 0; t < 1.05; t += 0.33333 )
2399 {
2400 Concrete::Length r = distanceAt( t );
2401 rmax = max( rmax, r );
2402 rmin = min( rmin, r );
2403 }
2404 return rmax <= rmin * 1.0001;
2405 }
2406
2407 const Concrete::Coords3D &
2408 Shapes::ApproximationPoly3D::getPoint( ) const
2409 {
2410 return p_;
2411 }
2412
2413
2414 Concrete::Length
2415 Shapes::ApproximationPoly3D::distanceAt( Concrete::Time t ) const
2416 {
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_ ) ) );
2420 }
2421
2422 Concrete::LengthSquared
2423 Shapes::ApproximationPoly3D::squaredDistanceAt( Concrete::Time t ) const
2424 {
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;
2429 }
2430
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 )
2433 { }
2434
2435 Shapes::ApproximationSegmentSection3D *
2436 Shapes::ApproximationSegmentSection3D::cutAfter( Concrete::Time t ) const
2437 {
2438 return new ApproximationSegmentSection3D( baseSeg_, steps_, t0_, t );
2439 }
2440
2441 Shapes::ApproximationSegmentSection3D *
2442 Shapes::ApproximationSegmentSection3D::cutBefore( Concrete::Time t ) const
2443 {
2444 return new ApproximationSegmentSection3D( baseSeg_, steps_, t, t1_ );
2445 }
2446
2447
2448 namespace Shapes
2449 {
2450 class HullSorter3D : public std::binary_function< const Concrete::Coords3D *, const Concrete::Coords3D *, bool >
2451 {
2452 const Concrete::Coords3D & p_;
2453 public:
2454 HullSorter3D( const Concrete::Coords3D & p ) : p_( p ) { }
2455 bool operator () ( const Concrete::Coords3D * p1, const Concrete::Coords3D * p2 )
2456 {
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.
2464 */
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;
2470 }
2471 };
2472 }
2473
2474 Concrete::Length
2475 Shapes::ApproximationSegmentSection3D::convexHullDistance( ) const
2476 {
2477 Concrete::Coords3D p( baseSeg_->getPoint( ) );
2478 std::list< const Concrete::Coords3D * > hullPoints;
2479
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_ );
2486
2487 Concrete::Length res = HUGE_VAL;
2488
2489 for( std::vector< const Concrete::Coords3D * >::iterator i = pointSet.begin( );
2490 i != pointSet.end( );
2491 ++i )
2492 {
2493 if( i != pointSet.begin( ) )
2494 {
2495 const Concrete::Coords3D * tmp = pointSet[0];
2496 pointSet[0] = *i;
2497 *i = tmp;
2498 }
2499 triangleDistance( pointSet, p, & res );
2500 }
2501
2502 if( res == HUGE_VAL )
2503 {
2504 /* This means that p was on the inside of each line, meaning it
2505 * is also inside the convex hull.
2506 */
2507 return 0;
2508 }
2509 return res;
2510 }
2511
2512 void
2513 Shapes::ApproximationSegmentSection3D::triangleDistance( std::vector< const Concrete::Coords3D * > & points, const Concrete::Coords3D & p, Concrete::Length * bestDist )
2514 {
2515 /*
2516 * Note: We will mess around with points, but it will be returned as passed.
2517 */
2518
2519 try
2520 {
2521 /* Remember:
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.
2524 */
2525
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] );
2528
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 ) )
2531 {
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.
2535 */
2536 return;
2537 }
2538
2539 Concrete::Length res = HUGE_VAL;
2540
2541 const Concrete::Coords3D pProj = p - tn * n;
2542
2543 std::vector< const Concrete::Coords3D * >::iterator i0 = points.begin( );
2544 ++i0;
2545 std::vector< const Concrete::Coords3D * >::iterator begin = i0;
2546 for( ; i0 != points.end( ); ++i0 )
2547 {
2548 const Concrete::Coords3D * tmp = *begin;
2549 *begin = *i0;
2550 *i0 = tmp;
2551
2552 std::vector< const Concrete::Coords3D * >::iterator j = begin;
2553 const Concrete::Coords3D & pInside( **j );
2554 ++j;
2555 const Concrete::Coords3D & p0( **j );
2556 ++j;
2557 const Concrete::Coords3D & p1( **j );
2558
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.)
2561 */
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 );
2566 if( d1Norm2 == 0 )
2567 {
2568 res = min( res, dp.norm( ) );
2569 goto next;
2570 }
2571
2572 /* Introduce new scope to prevent compiler warnings about goto past initializations.
2573 */
2574 {
2575 double sInside = Concrete::innerScalar( dInside, d1 ) / d1Norm2;
2576 const Concrete::Coords3D nOut = d1 * sInside - dInside;
2577 const Concrete::Coords3D dpProj = pProj - p0;
2578
2579 if( Concrete::innerScalar( nOut, nOut ) > 0 &&
2580 Concrete::innerScalar( nOut, dpProj ) < 0 )
2581 {
2582 /* pProj is on the same side as pInside
2583 */
2584 goto next;
2585 }
2586
2587 res = min( res, ( dp - d1 * ( Concrete::innerScalar( dpProj, d1 ) / d1Norm2 ) ).norm( ) );
2588 }
2589 next:
2590 /* Swap back so that points remains unchanged
2591 */
2592 *i0 = *begin;
2593 *begin = tmp;
2594 }
2595
2596 if( res == HUGE_VAL )
2597 {
2598 res = ( p - pProj ).norm( );
2599 }
2600
2601 *bestDist = min( *bestDist, res );
2602 }
2603 catch( const NonLocalExit::CrossDirectionOfParallel & ball )
2604 {
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.
2607 */
2608 throw Exceptions::InternalError( "Unprepared to handle collapsed triangle here. This algorithm should be re-implemented using the robust polytope distance computation." );
2609 }
2610 }
2611
2612
2613 Concrete::Time
2614 Shapes::ApproximationSegmentSection3D::splitTime( const Concrete::Time t_tol ) const
2615 {
2616 Concrete::Time d = 0.2 * ( t1_ - t0_ );
2617 return baseSeg_->splitTime( t0_ + d, t1_ - d, t_tol );
2618 }
2619
2620 Concrete::Time
2621 Shapes::ApproximationSegmentSection3D::globalTime( Concrete::Time t ) const
2622 {
2623 return steps_ + t;
2624 }
2625
2626 Concrete::Length
2627 Shapes::ApproximationSegmentSection3D::distanceAt( Concrete::Time t ) const
2628 {
2629 return baseSeg_->distanceAt( t );
2630 }
2631
2632 Concrete::Speed
2633 Shapes::ApproximationSegmentSection3D::maxSpeed( ) const
2634 {
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 >( );
2638 }
2639
2640 Concrete::Time
2641 Shapes::ApproximationSegmentSection3D::duration( ) const
2642 {
2643 return t1_ - t0_;
2644 }
2645
2646
2647 Concrete::Length
2648 Lang::ElementaryPath3D::arcLength( ) const
2649 {
2650 if( empty( ) )
2651 {
2652 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2653 }
2654 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2655 */
2656 Concrete::Length totlen = 0;
2657
2658 const Concrete::Length arcdelta = Computation::the_arcdelta;
2659
2660 const_iterator i1 = begin( );
2661 const_iterator i2 = i1;
2662 ++i2;
2663 for( ; i1 != end( ); ++i1, ++i2 )
2664 {
2665 if( i2 == end( ) )
2666 {
2667 if( closed_ )
2668 {
2669 i2 = begin( );
2670 }
2671 else
2672 {
2673 break;
2674 }
2675 }
2676 if( (*i1)->front_ == (*i1)->mid_ &&
2677 (*i2)->rear_ == (*i2)->mid_ )
2678 {
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;
2681 continue;
2682 }
2683
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 >( );
2696
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 >( );
2698
2699 if( segLengthBound < arcdelta )
2700 {
2701 totlen += segLengthBound;
2702 }
2703 else
2704 {
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 )
2708 {
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;
2717
2718 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
2719 tmpSum_l += dl;
2720 }
2721 totlen += tmpSum_l * dt.offtype< 0, 1 >( );
2722 }
2723 }
2724
2725 return totlen;
2726 }
2727
2728 Concrete::Length
2729 Lang::ElementaryPath3D::arcLength( Concrete::Time tRemaining ) const
2730 {
2731 if( empty( ) )
2732 {
2733 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2734 }
2735 if( tRemaining < 0 )
2736 {
2737 return negative_arcLength( -tRemaining );
2738 }
2739 if( isinfPhysical( tRemaining ) )
2740 {
2741 throw Exceptions::OutOfRange( "The arctime to infinity is not defined." );
2742 }
2743 if( tRemaining > duration( ) && ! closed_ )
2744 {
2745 throw Exceptions::OutOfRange( "Too big arctime for open path." );
2746 }
2747 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2748 */
2749 Concrete::Length totlen = 0;
2750
2751 const Concrete::Length arcdelta = Computation::the_arcdelta;
2752
2753 {
2754 const double revolutions = floor( tRemaining / Concrete::Time( duration( ) ) );
2755 if( revolutions > 0 )
2756 {
2757 totlen += revolutions * arcLength( );
2758 tRemaining -= revolutions * Concrete::Time( duration( ) );
2759 }
2760 }
2761
2762 const_iterator i1 = begin( );
2763 const_iterator i2 = i1;
2764 ++i2;
2765 for( ; tRemaining > 0; ++i1, ++i2, tRemaining -= Concrete::UNIT_TIME )
2766 {
2767 if( i1 == end( ) )
2768 {
2769 /* This could be considered an error situation, but it may be due to numeric
2770 * errors, so we take action accordingly.
2771 */
2772 break;
2773 }
2774 if( i2 == end( ) )
2775 {
2776 /* This could also be an error situation...
2777 */
2778 i2 = begin( );
2779 }
2780 if( (*i1)->front_ == (*i1)->mid_ &&
2781 (*i2)->rear_ == (*i2)->mid_ )
2782 {
2783 if( tRemaining <= 1 )
2784 {
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;
2792 break;
2793 }
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;
2796 continue;
2797 }
2798
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 >( );
2811
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 >( );
2813
2814 if( segLengthBound < arcdelta )
2815 {
2816 if( tRemaining <= 1 )
2817 {
2818 totlen += ( tRemaining / Concrete::UNIT_TIME ) * segLengthBound;
2819 break;
2820 }
2821 totlen += segLengthBound;
2822 }
2823 else
2824 {
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 )
2829 {
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;
2838
2839 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
2840 tmpSum_l += dl;
2841 }
2842 totlen += tmpSum_l * dt.offtype< 0, 1 >( );
2843 }
2844 }
2845
2846 return totlen;
2847 }
2848
2849 Concrete::Length
2850 Lang::ElementaryPath3D::negative_arcLength( Concrete::Time tRemaining ) const
2851 {
2852 if( empty( ) )
2853 {
2854 throw Exceptions::OutOfRange( "The empty path has no defined arclength." );
2855 }
2856 if( tRemaining < 0 )
2857 {
2858 throw Exceptions::InternalError( "Negative tRemaining in negative_arcLength." );
2859 }
2860 if( ! closed_ )
2861 {
2862 throw Exceptions::OutOfRange( "The arctime at negative lengths is not defined for open paths." );
2863 }
2864 if( isinfPhysical( tRemaining ) )
2865 {
2866 throw Exceptions::OutOfRange( "The arctime to infinity is not defined." );
2867 }
2868
2869 /* Multiplication by dt is extracted to outside the integrals for efficiency.
2870 */
2871 Concrete::Length totlen = 0;
2872
2873 const Concrete::Length arcdelta = Computation::the_arcdelta;
2874
2875 {
2876 const double revolutions = floor( tRemaining / Concrete::Time( duration( ) ) );
2877 if( revolutions > 0 )
2878 {
2879 totlen -= revolutions * arcLength( );
2880 tRemaining -= revolutions * Concrete::Time( duration( ) );
2881 }
2882 }
2883
2884 const_reverse_iterator i1 = rbegin( );
2885 const_reverse_iterator i2 = i1;
2886 if( closed_ )
2887 {
2888 i1 = rend( );
2889 --i1;
2890 i2 = rbegin( );
2891 }
2892 else
2893 {
2894 ++i2;
2895 }
2896 for( ; tRemaining > 0; ++i1, ++i2, tRemaining -= Concrete::UNIT_TIME )
2897 {
2898 if( i2 == rend( ) )
2899 {
2900 /* This could be considered an error situation, but it may be due to numeric
2901 * errors, so we take action accordingly.
2902 */
2903 break;
2904 }
2905 if( i1 == rend( ) )
2906 {
2907 i1 = rbegin( );
2908 }
2909 if( (*i1)->rear_ == (*i1)->mid_ &&
2910 (*i2)->front_ == (*i2)->mid_ )
2911 {
2912 if( tRemaining <= 1 )
2913 {
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;
2921 break;
2922 }
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;
2925 continue;
2926 }
2927
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 >( );
2940
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 >( );
2942
2943 if( segLengthBound < arcdelta )
2944 {
2945 if( tRemaining <= 1 )
2946 {
2947 totlen -= ( tRemaining / Concrete::UNIT_TIME ) * segLengthBound;
2948 break;
2949 }
2950 totlen -= segLengthBound;
2951 }
2952 else
2953 {
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 )
2958 {
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;
2967
2968 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
2969 tmpSum_l += dl;
2970 }
2971 totlen -= tmpSum_l * dt.offtype< 0, 1 >( );
2972 }
2973 }
2974
2975 return totlen;
2976 }
2977
2978 Concrete::SplineTime
2979 Lang::ElementaryPath3D::arcTime( const Concrete::Length & t, Concrete::Time t0 ) const
2980 {
2981 if( empty( ) )
2982 {
2983 throw Exceptions::OutOfRange( "The empty path has no arctimes defined." );
2984 }
2985 if( duration( ) == 0 )
2986 {
2987 throw Exceptions::OutOfRange( "The singleton path has no arctimes defined." );
2988 }
2989 if( t >= HUGE_VAL )
2990 {
2991 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
2992 }
2993 if( t < 0 )
2994 {
2995 return negative_arcTime( - t, t0 );
2996 }
2997
2998 Concrete::Time splineTime = t0;
2999 t0 = modPhysical( t0, Concrete::Time( duration( ) ) );
3000 if( t0 < 0 )
3001 {
3002 t0 += Concrete::Time( duration( ) );
3003 }
3004
3005 const_iterator i1 = begin( );
3006 while( t0 >= 1 )
3007 {
3008 t0 -= Concrete::UNIT_TIME;
3009 ++i1;
3010 if( i1 == end( ) )
3011 {
3012 if( ! closed_ )
3013 {
3014 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3015 }
3016 i1 = begin( );
3017 }
3018 }
3019 const_iterator i2 = i1;
3020 ++i2;
3021
3022 const Concrete::Length arcdelta = Computation::the_arcdelta;
3023 Concrete::Length remainingLength = t;
3024
3025 if( t0 > 0 )
3026 {
3027 if( i2 == end( ) )
3028 {
3029 if( ! closed_ )
3030 {
3031 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3032 }
3033 i2 = begin( );
3034 }
3035 if( (*i1)->front_ == (*i1)->mid_ &&
3036 (*i2)->rear_ == (*i2)->mid_ )
3037 {
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 )
3044 {
3045 remainingLength -= segRestLength;
3046 goto beginLoop;
3047 }
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;
3051 goto done;
3052 }
3053
3054 {
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 >( );
3067
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 >( );
3069
3070 if( segLengthBound < arcdelta )
3071 {
3072 remainingLength -= segLengthBound;
3073 if( remainingLength <= 0 )
3074 {
3075 splineTime += 0.5;
3076 goto done;
3077 }
3078 }
3079 else
3080 {
3081 Concrete::Time dt = Shapes::computeDt( segLengthBound );
3082
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 )
3086 {
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;
3095
3096 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
3097 tmpSum_l += dl;
3098 if( tmpSum_l >= remainingDiv_dt )
3099 {
3100 splineTime += t - t0;
3101 goto done;
3102 }
3103 }
3104 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
3105 }
3106 }
3107
3108 beginLoop:
3109 splineTime = Concrete::Time( ceil( splineTime.offtype< 0, 1 >( ) ) );
3110 ++i1;
3111 if( i1 == end( ) )
3112 {
3113 if( ! closed_ )
3114 {
3115 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3116 }
3117 i1 = begin( );
3118 }
3119 ++i2;
3120 }
3121
3122 for( ; ; ++i1, ++i2, splineTime += Concrete::UNIT_TIME )
3123 {
3124 if( i1 == end( ) )
3125 {
3126 /* If not closed, then the break occurs when i2 reaches end
3127 */
3128 i1 = begin( );
3129 }
3130 if( i2 == end( ) )
3131 {
3132 if( ! closed_ )
3133 {
3134 return Concrete::SplineTime( Concrete::Time( duration( ) ), true );
3135 }
3136 i2 = begin( );
3137 }
3138 if( (*i1)->front_ == (*i1)->mid_ &&
3139 (*i2)->rear_ == (*i2)->mid_ )
3140 {
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 )
3143 {
3144 remainingLength -= segLength;
3145 continue;
3146 }
3147 splineTime += Shapes::straightLineArcTime( remainingLength / segLength );
3148 goto done;
3149 }
3150
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 >( );
3163
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 >( );
3165
3166 if( segLengthBound < arcdelta )
3167 {
3168 remainingLength -= segLengthBound;
3169 if( remainingLength <= 0 )
3170 {
3171 splineTime += 0.5;
3172 goto done;
3173 }
3174 }
3175 else
3176 {
3177 Concrete::Time dt = Shapes::computeDt( segLengthBound );
3178
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 )
3182 {
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;
3191
3192 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
3193 tmpSum_l += dl;
3194 if( tmpSum_l >= remainingDiv_dt )
3195 {
3196 splineTime += t;
3197 goto done;
3198 }
3199 }
3200 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
3201 }
3202 }
3203
3204 done:
3205 return splineTime;
3206 }
3207
3208 Concrete::SplineTime
3209 Lang::ElementaryPath3D::negative_arcTime( const Concrete::Length deltaLen, Concrete::Time t0 ) const
3210 {
3211 if( deltaLen < 0 )
3212 {
3213 throw Exceptions::InternalError( "Negative t in negative_arcTime." );
3214 }
3215 if( empty( ) )
3216 {
3217 throw Exceptions::OutOfRange( "The empty path has no arctimes defined." );
3218 }
3219 if( duration( ) == 0 )
3220 {
3221 throw Exceptions::OutOfRange( "The singleton path has no arctimes defined." );
3222 }
3223
3224 if( deltaLen >= HUGE_VAL )
3225 {
3226 return Concrete::SplineTime( 0, true );
3227 }
3228
3229 Concrete::Time splineTime = t0;
3230 if( isClosed( ) )
3231 {
3232 t0 = modPhysical( t0, Concrete::Time( duration( ) ) );
3233 if( t0 < 0 )
3234 {
3235 t0 += Concrete::Time( duration( ) );
3236 }
3237 }
3238 else
3239 {
3240 if( t0 < 0 )
3241 {
3242 t0 = 0;
3243 }
3244 else if( t0 > Concrete::Time( duration( ) ) )
3245 {
3246 t0 = Concrete::Time( duration( ) );
3247 }
3248 }
3249 t0 = Concrete::Time( duration( ) ) - t0;
3250 // From here on, t0 is the (positive) backwards-time
3251
3252 const_reverse_iterator i1 = rbegin( );
3253 if( closed_ )
3254 {
3255 i1 = rend( );
3256 --i1;
3257 }
3258 while( t0 >= 1 )
3259 {
3260 t0 -= Concrete::UNIT_TIME;
3261 ++i1;
3262 if( i1 == rend( ) )
3263 {
3264 if( ! closed_ )
3265 {
3266 return Concrete::SplineTime( 0, true );
3267 }
3268 i1 = rbegin( );
3269 }
3270 }
3271 const_reverse_iterator i2 = i1;
3272 ++i2;
3273
3274 const Concrete::Length arcdelta = Computation::the_arcdelta;
3275 Concrete::Length remainingLength = deltaLen;
3276
3277 if( t0 > 0 )
3278 {
3279 if( i2 == rend( ) )
3280 {
3281 if( ! closed_ )
3282 {
3283 return Concrete::SplineTime( 0, true );
3284 }
3285 i2 = rbegin( );
3286 }
3287 if( (*i1)->rear_ == (*i1)->mid_ &&
3288 (*i2)->front_ == (*i2)->mid_ )
3289 {
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 )
3296 {
3297 remainingLength -= segRestLength;
3298 goto beginLoop;
3299 }
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;
3303 goto done;
3304 }
3305
3306 {
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 >( );
3319
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 >( );
3321
3322 if( segLengthBound < arcdelta )
3323 {
3324 remainingLength -= segLengthBound;
3325 if( remainingLength <= 0 )
3326 {
3327 splineTime -= 0.5;
3328 goto done;
3329 }
3330 }
3331 else
3332 {
3333 Concrete::Time dt = Shapes::computeDt( segLengthBound );
3334
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 )
3338 {
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;
3347
3348 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
3349 tmpSum_l += dl;
3350 if( tmpSum_l >= remainingDiv_dt )
3351 {
3352 splineTime -= t - t0;
3353 goto done;
3354 }
3355 }
3356 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
3357 }
3358 }
3359
3360 beginLoop:
3361 splineTime = Concrete::Time( floor( splineTime.offtype< 0, 1 >( ) ) );
3362 ++i1;
3363 if( i1 == rend( ) )
3364 {
3365 if( ! closed_ )
3366 {
3367 return Concrete::SplineTime( 0, true );
3368 }
3369 i1 = rbegin( );
3370 }
3371 ++i2;
3372 }
3373
3374 for( ; ; ++i1, ++i2, splineTime -= Concrete::UNIT_TIME )
3375 {
3376 if( i1 == rend( ) )
3377 {
3378 /* If not closed, then the break occurs when i2 reaches end
3379 */
3380 i1 = rbegin( );
3381 }
3382 if( i2 == rend( ) )
3383 {
3384 if( ! closed_ )
3385 {
3386 return Concrete::SplineTime( 0, true );
3387 }
3388 i2 = rbegin( );
3389 }
3390 if( (*i1)->rear_ == (*i1)->mid_ &&
3391 (*i2)->front_ == (*i2)->mid_ )
3392 {
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 )
3395 {
3396 remainingLength -= segLength;
3397 continue;
3398 }
3399 splineTime -= Shapes::straightLineArcTime( remainingLength / segLength );
3400 goto done;
3401 }
3402
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 >( );
3415
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 >( );
3417
3418 if( segLengthBound < arcdelta )
3419 {
3420 remainingLength -= segLengthBound;
3421 if( remainingLength <= 0 )
3422 {
3423 splineTime -= 0.5;
3424 goto done;
3425 }
3426 }
3427 else
3428 {
3429 Concrete::Time dt = Shapes::computeDt( segLengthBound );
3430
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 )
3434 {
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;
3443
3444 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
3445 tmpSum_l += dl;
3446 if( tmpSum_l >= remainingDiv_dt )
3447 {
3448 splineTime -= t;
3449 goto done;
3450 }
3451 }
3452 remainingLength -= tmpSum_l * dt.offtype< 0, 1 >( );
3453 }
3454 }
3455
3456 done:
3457 return splineTime;
3458 }
3459
3460 RefCountPtr< const Lang::ElementaryPath3D >
3461 Lang::ElementaryPath3D::subpath( const Concrete::SplineTime splt1, const Concrete::SplineTime splt2 ) const
3462 {
3463 if( empty( ) )
3464 {
3465 throw Exceptions::OutOfRange( "The empty path has no subpaths." );
3466 }
3467 Lang::ElementaryPath3D * res = new Lang::ElementaryPath3D;
3468
3469 if( splt2.t( ) < splt1.t( ) )
3470 {
3471 throw Exceptions::OutOfRange( "The path-times must come in ascending order." );
3472 }
3473 if( splt2.t( ) != HUGE_VAL && splt2.t( ) > splt1.t( ) + size( ) )
3474 {
3475 throw Exceptions::OutOfRange( "It is forbidden (although not logically necessary) to cycle more than one revolution." );
3476 }
3477
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( ) )
3481 {
3482 gt2 += size( );
3483 }
3484
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.
3489 */
3490 if( fmod( floor( gt1.offtype< 0, 1 >( ) ) + 1, size( ) ) == ceil( gt2.offtype< 0, 1 >( ) ) )
3491 {
3492 Concrete::Time t1;
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 >( );
3509
3510 Concrete::Time tc = t1 - Concrete::UNIT_TIME; /* Note the sign here! */
3511 Concrete::Time td = t1 - t2;
3512
3513 /* Compute new x polynomial using variable change t = t1 - td * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
3514 */
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 );
3519
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;
3525
3526 /* Same thing for y */
3527
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 );
3532
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;
3537
3538 /* Same thing for z */
3539
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 );
3544
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;
3549
3550
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.
3554 */
3555
3556 Concrete::PathPoint3D * ptmp = new Concrete::PathPoint3D( x0new, y0new, z0new );
3557 if( p1->front_ != p1->mid_ || p2->rear_ != p2->mid_ )
3558 {
3559 ptmp->front_ = new Concrete::Coords3D( x1new, y1new, z1new );
3560 }
3561 res->push_back( ptmp );
3562 ptmp = new Concrete::PathPoint3D( x3new, y3new, z3new );
3563 if( p1->front_ != p1->mid_ || p2->rear_ != p2->mid_ )
3564 {
3565 ptmp->rear_ = new Concrete::Coords3D( x2new, y2new, z2new );
3566 }
3567 res->push_back( ptmp );
3568
3569 return RefCountPtr< const Lang::ElementaryPath3D >( res );
3570 }
3571
3572
3573 /* Now, we treat the other case, where the cuts are on different segments. This results in a three phase
3574 * algorithm.
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.
3577 */
3578
3579 Concrete::Time t = 0;
3580 const_iterator i1 = begin( );
3581 for( ; t + 1 <= gt1; t += Concrete::UNIT_TIME, ++i1 )
3582 ;
3583 const_iterator i2 = i1;
3584 ++i2;
3585 if( i2 == end( ) )
3586 {
3587 i2 = begin( );
3588 }
3589
3590 Concrete::Time t1 = gt1 - t;
3591 Concrete::PathPoint3D * p1;
3592 Concrete::PathPoint3D * p2;
3593 if( t1 == 0 )
3594 {
3595 p1 = new Concrete::PathPoint3D( **i1 );
3596 p2 = new Concrete::PathPoint3D( **i2 );
3597 }
3598 else
3599 {
3600 /* Reverse time! */
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 >( );
3613
3614 t1 = Concrete::UNIT_TIME - t1;
3615
3616 /* Compute new x polynomial using variable change t = t1 * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
3617 */
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;
3627
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;
3637
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;
3647
3648 /* Now, reverse time again! */
3649
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.
3653 */
3654
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_ )
3657 {
3658 p1->front_ = new Concrete::Coords3D( x2new.offtype< 0, -3 >( ), y2new.offtype< 0, -3 >( ), z2new.offtype< 0, -3 >( ) );
3659 }
3660
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_ )
3663 {
3664 p2->rear_ = new Concrete::Coords3D( x1new.offtype< 0, -3 >( ), y1new.offtype< 0, -3 >( ), z1new.offtype< 0, -3 >( ) );
3665 }
3666 if( (*i2)->front_ != (*i2)->mid_ )
3667 {
3668 p2->front_ = new Concrete::Coords3D( *(*i2)->front_ );
3669 }
3670 }
3671
3672 /* At this point we know that the rest of this segment shall be included, since the cuts are not on the
3673 * same segment.
3674 */
3675
3676 for( ; t + 1 < gt2; t += Concrete::UNIT_TIME )
3677 {
3678 res->push_back( p1 );
3679 p1 = p2;
3680 i1 = i2;
3681 ++i2;
3682 if( i2 == end( ) )
3683 {
3684 i2 = begin( );
3685 }
3686 p2 = new Concrete::PathPoint3D( **i2 );
3687 }
3688
3689 /* Remember to delete p1 and p2 if unused! */
3690
3691 Concrete::Time t2 = gt2 - t;
3692 if( t2 == 1 )
3693 {
3694 res->push_back( p1 );
3695 res->push_back( p2 );
3696 }
3697 else
3698 {
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_;
3703
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_;
3717
3718 delete p1;
3719 delete p2;
3720
3721 /* Compute new x polynomial using variable change t = t2 * tNew, where tNew is a time parameter in in the range [ 0, 1 ].
3722 */
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;
3732
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 >( );
3737
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;
3742
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 >( );
3747
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;
3752
3753 p1 = new Concrete::PathPoint3D( x0new.offtype< 0, -3 >( ), y0new.offtype< 0, -3 >( ), z0new.offtype< 0, -3 >( ) );
3754 if( doInHandles )
3755 {
3756 p1->front_ = new Concrete::Coords3D( x1new.offtype< 0, -3 >( ), y1new.offtype< 0, -3 >( ), z1new.offtype< 0, -3 >( ) );
3757 }
3758 if( doBackHandle )
3759 {
3760 p1->rear_ = new Concrete::Coords3D( xn1, yn1, zn1 );
3761 }
3762 p2 = new Concrete::PathPoint3D( x3new.offtype< 0, -3 >( ), y3new.offtype< 0, -3 >( ), z3new.offtype< 0, -3 >( ) );
3763 if( doInHandles )
3764 {
3765 p2->rear_ = new Concrete::Coords3D( x2new.offtype< 0, -3 >( ), y2new.offtype< 0, -3 >( ), z2new.offtype< 0, -3 >( ) );
3766 }
3767
3768 res->push_back( p1 );
3769 res->push_back( p2 );
3770 }
3771
3772
3773 return RefCountPtr< const Lang::ElementaryPath3D >( res );
3774 }
3775
3776
3777 RefCountPtr< const Lang::ElementaryPath3D >
3778 Lang::ElementaryPath3D::reverse( ) const
3779 {
3780 /* Note that reversing a closed path is not quite as straight-forward as one might first think!
3781 */
3782
3783 Lang::ElementaryPath3D * res = new Lang::ElementaryPath3D;
3784 if( closed_ )
3785 {
3786 res->close( );
3787 }
3788
3789 const_iterator i = begin( );
3790 if( i == end( ) )
3791 {
3792 return RefCountPtr< const Lang::ElementaryPath3D >( res );
3793 }
3794
3795 if( closed_ )
3796 {
3797 // The first path point remains first!
3798 ++i;
3799 }
3800 for( ; i != end( ); ++i )
3801 {
3802 Concrete::PathPoint3D * newPoint = new Concrete::PathPoint3D( new Concrete::Coords3D( *((*i)->mid_) ) );
3803 if( (*i)->rear_ != (*i)->mid_ )
3804 {
3805 newPoint->front_ = new Concrete::Coords3D( *((*i)->rear_) );
3806 }
3807 if( (*i)->front_ != (*i)->mid_ )
3808 {
3809 newPoint->rear_ = new Concrete::Coords3D( *((*i)->front_) );
3810 }
3811 res->push_front( newPoint );
3812 }
3813 if( closed_ )
3814 {
3815 i = begin( );
3816 Concrete::PathPoint3D * newPoint = new Concrete::PathPoint3D( new Concrete::Coords3D( *((*i)->mid_) ) );
3817 if( (*i)->rear_ != (*i)->mid_ )
3818 {
3819 newPoint->front_ = new Concrete::Coords3D( *((*i)->rear_) );
3820 }
3821 if( (*i)->front_ != (*i)->mid_ )
3822 {
3823 newPoint->rear_ = new Concrete::Coords3D( *((*i)->front_) );
3824 }
3825 res->push_front( newPoint );
3826 }
3827
3828 return RefCountPtr< const Lang::ElementaryPath3D >( res );
3829 }
3830
3831 RefCountPtr< const Lang::ElementaryPath3D >
3832 Lang::ElementaryPath3D::upsample( const Computation::Upsampler3D & sampler ) const
3833 {
3834 throw Exceptions::NotImplemented( "ElementaryPath3D::upsample" );
3835 }
3836
3837 void
3838 Lang::ElementaryPath3D::show( std::ostream & os ) const
3839 {
3840 os << "Elementary 3D subpath with " << size( ) << " path points" ;
3841 }
3842
3843 void
3844 Lang::ElementaryPath3D::getRepresentativePoints( const Lang::Transform3D & tf, Concrete::Coords3D * p0, Concrete::Coords3D * p1, Concrete::Coords3D * p2 ) const
3845 {
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.
3849
3850 Concrete::Area bestArea( 0 );
3851
3852 for( const_iterator i0 = begin( ); ; )
3853 {
3854 Concrete::Coords3D tmp0( (*i0)->mid_->transformed( tf ) );
3855 const_iterator i1 = i0;
3856 ++i1;
3857 for( ; ; )
3858 {
3859 Concrete::Coords3D tmp1( (*i1)->mid_->transformed( tf ) );
3860 const_iterator i2 = i1;
3861 ++i2;
3862 for( ; i2 != end( ); ++i2 )
3863 {
3864 Concrete::Coords3D tmp2( (*i2)->mid_->transformed( tf ) );
3865 Concrete::Area a = Computation::triangleArea( tmp0, tmp1, tmp2 );
3866 if( a > bestArea )
3867 {
3868 *p0 = tmp0;
3869 *p1 = tmp1;
3870 *p2 = tmp2;
3871 bestArea = a;
3872 }
3873 }
3874 --i2; // now i2 points to the element before end
3875 ++i1;
3876 if( i1 == i2 )
3877 {
3878 break;
3879 }
3880 }
3881 --i1; // now i1 points to the second last element
3882 ++i0;
3883 if( i0 == i1 )
3884 {
3885 break;
3886 }
3887 }
3888 }
3889
3890 void
3891 Computation::SegmentPolynomialPair3D::kInit( )
3892 {
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 >( );
3897
3898 kxaD1_0 = kxaD0_1;
3899 kxaD1_1 = 2 * kxaD0_2;
3900 kxaD1_2 = 3 * kxaD0_3;
3901
3902 kxaD2_0 = kxaD1_1;
3903 kxaD2_1 = 2 * kxaD1_2;
3904
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 >( );
3909
3910 kyaD1_0 = kyaD0_1;
3911 kyaD1_1 = 2 * kyaD0_2;
3912 kyaD1_2 = 3 * kyaD0_3;
3913
3914 kyaD2_0 = kyaD1_1;
3915 kyaD2_1 = 2 * kyaD1_2;
3916
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 >( );
3921
3922 kzaD1_0 = kzaD0_1;
3923 kzaD1_1 = 2 * kzaD0_2;
3924 kzaD1_2 = 3 * kzaD0_3;
3925
3926 kzaD2_0 = kzaD1_1;
3927 kzaD2_1 = 2 * kzaD1_2;
3928
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 >( );
3933
3934 kxbD1_0 = kxbD0_1;
3935 kxbD1_1 = 2 * kxbD0_2;
3936 kxbD1_2 = 3 * kxbD0_3;
3937
3938 kxbD2_0 = kxbD1_1;
3939 kxbD2_1 = 2 * kxbD1_2;
3940
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 >( );
3945
3946 kybD1_0 = kybD0_1;
3947 kybD1_1 = 2 * kybD0_2;
3948 kybD1_2 = 3 * kybD0_3;
3949
3950 kybD2_0 = kybD1_1;
3951 kybD2_1 = 2 * kybD1_2;
3952
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 >( );
3957
3958 kzbD1_0 = kzbD0_1;
3959 kzbD1_1 = 2 * kzbD0_2;
3960 kzbD1_2 = 3 * kzbD0_3;
3961
3962 kzbD2_0 = kzbD1_1;
3963 kzbD2_1 = 2 * kzbD1_2;
3964 }
3965
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 )
3969 {
3970 kInit( );
3971
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 >( );
3978 }
3979
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 )
3983 {
3984 kInit( );
3985
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 >( );
3990 }
3991
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 )
3995 {
3996 kInit( );
3997
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 >( );
4002 }
4003
4004 bool
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
4006 {
4007 Concrete::Time ta;
4008 Concrete::Time tb;
4009 Concrete::LengthSquared last_f( HUGE_VAL );
4010 {
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 )
4015 {
4016 for( Concrete::Time ttb = t_blow + 0.5 * STEP_B; ttb < t_bhigh; ttb += STEP_B )
4017 {
4018 Concrete::LengthSquared tmp = squaredDistanceAt( tta, ttb );
4019 if( tmp < last_f )
4020 {
4021 last_f = tmp;
4022 ta = tta;
4023 tb = ttb;
4024 }
4025 }
4026 }
4027 }
4028 Concrete::Time last_ta = - Concrete::UNIT_TIME;
4029 Concrete::Time last_tb = - Concrete::UNIT_TIME;
4030
4031 while( ta < last_ta - t_atol || ta > last_ta + t_atol ||
4032 tb < last_tb - t_btol || tb > last_tb + t_btol )
4033 {
4034 last_ta = ta;
4035 last_tb = tb;
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.
4038 */
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 );
4044
4045 {
4046 /* x-components */
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;
4053
4054 Physical< 1, 0 > deltaD0 = faD0 - fbD0;
4055
4056 objectiveD1a += deltaD0 * faD1;
4057 objectiveD1b += - deltaD0 * fbD1;
4058 objectiveD2aa += faD1 * faD1 + deltaD0 * faD2;
4059 objectiveD2bb += fbD1 * fbD1 - deltaD0 * fbD2;
4060 objectiveD2ab += - faD1 * fbD1;
4061 }
4062
4063 {
4064 /* y-components */
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;
4071
4072 Physical< 1, 0 > deltaD0 = faD0 - fbD0;
4073
4074 objectiveD1a += deltaD0 * faD1;
4075 objectiveD1b += - deltaD0 * fbD1;
4076 objectiveD2aa += faD1 * faD1 + deltaD0 * faD2;
4077 objectiveD2bb += fbD1 * fbD1 - deltaD0 * fbD2;
4078 objectiveD2ab += - faD1 * fbD1;
4079 }
4080
4081 {
4082 /* z-components */
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;
4089
4090 Physical< 1, 0 > deltaD0 = faD0 - fbD0;
4091
4092 objectiveD1a += deltaD0 * faD1;
4093 objectiveD1b += - deltaD0 * fbD1;
4094 objectiveD2aa += faD1 * faD1 + deltaD0 * faD2;
4095 objectiveD2bb += fbD1 * fbD1 - deltaD0 * fbD2;
4096 objectiveD2ab += - faD1 * fbD1;
4097 }
4098
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 )
4102 {
4103 /* The minimization problem is not convex, locally.
4104 * Knowing that we have examined a grid of candidate points, we stop the minimization here.
4105 */
4106 break;
4107 }
4108 else
4109 {
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 );
4114 }
4115
4116 bool onBound = false;
4117 {
4118 double alpha = 1;
4119
4120 if( ta + alpha * step_a < t_alow )
4121 {
4122 alpha = ( t_alow - ta ) / step_a;
4123 onBound = true;
4124 }
4125 else if( ta + alpha * step_a > t_ahigh )
4126 {
4127 alpha = ( t_ahigh - ta ) / step_a;
4128 onBound = true;
4129 }
4130
4131 if( tb + alpha * step_b < t_blow )
4132 {
4133 alpha = ( t_blow - tb ) / step_b;
4134 onBound = true;
4135 }
4136 else if( tb + alpha * step_b > t_bhigh )
4137 {
4138 alpha = ( t_bhigh - tb ) / step_b;
4139 onBound = true;
4140 }
4141
4142 step_a *= alpha;
4143 step_b *= alpha;
4144 }
4145
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 ) )
4148 {
4149 step_a = step_a * 0.5;
4150 step_b = step_b * 0.5;
4151 f = squaredDistanceAt( ta + step_a, tb + step_b );
4152 onBound = false;
4153 }
4154 ta += step_a;
4155 tb += step_b;
4156 last_f = f;
4157
4158 if( onBound )
4159 {
4160 break;
4161 }
4162 }
4163
4164 *dst_a = ta;
4165 *dst_b = tb;
4166 return distanceAt( ta, tb ) < distance_tol;
4167 }
4168
4169 Concrete::LengthSquared
4170 Computation::SegmentPolynomialPair3D::squaredDistanceAt( Concrete::Time ta, Concrete::Time tb ) const
4171 {
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;
4182 }
4183
4184 Concrete::Length
4185 Computation::SegmentPolynomialPair3D::distanceAt( Concrete::Time ta, Concrete::Time tb ) const
4186 {
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 );
4197 }
4198
4199 Bezier::ControlPoints< Concrete::Coords3D >
4200 Computation::SegmentPolynomialPair3D::getControls_a( const Concrete::Time t_low, const Concrete::Time t_high ) const
4201 {
4202 return Bezier::ControlPoints< Concrete::Coords3D >( polyCoeffs_a.subSection( t_low.offtype< 0, 1 >( ), t_high.offtype< 0, 1 >( ) ) );
4203 }
4204
4205 Bezier::ControlPoints< Concrete::Coords3D >
4206 Computation::SegmentPolynomialPair3D::getControls_b( const Concrete::Time t_low, const Concrete::Time t_high ) const
4207 {
4208 return Bezier::ControlPoints< Concrete::Coords3D >( polyCoeffs_b.subSection( t_low.offtype< 0, 1 >( ), t_high.offtype< 0, 1 >( ) ) );
4209 }
4210
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 )
4214 { }
4215
4216 Computation::SegmentSectionPair3D *
4217 Computation::SegmentSectionPair3D::cutAfterAfter( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4218 {
4219 return new SegmentSectionPair3D( baseSegs, t_a0, ta - tol_a, t_b0, tb - tol_b );
4220 }
4221
4222 Computation::SegmentSectionPair3D *
4223 Computation::SegmentSectionPair3D::cutAfterBefore( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4224 {
4225 return new SegmentSectionPair3D( baseSegs, t_a0, ta - tol_a, tb + tol_b, t_b1 );
4226 }
4227
4228 Computation::SegmentSectionPair3D *
4229 Computation::SegmentSectionPair3D::cutBeforeAfter( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4230 {
4231 return new SegmentSectionPair3D( baseSegs, ta + tol_a, t_a1, t_b0, tb - tol_b );
4232 }
4233
4234 Computation::SegmentSectionPair3D *
4235 Computation::SegmentSectionPair3D::cutBeforeBefore( Concrete::Time ta, Concrete::Time tb, Concrete::Time tol_a, Concrete::Time tol_b ) const
4236 {
4237 return new SegmentSectionPair3D( baseSegs, ta + tol_a, t_a1, tb + tol_b, t_b1 );
4238 }
4239
4240 bool
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
4242 {
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,
4248 distance_tol ) )
4249 {
4250 /* An intersection was found to withing tolerances. Do not adjust for robust convergence! */
4251 return true;
4252 }
4253
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 )
4256 {
4257 *dst_a = t_a0 + da;
4258 *dst_b = t_b0 + db;
4259 }
4260 else if( *dst_a < t_a0 + da && *dst_b > t_b1 - db )
4261 {
4262 *dst_a = t_a0 + da;
4263 *dst_b = t_b1 - db;
4264 }
4265 else if( *dst_a > t_a1 - da && *dst_b < t_b0 + db )
4266 {
4267 *dst_a = t_a1 - da;
4268 *dst_b = t_b0 + db;
4269 }
4270 else if( *dst_a > t_a1 - da && *dst_b > t_b1 - db )
4271 {
4272 *dst_a = t_a1 - da;
4273 *dst_b = t_b1 - db;
4274 }
4275 return false;
4276 }
4277
4278 Concrete::Time
4279 Computation::SegmentSectionPair3D::globalTime_a( Concrete::Time t ) const
4280 {
4281 return steps_a( ) + t;
4282 }
4283
4284 Concrete::Length
4285 Computation::SegmentSectionPair3D::distanceAt( Concrete::Time ta, Concrete::Time tb ) const
4286 {
4287 return baseSegs->distanceAt( ta, tb );
4288 }
4289
4290 Concrete::LengthSquared
4291 Computation::SegmentSectionPair3D::squaredDistanceAt( Concrete::Time ta, Concrete::Time tb ) const
4292 {
4293 return baseSegs->squaredDistanceAt( ta, tb );
4294 }
4295
4296 bool
4297 Computation::SegmentSectionPair3D::convexHullSeparatedBy( Concrete::LengthSquared sep ) const
4298 {
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.
4303 */
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. */
4309 size_t nb;
4310 /* Initialize generators for each polytope. */
4311 if( baseSegs->straightLineSeg_a( ) )
4312 {
4313 na = 2;
4314 controls_a.getEndpoints( & ga[0] );
4315 }
4316 else
4317 {
4318 na = 4;
4319 controls_a.get( & ga[0] );
4320 }
4321 if( baseSegs->straightLineSeg_b( ) )
4322 {
4323 nb = 2;
4324 controls_b.getEndpoints( & gb[0] );
4325 }
4326 else
4327 {
4328 nb = 4;
4329 controls_b.get( & gb[0] );
4330 }
4331
4332 double double_distSquaredLow;
4333 double double_distSquaredHigh;
4334 QPSolverStatus status;
4335 Computation::polytope_distance_generator_form( DIM,
4336 na, & ga[0],
4337 nb, & gb[0],
4338 & double_distSquaredLow, & double_distSquaredHigh,
4339 Concrete::LengthSquared::offtype( sep ), -1,
4340 0, 0,
4341 0.5 * Concrete::Length::offtype( Computation::theDistanceTol ), /* Lagrange multiplier tolerance. */
4342 0, 0,
4343 0, 0, /* We don't care about where the optimum is */
4344 & status );
4345 if( status.code != Computation::QP_OK )
4346 {
4347 const char * binaryFilename = "qp-fail-data.binary";
4348 Computation::polytope_distance_generator_form_write_binary_data( binaryFilename,
4349 DIM,
4350 na, & ga[0],
4351 nb, & gb[0] );
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 ) );
4356 }
4357 Concrete::LengthSquared distSquaredLow( double_distSquaredLow );
4358 Concrete::LengthSquared distSquaredHigh( double_distSquaredHigh );
4359 if( distSquaredLow >= sep )
4360 {
4361 return true;
4362 }
4363 hull_dist_ = distSquaredHigh;
4364 return false;
4365 }
4366
4367 void
4368 Computation::SegmentSectionPair3D::update_t_a1( Concrete::Time t )
4369 {
4370 t_a1 = min( t_a1, t );
4371 }
4372
4373 bool
4374 Computation::SegmentSectionPair3D::isEmpty( ) const
4375 {
4376 return t_a1 < t_a0 || t_b1 < t_b0;
4377 }
4378
4379 void
4380 Computation::SegmentSectionPair3D::writeTimes( std::ostream & os ) const
4381 {
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 ;
4384 }
4385
4386
4387 Concrete::Speed
4388 Computation::SegmentSectionPair3D::maxSpeed_a( ) const
4389 {
4390 return baseSegs->maxSpeed_a( );
4391 }
4392
4393 Concrete::Speed
4394 Computation::SegmentSectionPair3D::maxSpeed_b( ) const
4395 {
4396 return baseSegs->maxSpeed_b( );
4397 }
4398
4399
4400 void
4401 Lang::ElementaryPath3D::gcMark( Kernel::GCMarkedSet & marked )
4402 { }
4403
4404 RefCountPtr< const Lang::ElementaryPath2D >
4405 Lang::ElementaryPath3D::make2D( Concrete::Length eyez ) const
4406 {
4407 Lang::ElementaryPath2D * res = new Lang::ElementaryPath2D;
4408 if( closed_ )
4409 {
4410 res->close( );
4411 }
4412
4413 if( eyez == HUGE_VAL )
4414 {
4415 for( const_iterator i = begin( ); i != end( ); ++i )
4416 {
4417 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( (*i)->mid_->make2D( eyez ) );
4418 if( (*i)->rear_ != (*i)->mid_ )
4419 {
4420 newPoint->rear_ = (*i)->rear_->make2D( eyez );
4421 }
4422 if( (*i)->front_ != (*i)->mid_ )
4423 {
4424 newPoint->front_ = (*i)->front_->make2D( eyez );
4425 }
4426 res->push_back( newPoint );
4427 }
4428 }
4429 else
4430 {
4431 const_iterator i1 = begin( );
4432 const_iterator i2 = i1;
4433 ++i2;
4434
4435 Lang::ElementaryPath2D cycleSegment;
4436 Concrete::Coords2D * passedRear_ = 0;
4437
4438 if( closed_ )
4439 {
4440 const_iterator last = end( );
4441 --last;
4442 makeSegment2D( & cycleSegment, & passedRear_, (*last)->mid_, (*last)->front_, (*i1)->rear_, (*i1)->mid_, eyez );
4443 }
4444
4445 for( ; i2 != end( ); ++i1, ++i2 )
4446 {
4447 makeSegment2D( res, & passedRear_, (*i1)->mid_, (*i1)->front_, (*i2)->rear_, (*i2)->mid_, eyez );
4448 }
4449
4450 if( closed_ )
4451 {
4452 if( passedRear_ != 0 )
4453 {
4454 cycleSegment.front( )->rear_ = passedRear_;
4455 }
4456 while( ! cycleSegment.empty( ) )
4457 {
4458 res->push_back( cycleSegment.front( ) );
4459 cycleSegment.pop_front( );
4460 }
4461 }
4462 else
4463 {
4464 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( (*i1)->mid_->make2D( eyez ) );
4465 if( passedRear_ != 0 )
4466 {
4467 newPoint->rear_ = passedRear_;
4468 }
4469 if( (*i1)->front_ != (*i1)->mid_ )
4470 {
4471 newPoint->front_ = (*i1)->front_->make2D( eyez );
4472 }
4473 res->push_back( newPoint );
4474 }
4475 }
4476
4477 return RefCountPtr< const Lang::ElementaryPath2D >( res );
4478 }
4479
4480 void
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 )
4482 {
4483 if( _p1 == _p0 && _p2 == _p3 )
4484 {
4485 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( _p0->make2D( eyez ) );
4486 if( *passedRear_ != 0 )
4487 {
4488 newPoint->rear_ = *passedRear_;
4489 *passedRear_ = 0;
4490 }
4491 dst->push_back( newPoint );
4492 return;
4493 }
4494
4495 Concrete::Coords3D p0 = *_p0;
4496 std::stack< Concrete::Coords3D > pointStack;
4497 pointStack.push( *_p3 );
4498 pointStack.push( *_p2 );
4499 pointStack.push( *_p1 );
4500
4501 while( pointStack.size( ) > 2 )
4502 {
4503 Concrete::Coords3D p1 = pointStack.top( );
4504 pointStack.pop( );
4505 Concrete::Coords3D p2 = pointStack.top( );
4506 pointStack.pop( );
4507 Concrete::Coords3D p3 = pointStack.top( );
4508 // Note that p3 is not popped!
4509
4510 if( viewDistortionTooBig( p0, p1, p2, p3, eyez, 0.003 ) )
4511 {
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_ );
4520 }
4521 else
4522 {
4523 Concrete::PathPoint2D * newPoint = new Concrete::PathPoint2D( p0.make2D( eyez ) );
4524 if( *passedRear_ != 0 )
4525 {
4526 newPoint->rear_ = *passedRear_;
4527 }
4528 newPoint->front_ = p1.make2D( eyez );
4529 dst->push_back( newPoint );
4530
4531 *passedRear_ = p2.make2D( eyez );
4532 p0 = p3;
4533 pointStack.pop( );
4534 }
4535 }
4536 }
4537
4538 double
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 )
4540 {
4541 Bezier::ControlPoints< Concrete::Coords3D > bezier3D( p0, p1, p2, p3 );
4542
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 );
4547
4548 Bezier::ControlPoints< Concrete::Coords2D > bezier2D( p02D, p12D, p22D, p32D );
4549
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 )
4555 {
4556 // The path is degenerate, so it isn't distorted at all.
4557 return false;
4558 }
4559 threshold *= _threshold;
4560
4561 for( double t = 0.25; t < 0.9; t += 0.25 )
4562 {
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 )
4566 {
4567 return true;
4568 }
4569 }
4570 return false;
4571 }
4572
4573 void
4574 Lang::ElementaryPath3D::dashifyIn2D( RefCountPtr< const Lang::Group2D > * res, Concrete::Length eyez, const RefCountPtr< const Kernel::GraphicsState > & metaState ) const
4575 {
4576 if( empty( ) )
4577 {
4578 return;
4579 }
4580
4581 Kernel::GraphicsState solidValue;
4582 solidValue.dash_ = Lang::THE_SOLID_DASH;
4583 RefCountPtr< const Kernel::GraphicsState > solidState( new Kernel::GraphicsState( solidValue, *metaState ) );
4584
4585 Lang::Dash::Iterator dashi = metaState->dash_->begin( );
4586
4587 Lang::ElementaryPath3D newPath;
4588
4589 Concrete::Length remainingLength = dashi.getLength( );
4590
4591 const_iterator i1 = begin( );
4592
4593 Concrete::Coords3D * passedRear_ = 0;
4594 if( (*i1)->rear_ != (*i1)->mid_ )
4595 {
4596 passedRear_ = new Concrete::Coords3D( *( (*i1)->rear_ ) );
4597 }
4598
4599 Concrete::Coords3D * lastMid_ = 0;
4600
4601 const_iterator i2 = i1;
4602 ++i2;
4603 for( ; i1 != end( ); ++i1, ++i2 )
4604 {
4605 if( i2 == end( ) )
4606 {
4607 if( closed_ )
4608 {
4609 i2 = begin( );
4610 lastMid_ = new Concrete::Coords3D( *( (*i2)->mid_ ) );
4611 }
4612 else
4613 {
4614 lastMid_ = new Concrete::Coords3D( *( (*i1)->mid_ ) );
4615 break;
4616 }
4617 }
4618 dashifySegment( res, newPath, remainingLength, & passedRear_,
4619 (*i1)->mid_, (*i1)->front_, (*i2)->rear_, (*i2)->mid_,
4620 eyez,
4621 solidState,
4622 dashi );
4623 }
4624
4625 if( dashi.isOn( ) )
4626 {
4627 Concrete::PathPoint3D * newPoint = new Concrete::PathPoint3D( lastMid_ );
4628 if( passedRear_ != 0 )
4629 {
4630 newPoint->rear_ = passedRear_;
4631 passedRear_ = 0;
4632 }
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 ),
4636 "S" ) ),
4637 *res,
4638 metaState ) );
4639 newPath.clear( );
4640 }
4641 else
4642 {
4643 delete lastMid_;
4644 if( passedRear_ != 0 )
4645 {
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.
4648 }
4649 if( ! newPath.empty( ) )
4650 {
4651 throw Exceptions::InternalError( "There are points in the newPath, but the dash iterator is not on." );
4652 }
4653 }
4654 }
4655
4656 void
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 )
4666 {
4667 if( p1 == p0 &&
4668 p2 == p3 )
4669 {
4670 if( dashi.isOn( ) )
4671 {
4672 Concrete::PathPoint3D * newPoint = new Concrete::PathPoint3D( new Concrete::Coords3D( *p0 ) );
4673 if( *passedRear_ != 0 )
4674 {
4675 newPoint->rear_ = *passedRear_;
4676 *passedRear_ = 0;
4677 }
4678 newPath.push_back( newPoint );
4679 // Note that on a straight segment, we don't set **passedRear_ to *p2
4680 }
4681
4682 const Concrete::Length segLength = hypotPhysical( p0->x_ - p3->x_, p0->y_ - p3->y_, p0->z_ - p3->z_ );
4683 if( segLength < remainingLength )
4684 {
4685 remainingLength -= segLength;
4686 return;
4687 }
4688
4689 Concrete::Length l = remainingLength;
4690 {
4691 if( dashi.isOn( ) )
4692 {
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 ),
4700 "S" ) ),
4701 *res,
4702 solidState ) );
4703 newPath.clear( );
4704 }
4705 }
4706
4707 while( l <= segLength )
4708 {
4709 ++dashi;
4710 if( ! newPath.empty( ) )
4711 {
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 ),
4719 "S" ) ),
4720 *res,
4721
4722 solidState ) );
4723 newPath.clear( );
4724 }
4725
4726 if( dashi.isOn( ) )
4727 {
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 );
4733 }
4734
4735 l += dashi.getLength( );
4736 }
4737
4738 remainingLength = l - segLength;
4739 return;
4740 }
4741
4742 Bezier::ControlPoints< Concrete::Coords3D > controls( *p0, *p1, *p2, *p3 );
4743 Bezier::PolyCoeffs< Concrete::Coords3D > poly( controls );
4744
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 >( );
4757
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 );
4760
4761 Concrete::Time t1 = 0;
4762 laterArcTime( x0, y0, z0, x1, y1, z1, x2, y2, z2, x3, y3, z3, & t1, & remainingLength, dt );
4763 if( t1 > 1 )
4764 {
4765 if( dashi.isOn( ) )
4766 {
4767 Concrete::PathPoint3D * newPoint = new Concrete::PathPoint3D( new Concrete::Coords3D( *p0 ) );
4768 if( *passedRear_ != 0 )
4769 {
4770 newPoint->rear_ = *passedRear_;
4771 *passedRear_ = 0;
4772 }
4773 newPoint->front_ = new Concrete::Coords3D( *p1 );
4774 newPath.push_back( newPoint );
4775 *passedRear_ = new Concrete::Coords3D( *p2 );
4776 }
4777 return;
4778 }
4779
4780 if( dashi.isOn( ) )
4781 {
4782 Bezier::ControlPoints< Concrete::Coords3D > stroke( poly.subSection( 0, t1.offtype< 0, 1 >( ) ) );
4783 {
4784 Concrete::PathPoint3D * newPoint = new Concrete::PathPoint3D( new Concrete::Coords3D( *p0 ) );
4785 if( *passedRear_ != 0 )
4786 {
4787 newPoint->rear_ = *passedRear_;
4788 *passedRear_ = 0;
4789 }
4790 newPoint->front_ = new Concrete::Coords3D( stroke.p1_ );
4791 newPath.push_back( newPoint );
4792 }
4793 {
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 );
4797 }
4798 *res = RefCountPtr< const Lang::Group2D >( new Lang::GroupPair2D( RefCountPtr< Lang::Drawable2D >( new Lang::PaintedPath2D( solidState,
4799 newPath.make2D( eyez ),
4800 "S" ) ),
4801 *res,
4802 solidState ) );
4803 newPath.clear( );
4804 }
4805
4806 while( true )
4807 {
4808 ++dashi;
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 );
4812 if( t1 > 1 )
4813 {
4814 if( dashi.isOn( ) )
4815 {
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_ );
4821 }
4822 return;
4823 }
4824
4825 if( dashi.isOn( ) )
4826 {
4827 Bezier::ControlPoints< Concrete::Coords3D > stroke( poly.subSection( t0.offtype< 0, 1 >( ), t1.offtype< 0, 1 >( ) ) );
4828 {
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 );
4833 }
4834 {
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 );
4838 }
4839 *res = RefCountPtr< const Lang::Group2D >( new Lang::GroupPair2D( RefCountPtr< Lang::Drawable2D >( new Lang::PaintedPath2D( solidState,
4840 newPath.make2D( eyez ),
4841 "S" ) ),
4842 *res,
4843 solidState ) );
4844 newPath.clear( );
4845 }
4846 }
4847 }
4848
4849 void
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 )
4851 {
4852 Physical< 1, -1 > tmpSum_l = *l / dt;
4853 for( Concrete::Time t = *_t; t < 1; t += dt )
4854 {
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;
4863
4864 Concrete::Length dl = hypotPhysical( vx, vy, vz ).offtype< 0, -1 >( );
4865 tmpSum_l -= dl.offtype< 0, 1 >( );
4866 if( tmpSum_l < 0 )
4867 {
4868 *_t = t - ( tmpSum_l / dl.offtype< 0, 1 >( ) ) * dt;
4869 *l = 0;
4870 return;
4871 }
4872 }
4873 *_t = HUGE_VAL;
4874 *l = tmpSum_l * dt;
4875 }
4876
4877 void
4878 Lang::ElementaryPath3D::pushCloseDeleteOther( std::list< Computation::SegmentSectionPair3D * > * work, Computation::SegmentSectionPair3D * item, Concrete::LengthSquared closeDist )
4879 {
4880 if( item->isEmpty( )
4881 || item->convexHullSeparatedBy( closeDist ) )
4882 {
4883 delete item;
4884 }
4885 else
4886 {
4887 work->push_back( item );
4888 }
4889 }
Graphics language