OpenCMT/uLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed most ( still units error )

Browse Source
This commit is contained in:
AndreaRigoni
2026-03-27 15:02:17 +00:00
parent 93e5602562
commit 038c6f99f4
22 changed files with 411 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
src/Math/Transform.h
View File

@@ -56,6 +56,67 @@
namespace uLib {
////////////////////////////////////////////////////////////////////////////////
///////// TRS PARAMETERS /////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
typedef Eigen::Affine3f AffineMatrix;
class TRS {
public:
Vector3f position = Vector3f::Zero();
Vector3f rotation = Vector3f::Zero();
Vector3f scaling = Vector3f::Ones();
TRS() = default;
TRS(const class AffineTransform& at);
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);
// 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));
}
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 & 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;
}
};
////////////////////////////////////////////////////////////////////////////////
///////// AFFINE TRANSFORM WRAPPER //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
@@ -64,21 +125,15 @@ class AffineTransform : virtual public Object {
public:
uLibTypeMacro(AffineTransform, Object)
/**
* @brief Grouped transformation parameters for property-based control.
*/
struct {
Vector3f Position = Vector3f::Zero();
Vector3f Orientation = Vector3f::Zero();
Vector3f Scale = Vector3f::Ones();
TRS Transform;
template <class ArchiveT>
void serialize(ArchiveT & ar, const unsigned int version) {
ar & HRPU(Position, "mm");
ar & HRPU(Orientation, "deg");
ar & HRP(Scale);
}
} 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;
@@ -140,11 +195,25 @@ public:
}
void SetPosition(const Vector3f v) {
this->Transform.Position = v;
this->Sync();
this->Transform.position = v;
this->Updated();
this->NotifyProperties();
}
Vector3f GetPosition() const { return this->Transform.position; }
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;
@@ -154,24 +223,15 @@ public:
Matrix3f GetRotation() const { return this->m_T.rotation(); }
void Translate(const Vector3f v) {
this->Transform.Position += v;
this->Transform.position += v;
this->Sync();
}
void Scale(const Vector3f v) {
this->Transform.Scale = this->Transform.Scale.cwiseProduct(v);
this->Transform.scaling = this->Transform.scaling.cwiseProduct(v);
this->Sync();
}
Vector3f GetScale() const { return this->Transform.Scale; }
void SetOrientation(const Vector3f v) {
this->Transform.Orientation = v;
this->Sync();
}
Vector3f GetOrientation() const { return this->Transform.Orientation; }
void Rotate(const Matrix3f m) {
this->m_T.rotate(m);
@@ -197,7 +257,7 @@ public:
{ this->m_T.rotate(Eigen::Quaternion<float>(q)); this->UpdatePropertiesFromMatrix(); }
void EulerYZYRotate(const Vector3f e) {
this->Transform.Orientation = e;
this->Transform.rotation = e;
this->Sync();
}
@@ -213,30 +273,12 @@ public:
* @brief Decomposes the internal matrix m_T back into Position, Orientation, and Scale properties.
*/
void UpdatePropertiesFromMatrix() {
// 1. Position
Transform.Position = m_T.translation();
this->Transform.FromMatrix(this->GetMatrix());
// 2. Scale
Matrix3f linear = m_T.linear();
Transform.Scale(0) = linear.col(0).norm();
Transform.Scale(1) = linear.col(1).norm();
Transform.Scale(2) = linear.col(2).norm();
// 3. Rotation (Normalization removes scale)
Matrix3f rotation = linear;
if (Transform.Scale(0) > 1e-6) rotation.col(0) /= Transform.Scale(0);
if (Transform.Scale(1) > 1e-6) rotation.col(1) /= Transform.Scale(1);
if (Transform.Scale(2) > 1e-6) rotation.col(2) /= Transform.Scale(2);
// Euler YZY (indices 1, 2, 1)
Vector3f euler = rotation.eulerAngles(1, 2, 1);
Transform.Orientation = euler / CLHEP::degree;
// Notify properties
PropertyBase* p;
if ((p = this->GetProperty("Transform.Position"))) p->Updated();
if ((p = this->GetProperty("Transform.Orientation"))) p->Updated();
if ((p = this->GetProperty("Transform.Scale"))) p->Updated();
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:
@@ -247,22 +289,19 @@ signals:
}
private:
/** Synchronizes m_T with properties */
void Sync() {
m_T = Eigen::Affine3f::Identity();
m_T.translate(Transform.Position);
// Orientation (using YZY order as implied by EulerYZYRotate)
Matrix3f mat;
mat = Eigen::AngleAxisf(Transform.Orientation.x() * CLHEP::degree, Vector3f::UnitY())
* Eigen::AngleAxisf(Transform.Orientation.y() * CLHEP::degree, Vector3f::UnitZ())
* Eigen::AngleAxisf(Transform.Orientation.z() * CLHEP::degree, Vector3f::UnitY());
m_T.rotate(mat);
m_T.scale(Transform.Scale);
}
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();
}
}