diff --git a/bindings/Mandos/python/Deformable3D.cpp b/bindings/Mandos/python/Deformable3D.cpp index ed4a3fbc2298aaf525fddeb1cc8480720046c08e..a0c9a8ae75dfc73b4479c084f13cac06af4fc090 100644 --- a/bindings/Mandos/python/Deformable3D.cpp +++ b/bindings/Mandos/python/Deformable3D.cpp @@ -11,6 +11,8 @@ #include #include +#include + void mandos::py::wrapDeformable3D(::py::module_ &m) { ::py::class_(m, "StableNeoHookean") // @@ -23,6 +25,11 @@ void mandos::py::wrapDeformable3D(::py::module_ &m) ::py::class_(m, "MassSpring") // .def("add_element", &MassSpring::addElement); + ::py::class_(m, "ConstantForce") // + .def("add_element", &ConstantForce::addElement) + .def("set_force_vector", &ConstantForce::setForceVector) + .def("get_force_vector", &ConstantForce::getForceVector); + // NOLINTNEXTLINE (bugprone-unused-raii) ::py::class_>( m, "SimulationCollider"); @@ -34,6 +41,7 @@ void mandos::py::wrapDeformable3D(::py::module_ &m) .def_property_readonly("f", &Deformable3D::f) .def_property_readonly("snh", &Deformable3D::snh) .def_property_readonly("mass_spring", &Deformable3D::massSpring) + .def_property_readonly("constant_force", &Deformable3D::constantForce) .def_property("particle_mass", &Deformable3D::vertexMass, &Deformable3D::setVertexMass) .def( "add_sphere_cloud", @@ -87,6 +95,11 @@ mandos::py::MassSpring mandos::py::Deformable3D::massSpring() return {simObject().potential(), simObject().mstate}; } +mandos::py::ConstantForce mandos::py::Deformable3D::constantForce() +{ + return {simObject().potential(), simObject().mstate}; +} + const std::vector &mandos::py::Deformable3D::vertexMass() const { return simObject().potential().vertexMass(); @@ -182,3 +195,25 @@ mandos::core::SimulationObjectHandle mandos::py::De { return m_handle; } + +void mandos::py::ConstantForce::addElement(const int index, mandos::core::Vec3 const &forceVector) +{ + m_constantForce.addElement(index, forceVector); +} + +mandos::core::Vec3 mandos::py::ConstantForce::getForceVector(int index) +{ + return m_constantForce.getForceVector(index); +} + +void mandos::py::ConstantForce::setForceVector(int index, mandos::core::Vec3 &newForceVector) +{ + m_constantForce.setForceVector(index, newForceVector); +} + +mandos::py::ConstantForce::ConstantForce(mandos::core::ConstantForce &constantForce, + mandos::core::MechanicalState &mstate) + : m_constantForce(constantForce) + , m_mstate(mstate) +{ +} \ No newline at end of file diff --git a/bindings/Mandos/python/Deformable3D.hpp b/bindings/Mandos/python/Deformable3D.hpp index 7fda32301c3cfa302c05ae3e998778d4f0e6bea1..b71e3f84366a7dbd0cf19519d5d75e63e97e2c0c 100644 --- a/bindings/Mandos/python/Deformable3D.hpp +++ b/bindings/Mandos/python/Deformable3D.hpp @@ -4,12 +4,14 @@ #include #include +#include #include #include #include #include #include + namespace py = pybind11; namespace mandos::py @@ -34,6 +36,18 @@ struct MassSpring { mandos::core::MechanicalState &m_mstate; }; +struct ConstantForce { + ConstantForce(mandos::core::ConstantForce &constantForce, + mandos::core::MechanicalState &mstate); + + void addElement(const int index, mandos::core::Vec3 const &forceVector); + void setForceVector(int index, mandos::core::Vec3 &newForceVector); + mandos::core::Vec3 getForceVector(int index); + + mandos::core::ConstantForce &m_constantForce; + mandos::core::MechanicalState &m_mstate; +}; + struct Deformable3D { explicit Deformable3D(core::SimulationObjectHandle handle); core::SimulationObject &simObject(); @@ -65,6 +79,8 @@ struct Deformable3D { MassSpring massSpring(); + ConstantForce constantForce(); + private: core::SimulationObjectHandle m_handle; }; diff --git a/examples/python/ConstantForce/constantForce.py b/examples/python/ConstantForce/constantForce.py new file mode 100644 index 0000000000000000000000000000000000000000..8ee2c1bbbb5c148d5aefd49382e673a504c59d22 --- /dev/null +++ b/examples/python/ConstantForce/constantForce.py @@ -0,0 +1,50 @@ +import sys +import numpy as np +import polyscope as ps +import pymandos + +def create_model(): + #create the simulation scene + model = pymandos.Model() + #Create two particles, in the second of them, we will apply a force + particles = model.add_deformable_3d() + particles.size = 2 + # set their initial position and velocity + particles.x = np.array([[0,0,0], [0,0,1]]) + particles.v = np.array([[0,0,0], [0,0,0]]) + # set their mass + particles.particle_mass = [0.1,0.1] + + # fix the first one (zero index) + particles.fix_particle(0) + + # create a force instance and apply a force in the second particle + cforce = particles.constant_force + cforce.add_element(1,[0.0,1.0,1.0]) + + model.compute_dag() + + ps.register_point_cloud("particles",particles.x, radius=0.1) + return model, particles + +def simulate_callback(model, particles): + step_parameters = pymandos.StepParameters() + step_parameters.h = 0.001 + step_parameters.newton_iterations =20 + step_parameters.cg_iterations = 100 + step_parameters.cg_error = 1e-8 + step_parameters.grad_norm = 1e-1 + step_parameters.line_search_iterations = 0 + result = pymandos.step(model, step_parameters) + print(result) + ps.get_point_cloud("particles").update_point_positions(particles.x) + + + +if __name__ == "__main__": + ps.init() + ps.set_up_dir("z_up") + model, particles = create_model() + ps.set_user_callback(lambda : simulate_callback(model, particles)) + ps.show() + diff --git a/src/Mandos/Core/CMakeLists.txt b/src/Mandos/Core/CMakeLists.txt index 96bdd4e5ff251fef335141ddb367d0e6eb62ef31..8d247fd51ed41f9528de696094c04a46992dc668 100644 --- a/src/Mandos/Core/CMakeLists.txt +++ b/src/Mandos/Core/CMakeLists.txt @@ -18,6 +18,7 @@ set(HEADERS Energies/StableNeoHookean.hpp Energies/LumpedMassInertia.hpp Energies/GravityEnergy.hpp + Energies/ConstantForce.hpp Energies/RigidBodyInertia.hpp Energies/MassSpring.hpp Energies/CollisionSpring.hpp @@ -66,6 +67,7 @@ set(SOURCES Energies/StableNeoHookean.cpp Energies/LumpedMassInertia.cpp Energies/GravityEnergy.cpp + Energies/ConstantForce.cpp Energies/RigidBodyInertia.cpp Energies/MassSpring.cpp Energies/CosseratRodEnergy.cpp diff --git a/src/Mandos/Core/Energies.hpp b/src/Mandos/Core/Energies.hpp index 702f2937c5e577fa1fb8928964a120f819e64b1a..7536da3a0f88f9eba0b8365106166b9f21e4664d 100644 --- a/src/Mandos/Core/Energies.hpp +++ b/src/Mandos/Core/Energies.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ struct Inertias { template <> struct Potentials { std::tuple diff --git a/src/Mandos/Core/Energies/ConstantForce.cpp b/src/Mandos/Core/Energies/ConstantForce.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e2af3dec7e671ff1f2916e6cf5df99c539524e2e --- /dev/null +++ b/src/Mandos/Core/Energies/ConstantForce.cpp @@ -0,0 +1,63 @@ +#include +#include + +namespace mandos::core +{ + +int ConstantForce::size() const +{ + return static_cast(m_indices.size()); +} + +Scalar ConstantForce::computeEnergy(const MechanicalState &mstate) const +{ + auto energy{Scalar{0}}; + for (auto i = 0UL; i < static_cast(size()); ++i) { + energy -= (mstate.m_x[static_cast(m_indices[i])].dot(m_forceVector[i])); + } + return energy; +} + +Scalar ConstantForce::computeEnergyAndGradient(MechanicalState &mstate) const +{ + auto energy{Scalar{0}}; + for (auto i = 0UL; i < static_cast(size()); ++i) { + energy -= (mstate.m_x[static_cast(m_indices[i])].dot(m_forceVector[i])); + mstate.m_grad[static_cast(m_indices[i])] -= m_forceVector[i]; + } + return energy; +} + +Scalar ConstantForce::computeEnergyGradientAndHessian(MechanicalState &mstate) const +{ + return ConstantForce::computeEnergyAndGradient(mstate); +} + +void ConstantForce::addElement(int index, Vec3 const &forceVector) +{ + // Store parameters + m_indices.push_back(index); + m_forceVector.push_back(forceVector); +} + +void ConstantForce::setForceVector(int index, Vec3 &newForceVector) +{ + for (auto i = 0UL; i < static_cast(size()); ++i) { + if (m_indices[i] == index) { + m_forceVector[i] = newForceVector; + } + } +} + +Vec3 ConstantForce::getForceVector(int index) +{ + for (auto i = 0UL; i < static_cast(size()); ++i) { + if (m_indices[i] == index) { + return m_forceVector[i]; + } + } + throw std::invalid_argument("There is no forceVector for this index"); + return Vec3::Zero(); +} + +} // namespace mandos::core diff --git a/src/Mandos/Core/Energies/ConstantForce.hpp b/src/Mandos/Core/Energies/ConstantForce.hpp new file mode 100644 index 0000000000000000000000000000000000000000..680098ae8c9c5a4c5c093076c428cd2996f54596 --- /dev/null +++ b/src/Mandos/Core/Energies/ConstantForce.hpp @@ -0,0 +1,79 @@ +#ifndef MANDOS_ENERGIES_CONSTANT_FORCE +#define MANDOS_ENERGIES_CONSTANT_FORCE + +#include + +#include + +namespace mandos::core +{ +class ConstantForce +{ +public: + /** + * @brief Number of energy elements + * + * @return int + */ + int size() const; + + /** + * @brief Computes the total potential energy for the constant force elements given the current state of the + * MechanicalState + * + * @param mstate Current state + * @return Scalar Sum of the potential energy of all the constant force elements + */ + Scalar computeEnergy(const MechanicalState &mstate) const; + + /** + * @brief Computes the potential energy and gradient for the constant force elements given the current state of + * the MechanicalState + * + * @param mstate Current state, and where to write the gradient + * @return Scalar Sum of the potential energy of all the constant force elements + */ + Scalar computeEnergyAndGradient(MechanicalState &mstate) const; + + /** + * @brief Computes the potential energy, the gradient and the hessian for the constant force elements given the + * current state of the MechanicalState + * + * @param mstate Current state, and where to write the gradient and hessian + * @return Scalar Sum of the potential energy of all the constant force elements + */ + Scalar computeEnergyGradientAndHessian(MechanicalState &mstate) const; + + /** + * @brief Add a new constant force element. + * + * @param index Index of the particle to apply the constant force + * @param forceVector force vector to be applied to the particle (Vector of 3, x,y,z) + */ + MANDOS_CORE_EXPORT void addElement(int index, Vec3 const &forceVector); + + /** + * @brief Set the force vector of an constant force element. + * For example change the force's value during the simulation. + * + * @param index Index of the particle to apply the constant force + * @param newforceVector new force vector to be applied to the particle (Vector of 3, x,y,z) + */ + MANDOS_CORE_EXPORT void setForceVector(int index, Vec3 &newForceVector); + + /** + * @brief get the force vector of an constant force element. + * + * @param index Index of the particle to apply the constant force + * @param forceVector force vector to return of the particle (Vector of 3, x,y,z) + */ + MANDOS_CORE_EXPORT Vec3 getForceVector(int index); + +private: + std::vector m_indices; + std::vector m_forceVector; +}; + +} // namespace mandos::core + +#endif // MANDOS_ENERGIES_CONSTANT_FORCE \ No newline at end of file