14#ifndef MOMENTUM_AFFINEMATRIX4X4_H
15#define MOMENTUM_AFFINEMATRIX4X4_H
17#include <momentum_namespace.h>
18#include <momentum_export.h>
22# pragma warning(disable: 4714)
26#include <agx/AffineMatrix4x4.h>
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);
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])
64 operator agx::AffineMatrix4x4()
const
66 return agx::AffineMatrix4x4((
double*)m_data);
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);
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 );
136 void set(
double const *
const ptr );
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 );
213 void setRotate(
double angle,
double x,
double y,
double z );
227 double angle2,
const Vec3& axis2,
228 double angle3,
const Vec3& axis3 );
315 double angle2,
const Vec3& axis2,
316 double angle3,
const Vec3& axis3 );
353 using Matrix4x4::preMult;
354 using Matrix4x4::postMult;
361 double at(
int i,
int j)
const;
364 void set(
double val,
int i,
int j);
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)
449 double x = this->m_data[3][0];
450 double y = this->m_data[3][1];
451 double z = this->m_data[3][2];
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]);
465 const int bitCompare = memcmp(this->m_data, m.m_data,
sizeof(this->m_data));
466 return (bitCompare == 0);
472 const int bitCompare = memcmp(this->m_data, m.m_data,
sizeof(this->m_data));
473 return (bitCompare != 0);
483 memcpy(this->m_data,
ptr,
sizeof(
double)*16);
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 )
493 Matrix4x4::set(a00, a01, a02, a03, a10, a11, a12, a13, a20, a21, a22, a23, a30, a31, a32, a33);
508 std::pair<Matrix4x4, bool> rotation = e.getAsMatrix4x4(*
this);
509 *
this = rotation.first;
524 std::pair<Matrix4x4, bool> rotation = euler.getAsMatrix4x4(*
this);
525 *
this = rotation.first;
532 quat.setRotate(from,to);
540 quat.setRotate( angle, axis);
548 quat.setRotate( angle, x, y, z);
554 double angle2,
const Vec3& axis2,
555 double angle3,
const Vec3& axis3 )
558 quat.setRotate(angle1, axis1,
567 this->m_data[3][0] = v[0];
568 this->m_data[3][1] = v[1];
569 this->m_data[3][2] = v[2];
575 this->m_data[3][0] = tx;
576 this->m_data[3][1] = ty;
577 this->m_data[3][2] = tz;
584 this->
set(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
614 m.setTranslate(x, y, z);
622 m.setRotate(from, to);
630 m.setRotate(angle, x, y, z);
638 m.setRotate(angle, axis);
644 double angle2,
const Vec3& axis2,
645 double angle3,
const Vec3& axis3 )
648 m.setRotate(angle1, axis1, angle2, axis2, angle3, axis3);
666 agx::prefetch<agx::L1>( lhs.m_data );
667 agx::prefetch<agx::L1>( rhs.m_data );
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);
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);
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);
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];
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];
722 for(
int row=0; row<3; ++row)
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 );
728 MOMENTUM_AFFINEMATRIX4X4_SET_ROW(row, t[0], t[1], t[2] )
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] )
740 for (
unsigned i = 0; i < 3; ++i)
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];
753 for (
unsigned i = 0; i < 3; ++i)
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];
769 this->
set( rhs.ptr() );
777 this->
set(rhs.ptr());
803 if (
this == &other )
825 inline Vec3
operator* (
const Vec3& v,
const AffineMatrix4x4& m)
832 inline Vec4
operator* (
const Vec4& v,
const AffineMatrix4x4& m)
839 inline bool AffineMatrix4x4::invert(
const AffineMatrix4x4& rhs)
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];
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];
856 x = rhs.m_data[3][0];
857 y = rhs.m_data[3][1];
858 z = rhs.m_data[3][2];
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]);
871 return double((*
this)(i,j));
884 return point * (*this);
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);
Matrix class for rigid transformations (translation, rotation).
Definition: AffineMatrix4x4.h:51
Vec3 getInvTranslate() const
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.
~AffineMatrix4x4()
Destructor.
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