diff --git a/Examples/PDBPicking/PBDPickingExample.cpp b/Examples/PDBPicking/PBDPickingExample.cpp
index c7f8b0eef6d44f663f13d292ef53ec34c8d4934c..f9c655267a487deffe73e0cd52092d68b0b8b7e5 100644
--- a/Examples/PDBPicking/PBDPickingExample.cpp
+++ b/Examples/PDBPicking/PBDPickingExample.cpp
@@ -211,22 +211,26 @@ main()
imstkNew<Capsule> geomUpperJaw;
geomUpperJaw->setLength(30.0);
- //geomUpperJaw->setPosition(Vec3d(0.0, 0.0, -12.5));
+ geomUpperJaw->setPosition(Vec3d(0.0, 0.0, -12.5));
geomUpperJaw->setRadius(1.0);
geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
objUpperJaw->setVisualGeometry(geomUpperJaw);
objUpperJaw->setCollidingGeometry(geomUpperJaw);
- //auto mapUpperJaw = std::make_shared<IsometricMap>(geomUpperJaw, upperSurfMesh);
- //objUpperJaw->setCollidingToVisualMap(mapUpperJaw);
+ /* auto mapUpperJaw = std::make_shared<IsometricMap>(geomUpperJaw, upperSurfMesh);
+ objUpperJaw->setCollidingToVisualMap(mapUpperJaw);*/
scene->addSceneObject(objUpperJaw);
imstkNew<Capsule> geomLowerJaw;
+ geomLowerJaw->setLength(30.0);
+ geomLowerJaw->setPosition(Vec3d(0.0, 0.0, -12.5));
+ geomLowerJaw->setRadius(1.0);
+ geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
- objLowerJaw->setVisualGeometry(lowerSurfMesh);
+ objLowerJaw->setVisualGeometry(geomLowerJaw);
objLowerJaw->setCollidingGeometry(geomLowerJaw);
- auto mapLowerJaw = std::make_shared<IsometricMap>(geomLowerJaw, lowerSurfMesh);
- objLowerJaw->setCollidingToVisualMap(mapLowerJaw);
+ //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);
@@ -242,7 +246,7 @@ main()
scene->getCollisionGraph()->addInteraction(pair);
// Camera
- scene->getActiveCamera()->setPosition(Vec3d(200, 200, 200));
+ scene->getActiveCamera()->setPosition(Vec3d(1, 1, 1) * 100.0);
scene->getActiveCamera()->setFocalPoint(Vec3d(0, 0, 0));
// Light
diff --git a/Source/Common/imstkMath.h b/Source/Common/imstkMath.h
index fed99309ed902ad6667757fc1ef7919b27b7f07a..feeef99e175b2f886ba4611a797b2d3184e90af2 100644
--- a/Source/Common/imstkMath.h
+++ b/Source/Common/imstkMath.h
@@ -166,4 +166,56 @@ using AffineTransform3d = Eigen::Affine3d;
#define MAX_F std::numeric_limits<float>::max()
#define MIN_F std::numeric_limits<float>::min()
#define VERY_SMALL_EPSILON_F std::numeric_limits<float>::epsilon()
+
+static inline Mat4d mat4dTranslate(const Vec3d& translate)
+{
+ return AffineTransform3d(Eigen::Translation3d(translate)).matrix();
+}
+static inline Mat4d mat4dScale(const Vec3d& scale)
+{
+ Mat4d mScale = Mat4d::Identity();
+ mScale(0, 0) = scale[0];
+ mScale(1, 1) = scale[1];
+ mScale(2, 2) = scale[2];
+ return mScale;
+}
+static inline Mat4d mat4dRotation(const Quatd& rotation)
+{
+ Mat4d mRot = Mat4d::Identity();
+ mRot.block<3, 3>(0, 0) = rotation.toRotationMatrix();
+ return mRot;
+}
+static inline Mat4d mat4dRotation(const Rotd& rotation)
+{
+ Mat4d mRot = Mat4d::Identity();
+ mRot.block<3, 3>(0, 0) = rotation.toRotationMatrix();
+ return mRot;
+}
+static inline Mat4d mat4dRotation(const Mat3d& rotation)
+{
+ Mat4d mRot = Mat4d::Identity();
+ mRot.block<3, 3>(0, 0) = rotation;
+ return mRot;
+}
+///
+/// \brief Translation, Rotation, Scaling decomposition, ignores shears
+///
+static inline void mat4dTRS(const Mat4d& m, Vec3d& t, Mat3d& r, Vec3d& s)
+{
+ // Assumes affine, no shear
+ const Vec3d& x = m.block<3, 1>(0, 0);
+ const Vec3d& y = m.block<3, 1>(0, 1);
+ const Vec3d& z = m.block<3, 1>(0, 2);
+
+ s = Vec3d(m.block<3, 1>(0, 0).norm(),
+ m.block<3, 1>(0, 1).norm(),
+ m.block<3, 1>(0, 2).norm());
+
+ r = Mat3d::Identity();
+ r.block<3, 1>(0, 0) = x.normalized();
+ r.block<3, 1>(0, 1) = y.normalized();
+ r.block<3, 1>(0, 2) = z.normalized();
+
+ t = m.block<3, 1>(0, 3);
+}
}
diff --git a/Source/Controllers/imstkLaparoscopicToolController.cpp b/Source/Controllers/imstkLaparoscopicToolController.cpp
index 8a32aa270ccc19c18731e32d9b2a5c791f905411..c9991925f8ba3bf4eb8211837950e1808c3adcfa 100644
--- a/Source/Controllers/imstkLaparoscopicToolController.cpp
+++ b/Source/Controllers/imstkLaparoscopicToolController.cpp
@@ -39,6 +39,15 @@ LaparoscopicToolController::LaparoscopicToolController(
m_jawRotationAxis(Vec3d(1, 0, 0))
{
trackingDevice->setButtonsEnabled(true);
+
+ // Record the transforms as 4x4 matrices (this should capture initial displacement/rotation of the jaws from the shaft)
+ m_shaftWorldTransform = mat4dRotation(m_shaft->getMasterGeometry()->getRotation()) * mat4dTranslate(m_shaft->getMasterGeometry()->getTranslation());
+
+ m_upperJawLocalTransform = mat4dRotation(m_upperJaw->getMasterGeometry()->getRotation()) * mat4dTranslate(m_upperJaw->getMasterGeometry()->getTranslation());
+ m_lowerJawLocalTransform = mat4dRotation(m_lowerJaw->getMasterGeometry()->getRotation()) * mat4dTranslate(m_lowerJaw->getMasterGeometry()->getTranslation());
+
+ m_upperJawWorldTransform = m_shaftWorldTransform * m_upperJawLocalTransform;
+ m_lowerJawWorldTransform = m_shaftWorldTransform * m_lowerJawLocalTransform;
}
void
@@ -53,8 +62,19 @@ LaparoscopicToolController::updateControlledObjects()
}
}
- Vec3d p = getPosition();
- Quatd r = getRotation();
+ const Vec3d controllerPosition = getPosition();
+ const Quatd controllerOrientation = getRotation();
+
+ // Controller transform
+ m_shaftWorldTransform = mat4dTranslate(controllerPosition) * mat4dRotation(controllerOrientation);
+ {
+ // 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);
+ }
// Update jaw angles
if (m_deviceClient->getButton(0))
@@ -62,28 +82,31 @@ LaparoscopicToolController::updateControlledObjects()
m_jawAngle += m_change;
m_jawAngle = (m_jawAngle > m_maxJawAngle) ? m_maxJawAngle : m_jawAngle;
}
-
if (m_deviceClient->getButton(1))
{
m_jawAngle -= m_change;
m_jawAngle = (m_jawAngle < 0.0) ? 0.0 : m_jawAngle;
}
- // Update orientation of parts
- Quatd jawRotUpper;
- jawRotUpper = r * Rotd(m_jawAngle, m_jawRotationAxis);
- m_upperJaw->getMasterGeometry()->setRotation(jawRotUpper);
+ m_upperJawLocalTransform = mat4dRotation(Rotd(m_jawAngle, m_jawRotationAxis));
+ m_lowerJawLocalTransform = mat4dRotation(Rotd(-m_jawAngle, m_jawRotationAxis));
+ m_upperJawWorldTransform = m_shaftWorldTransform * m_upperJawLocalTransform;
+ m_lowerJawWorldTransform = m_shaftWorldTransform * m_lowerJawLocalTransform;
- Quatd jawRotLower;
- jawRotLower = r * Rotd(-m_jawAngle, m_jawRotationAxis);
- m_lowerJaw->getMasterGeometry()->setRotation(jawRotLower);
-
- m_shaft->getMasterGeometry()->setRotation(r);
-
- // Update positions of parts
- m_shaft->getMasterGeometry()->setTranslation(p);
- m_upperJaw->getMasterGeometry()->setTranslation(p);
- m_lowerJaw->getMasterGeometry()->setTranslation(p);
+ {
+ Vec3d t, s;
+ Mat3d r;
+ mat4dTRS(m_upperJawWorldTransform, t, r, s);
+ m_upperJaw->getMasterGeometry()->setRotation(r);
+ m_upperJaw->getMasterGeometry()->setTranslation(t);
+ }
+ {
+ Vec3d t, s;
+ Mat3d r;
+ mat4dTRS(m_lowerJawWorldTransform, t, r, s);
+ m_lowerJaw->getMasterGeometry()->setRotation(r);
+ m_lowerJaw->getMasterGeometry()->setTranslation(t);
+ }
}
void
diff --git a/Source/Controllers/imstkLaparoscopicToolController.h b/Source/Controllers/imstkLaparoscopicToolController.h
index 2c1e1c9c41d504478ec2cafe31863fbe9de52a16..4b7d498fa154b02debd9165887771f9efd69047a 100644
--- a/Source/Controllers/imstkLaparoscopicToolController.h
+++ b/Source/Controllers/imstkLaparoscopicToolController.h
@@ -97,5 +97,13 @@ protected:
double m_maxJawAngle = PI / 6.0; ///< Maximum angle of the jaws
Vec3d m_jawRotationAxis; ///< Angle of the jaws
+
+ 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_upperJawLocalTransform = Mat4d::Identity(); // m_upperJawWorldTransform = m_shaftWorldTransform * m_upperJawLocalTransform
+ Mat4d m_lowerJawLocalTransform = Mat4d::Identity(); // m_lowerJawWorldTransform = m_shaftWorldTransform * m_lowerJawLocalTransform
};
} // imstk