246 lines
7.3 KiB
C++
246 lines
7.3 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.
|
|
|
|
//////////////////////////////////////////////////////////////////////////////*/
|
|
|
|
|
|
|
|
#ifndef U_GEOMETRY_H
|
|
#define U_GEOMETRY_H
|
|
|
|
#include "Core/Object.h"
|
|
#include "Math/Dense.h"
|
|
#include "Math/Transform.h"
|
|
#include <cmath>
|
|
|
|
namespace uLib {
|
|
|
|
|
|
class Geometry : virtual public Object {
|
|
protected:
|
|
Geometry* m_Parent = nullptr;
|
|
|
|
public:
|
|
uLibTypeMacro(Geometry, Object)
|
|
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
virtual Vector3f FromLinear(const Vector3f& cartesian_space) const {
|
|
return cartesian_space;
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
virtual Vector4f GetLocalPoint(const float x, const float y, const float z) const {
|
|
return GetLocalPoint(Vector4f(x,y,z,1));
|
|
}
|
|
|
|
virtual Vector4f GetWorldPoint(const Vector3f v) const {
|
|
return GetWorldPoint(Vector4f(v.x(), v.y(), v.z(), 1.0f));
|
|
}
|
|
|
|
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 LinearGeometry : public Geometry {
|
|
protected:
|
|
Affine3f m_T = Affine3f::Identity();
|
|
|
|
public:
|
|
uLibTypeMacro(LinearGeometry, Geometry)
|
|
|
|
|
|
|
|
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() {}
|
|
|
|
|
|
|
|
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 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());
|
|
}
|
|
|
|
};
|
|
|
|
class SphericalGeometry : public LinearGeometry {
|
|
public:
|
|
uLibTypeMacro(SphericalGeometry, LinearGeometry)
|
|
SphericalGeometry() {}
|
|
|
|
|
|
|
|
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();
|
|
return Vector3f(r * std::sin(theta) * std::cos(phi),
|
|
r * std::sin(theta) * std::sin(phi),
|
|
r * std::cos(theta));
|
|
}
|
|
|
|
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());
|
|
return Vector3f(r, theta, phi);
|
|
}
|
|
|
|
};
|
|
|
|
class ToroidalGeometry : public LinearGeometry {
|
|
public:
|
|
uLibTypeMacro(ToroidalGeometry, LinearGeometry)
|
|
ToroidalGeometry(float Rtor) : m_Rtor(Rtor) {}
|
|
|
|
|
|
|
|
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();
|
|
return Vector3f((m_Rtor + r * std::cos(theta)) * std::cos(phi),
|
|
(m_Rtor + r * std::cos(theta)) * std::sin(phi),
|
|
r * std::sin(theta));
|
|
}
|
|
|
|
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;
|
|
float z = linear.z();
|
|
float r = std::sqrt(delta_r * delta_r + z * z);
|
|
float theta = std::atan2(z, delta_r);
|
|
return Vector3f(r, theta, phi);
|
|
}
|
|
|
|
private:
|
|
float m_Rtor;
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // GEOMETRY_H
|