265 lines
7.9 KiB
C++
265 lines
7.9 KiB
C++
/*//////////////////////////////////////////////////////////////////////////////
|
|
// CMT Cosmic Muon Tomography project //////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
Copyright (c) 2014, Universita' degli Studi di Padova, INFN sez. di Padova
|
|
All rights reserved
|
|
|
|
Authors: Andrea Rigoni Garola < andrea.rigoni@pd.infn.it >
|
|
|
|
------------------------------------------------------------------
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU Lesser General Public
|
|
License as published by the Free Software Foundation; either
|
|
version 3.0 of the License, or (at your option) any later version.
|
|
|
|
This library is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
Lesser General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public
|
|
License along with this library.
|
|
|
|
//////////////////////////////////////////////////////////////////////////////*/
|
|
|
|
/*
|
|
* Copyright (C) 2012 Andrea Rigoni Garola <andrea.rigoni@pd.infn.it>
|
|
*
|
|
* This library is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU Lesser General Public License as
|
|
* published by the Free Software Foundation; either version 2.1 of the
|
|
* License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
|
|
* 02110-1301 USA
|
|
*
|
|
*/
|
|
|
|
#ifndef U_TRANSFORM_H
|
|
#define U_TRANSFORM_H
|
|
|
|
#include "Math/Dense.h"
|
|
#include "Math/Units.h"
|
|
#include <Eigen/Geometry>
|
|
|
|
namespace uLib {
|
|
|
|
using Eigen::Isometry3d;
|
|
using Eigen::Isometry3f;
|
|
|
|
using Eigen::Affine3d;
|
|
using Eigen::Affine3f;
|
|
|
|
using Eigen::Projective3d;
|
|
using Eigen::Projective3f;
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
///////// AFFINE TRANSFORM WRAPPER //////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
class AffineTransform : virtual public Object {
|
|
public:
|
|
uLibTypeMacro(AffineTransform, Object) protected :
|
|
|
|
Affine3f m_T;
|
|
AffineTransform *m_Parent;
|
|
|
|
public:
|
|
AffineTransform() : m_T(Matrix4f::Identity()), m_Parent(NULL) {}
|
|
|
|
AffineTransform(AffineTransform *parent)
|
|
: m_T(Matrix4f::Identity()), m_Parent(parent) {}
|
|
|
|
AffineTransform(const AffineTransform ©)
|
|
: m_T(copy.m_T), m_Parent(copy.m_Parent) {}
|
|
|
|
Affine3f &GetTransform() { return m_T; }
|
|
|
|
AffineTransform *GetParent() const { return this->m_Parent; }
|
|
|
|
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(); }
|
|
|
|
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 SetPosition(const Vector3f &v) { this->m_T.translation() = v; }
|
|
|
|
Vector3f GetPosition() const { return this->m_T.translation(); }
|
|
|
|
void SetRotation(const Matrix3f &m) { this->m_T.linear() = m; }
|
|
|
|
Matrix3f GetRotation() const { return this->m_T.rotation(); }
|
|
|
|
void Translate(const Vector3f &v) { this->m_T.translate(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());
|
|
}
|
|
|
|
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 Vector3f euler_axis) {
|
|
float angle = euler_axis.norm();
|
|
Rotate(angle, euler_axis);
|
|
}
|
|
|
|
void PreRotate(const Matrix3f &m) { this->m_T.prerotate(m); }
|
|
|
|
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 FlipAxes(int first, int second) {
|
|
Matrix3f mat = Matrix3f::Identity();
|
|
mat.col(first).swap(mat.col(second));
|
|
m_T.rotate(mat);
|
|
}
|
|
};
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
///////// TRS PARAMETERS /////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
typedef Eigen::Affine3f AffineMatrix;
|
|
|
|
class TRS : public AffineTransform {
|
|
|
|
uLibTypeMacro(TRS, AffineTransform) ULIB_SERIALIZE_ACCESS
|
|
// ULIB_DECLARE_PROPERTIES(TRS)
|
|
|
|
public :
|
|
|
|
Vector3f position = Vector3f::Zero();
|
|
Vector3f rotation = Vector3f::Zero();
|
|
Vector3f scaling = Vector3f::Ones();
|
|
|
|
TRS() = default;
|
|
|
|
TRS(const class AffineTransform &at) { this->FromMatrix(at.GetMatrix()); }
|
|
|
|
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.canonicalEulerAngles(2, 1, 0);
|
|
this->rotation = Vector3f(euler(2), euler(1), euler(0));
|
|
|
|
this->SetMatrix(mat);
|
|
this->NotifyPropertiesUpdated();
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
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::istream &operator>>(std::istream &is, TRS &trs) {
|
|
is >> trs.position >> trs.rotation >> trs.scaling;
|
|
return is;
|
|
}
|
|
|
|
} // namespace uLib
|
|
|
|
#endif // U_TRANSFORM_H
|