Skip to content
Snippets Groups Projects
Commit 46a82504 authored by Ye Han's avatar Ye Han :panda_face:
Browse files

ENH: Use local transforms to align visual/collding geometries

parent e089865f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Examples/PBDPicking/PBDPickingExample.cpp
+ 12
− 17
View file @ 46a82504
......@@ -205,33 +205,28 @@ main()
geomShaft->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
imstkNew<CollidingObject> objShaft("ShaftObject");
objShaft->setVisualGeometry(geomShaft);
objShaft->setVisualGeometry(pivotSurfMesh);
objShaft->setCollidingGeometry(geomShaft);
//objShaft->setCollidingToVisualMap(std::make_shared<IsometricMap>(geomShaft, pivotSurfMesh));
scene->addSceneObject(objShaft);
imstkNew<Capsule> geomUpperJaw;
geomUpperJaw->setLength(15.0);
geomUpperJaw->setTranslation(Vec3d(0.0, 0.0, -15.0));
geomUpperJaw->setLength(25.0);
geomUpperJaw->setTranslation(Vec3d(0.0, 1.0, -12.5));
geomUpperJaw->setRadius(1.0);
geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
objUpperJaw->setVisualGeometry(geomUpperJaw);
objUpperJaw->setVisualGeometry(upperSurfMesh);
objUpperJaw->setCollidingGeometry(geomUpperJaw);
/* auto mapUpperJaw = std::make_shared<IsometricMap>(geomUpperJaw, upperSurfMesh);
objUpperJaw->setCollidingToVisualMap(mapUpperJaw);*/
scene->addSceneObject(objUpperJaw);
imstkNew<Capsule> geomLowerJaw;
geomLowerJaw->setLength(15.0);
geomLowerJaw->setTranslation(Vec3d(0.0, 0.0, -15.0));
geomLowerJaw->setLength(25.0);
geomLowerJaw->setTranslation(Vec3d(0.0, -1.0, -12.5));
geomLowerJaw->setRadius(1.0);
geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
objLowerJaw->setVisualGeometry(geomLowerJaw);
objLowerJaw->setVisualGeometry(lowerSurfMesh);
objLowerJaw->setCollidingGeometry(geomLowerJaw);
//auto mapLowerJaw = std::make_shared<IsometricMap>(geomLowerJaw, lowerSurfMesh);
//objLowerJaw->setCollidingToVisualMap(mapLowerJaw);
scene->addSceneObject(objLowerJaw);
std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
......@@ -244,9 +239,9 @@ main()
// Add interaction pair for pbd picking
imstkNew<PbdObjectPickingPair> upperJawPickingPair(clothObj, objUpperJaw, CollisionDetection::Type::PointSetToCapsule);
//imstkNew<PbdObjectPickingPair> lowerJawPickingPair(clothObj, objLowerJaw, CollisionDetection::Type::PointSetToCapsule);
imstkNew<PbdObjectPickingPair> lowerJawPickingPair(clothObj, objLowerJaw, CollisionDetection::Type::PointSetToCapsule);
scene->getCollisionGraph()->addInteraction(upperJawPickingPair);
//scene->getCollisionGraph()->addInteraction(lowerJawPickingPair);
scene->getCollisionGraph()->addInteraction(lowerJawPickingPair);
// Camera
scene->getActiveCamera()->setPosition(Vec3d(1, 1, 1) * 100.0);
......@@ -287,19 +282,19 @@ main()
connect<Event>(sceneManager, EventType::PreUpdate, [&](Event*)
{
std::shared_ptr<PBDPickingCH> chUpper = std::static_pointer_cast<PBDPickingCH>(upperJawPickingPair->getCollisionHandlingA());
//std::shared_ptr<PBDPickingCH> chLower = std::static_pointer_cast<PBDPickingCH>(lowerJawPickingPair->getCollisionHandlingA());
std::shared_ptr<PBDPickingCH> chLower = std::static_pointer_cast<PBDPickingCH>(lowerJawPickingPair->getCollisionHandlingA());
// Activate picking
if (client->getButton(1))
{
chUpper->activatePickConstraints();
//chLower->activatePickConstraints();
chLower->activatePickConstraints();
}
// Unpick
if (client->getButton(0))
{
chUpper->removePickConstraints();
//chLower->removePickConstraints();
chLower->removePickConstraints();
}
});
......
Source/CollisionHandling/imstkPBDPickingCH.cpp
+ 4
− 4
View file @ 46a82504
......@@ -93,13 +93,13 @@ PBDPickingCH::addPickConstraints(std::shared_ptr<PbdObject> pbdObj, std::shared_
[&](const size_t idx) {
const auto& cd = m_colData->MAColData[idx];
if (m_pickedPtIdxOffset.find(cd.nodeIdx) == m_pickedPtIdxOffset.end())
if (m_pickedPtIdxOffset.find(cd.nodeIdx) == m_pickedPtIdxOffset.end() && model->getInvMass(cd.nodeIdx) != 0.0)
{
lock.lock();
auto pv = cd.penetrationVector;
auto rot = pickGeom->getRotation().transpose();
auto relativePos = rot * (model->getCurrentState()->getVertexPosition(cd.nodeIdx) - pickGeom->getPosition());
lock.lock();
m_pickedPtIdxOffset[cd.nodeIdx] = relativePos;
model->setFixedPoint(cd.nodeIdx);
model->getCurrentState()->setVertexPosition(cd.nodeIdx, pickGeom->getPosition() + rot.transpose() * relativePos);
......
Source/Controllers/imstkLaparoscopicToolController.cpp
+ 34
− 19
View file @ 46a82504
......@@ -28,9 +28,9 @@
namespace imstk
{
LaparoscopicToolController::LaparoscopicToolController(
std::shared_ptr<SceneObject> shaft,
std::shared_ptr<SceneObject> upperJaw,
std::shared_ptr<SceneObject> lowerJaw,
std::shared_ptr<CollidingObject> shaft,
std::shared_ptr<CollidingObject> upperJaw,
std::shared_ptr<CollidingObject> lowerJaw,
std::shared_ptr<DeviceClient> trackingDevice) :
TrackingDeviceControl(trackingDevice),
m_shaft(shaft),
......@@ -41,9 +41,13 @@ LaparoscopicToolController::LaparoscopicToolController(
trackingDevice->setButtonsEnabled(true);
// Record the transforms as 4x4 matrices (this should capture initial displacement/rotation of the jaws from the shaft)
m_shaftInitialTransform = mat4dTranslate(m_shaft->getMasterGeometry()->getTranslation()) * mat4dRotation(m_shaft->getMasterGeometry()->getRotation());
m_upperJawInitialTransform = mat4dTranslate(m_upperJaw->getMasterGeometry()->getTranslation()) * mat4dRotation(m_upperJaw->getMasterGeometry()->getRotation());
m_lowerJawInitialTransform = mat4dTranslate(m_lowerJaw->getMasterGeometry()->getTranslation()) * mat4dRotation(m_lowerJaw->getMasterGeometry()->getRotation());
m_shaftVisualTransform = mat4dTranslate(m_shaft->getVisualGeometry()->getTranslation()) * mat4dRotation(m_shaft->getVisualGeometry()->getRotation());
m_upperJawVisualTransform = mat4dTranslate(m_upperJaw->getVisualGeometry()->getTranslation()) * mat4dRotation(m_upperJaw->getVisualGeometry()->getRotation());
m_lowerJawVisualTransform = mat4dTranslate(m_lowerJaw->getVisualGeometry()->getTranslation()) * mat4dRotation(m_lowerJaw->getVisualGeometry()->getRotation());
m_shaftCollidingTransform = mat4dTranslate(m_shaft->getCollidingGeometry()->getTranslation()) * mat4dRotation(m_shaft->getCollidingGeometry()->getRotation());
m_upperJawCollidingTransform = mat4dTranslate(m_upperJaw->getCollidingGeometry()->getTranslation()) * mat4dRotation(m_upperJaw->getCollidingGeometry()->getRotation());
m_lowerJawCollidingTransform = mat4dTranslate(m_lowerJaw->getCollidingGeometry()->getTranslation()) * mat4dRotation(m_lowerJaw->getCollidingGeometry()->getRotation());
}
void
......@@ -63,14 +67,18 @@ LaparoscopicToolController::updateControlledObjects()
// Controller transform
m_controllerWorldTransform = mat4dTranslate(controllerPosition) * mat4dRotation(controllerOrientation);
m_shaftWorldTransform = m_controllerWorldTransform * m_shaftInitialTransform;
{
// TRS decompose and set shaft
Vec3d t, s;
Mat3d r;
mat4dTRS(m_shaftWorldTransform, t, r, s);
m_shaft->getMasterGeometry()->setRotation(r);
m_shaft->getMasterGeometry()->setTranslation(t);
mat4dTRS(m_controllerWorldTransform * m_shaftVisualTransform, t, r, s);
m_shaft->getVisualGeometry()->setRotation(r);
m_shaft->getVisualGeometry()->setTranslation(t);
mat4dTRS(m_controllerWorldTransform * m_shaftCollidingTransform, t, r, s);
m_shaft->getCollidingGeometry()->setRotation(r);
m_shaft->getCollidingGeometry()->setTranslation(t);
}
// Update jaw angles
......@@ -87,22 +95,29 @@ LaparoscopicToolController::updateControlledObjects()
m_upperJawLocalTransform = mat4dRotation(Rotd(m_jawAngle, m_jawRotationAxis));
m_lowerJawLocalTransform = mat4dRotation(Rotd(-m_jawAngle, m_jawRotationAxis));
m_upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawInitialTransform;
m_lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_upperJawInitialTransform;
{
Vec3d t, s;
Mat3d r;
mat4dTRS(m_upperJawWorldTransform, t, r, s);
m_upperJaw->getMasterGeometry()->setRotation(r);
m_upperJaw->getMasterGeometry()->setTranslation(t);
mat4dTRS(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisualTransform, t, r, s);
m_upperJaw->getVisualGeometry()->setRotation(r);
m_upperJaw->getVisualGeometry()->setTranslation(t);
mat4dTRS(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawCollidingTransform, t, r, s);
m_upperJaw->getCollidingGeometry()->setRotation(r);
m_upperJaw->getCollidingGeometry()->setTranslation(t);
}
{
Vec3d t, s;
Mat3d r;
mat4dTRS(m_lowerJawWorldTransform, t, r, s);
m_lowerJaw->getMasterGeometry()->setRotation(r);
m_lowerJaw->getMasterGeometry()->setTranslation(t);
mat4dTRS(m_controllerWorldTransform * m_lowerJawLocalTransform * m_upperJawVisualTransform, t, r, s);
m_lowerJaw->getVisualGeometry()->setRotation(r);
m_lowerJaw->getVisualGeometry()->setTranslation(t);
mat4dTRS(m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawCollidingTransform, t, r, s);
m_lowerJaw->getCollidingGeometry()->setRotation(r);
m_lowerJaw->getCollidingGeometry()->setTranslation(t);
}
}
......
Source/Controllers/imstkLaparoscopicToolController.h
+ 16
− 16
View file @ 46a82504
......@@ -25,7 +25,7 @@
namespace imstk
{
class SceneObject;
class CollidingObject;
///
/// \class LaparoscopicToolController
......@@ -42,9 +42,9 @@ public:
/// \brief Constructor
///
LaparoscopicToolController(
std::shared_ptr<SceneObject> shaft,
std::shared_ptr<SceneObject> upperJaw,
std::shared_ptr<SceneObject> lowerJaw,
std::shared_ptr<CollidingObject> shaft,
std::shared_ptr<CollidingObject> upperJaw,
std::shared_ptr<CollidingObject> lowerJaw,
std::shared_ptr<DeviceClient> trackingDevice);
LaparoscopicToolController() = delete; //not allowed for now
......@@ -88,9 +88,9 @@ public:
inline double getMaxJawAngle() const { return m_maxJawAngle; }
protected:
std::shared_ptr<SceneObject> m_shaft; ///< Tool shaft
std::shared_ptr<SceneObject> m_upperJaw; ///< Tool upper jaw
std::shared_ptr<SceneObject> m_lowerJaw; ///< Tool lower jaw
std::shared_ptr<CollidingObject> m_shaft; ///< Tool shaft
std::shared_ptr<CollidingObject> m_upperJaw; ///< Tool upper jaw
std::shared_ptr<CollidingObject> m_lowerJaw; ///< Tool lower jaw
double m_jawAngle = PI / 6.0; ///< Angle of the jaws
double m_change = 6.0e-5; ///< Amount of change in jaw angle per frame
......@@ -98,17 +98,17 @@ protected:
Vec3d m_jawRotationAxis; ///< Angle of the jaws
Mat4d m_controllerWorldTransform = Mat4d::Identity(); // Final world transform of the controller
Mat4d m_controllerWorldTransform = Mat4d::Identity(); // Final world transform of the controller
Mat4d m_shaftWorldTransform = Mat4d::Identity(); // Final world transform of the shaft
Mat4d m_upperJawWorldTransform = Mat4d::Identity(); // Final world transform of the upper jaw
Mat4d m_lowerJawWorldTransform = Mat4d::Identity(); // Final world transform of the lower jaw
Mat4d m_shaftVisualTransform = Mat4d::Identity(); // Initial local transform of the visual shaft
Mat4d m_upperJawVisualTransform = Mat4d::Identity(); // Initial local transform of the visual upper jaw
Mat4d m_lowerJawVisualTransform = Mat4d::Identity(); // Initial local transform of the visual lower jaw
Mat4d m_shaftInitialTransform = Mat4d::Identity(); // Initial local transform of the shaft
Mat4d m_upperJawInitialTransform = Mat4d::Identity(); // Initial local transform of the upper jaw
Mat4d m_lowerJawInitialTransform = Mat4d::Identity(); // Initial local transform of the lower jaw
Mat4d m_shaftCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding shaft
Mat4d m_upperJawCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding upper jaw
Mat4d m_lowerJawCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding lower jaw
Mat4d m_upperJawLocalTransform = Mat4d::Identity(); // m_upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawInitialTransform
Mat4d m_lowerJawLocalTransform = Mat4d::Identity(); // m_lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawInitialTransform
Mat4d m_upperJawLocalTransform = Mat4d::Identity(); // upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisual/CollidingTransform
Mat4d m_lowerJawLocalTransform = Mat4d::Identity(); // lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawVisual/CollidingTransform
};
} // imstk
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment