diff --git a/Examples/FemurCut/FemurCutExample.cpp b/Examples/FemurCut/FemurCutExample.cpp index be222f4dae581660728c1e7cbc6b329d1bd05d9e..704074a4c31904e5fe0640c72eb970583390275f 100644 --- a/Examples/FemurCut/FemurCutExample.cpp +++ b/Examples/FemurCut/FemurCutExample.cpp @@ -56,7 +56,7 @@ makeRigidObj(const std::string& name) rbdModel->getConfig()->m_maxNumIterations = 8; rbdModel->getConfig()->m_velocityDamping = 1.0; rbdModel->getConfig()->m_angularVelocityDamping = 1.0; - rbdModel->getConfig()->m_maxNumConstraints = 20; + rbdModel->getConfig()->m_maxNumConstraints = 40; // Create the first rbd, plane floor imstkNew<RigidObject2> rigidObj(name); @@ -158,8 +158,8 @@ main() imstkNew<RigidObjectController> controller(rbdObj, hapticDeviceClient); { - controller->setLinearKd(1000.0 * 0.9); - controller->setLinearKs(100000.0 * 0.9); + controller->setLinearKd(1000.0); + controller->setLinearKs(100000.0); controller->setAngularKs(300000000.0); controller->setAngularKd(400000.0); controller->setForceScaling(0.001); @@ -187,13 +187,13 @@ main() ghostMesh->setRotation(controller->getRotation()); ghostMesh->updatePostTransformData(); ghostMesh->postModified(); - }); + }); imstkNew<SimulationManager> driver; driver->addModule(viewer); driver->addModule(sceneManager); driver->addModule(hapticManager); - driver->setDesiredDt(0.001); // Little over 1000ups + driver->setDesiredDt(0.001); // Exactly 1000ups { imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice()); diff --git a/Source/Common/Parallel/imstkParallelFor.h b/Source/Common/Parallel/imstkParallelFor.h index 3fa9b397cccfe374076bff5cf1133a5924696074..77b26c93e89a844f9e0c90d21f62234830ab2015 100644 --- a/Source/Common/Parallel/imstkParallelFor.h +++ b/Source/Common/Parallel/imstkParallelFor.h @@ -29,18 +29,32 @@ namespace ParallelUtils { /// /// \brief Execute a function in parallel over a range [beginIdx, endIdx) of indices +/// \param start index +/// \param end index +/// \param function to execute that takes index +/// \param if less than maxN falls back to normal for loop /// template<class IndexType, class Function> void -parallelFor(const IndexType beginIdx, const IndexType endIdx, Function&& function) +parallelFor(const IndexType beginIdx, const IndexType endIdx, Function&& function, const bool doParallel = true) { - tbb::parallel_for(tbb::blocked_range<IndexType>(beginIdx, endIdx), - [&](const tbb::blocked_range<IndexType>& r) { - for (IndexType i = r.begin(), iEnd = r.end(); i < iEnd; ++i) - { - function(i); - } - }); + if (doParallel) + { + tbb::parallel_for(tbb::blocked_range<IndexType>(beginIdx, endIdx), + [&](const tbb::blocked_range<IndexType>& r) { + for (IndexType i = r.begin(), iEnd = r.end(); i < iEnd; ++i) + { + function(i); + } + }); + } + else + { + for (IndexType i = beginIdx; i < endIdx; i++) + { + function(i); + } + } } /// @@ -48,9 +62,9 @@ parallelFor(const IndexType beginIdx, const IndexType endIdx, Function&& functio /// template<class IndexType, class Function> void -parallelFor(const IndexType endIdx, Function&& function) +parallelFor(const IndexType endIdx, Function&& function, const bool doParallel = true) { - parallelFor(IndexType(0), endIdx, std::forward<Function>(function)); + parallelFor(IndexType(0), endIdx, std::forward<Function>(function), doParallel); } /// diff --git a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp index 63bead3b526aef199c33492db94c71f9cbf48725..6baab4abac0e5c391415fc20fbe14102219676c8 100644 --- a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp +++ b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp @@ -125,70 +125,77 @@ LevelSetModel::evolve() return; } - // index, coordinates, force, forward/backward gradient magnitude, curvature - //std::vector<std::tuple<size_t, Vec3i, double, Vec2d, double>> nodeUpdates; - //nodeUpdates.reserve(m_nodesToUpdate.size()); + // Setup a map of 0 based index -> image sparse index m_nodesToUpdate to parallelize + std::vector<size_t> baseIndexToImageIndex; + baseIndexToImageIndex.reserve(m_nodesToUpdate.size()); + for (std::unordered_map<size_t, std::tuple<Vec3i, double>>::iterator iter = m_nodesToUpdate.begin(); iter != m_nodesToUpdate.end(); iter++) + { + baseIndexToImageIndex.push_back(iter->first); + } // Compute gradients - const double constantVel = m_config->m_constantVelocity; + const double constantVel = m_config->m_constantVelocity; + std::tuple<size_t, Vec3i, double, Vec2d> val; for (int j = 0; j < m_config->m_substeps; j++) { - noteUpdatePoolSize = 0; - for (std::unordered_map<size_t, std::tuple<Vec3i, double>>::iterator iter = m_nodesToUpdate.begin(); iter != m_nodesToUpdate.end(); iter++) - { - const size_t index = iter->first; - const Vec3i& coords = std::get<0>(iter->second); - const double f = std::get<1>(iter->second); + ParallelUtils::parallelFor(baseIndexToImageIndex.size(), [&](const size_t i) + { + std::tuple<size_t, Vec3i, double, Vec2d, double>& outputVal = m_nodeUpdatePool[i]; + const size_t& index = std::get<0>(outputVal) = baseIndexToImageIndex[i]; - // Gradients - const Vec3d gradPos = m_forwardGrad(Vec3d(coords[0], coords[1], coords[2])); - const Vec3d gradNeg = m_backwardGrad(Vec3d(coords[0], coords[1], coords[2])); + std::tuple<Vec3i, double>& inputVal = m_nodesToUpdate[index]; - Vec3d gradNegMax = gradNeg.cwiseMax(0.0); - Vec3d gradNegMin = gradNeg.cwiseMin(0.0); - Vec3d gradPosMax = gradPos.cwiseMax(0.0); - Vec3d gradPosMin = gradPos.cwiseMin(0.0); + const Vec3i& coords = std::get<1>(outputVal) = std::get<0>(inputVal); + std::get<2>(outputVal) = std::get<1>(inputVal); - // Square them - gradNegMax = gradNegMax.cwiseProduct(gradNegMax); - gradNegMin = gradNegMin.cwiseProduct(gradNegMin); - gradPosMax = gradPosMax.cwiseProduct(gradPosMax); - gradPosMin = gradPosMin.cwiseProduct(gradPosMin); + // Gradients + const Vec3d gradPos = m_forwardGrad(Vec3d(coords[0], coords[1], coords[2])); + const Vec3d gradNeg = m_backwardGrad(Vec3d(coords[0], coords[1], coords[2])); - const double posMag = - gradNegMax[0] + gradNegMax[1] + gradNegMax[2] + - gradPosMin[0] + gradPosMin[1] + gradPosMin[2]; + Vec3d gradNegMax = gradNeg.cwiseMax(0.0); + Vec3d gradNegMin = gradNeg.cwiseMin(0.0); + Vec3d gradPosMax = gradPos.cwiseMax(0.0); + Vec3d gradPosMin = gradPos.cwiseMin(0.0); - const double negMag = - gradNegMin[0] + gradNegMin[1] + gradNegMin[2] + - gradPosMax[0] + gradPosMax[1] + gradPosMax[2]; + // Square them + gradNegMax = gradNegMax.cwiseProduct(gradNegMax); + gradNegMin = gradNegMin.cwiseProduct(gradNegMin); + gradPosMax = gradPosMax.cwiseProduct(gradPosMax); + gradPosMin = gradPosMin.cwiseProduct(gradPosMin); - // Curvature - //const double kappa = m_curvature(Vec3d(coords[0], coords[1], coords[2])); + const double posMag = + gradNegMax[0] + gradNegMax[1] + gradNegMax[2] + + gradPosMin[0] + gradPosMin[1] + gradPosMin[2]; - m_nodeUpdatePool[noteUpdatePoolSize++] = std::tuple<size_t, Vec3i, double, Vec2d, double>(index, coords, f, Vec2d(negMag, posMag), 0.0); - } + const double negMag = + gradNegMin[0] + gradNegMin[1] + gradNegMin[2] + + gradPosMax[0] + gradPosMax[1] + gradPosMax[2]; - // Update levelset - for (size_t i = 0; i < noteUpdatePoolSize; i++) - //ParallelUtils::parallelFor(noteUpdatePoolSize, [&](const size_t& i) - { - const size_t index = std::get<0>(m_nodeUpdatePool[i]); - const double vel = std::get<2>(m_nodeUpdatePool[i]) + constantVel; - const Vec2d& g = std::get<3>(m_nodeUpdatePool[i]); - //const double kappa = std::get<4>(nodeUpdates[i]); + std::get<3>(outputVal) = Vec2d(negMag, posMag); - // If speed function positive use forward difference (posMag) - if (vel > 0.0) - { - imgPtr[index] += dt * (vel * std::sqrt(g[0]) /*+ kappa * k*/); - } - // If speed function negative use backward difference (negMag) - else if (vel < 0.0) + // Curvature + //const double kappa = m_curvature(Vec3d(coords[0], coords[1], coords[2])); + }, baseIndexToImageIndex.size() > 50); + + // Update levelset + ParallelUtils::parallelFor(baseIndexToImageIndex.size(), [&](const size_t& i) { - imgPtr[index] += dt * (vel * std::sqrt(g[1]) /*+ kappa * k*/); - } - }//); + const size_t index = std::get<0>(m_nodeUpdatePool[i]); + const double vel = std::get<2>(m_nodeUpdatePool[i]) + constantVel; + const Vec2d& g = std::get<3>(m_nodeUpdatePool[i]); + //const double kappa = std::get<4>(nodeUpdates[i]); + + // If speed function positive use forward difference (posMag) + if (vel > 0.0) + { + imgPtr[index] += dt * (vel * std::sqrt(g[0]) /*+ kappa * k*/); + } + // If speed function negative use backward difference (negMag) + else if (vel < 0.0) + { + imgPtr[index] += dt * (vel * std::sqrt(g[1]) /*+ kappa * k*/); + } + }, noteUpdatePoolSize > m_maxVelocitiesParallel); } m_nodesToUpdate.clear(); diff --git a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h index 39a149f1f4707bedfc01383593b270df50743c35..cd0ee2eb99348a10580bd6dd7f56883762c3584b 100644 --- a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h +++ b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h @@ -121,6 +121,7 @@ protected: std::unordered_map<size_t, std::tuple<Vec3i, double>> m_nodesToUpdate; std::vector<std::tuple<size_t, Vec3i, double, Vec2d, double>> m_nodeUpdatePool; size_t noteUpdatePoolSize; + size_t m_maxVelocitiesParallel = 100; // In sparse mode, if surpass this value, switch to parallel std::shared_ptr<ImageData> m_gradientMagnitudes = nullptr; ///> Gradient magnitude field when using dense std::shared_ptr<ImageData> m_velocities = nullptr; diff --git a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp index 5287559612b01f2c862bd80b7df13abc5220ea30..eb3bd0ae9f11f13dc7f6862e14478b754fa83a5e 100644 --- a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp +++ b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp @@ -172,20 +172,18 @@ RigidBodyModel2::computeTentativeVelocities() const Vec3d& fG = m_config->m_gravity; // Sum gravity to the forces - //ParallelUtils::parallelFor(forces.size(), [&forces, &fG](const size_t& i) - for (size_t i = 0; i < forces.size(); i++) - { - forces[i] += fG; - } //); + ParallelUtils::parallelFor(forces.size(), [&forces, &fG](const int& i) + { + forces[i] += fG; + }, forces.size() > m_maxBodiesParallel); // Compute the desired velocites, later we will solve for the proper velocities, // adjusted for the constraints - //ParallelUtils::parallelFor(tentativeVelocities.size(), [&](const size_t& i) - for (size_t i = 0; i < tentativeAngularVelocities.size(); i++) - { - tentativeVelocities[i] += forces[i] * invMasses[i] * dt; - tentativeAngularVelocities[i] += invInteriaTensors[i] * torques[i] * dt; - } //); + ParallelUtils::parallelFor(tentativeVelocities.size(), [&](const size_t& i) + { + tentativeVelocities[i] += forces[i] * invMasses[i] * dt; + tentativeAngularVelocities[i] += invInteriaTensors[i] * torques[i] * dt; + }, tentativeVelocities.size() > m_maxBodiesParallel); } void @@ -362,36 +360,35 @@ RigidBodyModel2::integrate() StdVectorOfVec3d& forces = getCurrentState()->getForces(); StdVectorOfVec3d& torques = getCurrentState()->getTorques(); - //ParallelUtils::parallelFor(positions.size(), [&](const size_t& i) - for (size_t i = 0; i < positions.size(); i++) - { - if (!isStatic[i]) - { - velocities[i] += forces[i] * invMasses[i] * dt; - velocities[i] *= velocityDamping; - angularVelocities[i] += invInteriaTensors[i] * torques[i] * dt; - angularVelocities[i] *= angularVelocityDamping; - positions[i] += velocities[i] * dt; + ParallelUtils::parallelFor(positions.size(), [&](const size_t& i) { - const Quatd q = Quatd(0.0, - angularVelocities[i][0], - angularVelocities[i][1], - angularVelocities[i][2]) * orientations[i]; - orientations[i].x() += q.x() * dt; - orientations[i].y() += q.y() * dt; - orientations[i].z() += q.z() * dt; - orientations[i].w() += q.w() * dt; - orientations[i].normalize(); - } - } + if (!isStatic[i]) + { + velocities[i] += forces[i] * invMasses[i] * dt; + velocities[i] *= velocityDamping; + angularVelocities[i] += invInteriaTensors[i] * torques[i] * dt; + angularVelocities[i] *= angularVelocityDamping; + positions[i] += velocities[i] * dt; + { + const Quatd q = Quatd(0.0, + angularVelocities[i][0], + angularVelocities[i][1], + angularVelocities[i][2]) * orientations[i]; + orientations[i].x() += q.x() * dt; + orientations[i].y() += q.y() * dt; + orientations[i].z() += q.z() * dt; + orientations[i].w() += q.w() * dt; + orientations[i].normalize(); + } + } - // Reset - m_bodies[i]->m_prevForce = forces[i]; - forces[i] = Vec3d(0.0, 0.0, 0.0); - torques[i] = Vec3d(0.0, 0.0, 0.0); - tentativeVelocities[i] = velocities[i]; - tentativeAngularVelocities[i] = angularVelocities[i]; - } //); + // Reset + m_bodies[i]->m_prevForce = forces[i]; + forces[i] = Vec3d(0.0, 0.0, 0.0); + torques[i] = Vec3d(0.0, 0.0, 0.0); + tentativeVelocities[i] = velocities[i]; + tentativeAngularVelocities[i] = angularVelocities[i]; + }, positions.size() > m_maxBodiesParallel); } void diff --git a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h index dbe665f78fed0432e9a4db2fd1d3f6f22f3f0e84..fad63f7ea0f54f189ecb8a79b567ebb0c24d6943 100644 --- a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h +++ b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h @@ -149,9 +149,10 @@ protected: std::list<std::shared_ptr<RbdConstraint>> m_constraints; std::vector<std::shared_ptr<RigidBody>> m_bodies; std::unordered_map<RigidBody*, StorageIndex> m_locations; - bool m_modified = true; + bool m_modified = true; + size_t m_maxBodiesParallel = 10; // After 10 bodies, parallel for's are used - Eigen::VectorXd F; // Reaction forces + Eigen::VectorXd F; // Reaction forces }; } } \ No newline at end of file diff --git a/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h b/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h index a8fc60ce11284de242cf4d5d885ee91dc3578c5d..dd294b2b427004c7bd09d8ef502ffdab4dd26121 100644 --- a/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h +++ b/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h @@ -141,7 +141,7 @@ struct ImplicitFunctionBackwardGradient : public ImplicitFunctionGradient struct StructuredForwardGradient : public ImplicitFunctionGradient { public: - Vec3d operator()(const Vec3d& pos) const override + inline Vec3d operator()(const Vec3d& pos) const override { const SignedDistanceField& funcRef = *static_cast<SignedDistanceField*>(m_func.get()); const double centralValue = funcRef.getFunctionValueCoord(Vec3i(static_cast<int>(pos[0]), static_cast<int>(pos[1]), static_cast<int>(pos[2]))); @@ -171,7 +171,7 @@ struct StructuredForwardGradient : public ImplicitFunctionGradient struct StructuredBackwardGradient : public ImplicitFunctionGradient { public: - Vec3d operator()(const Vec3d& pos) const override + inline Vec3d operator()(const Vec3d& pos) const override { const SignedDistanceField& funcRef = *static_cast<SignedDistanceField*>(m_func.get()); const double centralValue = funcRef.getFunctionValueCoord(Vec3i(static_cast<int>(pos[0]), static_cast<int>(pos[1]), static_cast<int>(pos[2]))); diff --git a/Source/Geometry/Implicit/imstkSignedDistanceField.cpp b/Source/Geometry/Implicit/imstkSignedDistanceField.cpp index 15da73db818bda36ae2eb76bfe7fe3fcb8872f42..e32bb168f8c6f094c11f1d3e5c56fb0dd2259adc 100644 --- a/Source/Geometry/Implicit/imstkSignedDistanceField.cpp +++ b/Source/Geometry/Implicit/imstkSignedDistanceField.cpp @@ -116,22 +116,6 @@ SignedDistanceField::getFunctionValue(const Vec3d& pos) const } } -double -SignedDistanceField::getFunctionValueCoord(const Vec3i& coord) const -{ - if (coord[0] < m_imageDataSdf->getDimensions()[0] && coord[0] > 0 - && coord[1] < m_imageDataSdf->getDimensions()[1] && coord[1] > 0 - && coord[2] < m_imageDataSdf->getDimensions()[2] && coord[2] > 0) - { - const Vec3i clampedCoord = coord.cwiseMin(m_imageDataSdf->getDimensions() - Vec3i(1, 1, 1)).cwiseMax(0); - return (*m_scalars)[m_imageDataSdf->getScalarIndex(clampedCoord)] * m_scale; - } - else - { - return std::numeric_limits<double>::min(); - } -} - void SignedDistanceField::computeBoundingBox(Vec3d& min, Vec3d& max, const double paddingPercent) { diff --git a/Source/Geometry/Implicit/imstkSignedDistanceField.h b/Source/Geometry/Implicit/imstkSignedDistanceField.h index 1c05fe5f2bde9842b49e58c987909a2442815671..c8095de257fe821dc93a6d5369a7ae61de3b4ffb 100644 --- a/Source/Geometry/Implicit/imstkSignedDistanceField.h +++ b/Source/Geometry/Implicit/imstkSignedDistanceField.h @@ -21,12 +21,14 @@ #pragma once +#include "imstkDataArray.h" +#include "imstkImageData.h" #include "imstkImplicitGeometry.h" namespace imstk { -class ImageData; -template<typename T> class DataArray; +//class ImageData; +//template<typename T> class DataArray; /// /// \class SignedDistanceField @@ -60,12 +62,25 @@ public: /// /// \brief Returns signed distance to surface at pos, returns clamped/nearest if out of bounds /// - double getFunctionValue(const Vec3d& pos) const override; + double SignedDistanceField::getFunctionValue(const Vec3d& pos) const; /// /// \brief Returns signed distance to surface at coordinate - /// - double getFunctionValueCoord(const Vec3i& coord) const; + /// inlined for performance + /// + inline double SignedDistanceField::getFunctionValueCoord(const Vec3i& coord) const + { + if (coord[0] < m_imageDataSdf->getDimensions()[0] && coord[0] > 0 + && coord[1] < m_imageDataSdf->getDimensions()[1] && coord[1] > 0 + && coord[2] < m_imageDataSdf->getDimensions()[2] && coord[2] > 0) + { + return (*m_scalars)[m_imageDataSdf->getScalarIndex(coord)] * m_scale; + } + else + { + return std::numeric_limits<double>::max(); + } + } /// /// \brief Returns the bounds of the field diff --git a/Source/SimulationManager/imstkSimulationManager.cpp b/Source/SimulationManager/imstkSimulationManager.cpp index faa0aed82ec611a5c63adf78a247338e898d73fb..76481bdb5bbdcb22fe058a48e467ff57061333e2 100644 --- a/Source/SimulationManager/imstkSimulationManager.cpp +++ b/Source/SimulationManager/imstkSimulationManager.cpp @@ -147,21 +147,22 @@ SimulationManager::start() accumulator += passedTime; // Compute number of steps we can take (total time previously took / desired time step) - m_numSteps = 0; - while (accumulator >= desiredDt_ms) { - m_numSteps++; - accumulator -= desiredDt_ms; + // Divided out and floor + m_numSteps = static_cast<int>(accumulator / desiredDt_ms); + // Set the remainder + accumulator = accumulator - m_numSteps * desiredDt_ms; + // Flatten out the remainder over our desired dt + m_dt = desiredDt_ms; + if (m_numSteps != 0) + { + m_dt += accumulator / m_numSteps; + accumulator = 0.0; + } + m_dt *= 0.001; // ms->s } - // Flatten out the leftover accumulation in our chosen dt - m_dt = desiredDt_ms; - if (m_numSteps != 0) - { - m_dt += accumulator / m_numSteps; - accumulator = 0.0; - } - m_dt *= 0.001; // ms->s + //printf("%d steps at %f\n", m_numSteps, m_dt); // Optional smoothening + loss here diff --git a/Source/SimulationManager/imstkSimulationManager.h b/Source/SimulationManager/imstkSimulationManager.h index a2e5953cff8f1668ad5519d2b1c0875eff297f1c..b2949b425e64667821bca2fbd63a654e36a40b69 100644 --- a/Source/SimulationManager/imstkSimulationManager.h +++ b/Source/SimulationManager/imstkSimulationManager.h @@ -53,7 +53,7 @@ public: }; public: - ~SimulationManager() override= default; + virtual ~SimulationManager() override= default; public: /// @@ -123,6 +123,6 @@ protected: ThreadingType m_threadType = ThreadingType::STL; double m_desiredDt = 0.003; // Desired timestep double m_dt = 0.0; // Actual timestep - double m_numSteps = 0; + int m_numSteps = 0; }; }; \ No newline at end of file