Trax3 3.1.0
trax track library
Loading...
Searching...
No Matches
TrackJoint.h
1// trax track library
2// AD 2014
3//
4// "we are all the voices under the tracks"
5//
6// Funeral Party
7//
8// Copyright (c) 2025 Trend Redaktions- und Verlagsgesellschaft mbH
9// Copyright (c) 2019 Marc-Michael Horstmann
10//
11// Permission is hereby granted to any person obtaining a copy of this software
12// and associated source code (the "Software"), to use, view, and study the
13// Software for personal or internal business purposes, subject to the following
14// conditions:
15//
16// 1. Redistribution, modification, sublicensing, or commercial use of the
17// Software is NOT permitted without prior written consent from the copyright
18// holder.
19//
20// 2. The Software is provided "AS IS", without warranty of any kind, express
21// or implied.
22//
23// 3. All copies of the Software must retain this license notice.
24//
25// For further information, please contact: horstmann.marc@trendverlag.de
26
27#pragma once
28
29#include "trax/Configuration.h"
30#include "trax/TrackData.h"
31#include "trax/Units.h"
32
33#include <iostream>
34#include <memory>
35#include <sstream>
36
37namespace trax{
38
44 template<typename Valtype>
45 class TrackJoint : public TrackData<Valtype>{
46 public:
47 TrackJoint() noexcept;
48 virtual ~TrackJoint(){}
49
50
55 void SetTimeStep( Time step ) noexcept;
56
57
59 Time GetTimeStep() const noexcept;
60
61
63 bool SetMovingBody( const spat::Frame<Valtype>& bodyFrame ) noexcept;
64
65
67 const spat::Frame<Valtype>& GetMovingBody() const noexcept;
68
69
71 void SetMovingBodyCOM( const spat::Frame<Valtype>& localCom ) noexcept;
72
73
75 bool SetTrackBody( const spat::Frame<Valtype>& bodyFrame ) noexcept;
76
77
79 const spat::Frame<Valtype>& GetTrackBody() const noexcept;
80
81
83 void SetTrackBodyCOM( const spat::Frame<Valtype>& localCom ) noexcept;
84
85
89 void Anchor( const spat::Frame<Valtype>& anchor ) noexcept;
90
91
93 inline const spat::Frame<Valtype>& Anchor() const noexcept;
94
95
97 inline const spat::Frame<Valtype>& GlobalAnchor() const noexcept;
98
99
109
111 void NormalForceLimits( Valtype min, Valtype max );
112
113
115 void BinormalForceLimits( Valtype min, Valtype max );
116
117
119 void NormalTorqueLimit( Valtype max );
120
121
123 void TorqueLimit( Valtype max );
124
125
127 void MotorTarget( Valtype targetVelocity ) noexcept;
128
129
131 void MotorForceLimits( Valtype min, Valtype max );
132
133
135 void MotorForceMax( Valtype max );
136
137
139 void MotorForceMin( Valtype min );
140
141
143 Valtype MotorTarget() const noexcept;
144
145
147 Valtype MotorForceMax() const noexcept;
148
149
151 Valtype MotorForceMin() const noexcept;
153
154
161 void ThresholdP( Valtype threshold ) noexcept;
162
170 void ThresholdT( Valtype threshold ) noexcept;
171
178 void ThresholdN( Valtype threshold ) noexcept;
179
186 void ThresholdB( Valtype threshold ) noexcept;
187
188
196 void Flange( Valtype flange ) noexcept;
197
198
211 void ErrorReductionParameter( Valtype erp ) noexcept;
212
213
215 Valtype ErrorReductionParameter() const noexcept;
216
217
224 void SetDerailed( bool bDerailed = true ) noexcept;
225
226
228 bool IsDerailed() const noexcept;
229
230
238 inline unsigned short DimensionsTotal() const noexcept;
239
241 inline bool IsAngularConstraint( unsigned short dim ) const noexcept;
242
244 inline bool IsDriveConstraint( unsigned short dim ) const noexcept;
245
247 inline bool IsConstraintLimited( unsigned short dim ) const noexcept;
248
250 inline void Jacobian(
251 unsigned short dim,
252 spat::Vector<Valtype>& Jm,
253 spat::Vector<Valtype>& Rm,
254 spat::Vector<Valtype>& Jt,
255 spat::Vector<Valtype>& Rt,
256 Valtype& VTarget,
257 Valtype& E,
258 Valtype& fmin,
259 Valtype& fmax ) const noexcept
260 {
261 if( bMovingTrack ){
262 switch( dim ){
263 case 0:
264 Constraint1( Jm, Rm, Jt, Rt, E, fmin, fmax );
265 break;
266 case 1:
267 Constraint2( Jm, Rm, Jt, Rt, E, fmin, fmax );
268 break;
269 case 2:
270 Constraint3( Jm, Rm, Jt, Rt, E, fmin, fmax );
271 break;
272 case 3:
273 Constraint4( Rm, Rt, E, fmin, fmax );
274 break;
275 case 4:
276 Constraint5( Jm, Rm, Jt, Rt, E, fmin, fmax );
277 break;
278 case 5:
279 Constraint6( Jm, Rm, Jt, Rt, VTarget, fmin, fmax );
280 break;
281 default:
282 assert( !"Dimension index invalid!" );
283 }
284 }
285 else{
286 switch( dim ){
287 case 0:
288 Constraint1( Jm, Rm, E, fmin, fmax );
289 break;
290 case 1:
291 Constraint2( Jm, Rm, E, fmin, fmax );
292 break;
293 case 2:
294 Constraint3( Jm, Rm, E, fmin, fmax );
295 break;
296 case 3:
297 Constraint4( Rm, E, fmin, fmax );
298 break;
299 case 4:
300 Constraint5( Jm, Rm, E, fmin, fmax );
301 break;
302 case 5:
303 Constraint6( Jm, Rm, VTarget, fmin, fmax );
304 break;
305 default:
306 assert( !"Dimension index invalid!" );
307 }
308 }
309
310 E *= erp;
311 }
312
313
314 using TrackData<Valtype>::F;
315 using TrackData<Valtype>::wF;
316 using TrackData<Valtype>::c;
317 using TrackData<Valtype>::t;
318
319
322 inline spat::Vector<Valtype> MovingBodyGlobalCOM2AnchorOffset() const noexcept{
323 return dA;
324 }
325
328 inline void Precalculate( const spat::Frame<Valtype>& bodyFrame ) noexcept;
329
330 virtual bool IsMovableBodySleeping() const noexcept = 0;
331
332 virtual void WakeUp() const = 0;
333
334 virtual void UpdateBodies() = 0;
335
336 virtual void UpdateTrackBody( std::shared_ptr<const struct Body> pToBody ) = 0;
337
338 virtual void release() = 0;
339 protected:
340 virtual void markDirty() noexcept = 0;
341
342 private:
343 spat::Frame<Valtype> Fm;
344 spat::Frame<Valtype> Ft;
345 spat::Frame<Valtype> A;
346
347 spat::Frame<Valtype> m_MovingBodyCOM;
348 spat::Frame<Valtype> m_TrackBodyCOM;
349 spat::Frame<Valtype> m_MovingBodyFrame;
350 spat::Frame<Valtype> m_TrackBodyFrame;
351 spat::Frame<Valtype> m_LocalAnchor; // The frame relative to Fm to be aligned with the track frame.
352
353
354 Valtype erp;
355 Valtype vTarget;
356 Valtype motor_force_min;
357 Valtype motor_force_max;
358 bool motor_force_limited;
359 Valtype flange;
360
361 // Relative rotations to be applied to fix error between
362 // body orientation and supposed track orientation in global coordinates.
363 spat::Vector<Valtype> eT;
364 spat::Vector<Valtype> eN;
365 spat::Vector<Valtype> eB;
366 spat::Vector<Valtype> eP;
367 //precalculated values:
368 spat::Vector<Valtype> dA;
369 spat::Vector<Valtype> dF;
370
371 unsigned short m_Dims, m_LimDims;
372 bool bDerailed = false;
373
374 Valtype normal_force_min;
375 Valtype normal_force_max;
376 bool normal_force_limited;
377 Valtype binormal_force_min;
378 Valtype binormal_force_max;
379 bool binormal_force_limited;
380 Valtype normal_torque_max;
381 bool normal_torque_limited;
382 Valtype torque_max;
383 bool torque_limited;
384
385 bool thresholdPViolated;
386 Valtype thresholdP;
387 bool thresholdTViolated;
388 Valtype thresholdT;
389 bool thresholdNViolated;
390 Valtype thresholdN;
391 bool thresholdBViolated;
392 Valtype thresholdB;
393
394 bool bMovingTrack;
395 Time timeStep = fixed_timestep;
396
397 static const Valtype maxval;
398
399 // Wt,Wm: angular velocities of the two bodies
400 // Vt,Vm: linear velocities of the two bodies
401 // E: error correction factors
402
403 //Va = Vm + Wm x (A.P - Fm.P) : linear velocity of anchor point
404 //Wa = Wm : angular velocity of anchor point
405 //Vp = Vt + Wt x (wF.P - Ft.P) : linear velocitiy of track point
406 //Wp = Wt : angular velocity of track point
407
408 // Jm*Vm + Rm*Wm + Jt*Vt + Rt*Wt = erp/dt * E : constraint equation
409
410
415 inline void Constraint1(
416 spat::Vector<Valtype>& Jm1,
417 spat::Vector<Valtype>& Rm1,
418 spat::Vector<Valtype>& Jt1,
419 spat::Vector<Valtype>& Rt1,
420 Valtype& E1,
421 Valtype& f1min,
422 Valtype& f1max ) const noexcept
423 {
424 //Jm1 = wF.N;
425 //Rm1 = (A.P - Fm.P) % wF.N;
426 //Jt1 = -wF.N;
427 //Rt1 = wF.N % (wF.P - Ft.P);
428 //E1 = (wF.P - A.P) * wF.N;
429 //f1min = normal_force_min;
430 //f1max = normal_force_max;
431
432 Jm1 = wF.N;
433 Rm1 = dA % wF.N;
434 Jt1 = -wF.N;
435 Rt1 = wF.N % dF;
436 E1 = eP * wF.N;
437 f1min = normal_force_min;
438 f1max = normal_force_max;
439 }
440
441 inline void Constraint1(
442 spat::Vector<Valtype>& Jm1,
443 spat::Vector<Valtype>& Rm1,
444 Valtype& E1,
445 Valtype& f1min,
446 Valtype& f1max ) const noexcept
447 {
448 Jm1 = wF.N;
449 Rm1 = dA % wF.N;
450 E1 = eP * wF.N;
451 f1min = normal_force_min;
452 f1max = normal_force_max;
453 }
454
455
460 inline void Constraint2(
461 spat::Vector<Valtype>& Jm2,
462 spat::Vector<Valtype>& Rm2,
463 spat::Vector<Valtype>& Jt2,
464 spat::Vector<Valtype>& Rt2,
465 Valtype& E2,
466 Valtype& f2min,
467 Valtype& f2max ) const noexcept
468 {
469 //Jm2 = wF.B;
470 //Rm2 = (A.P - Fm.P) % wF.B;
471 //Jt2 = -wF.B;
472 //Rt2 = wF.B % (wF.P - Ft.P);
473 //E2 = (wF.P - A.P) * wF.B;
474 //f2min = binormal_force_min;
475 //f2max = binormal_force_max;
476
477 Jm2 = wF.B;
478 Rm2 = dA % wF.B;
479 Jt2 = -wF.B;
480 Rt2 = wF.B % dF;
481 E2 = eP * wF.B;
482 f2min = binormal_force_min;
483 f2max = binormal_force_max;
484 }
485
486 inline void Constraint2(
487 spat::Vector<Valtype>& Jm2,
488 spat::Vector<Valtype>& Rm2,
489 Valtype& E2,
490 Valtype& f2min,
491 Valtype& f2max ) const noexcept
492 {
493 Jm2 = wF.B;
494 Rm2 = dA % wF.B;
495 E2 = eP * wF.B;
496 f2min = binormal_force_min;
497 f2max = binormal_force_max;
498 }
499
504 inline void Constraint3(
505 spat::Vector<Valtype>& Jm3,
506 spat::Vector<Valtype>& Rm3,
507 spat::Vector<Valtype>& Jt3,
508 spat::Vector<Valtype>& Rt3,
509 Valtype& E3,
510 Valtype& f3min,
511 Valtype& f3max ) const noexcept
512 {
513 //Jm3 = -t * wF.T;
514 //Rm3 = wF.T - t * (A.P - Fm.P) % wF.T;
515 //Jt3 = t * wF.T;
516 //Rt3 = -wF.T + t * (wF.P - Ft.P) % wF.T;
517 //E3 = (A.N % wF.N + A.B % wF.B) * wF.T;
518 //f3min = -torque_max;
519 //f3max = torque_max;
520
521 Jm3 = -t * wF.T;
522 Rm3 = wF.T - t * dA % wF.T;
523 Jt3 = t * wF.T;
524 Rt3 = -wF.T + t * dF % wF.T;
525 E3 = (eN + eB) * wF.T;
526 f3min = -torque_max;
527 f3max = torque_max;
528 }
529
530 inline void Constraint3(
531 spat::Vector<Valtype>& Jm3,
532 spat::Vector<Valtype>& Rm3,
533 Valtype& E3,
534 Valtype& f3min,
535 Valtype& f3max ) const noexcept
536 {
537 Jm3 = -t * wF.T;
538 Rm3 = wF.T - t * dA % wF.T;
539 E3 = (eN + eB) * wF.T;
540 f3min = -torque_max;
541 f3max = torque_max;
542 }
543
544
549 inline void Constraint4(
550 spat::Vector<Valtype>& Rm4,
551 spat::Vector<Valtype>& Rt4,
552 Valtype& E4,
553 Valtype& f4min,
554 Valtype& f4max ) const noexcept
555 {
556 //Rm4 = F.N;
557 //Rt4 = -F.N;
558 //E4 = (A.T % wF.T + A.N % wF.N + A.B % wF.B) * F.N;
559 //f4min = -normal_torque_max;
560 //f4max = normal_torque_max;
561
562 Rm4 = F.N;
563 Rt4 = -F.N;
564 E4 = (eT + eN + eB) * F.N;
565 f4min = -normal_torque_max;
566 f4max = normal_torque_max;
567
568 }
569
570 inline void Constraint4(
571 spat::Vector<Valtype>& Rm4,
572 Valtype& E4,
573 Valtype& f4min,
574 Valtype& f4max ) const noexcept
575 {
576 Rm4 = F.N;
577 E4 = (eT + eN + eB) * F.N;
578 f4min = -normal_torque_max;
579 f4max = normal_torque_max;
580 }
581
582
587 inline void Constraint5(
588 spat::Vector<Valtype>& Jm5,
589 spat::Vector<Valtype>& Rm5,
590 spat::Vector<Valtype>& Jt5,
591 spat::Vector<Valtype>& Rt5,
592 Valtype& E5,
593 Valtype& f5min,
594 Valtype& f5max ) const noexcept
595 {
596 //Jm5 = -c * wF.T;
597 //Rm5 = F.B - c * (A.P - Fm.P) % wF.T;
598 //Jt5 = c * wF.T;
599 //Rt5 = -F.B + c * (wF.P - Ft.P) % wF.T;
600 //E5 = (A.T % wF.T + A.N % wF.N + A.B % wF.B) * F.B;
601 //f5min = -torque_max;
602 //f5max = torque_max;
603
604 Jm5 = -c * wF.T;
605 Rm5 = F.B - c * dA % wF.T;
606 Jt5 = c * wF.T;
607 Rt5 = -F.B + c * dF % wF.T;
608 E5 = (eT + eN + eB) * F.B;
609 f5min = -torque_max;
610 f5max = torque_max;
611 }
612
613 inline void Constraint5(
614 spat::Vector<Valtype>& Jm5,
615 spat::Vector<Valtype>& Rm5,
616 Valtype& E5,
617 Valtype& f5min,
618 Valtype& f5max ) const noexcept
619 {
620 Jm5 = -c * wF.T;
621 Rm5 = F.B - c * dA % wF.T;
622 E5 = (eT + eN + eB) * F.B;
623 f5min = -torque_max;
624 f5max = torque_max;
625 }
626
627
632 inline void Constraint6(
633 spat::Vector<Valtype>& Jm6,
634 spat::Vector<Valtype>& Rm6,
635 spat::Vector<Valtype>& Jt6,
636 spat::Vector<Valtype>& Rt6,
637 Valtype& VTarget6,
638 Valtype& f6min,
639 Valtype& f6max ) const noexcept
640 {
641 //Jm6 = wF.T;
642 //Rm6 = (A.P - Fm.P) % wF.T;
643 //Jt6 = -wF.T;
644 //Rt6 = wF.T % (wF.P - Ft.P);
645 //VTarget6= vTarget;
646 //f6min = motor_force_min;
647 //f6max = motor_force_max;
648
649 Jm6 = wF.T;
650 Rm6 = dA % wF.T;
651 Jt6 = -wF.T;
652 Rt6 = wF.T % dF;
653 VTarget6= vTarget;
654 f6min = motor_force_min;
655 f6max = motor_force_max;
656 }
657
658 inline void Constraint6(
659 spat::Vector<Valtype>& Jm6,
660 spat::Vector<Valtype>& Rm6,
661 Valtype& VTarget6,
662 Valtype& f6min,
663 Valtype& f6max ) const noexcept
664 {
665 Jm6 = wF.T;
666 Rm6 = dA % wF.T;
667 VTarget6= vTarget;
668 f6min = motor_force_min;
669 f6max = motor_force_max;
670 }
671 };
672
673template<typename Valtype>
674const Valtype TrackJoint<Valtype>::maxval = infinite;
675
676template<typename Valtype>
677inline TrackJoint<Valtype>::TrackJoint() noexcept
678 : erp {0.25f},
679 vTarget {0},
680 motor_force_min {0},
681 motor_force_max {0},
682 motor_force_limited {true},
683 flange {maxval},
684 eT {spat::Vector<Valtype>::specials::null},
685 eN {spat::Vector<Valtype>::specials::null},
686 eB {spat::Vector<Valtype>::specials::null},
687 eP {spat::Vector<Valtype>::specials::null},
688 dA {spat::Vector<Valtype>::specials::null},
689 dF {spat::Vector<Valtype>::specials::null},
690 m_Dims {0},
691 m_LimDims {0},
692 normal_force_min {-maxval},
693 normal_force_max {maxval},
694 normal_force_limited {false},
695 binormal_force_min {0},
696 binormal_force_max {maxval},
697 binormal_force_limited {true},
698 normal_torque_max {maxval},
699 normal_torque_limited {false},
700 torque_max {maxval},
701 torque_limited {false},
702 thresholdPViolated {false},
703 thresholdP {static_cast<Valtype>(default_derailing_distance.Units())},
704 thresholdTViolated {false},
705 thresholdT {1},
706 thresholdNViolated {false},
707 thresholdN {1},
708 thresholdBViolated {false},
709 thresholdB {1},
710 bMovingTrack {false}
711{
712 TrackData<Valtype>::Init();
713 Fm.Init();
714 Ft.Init();
715 A.Init();
716 m_MovingBodyCOM.Init();
717 m_TrackBodyCOM.Init();
718 m_MovingBodyFrame.Init();
719 m_TrackBodyFrame.Init();
720 m_LocalAnchor.Init();
721}
722
723template<typename Valtype>
724inline void TrackJoint<Valtype>::SetTimeStep(Time step) noexcept{
725 assert( step >= 0_s );
726 timeStep = step;
727}
728
729template<typename Valtype>
730inline Time TrackJoint<Valtype>::GetTimeStep() const noexcept{
731 return timeStep;
732}
733
734template<typename Valtype>
735inline void TrackJoint<Valtype>::Precalculate( const spat::Frame<Valtype>& bodyFrame ) noexcept{
736 m_MovingBodyFrame = bodyFrame;
737 A = m_LocalAnchor;
738 m_MovingBodyFrame.ToParent( A );
739 Fm = m_MovingBodyCOM;
740 m_MovingBodyFrame.ToParent( Fm );
741
742 // calculate error vectors:
743 eT = A.T % wF.T; // the angular errors are accurate only if the
744 eN = A.N % wF.N; // angle is < pi/2. Especially if the vectors
745 eB = A.B % wF.B; // are antiparallel, no derailment will happen.
746 eP = wF.P - A.P;
747
748 //precalculated values:
749 dA = A.P - Fm.P;
750}
751
752template<typename Valtype>
753inline bool TrackJoint<Valtype>::SetMovingBody( const spat::Frame<Valtype>& bodyFrame ) noexcept{
754 Precalculate( bodyFrame );
755
756 // threshold:
757 //std::cout << "Positional error: " << eP.Length() << std::endl;
758
759 if( eP.Length() > thresholdP ){
760 thresholdPViolated = true;
761 m_Dims = 0;
762 m_LimDims = 0;
763 std::clog << Verbosity::detailed << "Trackjoint: thresholdPViolated violated! (" << eP.Length() << " > " << thresholdP << ")" << std::endl;
764 return false;
765 }
766 else
767 thresholdPViolated = false;
768
769 if( eN.Length() > thresholdT &&
770 eB.Length() > thresholdT )
771 {
772 thresholdTViolated = true;
773 m_Dims = 0;
774 m_LimDims = 0;
775 std::clog << Verbosity::detailed << "Trackjoint: thresholdTViolated violated!" << std::endl;
776 return false;
777 }
778 else
779 thresholdTViolated = false;
780
781 if( eT.Length() > thresholdN &&
782 eB.Length() > thresholdN ){
783 thresholdNViolated = true;
784 m_Dims = 0;
785 m_LimDims = 0;
786 std::clog << Verbosity::detailed << "Trackjoint: thresholdNViolated violated!" << std::endl;
787 return false;
788 }
789 else
790 thresholdNViolated = false;
791
792 if( eT.Length() > thresholdB &&
793 eN.Length() > thresholdB )
794 {
795 thresholdBViolated = true;
796 m_Dims = 0;
797 m_LimDims = 0;
798 std::clog << Verbosity::detailed << "Trackjoint: thresholdBViolated violated!" << std::endl;
799 return false;
800 }
801 else
802 thresholdBViolated = false;
803
804 if( motor_force_min < 0 || motor_force_max > 0 ){
805 m_Dims = 6;
806 m_LimDims = (normal_force_limited ? 1u : 0) +
807 (binormal_force_limited ? 1u : 0) +
808 (normal_torque_limited ? 1u : 0) +
809 (torque_limited ? 2u : 0) +
810 (motor_force_limited ? 1u : 0);
811 }
812 else{
813 m_Dims = 5;
814 m_LimDims = (normal_force_limited ? 1u : 0) +
815 (binormal_force_limited ? 1u : 0) +
816 (normal_torque_limited ? 1u : 0) +
817 (torque_limited ? 2u : 0);
818 }
819
820 if( binormal_force_min >= Valtype{0} && eP * wF.B < -flange )
821 // out of grip of the track; leave things to external forces ...
822 {
823 m_Dims = 0;
824 m_LimDims = 0;
825 std::clog << Verbosity::detailed << "Trackjoint: levitating!" << std::endl;
826 }
827
828 if( bDerailed )
829 {
830 m_Dims = 0;
831 m_LimDims = 0;
832 }
833
834 bMovingTrack = false;
835 return true;
836}
837
838template<typename Valtype>
839inline const spat::Frame<Valtype>& TrackJoint<Valtype>::GetMovingBody() const noexcept{
840 return m_MovingBodyFrame;
841}
842
843template<typename Valtype>
844inline void TrackJoint<Valtype>::SetMovingBodyCOM( const spat::Frame<Valtype>& localCom ) noexcept{
845 m_MovingBodyCOM = localCom;
846}
847
848template<typename Valtype>
849inline bool TrackJoint<Valtype>::SetTrackBody( const spat::Frame<Valtype>& bodyFrame ) noexcept{
850 m_TrackBodyFrame = bodyFrame;
851 Ft = m_TrackBodyCOM;
852 m_TrackBodyFrame.ToParent( Ft );
853
854 //precalculated values:
855 dF = wF.P - Ft.P;
856
857 bMovingTrack = true;
858 return true;
859}
860
861template<typename Valtype>
862inline const spat::Frame<Valtype>& TrackJoint<Valtype>::GetTrackBody() const noexcept{
863 return m_TrackBodyFrame;
864}
865
866template<typename Valtype>
867inline void TrackJoint<Valtype>::SetTrackBodyCOM( const spat::Frame<Valtype>& localCom ) noexcept{
868 m_TrackBodyCOM = localCom;
869}
870
871template<typename Valtype>
872inline void TrackJoint<Valtype>::Anchor( const spat::Frame<Valtype>& anchor ) noexcept{
873 m_LocalAnchor = anchor;
874 UpdateBodies();
875}
876
877template<typename Valtype>
878inline const spat::Frame<Valtype>& TrackJoint<Valtype>::Anchor() const noexcept{
879 return m_LocalAnchor;
880}
881
882template<typename Valtype>
883inline const spat::Frame<Valtype>& TrackJoint<Valtype>::GlobalAnchor() const noexcept{
884 return A;
885}
886
887template<typename Valtype>
888inline void TrackJoint<Valtype>::NormalForceLimits( Valtype min, Valtype max ){
889 if( min > max )
890 throw std::invalid_argument( "NormalForceLimits min can't be greater than max!" );
891
892 normal_force_min = min;
893 normal_force_max = max;
894
895 normal_force_limited = normal_force_min > -maxval ||
896 normal_force_max < maxval;
897
898 markDirty();
899}
900
901template<typename Valtype>
902inline void TrackJoint<Valtype>::BinormalForceLimits(Valtype min,Valtype max){
903 if( min > max )
904 throw std::invalid_argument( "BinormalForceLimits min can't be greater than max!" );
905
906 binormal_force_min = min;
907 binormal_force_max = max;
908
909 binormal_force_limited = binormal_force_min > -maxval ||
910 binormal_force_max < maxval;
911
912 markDirty();
913}
914
915template<typename Valtype>
916inline void TrackJoint<Valtype>::NormalTorqueLimit(Valtype max){
917 if( max < 0 )
918 throw std::invalid_argument( "NormalTorqueLimit max can not be negative!" );
919
920 normal_torque_max = max;
921 normal_torque_limited = normal_torque_max < maxval;
922
923 markDirty();
924}
925
926template<typename Valtype>
927inline void TrackJoint<Valtype>::TorqueLimit( Valtype max ){
928 if( max < 0 )
929 throw std::invalid_argument( "TorqueLimit max can not be negative!" );
930
931 torque_max = max;
932 torque_limited = torque_max < maxval;
933
934 markDirty();
935}
936
937template<typename Valtype>
938inline void TrackJoint<Valtype>::MotorTarget( Valtype velocity ) noexcept{
939 vTarget = velocity;
940 markDirty();
941}
942
943template<typename Valtype>
944inline void TrackJoint<Valtype>::MotorForceLimits( Valtype min, Valtype max ){
945 if( min > max ){
946 std::ostringstream stream;
947 stream << "Track_Joint: motor_force_min (" << min << ") can't be greater than motor_force_max (" << max << ")!";
948 throw std::invalid_argument( stream.str() );
949 }
950
951 motor_force_min = min;
952 motor_force_max = max;
953
954 motor_force_limited = motor_force_min > -maxval ||
955 motor_force_max < maxval;
956
957 markDirty();
958}
959
960template<typename Valtype>
961inline void TrackJoint<Valtype>::MotorForceMax( Valtype force ){
962 if( force < motor_force_min ){
963 std::ostringstream stream;
964 stream << "Track_Joint: motor force (" << force << ") can not be smaller than motor_force_min (" << motor_force_min << ")!";
965 throw std::invalid_argument( stream.str() );
966 }
967
968 motor_force_max = force;
969 motor_force_limited = motor_force_min > -maxval ||
970 motor_force_max < maxval;
971
972 markDirty();
973}
974
975template<typename Valtype>
976inline void TrackJoint<Valtype>::MotorForceMin( Valtype force ){
977 if( force > motor_force_max ){
978 std::ostringstream stream;
979 stream << "Track_Joint motor force (" << force << ") can not be greater than motor_force_max (" << motor_force_max << ")!";
980 throw std::invalid_argument( stream.str() );
981 }
982
983 motor_force_min = force;
984 motor_force_limited = motor_force_min > -maxval ||
985 motor_force_max < maxval;
986
987 markDirty();
988}
989
990template<typename Valtype>
991inline Valtype TrackJoint<Valtype>::MotorTarget() const noexcept
992{
993 return vTarget;
994}
995
996template<typename Valtype>
997inline Valtype TrackJoint<Valtype>::MotorForceMax() const noexcept
998{
999 return motor_force_max;
1000}
1001
1002template<typename Valtype>
1003inline Valtype TrackJoint<Valtype>::MotorForceMin() const noexcept
1004{
1005 return motor_force_min;
1006}
1007
1008template<typename Valtype>
1009inline void TrackJoint<Valtype>::ThresholdP( Valtype threshold ) noexcept{
1010 thresholdP = threshold;
1011 markDirty();
1012}
1013
1014template<typename Valtype>
1015inline void TrackJoint<Valtype>::ThresholdT( Valtype threshold ) noexcept{
1016 thresholdT = std::abs(std::sin( threshold ));
1017 markDirty();
1018}
1019
1020template<typename Valtype>
1021inline void TrackJoint<Valtype>::ThresholdN( Valtype threshold ) noexcept{
1022 thresholdN = std::abs(std::sin( threshold ));
1023 markDirty();
1024}
1025
1026template<typename Valtype>
1027inline void TrackJoint<Valtype>::ThresholdB( Valtype threshold ) noexcept{
1028 thresholdB = std::abs(std::sin( threshold ));
1029 markDirty();
1030}
1031
1032template<typename Valtype>
1033inline void TrackJoint<Valtype>::Flange( const Valtype flange_ ) noexcept{
1034 flange = flange_;
1035}
1036
1037template<typename Valtype>
1038inline void TrackJoint<Valtype>::ErrorReductionParameter( Valtype _erp ) noexcept
1039{
1040 erp = std::clamp( _erp, static_cast<Valtype>(0), static_cast<Valtype>(1) );
1041}
1042
1043template<typename Valtype>
1044inline Valtype TrackJoint<Valtype>::ErrorReductionParameter() const noexcept
1045{
1046 return erp;
1047}
1048
1049template<typename Valtype>
1050void TrackJoint<Valtype>::SetDerailed( bool _bDerailed ) noexcept{
1051 m_Dims = 0;
1052 m_LimDims = 0;
1053 bDerailed = _bDerailed;
1054 markDirty();
1055}
1056
1057template<typename Valtype>
1058bool TrackJoint<Valtype>::IsDerailed() const noexcept{
1059 return bDerailed;
1060}
1061
1062template<typename Valtype>
1063inline unsigned short TrackJoint<Valtype>::DimensionsTotal() const noexcept{
1064 return m_Dims;
1065}
1066
1067template<typename Valtype>
1068inline bool TrackJoint<Valtype>::IsAngularConstraint( unsigned short dim ) const noexcept{
1069 return dim == 2 || dim == 3 || dim == 4;
1070}
1071
1072template<typename Valtype>
1073inline bool TrackJoint<Valtype>::IsDriveConstraint( unsigned short dim ) const noexcept{
1074 return dim == 5;
1075}
1076
1077template<typename Valtype>
1078inline bool TrackJoint<Valtype>::IsConstraintLimited( unsigned short dim ) const noexcept
1079{
1080 switch( dim ){
1081 case 0:
1082 return normal_force_limited;
1083 case 1:
1084 return binormal_force_limited;
1085 case 2:
1086 return torque_limited;
1087 case 3:
1088 return normal_torque_limited;
1089 case 4:
1090 return torque_limited;
1091 case 5:
1092 return motor_force_limited;
1093 }
1094
1095 return false;
1096}
1097
1098}
trax::TrackJoint::ThresholdT
void ThresholdT(Valtype threshold) noexcept
Sets the threshold to switch off the joint when violated.
Definition TrackJoint.h:1015
trax::TrackJoint::SetTrackBodyCOM
void SetTrackBodyCOM(const spat::Frame< Valtype > &localCom) noexcept
Set the track bodie's center of mass relative to the track bodie's pose.
Definition TrackJoint.h:867
trax::TrackJoint::Precalculate
void Precalculate(const spat::Frame< Valtype > &bodyFrame) noexcept
Set the global pose of the body moving along the track and precalculates internal values like the glo...
Definition TrackJoint.h:735
trax::TrackJoint::MotorTarget
Valtype MotorTarget() const noexcept
Definition TrackJoint.h:991
trax::TrackJoint::MotorForceLimits
void MotorForceLimits(Valtype min, Valtype max)
Sets the limits for the motor force to be applied at the anchor point.
Definition TrackJoint.h:944
trax::TrackJoint::NormalForceLimits
void NormalForceLimits(Valtype min, Valtype max)
Sets the limits for the normal force to be applied at the anchor point.
Definition TrackJoint.h:888
trax::TrackJoint::SetTimeStep
void SetTimeStep(Time step) noexcept
Sets the time step for this trackjoint.
Definition TrackJoint.h:724
trax::TrackJoint::SetMovingBodyCOM
void SetMovingBodyCOM(const spat::Frame< Valtype > &localCom) noexcept
Set the moving bodie's center of mass relative to the bodie's pose.
Definition TrackJoint.h:844
trax::TrackJoint::ThresholdN
void ThresholdN(Valtype threshold) noexcept
Sets the threshold to switch off the joint when violated.
Definition TrackJoint.h:1021
trax::TrackJoint::BinormalForceLimits
void BinormalForceLimits(Valtype min, Valtype max)
Sets the limits for the binormal force to be applied at the anchor point.
Definition TrackJoint.h:902
trax::TrackJoint::GlobalAnchor
const spat::Frame< Valtype > & GlobalAnchor() const noexcept
Definition TrackJoint.h:883
trax::TrackJoint::SetTrackBody
bool SetTrackBody(const spat::Frame< Valtype > &bodyFrame) noexcept
Set the global pose of the body moving the track system.
Definition TrackJoint.h:849
trax::TrackJoint::GetTimeStep
Time GetTimeStep() const noexcept
Definition TrackJoint.h:730
trax::TrackJoint::DimensionsTotal
unsigned short DimensionsTotal() const noexcept
Definition TrackJoint.h:1063
trax::TrackJoint::IsConstraintLimited
bool IsConstraintLimited(unsigned short dim) const noexcept
Definition TrackJoint.h:1078
trax::TrackJoint::MovingBodyGlobalCOM2AnchorOffset
spat::Vector< Valtype > MovingBodyGlobalCOM2AnchorOffset() const noexcept
Definition TrackJoint.h:322
trax::TrackJoint::IsDriveConstraint
bool IsDriveConstraint(unsigned short dim) const noexcept
Definition TrackJoint.h:1073
trax::TrackJoint::ErrorReductionParameter
Valtype ErrorReductionParameter() const noexcept
Definition TrackJoint.h:1044
trax::TrackJoint::IsDerailed
bool IsDerailed() const noexcept
Definition TrackJoint.h:1058
trax::TrackJoint::GetMovingBody
const spat::Frame< Valtype > & GetMovingBody() const noexcept
Definition TrackJoint.h:839
trax::TrackJoint::MotorForceMin
Valtype MotorForceMin() const noexcept
Definition TrackJoint.h:1003
trax::TrackJoint::Anchor
const spat::Frame< Valtype > & Anchor() const noexcept
Definition TrackJoint.h:878
trax::TrackJoint::GetTrackBody
const spat::Frame< Valtype > & GetTrackBody() const noexcept
Definition TrackJoint.h:862
trax::TrackJoint::SetMovingBody
bool SetMovingBody(const spat::Frame< Valtype > &bodyFrame) noexcept
Set the global pose of the body moving along the track.
Definition TrackJoint.h:753
trax::TrackJoint::Flange
void Flange(Valtype flange) noexcept
If there is no holding force in binormal direction (BinormalForceLimitMin >= 0) the flange height wil...
Definition TrackJoint.h:1033
trax::TrackJoint::ThresholdB
void ThresholdB(Valtype threshold) noexcept
Sets the threshold to switch off the joint when violated.
Definition TrackJoint.h:1027
trax::TrackJoint::NormalTorqueLimit
void NormalTorqueLimit(Valtype max)
Sets the limit for the normal torque to be applied at the anchor point [-max,max].
Definition TrackJoint.h:916
trax::TrackJoint::SetDerailed
void SetDerailed(bool bDerailed=true) noexcept
Sets the status of the track joint to derailed.
Definition TrackJoint.h:1050
trax::TrackJoint::IsAngularConstraint
bool IsAngularConstraint(unsigned short dim) const noexcept
Definition TrackJoint.h:1068
trax::TrackJoint::MotorForceMax
Valtype MotorForceMax() const noexcept
Definition TrackJoint.h:997
trax::TrackJoint::ThresholdP
void ThresholdP(Valtype threshold) noexcept
Sets the threshold to switch off the joint when violated.
Definition TrackJoint.h:1009
trax::TrackJoint::Jacobian
void Jacobian(unsigned short dim, spat::Vector< Valtype > &Jm, spat::Vector< Valtype > &Rm, spat::Vector< Valtype > &Jt, spat::Vector< Valtype > &Rt, Valtype &VTarget, Valtype &E, Valtype &fmin, Valtype &fmax) const noexcept
Fills the matrix values for a specific constraint.
Definition TrackJoint.h:250
trax::TrackJoint::TorqueLimit
void TorqueLimit(Valtype max)
Sets the limit for the tangent and binormal torque to be applied at the anchor point [-max,...
Definition TrackJoint.h:927
dim
The namespace provides classes and methods for the dim library.
Definition DimensionedValues.h:178
dim::infinite
constexpr Real infinite
Positive infinity value.
Definition DimensionedValues.h:349
dim::_s
constexpr Real _s(Time t) noexcept
Dimensionated Values conversion functions.
Definition DimensionedValues.h:1535
dim::Time
Value< Dimension< 0, 0, 1 > > Time
Time.
Definition DimensionedValues.h:329
spat
The namespace provides classes and methods for spatial computations.
Definition Box.h:32
std
STL namespace.
trax
Namespace of all the trax track libraries classes and methods.
Definition Collection.h:17
trax::fixed_timestep
constexpr Time fixed_timestep
The fixed timestep to use with the simulation.
Definition Units.h:136
trax::default_derailing_distance
constexpr Length default_derailing_distance
A default value for the acceptable distance between a track position and something beeing still consi...
Definition Units.h:128
spat::Frame
A Frame ("TNBFrame") describes a location in 3d space and an orientation using a right handed coordin...
Definition Frame.h:52
spat::Vector
Implements a 3D - vector in cartesian coordinates.
Definition Vector.h:48
spat::Vector::specials::null
@ null
Vector with all parameters zeroed.
Definition Vector.h:58
trax::Body
A dynamic object in physical space.
Definition Body.h:44
trax::TrackData
Full geometrical data at a point on a track.
Definition TrackData.h:36
trax::TrackData::Init
void Init() noexcept
Initializes all values.
Definition TrackData.h:58
trax::TrackData::F
spat::Frame< Valtype > F
TNB Frame of the curve.
Definition TrackData.h:37
trax::TrackData::t
Valtype t
torsion
Definition TrackData.h:40
trax::TrackData::wF
spat::Frame< Valtype > wF
twisted TNB Frame
Definition TrackData.h:38
trax::TrackData::c
Valtype c
curvature
Definition TrackData.h:39