refactor: update Puppet transform logic to support AffineTransform world matrices and improve selection highlighting
This commit is contained in:
@@ -21,7 +21,7 @@ namespace uLib {
|
||||
|
||||
Assembly::Assembly()
|
||||
: ObjectsContext(),
|
||||
AffineTransform(),
|
||||
TRS(),
|
||||
m_BBoxMin(Vector3f::Zero()),
|
||||
m_BBoxMax(Vector3f::Zero()),
|
||||
m_ShowBoundingBox(false),
|
||||
@@ -31,7 +31,7 @@ Assembly::Assembly()
|
||||
|
||||
Assembly::Assembly(const Assembly ©)
|
||||
: ObjectsContext(copy),
|
||||
AffineTransform(copy),
|
||||
TRS(copy),
|
||||
m_BBoxMin(copy.m_BBoxMin),
|
||||
m_BBoxMax(copy.m_BBoxMax),
|
||||
m_ShowBoundingBox(copy.m_ShowBoundingBox),
|
||||
@@ -55,6 +55,10 @@ void Assembly::AddObject(Object *obj) {
|
||||
this->ComputeBoundingBox();
|
||||
this->Updated(); // Signal that assembly itself changed (AABB-wise)
|
||||
});
|
||||
|
||||
// Parent -> Child propagation for world matrix updates
|
||||
Object::connect(this, &Object::Updated, obj, &Object::Updated);
|
||||
|
||||
this->ComputeBoundingBox();
|
||||
}
|
||||
|
||||
|
||||
@@ -43,9 +43,9 @@ namespace uLib {
|
||||
* A bounding box is automatically computed from all contained objects and
|
||||
* can be queried or shown/hidden through the VTK puppet.
|
||||
*/
|
||||
class Assembly : public ObjectsContext, public AffineTransform {
|
||||
class Assembly : public ObjectsContext, public TRS {
|
||||
public:
|
||||
uLibTypeMacro(Assembly, ObjectsContext, AffineTransform)
|
||||
uLibTypeMacro(Assembly, ObjectsContext, TRS)
|
||||
virtual const char *GetClassName() const override { return "Assembly"; }
|
||||
|
||||
Assembly();
|
||||
@@ -54,7 +54,7 @@ public:
|
||||
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("AffineTransform", boost::serialization::base_object<AffineTransform>(*this));
|
||||
ar & boost::serialization::make_nvp("TRS", boost::serialization::base_object<TRS>(*this));
|
||||
ar & boost::serialization::make_hrp("GroupSelection", m_GroupSelection);
|
||||
}
|
||||
|
||||
|
||||
@@ -39,20 +39,21 @@ namespace uLib {
|
||||
* @brief Represents an oriented bounding box (OBB) within a hierarchical
|
||||
* transformation system.
|
||||
*
|
||||
* ContainerBox inherits from AffineTransform, which defines its parent
|
||||
* ContainerBox inherits from TRS, which defines its parent
|
||||
* coordinate system. It contains an internal local transformation (m_LocalT)
|
||||
* that defines the box's specific origin and size relative to its own
|
||||
* coordinate system.
|
||||
*/
|
||||
class ContainerBox : public AffineTransform {
|
||||
class ContainerBox : public TRS {
|
||||
|
||||
public:
|
||||
uLibTypeMacro(ContainerBox, AffineTransform)
|
||||
uLibTypeMacro(ContainerBox, TRS)
|
||||
|
||||
virtual const char * GetClassName() const override { return "ContainerBox"; }
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
// PROPERTIES //
|
||||
|
||||
Vector3f Size;
|
||||
Vector3f Origin;
|
||||
|
||||
@@ -86,7 +87,7 @@ public:
|
||||
*/
|
||||
ContainerBox(const ContainerBox ©)
|
||||
: m_LocalT(this), // Reset parent to the new object
|
||||
AffineTransform(copy),
|
||||
TRS(copy),
|
||||
Size(copy.Size),
|
||||
Origin(copy.Origin) {
|
||||
ULIB_ACTIVATE_PROPERTIES(*this);
|
||||
@@ -98,6 +99,7 @@ public:
|
||||
*/
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("TRS", boost::serialization::base_object<TRS>(*this));
|
||||
ar & HRP(Size);
|
||||
ar & HRP(Origin);
|
||||
}
|
||||
|
||||
@@ -39,10 +39,10 @@ namespace uLib {
|
||||
* The cylinder orientation is defined by the Axis property (0=X, 1=Y, 2=Z).
|
||||
* By default, it is aligned with the Y axis (Axis=1).
|
||||
*/
|
||||
class Cylinder : public AffineTransform {
|
||||
class Cylinder : public TRS {
|
||||
|
||||
public:
|
||||
uLibTypeMacro(Cylinder, AffineTransform)
|
||||
uLibTypeMacro(Cylinder, TRS)
|
||||
|
||||
/**
|
||||
* @brief PROPERTIES
|
||||
@@ -74,7 +74,7 @@ public:
|
||||
* @brief Copy constructor.
|
||||
*/
|
||||
Cylinder(const Cylinder ©)
|
||||
: m_LocalT(this), AffineTransform(copy), Radius(copy.Radius), Height(copy.Height), Axis(copy.Axis) {
|
||||
: m_LocalT(this), TRS(copy), Radius(copy.Radius), Height(copy.Height), Axis(copy.Axis) {
|
||||
ULIB_ACTIVATE_PROPERTIES(*this);
|
||||
this->Sync();
|
||||
}
|
||||
@@ -84,6 +84,7 @@ public:
|
||||
*/
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("TRS", boost::serialization::base_object<TRS>(*this));
|
||||
ar & HRP(Radius);
|
||||
ar & HRP(Height);
|
||||
ar & HRP(Axis);
|
||||
|
||||
@@ -35,12 +35,22 @@
|
||||
|
||||
namespace uLib {
|
||||
|
||||
class Geometry : public AffineTransform {
|
||||
|
||||
class Geometry : virtual public Object {
|
||||
protected:
|
||||
Geometry* m_Parent = nullptr;
|
||||
|
||||
public:
|
||||
uLibTypeMacro(Geometry, AffineTransform)
|
||||
uLibTypeMacro(Geometry, Object)
|
||||
|
||||
virtual const char * GetClassName() const override { return "Geometry"; }
|
||||
|
||||
virtual void SetParent(Geometry* p) { m_Parent = p; }
|
||||
virtual Geometry* GetParent() const { return m_Parent; }
|
||||
|
||||
virtual bool IsLinear() const { return false; }
|
||||
virtual bool IsPure() const { return false; }
|
||||
|
||||
virtual Vector3f ToLinear(const Vector3f& curved_space) const {
|
||||
return curved_space;
|
||||
}
|
||||
@@ -49,38 +59,120 @@ public:
|
||||
return cartesian_space;
|
||||
}
|
||||
|
||||
inline Vector4f GetWorldPoint(const Vector4f v) const {
|
||||
Vector3f lin = ToLinear(Vector3f(v.x(), v.y(), v.z()));
|
||||
return this->GetWorldMatrix() * Vector4f(lin.x(), lin.y(), lin.z(), v.w());
|
||||
virtual Vector4f GetWorldPoint(const Vector4f v) const = 0;
|
||||
virtual Vector4f GetLocalPoint(const Vector4f v) const = 0;
|
||||
|
||||
virtual Vector4f GetWorldPoint(const float x, const float y, const float z) const {
|
||||
return GetWorldPoint(Vector4f(x,y,z,1));
|
||||
}
|
||||
|
||||
inline Vector4f GetWorldPoint(const float x, const float y, const float z) {
|
||||
return this->GetWorldPoint(Vector4f(x,y,z,1));
|
||||
virtual Vector4f GetLocalPoint(const float x, const float y, const float z) const {
|
||||
return GetLocalPoint(Vector4f(x,y,z,1));
|
||||
}
|
||||
|
||||
inline Vector4f GetLocalPoint(const Vector4f v) const {
|
||||
Vector4f loc_lin = this->GetWorldMatrix().inverse() * v;
|
||||
Vector3f curv = FromLinear(Vector3f(loc_lin.x(), loc_lin.y(), loc_lin.z()));
|
||||
return Vector4f(curv.x(), curv.y(), curv.z(), loc_lin.w());
|
||||
virtual Vector4f GetWorldPoint(const Vector3f v) const {
|
||||
return GetWorldPoint(Vector4f(v.x(), v.y(), v.z(), 1.0f));
|
||||
}
|
||||
|
||||
inline Vector4f GetLocalPoint(const float x, const float y, const float z) {
|
||||
return this->GetLocalPoint(Vector4f(x,y,z,1));
|
||||
virtual Vector4f GetLocalPoint(const Vector3f v) const {
|
||||
return GetLocalPoint(Vector4f(v.x(), v.y(), v.z(), 1.0f));
|
||||
}
|
||||
|
||||
virtual void Translate(Vector3f t) = 0;
|
||||
virtual void Rotate(Vector3f r) = 0;
|
||||
virtual void Scale(Vector3f s) = 0;
|
||||
|
||||
};
|
||||
|
||||
class CylindricalGeometry : public Geometry {
|
||||
|
||||
|
||||
class LinearGeometry : public Geometry {
|
||||
protected:
|
||||
Affine3f m_T = Affine3f::Identity();
|
||||
|
||||
public:
|
||||
uLibTypeMacro(CylindricalGeometry, Geometry)
|
||||
uLibTypeMacro(LinearGeometry, Geometry)
|
||||
|
||||
virtual const char * GetClassName() const override { return "LinearGeometry"; }
|
||||
|
||||
virtual bool IsLinear() const override { return true; }
|
||||
virtual bool IsPure() const override { return true; }
|
||||
|
||||
virtual Vector4f GetWorldPoint(const Vector4f v) const override {
|
||||
Vector3f lin_v = ToLinear(v.head<3>());
|
||||
Vector4f v_lin(lin_v.x(), lin_v.y(), lin_v.z(), v.w());
|
||||
|
||||
Affine3f combined = m_T;
|
||||
const Geometry* curr = m_Parent;
|
||||
while (curr && curr->IsLinear() && curr->IsPure()) {
|
||||
combined = static_cast<const LinearGeometry*>(curr)->m_T * combined;
|
||||
curr = curr->GetParent();
|
||||
}
|
||||
|
||||
Vector4f v_res = combined.matrix() * v_lin;
|
||||
if (curr) return curr->GetWorldPoint(v_res);
|
||||
return v_res;
|
||||
}
|
||||
|
||||
virtual Vector4f GetLocalPoint(const Vector4f v) const override {
|
||||
Vector4f v_parent = m_Parent ? m_Parent->GetLocalPoint(v) : v;
|
||||
Vector4f v_loc_lin = m_T.inverse().matrix() * v_parent;
|
||||
Vector3f v_curv = FromLinear(v_loc_lin.head<3>());
|
||||
return Vector4f(v_curv.x(), v_curv.y(), v_curv.z(), v_loc_lin.w());
|
||||
}
|
||||
|
||||
virtual void Translate(Vector3f t) override {
|
||||
m_T.translate(t);
|
||||
}
|
||||
|
||||
virtual void Rotate(Vector3f r) override {
|
||||
this->EulerYZYRotate(r);
|
||||
}
|
||||
|
||||
virtual void Scale(Vector3f s) override {
|
||||
m_T.scale(s);
|
||||
}
|
||||
|
||||
void SetPosition(const Vector3f& v) { m_T.translation() = v; }
|
||||
Vector3f GetPosition() const { return m_T.translation(); }
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
const Affine3f& GetTransform() const { return m_T; }
|
||||
void SetTransform(const Affine3f& t) { m_T = t; }
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
class CylindricalGeometry : public LinearGeometry {
|
||||
public:
|
||||
uLibTypeMacro(CylindricalGeometry, LinearGeometry)
|
||||
CylindricalGeometry() {}
|
||||
|
||||
Vector3f ToLinear(const Vector3f& cylindrical) const {
|
||||
virtual const char * GetClassName() const override { return "CylindricalGeometry"; }
|
||||
|
||||
virtual bool IsPure() const override { return false; }
|
||||
|
||||
Vector3f ToLinear(const Vector3f& cylindrical) const override {
|
||||
return Vector3f(cylindrical.x() * std::cos(cylindrical.y()),
|
||||
cylindrical.x() * std::sin(cylindrical.y()),
|
||||
cylindrical.z());
|
||||
}
|
||||
|
||||
Vector3f FromLinear(const Vector3f& linear) const {
|
||||
Vector3f FromLinear(const Vector3f& linear) const override {
|
||||
float r = std::sqrt(linear.x() * linear.x() + linear.y() * linear.y());
|
||||
float phi = std::atan2(linear.y(), linear.x());
|
||||
return Vector3f(r, phi, linear.z());
|
||||
@@ -88,14 +180,16 @@ public:
|
||||
|
||||
};
|
||||
|
||||
class SphericalGeometry : public Geometry {
|
||||
class SphericalGeometry : public LinearGeometry {
|
||||
public:
|
||||
uLibTypeMacro(SphericalGeometry, Geometry)
|
||||
uLibTypeMacro(SphericalGeometry, LinearGeometry)
|
||||
SphericalGeometry() {}
|
||||
|
||||
virtual const char * GetClassName() const override { return "SphericalGeometry"; }
|
||||
|
||||
Vector3f ToLinear(const Vector3f& spherical) const {
|
||||
virtual bool IsPure() const override { return false; }
|
||||
|
||||
Vector3f ToLinear(const Vector3f& spherical) const override {
|
||||
float r = spherical.x();
|
||||
float theta = spherical.y();
|
||||
float phi = spherical.z();
|
||||
@@ -104,7 +198,7 @@ public:
|
||||
r * std::cos(theta));
|
||||
}
|
||||
|
||||
Vector3f FromLinear(const Vector3f& linear) const {
|
||||
Vector3f FromLinear(const Vector3f& linear) const override {
|
||||
float r = linear.norm();
|
||||
float theta = (r == 0.0f) ? 0.0f : std::acos(linear.z() / r);
|
||||
float phi = std::atan2(linear.y(), linear.x());
|
||||
@@ -113,14 +207,16 @@ public:
|
||||
|
||||
};
|
||||
|
||||
class ToroidalGeometry : public Geometry {
|
||||
class ToroidalGeometry : public LinearGeometry {
|
||||
public:
|
||||
uLibTypeMacro(ToroidalGeometry, Geometry)
|
||||
uLibTypeMacro(ToroidalGeometry, LinearGeometry)
|
||||
ToroidalGeometry(float Rtor) : m_Rtor(Rtor) {}
|
||||
|
||||
virtual const char * GetClassName() const override { return "ToroidalGeometry"; }
|
||||
|
||||
Vector3f ToLinear(const Vector3f& toroidal) const {
|
||||
virtual bool IsPure() const override { return false; }
|
||||
|
||||
Vector3f ToLinear(const Vector3f& toroidal) const override {
|
||||
float r = toroidal.x();
|
||||
float theta = toroidal.y();
|
||||
float phi = toroidal.z();
|
||||
@@ -129,7 +225,7 @@ public:
|
||||
r * std::sin(theta));
|
||||
}
|
||||
|
||||
Vector3f FromLinear(const Vector3f& linear) const {
|
||||
Vector3f FromLinear(const Vector3f& linear) const override {
|
||||
float phi = std::atan2(linear.y(), linear.x());
|
||||
float r_xy = std::sqrt(linear.x() * linear.x() + linear.y() * linear.y());
|
||||
float delta_r = r_xy - m_Rtor;
|
||||
|
||||
@@ -10,6 +10,7 @@
|
||||
|
||||
namespace uLib {
|
||||
|
||||
ULIB_REGISTER_OBJECT(TRS)
|
||||
ULIB_REGISTER_OBJECT(ContainerBox)
|
||||
ULIB_REGISTER_OBJECT(Cylinder)
|
||||
ULIB_REGISTER_OBJECT(Assembly)
|
||||
|
||||
@@ -34,10 +34,10 @@
|
||||
|
||||
namespace uLib {
|
||||
|
||||
class QuadMesh : public AffineTransform
|
||||
class QuadMesh : public TRS
|
||||
{
|
||||
public:
|
||||
uLibTypeMacro(QuadMesh, AffineTransform)
|
||||
uLibTypeMacro(QuadMesh, TRS)
|
||||
|
||||
virtual const char * GetClassName() const override { return "QuadMesh"; }
|
||||
|
||||
|
||||
@@ -57,21 +57,150 @@
|
||||
namespace uLib {
|
||||
|
||||
|
||||
|
||||
using Eigen::Isometry3f;
|
||||
using Eigen::Isometry3d;
|
||||
|
||||
using Eigen::Affine3f;
|
||||
using Eigen::Affine3d;
|
||||
|
||||
using Eigen::Projective3f;
|
||||
using Eigen::Projective3d;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
///////// 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 {
|
||||
class TRS : public AffineTransform {
|
||||
|
||||
public:
|
||||
uLibTypeMacro(TRS, AffineTransform)
|
||||
|
||||
Vector3f position = Vector3f::Zero();
|
||||
Vector3f rotation = Vector3f::Zero();
|
||||
Vector3f scaling = Vector3f::Ones();
|
||||
|
||||
TRS() = default;
|
||||
|
||||
TRS(const class AffineTransform& at);
|
||||
TRS(const class AffineTransform& at) {
|
||||
this->FromMatrix(at.GetMatrix());
|
||||
}
|
||||
|
||||
TRS(const Matrix4f& mat) {
|
||||
this->FromMatrix(mat);
|
||||
@@ -90,221 +219,60 @@ public:
|
||||
if (this->scaling(1) > 1e-6) rot.col(1) /= this->scaling(1);
|
||||
if (this->scaling(2) > 1e-6) rot.col(2) /= this->scaling(2);
|
||||
|
||||
// Decompose to Euler angles matching VTK (M = Rz * Ry * Rx)
|
||||
// Store internally as RADIANS (standard for uLib properties)
|
||||
Vector3f euler = rot.eulerAngles(2, 1, 0);
|
||||
this->rotation = Vector3f(euler(2), euler(1), euler(0));
|
||||
|
||||
this->SetMatrix(mat);
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT & ar, const unsigned int version) {
|
||||
ar & HRPU(position, "mm");
|
||||
ar & HRPU(rotation, "deg"); // Metadata informs UI to convert to/from degrees
|
||||
ar & HRPU(rotation, "rad");
|
||||
ar & HRP(scaling);
|
||||
}
|
||||
|
||||
AffineMatrix GetAffineMatrix() const {
|
||||
AffineMatrix t = AffineMatrix::Identity();
|
||||
t.translate(position);
|
||||
|
||||
// rotation is in Radians here
|
||||
t.rotate(Eigen::AngleAxisf(rotation.z(), Vector3f::UnitZ()));
|
||||
t.rotate(Eigen::AngleAxisf(rotation.y(), Vector3f::UnitY()));
|
||||
t.rotate(Eigen::AngleAxisf(rotation.x(), Vector3f::UnitX()));
|
||||
|
||||
t.scale(scaling);
|
||||
return t;
|
||||
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();
|
||||
}
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
///////// AFFINE TRANSFORM WRAPPER //////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class AffineTransform : virtual public Object {
|
||||
public:
|
||||
uLibTypeMacro(AffineTransform, Object)
|
||||
|
||||
TRS Transform;
|
||||
|
||||
private:
|
||||
void NotifyProperties() {
|
||||
PropertyBase *p;
|
||||
if ((p = this->GetProperty("Transform.position"))) p->Updated();
|
||||
if ((p = this->GetProperty("Transform.rotation"))) p->Updated();
|
||||
if ((p = this->GetProperty("Transform.scaling"))) p->Updated();
|
||||
}
|
||||
|
||||
protected:
|
||||
Eigen::Affine3f m_T;
|
||||
AffineTransform *m_Parent;
|
||||
|
||||
public:
|
||||
AffineTransform() :
|
||||
m_T(Matrix4f::Identity()),
|
||||
m_Parent(NULL)
|
||||
{
|
||||
ULIB_ACTIVATE_PROPERTIES(*this);
|
||||
this->Sync();
|
||||
}
|
||||
|
||||
virtual ~AffineTransform() {}
|
||||
|
||||
AffineTransform(AffineTransform *parent) :
|
||||
m_T(Matrix4f::Identity()),
|
||||
m_Parent(parent)
|
||||
{
|
||||
ULIB_ACTIVATE_PROPERTIES(*this);
|
||||
this->Sync();
|
||||
}
|
||||
|
||||
AffineTransform(const AffineTransform ©) :
|
||||
m_T(copy.m_T),
|
||||
m_Parent(copy.m_Parent),
|
||||
Transform(copy.Transform)
|
||||
{
|
||||
ULIB_ACTIVATE_PROPERTIES(*this);
|
||||
this->Sync();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Registration of properties in groups.
|
||||
*/
|
||||
template <class ArchiveT>
|
||||
void serialize(ArchiveT & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("Transform", Transform);
|
||||
}
|
||||
|
||||
Eigen::Affine3f& GetTransform() { return m_T; }
|
||||
|
||||
AffineTransform *GetParent() const { return this->m_Parent; }
|
||||
|
||||
void SetParent(AffineTransform *name) { this->m_Parent = name; }
|
||||
|
||||
void SetMatrix (Matrix4f mat) {
|
||||
m_T.matrix() = mat;
|
||||
this->UpdatePropertiesFromMatrix();
|
||||
}
|
||||
|
||||
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 SetPosition(const Vector3f v) {
|
||||
this->Transform.position = v;
|
||||
this->Updated();
|
||||
this->NotifyProperties();
|
||||
}
|
||||
Vector3f GetPosition() const { return this->Transform.position; }
|
||||
|
||||
void SetOrientation(const Vector3f v) {
|
||||
this->Transform.rotation = v;
|
||||
this->Updated();
|
||||
this->NotifyProperties();
|
||||
}
|
||||
Vector3f GetOrientation() const { return this->Transform.rotation; }
|
||||
|
||||
void SetScale(const Vector3f v) {
|
||||
this->Transform.scaling = v;
|
||||
this->Updated();
|
||||
this->NotifyProperties();
|
||||
}
|
||||
Vector3f GetScale() const { return this->Transform.scaling; }
|
||||
|
||||
void SetRotation(const Matrix3f m) {
|
||||
this->m_T.linear() = m;
|
||||
this->UpdatePropertiesFromMatrix();
|
||||
}
|
||||
|
||||
Matrix3f GetRotation() const { return this->m_T.rotation(); }
|
||||
|
||||
void Translate(const Vector3f v) {
|
||||
this->Transform.position += v;
|
||||
this->Sync();
|
||||
}
|
||||
|
||||
void Scale(const Vector3f v) {
|
||||
this->Transform.scaling = this->Transform.scaling.cwiseProduct(v);
|
||||
this->Sync();
|
||||
}
|
||||
|
||||
|
||||
void Rotate(const Matrix3f m) {
|
||||
this->m_T.rotate(m);
|
||||
this->UpdatePropertiesFromMatrix();
|
||||
}
|
||||
|
||||
void Rotate(const float angle, Vector3f axis)
|
||||
{
|
||||
axis.normalize();
|
||||
Eigen::AngleAxisf ax(angle,axis);
|
||||
this->m_T.rotate(Eigen::Quaternion<float>(ax));
|
||||
this->UpdatePropertiesFromMatrix();
|
||||
}
|
||||
|
||||
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); this->UpdatePropertiesFromMatrix(); }
|
||||
|
||||
void QuaternionRotate(const Vector4f q)
|
||||
{ this->m_T.rotate(Eigen::Quaternion<float>(q)); this->UpdatePropertiesFromMatrix(); }
|
||||
|
||||
void EulerYZYRotate(const Vector3f e) {
|
||||
this->Transform.rotation = e;
|
||||
this->Sync();
|
||||
}
|
||||
|
||||
void FlipAxes(int first, int second)
|
||||
{
|
||||
Matrix3f mat = Matrix3f::Identity();
|
||||
mat.col(first).swap(mat.col(second));
|
||||
m_T.rotate(mat);
|
||||
this->UpdatePropertiesFromMatrix();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Decomposes the internal matrix m_T back into Position, Orientation, and Scale properties.
|
||||
*/
|
||||
void UpdatePropertiesFromMatrix() {
|
||||
this->Transform.FromMatrix(this->GetMatrix());
|
||||
|
||||
PropertyBase *p;
|
||||
if ((p = this->GetProperty("Transform.position"))) p->Updated();
|
||||
if ((p = this->GetProperty("Transform.rotation"))) p->Updated();
|
||||
if ((p = this->GetProperty("Transform.scaling"))) p->Updated();
|
||||
}
|
||||
|
||||
signals:
|
||||
/** Signal emitted when properties change */
|
||||
virtual void Updated() override {
|
||||
this->Sync();
|
||||
ULIB_SIGNAL_EMIT(Object::Updated);
|
||||
}
|
||||
|
||||
private:
|
||||
void Sync() {
|
||||
m_T.matrix() = this->Transform.GetAffineMatrix().matrix();
|
||||
}
|
||||
};
|
||||
|
||||
inline TRS::TRS(const AffineTransform& at) {
|
||||
this->position = at.GetPosition();
|
||||
this->rotation = at.GetOrientation();
|
||||
this->scaling = at.GetScale();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
} // uLib
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -37,10 +37,10 @@
|
||||
|
||||
namespace uLib {
|
||||
|
||||
class TriangleMesh : public AffineTransform
|
||||
class TriangleMesh : public TRS
|
||||
{
|
||||
public:
|
||||
uLibTypeMacro(TriangleMesh, AffineTransform)
|
||||
uLibTypeMacro(TriangleMesh, TRS)
|
||||
|
||||
virtual const char * GetClassName() const override { return "TriangleMesh"; }
|
||||
|
||||
|
||||
@@ -53,7 +53,7 @@ int main()
|
||||
///////////////// GEOMETRY TESTING ///////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
Geometry Geo;
|
||||
LinearGeometry Geo;
|
||||
|
||||
Geo.SetPosition(Vector3f(1,1,1));
|
||||
Geo.EulerYZYRotate(Vector3f(0,0,0));
|
||||
@@ -77,7 +77,7 @@ int main()
|
||||
|
||||
Geo.EulerYZYRotate(Vector3f(0,0,M_PI_2));
|
||||
wp = Geo.GetWorldPoint(HPoint3f(1,1,1));
|
||||
// std::cout << "Geometry matrix\n" << Geo.GetTransform() << "\n";
|
||||
// std::cout << "Geometry matrix\n" << Geo.GetTransform().matrix() << "\n";
|
||||
// std::cout << "World 1,1,1 coords\n" << wp << "\n";
|
||||
TEST0( Vector4f0(wp - HPoint3f(0,2,2)) );
|
||||
|
||||
@@ -122,6 +122,27 @@ int main()
|
||||
TEST0( Vector4f0(recovered.homogeneous() - tor_pt.homogeneous()) );
|
||||
}
|
||||
|
||||
// PARENT GEOMETRY TESTING
|
||||
{
|
||||
LinearGeometry parent;
|
||||
parent.Translate(Vector3f(10, 0, 0));
|
||||
|
||||
LinearGeometry child;
|
||||
child.SetParent(&parent);
|
||||
child.Translate(Vector3f(0, 5, 0));
|
||||
|
||||
HPoint3f wp = child.GetWorldPoint(HPoint3f(1, 1, 1));
|
||||
TEST0( Vector4f0(wp - HPoint3f(11, 6, 1)) );
|
||||
|
||||
CylindricalGeometry cparent;
|
||||
LinearGeometry grandchild;
|
||||
grandchild.SetParent(&cparent);
|
||||
grandchild.Translate(Vector3f(1, 0, 0));
|
||||
|
||||
HPoint3f gp = grandchild.GetWorldPoint(HPoint3f(1, M_PI_2, 0));
|
||||
TEST0( Vector4f0(gp - HPoint3f(0, 2, 0)) );
|
||||
}
|
||||
|
||||
END_TESTING;
|
||||
}
|
||||
|
||||
|
||||
@@ -31,8 +31,10 @@
|
||||
static int _fail = 0; \
|
||||
printf("..:: Testing " #name " ::..\n");
|
||||
|
||||
#define TEST1(val) if ((val)==0) { printf("Assertion failed: %s != 0\n", #val); _fail++; }
|
||||
#define TEST0(val) if ((val)!=0) { printf("Assertion failed: %s != 0\n", #val); _fail++; }
|
||||
#define TEST1(val) _fail += (val)==0
|
||||
#define TEST0(val) _fail += (val)!=0
|
||||
#define ASSERT_EQUAL(a,b) if((a)!=(b)) { printf("Assertion failed: " #a " != " #b " at line %d\n", __LINE__); _fail++; }
|
||||
#define ASSERT_NOT_NULL(ptr) if((ptr)==NULL) { printf("Assertion failed: " #ptr " is NULL at line %d\n", __LINE__); _fail++; }
|
||||
#define END_TESTING return _fail;
|
||||
|
||||
#define ASSERT_EQ(a, b) if ((a) != (b)) { printf("Assertion failed: %s != %s\n", #a, #b); _fail++; }
|
||||
Reference in New Issue
Block a user