refactor: unify Object signal system, update property connections, and integrate Eigen3 into Root module
This commit is contained in:
AndreaRigoni
@@ -23,8 +23,6 @@
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////*/
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Copyright (C) 2012 Andrea Rigoni Garola <andrea.rigoni@pd.infn.it>
|
||||
*
|
||||
@@ -45,142 +43,120 @@
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef U_TRANSFORM_H
|
||||
#define U_TRANSFORM_H
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
#include "Math/Units.h"
|
||||
#include "Math/Dense.h"
|
||||
|
||||
#include "Math/Units.h"
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace uLib {
|
||||
|
||||
|
||||
|
||||
using Eigen::Isometry3f;
|
||||
using Eigen::Isometry3d;
|
||||
using Eigen::Isometry3f;
|
||||
|
||||
using Eigen::Affine3f;
|
||||
using Eigen::Affine3d;
|
||||
using Eigen::Affine3f;
|
||||
|
||||
using Eigen::Projective3f;
|
||||
using Eigen::Projective3d;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
using Eigen::Projective3f;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
///////// AFFINE TRANSFORM WRAPPER //////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
class AffineTransform : virtual public Object {
|
||||
public:
|
||||
uLibTypeMacro(AffineTransform, Object)
|
||||
protected:
|
||||
uLibTypeMacro(AffineTransform, Object) protected :
|
||||
|
||||
Affine3f m_T;
|
||||
AffineTransform *m_Parent;
|
||||
Affine3f m_T;
|
||||
AffineTransform *m_Parent;
|
||||
|
||||
public:
|
||||
AffineTransform() :
|
||||
m_T(Matrix4f::Identity()),
|
||||
m_Parent(NULL)
|
||||
{}
|
||||
AffineTransform() : m_T(Matrix4f::Identity()), m_Parent(NULL) {}
|
||||
|
||||
AffineTransform(AffineTransform *parent) :
|
||||
m_T(Matrix4f::Identity()),
|
||||
m_Parent(parent)
|
||||
{}
|
||||
AffineTransform(AffineTransform *parent)
|
||||
: m_T(Matrix4f::Identity()), m_Parent(parent) {}
|
||||
|
||||
AffineTransform(const AffineTransform ©) :
|
||||
m_T(copy.m_T),
|
||||
m_Parent(copy.m_Parent)
|
||||
{}
|
||||
AffineTransform(const AffineTransform ©)
|
||||
: m_T(copy.m_T), m_Parent(copy.m_Parent) {}
|
||||
|
||||
Affine3f& GetTransform() { return m_T; }
|
||||
Affine3f &GetTransform() { return m_T; }
|
||||
|
||||
AffineTransform *GetParent() const { return this->m_Parent; }
|
||||
AffineTransform *GetParent() const { return this->m_Parent; }
|
||||
|
||||
void SetParent(AffineTransform *name) { this->m_Parent = name; }
|
||||
void SetParent(AffineTransform *name) { this->m_Parent = name; }
|
||||
|
||||
void SetMatrix (const Matrix4f &mat) { m_T.matrix() = mat; }
|
||||
Matrix4f& GetMatrix () { return m_T.matrix(); }
|
||||
const Matrix4f& GetMatrix () const { return m_T.matrix(); }
|
||||
void SetMatrix(const Matrix4f &mat) { m_T.matrix() = mat; }
|
||||
Matrix4f &GetMatrix() { return m_T.matrix(); }
|
||||
const Matrix4f &GetMatrix() const { return m_T.matrix(); }
|
||||
|
||||
Matrix4f GetWorldMatrix() const
|
||||
{
|
||||
if(!m_Parent) return m_T.matrix();
|
||||
else return m_Parent->GetWorldMatrix() * m_T.matrix(); // T = B * A //
|
||||
}
|
||||
Matrix4f GetWorldMatrix() const {
|
||||
if (!m_Parent)
|
||||
return m_T.matrix();
|
||||
else
|
||||
return m_Parent->GetWorldMatrix() * m_T.matrix(); // T = B * A //
|
||||
}
|
||||
|
||||
void SetWorldMatrix(const Matrix4f &mat)
|
||||
{
|
||||
if(!m_Parent) m_T.matrix() = mat;
|
||||
else m_T.matrix() = m_Parent->GetWorldMatrix().inverse() * mat;
|
||||
}
|
||||
void SetWorldMatrix(const Matrix4f &mat) {
|
||||
if (!m_Parent)
|
||||
m_T.matrix() = mat;
|
||||
else
|
||||
m_T.matrix() = m_Parent->GetWorldMatrix().inverse() * mat;
|
||||
}
|
||||
|
||||
void SetPosition(const Vector3f &v) { this->m_T.translation() = v; }
|
||||
void SetPosition(const Vector3f &v) { this->m_T.translation() = v; }
|
||||
|
||||
Vector3f GetPosition() const { return this->m_T.translation(); }
|
||||
Vector3f GetPosition() const { return this->m_T.translation(); }
|
||||
|
||||
void SetRotation(const Matrix3f &m) { this->m_T.linear() = m; }
|
||||
void SetRotation(const Matrix3f &m) { this->m_T.linear() = m; }
|
||||
|
||||
Matrix3f GetRotation() const { return this->m_T.rotation(); }
|
||||
Matrix3f GetRotation() const { return this->m_T.rotation(); }
|
||||
|
||||
void Translate(const Vector3f &v) { this->m_T.translate(v); }
|
||||
void Translate(const Vector3f &v) { this->m_T.translate(v); }
|
||||
|
||||
void Scale(const Vector3f &v) { this->m_T.scale(v); }
|
||||
void Scale(const Vector3f &v) { this->m_T.scale(v); }
|
||||
|
||||
Vector3f GetScale() const {
|
||||
return Vector3f(this->m_T.linear().col(0).norm(),
|
||||
this->m_T.linear().col(1).norm(),
|
||||
this->m_T.linear().col(2).norm());
|
||||
}
|
||||
Vector3f GetScale() const {
|
||||
return Vector3f(this->m_T.linear().col(0).norm(),
|
||||
this->m_T.linear().col(1).norm(),
|
||||
this->m_T.linear().col(2).norm());
|
||||
}
|
||||
|
||||
void Rotate(const Matrix3f &m) { this->m_T.rotate(m); }
|
||||
|
||||
void Rotate(const Matrix3f &m) { this->m_T.rotate(m); }
|
||||
void Rotate(const float angle, Vector3f axis) {
|
||||
axis.normalize(); // prehaps not necessary ( see eigens )
|
||||
Eigen::AngleAxisf ax(angle, axis);
|
||||
this->m_T.rotate(Eigen::Quaternion<float>(ax));
|
||||
}
|
||||
|
||||
void Rotate(const float angle, Vector3f axis)
|
||||
{
|
||||
axis.normalize(); // prehaps not necessary ( see eigens )
|
||||
Eigen::AngleAxisf ax(angle,axis);
|
||||
this->m_T.rotate(Eigen::Quaternion<float>(ax));
|
||||
}
|
||||
void Rotate(const Vector3f euler_axis) {
|
||||
float angle = euler_axis.norm();
|
||||
Rotate(angle, euler_axis);
|
||||
}
|
||||
|
||||
void Rotate(const Vector3f euler_axis) {
|
||||
float angle = euler_axis.norm();
|
||||
Rotate(angle,euler_axis);
|
||||
}
|
||||
void PreRotate(const Matrix3f &m) { this->m_T.prerotate(m); }
|
||||
|
||||
void PreRotate(const Matrix3f &m) { this->m_T.prerotate(m); }
|
||||
void QuaternionRotate(const Vector4f &q) {
|
||||
this->m_T.rotate(Eigen::Quaternion<float>(q));
|
||||
}
|
||||
|
||||
void QuaternionRotate(const Vector4f &q)
|
||||
{ this->m_T.rotate(Eigen::Quaternion<float>(q)); }
|
||||
void EulerYZYRotate(const Vector3f &e) {
|
||||
Matrix3f mat;
|
||||
mat = Eigen::AngleAxisf(e.x(), Vector3f::UnitY()) *
|
||||
Eigen::AngleAxisf(e.y(), Vector3f::UnitZ()) *
|
||||
Eigen::AngleAxisf(e.z(), Vector3f::UnitY());
|
||||
m_T.rotate(mat);
|
||||
}
|
||||
|
||||
void EulerYZYRotate(const Vector3f &e) {
|
||||
Matrix3f mat;
|
||||
mat = Eigen::AngleAxisf(e.x(), Vector3f::UnitY())
|
||||
* Eigen::AngleAxisf(e.y(), Vector3f::UnitZ())
|
||||
* Eigen::AngleAxisf(e.z(), Vector3f::UnitY());
|
||||
m_T.rotate(mat);
|
||||
}
|
||||
|
||||
void FlipAxes(int first, int second)
|
||||
{
|
||||
Matrix3f mat = Matrix3f::Identity();
|
||||
mat.col(first).swap(mat.col(second));
|
||||
m_T.rotate(mat);
|
||||
}
|
||||
void FlipAxes(int first, int second) {
|
||||
Matrix3f mat = Matrix3f::Identity();
|
||||
mat.col(first).swap(mat.col(second));
|
||||
m_T.rotate(mat);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
///////// TRS PARAMETERS /////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -189,116 +165,99 @@ typedef Eigen::Affine3f AffineMatrix;
|
||||
|
||||
class TRS : public AffineTransform {
|
||||
|
||||
uLibTypeMacro(TRS, AffineTransform)
|
||||
ULIB_SERIALIZE_ACCESS
|
||||
uLibTypeMacro(TRS, AffineTransform) ULIB_SERIALIZE_ACCESS
|
||||
// ULIB_DECLARE_PROPERTIES(TRS)
|
||||
|
||||
public:
|
||||
public :
|
||||
|
||||
Vector3f position = Vector3f::Zero();
|
||||
Vector3f rotation = Vector3f::Zero();
|
||||
Vector3f scaling = Vector3f::Ones();
|
||||
Vector3f rotation = Vector3f::Zero();
|
||||
Vector3f scaling = Vector3f::Ones();
|
||||
|
||||
TRS() = default;
|
||||
TRS() = default;
|
||||
|
||||
TRS(const class AffineTransform& at) {
|
||||
this->FromMatrix(at.GetMatrix());
|
||||
}
|
||||
TRS(const class AffineTransform &at) { this->FromMatrix(at.GetMatrix()); }
|
||||
|
||||
TRS(const Matrix4f& mat) {
|
||||
this->FromMatrix(mat);
|
||||
}
|
||||
TRS(const Matrix4f &mat) { this->FromMatrix(mat); }
|
||||
|
||||
void FromMatrix(const Matrix4f& mat) {
|
||||
this->position = mat.block<3,1>(0,3);
|
||||
|
||||
Matrix3f linear = mat.block<3,3>(0,0);
|
||||
this->scaling(0) = linear.col(0).norm();
|
||||
this->scaling(1) = linear.col(1).norm();
|
||||
this->scaling(2) = linear.col(2).norm();
|
||||
|
||||
Matrix3f rot = linear;
|
||||
if (this->scaling(0) > 1e-6) rot.col(0) /= this->scaling(0);
|
||||
if (this->scaling(1) > 1e-6) rot.col(1) /= this->scaling(1);
|
||||
if (this->scaling(2) > 1e-6) rot.col(2) /= this->scaling(2);
|
||||
|
||||
Vector3f euler = rot.eulerAngles(2, 1, 0);
|
||||
this->rotation = Vector3f(euler(2), euler(1), euler(0));
|
||||
|
||||
this->SetMatrix(mat);
|
||||
this->NotifyPropertiesUpdated();
|
||||
}
|
||||
void FromMatrix(const Matrix4f &mat) {
|
||||
this->position = mat.block<3, 1>(0, 3);
|
||||
|
||||
void SetPosition(const Vector3f &v) {
|
||||
position = v;
|
||||
this->AffineTransform::SetPosition(v);
|
||||
}
|
||||
|
||||
void SetRotation(const Vector3f &v) {
|
||||
rotation = v;
|
||||
this->SyncMatrix();
|
||||
}
|
||||
|
||||
void SetOrientation(const Vector3f &v) { SetRotation(v); }
|
||||
|
||||
void SetScale(const Vector3f &v) {
|
||||
scaling = v;
|
||||
this->SyncMatrix();
|
||||
}
|
||||
Matrix3f linear = mat.block<3, 3>(0, 0);
|
||||
this->scaling(0) = linear.col(0).norm();
|
||||
this->scaling(1) = linear.col(1).norm();
|
||||
this->scaling(2) = linear.col(2).norm();
|
||||
|
||||
void SyncMatrix() {
|
||||
this->GetTransform() = GetAffineMatrix();
|
||||
}
|
||||
Matrix3f rot = linear;
|
||||
if (this->scaling(0) > 1e-6)
|
||||
rot.col(0) /= this->scaling(0);
|
||||
if (this->scaling(1) > 1e-6)
|
||||
rot.col(1) /= this->scaling(1);
|
||||
if (this->scaling(2) > 1e-6)
|
||||
rot.col(2) /= this->scaling(2);
|
||||
|
||||
void Updated() override {
|
||||
this->SyncMatrix();
|
||||
this->NotifyPropertiesUpdated();
|
||||
this->AffineTransform::Updated();
|
||||
}
|
||||
Vector3f euler = rot.canonicalEulerAngles(2, 1, 0);
|
||||
this->rotation = Vector3f(euler(2), euler(1), euler(0));
|
||||
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT & ar, const unsigned int version) {
|
||||
ar & HRPU(position, "mm");
|
||||
ar & HRPU(rotation, "rad");
|
||||
ar & HRP(scaling);
|
||||
}
|
||||
|
||||
this->SetMatrix(mat);
|
||||
this->NotifyPropertiesUpdated();
|
||||
}
|
||||
|
||||
AffineMatrix GetAffineMatrix() const {
|
||||
AffineMatrix m = AffineMatrix::Identity();
|
||||
m.translate(position);
|
||||
m.rotate(Eigen::AngleAxisf(rotation.z(), Vector3f::UnitZ()));
|
||||
m.rotate(Eigen::AngleAxisf(rotation.y(), Vector3f::UnitY()));
|
||||
m.rotate(Eigen::AngleAxisf(rotation.x(), Vector3f::UnitX()));
|
||||
m.scale(scaling);
|
||||
return m;
|
||||
}
|
||||
void SetPosition(const Vector3f &v) {
|
||||
position = v;
|
||||
this->AffineTransform::SetPosition(v);
|
||||
}
|
||||
|
||||
Matrix4f GetMatrix() const {
|
||||
return this->GetAffineMatrix().matrix();
|
||||
}
|
||||
void SetRotation(const Vector3f &v) {
|
||||
rotation = v;
|
||||
this->SyncMatrix();
|
||||
}
|
||||
|
||||
void SetOrientation(const Vector3f &v) { SetRotation(v); }
|
||||
|
||||
|
||||
|
||||
void SetScale(const Vector3f &v) {
|
||||
scaling = v;
|
||||
this->SyncMatrix();
|
||||
}
|
||||
|
||||
void SyncMatrix() { this->GetTransform() = GetAffineMatrix(); }
|
||||
|
||||
void Updated() override {
|
||||
this->SyncMatrix();
|
||||
this->NotifyPropertiesUpdated();
|
||||
this->AffineTransform::Updated();
|
||||
}
|
||||
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT &ar, const unsigned int version) {
|
||||
ar &HRPU(position, "mm");
|
||||
ar &HRPU(rotation, "rad");
|
||||
ar &HRP(scaling);
|
||||
}
|
||||
|
||||
AffineMatrix GetAffineMatrix() const {
|
||||
AffineMatrix m = AffineMatrix::Identity();
|
||||
m.translate(position);
|
||||
m.rotate(Eigen::AngleAxisf(rotation.z(), Vector3f::UnitZ()));
|
||||
m.rotate(Eigen::AngleAxisf(rotation.y(), Vector3f::UnitY()));
|
||||
m.rotate(Eigen::AngleAxisf(rotation.x(), Vector3f::UnitX()));
|
||||
m.scale(scaling);
|
||||
return m;
|
||||
}
|
||||
|
||||
Matrix4f GetMatrix() const { return this->GetAffineMatrix().matrix(); }
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline std::ostream& operator<<(std::ostream& os, const TRS& trs) {
|
||||
os << trs.position << " " << trs.rotation << " " << trs.scaling;
|
||||
return os;
|
||||
inline std::ostream &operator<<(std::ostream &os, const TRS &trs) {
|
||||
os << trs.position << " " << trs.rotation << " " << trs.scaling;
|
||||
return os;
|
||||
}
|
||||
|
||||
inline std::istream& operator>>(std::istream& is, TRS& trs) {
|
||||
is >> trs.position >> trs.rotation >> trs.scaling;
|
||||
return is;
|
||||
inline std::istream &operator>>(std::istream &is, TRS &trs) {
|
||||
is >> trs.position >> trs.rotation >> trs.scaling;
|
||||
return is;
|
||||
}
|
||||
|
||||
} // uLib
|
||||
} // namespace uLib
|
||||
|
||||
|
||||
|
||||
#endif//U_TRANSFORM_H
|
||||
#endif // U_TRANSFORM_H
|
||||
|
||||
Reference in New Issue
Block a user