/*////////////////////////////////////////////////////////////////////////////// // 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 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(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