diff --git a/bindings/Mandos/python/CMakeLists.txt b/bindings/Mandos/python/CMakeLists.txt index 28e92b3ead2cb5055fd1e9d92cd61e6171e388c4..84d5923bf27c73c72f9aff133bb1e45aac71d71c 100644 --- a/bindings/Mandos/python/CMakeLists.txt +++ b/bindings/Mandos/python/CMakeLists.txt @@ -11,10 +11,12 @@ set(SOURCES Differentiable.cpp Energies/StableNeoHookean.cpp Energies/ARAP.cpp + Energies/AirDrag.cpp Energies/MassSpring.cpp Energies/CosseratBendingRod.cpp Energies/CosseratRodAlignment.cpp Energies/ConstantForce.cpp + Energies/PlaneField.cpp ) set(HEADERS @@ -26,11 +28,14 @@ set(HEADERS IdentityMapping.hpp Energies/StableNeoHookean.hpp Energies/ARAP.hpp + Energies/AirDrag.hpp Energies/MassSpring.hpp Energies/CosseratBendingRod.hpp Energies/CosseratRodAlignment.hpp Energies/ConstantForce.hpp Differentiable.hpp + Energies/PlaneField.hpp + Differentiable.hpp ) nanobind_add_module(mandos_pylib STABLE_ABI ${SOURCES} ${HEADERS}) diff --git a/bindings/Mandos/python/Deformable3D.cpp b/bindings/Mandos/python/Deformable3D.cpp index 820086fbdf5675dfaae762896e9466a68288f469..bc2b2d44f45c35a6353ba6865f8e0134d4b0ad13 100644 --- a/bindings/Mandos/python/Deformable3D.cpp +++ b/bindings/Mandos/python/Deformable3D.cpp @@ -27,64 +27,89 @@ void mandos::py::wrapDeformable3D(nb::module_ &m) nb::class_>( m, "SimulationCollider"); - nb::class_(m, "Deformable3D") - .def_prop_rw("size", &Deformable3D::size, &Deformable3D::resize) - .def_prop_rw("x", &Deformable3D::x, &Deformable3D::setX) - .def_prop_rw("v", &Deformable3D::v, &Deformable3D::setV) - .def_prop_ro("f", &Deformable3D::f) - .def_prop_ro("snh", &Deformable3D::snh) - .def_prop_ro("arap", &Deformable3D::arap) - .def_prop_ro("mass_spring", &Deformable3D::massSpring) - .def_prop_ro("constant_force", &Deformable3D::constantForce) - .def_prop_rw("particle_mass", &Deformable3D::vertexMass, &Deformable3D::setVertexMass) - .def("disable_gravity", - [](Deformable3D &self) { self.simObject().potential().disable(); }) - .def( - "add_sphere_cloud", - [](Deformable3D &def3d, mandos::core::Scalar radius) { - auto &scs = std::get>(def3d.simObject().colliders()); - auto &sc = scs.emplace_back(radius); - sc.update(def3d.simObject().mstate); - return mandos::core::collisions::SimulationCollider{ - def3d.handle(), static_cast(scs.size() - 1)}; - }) - .def( - "add_collision_mesh", - [](Deformable3D &def3d, mandos::core::SurfaceMesh &surfaceMesh, mandos::core::Scalar dhat) { - auto &cms = std::get>(def3d.simObject().colliders()); - auto &cm = cms.emplace_back(surfaceMesh, dhat); - cm.update(def3d.simObject().mstate); - return mandos::core::collisions::SimulationCollider{ - def3d.handle(), static_cast(cms.size() - 1)}; - }, - nb::kw_only(), - nb::arg("mesh"), - nb::arg("dhat")) - .def( - "add_collision_mesh", - [](Deformable3D &def3d, - const std::vector> &edges, - const std::vector> &triangles, - mandos::core::Scalar dhat) { - auto &cms = std::get>(def3d.simObject().colliders()); - auto &cm = cms.emplace_back(def3d.simObject().mstate.m_x, edges, triangles, dhat); - cm.update(def3d.simObject().mstate); - return mandos::core::collisions::SimulationCollider{ - def3d.handle(), static_cast(cms.size() - 1)}; - }, - nb::kw_only(), - nb::arg("edges") = std::vector>{}, - nb::arg("triangles") = std::vector>{}, - nb::arg("dhat")) - .def("fix_particle", - &Deformable3D::fixParticle, - nb::arg("index"), - nb::arg("x") = true, - nb::arg("y") = true, - nb::arg("z") = true) - .def("fixed_dof_vector", &Deformable3D::getFixedDofVector); + auto deformable3d = + nb::class_(m, "Deformable3D") + .def_prop_rw("size", &Deformable3D::size, &Deformable3D::resize) + .def_prop_rw("x", &Deformable3D::x, &Deformable3D::setX) + .def_prop_rw("v", &Deformable3D::v, &Deformable3D::setV) + .def_prop_ro("f", &Deformable3D::f, nb::rv_policy::move) + .def_prop_ro("snh", &Deformable3D::snh) + .def_prop_ro("arap", &Deformable3D::arap) + .def_prop_ro("air_drag", &Deformable3D::airDrag) + .def_prop_ro("mass_spring", &Deformable3D::massSpring) + .def_prop_ro("constant_force", &Deformable3D::constantForce) + .def_prop_rw("particle_mass", &Deformable3D::vertexMass, &Deformable3D::setVertexMass) + .def("disable_gravity", + [](Deformable3D &self) { self.simObject().potential().disable(); }) + .def("add_sphere_cloud", + [](Deformable3D &def3d, mandos::core::Scalar radius) { + auto &scs = std::get>(def3d.simObject().colliders()); + auto &sc = scs.emplace_back(radius); + sc.update(def3d.simObject().mstate); + return mandos::core::collisions::SimulationCollider{ + def3d.handle(), static_cast(scs.size() - 1)}; + }) + .def( + "add_collision_mesh", + [](Deformable3D &def3d, mandos::core::SurfaceMesh &surfaceMesh, mandos::core::Scalar dhat) { + auto &cms = std::get>(def3d.simObject().colliders()); + auto &cm = cms.emplace_back(surfaceMesh, dhat); + cm.update(def3d.simObject().mstate); + return mandos::core::collisions::SimulationCollider{ + def3d.handle(), static_cast(cms.size() - 1)}; + }, + nb::kw_only(), + nb::arg("mesh"), + nb::arg("dhat")) + .def( + "add_collision_mesh", + [](Deformable3D &def3d, + const std::vector> &edges, + const std::vector> &triangles, + mandos::core::Scalar dhat) { + auto &cms = std::get>(def3d.simObject().colliders()); + auto &cm = cms.emplace_back(def3d.simObject().mstate.m_x, edges, triangles, dhat); + cm.update(def3d.simObject().mstate); + return mandos::core::collisions::SimulationCollider{ + def3d.handle(), static_cast(cms.size() - 1)}; + }, + nb::kw_only(), + nb::arg("edges") = std::vector>{}, + nb::arg("triangles") = std::vector>{}, + nb::arg("dhat")) + .def("fix_particle", + &Deformable3D::fixParticle, + nb::arg("index"), + nb::arg("x") = true, + nb::arg("y") = true, + nb::arg("z") = true) + .def("fixed_dof_vector", &Deformable3D::getFixedDofVector); + + // Differential parameters + //-------------------------- + // Mass Spring + deformable3d.def("diff", [](const Deformable3D &self, mandos::core::MassSpring::Parameters parameter) { + switch (parameter) { + case mandos::core::MassSpring::Stiffness: { + mandos::core::DiffParameterHandle + paramHandle(self.handle()); + return mandos::core::DiffParameterHandleV(paramHandle); + } + case mandos::core::MassSpring::RestLength: { + mandos::core::DiffParameterHandle + paramHandle(self.handle()); + return mandos::core::DiffParameterHandleV(paramHandle); + } + } + throw std::invalid_argument("Parameter not implemented!"); + }); } mandos::core::SimulationObject &mandos::py::Deformable3D::simObject() @@ -107,6 +132,11 @@ mandos::core::ARAP &mandos::py::Deformable3D::arap() return simObject().potential(); } +mandos::core::AirDrag &mandos::py::Deformable3D::airDrag() +{ + return simObject().dissipativePotential(); +} + mandos::core::MassSpring &mandos::py::Deformable3D::massSpring() { return simObject().potential(); @@ -156,11 +186,7 @@ void mandos::py::Deformable3D::setX(const Eigen::Matrix(nParticles)); - simObject().mstate.m_v.resize(static_cast(nParticles)); - simObject().mstate.m_grad.resize(static_cast(nParticles)); - simObject().mstate.m_hessian.resize(3 * static_cast(nParticles), - 3 * static_cast(nParticles)); + simObject().mstate.resize(nParticles); } int mandos::py::Deformable3D::size() const @@ -174,10 +200,10 @@ void mandos::py::Deformable3D::fixParticle(int index, bool x, bool y, bool z) getFixedDofVector().push_back(3 * index); } if (y) { - getFixedDofVector().push_back(3 * index + 1); + getFixedDofVector().push_back((3 * index) + 1); } if (z) { - getFixedDofVector().push_back(3 * index + 2); + getFixedDofVector().push_back((3 * index) + 2); } } diff --git a/bindings/Mandos/python/Deformable3D.hpp b/bindings/Mandos/python/Deformable3D.hpp index b1f02746c528e524994335160e6eea930a2c751e..dcf3c86150b733fcf88c2ef96bd356c10a55e11d 100644 --- a/bindings/Mandos/python/Deformable3D.hpp +++ b/bindings/Mandos/python/Deformable3D.hpp @@ -49,6 +49,8 @@ struct Deformable3D { mandos::core::ARAP &arap(); + mandos::core::AirDrag &airDrag(); + mandos::core::MassSpring &massSpring(); mandos::core::ConstantForce &constantForce(); diff --git a/bindings/Mandos/python/Energies/AirDrag.cpp b/bindings/Mandos/python/Energies/AirDrag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b33f4139f01c66f2a54975f5d510514083fc7087 --- /dev/null +++ b/bindings/Mandos/python/Energies/AirDrag.cpp @@ -0,0 +1,15 @@ +#include +#include + +#include + +#include +#include + +void mandos::py::energies::wrapAirDrag(nb::module_ &m) +{ + auto airDrag = nb::class_(m, "AirDrag"); + + airDrag.def("add_element", &mandos::core::AirDrag::addElement); + airDrag.def("size", &mandos::core::AirDrag::size); +} diff --git a/bindings/Mandos/python/Energies/AirDrag.hpp b/bindings/Mandos/python/Energies/AirDrag.hpp new file mode 100644 index 0000000000000000000000000000000000000000..19a131b105b7579188428d077911c34980c76eee --- /dev/null +++ b/bindings/Mandos/python/Energies/AirDrag.hpp @@ -0,0 +1,13 @@ +#ifndef MANDOS_PY_ENERGIES_AIRDRAG_HPP +#define MANDOS_PY_ENERGIES_AIRDRAG_HPP + +#include + +namespace nb = nanobind; + +namespace mandos::py::energies +{ +void wrapAirDrag(nb::module_ &m); +} + +#endif // MANDOS_PY_ENERGIES_AIRDRAG_HPP diff --git a/bindings/Mandos/python/Energies/PlaneField.cpp b/bindings/Mandos/python/Energies/PlaneField.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6678a6f19e6ed46d9c5b4eea44884975ae37543b --- /dev/null +++ b/bindings/Mandos/python/Energies/PlaneField.cpp @@ -0,0 +1,50 @@ +#include + +#include + +#include +#include + +#include +#include +#include + +void mandos::py::energies::wrapPlaneField(nb::module_ &m) +{ + auto planeField = nb::class_(m, "PlaneField"); + nb::class_(planeField, "ParameterSet") // + .def(nb::init<>()) // + .def(nb::init(), // + nb::kw_only(), + nb::arg("p"), + nb::arg("normal"), + nb::arg("stiffness"), + nb::arg("coulomb"), + nb::arg("surface")) + .def_rw("point", &core::PlaneField::ParameterSet::point) + .def_rw("normal", &core::PlaneField::ParameterSet::normal) + .def_rw("stiffness", &core::PlaneField::ParameterSet::stiffness) + .def_rw("coulomb", &core::PlaneField::ParameterSet::coulombConst) + .def_rw("mesh", &core::PlaneField::ParameterSet::mesh); + + planeField + .def("set_parameter_set", + [](mandos::core::PlaneField &self, const mandos::core::PlaneField::ParameterSet ¶meterSet) { + self.setParameterSet(parameterSet); + }) + .def("get_parameter_set", [](const mandos::core::PlaneField &self) { return self.getParameterSet(); }) + .def("get_world_positions", &core::PlaneField::getWorldPositions) + .def("get_world_velocitioes", &core::PlaneField::getWorldVelocities) + .def("compute_friction_forces", + [](core::PlaneField &self, RigidBody3D &rb, core::Scalar h) { + return self.computeFrictionForces(rb.simObject().mstate, h); + }) + .def("compute_friction_rb_forces", + [](core::PlaneField &self, RigidBody3D &rb, core::Scalar h) { + return self.computeFrictionRBForces(rb.simObject().mstate, h); + }); +} diff --git a/bindings/Mandos/python/Energies/PlaneField.hpp b/bindings/Mandos/python/Energies/PlaneField.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0d38ee53359419e1d5eb331cd8f0997d0ff943f0 --- /dev/null +++ b/bindings/Mandos/python/Energies/PlaneField.hpp @@ -0,0 +1,15 @@ +#ifndef MANDOS_PY_ENERGIES_PLANEFIELD_HPP +#define MANDOS_PY_ENERGIES_PLANEFIELD_HPP + +#include + +#include + +namespace nb = nanobind; + +namespace mandos::py::energies +{ +void wrapPlaneField(nb::module_ &m); +} + +#endif // MANDOS_PY_ENERGIES_MASSSPRING_HPP diff --git a/bindings/Mandos/python/Mandos.cpp b/bindings/Mandos/python/Mandos.cpp index 702cd916846bf3388c08b49a41c5fffd533ff010..b88d157aea59cb3b920e8f127246bfbf312b2fef 100644 --- a/bindings/Mandos/python/Mandos.cpp +++ b/bindings/Mandos/python/Mandos.cpp @@ -10,10 +10,12 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -36,7 +38,9 @@ NB_MODULE(mandos_pylib, m) mandos::py::energies::wrapCosseratRodAlignment(energies); mandos::py::energies::wrapStableNeoHookean(energies); mandos::py::energies::wrapARAP(energies); + mandos::py::energies::wrapAirDrag(energies); mandos::py::energies::wrapConstantForce(energies); + mandos::py::energies::wrapPlaneField(energies); nb::class_(m, "StepParameters") .def(nb::init()) diff --git a/bindings/Mandos/python/Model.cpp b/bindings/Mandos/python/Model.cpp index 7eb1f624ca4058b20c5447abfd8896ab39736056..29c21fe7bed00e99eb097ccb530cca50a9578bbf 100644 --- a/bindings/Mandos/python/Model.cpp +++ b/bindings/Mandos/python/Model.cpp @@ -107,6 +107,24 @@ void mandos::py::wrapModel(nb::module_ &m) return RigidBodyCloud3D{vd}; }, nb::arg("name") = std::optional{}) + .def( + "add_rigidbody_global_cloud", + [](Model &model, std::optional name = {}) { + if (name) { + if (model.m_rigidBodyGlobalCloudObjects.contains(name.value())) { + throw std::invalid_argument( + fmt::format("Model already contains a RigidBodyCloud object named {}", name.value()) + .c_str()); + } + } + + auto vd{model.add()}; + if (name) { + model.m_rigidBodyGlobalCloudObjects[name.value()] = vd; + } + return RigidBodyCloud3D{vd}; + }, + nb::arg("name") = std::optional{}) .def("get_deformable3d", [](Model &model, const std::string &name) -> std::optional { if (auto it = model.m_particle3DObjects.find(name); it != std::end(model.m_particle3DObjects)) { @@ -122,13 +140,21 @@ void mandos::py::wrapModel(nb::module_ &m) return {}; }) .def("get_rigidbody_cloud", - [](Model &model, const std::string &name) -> std::optional { + [](Model &model, const std::string &name) -> std::optional { if (auto it = model.m_rigidBodyCloudObjects.find(name); it != std::end(model.m_rigidBodyCloudObjects)) { return RigidBodyCloud3D{it->second}; } return {}; }) + .def("get_rigidbody_global_cloud", + [](Model &model, const std::string &name) -> std::optional { + if (auto it = model.m_rigidBodyGlobalCloudObjects.find(name); + it != std::end(model.m_rigidBodyGlobalCloudObjects)) { + return RigidBodyCloud3D{it->second}; + } + return {}; + }) .def("get_rigidbody_global", [](Model &model, const std::string &name) -> std::optional> { if (auto it = model.m_rigidBodyGlobalObjects.find(name); @@ -150,7 +176,7 @@ void mandos::py::wrapModel(nb::module_ &m) .def( "add_rigidbody_point_mapping", - [](Model &model, mandos::py::RigidBodyCloud3D &rigidBodyCloud, std::optional to_name) { + [](Model &model, mandos::py::RigidBodyCloud3DLocal &rigidBodyCloud, std::optional to_name) { if (to_name) { if (model.m_particle3DObjects.contains(to_name.value())) { throw std::invalid_argument( @@ -187,6 +213,26 @@ void mandos::py::wrapModel(nb::module_ &m) nb::arg(), nb::arg("to_name") = std::optional{}) + .def( + "add_rigidbody_global_point_mapping", + [](Model &model, mandos::py::RigidBodyCloud3DGlobal &rigidBodyCloud, std::optional to_name) { + if (to_name) { + if (model.m_particle3DObjects.contains(to_name.value())) { + throw std::invalid_argument( + fmt::format("Model already contains a Deformable3D named {}", to_name.value()).c_str()); + } + } + + auto mapping = RigidBodyGlobalPointMapping(rigidBodyCloud, model); + if (to_name) { + auto deformable = mapping.m_deformable; + model.m_particle3DObjects[to_name.value()] = deformable.handle(); + } + return mapping; + }, + nb::arg(), + nb::arg("to_name") = std::optional{}) + ///////////////// Collision pairs ////////////////////// .def( @@ -275,15 +321,13 @@ void mandos::py::wrapModel(nb::module_ &m) model.setState(x, v); }) .def("compute_forces", - [](Model &model, const mandos::core::Scalar h) { + [](Model &model, const mandos::core::Scalar h) -> core::Vec { auto nDof = model.nDof(); mandos::core::Vec g(nDof); g.setZero(); - // We need to compute advection to ensure inertial forces are correct - model.computeAdvection(h); - model.computeEnergyAndGradient(h, g); + return g; }) .def("detect_collisions", [](Model &model) { diff --git a/bindings/Mandos/python/Model.hpp b/bindings/Mandos/python/Model.hpp index 1bd869e0e1b92d883a28e526e629a5b545a45eab..a28d94fd6e5fb11f9008f1b3273efc964c773e5e 100644 --- a/bindings/Mandos/python/Model.hpp +++ b/bindings/Mandos/python/Model.hpp @@ -18,10 +18,12 @@ struct Model : public mandos::core::Model { std::unordered_map> m_rigidBodyObjects; std::unordered_map> m_rigidBodyGlobalObjects; std::unordered_map> m_rigidBodyCloudObjects; + std::unordered_map> + m_rigidBodyGlobalCloudObjects; std::unordered_map> m_collisionStateObjects; }; void wrapModel(nb::module_ &m); } // namespace mandos::py -#endif // MANDOS_PY_MODEL_HPP \ No newline at end of file +#endif // MANDOS_PY_MODEL_HPP diff --git a/bindings/Mandos/python/RigidBody.cpp b/bindings/Mandos/python/RigidBody.cpp index 25258bdacc65da3fd5a7d965466f28f78f623f0f..6efc144eb4ff8f9b76eeaa527f00182ecc3bea87 100644 --- a/bindings/Mandos/python/RigidBody.cpp +++ b/bindings/Mandos/python/RigidBody.cpp @@ -4,6 +4,7 @@ #include "Mandos/Core/Collisions/SimulationCollider.hpp" #include "Mandos/Core/Energies/CosseratRodAlignment.hpp" #include "Mandos/Core/Energies/MassSpring.hpp" +#include "Mandos/Core/Energies/PlaneField.hpp" #include "Mandos/Core/Mesh.hpp" #include "Mandos/Core/linear_algebra.hpp" #include @@ -20,12 +21,81 @@ NB_MAKE_OPAQUE(mandos::core::DiffParameterHandleV) +namespace +{ + +template +auto wrapRigidBodyCloud(nb::module_ &m, const char *name) +{ + using RBCloud = mandos::py::RigidBodyCloud3D; + return nb::class_(m, name) + .def_prop_rw("size", &RBCloud::size, &RBCloud::resize) + .def_prop_rw("x", &RBCloud::x, &RBCloud::setX) + .def_prop_rw("v", &RBCloud::v, &RBCloud::setV) + .def_prop_ro("grad", &RBCloud::grad) + .def_prop_ro("hessian", &RBCloud::hessian) + .def_prop_rw("mass", &RBCloud::mass, &RBCloud::setMass) + .def_prop_rw("inertiaTensor", &RBCloud::inertiaTensor, &RBCloud::setInertiaTensor) + .def("enable_gravity", + [](RBCloud &self) { self.simObject().template potential().enable(); }) + .def("disable_gravity", + [](RBCloud &self) { self.simObject().template potential().disable(); }) + .def_prop_ro("mass_spring", &RBCloud::massSpring) + .def_prop_ro("cosserat_bending_rod", &RBCloud::cosseratBendingRod) + .def_prop_ro("cosserat_rod_alignment", &RBCloud::cosseratRodAlignment) + .def_prop_ro("air_drag", &RBCloud::airDrag) + .def("get_transform", + [](const RBCloud &self, std::size_t index) { + mandos::core::Mat4 transform = mandos::core::Mat4::Zero(); + mandos::core::Vec6 x = self.simObject().mstate.m_x[index].template segment<6>(0); + transform.block<3, 3>(0, 0) = mandos::core::rotationExpMap(x.segment<3>(3)); // Rotation + transform.block<3, 1>(0, 3) = x.segment<3>(0); // Position + transform(3, 3) = 1.0; + return transform; + }) + .def("fix_translation", + &RBCloud::fixRigidBodyTranslation, + nb::arg("index"), + nb::arg("x") = true, + nb::arg("y") = true, + nb::arg("z") = true) + .def("fix_rotation", &RBCloud::fixRigidBodyRotation, nb::arg("index")) + .def("clear_fixing", &RBCloud::clearFixing) + .def("fixed_dof_vector", &RBCloud::getFixedDofVector) + .def( + "add_sdf", + [](RBCloud &self, + int rbIndex, + const mandos::core::SurfaceMesh &mesh, + mandos::core::Scalar contactOffset, + int nbVoxels, + bool flip = false) { + auto &sdfs = std::get>(self.simObject().colliders()); + auto &sdf = sdfs.emplace_back(mesh, contactOffset, rbIndex, nbVoxels, flip); + sdf.update(self.simObject().mstate); + return mandos::core::collisions::SimulationCollider( + self.handle(), static_cast(sdfs.size() - 1)); + }, + nb::arg(), + nb::arg("rigidbody_index"), + nb::arg("contactOffset"), + nb::arg("nb_voxels") = 256, + nb::arg("flip") = false); +} + +} // namespace + namespace mandos::py { void wrapRigidBody(nb::module_ &m) { nb::class_>( m, "SimulationCollider") + .def_prop_ro( + "surface_mesh", + [](const mandos::core::collisions::SimulationCollider &self) { + return self.collider().mesh(); + }) .def("distance", [](const mandos::core::collisions::SimulationCollider &collider, const mandos::core::Vec3 &v) { return collider.collider().vdb().distance(v); }); @@ -39,6 +109,10 @@ void wrapRigidBody(nb::module_ &m) .def_prop_rw("inertiaTensor", &RigidBody3D::inertiaTensor, &RigidBody3D::setInertiaTensor) + .def_prop_ro("plane_field", + [](RigidBody3D &rb) -> mandos::core::PlaneField & { + return rb.simObject().dissipativePotential(); + }) .def( "add_sdf", [](RigidBody3D &rb, @@ -47,7 +121,7 @@ void wrapRigidBody(nb::module_ &m) int nbVoxels, bool flip = false) { auto &sdfs = std::get>(rb.simObject().colliders()); - auto &sdf = sdfs.emplace_back(mesh, contactOffset, nbVoxels, flip); + auto &sdf = sdfs.emplace_back(mesh, contactOffset, 0, nbVoxels, flip); sdf.update(rb.simObject().mstate); return mandos::core::collisions::SimulationCollider( rb.handle(), static_cast(sdfs.size() - 1)); @@ -103,53 +177,34 @@ void wrapRigidBody(nb::module_ &m) return transform; }); - nb::class_(m, "RigidBodyCloud3D") - .def_prop_rw("size", &RigidBodyCloud3D::size, &RigidBodyCloud3D::resize) - .def_prop_rw("x", &RigidBodyCloud3D::x, &RigidBodyCloud3D::setX) - .def_prop_rw("v", &RigidBodyCloud3D::v, &RigidBodyCloud3D::setV) - .def_prop_ro("grad", &RigidBodyCloud3D::grad) - .def_prop_ro("hessian", &RigidBodyCloud3D::hessian) - .def_prop_rw("mass", &RigidBodyCloud3D::mass, &RigidBodyCloud3D::setMass) - .def_prop_rw("inertiaTensor", &RigidBodyCloud3D::inertiaTensor, &RigidBodyCloud3D::setInertiaTensor) - .def("enable_gravity", - [](RigidBodyCloud3D &self) { self.simObject().potential().enable(); }) - .def("disable_gravity", - [](RigidBodyCloud3D &self) { self.simObject().potential().disable(); }) - .def_prop_ro("mass_spring", &RigidBodyCloud3D::massSpring) - .def_prop_ro("cosserat_bending_rod", &RigidBodyCloud3D::cosseratBendingRod) - .def_prop_ro("cosserat_rod_alignment", &RigidBodyCloud3D::cosseratRodAlignment) - .def("get_transform", - [](const RigidBodyCloud3D &self, std::size_t index) { - core::Mat4 transform = core::Mat4::Zero(); - core::Vec6 x = self.simObject().mstate.m_x[index].segment<6>(0); - transform.block<3, 3>(0, 0) = core::rotationExpMap(x.segment<3>(3)); // Rotation - transform.block<3, 1>(0, 3) = x.segment<3>(0); // Position - transform(3, 3) = 1.0; - return transform; - }) - .def("fix_translation", - &RigidBodyCloud3D::fixRigidBodyTranslation, - nb::arg("index"), - nb::arg("x") = true, - nb::arg("y") = true, - nb::arg("z") = true) - .def("fix_rotation", &RigidBodyCloud3D::fixRigidBodyRotation, nb::arg("index")) - .def("clear_fixing", &RigidBodyCloud3D::clearFixing) - .def("fixed_dof_vector", &RigidBodyCloud3D::getFixedDofVector); + auto rigid_body_cloud = wrapRigidBodyCloud(m, "rigid_body_cloud"); + auto rigid_body_global_cloud = wrapRigidBodyCloud(m, "rigid_body_global_cloud"); + + rigid_body_cloud.def("diff", [](const RigidBodyCloud3DLocal &self, core::CosseratBendingRod::Parameters parameter) { + switch (parameter) { + case core::CosseratBendingRod::IntrinsicDarboux: { + mandos::core::DiffParameterHandle + paramHandle(self.handle()); + return mandos::core::DiffParameterHandleV(paramHandle); + } + } + throw std::invalid_argument("Parameter not implemented!"); + }); } template RigidBody3D::RigidBody3D(core::SimulationObjectHandle handle) : m_handle(handle) { - simObject().mstate.m_x.resize(1); - simObject().mstate.m_v.resize(1); - simObject().mstate.m_grad.resize(1); - simObject().mstate.m_hessian.resize(6, 6); + if (simObject().mstate.m_x.size() != 1) { + simObject().mstate.resize(1); - simObject().template inertia().mass().resize(1); - simObject().template inertia().inertiaTensor().resize(1); - simObject().template potential().vertexMass().resize(1); + simObject().template inertia().mass().resize(1); + simObject().template inertia().inertiaTensor().resize(1); + simObject().template potential().vertexMass().resize(1); + } } template @@ -170,19 +225,20 @@ mandos::core::SimulationObjectHandle RigidBody3D::handle() } template -mandos::core::Vec6 RigidBody3D::x() const +Eigen::Map RigidBody3D::x() const { - return simObject().mstate.m_x[0]; + return {simObject().mstate.m_x[0].data(), 6}; } + template void RigidBody3D::setX(const mandos::core::Vec6 &x) { simObject().mstate.m_x[0] = x; } template -mandos::core::Vec6 RigidBody3D::v() const +Eigen::Map RigidBody3D::v() const { - return simObject().mstate.m_v[0]; + return {simObject().mstate.m_v[0].data(), 6}; } template void RigidBody3D::setV(const mandos::core::Vec6 &v) @@ -190,14 +246,14 @@ void RigidBody3D::setV(const mandos::core::Vec6 &v) simObject().mstate.m_v[0] = v; } template -mandos::core::Vec6 RigidBody3D::grad() const +Eigen::Map RigidBody3D::grad() const { - return simObject().mstate.m_grad[0]; + return {simObject().mstate.m_grad[0].data(), 6}; } template -mandos::core::Mat6 RigidBody3D::hessian() const +Eigen::Map RigidBody3D::hessian() const { - return simObject().mstate.m_hessian.toDense(); + return {simObject().mstate.m_hessian.toDense().data(), 6, 6}; } template void RigidBody3D::setMass(core::Scalar mass) @@ -220,126 +276,167 @@ const mandos::core::Mat3 &RigidBody3D::inertiaTensor() const { return simObject().template inertia().inertiaTensor()[0]; } -RigidBodyCloud3D::RigidBodyCloud3D(core::SimulationObjectHandle handle) + +template +RigidBodyCloud3D::RigidBodyCloud3D(core::SimulationObjectHandle handle) : m_handle(handle) { } -mandos::core::SimulationObject &RigidBodyCloud3D::simObject() +template +mandos::core::SimulationObject &RigidBodyCloud3D::simObject() { return m_handle.simulationObject(); } -const mandos::core::SimulationObject &RigidBodyCloud3D::simObject() const +template +const mandos::core::SimulationObject &RigidBodyCloud3D::simObject() const { return m_handle.simulationObject(); } -void RigidBodyCloud3D::resize(int n) +template +void RigidBodyCloud3D::resize(int n) { - simObject().mstate.m_x.resize(static_cast(n)); - simObject().mstate.m_v.resize(static_cast(n)); - simObject().mstate.m_grad.resize(static_cast(n)); - simObject().mstate.m_hessian.resize(6 * static_cast(n), // - 6 * static_cast(n)); + simObject().mstate.resize(n); } -int RigidBodyCloud3D::size() const +template +int RigidBodyCloud3D::size() const { return static_cast(simObject().mstate.m_x.size()); } -Eigen::Map> RigidBodyCloud3D::x() const +template +Eigen::Map> RigidBodyCloud3D::x() + const { return {simObject().mstate.m_x.data()->data(), static_cast(simObject().mstate.m_x.size()), 6}; } -void RigidBodyCloud3D::setX(const Eigen::Matrix &x) +template +void RigidBodyCloud3D::setX(const Eigen::Matrix &x) { Eigen::Matrix::MapType( simObject().mstate.m_x.data()->data(), static_cast(simObject().mstate.m_x.size()), 6) = x; } -Eigen::Map> RigidBodyCloud3D::v() const +template +Eigen::Map> RigidBodyCloud3D::v() + const { return {simObject().mstate.m_v.data()->data(), // static_cast(simObject().mstate.m_v.size()), 6}; } -void RigidBodyCloud3D::setV(const Eigen::Matrix &v) +template +void RigidBodyCloud3D::setV(const Eigen::Matrix &v) { Eigen::Matrix::MapType( simObject().mstate.m_v.data()->data(), static_cast(simObject().mstate.m_v.size()), 6) = v; } -void RigidBodyCloud3D::setMass(std::vector mass) +template +void RigidBodyCloud3D::setMass(const std::vector &mass) { simObject().template inertia().mass() = mass; - simObject().template potential().vertexMass() = std::move(mass); + simObject().template potential().vertexMass() = mass; } -const std::vector &RigidBodyCloud3D::mass() +template +const std::vector &RigidBodyCloud3D::mass() { return simObject().template inertia().mass(); } -void RigidBodyCloud3D::setInertiaTensor(std::vector inertiaTensor) +template +void RigidBodyCloud3D::setInertiaTensor(const std::vector &inertiaTensor) { - simObject().template inertia().inertiaTensor() = std::move(inertiaTensor); + simObject().template inertia().inertiaTensor() = inertiaTensor; } -const std::vector &RigidBodyCloud3D::inertiaTensor() const +template +const std::vector &RigidBodyCloud3D::inertiaTensor() const { return simObject().template inertia().inertiaTensor(); } -Eigen::Map> RigidBodyCloud3D::grad() const +template +Eigen::Map> +RigidBodyCloud3D::grad() const { return {simObject().mstate.m_grad.data()->data(), // static_cast(simObject().mstate.m_grad.size()), 6}; } -const mandos::core::SparseMat &RigidBodyCloud3D::hessian() const +template +const mandos::core::SparseMat &RigidBodyCloud3D::hessian() const { return simObject().mstate.m_hessian; } -mandos::core::MassSpring &RigidBodyCloud3D::massSpring() +template +mandos::core::MassSpring &RigidBodyCloud3D::massSpring() { return simObject().template potential(); } -mandos::core::CosseratBendingRod &RigidBodyCloud3D::cosseratBendingRod() +template +mandos::core::CosseratBendingRod &RigidBodyCloud3D::cosseratBendingRod() { return simObject().template potential(); } -mandos::core::CosseratRodAlignment &RigidBodyCloud3D::cosseratRodAlignment() +template +mandos::core::CosseratRodAlignment &RigidBodyCloud3D::cosseratRodAlignment() { return simObject().template potential(); } -void RigidBodyCloud3D::fixRigidBodyTranslation(int index, bool x, bool y, bool z) +template <> +mandos::core::AirDrag &RigidBodyCloud3D::airDrag() +{ + // TODO + return *new core::AirDrag(); +} + +template <> +mandos::core::AirDrag &RigidBodyCloud3D::airDrag() +{ + return simObject().template dissipativePotential(); +} + +template +void RigidBodyCloud3D::fixRigidBodyTranslation(int index, bool x, bool y, bool z) { if (x) { getFixedDofVector().push_back(6 * index); } if (y) { - getFixedDofVector().push_back(6 * index + 1); + getFixedDofVector().push_back((6 * index) + 1); } if (z) { - getFixedDofVector().push_back(6 * index + 2); + getFixedDofVector().push_back((6 * index) + 2); } } -void RigidBodyCloud3D::fixRigidBodyRotation(int index) +template +void RigidBodyCloud3D::fixRigidBodyRotation(int index) { - getFixedDofVector().push_back(6 * index + 3); - getFixedDofVector().push_back(6 * index + 4); - getFixedDofVector().push_back(6 * index + 5); + getFixedDofVector().push_back((6 * index) + 3); + getFixedDofVector().push_back((6 * index) + 4); + getFixedDofVector().push_back((6 * index) + 5); } -void RigidBodyCloud3D::clearFixing() +template +void RigidBodyCloud3D::clearFixing() { getFixedDofVector().clear(); } -std::vector &RigidBodyCloud3D::getFixedDofVector() +template +std::vector &RigidBodyCloud3D::getFixedDofVector() { return std::get(simObject().projections).indices(); } -template struct mandos::py::RigidBody3D; -template struct mandos::py::RigidBody3D; -mandos::core::SimulationObjectHandle RigidBodyCloud3D::handle() const +template +mandos::core::SimulationObjectHandle RigidBodyCloud3D::handle() const { return m_handle; } + +template struct mandos::py::RigidBody3D; +template struct mandos::py::RigidBody3D; + +template struct mandos::py::RigidBodyCloud3D; +template struct mandos::py::RigidBodyCloud3D; + } // namespace mandos::py diff --git a/bindings/Mandos/python/RigidBody.hpp b/bindings/Mandos/python/RigidBody.hpp index 4c55d9b4021fde4970d14896936d3dc4f34be842..34115bfddba78653c699d93f7e0932fb29e72f74 100644 --- a/bindings/Mandos/python/RigidBody.hpp +++ b/bindings/Mandos/python/RigidBody.hpp @@ -27,17 +27,17 @@ struct RigidBody3D { const mandos::core::SimulationObject &simObject() const; mandos::core::SimulationObjectHandle handle(); - mandos::core::Vec6 x() const; + Eigen::Map x() const; void setX(const mandos::core::Vec6 &x); - mandos::core::Vec6 v() const; + Eigen::Map v() const; void setV(const mandos::core::Vec6 &v); - mandos::core::Vec6 grad() const; + Eigen::Map grad() const; - mandos::core::Mat6 hessian() const; + Eigen::Map hessian() const; void setMass(core::Scalar mass); @@ -54,12 +54,13 @@ private: extern template struct RigidBody3D; extern template struct RigidBody3D; +template struct RigidBodyCloud3D { - explicit RigidBodyCloud3D(core::SimulationObjectHandle handle); + explicit RigidBodyCloud3D(core::SimulationObjectHandle handle); - mandos::core::SimulationObject &simObject(); - const mandos::core::SimulationObject &simObject() const; - mandos::core::SimulationObjectHandle handle() const; + mandos::core::SimulationObject &simObject(); + const mandos::core::SimulationObject &simObject() const; + mandos::core::SimulationObjectHandle handle() const; void resize(int n); @@ -73,11 +74,11 @@ struct RigidBodyCloud3D { void setV(const Eigen::Matrix &v); - void setMass(std::vector mass); + void setMass(const std::vector &mass); const std::vector &mass(); - void setInertiaTensor(std::vector inertiaTensor); + void setInertiaTensor(const std::vector &inertiaTensor); const std::vector &inertiaTensor() const; @@ -88,6 +89,7 @@ struct RigidBodyCloud3D { mandos::core::MassSpring &massSpring(); mandos::core::CosseratBendingRod &cosseratBendingRod(); mandos::core::CosseratRodAlignment &cosseratRodAlignment(); + mandos::core::AirDrag &airDrag(); void fixRigidBodyTranslation(int index, bool x = true, bool y = true, bool z = true); @@ -97,9 +99,18 @@ struct RigidBodyCloud3D { std::vector &getFixedDofVector(); private: - core::SimulationObjectHandle m_handle; + core::SimulationObjectHandle m_handle; }; +template <> +mandos::core::AirDrag &RigidBodyCloud3D::airDrag(); + +template <> +mandos::core::AirDrag &RigidBodyCloud3D::airDrag(); + +using RigidBodyCloud3DLocal = RigidBodyCloud3D; +using RigidBodyCloud3DGlobal = RigidBodyCloud3D; + void wrapRigidBody(nb::module_ &m); } // namespace mandos::py diff --git a/bindings/Mandos/python/RigidBodyPointMapping.cpp b/bindings/Mandos/python/RigidBodyPointMapping.cpp index e3c2c67f2769feac6aae124d223d7788f529c13d..a1ecd9b74e8b104c760ad21dd818607b38dcd48f 100644 --- a/bindings/Mandos/python/RigidBodyPointMapping.cpp +++ b/bindings/Mandos/python/RigidBodyPointMapping.cpp @@ -8,7 +8,7 @@ namespace mandos::py { -RigidBodyPointMapping::RigidBodyPointMapping(RigidBodyCloud3D &rigidBodyCloud, Model &model) +RigidBodyPointMapping::RigidBodyPointMapping(RigidBodyCloud3DLocal &rigidBodyCloud, Model &model) : m_deformable(model.add()) , m_rigidBodyCloud(rigidBodyCloud) @@ -28,12 +28,44 @@ void RigidBodyPointMapping::addParticle(const core::Vec3 &localPos, int rigidBod if (rigidBodyIndex >= m_rigidBodyCloud.size()) { throw std::invalid_argument("rigidBodyIndex is out of bounds for the rigidBodyCloud"); } - auto &mappings = m_rigidBodyCloud.simObject().mappings(); - mappings[m_mapping_index].addLocalPoint(localPos, rigidBodyIndex); - m_deformable.resize(mapping().size()); - m_deformable.setX(Eigen::Matrix::Zero(mapping().size(), 3)); + auto &m = mapping(); + m.addLocalPoint(localPos, rigidBodyIndex); + m_deformable.resize(m.size()); + m_deformable.setX(Eigen::Matrix::Zero(m.size(), 3)); + m_deformable.setV(Eigen::Matrix::Zero(m.size(), 3)); mapping().apply(mapping().from()->mstate.m_x, mapping().to()->mstate.m_x); + mapping().applyJ(mapping().from()->mstate.m_v, mapping().to()->mstate.m_v); +} + +RigidBodyGlobalPointMapping::RigidBodyGlobalPointMapping(RigidBodyCloud3DGlobal &rigidBodyCloud, Model &model) + : m_deformable(model.add()) + , m_rigidBodyCloud(rigidBodyCloud) + +{ + auto &mappings = m_rigidBodyCloud.simObject().mappings(); + m_mapping_index = static_cast(mappings.size()); + mappings.emplace_back(m_rigidBodyCloud.handle(), m_deformable.handle()); +} + +core::RigidBodyGlobalPointMapping &RigidBodyGlobalPointMapping::mapping() +{ + return m_rigidBodyCloud.simObject().mappings()[m_mapping_index]; +} + +void RigidBodyGlobalPointMapping::addParticle(const core::Vec3 &localPos, int rigidBodyIndex) +{ + if (rigidBodyIndex >= m_rigidBodyCloud.size()) { + throw std::invalid_argument("rigidBodyIndex is out of bounds for the rigidBodyCloud"); + } + auto &m = mapping(); + m.addLocalPoint(localPos, rigidBodyIndex); + m_deformable.resize(m.size()); + m_deformable.setX(Eigen::Matrix::Zero(m.size(), 3)); + m_deformable.setV(Eigen::Matrix::Zero(m.size(), 3)); + + mapping().apply(mapping().from()->mstate.m_x, mapping().to()->mstate.m_x); + mapping().applyJ(mapping().from()->mstate.m_v, mapping().to()->mstate.m_v); } void wrapRigidBodyPointMapping(nb::module_ &m) @@ -45,6 +77,13 @@ void wrapRigidBodyPointMapping(nb::module_ &m) mapping.addParticle(mandos::core::Vec3{r[0], r[1], r[2]}, index); }) .def_prop_ro("deformable", [](const RigidBodyPointMapping &self) { return self.m_deformable; }); + + nb::class_(m, "RigidBodyGlobalPointMapping") + .def("add_particle", &RigidBodyGlobalPointMapping::addParticle) + .def( + "add_particle", + [](RigidBodyGlobalPointMapping &mapping, const core::Vec3 &r, int index) { mapping.addParticle(r, index); }) + .def_prop_ro("deformable", [](const RigidBodyGlobalPointMapping &self) { return self.m_deformable; }); } } // namespace mandos::py diff --git a/bindings/Mandos/python/RigidBodyPointMapping.hpp b/bindings/Mandos/python/RigidBodyPointMapping.hpp index f02ff961aeb508059293ffb01e2c029388962d7e..fc85ef747d37c60c9fdfc2cf14899dc580082c8a 100644 --- a/bindings/Mandos/python/RigidBodyPointMapping.hpp +++ b/bindings/Mandos/python/RigidBodyPointMapping.hpp @@ -12,17 +12,29 @@ namespace nb = nanobind; namespace mandos::py { struct RigidBodyPointMapping { - RigidBodyPointMapping(RigidBodyCloud3D &rigidBodyCloud, Model &model); + RigidBodyPointMapping(RigidBodyCloud3DLocal &rigidBodyCloud, Model &model); void addParticle(const core::Vec3 &localPos, int rigidBodyIndex); core::RigidBodyPointMapping &mapping(); py::Deformable3D m_deformable; - py::RigidBodyCloud3D &m_rigidBodyCloud; + py::RigidBodyCloud3DLocal &m_rigidBodyCloud; unsigned int m_mapping_index; }; void wrapRigidBodyPointMapping(nb::module_ &m); + +struct RigidBodyGlobalPointMapping { + RigidBodyGlobalPointMapping(RigidBodyCloud3DGlobal &rigidBodyCloud, Model &model); + + void addParticle(const core::Vec3 &localPos, int rigidBodyIndex); + + core::RigidBodyGlobalPointMapping &mapping(); + + py::Deformable3D m_deformable; + py::RigidBodyCloud3DGlobal &m_rigidBodyCloud; + unsigned int m_mapping_index; +}; } // namespace mandos::py #endif // MANDOS_PY_RIGIDBODYPOINTMAPPING_HPP diff --git a/src/Mandos/Core/CMakeLists.txt b/src/Mandos/Core/CMakeLists.txt index 405f3998f4f766f2d411c43d300e865e3776fe7b..f2a0ff7f1499a1a054ad27d0086eb0fa1c6067b7 100644 --- a/src/Mandos/Core/CMakeLists.txt +++ b/src/Mandos/Core/CMakeLists.txt @@ -26,10 +26,14 @@ set(HEADERS Energies/CosseratBendingRod.hpp Energies/CosseratRodAlignment.hpp Energies/CollisionSpring.hpp + Energies/Friction.hpp + Energies/AirDrag.hpp + Energies/PlaneField.hpp Projections/FixedProjection.hpp Mappings/IdentityMapping.hpp Mappings/BarycentricMapping.hpp Mappings/RigidBodyPointMapping.hpp + Mappings/RigidBodyGlobalPointMapping.hpp Mappings/CollisionMapping.hpp Collisions/SDF.hpp Collisions/SimulationCollider.hpp @@ -79,10 +83,14 @@ set(SOURCES Energies/CosseratBendingRod.cpp Energies/CosseratRodAlignment.cpp Energies/CollisionSpring.cpp + Energies/Friction.cpp + Energies/AirDrag.cpp + Energies/PlaneField.cpp Projections/FixedProjection.cpp Mappings/IdentityMapping.cpp Mappings/BarycentricMapping.cpp Mappings/RigidBodyPointMapping.cpp + Mappings/RigidBodyGlobalPointMapping.cpp Mappings/CollisionMapping.cpp Collisions/SDF.cpp Collisions/SphereCloud.cpp diff --git a/src/Mandos/Core/Collisions/CollisionDetection.cpp b/src/Mandos/Core/Collisions/CollisionDetection.cpp index 809bf48bce64529a17786f0b74e0593daf7faa93..c6ea7042251ff4eb1b7f834d56488ff12c70e946 100644 --- a/src/Mandos/Core/Collisions/CollisionDetection.cpp +++ b/src/Mandos/Core/Collisions/CollisionDetection.cpp @@ -4,6 +4,7 @@ #include +#include #include #include @@ -12,6 +13,10 @@ #include #include #include +// #include + +// // Limit to one core for DEBUG +// tbb::global_control c(tbb::global_control::max_allowed_parallelism, 1); namespace mandos::core::collisions { @@ -25,37 +30,38 @@ std::vector> collisions(const SDF &sdf, const Sph tbb::enumerable_thread_specific>> contactsTLS; - tbb::parallel_for(tbb::blocked_range(0, centers.size()), - [&](const tbb::blocked_range &range) { - auto &contacts = contactsTLS.local(); - for (auto sId = range.begin(); sId != range.end(); ++sId) { - const auto ¢er = centers[sId]; - const auto r = radius[sId]; - - auto d = sdf.vdb().distance(center); - // If the distance is below the radius of the sphere, we consider there is a contact - if (d < r) { - // Compute the gradient of the distance field at this particular location using finite - // differences - const auto rdiff = 0.01 * sdf.vdb().voxelSize(); - auto xplus = sdf.vdb().distance(center + Vec3{rdiff, 0, 0}); - auto xminus = sdf.vdb().distance(center - Vec3{rdiff, 0, 0}); - auto yplus = sdf.vdb().distance(center + Vec3{0, rdiff, 0}); - auto yminus = sdf.vdb().distance(center - Vec3{0, rdiff, 0}); - auto zplus = sdf.vdb().distance(center + Vec3{0, 0, rdiff}); - auto zminus = sdf.vdb().distance(center - Vec3{0, 0, rdiff}); - - const auto n = Vec3{xplus - xminus, yplus - yminus, zplus - zminus}.normalized(); - - const auto primitiveId = sdf.vdb().closestPolygon(center); - - const ContactEventSide c0Side{primitiveId, center - d * n}; - const ContactEventSide c1Side{static_cast(sId)}; - - contacts.emplace_back(c0Side, c1Side, d, n); - } - } - }); + tbb::parallel_for( + tbb::blocked_range(0, centers.size()), [&](const tbb::blocked_range &range) { + auto &contacts = contactsTLS.local(); + for (auto sId = range.begin(); sId != range.end(); ++sId) { + const auto ¢er = centers[sId]; + const auto r = radius[sId]; + + auto d = sdf.vdb().distance(center); + // If the distance is below the radius of the sphere, we consider there is a contact + if (d < r) { + // Compute the gradient of the distance field at this particular location using finite + // differences + const auto rdiff = 0.01 * sdf.vdb().voxelSize(); + auto xplus = sdf.vdb().distance(center + Vec3{rdiff, 0, 0}); + auto xminus = sdf.vdb().distance(center - Vec3{rdiff, 0, 0}); + auto yplus = sdf.vdb().distance(center + Vec3{0, rdiff, 0}); + auto yminus = sdf.vdb().distance(center - Vec3{0, rdiff, 0}); + auto zplus = sdf.vdb().distance(center + Vec3{0, 0, rdiff}); + auto zminus = sdf.vdb().distance(center - Vec3{0, 0, rdiff}); + + const auto n = Vec3{xplus - xminus, yplus - yminus, zplus - zminus}.normalized(); + + const auto primitiveId = sdf.vdb().closestPolygon(center); + + const ContactEventSide c0Side{ + .rbIndex = sdf.rbIndex(), .primitiveId = primitiveId, .contactPoint = center - d * n}; + const ContactEventSide c1Side{static_cast(sId)}; + + contacts.emplace_back(c0Side, c1Side, d, n); + } + } + }); std::vector> contacts; const auto flattened = tbb::flatten2d(contactsTLS); @@ -260,18 +266,51 @@ void queryEdgeCandidates(void *userPtr, RTCCollision *collisions, unsigned int n auto bar = barycentricsClosestPointSegment2Segment({p1, q1}, {p2, q2}); const Vec3 c1 = p1 + (q1 - p1) * bar.x(); const Vec3 c2 = p2 + (q2 - p2) * bar.y(); + constexpr auto tol{1e-8}; if ((c2 - c1).squaredNorm() < dHatSq) { - return bar; + if (((bar.x() > tol) && (bar.x() < 1.0 - tol)) and + ((bar.y() > tol) && + (bar.y() < 1.0 - tol))) { // The particle must be directly above or below the edge + return bar; + } } return {}; }; + auto isPointInside = [](const CollisionMesh &cm, size_t index, const Vec3 &point) -> bool { + auto pointTriangleDistance = [&cm, index, &point](size_t i) -> Scalar { + if (const int t = cm.edgeTriangleNeighbours()[index][i]; t >= 0) { + auto ut = static_cast(t); + const Vec3 &xA = cm.vertices()[static_cast(cm.triangles()[ut][0])]; + const Vec3 &xB = cm.vertices()[static_cast(cm.triangles()[ut][1])]; + const Vec3 &xC = cm.vertices()[static_cast(cm.triangles()[ut][2])]; + const Vec3 n = ((xB - xA).cross(xC - xA)).normalized(); + return n.dot(point - xA); + } + return 1.0; + }; + constexpr auto tol{1e-4}; + const auto d0 = pointTriangleDistance(0); + const auto d1 = pointTriangleDistance(1); + + if (d0 < 0.0 && d1 <= tol) { + return true; + } + if (d0 <= tol && d1 < 0.0) { + return true; + } + return false; + }; + if (auto bar = checkQuery(x00, x01, x10, x11); bar) { const Vec3 c1 = x00 + (x01 - x00) * bar->x(); const Vec3 c2 = x10 + (x11 - x10) * bar->y(); ContactEvent contact; - contact.distance = dHat; + contact.distance = (c2 - c1).norm(); contact.normal = (c2 - c1).normalized(); + if (isPointInside(cm0, collision.primID0, c2) && isPointInside(cm1, collision.primID1, c1)) { + contact.normal *= -1; + } contact.c0Contact.barycentricCoordinates = {1 - bar->x(), bar->x(), 0}; contact.c0Contact.indices = {indices0[0], indices0[1], 0}; contact.c1Contact.barycentricCoordinates = {1 - bar->y(), bar->y(), 0}; @@ -432,7 +471,9 @@ void queryTrianglePointCandidates(void *userPtr, RTCCollision *collisions, unsig auto checkQuery = [dHatSq](const auto &p, const auto &a, const auto &b, const auto &c) -> std::optional { auto bar = barycentrisClosestPointPoint2Triangle(p, {a, b, c}); if (((bar.x() * a + bar.y() * b + bar.z() * c) - p).squaredNorm() < dHatSq) { - return bar; + if ((bar.array() > 0).all()) { // The particle must be directly above or below the triangle + return bar; + } } return {}; }; @@ -440,7 +481,8 @@ void queryTrianglePointCandidates(void *userPtr, RTCCollision *collisions, unsig // From triangle in GeomID0 to each vertex of GeomID1 if (auto bar = checkQuery(x1, x00, x01, x02); bar) { ContactEvent contact; - contact.distance = dHat; + const Vec3 triPoint = (bar.value().x() * x00) + (bar.value().y() * x01) + (bar.value().z() * x02); + contact.distance = (triPoint - x1).norm(); if constexpr (CM0First) { contact.normal = n0; @@ -469,17 +511,6 @@ std::vector> collisions(const Collisi query.cm1 = &cm1; query.isSelfCollision = (&cm0 == &cm1); - // { - // ZoneScopedN("collisions.triangles/triangles"); - // rtcCollide(cm0.trianglesScene(), // - // cm1.trianglesScene(), - // &(queryTriangleCandidates), - // // NOLINTNEXTLINE - // reinterpret_cast(&query)); - - // query.candidates.clear(); - // } - { ZoneScopedN("collisions.triangles/vertices"); tbb::task_group g; diff --git a/src/Mandos/Core/Collisions/CollisionMesh.cpp b/src/Mandos/Core/Collisions/CollisionMesh.cpp index 681eb31e1a0bc4a06eec1f92561ff59447674c47..28664803d3d512f4a950d6be0e625aad583bc27e 100644 --- a/src/Mandos/Core/Collisions/CollisionMesh.cpp +++ b/src/Mandos/Core/Collisions/CollisionMesh.cpp @@ -10,6 +10,7 @@ #include #include +#include #include namespace @@ -111,6 +112,59 @@ void edgeBBoxBoundFunction(const RTCBoundsFunctionArguments *args) args->bounds_o->upper_y = static_cast(bbox.max().y() + userPtr->dhat); args->bounds_o->upper_z = static_cast(bbox.max().z() + userPtr->dhat); } + +// Custom hash for a pair of ints (the sorted edge) +struct EdgeHash { + std::size_t operator()(const std::array &edge) const + { + // A simple hashing formula + return std::hash()(edge[0]) ^ (std::hash()(edge[1]) << 1U); + } +}; + +void findEdgeTriangleNeighbours(const std::vector> &triangles, + const std::vector> &edges, + std::vector> &edgeTriNeighbours) +{ + edgeTriNeighbours.resize(edges.size(), {-1, -1}); + + // Create a lookup: key: sorted edge (pair of vertex indices), value: vector of triangle indices + std::unordered_map, std::vector, EdgeHash> edgeToTriangles; + + // Populate the lookup using triangles + for (size_t t = 0; t < triangles.size(); ++t) { + const auto &tri = triangles[t]; + // Each triangle has three edges; define each as a sorted pair. + const std::array, 3> triEdges = {{{std::min(tri[0], tri[1]), std::max(tri[0], tri[1])}, + {std::min(tri[1], tri[2]), std::max(tri[1], tri[2])}, + {std::min(tri[2], tri[0]), std::max(tri[2], tri[0])}}}; + for (const auto &e : triEdges) { + edgeToTriangles[e].push_back(static_cast(t)); + } + } + + // Process the provided edges + for (size_t i = 0; i < edges.size(); ++i) { + // For consistency, sort the edge vertices + std::array sortedEdge = edges[i]; + std::ranges::sort(sortedEdge); + + // Find this edge in our lookup table + auto it = edgeToTriangles.find(sortedEdge); + if (it != edgeToTriangles.end()) { + const auto &triangleIndices = it->second; + if (!triangleIndices.empty()) { + // Always assign the first neighbour found + edgeTriNeighbours[i][0] = triangleIndices[0]; + // If a second triangle is available, assign it + if (triangleIndices.size() > 1) { + edgeTriNeighbours[i][1] = triangleIndices[1]; + } + } + } + } +} + } // namespace namespace mandos::core::collisions @@ -120,7 +174,6 @@ CollisionMesh::CollisionMesh(const SurfaceMesh &mesh, Scalar dhat) : CollisionMesh(mesh.vertices, {}, mesh.indices, dhat) { } - CollisionMesh::CollisionMesh(const std::vector &vertices, const std::vector> &edges, const std::vector> &triangles, @@ -168,6 +221,9 @@ CollisionMesh::CollisionMesh(const std::vector &vertices, rtcReleaseGeometry(m_trianglesGeometry); rtcReleaseGeometry(m_edgesGeometry); rtcReleaseGeometry(m_verticesGeometry); + + // Find edge triangle neighbours + findEdgeTriangleNeighbours(m_triangles, m_edges, m_edgeTriangleNeighbours); } void CollisionMesh::update(const MechanicalState &mstate) @@ -233,6 +289,11 @@ const std::vector> &CollisionMesh::edges() const return m_edges; } +const std::vector> &CollisionMesh::edgeTriangleNeighbours() const +{ + return m_edgeTriangleNeighbours; +} + Scalar CollisionMesh::dhat() const { return m_dHat; @@ -262,4 +323,4 @@ RTCScene CollisionMesh::verticesScene() const { return m_verticesScene; } -} // namespace mandos::core::collisions \ No newline at end of file +} // namespace mandos::core::collisions diff --git a/src/Mandos/Core/Collisions/CollisionMesh.hpp b/src/Mandos/Core/Collisions/CollisionMesh.hpp index 7f8a3f667a4fbdeb59ffe1a24ea4927f5e6952de..322e3b8ea52ee102435be55f5225f3c05874c062 100644 --- a/src/Mandos/Core/Collisions/CollisionMesh.hpp +++ b/src/Mandos/Core/Collisions/CollisionMesh.hpp @@ -55,6 +55,7 @@ public: Scalar dhat() const; const std::vector> &edges() const; + const std::vector> &edgeTriangleNeighbours() const; const std::vector> &triangles() const; const std::vector &vertices() const; @@ -66,6 +67,7 @@ private: RTCGeometry m_verticesGeometry; std::vector> m_edges; + std::vector> m_edgeTriangleNeighbours; RTCScene m_edgesScene; RTCGeometry m_edgesGeometry; @@ -82,4 +84,4 @@ struct ContactEventSide { } // namespace mandos::core::collisions -#endif // MANDOS_CORE_COLLISIONS_COLLISIONMESH_HPP \ No newline at end of file +#endif // MANDOS_CORE_COLLISIONS_COLLISIONMESH_HPP diff --git a/src/Mandos/Core/Collisions/SDF.cpp b/src/Mandos/Core/Collisions/SDF.cpp index 26a3906b65eea95651019280c3f67825dc986a1c..5a0f2a3f691578b9bb8b76c1d2d6cf8742aebe0e 100644 --- a/src/Mandos/Core/Collisions/SDF.cpp +++ b/src/Mandos/Core/Collisions/SDF.cpp @@ -85,41 +85,50 @@ const SDF::VDB &SDF::vdb() const return m_vdb; } -SDF::SDF(const SurfaceMesh &mesh, Scalar contactOffset, int nbVoxels, bool flip) +SDF::SDF(const SurfaceMesh &mesh, Scalar contactOffset, int rbIndex, int nbVoxels, bool flip) : m_vdb(mesh, contactOffset, nbVoxels, flip) , m_surfaceMesh(mesh) + , m_rbIndex(rbIndex) { } void SDF::update(const MechanicalState &mstate) { - m_vdb.update(mstate); + m_vdb.update(mstate, m_rbIndex); } void SDF::update(const MechanicalState &mstate) { - m_vdb.update(mstate); + m_vdb.update(mstate, m_rbIndex); } const SurfaceMesh &SDF::mesh() const { return m_surfaceMesh; } -void SDF::VDB::update(const MechanicalState &mstate) + +int SDF::rbIndex() const +{ + return m_rbIndex; +} + +void SDF::VDB::update(const MechanicalState &mstate, int rbIndex) { mandos::core::Mat4 transform = mandos::core::Mat4::Zero(); - transform.block<3, 3>(0, 0) = mandos::core::rotationExpMap(mstate.m_x[0].segment<3>(3)); // Rotation - transform.block<3, 1>(0, 3) = mstate.m_x[0].segment<3>(0); // Position + transform.block<3, 3>(0, 0) = + mandos::core::rotationExpMap(mstate.m_x[static_cast(rbIndex)].segment<3>(3)); // Rotation + transform.block<3, 1>(0, 3) = mstate.m_x[static_cast(rbIndex)].segment<3>(0); // Position transform(3, 3) = 1.0; m_transform = transform; m_invTransform = m_transform.inverse(); } -void SDF::VDB::update(const MechanicalState &mstate) +void SDF::VDB::update(const MechanicalState &mstate, int rbIndex) { mandos::core::Mat4 transform = mandos::core::Mat4::Zero(); - transform.block<3, 3>(0, 0) = mandos::core::rotationExpMap(mstate.m_x[0].segment<3>(3)); // Rotation - transform.block<3, 1>(0, 3) = mstate.m_x[0].segment<3>(0); // Position + transform.block<3, 3>(0, 0) = + mandos::core::rotationExpMap(mstate.m_x[static_cast(rbIndex)].segment<3>(3)); // Rotation + transform.block<3, 1>(0, 3) = mstate.m_x[static_cast(rbIndex)].segment<3>(0); // Position transform(3, 3) = 1.0; m_transform = transform; m_invTransform = m_transform.inverse(); diff --git a/src/Mandos/Core/Collisions/SDF.hpp b/src/Mandos/Core/Collisions/SDF.hpp index 3e9f085920823db264a37054d113ca201fa6217a..369c7b27b14c8abf5792a92ca6441c5aa965a176 100644 --- a/src/Mandos/Core/Collisions/SDF.hpp +++ b/src/Mandos/Core/Collisions/SDF.hpp @@ -1,19 +1,17 @@ #ifndef MANDOS_CORE_COLLISIONS_SDF_H #define MANDOS_CORE_COLLISIONS_SDF_H -#include #include +#include #include #include #include +#include #include #include -#include "Mandos/Core/Collisions/ContactEvent.hpp" -#include "Mandos/Core/MechanicalState.hpp" -#include "Mandos/Core/MechanicalStates/Particle3D.hpp" namespace mandos::core::collisions { @@ -40,8 +38,8 @@ class MANDOS_CORE_EXPORT SDF Scalar distance(const mandos::core::Vec3 &query) const; PolygonIndexGrid::ValueType closestPolygon(const mandos::core::Vec3 &query) const; - void update(const MechanicalState &mstate); - void update(const MechanicalState &mstate); + void update(const MechanicalState &mstate, int rbIndex); + void update(const MechanicalState &mstate, int rbIndex); const Eigen::Transform &transform() const; Scalar voxelSize() const; @@ -66,35 +64,30 @@ public: * @param nbVoxels Number of voxels used in the longest side of the mesh. Increasing this value will increase * accuracy of the underlying SDF, but will dramatically incrase memory consumption and build time. By default 256. */ - SDF(const SurfaceMesh &mesh, Scalar contactOffset, int nbVoxels = 256, bool flip = false); + SDF(const SurfaceMesh &mesh, Scalar contactOffset, int rbIndex, int nbVoxels = 256, bool flip = false); const VDB &vdb() const; const SurfaceMesh &mesh() const; + int rbIndex() const; void update(const MechanicalState &mstate); void update(const MechanicalState &mstate); const Eigen::Transform &transform() const; - // /** - // * @brief Given a point, computes the contact information between the point and the SDF - // * - // * @param point - // * @param out - // */ - // void compute_contact_geometry(const Vec3 &point, ContactEvent &out) const; - private: VDB m_vdb; SurfaceMesh m_surfaceMesh; + int m_rbIndex; }; template <> struct ContactEventSide { + int rbIndex; int primitiveId; Vec3 contactPoint; }; } // namespace mandos::core::collisions -#endif // MANDOS_CORE_COLLISIONS_SDF_H \ No newline at end of file +#endif // MANDOS_CORE_COLLISIONS_SDF_H diff --git a/src/Mandos/Core/Energies.hpp b/src/Mandos/Core/Energies.hpp index c1a7e0bf6dc501907ca0ea91e3a70a24224a0e5f..7c8b17a19d08f20457f477c851743573a1fc9d33 100644 --- a/src/Mandos/Core/Energies.hpp +++ b/src/Mandos/Core/Energies.hpp @@ -15,9 +15,12 @@ #include #include #include +#include -#include +#include "Mandos/Core/Energies/Friction.hpp" +#include "Mandos/Core/Energies/AirDrag.hpp" #include +#include namespace mandos::core { @@ -65,6 +68,11 @@ struct Potentials { m_potentials; }; +template <> +struct DissipativePotentials { + std::tuple m_dissipativePotentials; +}; + template <> struct Inertias { std::tuple m_inertias; @@ -79,6 +87,11 @@ struct Potentials { m_potentials; }; +template <> +struct DissipativePotentials { + std::tuple m_dissipativePotentials; +}; + template <> struct Inertias { std::tuple m_inertias; @@ -86,7 +99,16 @@ struct Inertias { template <> struct Potentials { - std::tuple m_potentials; + std::tuple + m_potentials; +}; + +template <> +struct DissipativePotentials { + std::tuple<> m_dissipativePotentials; }; } // namespace mandos::core diff --git a/src/Mandos/Core/Energies/AirDrag.cpp b/src/Mandos/Core/Energies/AirDrag.cpp new file mode 100644 index 0000000000000000000000000000000000000000..29bed991af7dcdd7bf293d4d3fb1e0ec901ee836 --- /dev/null +++ b/src/Mandos/Core/Energies/AirDrag.cpp @@ -0,0 +1,183 @@ +#include +#include + +#include + +namespace mandos::core +{ + +int AirDrag::size() const +{ + return static_cast(m_indices.size()); +} + +Scalar AirDrag::computeEnergy(const MechanicalState &mstate, Scalar h) const +{ + Scalar energy{0}; + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto index = static_cast(m_indices[i]); + const Vec3 &v = (mstate.m_x[index] - mstate.m_x0[index]) / h; + energy += 0.5 * h * m_dragCoefficient[i] * v.squaredNorm(); + } + return energy; +} + +Scalar AirDrag::computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const +{ + Scalar energy{0}; + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto index = static_cast(m_indices[i]); + const Vec3 &v = (mstate.m_x[index] - mstate.m_x0[index]) / h; + energy += 0.5 * h * m_dragCoefficient[i] * v.squaredNorm(); + + mstate.m_grad[static_cast(m_indices[i])] += m_dragCoefficient[i] * v; + } + return energy; +} + +Scalar AirDrag::computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const +{ + Scalar energy{0}; + std::vector triplets; + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto &index = static_cast(m_indices[i]); + const Vec3 &v = (mstate.m_x[index] - mstate.m_x0[index]) / h; + energy += 0.5 * h * m_dragCoefficient[i] * v.squaredNorm(); + + mstate.m_grad[index] += m_dragCoefficient[i] * v; + + // Diagonal hessian + const Scalar hessian = m_dragCoefficient[i] / h; + triplets.emplace_back(3 * index + 0, 3 * index + 0, hessian); + triplets.emplace_back(3 * index + 1, 3 * index + 1, hessian); + triplets.emplace_back(3 * index + 2, 3 * index + 2, hessian); + } + { + ZoneScopedN("AirDrag.globalAssembly"); + SparseMat hessian; + hessian.resize(mstate.size(), mstate.size()); + hessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += hessian; + } + return energy; +} + +Scalar AirDrag::computeEnergy(const MechanicalState &mstate, Scalar h) const +{ + Scalar energy{0}; + const Vec3 r = Vec3(1.0, 0.0, 0.0); + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto index = static_cast(m_indices[i]); + const Mat3 R = rotationExpMap(mstate.m_x[index].segment<3>(3)); + const Vec3 x = R * r + mstate.m_x[index].segment<3>(0); + const Mat3 R0 = rotationExpMap(mstate.m_x0[index].segment<3>(3)); + const Vec3 x0 = R0 * r + mstate.m_x0[index].segment<3>(0); + const Vec3 &v = (x - x0) / h; + energy += 0.5 * h * m_dragCoefficient[i] * v.squaredNorm(); + } + return energy; +} + +Scalar AirDrag::computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const +{ + Scalar energy{0}; + const Vec3 r = Vec3(1.0, 0.0, 0.0); + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto index = static_cast(m_indices[i]); + const Mat3 R = rotationExpMap(mstate.m_x[index].segment<3>(3)); + const Vec3 x = R * r + mstate.m_x[index].segment<3>(0); + const Mat3 R0 = rotationExpMap(mstate.m_x0[index].segment<3>(3)); + const Vec3 x0 = R0 * r + mstate.m_x0[index].segment<3>(0); + const Vec3 &v = (x - x0) / h; + energy += 0.5 * h * m_dragCoefficient[i] * v.squaredNorm(); + + const Vec3 grad = m_dragCoefficient[i] * v; + + mstate.m_grad[static_cast(m_indices[i])].segment<3>(0) += grad; + mstate.m_grad[static_cast(m_indices[i])].segment<3>(3) += skew(R * r) * grad; + } + return energy; +} + +Scalar AirDrag::computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const +{ + Scalar energy{0}; + std::vector triplets; + const Vec3 r = Vec3(1.0, 0.0, 0.0); + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto index = static_cast(m_indices[i]); + const Mat3 R = rotationExpMap(mstate.m_x[index].segment<3>(3)); + const Vec3 x = R * r + mstate.m_x[index].segment<3>(0); + const Mat3 R0 = rotationExpMap(mstate.m_x0[index].segment<3>(3)); + const Vec3 x0 = R0 * r + mstate.m_x0[index].segment<3>(0); + const Vec3 &v = (x - x0) / h; + energy += 0.5 * h * m_dragCoefficient[i] * v.squaredNorm(); + + const Vec3 grad = m_dragCoefficient[i] * v; + + mstate.m_grad[static_cast(m_indices[i])].segment<3>(0) += grad; + mstate.m_grad[static_cast(m_indices[i])].segment<3>(3) += skew(R * r) * grad; + + // Diagonal hessian + const Scalar hessian = m_dragCoefficient[i] / h; + triplets.emplace_back(6 * index + 0, 6 * index + 0, hessian); + triplets.emplace_back(6 * index + 1, 6 * index + 1, hessian); + triplets.emplace_back(6 * index + 2, 6 * index + 2, hessian); + // const Vec3 Rgrad = skew(R * r) * grad; + // const Mat3 Rhessian = -hessian * skew(R * r) * skew(R * r) + + // 0.5 * hessian * (skew(Rgrad) * skew(R * r) + skew(R * r) * skew(Rgrad)); + + const Mat3 Rhessian = -hessian * skew(R * r) * skew(R * r); + + for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int k = 0; k < 3; ++k) { + triplets.emplace_back(6 * index + 3 + j, 6 * index + 3 + k, Rhessian(j, k)); + } + } + } + { + ZoneScopedN("AirDrag.globalAssembly"); + SparseMat hessian; + hessian.resize(mstate.size(), mstate.size()); + hessian.setFromTriplets(triplets.begin(), triplets.end()); + // std::cout << hessian.toDense() << std::endl; + mstate.m_hessian += hessian; + } + return energy; +} + +void AirDrag::computeLaggedPositionHessian(MechanicalState &mstate, Scalar h) const +{ + std::vector triplets; + for (size_t i = 0; i < m_indices.size(); ++i) { + const auto &index = static_cast(m_indices[i]); + // Diagonal hessian + const Scalar hessian = -m_dragCoefficient[i] / h; + triplets.emplace_back(index + 0, index + 0, hessian); + triplets.emplace_back(index + 1, index + 1, hessian); + triplets.emplace_back(index + 2, index + 2, hessian); + } + { + ZoneScopedN("AirDrag.globalAssembly"); + SparseMat hessian(mstate.size(), mstate.size()); + hessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += hessian; + } +} +void AirDrag::computeLaggedPositionHessian(MechanicalState & /*mstate*/, Scalar /*h*/) const +{ + // TODO +} + +void AirDrag::addElement(int index, Scalar dragCoefficient) +{ + m_dragCoefficient.push_back(dragCoefficient); + m_indices.push_back(index); +} + +void AirDrag::clear() +{ + m_indices.clear(); + m_dragCoefficient.clear(); +} +} // namespace mandos::core diff --git a/src/Mandos/Core/Energies/AirDrag.hpp b/src/Mandos/Core/Energies/AirDrag.hpp new file mode 100644 index 0000000000000000000000000000000000000000..903bcb4ea59fc39e1206fade8582faca5e06aa36 --- /dev/null +++ b/src/Mandos/Core/Energies/AirDrag.hpp @@ -0,0 +1,70 @@ +#ifndef MANDOS_ENERGIES_AIRDRAG_H_ +#define MANDOS_ENERGIES_AIRDRAG_H_ + +#include +#include "Mandos/Core/MechanicalStates/RigidBody.hpp" + +namespace mandos::core +{ +class MANDOS_CORE_EXPORT AirDrag +{ +public: + /** + * @brief Number of energy elements + * + * @return int + */ + int size() const; + /** + * @brief Computes the total drag energy dissipated due to air resistance + * for the current mechanical state + * + * @param mstate Current mechanical state + * @param h Time step size + * @return Scalar Total drag energy + */ + Scalar computeEnergy(const MechanicalState &mstate, Scalar h) const; + Scalar computeEnergy(const MechanicalState &mstate, Scalar h) const; + + /** + * @brief Computes the air drag energy and corresponding gradient (drag force) + * applied to the system due to air resistance + * + * @param mstate Current mechanical state, and where the gradient will be written + * @param h Time step size + * @return Scalar Total drag energy + */ + Scalar computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const; + Scalar computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const; + + /** + * @brief Computes the air drag energy, gradient (drag force), and Hessian + * for the given mechanical state + * + * @param mstate Current mechanical state, and where gradient and Hessian will be written + * @param h Time step size + * @return Scalar Total drag energy + */ + Scalar computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const; + Scalar computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const; + + void computeLaggedPositionHessian(MechanicalState &mstate, Scalar h) const; + void computeLaggedPositionHessian(MechanicalState &mstate, Scalar h) const; + + /** + * @brief Adds a new air drag element acting on a specified index + * + * @param index Index of the element (particle or rigid body) to apply air drag to + * @param dragCoefficient Coefficient of air resistance for the element + */ + void addElement(int index, Scalar dragCoefficient); + + void clear(); + +private: + std::vector m_dragCoefficient; + std::vector m_indices; +}; +} // namespace mandos::core + +#endif // MANDOS_ENERGIES_AIRDRAG_H_ diff --git a/src/Mandos/Core/Energies/CosseratBendingRod.cpp b/src/Mandos/Core/Energies/CosseratBendingRod.cpp index 8676d9300b9ec51443fc52e22a154639bfa3d789..4efdca4a979c464975254a9c1126b3cf1366a164 100644 --- a/src/Mandos/Core/Energies/CosseratBendingRod.cpp +++ b/src/Mandos/Core/Energies/CosseratBendingRod.cpp @@ -3,10 +3,46 @@ #include #include +#include + #include #include +namespace +{ +TinyAD::Double<6> computeEnergyAD(const mandos::core::MechanicalState &mstate, + std::size_t iA, + std::size_t iB, + mandos::core::Scalar restLength, + const mandos::core::Vec3 &stiffnessTensor, + const mandos::core::Vec3 &intrinsicDarboux) +{ + Eigen::Vector, 3> thetaA; + thetaA[0] = TinyAD::Double<6>(mstate.m_x[iA][3 + 0], 0); + thetaA[1] = TinyAD::Double<6>(mstate.m_x[iA][3 + 1], 1); + thetaA[2] = TinyAD::Double<6>(mstate.m_x[iA][3 + 2], 2); + Eigen::Vector, 3> thetaB; + thetaB[0] = TinyAD::Double<6>(mstate.m_x[iB][3 + 0], 3 + 0); + thetaB[1] = TinyAD::Double<6>(mstate.m_x[iB][3 + 1], 3 + 1); + thetaB[2] = TinyAD::Double<6>(mstate.m_x[iB][3 + 2], 3 + 2); + + const auto RA = mandos::core::rotationExpMap(thetaA).eval(); + const auto RB = mandos::core::rotationExpMap(thetaB).eval(); + + const mandos::core::Mat3 J = 0.5 * mandos::core::Vec3(-stiffnessTensor(0) + stiffnessTensor(1) + stiffnessTensor(2), + stiffnessTensor(0) - stiffnessTensor(1) + stiffnessTensor(2), + stiffnessTensor(0) + stiffnessTensor(1) - stiffnessTensor(2)) + .asDiagonal(); + + const mandos::core::Mat3 Ak = + 0.5 * (mandos::core::skew(intrinsicDarboux) * J + J * mandos::core::skew(intrinsicDarboux)); + const mandos::core::Mat3 JAk = J * mandos::core::Scalar{1.0} / restLength + Ak; + return -(RB.transpose() * RA * JAk).trace(); // Bending energy +} + +} // namespace + namespace mandos::core { @@ -238,4 +274,83 @@ Scalar computeEnergyGradientAndHessian(const CosseratBendingRod &bending, Mechan return energy; } + +Scalar computeEnergy(const CosseratBendingRod &bending, const MechanicalState &mstate) +{ + Scalar energy{0}; + for (auto i = 0UL; i < static_cast(bending.size()); ++i) { + const auto iA = static_cast(bending.indices()[i][0]); + const auto iB = static_cast(bending.indices()[i][1]); + + energy += + computeEnergyAD( + mstate, iA, iB, bending.restLength()[i], bending.stiffnessTensor()[i], bending.intrinsicDarboux()[i]) + .val; + } + return energy; +} + +Scalar computeEnergyAndGradient(const CosseratBendingRod &bending, MechanicalState &mstate) +{ + Scalar energy{0}; + for (auto i = 0UL; i < static_cast(bending.size()); ++i) { + const auto iA = static_cast(bending.indices()[i][0]); + const auto iB = static_cast(bending.indices()[i][1]); + + const auto e = computeEnergyAD( + mstate, iA, iB, bending.restLength()[i], bending.stiffnessTensor()[i], bending.intrinsicDarboux()[i]); + energy += e.val; + mstate.m_grad[iA].segment<3>(3) += e.grad.segment<3>(0); + mstate.m_grad[iB].segment<3>(3) += e.grad.segment<3>(3); + } + return energy; +} + +Scalar computeEnergyGradientAndHessian(const CosseratBendingRod &bending, MechanicalState &mstate) +{ + Scalar energy{0}; + std::vector triplets; + for (auto i = 0UL; i < static_cast(bending.size()); ++i) { + const auto iA = static_cast(bending.indices()[i][0]); + const auto iB = static_cast(bending.indices()[i][1]); + + const auto e = computeEnergyAD( + mstate, iA, iB, bending.restLength()[i], bending.stiffnessTensor()[i], bending.intrinsicDarboux()[i]); + + energy += e.val; + + mstate.m_grad[iA].segment<3>(3) += e.grad.segment<3>(0); + mstate.m_grad[iB].segment<3>(3) += e.grad.segment<3>(3); + + Mat3 hessianA = e.Hess.block<3, 3>(0, 0); + Mat3 hessianB = e.Hess.block<3, 3>(3, 3); + Mat3 hessianAB = e.Hess.block<3, 3>(0, 3); // REVIEW: transpose? + + // Construct Sparse hessian + // --------------------------------------------------------------- + for (auto k = 0; k < 3; k++) { + for (auto j = 0; j < 3; j++) { + triplets.emplace_back(6 * static_cast(iA) + 3 + k, + 6 * static_cast(iA) + 3 + j, + hessianA(k, j)); + triplets.emplace_back(6 * static_cast(iA) + 3 + k, + 6 * static_cast(iB) + 3 + j, + hessianAB(k, j)); + triplets.emplace_back(6 * static_cast(iB) + 3 + k, + 6 * static_cast(iA) + 3 + j, + hessianAB(j, k)); + triplets.emplace_back(6 * static_cast(iB) + 3 + k, + 6 * static_cast(iB) + 3 + j, + hessianB(k, j)); + } + } + } + + SparseMat energyHessian; + energyHessian.resize(mstate.size(), mstate.size()); + energyHessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += energyHessian; + + return energy; +} } // namespace mandos::core diff --git a/src/Mandos/Core/Energies/CosseratBendingRod.hpp b/src/Mandos/Core/Energies/CosseratBendingRod.hpp index f12d6773fedd3d1f03df69d9ab6a1f28e310171c..1a8d1ecd76612cba158d28fa9e9018b74a95a5c8 100644 --- a/src/Mandos/Core/Energies/CosseratBendingRod.hpp +++ b/src/Mandos/Core/Energies/CosseratBendingRod.hpp @@ -2,6 +2,7 @@ #define MANDOS_ENERGIES_COSSEATRODBENDING_H #include +#include #include @@ -67,6 +68,13 @@ Scalar MANDOS_CORE_EXPORT computeEnergyAndGradient(const CosseratBendingRod &ben Scalar MANDOS_CORE_EXPORT computeEnergyGradientAndHessian(const CosseratBendingRod &bending, MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergy(const CosseratBendingRod &bending, + const MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergyAndGradient(const CosseratBendingRod &bending, + MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergyGradientAndHessian(const CosseratBendingRod &bending, + MechanicalState &mstate); + MANDOS_CORE_EXPORT Vec3 computeDarbouxVector(Scalar L0, const Mat3 &R1, const Mat3 &R2); template <> diff --git a/src/Mandos/Core/Energies/CosseratRodAlignment.cpp b/src/Mandos/Core/Energies/CosseratRodAlignment.cpp index 158ec69e70f8f4f21d062a2835fa54f388a4241d..155a5c2a7214b44ae4a22c7c539fce8c95eac843 100644 --- a/src/Mandos/Core/Energies/CosseratRodAlignment.cpp +++ b/src/Mandos/Core/Energies/CosseratRodAlignment.cpp @@ -8,7 +8,34 @@ #include -#include +namespace +{ + +TinyAD::Double<9> computeEnergyAD(const mandos::core::MechanicalState &mstate, + std::size_t iA, + std::size_t iB, + mandos::core::Scalar restLength, + mandos::core::Scalar cosseratStiffness) +{ + Eigen::Vector, 3> xA; + xA[0] = TinyAD::Double<9>(mstate.m_x[iA][0], 0); + xA[1] = TinyAD::Double<9>(mstate.m_x[iA][1], 1); + xA[2] = TinyAD::Double<9>(mstate.m_x[iA][2], 2); + Eigen::Vector, 3> xB; + xB[0] = TinyAD::Double<9>(mstate.m_x[iB][0], 3 + 0); + xB[1] = TinyAD::Double<9>(mstate.m_x[iB][1], 3 + 1); + xB[2] = TinyAD::Double<9>(mstate.m_x[iB][2], 3 + 2); + Eigen::Vector, 3> theta; + theta[0] = TinyAD::Double<9>(mstate.m_x[iA][3 + 0], 6 + 0); + theta[1] = TinyAD::Double<9>(mstate.m_x[iA][3 + 1], 6 + 1); + theta[2] = TinyAD::Double<9>(mstate.m_x[iA][3 + 2], 6 + 2); + + const auto R = mandos::core::rotationExpMap(theta); + + return restLength * cosseratStiffness * (1.0 - R.col(2).dot((xB - xA).normalized())); +} + +} // namespace namespace mandos::core { @@ -85,9 +112,10 @@ Scalar computeEnergy(const CosseratRodAlignment &alignment, const MechanicalStat // --------------------------------------------------------------- const Vec3 xA = mstate.m_x[iA].segment<3>(0); const Vec3 xB = mstate.m_x[iB].segment<3>(0); - const Mat3 RA = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); + const Mat3 R = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); - energy += -alignment.restLength()[i] * alignment.cosseratStiffness()[i] * RA.col(2).dot((xA - xB).normalized()); + energy += + alignment.restLength()[i] * alignment.cosseratStiffness()[i] * (1.0 - R.col(2).dot((xB - xA).normalized())); } return energy; } @@ -103,19 +131,20 @@ Scalar computeEnergyAndGradient(const CosseratRodAlignment &alignment, Mechanica // --------------------------------------------------------------- const Vec3 xA = mstate.m_x[iA].segment<3>(0); const Vec3 xB = mstate.m_x[iB].segment<3>(0); - const Mat3 RA = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); + const Mat3 R = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); + + const Scalar L0 = alignment.restLength()[i]; + const Scalar stiffness = alignment.cosseratStiffness()[i]; const auto oneOverL = mandos::core::Scalar{1.0} / (xA - xB).norm(); - const auto u = ((xA - xB) * oneOverL).eval(); + const auto u = ((xB - xA) * oneOverL).eval(); const auto uut = (u * u.transpose()).eval(); - energy += -alignment.restLength()[i] * alignment.cosseratStiffness()[i] * RA.col(2).dot(u); + energy += L0 * stiffness * (1.0 - R.col(2).dot(u)); - const Vec3 &grad = -alignment.cosseratStiffness()[i] * alignment.restLength()[i] * (Mat3::Identity() - uut) * - oneOverL * RA.col(2); + const Vec3 &grad = stiffness * L0 * oneOverL * (Mat3::Identity() - uut) * R.col(2); mstate.m_grad[iA].segment<3>(0) += grad; - mstate.m_grad[iA].segment<3>(3) += alignment.cosseratStiffness()[i] * alignment.restLength()[i] * - u.transpose() * mandos::core::skew(RA.col(2)); + mstate.m_grad[iA].segment<3>(3) += stiffness * L0 * u.transpose() * mandos::core::skew(R.col(2)); mstate.m_grad[iB].segment<3>(0) -= grad; } @@ -134,19 +163,20 @@ Scalar computeEnergyGradientAndHessian(const CosseratRodAlignment &alignment, Me // --------------------------------------------------------------- const Vec3 xA = mstate.m_x[iA].segment<3>(0); const Vec3 xB = mstate.m_x[iB].segment<3>(0); - const Mat3 RA = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); + const Mat3 R = mandos::core::rotationExpMap(mstate.m_x[iA].segment<3>(3)); - const auto oneOverL = mandos::core::Scalar{1.0} / (xA - xB).norm(); - const auto u = ((xA - xB) * oneOverL).eval(); + const Scalar L0 = alignment.restLength()[i]; + const Scalar stiffness = alignment.cosseratStiffness()[i]; + + const auto oneOverL = mandos::core::Scalar{1.0} / (xB - xA).norm(); + const auto u = ((xB - xA) * oneOverL).eval(); const auto uut = (u * u.transpose()).eval(); - energy += -alignment.restLength()[i] * alignment.cosseratStiffness()[i] * RA.col(2).dot(u); + energy += L0 * stiffness * (1.0 - R.col(2).dot(u)); - const Vec3 &grad = -alignment.cosseratStiffness()[i] * alignment.restLength()[i] * (Mat3::Identity() - uut) * - oneOverL * RA.col(2); + const Vec3 &grad = stiffness * L0 * oneOverL * (Mat3::Identity() - uut) * R.col(2); mstate.m_grad[iA].segment<3>(0) += grad; - mstate.m_grad[iA].segment<3>(3) += alignment.cosseratStiffness()[i] * alignment.restLength()[i] * - u.transpose() * mandos::core::skew(RA.col(2)); + mstate.m_grad[iA].segment<3>(3) += stiffness * L0 * u.transpose() * mandos::core::skew(R.col(2)); mstate.m_grad[iB].segment<3>(0) -= grad; @@ -154,30 +184,110 @@ Scalar computeEnergyGradientAndHessian(const CosseratRodAlignment &alignment, Me Mat6 hessianB = Mat6::Zero(); Mat6 hessianAB = Mat6::Zero(); - const Mat3 &du_dxa = (Mat3::Identity() - uut) * oneOverL; - const Vec3 &d3 = RA.col(2); + const Mat3 &du_dxa = -(Mat3::Identity() - uut) * oneOverL; + const Vec3 &d3 = R.col(2); const Mat3 &d2u_dxa2_d3 = oneOverL * oneOverL * (u.dot(d3) * (3 * uut - Mat3::Identity()) - (u * d3.transpose() + d3 * u.transpose())); - const Mat3 &hessEp_dx2 = -alignment.cosseratStiffness()[i] * alignment.restLength()[i] * d2u_dxa2_d3; - const Mat3 &uRAz = mandos::core::skew(u) * mandos::core::skew(RA.col(2)); - const Mat3 &hessEp_dthetaA2 = - -0.5 * alignment.cosseratStiffness()[i] * alignment.restLength()[i] * (uRAz + uRAz.transpose()); - const Mat3 &hessEp_dxdthetaA = - alignment.cosseratStiffness()[i] * alignment.restLength()[i] * du_dxa * mandos::core::skew(RA.col(2)); - const Mat3 &hessEp_dthetaAdx = - -alignment.cosseratStiffness()[i] * alignment.restLength()[i] * mandos::core::skew(RA.col(2)) * (-du_dxa); - - hessianA.block<3, 3>(0, 0) += hessEp_dx2; - hessianA.block<3, 3>(0, 3) += hessEp_dxdthetaA; - hessianA.block<3, 3>(3, 0) += hessEp_dxdthetaA.transpose(); - hessianA.block<3, 3>(3, 3) += hessEp_dthetaA2; - - hessianB.block<3, 3>(0, 0) += hessEp_dx2; - - hessianAB.block<3, 3>(0, 0) -= hessEp_dx2; - hessianAB.block<3, 3>(3, 0) += hessEp_dthetaAdx; + const Mat3 &hessEp_dx2 = -stiffness * L0 * d2u_dxa2_d3; + const Mat3 &uRAz = mandos::core::skew(u) * mandos::core::skew(R.col(2)); + const Mat3 &hessEp_dtheta2 = -0.5 * stiffness * L0 * (uRAz + uRAz.transpose()); + const Mat3 &hessEp_dxAdtheta = stiffness * L0 * du_dxa * mandos::core::skew(R.col(2)); + + hessianA.block<3, 3>(0, 0) += hessEp_dx2; // xA2 + hessianA.block<3, 3>(0, 3) += hessEp_dxAdtheta; // xA theta + hessianA.block<3, 3>(3, 0) += hessEp_dxAdtheta.transpose(); // theta xA + hessianA.block<3, 3>(3, 3) += hessEp_dtheta2; // theta2 + + hessianB.block<3, 3>(0, 0) += hessEp_dx2; // xB2 + + hessianAB.block<3, 3>(0, 0) -= hessEp_dx2; // xA xB + hessianAB.block<3, 3>(3, 0) += -hessEp_dxAdtheta.transpose(); // theta xB + + // Construct Sparse hessian + // --------------------------------------------------------------- + for (auto k = 0; k < 6; k++) { + for (auto j = 0; j < 6; j++) { + triplets.emplace_back( + 6 * static_cast(iA) + k, 6 * static_cast(iA) + j, hessianA(k, j)); + triplets.emplace_back( + 6 * static_cast(iA) + k, 6 * static_cast(iB) + j, hessianAB(k, j)); + triplets.emplace_back( + 6 * static_cast(iB) + k, 6 * static_cast(iA) + j, hessianAB(j, k)); + triplets.emplace_back( + 6 * static_cast(iB) + k, 6 * static_cast(iB) + j, hessianB(k, j)); + } + } + } + + SparseMat energyHessian; + energyHessian.resize(mstate.size(), mstate.size()); + energyHessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += energyHessian; + + return energy; +} + +Scalar computeEnergy(const CosseratRodAlignment &alignment, const MechanicalState &mstate) +{ + Scalar energy{0}; + for (auto i = 0UL; i < static_cast(alignment.size()); ++i) { + const auto iA = static_cast(alignment.indices()[i][0]); + const auto iB = static_cast(alignment.indices()[i][1]); + + energy += computeEnergyAD(mstate, iA, iB, alignment.restLength()[i], alignment.cosseratStiffness()[i]).val; + } + return energy; +} + +Scalar computeEnergyAndGradient(const CosseratRodAlignment &alignment, MechanicalState &mstate) +{ + Scalar energy{0}; + for (auto i = 0UL; i < static_cast(alignment.size()); ++i) { + const auto iA = static_cast(alignment.indices()[i][0]); + const auto iB = static_cast(alignment.indices()[i][1]); + + const auto e = computeEnergyAD(mstate, iA, iB, alignment.restLength()[i], alignment.cosseratStiffness()[i]); + energy += e.val; + + mstate.m_grad[iA].segment<3>(0) += e.grad.segment<3>(0); + mstate.m_grad[iA].segment<3>(3) += e.grad.segment<3>(6); + mstate.m_grad[iB].segment<3>(0) += e.grad.segment<3>(3); + } + return energy; +} + +Scalar computeEnergyGradientAndHessian(const CosseratRodAlignment &alignment, + MechanicalState &mstate) +{ + Scalar energy{0}; + std::vector triplets; + for (auto i = 0UL; i < static_cast(alignment.size()); ++i) { + const auto iA = static_cast(alignment.indices()[i][0]); + const auto iB = static_cast(alignment.indices()[i][1]); + + const auto e = computeEnergyAD(mstate, iA, iB, alignment.restLength()[i], alignment.cosseratStiffness()[i]); + + energy += e.val; + + mstate.m_grad[iA].segment<3>(0) += e.grad.segment<3>(0); + mstate.m_grad[iA].segment<3>(3) += e.grad.segment<3>(6); + mstate.m_grad[iB].segment<3>(0) += e.grad.segment<3>(3); + + Mat6 hessianA = Mat6::Zero(); + Mat6 hessianB = Mat6::Zero(); + Mat6 hessianAB = Mat6::Zero(); + + hessianA.block<3, 3>(0, 0) += e.Hess.block<3, 3>(0, 0); + hessianA.block<3, 3>(0, 3) += e.Hess.block<3, 3>(0, 6); + hessianA.block<3, 3>(3, 0) += e.Hess.block<3, 3>(6, 0); + hessianA.block<3, 3>(3, 3) += e.Hess.block<3, 3>(6, 6); + + hessianB.block<3, 3>(0, 0) += e.Hess.block<3, 3>(3, 3); + + hessianAB.block<3, 3>(0, 0) += e.Hess.block<3, 3>(0, 3); + hessianAB.block<3, 3>(3, 0) += e.Hess.block<3, 3>(6, 3); // REVIEW: maybe transposed? // Construct Sparse hessian // --------------------------------------------------------------- diff --git a/src/Mandos/Core/Energies/CosseratRodAlignment.hpp b/src/Mandos/Core/Energies/CosseratRodAlignment.hpp index 6d675ac07493f399b1bbc9f9ea72b193c2207fb3..61e7a9486688db15e2742ded4b13f44a7299892f 100644 --- a/src/Mandos/Core/Energies/CosseratRodAlignment.hpp +++ b/src/Mandos/Core/Energies/CosseratRodAlignment.hpp @@ -1,8 +1,9 @@ #ifndef MANDOS_ENERGIES_COSSERATRODALIGNMENT_H #define MANDOS_ENERGIES_COSSERATRODALIGNMENT_H +#include #include -#include "Mandos/Core/MechanicalState.hpp" +#include namespace mandos::core { @@ -25,33 +26,6 @@ public: */ int size() const; - /** - * @brief Computes the total potential energy for the Cosserat Rod given the current state of the - * MechanicalState - * - * @param mstate Current state - * @return Scalar Sum of the potential energy of all the spring elements - */ - Scalar computeEnergy(const MechanicalState &mstate) const; - - /** - * @brief Computes the potential energy and gradient for all the Cosserat Rod 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 Cosserat Rod elements - */ - Scalar computeEnergyAndGradient(MechanicalState &mstate) const; - - /** - * @brief Computes the potential energy, the gradient and the hessian for the Cosserat Rods 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 Cosserat Rod elements - */ - Scalar computeEnergyGradientAndHessian(MechanicalState &mstate) const; - void addElement(const std::array &indices, const ParameterSet ¶meterSet); ParameterSet getParameterSet(int elementId) const; @@ -80,6 +54,13 @@ Scalar MANDOS_CORE_EXPORT computeEnergyAndGradient(const CosseratRodAlignment &a Scalar MANDOS_CORE_EXPORT computeEnergyGradientAndHessian(const CosseratRodAlignment &alignment, MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergy(const CosseratRodAlignment &alignment, + const MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergyAndGradient(const CosseratRodAlignment &alignment, + MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergyGradientAndHessian(const CosseratRodAlignment &alignment, + MechanicalState &mstate); + } // namespace mandos::core #endif // MANDOS_ENERGIES_COSSERATRODALIGMENT_H diff --git a/src/Mandos/Core/Energies/Friction.cpp b/src/Mandos/Core/Energies/Friction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..59a27932f0551b18f3e0a5cb4b8d4dd0199ccb6c --- /dev/null +++ b/src/Mandos/Core/Energies/Friction.cpp @@ -0,0 +1,262 @@ +#include +#include + +namespace +{ +inline mandos::core::Scalar interpolationFunc(mandos::core::Scalar z, mandos::core::Scalar vMax) +{ + if (z <= vMax) { + return (-z * z * z / (3.0 * vMax * vMax)) + (z * z / vMax); + } + return z - (vMax / 3.0); +} + +inline mandos::core::Scalar interpolationFuncPrimeDiv(mandos::core::Scalar z, mandos::core::Scalar vMax) +{ + if (z <= vMax) { + return (-z / (vMax * vMax)) + (2.0 / vMax); + } + return 1.0 / z; +} + +[[maybe_unused]] inline mandos::core::Scalar interpolationFuncPrimeDivPrime(mandos::core::Scalar z, + mandos::core::Scalar vMax) +{ + if (z <= vMax) { + return (-1.0 / (vMax * vMax)); + } + return -1.0 / (z * z); +} + +inline mandos::core::Scalar computeCollisionDistance( + const mandos::core::MechanicalState &mstate, + const mandos::core::Vec3 &n, + size_t collisionId, + size_t nCollisions) +{ + const mandos::core::Vec3 xA0 = mstate.m_x0[collisionId]; + const mandos::core::Vec3 xB0 = mstate.m_x0[nCollisions + collisionId]; + return (n.dot(xB0 - xA0)); +} + +inline mandos::core::Scalar computeFrictionEnergy( + const mandos::core::MechanicalState &mstate, + mandos::core::Scalar coulombConst, + mandos::core::Scalar F, + mandos::core::Scalar h, + mandos::core::Scalar vMax, + const mandos::core::Vec3 &n, + size_t collisionId, + size_t nCollisions) + +{ + const mandos::core::Vec3 vA = (mstate.m_x[collisionId] - mstate.m_x0[collisionId]) / h; + const mandos::core::Vec3 vB = (mstate.m_x[nCollisions + collisionId] - mstate.m_x0[nCollisions + collisionId]) / h; + const mandos::core::Vec3 v = vA - vB; + const mandos::core::Vec3 vTangent = v - n * n.transpose() * v; + // IPC friction + return h * coulombConst * F * interpolationFunc(vTangent.norm(), vMax); + // Simple damping + // return 0.5 * h * coulombConst * vTangent.dot(vTangent); +} +inline mandos::core::Vec3 computeFrictionEnergyGradient( + const mandos::core::MechanicalState &mstate, + mandos::core::Scalar coulombConst, + mandos::core::Scalar F, + mandos::core::Scalar h, + mandos::core::Scalar vMax, + const mandos::core::Vec3 &n, + size_t collisionId, + size_t nCollisions) + +{ + const mandos::core::Vec3 vA = (mstate.m_x[collisionId] - mstate.m_x0[collisionId]) / h; + const mandos::core::Vec3 vB = (mstate.m_x[nCollisions + collisionId] - mstate.m_x0[nCollisions + collisionId]) / h; + const mandos::core::Vec3 v = vA - vB; + const mandos::core::Vec3 vTangent = v - n * n.transpose() * v; + // IPC friction + const mandos::core::Scalar vTanNorm = vTangent.norm(); + return coulombConst * F * interpolationFuncPrimeDiv(vTanNorm, vMax) * vTangent; + // return coulombConst * vTangent; // Simple damping +} +inline mandos::core::Mat3 computeFrictionEnergyHessian( + const mandos::core::MechanicalState &mstate, + mandos::core::Scalar coulombConst, + mandos::core::Scalar F, + mandos::core::Scalar h, + mandos::core::Scalar vMax, + const mandos::core::Vec3 &n, + size_t collisionId, + size_t nCollisions) + +{ + const mandos::core::Vec3 vA = (mstate.m_x[collisionId] - mstate.m_x0[collisionId]) / h; + const mandos::core::Vec3 vB = (mstate.m_x[nCollisions + collisionId] - mstate.m_x0[nCollisions + collisionId]) / h; + const mandos::core::Vec3 v = vA - vB; + const mandos::core::Mat3 TanProj = (mandos::core::Mat3::Identity() - n * n.transpose()); + // return 1.0 / h * coulombConst * TanProj; // Simple Damping + + const mandos::core::Vec3 vTangent = TanProj * v; + const mandos::core::Scalar vTanNorm = vTangent.norm(); + const mandos::core::Scalar lambdaB = 1.0 / h * coulombConst * F * interpolationFuncPrimeDiv(vTanNorm, vMax); + return lambdaB * TanProj; // Hessian Approximation + + // const mandos::core::Scalar oneOverVTanNorm = + // (vTanNorm < std::numeric_limits::min()) ? 0.0 : 1.0 / vTanNorm; + // return 1.0 / h * coulombConst * F * + // (interpolationFuncPrimeDivPrime(vTanNorm, vMax) * oneOverVTanNorm * vTangent * vTangent.transpose() + + // interpolationFuncPrimeDiv(vTanNorm, vMax) * TanProj); + + // const mandos::core::Vec3 b = vTangent.cross(n) / vTanNorm; + // const mandos::core::Scalar lambdaVt = [&]() -> mandos::core::Scalar { + // if (vTanNorm <= vMax) { + // return 1.0 / h * coulombConst * F * interpolationFuncPrimePrime(vTanNorm, vMax); + // } + // return lambdaB; + // }(); + + // return lambdaVt * vTangent * vTangent.transpose() / (vTanNorm * vTanNorm) + lambdaB * b * b.transpose(); +} + +} // namespace + +namespace mandos::core +{ + +int Friction::size() const +{ + return static_cast(m_coulombConst.size()); +} + +Scalar Friction::computeEnergy(const MechanicalState &mstate, Scalar h) const +{ + ZoneScopedN("Friction.computeEnergy"); + const auto nCollisions{m_coulombConst.size()}; + Scalar energy{0}; + for (unsigned int i = 0; i < nCollisions; ++i) { + if (computeCollisionDistance(mstate, m_normal[i], i, nCollisions) >= m_threshold[i]) { + // No friction if there is no collision + continue; + } + energy += computeFrictionEnergy( + mstate, m_coulombConst[i], m_laggedForce[i], h, m_vMax[i], m_normal[i], i, nCollisions); + } + return energy; +} + +Scalar Friction::computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const +{ + ZoneScopedN("Friction.computeEnergyAndGradient"); + const auto nCollisions{m_coulombConst.size()}; + if (nCollisions == 0) { + return 0; + } + Scalar energy{0}; + for (unsigned int i = 0; i < nCollisions; ++i) { + if (computeCollisionDistance(mstate, m_normal[i], i, nCollisions) >= m_threshold[i]) { + // No friction if there is no collision + continue; + } + energy += computeFrictionEnergy( + mstate, m_coulombConst[i], m_laggedForce[i], h, m_vMax[i], m_normal[i], i, nCollisions); + const Vec3 grad = computeFrictionEnergyGradient( + mstate, m_coulombConst[i], m_laggedForce[i], h, m_vMax[i], m_normal[i], i, nCollisions); + + // Set gradients to global structure + mstate.m_grad[i] += grad; + mstate.m_grad[nCollisions + i] += -grad; + } + return energy; +} + +Scalar Friction::computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const +{ + ZoneScopedN("Friction.computeEnergyGradientAndHessian"); + const auto nCollisions{m_coulombConst.size()}; + if (nCollisions == 0) { + return 0; + } + std::vector triplets; + Scalar energy{0}; + for (unsigned int i = 0; i < nCollisions; ++i) { + if (computeCollisionDistance(mstate, m_normal[i], i, nCollisions) >= m_threshold[i]) { + // No friction if there is no collision + continue; + } + // Scalar Fnew = mstate.m_grad[i].norm(); + energy += computeFrictionEnergy( + mstate, m_coulombConst[i], m_laggedForce[i], h, m_vMax[i], m_normal[i], i, nCollisions); + const Vec3 grad = computeFrictionEnergyGradient( + mstate, m_coulombConst[i], m_laggedForce[i], h, m_vMax[i], m_normal[i], i, nCollisions); + + const Mat3 hess = computeFrictionEnergyHessian( + mstate, m_coulombConst[i], m_laggedForce[i], h, m_vMax[i], m_normal[i], i, nCollisions); + + // Set gradients to global structure + mstate.m_grad[i] += grad; + mstate.m_grad[nCollisions + i] += -grad; + + // Hessian triplets + for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int k = 0; k < 3; ++k) { + triplets.emplace_back(3 * i + j, 3 * i + k, hess(j, k)); // AA + triplets.emplace_back(3 * i + j, 3 * (nCollisions + i) + k, -hess(j, k)); // AB + triplets.emplace_back(3 * (nCollisions + i) + j, 3 * i + k, -hess(j, k)); // BA + triplets.emplace_back(3 * (nCollisions + i) + j, 3 * (nCollisions + i) + k, hess(j, k)); // BB + } + } + } + { + ZoneScopedN("Friction.globalAssembly"); + SparseMat hessian; + hessian.resize(mstate.size(), mstate.size()); + hessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += hessian; + } + return energy; +} + +void Friction::computeLaggedPositionHessian(MechanicalState &mstate, Scalar h) const +{ + std::vector triplets; + const auto nCollisions{m_coulombConst.size()}; + for (unsigned int i = 0; i < nCollisions; ++i) { + if (computeCollisionDistance(mstate, m_normal[i], i, nCollisions) >= m_threshold[i]) { + // No friction if there is no collision + continue; + } + const Vec3 &n = m_normal[i]; + const Mat3 hess = -m_coulombConst[i] / h * (Mat3::Identity() - n * n.transpose()); + // Hessian triplets + for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int k = 0; k < 3; ++k) { + triplets.emplace_back(3 * i + j, 3 * i + k, hess(j, k)); // AA + triplets.emplace_back(3 * i + j, 3 * (nCollisions + i) + k, -hess(j, k)); // AB + triplets.emplace_back(3 * (nCollisions + i) + j, 3 * i + k, -hess(j, k)); // BA + triplets.emplace_back(3 * (nCollisions + i) + j, 3 * (nCollisions + i) + k, hess(j, k)); // BB + } + } + } + SparseMat hess(mstate.size(), mstate.size()); + hess.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += hess; +} + +void Friction::addElement(Scalar coulombConst, Scalar laggedForce, Scalar vMax, Scalar threshold, const Vec3 &normal) +{ + m_coulombConst.push_back(coulombConst); + m_laggedForce.push_back(laggedForce); + m_vMax.push_back(vMax); + m_threshold.push_back(threshold); + m_normal.push_back(normal); +} + +void Friction::clear() +{ + m_coulombConst.clear(); + m_laggedForce.clear(); + m_vMax.clear(); + m_normal.clear(); + m_threshold.clear(); +} +} // namespace mandos::core diff --git a/src/Mandos/Core/Energies/Friction.hpp b/src/Mandos/Core/Energies/Friction.hpp new file mode 100644 index 0000000000000000000000000000000000000000..7bf34afc606a9be62e541e32dd1a8e86ff176804 --- /dev/null +++ b/src/Mandos/Core/Energies/Friction.hpp @@ -0,0 +1,68 @@ +#ifndef MANDOS_ENERGIES_FRICTION_H_ +#define MANDOS_ENERGIES_FRICTION_H_ + +#include + +namespace mandos::core +{ +class Friction +{ +public: + /** + * @brief Number of energy elements + * + * @return int + */ + int size() const; + + /** + * @brief Computes the total potential energy for the spring elements given the current state of the + * MechanicalState + * + * @param mstate Current state + * @return Scalar Sum of the potential energy of all the spring elements + */ + Scalar computeEnergy(const MechanicalState &mstate, Scalar h) const; + + /** + * @brief Computes the potential energy and gradient for the spring 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 spring elements + */ + Scalar computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const; + + /** + * @brief Computes the potential energy, the gradient and the hessian for the spring 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 spring elements + */ + Scalar computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const; + + void computeLaggedPositionHessian(MechanicalState &mstate, Scalar h) const; + + /** + * @brief Add a new spring element. The particles involved in the spring element are implicit. Each pair of + * particles in the MechanicalState will have a spring between them with rest length the set threshold + * + * @param stiffness Spring stiffness + * @param threshold Threshold distance to start considering the contact as active + * @param normal Direction to push the particle away from each other + */ + void addElement(Scalar coulombConst, Scalar laggedForce, Scalar vMax, Scalar threshold, const Vec3 &normal); + + void clear(); + +private: + std::vector m_coulombConst; + std::vector m_laggedForce; + std::vector m_threshold; + std::vector m_vMax; + std::vector m_normal; +}; +} // namespace mandos::core + +#endif // MANDOS_ENERGIES_FRICTION_H_ diff --git a/src/Mandos/Core/Energies/MassSpring.cpp b/src/Mandos/Core/Energies/MassSpring.cpp index 61776aa1fdba3df02f098784b17a98e8c9ccdcd0..dbb64b04a828c11f42393f4bcb129ca18ba893ba 100644 --- a/src/Mandos/Core/Energies/MassSpring.cpp +++ b/src/Mandos/Core/Energies/MassSpring.cpp @@ -318,4 +318,68 @@ Scalar computeEnergyGradientAndHessian(const MassSpring &ms, MechanicalState &mstate) +{ + Scalar energy{0}; + for (auto i = 0UL; i < static_cast(ms.size()); ++i) { + const Vec3 xA = mstate.m_x[static_cast(ms.indices()[i][0])].segment<3>(0); + const Vec3 xB = mstate.m_x[static_cast(ms.indices()[i][1])].segment<3>(0); + + energy += ::computeEnergy(xA, xB, ms.stiffness()[i], ms.restLength()[i], ms.normalizeStiffness()); + } + return energy; +} + +Scalar computeEnergyAndGradient(const MassSpring &ms, MechanicalState &mstate) +{ + Scalar energy{0}; + for (auto i = 0UL; i < static_cast(ms.size()); ++i) { + const auto &indices{ms.indices()[i]}; + const Vec3 xA = mstate.m_x[static_cast(indices[0])].segment<3>(0); + const Vec3 xB = mstate.m_x[static_cast(indices[1])].segment<3>(0); + + const auto [lenergy, grad] = + ::computeEnergyAndGradient(xA, xB, ms.stiffness()[i], ms.restLength()[i], ms.normalizeStiffness()); + energy += lenergy; + + mstate.m_grad[static_cast(indices[0])].segment<3>(0) += grad; + mstate.m_grad[static_cast(indices[1])].segment<3>(0) -= grad; + } + return energy; +} + +Scalar computeEnergyGradientAndHessian(const MassSpring &ms, MechanicalState &mstate) +{ + Scalar energy{0}; + std::vector triplets; + for (auto i = 0UL; i < static_cast(ms.size()); ++i) { + const auto &indices{ms.indices()[i]}; + const Vec3 xA = mstate.m_x[static_cast(indices[0])].segment<3>(0); + const Vec3 xB = mstate.m_x[static_cast(indices[1])].segment<3>(0); + + const auto [lenergy, grad, hessian] = + ::computeEnergyGradientAndHessian(xA, xB, ms.stiffness()[i], ms.restLength()[i], ms.normalizeStiffness()); + energy += lenergy; + + mstate.m_grad[static_cast(indices[0])].segment<3>(0) += grad; + mstate.m_grad[static_cast(indices[1])].segment<3>(0) -= grad; + + for (auto j = 0; j < 3; ++j) { + for (auto k = 0; k < 3; ++k) { + triplets.emplace_back(6 * indices[0] + j, 6 * indices[0] + k, hessian(j, k)); + triplets.emplace_back(6 * indices[0] + j, 6 * indices[1] + k, -hessian(j, k)); + triplets.emplace_back(6 * indices[1] + j, 6 * indices[0] + k, -hessian(j, k)); + triplets.emplace_back(6 * indices[1] + j, 6 * indices[1] + k, hessian(j, k)); + } + } + } + + SparseMat energyHessian; + energyHessian.resize(mstate.size(), mstate.size()); + energyHessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += energyHessian; + + return energy; +} + } // namespace mandos::core diff --git a/src/Mandos/Core/Energies/MassSpring.hpp b/src/Mandos/Core/Energies/MassSpring.hpp index be0fa4a8e88ce20e2d4203ac6323006d9e680597..a017a1aba492d6119480e21762d01a29f5d1e11a 100644 --- a/src/Mandos/Core/Energies/MassSpring.hpp +++ b/src/Mandos/Core/Energies/MassSpring.hpp @@ -3,6 +3,7 @@ #include #include +#include #include @@ -71,6 +72,11 @@ Scalar MANDOS_CORE_EXPORT computeEnergy(const MassSpring &ms, const MechanicalSt Scalar MANDOS_CORE_EXPORT computeEnergyAndGradient(const MassSpring &ms, MechanicalState &mstate); Scalar MANDOS_CORE_EXPORT computeEnergyGradientAndHessian(const MassSpring &ms, MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergy(const MassSpring &ms, const MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergyAndGradient(const MassSpring &ms, MechanicalState &mstate); +Scalar MANDOS_CORE_EXPORT computeEnergyGradientAndHessian(const MassSpring &ms, + MechanicalState &mstate); + template <> Scalar MassSpring::computeEnergyGradientParameterDerivative( MechanicalState &mstate, diff --git a/src/Mandos/Core/Energies/PlaneField.cpp b/src/Mandos/Core/Energies/PlaneField.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2f307df4a54463e2bd162d162eb749422e88e85f --- /dev/null +++ b/src/Mandos/Core/Energies/PlaneField.cpp @@ -0,0 +1,241 @@ +#include + +#include + +#include "Mandos/Core/linear_algebra.hpp" + +#include + +namespace +{ + +// template +// inline Eigen::Matrix3 rotationExpMapLinear(const Eigen::MatrixBase &theta) +// { +// return mandos::core::Mat3::Identity() + mandos::core::skew(theta); +// } + +[[maybe_unused]] mandos::core::Scalar computeContactEnergy(const mandos::core::Scalar stiffness, + const mandos::core::Scalar d) +{ + return (0.5 * stiffness * d * d); +} +[[maybe_unused]] mandos::core::Vec3 computeContactEnergyGradient(const mandos::core::Scalar stiffness, + const mandos::core::Scalar d, + const mandos::core::Vec3 &normal) +{ + return stiffness * d * normal; +} +[[maybe_unused]] mandos::core::Mat3 computeContactEnergyHessian(const mandos::core::Scalar stiffness, + const mandos::core::Vec3 &normal) +{ + return stiffness * normal * normal.transpose(); +} +[[maybe_unused]] Eigen::Matrix rigidPointJacobian(const mandos::core::Mat3 &R, + const mandos::core::Vec3 &localPoint) +{ + Eigen::Matrix J; + J.block<3, 3>(0, 0) = mandos::core::Mat3::Identity(); + J.block<3, 3>(0, 3) = -mandos::core::skew(R * localPoint); + return J; +} +[[maybe_unused]] mandos::core::Mat6 rigidPointJacobianPrime(const mandos::core::Mat3 &R, + const mandos::core::Vec3 &localPoint, + const mandos::core::Vec3 &v) +{ + using mandos::core::skew; + mandos::core::Mat6 J = mandos::core::Mat6::Zero(); + J.block<3, 3>(3, 3) = 0.5 * (skew(R * localPoint) * skew(v) + skew(v) * skew(R * localPoint)); + return J; +} + +auto computePlaneFieldEnergy(const mandos::core::Vec6 &x, + const mandos::core::Vec6 &x0, + mandos::core::Scalar h, + const mandos::core::PlaneField::ParameterSet ¶meterSet) +{ + using mandos::core::Mat3; + using mandos::core::Vec3; + const Vec3 com = x.segment<3>(0); + const Vec3 com0 = x0.segment<3>(0); + + const Mat3 R = mandos::core::rotationExpMap(x.segment<3>(3)); + const Mat3 R0 = mandos::core::rotationExpMap(x0.segment<3>(3)); + + using ADouble = TinyAD::Double<6>; + const Eigen::Vector3 comAD = + Eigen::Vector3(ADouble(com.x(), 0), ADouble(com.y(), 1), ADouble(com.z(), 2)); + const Eigen::Vector3 thetaAD = Eigen::Vector3(ADouble(0.0, 3), ADouble(0.0, 4), ADouble(0.0, 5)); + const Eigen::Matrix3 R_AD = mandos::core::rotationExpMap(thetaAD) * R; + + const Mat3 tanProj = Mat3::Identity() - parameterSet.normal * parameterSet.normal.transpose(); + + ADouble energy{0}; + for (const auto &v : parameterSet.mesh.vertices) { + const Eigen::Vector3 p = R_AD * v + comAD; + const Vec3 p0 = R0 * v + com0; // p at beggining of step + + const ADouble d = (p - parameterSet.point).dot(parameterSet.normal); + const mandos::core::Scalar d0 = (p0 - parameterSet.point).dot(parameterSet.normal); + if (d0 < mandos::core::Scalar{0}) { + const Eigen::Vector3 vT = tanProj * (p - p0) / h; + const ADouble contact = 0.5 * parameterSet.stiffness * d * d; + const ADouble friction = 0.5 * h * parameterSet.coulombConst * vT.squaredNorm() * parameterSet.stiffness; + energy += contact + friction; + } + } + return energy; +} + +} // namespace + +namespace mandos::core +{ +void PlaneField::setParameterSet(const ParameterSet ¶meterSet) +{ + m_parameterSet = parameterSet; +} + +PlaneField::ParameterSet PlaneField::getParameterSet() const +{ + return m_parameterSet; +} + +// for (const auto &v : m_parameterSet.mesh.vertices) { +// const Scalar d = ((com + R * v) - m_parameterSet.point).dot(m_parameterSet.normal); +// if (d < mandos::core::Scalar{0}) { +// Eigen::Matrix J = rigidPointJacobian(R, v); +// energy += computeContactEnergy(m_parameterSet.stiffness, d); +// const Vec3 grad3 = computeContactEnergyGradient(m_parameterSet.stiffness, d, m_parameterSet.normal); +// const Mat3 hess3 = computeContactEnergyHessian(m_parameterSet.stiffness, m_parameterSet.normal); +// mstate.m_grad[0] += J.transpose() * grad3; +// hessian += J.transpose() * hess3 * J + rigidPointJacobianPrime(R, v, grad3); +// } +// } +Scalar PlaneField::computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const +{ + const auto energy = computePlaneFieldEnergy(mstate.m_x[0], mstate.m_x0[0], h, m_parameterSet); + mstate.m_grad[0] += energy.grad; + + std::vector triplets; + for (auto i = 0; i < 6; ++i) { + for (auto j = 0; j < 6; ++j) { + triplets.emplace_back(i, j, energy.Hess(i, j)); + } + } + + SparseMat energyHessian; + energyHessian.resize(mstate.size(), mstate.size()); + energyHessian.setFromTriplets(triplets.begin(), triplets.end()); + mstate.m_hessian += energyHessian; + + return energy.val; +} + +Scalar PlaneField::computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const +{ + const auto energy = computePlaneFieldEnergy(mstate.m_x[0], mstate.m_x0[0], h, m_parameterSet); + mstate.m_grad[0] += energy.grad; + + return energy.val; +} + +Scalar PlaneField::computeEnergy(const MechanicalState &mstate, Scalar h) const +{ + const auto energy = computePlaneFieldEnergy(mstate.m_x[0], mstate.m_x0[0], h, m_parameterSet); + + return energy.val; +} +PlaneField::ParameterSet::ParameterSet(const Vec3 &p, + const Vec3 &normal_, + Scalar stiffness_, + Scalar coulombConst_, + const SurfaceMesh &mesh_) + : point(p) + , normal(normal_.normalized()) + , stiffness(stiffness_) + , coulombConst(coulombConst_) + , mesh(mesh_) + +{ +} + +void PlaneField::computeLaggedPositionHessian(MechanicalState & /*mstate*/, Scalar /*h*/) const +{ + if (!m_parameterSet.mesh.vertices.empty()) { + throw std::runtime_error("TODO: Not implemented"); + } +} + +std::vector PlaneField::getWorldPositions(const Vec6 &x) +{ + const Vec3 com = x.segment<3>(0); + const Mat3 R = rotationExpMap(x.segment<3>(3)); + std::vector positions; + for (const auto &v : m_parameterSet.mesh.vertices) { + const Vec3 pos = R * v + com; + positions.push_back(pos); + } + return positions; +} + +std::vector PlaneField::getWorldVelocities(const Vec6 &x, const Vec6 &v, Scalar h) +{ + const Vec3 com = x.segment<3>(0); + const Vec3 vCom = v.segment<3>(0); + const Vec3 com0 = com - h * vCom; + + const Vec3 omega = v.segment<3>(3); + const Mat3 R = rotationExpMap(x.segment<3>(3)); + const Mat3 R0 = rotationExpMap(h * omega).transpose() * R; + std::vector velocities; + for (const auto &r : m_parameterSet.mesh.vertices) { + const Vec3 p = R * r + com; + const Vec3 p0 = R0 * r + com0; // p at beggining of step + const Vec3 vel = (p - p0) / h; + velocities.push_back(vel); + } + return velocities; +} + +std::vector PlaneField::computeFrictionForces(const MechanicalState &mstate, Scalar h) +{ + using mandos::core::Mat3; + using mandos::core::Vec3; + const Vec3 com = mstate.m_x[0].segment<3>(0); + const Vec3 com0 = mstate.m_x0[0].segment<3>(0); + + const Mat3 R = mandos::core::rotationExpMap(mstate.m_x[0].segment<3>(3)); + const Mat3 R0 = mandos::core::rotationExpMap(mstate.m_x0[0].segment<3>(3)); + + const Mat3 tanProj = Mat3::Identity() - m_parameterSet.normal * m_parameterSet.normal.transpose(); + + std::vector forces; + for (const auto &v : m_parameterSet.mesh.vertices) { + const Vec3 p = R * v + com; + const Vec3 p0 = R0 * v + com0; // p at beggining of step + + const Scalar d = (p - m_parameterSet.point).dot(m_parameterSet.normal); + if (d < mandos::core::Scalar{0}) { + const Vec3 vT = tanProj * (p - p0) / h; + const Vec3 friction = -m_parameterSet.coulombConst * vT * d + + 0.5 * h * m_parameterSet.coulombConst * vT.squaredNorm() * m_parameterSet.normal; + ; + forces.emplace_back(-friction); + } else { + forces.emplace_back(Vec3::Zero()); + } + } + + return forces; +} + +Vec6 PlaneField::computeFrictionRBForces(const MechanicalState &mstate, Scalar h) +{ + ParameterSet ps = m_parameterSet; + ps.stiffness = 0.0; + const auto energy = computePlaneFieldEnergy(mstate.m_x[0], mstate.m_x0[0], h, ps); + return -energy.grad; +} + +} // namespace mandos::core diff --git a/src/Mandos/Core/Energies/PlaneField.hpp b/src/Mandos/Core/Energies/PlaneField.hpp new file mode 100644 index 0000000000000000000000000000000000000000..a63c600218fd13e384024c3ba1f453c427632014 --- /dev/null +++ b/src/Mandos/Core/Energies/PlaneField.hpp @@ -0,0 +1,89 @@ +#ifndef MANDOS_ENERGIES_PLANEFIELD_H +#define MANDOS_ENERGIES_PLANEFIELD_H + +#include + +#include + +#include + +#include + +namespace mandos::core +{ +class MANDOS_CORE_EXPORT PlaneField +{ +public: + struct MANDOS_CORE_EXPORT ParameterSet { + ParameterSet() = default; + ParameterSet(const mandos::core::Vec3 &p, + const mandos::core::Vec3 &normal, + mandos::core::Scalar stiffness, + mandos::core::Scalar coulombConst, + const mandos::core::SurfaceMesh &mesh); + + mandos::core::Vec3 point{mandos::core::Vec3::Zero()}; + mandos::core::Vec3 normal{mandos::core::Vec3::Zero()}; + mandos::core::Scalar stiffness{0}; + mandos::core::Scalar coulombConst{0}; + + mandos::core::SurfaceMesh mesh{}; + }; + + /** + * @brief Number of energy elements + * + * @return int + */ + constexpr static int size() + { + return 1; + } + + /** + * @brief Computes the contact potential energy due to penetration of the rigid body into the plane + * + * @param mstate Current mechanical state of the rigid body + * @param h Time step (not used in energy itself but may relate to time-dependent logic) + * @return Scalar Contact potential energy (zero if no penetration) + */ + Scalar computeEnergy(const MechanicalState &mstate, Scalar h) const; + + /** + * @brief Computes contact potential energy and its gradient (force) with respect to the rigid body state + * + * @param mstate Current mechanical state (gradient will be accumulated here) + * @param h Time step + * @return Scalar Contact potential energy + */ + Scalar computeEnergyAndGradient(MechanicalState &mstate, Scalar h) const; + + /** + * @brief Computes energy, gradient (force), and Hessian (stiffness matrix) for the plane contact + * + * @param mstate Current mechanical state (gradient and Hessian will be accumulated) + * @param h Time step + * @return Scalar Contact potential energy + */ + Scalar computeEnergyGradientAndHessian(MechanicalState &mstate, Scalar h) const; + + void computeLaggedPositionHessian(MechanicalState &mstate, Scalar h) const; + + ParameterSet getParameterSet() const; + + void setParameterSet(const ParameterSet ¶meterSet); + + std::vector getWorldPositions(const Vec6 &x); + std::vector getWorldVelocities(const Vec6 &x, const Vec6 &v, Scalar h); + + std::vector computeFrictionForces(const MechanicalState &mstate, Scalar h); + Vec6 computeFrictionRBForces(const MechanicalState &mstate, Scalar h); + +private: + TinyAD::Double<6> computeEnergyAD(const MechanicalState &mstate, Scalar /*h*/) const; + ParameterSet m_parameterSet; +}; + +} // namespace mandos::core + +#endif // MANDOS_ENERGIES_SPRINGS_H diff --git a/src/Mandos/Core/Mappings.hpp b/src/Mandos/Core/Mappings.hpp index 26d2abba1dbd216058e323b628b5d09d71f69533..fc80853ea0b33cc9eb27d314a3d0d182e2b5eb23 100644 --- a/src/Mandos/Core/Mappings.hpp +++ b/src/Mandos/Core/Mappings.hpp @@ -21,6 +21,7 @@ #include #include #include +#include "Mandos/Core/Mappings/RigidBodyGlobalPointMapping.hpp" namespace mandos::core { @@ -62,7 +63,9 @@ struct Mappings { template <> struct Mappings { - static constexpr bool HasMappings = false; + static constexpr bool HasMappings = true; + using types = utilities::typelist; + types::template map::template as m_mappings; }; } // namespace mandos::core diff --git a/src/Mandos/Core/Mappings/CollisionMapping.cpp b/src/Mandos/Core/Mappings/CollisionMapping.cpp index ed8d014d0eacd54c04aaf67d8a023f4d5a29825d..384d5ac099046ea01449ae9559f6014546a35042 100644 --- a/src/Mandos/Core/Mappings/CollisionMapping.cpp +++ b/src/Mandos/Core/Mappings/CollisionMapping.cpp @@ -29,48 +29,50 @@ void updateMapping(const collisions::ContactEventSide &contact, ZoneScopedN("updateMapping"); // Compute the current RB transform const auto &mstate = mapping.from()->mstate; - const Vec3 r = mandos::core::rotationExpMap(mstate.m_x[0].segment<3>(3)).transpose() * - (contact.contactPoint - mstate.m_x[0].segment<3>(0)); + const Vec6 &x = mstate.m_x[static_cast(contact.rbIndex)]; + const Vec3 r = mandos::core::rotationExpMap(x.segment<3>(3)).transpose() * (contact.contactPoint - x.segment<3>(0)); - const MappingInfo mappingInfo{ - r, mapping.from()->mstate, static_cast(collisionParticle)}; + const MappingInfo mappingInfo{.m_r = r, + .mstate = mapping.from()->mstate, + .m_rbIndex = static_cast(contact.rbIndex), + .m_toId = static_cast(collisionParticle)}; mapping.addMappingInfo(mappingInfo); } -void MappingInfo::apply(const std::vector &from, std::vector &to) const -{ - ZoneScopedN("MappingInfo.apply"); - - const Vec3 centerOfMass = from[0].segment<3>(0); - const Mat3 R = rotationExpMap(from[0].segment<3>(3)); - - to[m_toId] += R * m_r + centerOfMass; -} - void updateMapping(const collisions::ContactEventSide &contact, CollisionMapping &mapping, int collisionParticle) { ZoneScopedN("updateMapping"); - const MappingInfo mappingInfo{contact.indices, // - contact.barycentricCoordinates, - static_cast(collisionParticle)}; + const MappingInfo mappingInfo{.indices = contact.indices, // + .barycentric = contact.barycentricCoordinates, + .m_toId = static_cast(collisionParticle)}; mapping.addMappingInfo(mappingInfo); } +void MappingInfo::apply(const std::vector &from, std::vector &to) const +{ + ZoneScopedN("MappingInfo.apply"); + + const Vec3 centerOfMass = from[m_rbIndex].segment<3>(0); + const Mat3 R = rotationExpMap(from[m_rbIndex].segment<3>(3)); + + to[m_toId] = R * m_r + centerOfMass; +} + void MappingInfo::applyJ(const std::vector &from, std::vector &to) const { - const Mat3 R = rotationExpMap(mstate.m_x[0].segment<3>(3)); + const Mat3 R = rotationExpMap(mstate.m_x[m_rbIndex].segment<3>(3)); const Mat3 thetaJ = -skew(R * m_r); - to[m_toId] += from[0].segment<3>(0); - to[m_toId] += thetaJ * from[0].segment<3>(3); + to[m_toId] += from[m_rbIndex].segment<3>(0); + to[m_toId] += thetaJ * from[m_rbIndex].segment<3>(3); } void MappingInfo::applyJT(std::vector &from, const std::vector &to) const { - const Mat3 R = rotationExpMap(mstate.m_x[0].segment<3>(3)); + const Mat3 R = rotationExpMap(mstate.m_x[m_rbIndex].segment<3>(3)); const Mat3 thetaJT = skew(R * m_r); - from[0].segment<3>(0) += to[m_toId]; - from[0].segment<3>(3) += thetaJT * to[m_toId]; + from[m_rbIndex].segment<3>(0) += to[m_toId]; + from[m_rbIndex].segment<3>(3) += thetaJT * to[m_toId]; } void MappingInfo::apply(const std::vector &from, std::vector &to) const diff --git a/src/Mandos/Core/Mappings/RigidBodyGlobalPointMapping.cpp b/src/Mandos/Core/Mappings/RigidBodyGlobalPointMapping.cpp new file mode 100644 index 0000000000000000000000000000000000000000..248b5dff967530761ace6b622f2016236c7d01ef --- /dev/null +++ b/src/Mandos/Core/Mappings/RigidBodyGlobalPointMapping.cpp @@ -0,0 +1,92 @@ +#include +#include +#include + +namespace mandos::core +{ + +RigidBodyGlobalPointMapping::RigidBodyGlobalPointMapping(SimulationObjectHandle from, + SimulationObjectHandle to) + : m_from(from) + , m_to(to) +{ +} + +int RigidBodyGlobalPointMapping::size() const +{ + return static_cast(m_rigidBodyIndex.size()); +} + +void RigidBodyGlobalPointMapping::applyJT(std::vector &from, const std::vector &to) const +{ + assert(m_rigidBodyIndex.size() == to.size()); + for (unsigned int i = 0; i < m_rigidBodyIndex.size(); ++i) { + const auto iR = static_cast(m_rigidBodyIndex[i]); + const Vec3 &theta = m_from->mstate.m_x[iR].segment<3>(3); + const Mat3 R = rotationExpMap(theta); + const Mat3 thetaJT = computeLocalToGlobalAxisAngleJacobian(theta).transpose() * skew(R * m_localCoord[i]); + + from[iR].segment<3>(0) += to[i]; + from[iR].segment<3>(3) += thetaJT * to[i]; + } +} + +void RigidBodyGlobalPointMapping::applyJ(const std::vector &from, std::vector &to) const +{ + assert(m_rigidBodyIndex.size() == to.size()); + for (unsigned int i = 0; i < m_rigidBodyIndex.size(); ++i) { + const auto iR = static_cast(m_rigidBodyIndex[i]); + const Vec3 &theta = m_from->mstate.m_x[iR].segment<3>(3); + const Mat3 R = rotationExpMap(theta); + const Mat3 thetaJ = -skew(R * m_localCoord[i]) * computeLocalToGlobalAxisAngleJacobian(theta); + to[i] += from[iR].segment<3>(0) + thetaJ * from[iR].segment<3>(3); + } +} + +void RigidBodyGlobalPointMapping::apply(const std::vector &from, std::vector &to) const +{ + for (unsigned int i = 0; i < m_rigidBodyIndex.size(); ++i) { + const auto iR = static_cast(m_rigidBodyIndex[i]); + const Vec3 centerOfMass = from[iR].segment<3>(0); + const Mat3 R = rotationExpMap(from[iR].segment<3>(3)); + + to[i] += R * m_localCoord[i] + centerOfMass; + } +} + +SimulationObjectHandle RigidBodyGlobalPointMapping::to() const +{ + return m_to; +} + +SimulationObjectHandle RigidBodyGlobalPointMapping::from() const +{ + return m_from; +} + +SparseMat RigidBodyGlobalPointMapping::J() const +{ + SparseMat J = SparseMat(m_to->mstate.size(), m_from->mstate.size()); + std::vector triplets; + + for (unsigned int i = 0; i < m_rigidBodyIndex.size(); ++i) { + const auto iR = static_cast(m_rigidBodyIndex[i]); + const Mat3 R = rotationExpMap(m_from->mstate.m_x[iR].segment<3>(3)); + const Mat3 thetaJ = -skew(R * m_localCoord[i]); + for (unsigned int j = 0; j < 3; ++j) { + triplets.emplace_back(3 * i + j, 6 * iR + j, 1.0); // Identity + for (unsigned int k = 0; k < 3; ++k) { + triplets.emplace_back(3 * i + j, 6 * iR + 3 + k, thetaJ(j, k)); + } + } + } + J.setFromTriplets(triplets.begin(), triplets.end()); + return J; +} + +void RigidBodyGlobalPointMapping::addLocalPoint(const Vec3 &localPoint, int rigidBodyIndex) +{ + m_localCoord.push_back(localPoint); + m_rigidBodyIndex.push_back(rigidBodyIndex); +} +} // namespace mandos::core diff --git a/src/Mandos/Core/Mappings/RigidBodyGlobalPointMapping.hpp b/src/Mandos/Core/Mappings/RigidBodyGlobalPointMapping.hpp new file mode 100644 index 0000000000000000000000000000000000000000..fd0ab8bebe2dd7dba636a90ed703ac501084e50c --- /dev/null +++ b/src/Mandos/Core/Mappings/RigidBodyGlobalPointMapping.hpp @@ -0,0 +1,45 @@ +#ifndef MANDOS_MAPPINGS_RIGIDBODYGLOBAL_POINT_MAPPING_H +#define MANDOS_MAPPINGS_RIGIDBODYGLOBAL_POINT_MAPPING_H + +#include + +#include +#include +#include +#include + +#include + +namespace mandos::core +{ + +class MANDOS_CORE_EXPORT RigidBodyGlobalPointMapping +{ +public: + using From = SimulationObjectHandle; + using To = SimulationObjectHandle; + + RigidBodyGlobalPointMapping(SimulationObjectHandle from, + SimulationObjectHandle to); + + void apply(const std::vector &from, std::vector &to) const; + void applyJ(const std::vector &from, std::vector &to) const; + void applyJT(std::vector &from, const std::vector &to) const; + + SimulationObjectHandle from() const; + SimulationObjectHandle to() const; + + void addLocalPoint(const Vec3 &localPoint, int rigidBodyIndex); + + int size() const; + SparseMat J() const; + +private: + SimulationObjectHandle m_from; + SimulationObjectHandle m_to; + + std::vector m_localCoord; + std::vector m_rigidBodyIndex; +}; +} // namespace mandos::core +#endif // RIGIDBODYPOINTMAPPING_H_ diff --git a/src/Mandos/Core/Model.cpp b/src/Mandos/Core/Model.cpp index cbdd10fa3870acddd815b8090d7a8a7132153941..1327f33c4e8db4618cda0b99e2f1b66cb6ae0c0e 100644 --- a/src/Mandos/Core/Model.cpp +++ b/src/Mandos/Core/Model.cpp @@ -576,9 +576,24 @@ void Model::detectCollisions() collisionSpringEnergy.addElement(collisionPair.stiffness, collisionPair.threshold, contact.normal); } - collisionState.setZero(); mapping0.apply(c0.simulationObject().mstate.m_x, collisionState.m_x); mapping1.apply(c1.simulationObject().mstate.m_x, collisionState.m_x); + mapping0.apply(c0.simulationObject().mstate.m_x0, collisionState.m_x0); + mapping1.apply(c1.simulationObject().mstate.m_x0, collisionState.m_x0); + + // Set up Friction + auto &frictionEnergy = collisionPair.collisionState().template dissipativePotential(); + frictionEnergy.clear(); + // Compute Lagged Collision Force + mandos::core::computeEnergyAndGradient(collisionSpringEnergy, collisionState); + for (auto cId = 0UL; cId < contactEvents.size(); ++cId) { + const auto &contact = contactEvents[cId]; + const auto F = collisionState.m_grad[cId].norm(); + frictionEnergy.addElement( + collisionPair.coulombConst, F, collisionPair.vMax, collisionPair.threshold, contact.normal); + } + // Clear from lagged Collision Forces + collisionState.clearGradient(); } }, m_collisionPairs); diff --git a/tests/Colliders/tst_Colliders.cpp b/tests/Colliders/tst_Colliders.cpp index 87ea8a569e746193788df07dc5f5aa155b66dc3b..c8e7cd0bd7f32e4a0b6fa49ed8cf08232954be46 100644 --- a/tests/Colliders/tst_Colliders.cpp +++ b/tests/Colliders/tst_Colliders.cpp @@ -34,7 +34,7 @@ TEST_CASE("SDF") SECTION("SDF") { - const mandos::core::collisions::SDF sdfCollider(cube, 0.2, 32); + const mandos::core::collisions::SDF sdfCollider(cube, 0.2, 0, 32); REQUIRE(sdfCollider.vdb().isInside({0.9, 1.0, -1.0}) == true); REQUIRE(sdfCollider.vdb().isInside({1.9, 1.0, -1.0}) == false); @@ -45,7 +45,7 @@ TEST_CASE("SDF") SECTION("Flipped SDF") { - const mandos::core::collisions::SDF sdfCollider{cube, 0.2, 32, true}; + const mandos::core::collisions::SDF sdfCollider(cube, 0.2, 0, 32, true); REQUIRE(sdfCollider.vdb().isInside({0.9, 1.0, -1.0}) == false); REQUIRE(sdfCollider.vdb().isInside({1.9, 1.0, -1.0}) == true); diff --git a/tests/CosseratRod/tst_CosseratRod.cpp b/tests/CosseratRod/tst_CosseratRod.cpp index e704f23594f5ab16dccebeeabca04ab5572c12f9..1543618f25cc4df9127b0923550383e3a9c960aa 100644 --- a/tests/CosseratRod/tst_CosseratRod.cpp +++ b/tests/CosseratRod/tst_CosseratRod.cpp @@ -22,7 +22,7 @@ template inline Eigen::Matrix3 rotationExpMap0(const Eigen::Vector3 &w) { using mandos::core::skew; - return Eigen::Matrix3::Identity() + skew(w) + 0.5 * skew(w) * skew(w); + return Eigen::Matrix3::Identity() + skew(w) + (0.5 * skew(w) * skew(w)); } } // namespace @@ -45,7 +45,7 @@ TEST_CASE("COSSERAT ROD ENERGY DERIVATIVES") const mandos::core::Scalar L0 = 2.0; const mandos::core::Scalar Ks = 100.0; - const mandos::core::Scalar cosseratStiffness = 100.0; + const mandos::core::Scalar cosseratStiffness = 1.0; const mandos::core::Vec3 stiffnessTensor = 1.0 * mandos::core::Vec3::Ones(); mandos::core::MassSpring massSpringEnergy; @@ -101,7 +101,12 @@ TEST_CASE("COSSERAT ROD ENERGY DERIVATIVES") const auto cosseratRodAlignmentParameterSet = cosseratRodAlignmentEnergy.getParameterSet(static_cast(segmentId)); const mandos::core::Scalar elementL0 = massSpringParameterSet.restLength; - const mandos::core::Scalar elementKs = massSpringParameterSet.stiffness; + mandos::core::Scalar elementKs = massSpringParameterSet.stiffness; + + if (L0 > std::numeric_limits::epsilon()) { + elementKs = elementKs / L0; + } + const mandos::core::Scalar elementCosseratStiffness = cosseratRodAlignmentParameterSet.cosseratStiffness; // Degrees of freedom @@ -125,8 +130,8 @@ TEST_CASE("COSSERAT ROD ENERGY DERIVATIVES") const Eigen::Matrix3 RB = rotationExpMap0(thetaB) * mandos::core::rotationExpMap(thetaBvalue); // Stretch Energy - const T L = (xA - xB).norm(); - const T Vs = 0.5 * elementKs / elementL0 * (L - elementL0) * (L - elementL0); + const T L = (xB - xA).norm(); + const T Vs = 0.5 * elementKs * (L - elementL0) * (L - elementL0); // Bending Energy T Vb = 0; @@ -152,8 +157,8 @@ TEST_CASE("COSSERAT ROD ENERGY DERIVATIVES") } // Cosserat Constraint Energy - const Eigen::Vector3 u = ((xA - xB) / (xA - xB).norm()).eval(); - const T Vc = -elementL0 * elementCosseratStiffness * RA.col(2).dot(u); + const Eigen::Vector3 u = ((xB - xA) / L).eval(); + const T Vc = elementL0 * elementCosseratStiffness * (1.0 - RA.col(2).dot(u)); return Vs + Vb + Vc; });