Momentum Scripting v1
Loading...
Searching...
No Matches
AffineMatrix4x4.h
1/*
2Copyright 2007-2025. Algoryx Simulation AB.
3
4All AGX source code, intellectual property, documentation, sample code,
5tutorials, scene files and technical white papers, are copyrighted, proprietary
6and confidential material of Algoryx Simulation AB. You may not download, read,
7store, distribute, publish, copy or otherwise disseminate, use or expose this
8material without having a written signed agreement with Algoryx Simulation AB.
9
10Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
11from using this software, unless otherwise stated in written agreements with
12Algoryx Simulation AB.
13*/
14#ifndef MOMENTUM_AFFINEMATRIX4X4_H
15#define MOMENTUM_AFFINEMATRIX4X4_H
16
17#include <momentum_namespace.h>
18#include <momentum_export.h>
19
20#ifdef _MSC_VER
21# pragma warning(push)
22# pragma warning(disable: 4714) // Disable warnings about not able to force inline
23#endif
24
25#include "Matrix4x4.h"
26#include <agx/AffineMatrix4x4.h>
27
29{
30 class EulerAngles;
31
32#define MOMENTUM_AFFINEMATRIX4X4_SET_ROW(row, v1, v2, v3 ) \
33 this->m_data[(row)][0] = (v1); \
34 this->m_data[(row)][1] = (v2); \
35 this->m_data[(row)][2] = (v3);
36
37#define MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(a,b,r,c) \
38 ((a).m_data[r][0] * (b).m_data[0][c]) \
39 +((a).m_data[r][1] * (b).m_data[1][c]) \
40 +((a).m_data[r][2] * (b).m_data[2][c])
41
50 class MOMENTUM_EXPORT AffineMatrix4x4 : public Matrix4x4
51 {
52 public:
53 /*=====================================================
54 Constructors
55 =======================================================*/
56
61
62#ifndef SWIG
64 operator agx::AffineMatrix4x4() const
65 {
66 return agx::AffineMatrix4x4((double*)m_data);
67 }
69
70 AffineMatrix4x4(const agx::AffineMatrix4x4& mat)
71 {
72 for (auto i = 0; i < 4; i++)
73 for (auto j = 0; j < 4; j++)
74 this->m_data[i][j] = mat.at(i, j);
75 }
76
77#endif
80
82 AffineMatrix4x4( double const * const ptr );
83
84
86 AffineMatrix4x4(const Quat& rotation, const Vec3& translation = Vec3());
87
89 AffineMatrix4x4( const EulerAngles& rotation, const Vec3& translation = Vec3());
90
91
93 AffineMatrix4x4( double a00, double a01, double a02, double a03,
94 double a10, double a11, double a12, double a13,
95 double a20, double a21, double a22, double a23,
96 double a30, double a31, double a32, double a33 );
97
98
101
102 /*=====================================================
103 Accessors
104 =======================================================*/
105
111
116
117
118 /*=====================================================
119 Mutators
120 =======================================================*/
121
136 void set( double const * const ptr );
137
138
153 void set( double a00, double a01, double a02, double a03,
154 double a10, double a11, double a12, double a13,
155 double a20, double a21, double a22, double a23,
156 double a30, double a31, double a32, double a33 );
157
164 void set(const Quat& q);
165
172 void set( const EulerAngles& e );
173
174
181 void setRotate(const Quat& q);
182
189 void setRotate( const EulerAngles& euler );
190
199 void setRotate( const Vec3& from, const Vec3& to );
200
206 void setRotate(double angle, const Vec3& axis);
207
213 void setRotate( double angle, double x, double y, double z );
214
226 void setRotate( double angle1, const Vec3& axis1,
227 double angle2, const Vec3& axis2,
228 double angle3, const Vec3& axis3 );
229
234 void setTranslate( const Vec3& t );
235
240 void setTranslate(double tx, double ty, double tz);
241
246
247
248 /*=====================================================
249 Static methods
250 =======================================================*/
251
252 /*
253 Generates a new matrix of a specific type.
254 */
255
256
257 static AffineMatrix4x4 crossMatrix(const Vec3& vec);
258
264 static AffineMatrix4x4 translate( const Vec3& dv );
265
273 static AffineMatrix4x4 translate( double x, double y, double z );
274
281 static AffineMatrix4x4 rotate( const Vec3& from, const Vec3& to );
282
283
292 static AffineMatrix4x4 rotate( double angle, double x, double y, double z );
293
300 static AffineMatrix4x4 rotate( double angle, const Vec3& axis );
301
314 static AffineMatrix4x4 rotate( double angle1, const Vec3& axis1,
315 double angle2, const Vec3& axis2,
316 double angle3, const Vec3& axis3 );
317
322 static AffineMatrix4x4 rotate(const EulerAngles& euler);
323
324 /*=====================================================
325 Operators
326 =======================================================*/
327
328 AffineMatrix4x4 operator* ( const AffineMatrix4x4 &m ) const;
329 Matrix4x4 operator* ( const Matrix4x4 &m ) const;
330#ifndef SWIG
331 AffineMatrix4x4& operator= ( const AffineMatrix4x4& rhs );
332 AffineMatrix4x4& operator= (const Matrix4x4& rhs);
333#endif
334 Vec3 operator* ( const Vec3& v ) const;
335 Vec4 operator* ( const Vec4& v ) const;
336
337 void operator*= ( const AffineMatrix4x4& other );
338 bool operator== ( const AffineMatrix4x4& m ) const;
339 bool operator!= ( const AffineMatrix4x4& m ) const;
340
344 Vec3 transformPoint(const Vec3& point) const;
345
349 Vec3 transformVector(const Vec3& vector) const;
350
351 void mult( const AffineMatrix4x4&, const AffineMatrix4x4& );
352 // void multSSE(const AffineMatrix4x4& lhs, const AffineMatrix4x4& rhs);
353 using Matrix4x4::preMult;
354 using Matrix4x4::postMult;
355 void preMult( const AffineMatrix4x4& );
356 void postMult( const AffineMatrix4x4& );
359
361 double at(int i, int j) const;
362
364 void set(double val, int i, int j);
365
367
368 protected:
369 bool invert( const AffineMatrix4x4& rhs );
371 };
372
373}
374
375
376
377/* ****************************************** */
378/* * IMPLEMENTATION ** */
379/*
380/* ****************************************** */
382namespace MOMENTUM_NAMESPACE
383{
384#if !defined(SWIG)
385
386 /*=====================================================
387 Constructors
388 =======================================================*/
389
390
392 {
393 this->setIdentity();
394 }
395
396 inline AffineMatrix4x4::AffineMatrix4x4( const AffineMatrix4x4& mat ) : Matrix4x4( mat )
397 {
398 }
399
400
401 inline AffineMatrix4x4::AffineMatrix4x4( double a00, double a01, double a02, double a03,
402 double a10, double a11, double a12, double a13,
403 double a20, double a21, double a22, double a23,
404 double a30, double a31, double a32, double a33 ) : Matrix4x4(a00, a01, a02, a03, a10, a11, a12, a13, a20, a21, a22, a23, a30, a31, a32, a33)
405 {
406 agxAssert(this->isRigidTransformation());
407 }
408
409
410 inline AffineMatrix4x4::AffineMatrix4x4( double const * const ptr )
411 {
412 set( ptr );
413 }
414
415
416
417 inline AffineMatrix4x4::AffineMatrix4x4(const Quat& quat, const Vec3& translation) : Matrix4x4(quat, translation)
418 {
419 }
420
421
422
423 inline AffineMatrix4x4::AffineMatrix4x4(EulerAngles const& e, const Vec3& translation ) : Matrix4x4(e, translation)
424 {
425 }
426
427
428
430 {}
431
432
433 /*=====================================================
434 Queries
435 =======================================================*/
436
437
438
439 inline AffineMatrix4x4 AffineMatrix4x4::inverse() const
440 {
442 m.invert( *this );
443 return m;
444 }
445
446
447 inline Vec3 AffineMatrix4x4::getInvTranslate() const
448 {
449 double x = this->m_data[3][0];
450 double y = this->m_data[3][1];
451 double z = this->m_data[3][2];
452
453 Vec3 invTranslate;
454
455 invTranslate[0] = -( x * this->m_data[0][0] + y* this->m_data[0][1] + z * this->m_data[0][2]);
456 invTranslate[1] = -( x * this->m_data[1][0] + y* this->m_data[1][1] + z * this->m_data[1][2]);
457 invTranslate[2] = -( x * this->m_data[2][0] + y* this->m_data[2][1] + z * this->m_data[2][2]);
458
459 return invTranslate;
460 }
461
462
463 inline bool AffineMatrix4x4::operator== (const AffineMatrix4x4& m) const
464 {
465 const int bitCompare = memcmp(this->m_data, m.m_data, sizeof(this->m_data));
466 return (bitCompare == 0);
467 }
468
469
470 inline bool AffineMatrix4x4::operator!= (const AffineMatrix4x4& m) const
471 {
472 const int bitCompare = memcmp(this->m_data, m.m_data, sizeof(this->m_data));
473 return (bitCompare != 0);
474 }
475
476
477 /*=====================================================
478 Setters
479 =======================================================*/
480
481 inline void AffineMatrix4x4::set( double const * const ptr )
482 {
483 memcpy(this->m_data, ptr, sizeof(double)*16);
484 agxAssert(this->isRigidTransformation());
485 }
486
487 inline void AffineMatrix4x4::set( double a00, double a01, double a02, double a03,
488 double a10, double a11, double a12, double a13,
489 double a20, double a21, double a22, double a23,
490 double a30, double a31, double a32, double a33 )
491 {
492
493 Matrix4x4::set(a00, a01, a02, a03, a10, a11, a12, a13, a20, a21, a22, a23, a30, a31, a32, a33);
494 }
495
496
497 inline void AffineMatrix4x4::set(const Quat& q)
498 {
499 this->setRotate(q);
500 this->setTranslate(0.0, 0.0, 0.0);
501 }
502
503
504
505
506 inline void AffineMatrix4x4::set( const EulerAngles& e )
507 {
508 std::pair<Matrix4x4, bool> rotation = e.getAsMatrix4x4(*this);
509 *this = rotation.first;
510 this->setTranslate(0.0, 0.0, 0.0);
511 }
512
513
514 inline void AffineMatrix4x4::setRotate(const Quat& q)
515 {
517 }
518
519
520
521
522 inline void AffineMatrix4x4::setRotate( const EulerAngles& euler )
523 {
524 std::pair<Matrix4x4, bool> rotation = euler.getAsMatrix4x4(*this);
525 *this = rotation.first;
526 }
527
528
529 inline void AffineMatrix4x4::setRotate( const Vec3& from, const Vec3& to )
530 {
531 Quat quat;
532 quat.setRotate(from,to);
533 this->setRotate(quat);
534 }
535
536
537 inline void AffineMatrix4x4::setRotate( double angle, const Vec3& axis )
538 {
539 Quat quat;
540 quat.setRotate( angle, axis);
541 this->setRotate(quat);
542 }
543
544
545 inline void AffineMatrix4x4::setRotate( double angle, double x, double y, double z )
546 {
547 Quat quat;
548 quat.setRotate( angle, x, y, z);
549 this->setRotate(quat);
550 }
551
552
553 inline void AffineMatrix4x4::setRotate( double angle1, const Vec3& axis1,
554 double angle2, const Vec3& axis2,
555 double angle3, const Vec3& axis3 )
556 {
557 Quat quat;
558 quat.setRotate(angle1, axis1,
559 angle2, axis2,
560 angle3, axis3);
561 this->setRotate(quat);
562 }
563
564
565 inline void AffineMatrix4x4::setTranslate( const Vec3& v )
566 {
567 this->m_data[3][0] = v[0];
568 this->m_data[3][1] = v[1];
569 this->m_data[3][2] = v[2];
570 }
571
572
573 inline void AffineMatrix4x4::setTranslate(double tx, double ty, double tz)
574 {
575 this->m_data[3][0] = tx;
576 this->m_data[3][1] = ty;
577 this->m_data[3][2] = tz;
578 }
579
580
581
582 inline void AffineMatrix4x4::setIdentity()
583 {
584 this->set(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
585 }
586
587
588 /*=====================================================
589 Static methods
590 =======================================================*/
591
592
593 inline AffineMatrix4x4 AffineMatrix4x4::crossMatrix(const Vec3& v)
594 {
595 return AffineMatrix4x4( 0, -v.z(), v.y(), 0,
596 v.z(), 0, -v.x(), 0,
597 -v.y(), v.x(), 0, 0,
598 0, 0, 0, 1 );
599 }
600
601
602
603 inline AffineMatrix4x4 AffineMatrix4x4::translate( const Vec3& dv )
604 {
606 m.setTranslate(dv);
607 return m;
608 }
609
610
611 inline AffineMatrix4x4 AffineMatrix4x4::translate( double x, double y, double z )
612 {
614 m.setTranslate(x, y, z);
615 return m;
616 }
617
618
619 inline AffineMatrix4x4 AffineMatrix4x4::rotate( const Vec3& from, const Vec3& to )
620 {
622 m.setRotate(from, to);
623 return m;
624 }
625
626
627 inline AffineMatrix4x4 AffineMatrix4x4::rotate( double angle, double x, double y, double z )
628 {
630 m.setRotate(angle, x, y, z);
631 return m;
632 }
633
634
635 inline AffineMatrix4x4 AffineMatrix4x4::rotate( double angle, const Vec3& axis )
636 {
638 m.setRotate(angle, axis);
639 return m;
640 }
641
642
643 inline AffineMatrix4x4 AffineMatrix4x4::rotate( double angle1, const Vec3& axis1,
644 double angle2, const Vec3& axis2,
645 double angle3, const Vec3& axis3 )
646 {
648 m.setRotate(angle1, axis1, angle2, axis2, angle3, axis3);
649 return m;
650 }
651
652 inline AffineMatrix4x4 AffineMatrix4x4::rotate(const EulerAngles& euler)
653 {
655 m.setRotate(euler);
656 return m;
657 }
658
659
660 /*=====================================================
661 Operators
662 =======================================================*/
663
664 inline void AffineMatrix4x4::mult( const AffineMatrix4x4& lhs, const AffineMatrix4x4& rhs )
665 {
666 agx::prefetch<agx::L1>( lhs.m_data );
667 agx::prefetch<agx::L1>( rhs.m_data );
668
669 if (&lhs==this)
670 {
671 postMult(rhs);
672 return;
673 }
674 if (&rhs==this)
675 {
676 preMult(lhs);
677 return;
678 }
679
680 this->m_data[0][0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 0, 0);
681 this->m_data[0][1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 0, 1);
682 this->m_data[0][2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 0, 2);
683
684 this->m_data[1][0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 1, 0);
685 this->m_data[1][1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 1, 1);
686 this->m_data[1][2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 1, 2);
687
688 this->m_data[2][0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 2, 0);
689 this->m_data[2][1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 2, 1);
690 this->m_data[2][2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 2, 2);
691
692 this->m_data[3][0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 3, 0) + rhs.m_data[3][0];
693 this->m_data[3][1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 3, 1) + rhs.m_data[3][1];
694 this->m_data[3][2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3(lhs, rhs, 3, 2) + rhs.m_data[3][2];
695
696 }
697
698
699 inline void AffineMatrix4x4::preMult( const AffineMatrix4x4& other )
700 {
701
702 // more efficient method just use a T[4] for temporary storage.
703 double t[4];
704 for(int col=0; col<3; ++col) {
705 t[0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( other, *this, 0, col );
706 t[1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( other, *this, 1, col );
707 t[2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( other, *this, 2, col );
708 t[3] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( other, *this, 3, col );
709 this->m_data[0][col] = t[0];
710 this->m_data[1][col] = t[1];
711 this->m_data[2][col] = t[2];
712 this->m_data[3][col] = t[3] + this->m_data[3][col];
713 }
714 }
715
716
717 inline void AffineMatrix4x4::postMult( const AffineMatrix4x4& other )
718 {
719
720 // more efficient method just use a T[3] for temporary storage.
721 double t[3];
722 for(int row=0; row<3; ++row)
723 {
724 t[0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( *this, other, row, 0 );
725 t[1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( *this, other, row, 1 );
726 t[2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( *this, other, row, 2 );
727
728 MOMENTUM_AFFINEMATRIX4X4_SET_ROW(row, t[0], t[1], t[2] )
729 }
730 // treat last row differently
731 t[0] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( *this, other, 3, 0 ) + other.m_data[3][0];
732 t[1] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( *this, other, 3, 1 ) + other.m_data[3][1];
733 t[2] = MOMENTUM_AFFINEMATRIX4X4_INNER_PRODUCT_3( *this, other, 3, 2 ) + other.m_data[3][2];
734 MOMENTUM_AFFINEMATRIX4X4_SET_ROW(3, t[0], t[1], t[2] )
735 }
736
737
738 inline AffineMatrix4x4& AffineMatrix4x4::preMultTranslate( const Vec3& v )
739 {
740 for (unsigned i = 0; i < 3; ++i)
741 {
742 this->m_data[3][0] += v[i]*this->m_data[i][0];
743 this->m_data[3][1] += v[i]*this->m_data[i][1];
744 this->m_data[3][2] += v[i]*this->m_data[i][2];
745 this->m_data[3][3] += v[i]*this->m_data[i][3];
746 }
747 return *this;
748 }
749
750
751 inline AffineMatrix4x4& AffineMatrix4x4::postMultTranslate( const Vec3& v )
752 {
753 for (unsigned i = 0; i < 3; ++i)
754 {
755 this->m_data[0][i] += v[i]*this->m_data[0][3];
756 this->m_data[1][i] += v[i]*this->m_data[1][3];
757 this->m_data[2][i] += v[i]*this->m_data[2][3];
758 this->m_data[3][i] += v[i]*this->m_data[3][3];
759 }
760 return *this;
761 }
762
763
764
765
766 inline AffineMatrix4x4& AffineMatrix4x4::operator= ( const AffineMatrix4x4& rhs )
767 {
768 if ( this != &rhs )
769 this->set( rhs.ptr() );
770
771 return *this;
772 }
773
774 inline AffineMatrix4x4& AffineMatrix4x4::operator= (const Matrix4x4& rhs)
775 {
776 if (this != &rhs)
777 this->set(rhs.ptr());
778
779 return *this;
780 }
781
782
783
784 inline AffineMatrix4x4 AffineMatrix4x4::operator* ( const AffineMatrix4x4 &m ) const
785 {
787 r.mult( *this, m );
788 return r;
789 }
790
791
792 inline Matrix4x4 AffineMatrix4x4::operator* ( const Matrix4x4 &m ) const
793 {
794 Matrix4x4 r;
795 r.mult( *this, m );
796 return r;
797 }
798
799
800
801 inline void AffineMatrix4x4::operator*= ( const AffineMatrix4x4& other )
802 {
803 if ( this == &other )
804 {
805 AffineMatrix4x4 temp( other );
806 postMult( temp );
807 }
808 else postMult( other );
809 }
810
811
812
813 inline Vec3 AffineMatrix4x4::operator* (const Vec3& v) const
814 {
815 return postMult(v);
816 }
817
818
819 inline Vec4 AffineMatrix4x4::operator* (const Vec4& v) const
820 {
821 return postMult(v);
822 }
823
824
825 inline Vec3 operator* (const Vec3& v, const AffineMatrix4x4& m)
826 {
827 return m.preMult(v);
828 }
829
830
831
832 inline Vec4 operator* (const Vec4& v, const AffineMatrix4x4& m)
833 {
834 return m.preMult(v);
835 }
836
837
838
839 inline bool AffineMatrix4x4::invert( const AffineMatrix4x4& rhs)
840 {
841 double x,y,z;
842
843 // diagonal
844 this->m_data[0][0] = rhs.m_data[0][0];
845 this->m_data[1][1] = rhs.m_data[1][1];
846 this->m_data[2][2] = rhs.m_data[2][2];
847
848 // rotation
849 this->m_data[0][1] = rhs.m_data[1][0];
850 this->m_data[1][0] = rhs.m_data[0][1];
851 this->m_data[0][2] = rhs.m_data[2][0];
852 this->m_data[2][0] = rhs.m_data[0][2];
853 this->m_data[1][2] = rhs.m_data[2][1];
854 this->m_data[2][1] = rhs.m_data[1][2];
855
856 x = rhs.m_data[3][0];
857 y = rhs.m_data[3][1];
858 z = rhs.m_data[3][2];
859
860 // translation
861 this->m_data[3][0] = -( x * this->m_data[0][0] + y* this->m_data[1][0] + z * this->m_data[2][0]);
862 this->m_data[3][1] = -( x * this->m_data[0][1] + y* this->m_data[1][1] + z * this->m_data[2][1]);
863 this->m_data[3][2] = -( x * this->m_data[0][2] + y* this->m_data[1][2] + z * this->m_data[2][2]);
864
865 return true;
866 }
867
868
869 inline double AffineMatrix4x4::at(int i, int j) const
870 {
871 return double((*this)(i,j));
872 }
873
874
875 inline void AffineMatrix4x4::set(double val, int i, int j)
876 {
877 (*this)(i,j) = val;
878 }
879
880
881
882 inline Vec3 AffineMatrix4x4::transformPoint(const Vec3& point) const
883 {
884 return point * (*this);
885 }
886
887
888 inline Vec3 AffineMatrix4x4::transformVector(const Vec3& vector) const
889 {
890 return this->transform3x3(vector);
891 }
893#endif // swig
894
895 inline bool equivalent(const AffineMatrix4x4& a, const AffineMatrix4x4& b, double epsilon = double(MOMENTUM_EQUIVALENT_EPSILON))
896 {
897 return
898 agx::equivalent(a(0, 0), b(0, 0), epsilon) &&
899 agx::equivalent(a(0, 1), b(0, 1), epsilon) &&
900 agx::equivalent(a(0, 2), b(0, 2), epsilon) &&
901 agx::equivalent(a(0, 3), b(0, 3), epsilon) &&
902 agx::equivalent(a(1, 0), b(1, 0), epsilon) &&
903 agx::equivalent(a(1, 1), b(1, 1), epsilon) &&
904 agx::equivalent(a(1, 2), b(1, 2), epsilon) &&
905 agx::equivalent(a(1, 3), b(1, 3), epsilon) &&
906 agx::equivalent(a(2, 0), b(2, 0), epsilon) &&
907 agx::equivalent(a(2, 1), b(2, 1), epsilon) &&
908 agx::equivalent(a(2, 2), b(2, 2), epsilon) &&
909 agx::equivalent(a(2, 3), b(2, 3), epsilon) &&
910 agx::equivalent(a(3, 0), b(3, 0), epsilon) &&
911 agx::equivalent(a(3, 1), b(3, 1), epsilon) &&
912 agx::equivalent(a(3, 2), b(3, 2), epsilon) &&
913 agx::equivalent(a(3, 3), b(3, 3), epsilon);
914 }
915
916
917}
918
919
920
921#ifdef _MSC_VER
922# pragma warning(pop)
923#endif
924
925#endif
Matrix class for rigid transformations (translation, rotation).
Definition: AffineMatrix4x4.h:51
void preMult(const AffineMatrix4x4 &)
bool operator!=(const AffineMatrix4x4 &m) const
static AffineMatrix4x4 rotate(const Vec3 &from, const Vec3 &to)
Return a matrix which rotate a vector from from to to.
AffineMatrix4x4 & postMultTranslate(const Vec3 &v)
static AffineMatrix4x4 rotate(double angle, const Vec3 &axis)
Return a matrix which rotate angle radians around vector axis.
AffineMatrix4x4(const EulerAngles &rotation, const Vec3 &translation=Vec3())
Create a matrix from Euler angles.
double at(int i, int j) const
AffineMatrix4x4 & preMultTranslate(const Vec3 &v)
static AffineMatrix4x4 rotate(double angle, double x, double y, double z)
Return a matrix which rotate angle radians around vector [x,y,z].
void mult(const AffineMatrix4x4 &, const AffineMatrix4x4 &)
AffineMatrix4x4(double a00, double a01, double a02, double a03, double a10, double a11, double a12, double a13, double a20, double a21, double a22, double a23, double a30, double a31, double a32, double a33)
Create a matrix from 16 double scalars.
void setRotate(const Quat &q)
Set the rotational part of the matrix using the specified quaternion leaving the translational part u...
void set(const Quat &q)
Set the rotational part of the matrix using the specified quaternion and the translational part to 0,...
static AffineMatrix4x4 rotate(double angle1, const Vec3 &axis1, double angle2, const Vec3 &axis2, double angle3, const Vec3 &axis3)
Return a matrix which rotate angle1 radians around vector axis1 and angle2 radians around vector axis...
void set(double const *const ptr)
Set the entire matrix using values from the array with 16 elements using the following order:
static AffineMatrix4x4 rotate(const EulerAngles &euler)
Return a matrix with the rotational part specified by the euler angles argument.
Vec3 transformPoint(const Vec3 &point) const
Transforms a (mathematical) point, using both rotation and translation.
void setRotate(double angle, double x, double y, double z)
Set the rotational part of a matrix which rotate angle radians around vector x y z.
AffineMatrix4x4 inverse() const
Quick inverse, transpose rotation part, and change sign of translation part.
bool operator==(const AffineMatrix4x4 &m) const
AffineMatrix4x4 operator*(const AffineMatrix4x4 &m) const
void postMult(const AffineMatrix4x4 &)
static AffineMatrix4x4 crossMatrix(const Vec3 &vec)
AffineMatrix4x4(const agx::AffineMatrix4x4 &mat)
Definition: AffineMatrix4x4.h:70
AffineMatrix4x4(const Quat &rotation, const Vec3 &translation=Vec3())
Create a matrix from a quaternion.
void setRotate(double angle1, const Vec3 &axis1, double angle2, const Vec3 &axis2, double angle3, const Vec3 &axis3)
Set the rotational part of a matrix which rotate angle1 radians around vector axis1 and angle2 radian...
void operator*=(const AffineMatrix4x4 &other)
AffineMatrix4x4 & operator=(const AffineMatrix4x4 &rhs)
AffineMatrix4x4()
Creates a new matrix, initialized to be an identity matrix.
void setRotate(const Vec3 &from, const Vec3 &to)
Set the rotational part of the matrix using the specified two vectors.
void set(const EulerAngles &e)
Set the rotational part of the matrix using the specified euler angles and the translational part to ...
void setRotate(const EulerAngles &euler)
Set the rotational part of the matrix using the specified euler angles leaving the translational part...
void setRotate(double angle, const Vec3 &axis)
Set the rotational part of a matrix which rotate angle radians around vector axis.
AffineMatrix4x4(double const *const ptr)
Create a matrix from a vector of 16 reals.
void setIdentity()
Set the value of the matrix to the identity matrix.
AffineMatrix4x4(const AffineMatrix4x4 &mat)
Copy constructor.
Vec3 transformVector(const Vec3 &vector) const
Transforms a (mathematical) vector, using only rotation.
void set(double a00, double a01, double a02, double a03, double a10, double a11, double a12, double a13, double a20, double a21, double a22, double a23, double a30, double a31, double a32, double a33)
Set the entire matrix using values from the 16 elements using the following order:
static AffineMatrix4x4 translate(double x, double y, double z)
Return a matrix which translates according to vector [x,y,z].
void set(double val, int i, int j)
Set the value of element i,j.
void setTranslate(const Vec3 &t)
Set the translational part of the matrix using the vector t.
static AffineMatrix4x4 translate(const Vec3 &dv)
Return a matrix which translates according to dv.
void setTranslate(double tx, double ty, double tz)
Set the translational part of the matrix using the vector x, y, z.
This class provides conversion services between Euler angles in any of the 24 conventions and corresp...
Definition: EulerAngles.h:64
Matrix class for affine transformations.
Definition: Matrix4x4.h:51
Matrix4x4 set(double const *const ptr)
Definition: Matrix4x4.h:463
double * ptr()
Definition: Matrix4x4.h:615
bool isRigidTransformation() const
Definition: Matrix4x4.h:433
Vec3 transform3x3(const Vec3 &vIn) const
Definition: Matrix4x4.h:883
Matrix4x4()
Creates a new matrix, initialized to be an identity matrix.
Definition: Matrix4x4.h:309
Matrix4x4 setRotate(const Quat &q)
The object holding quaternions and providing operations on these.
Definition: Quat.h:55
A 3 dimensional vector which can be used to define a point or a vector and contains basic arithmetic.
Definition: Vec3.h:40
A 4 dimensional vector and contains basic arithmetic.
Definition: Vec4.h:33
Namespace for Momentum Scripting API.
Definition: AffineMatrix4x4.h:29
const double MOMENTUM_EQUIVALENT_EPSILON
Definition: momentum_math.h:30
Vec3 operator*(const Vec3 &v, const Matrix3x3 &m)
Definition: Matrix3x3.h:746
bool equivalent(const Matrix3x3 &a, const Matrix3x3 &b, double epsilon=1e-6)
Definition: Matrix3x3.h:905