ENH: Transform fix

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

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);
+}
 }

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

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