ENH: Improve Endoscopy flythrough

Modules/Scripted/Endoscopy/Endoscopy.py

@@ -499,19 +499,20 @@ def onSaveOrientationButtonClicked(self):
         # Compute new dictionary key and value
         resampledCurve = self.logic.resampledCurve
         resampledCurveControlPointIndex = int(self.flythroughFrameSlider.value)
-        distanceAlongCurve = EndoscopyLogic.distanceAlongCurveOfNthControlPoint(
+        distanceAlongCurve = EndoscopyLogic.DistanceAlongCurveOfNthControlPoint(
             resampledCurve, resampledCurveControlPointIndex
         )
-        worldMatrix4x4 = vtk.vtkMatrix4x4()
-        # TODO: The next line doesn't work; how do we fix this?  We want the current orientation the user is seeing in
-        # the 3D pane, however it got set, but this isn't grabbing that.
-        self.transform.GetMatrixTransformToParent(worldMatrix4x4)
-        worldMatrix3x3 = np.zeros((3, 3))
-        for col in range(3):
-            for row in range(3):
-                worldMatrix3x3[row, col] = worldMatrix4x4.GetElement(row, col)
+        cameraPosition = self.camera.GetPosition()
+        focalPoint = self.camera.GetFocalPoint()
+        viewUp = self.camera.GetViewUp()
+        worldMatrix3x3 = EndoscopyLogic.BuildCameraMatrix3x3(cameraPosition, focalPoint, viewUp)
         worldOrientation = EndoscopyLogic.Matrix3x3ToOrientation(worldMatrix3x3)
 
+        # Keep the cursorNode and camera orientations in sync: set cursorNode's position and orientation
+        worldMatrix4x4 = EndoscopyLogic.BuildCameraMatrix4x4(cameraPosition, worldMatrix3x3)
+        # TODO: Should we do this even earlier, by catching camera change events?
+        self.transform.SetMatrixTransformToParent(worldMatrix4x4)
+
         # Add new dictionary key-value pair
         cameraOrientations = self.loadCameraOrientations()
         cameraOrientations[distanceAlongCurve] = worldOrientation
@@ -585,33 +586,22 @@ def flyTo(self, resampledCurveControlPointIndex):
         cameraPosition = np.zeros((3,))
         self.logic.resampledCurve.GetNthControlPointPositionWorld(resampledCurveControlPointIndex, cameraPosition)
 
-        focalPointPosition = np.zeros((3,))
-        self.logic.resampledCurve.GetNthControlPointPositionWorld(
-            resampledCurveControlPointIndex + 1, focalPointPosition
-        )
+        focalPoint = np.zeros((3,))
+        self.logic.resampledCurve.GetNthControlPointPositionWorld(resampledCurveControlPointIndex + 1, focalPoint)
 
-        worldOrientation = EndoscopyLogic.getWorldOrientation(
+        worldOrientation = EndoscopyLogic.GetWorldOrientation(
             self.logic.resampledCurve, resampledCurveControlPointIndex
         )
         worldMatrix3x3 = EndoscopyLogic.OrientationToMatrix3x3(worldOrientation)
+        worldMatrix4x4 = EndoscopyLogic.BuildCameraMatrix4x4(cameraPosition, worldMatrix3x3)
 
-        # Build a 4x4 matrix from the 3x3 matrix and the camera position
-        worldMatrix4x4 = vtk.vtkMatrix4x4()
-        worldMatrix4x4.SetElement(3, 3, 1.0)
-        for col in range(3):
-            for row in range(3):
-                worldMatrix4x4.SetElement(row, col, worldMatrix3x3[row, col])
-            worldMatrix4x4.SetElement(3, col, 0.0)
-            worldMatrix4x4.SetElement(col, 3, cameraPosition[col])
-
-        # Set the camera and cameraNode
+        # Set the camera, cameraNode, and cursor matrix
         wasModified = self.cameraNode.StartModify()
         self.camera.SetPosition(*cameraPosition)
-        self.camera.SetFocalPoint(*focalPointPosition)
-        # TODO: Are these OrthogonalizeViewUp and SetMatrixTransformToParent calls both appropriate?
-        self.camera.OrthogonalizeViewUp()
+        self.camera.SetFocalPoint(*focalPoint)
+        self.camera.SetViewUp(*worldMatrix3x3[:, 1])
+        self.cameraNode.ResetClippingRange()  # TODO: Experiment with not doing this
         self.transform.SetMatrixTransformToParent(worldMatrix4x4)
-        self.cameraNode.ResetClippingRange()
         self.cameraNode.EndModify(wasModified)
 
 
@@ -746,7 +736,7 @@ def interpolateOrientations(self):
         # distances along self.resampledCurve with a fudgeFactor so that the lengths do come out the same.
         for resampledCurveControlPointIndex in range(self.numberOfResampledCurveControlPoints):
             distanceAlongInputCurve = (
-                EndoscopyLogic.distanceAlongCurveOfNthControlPoint(self.resampledCurve, resampledCurveControlPointIndex)
+                EndoscopyLogic.DistanceAlongCurveOfNthControlPoint(self.resampledCurve, resampledCurveControlPointIndex)
                 * fudgeFactor
             )
             quaternion = np.zeros((4,))
@@ -755,7 +745,7 @@ def interpolateOrientations(self):
             self.setRelativeOrientation(self.resampledCurve, resampledCurveControlPointIndex, relativeOrientation)
         self.resampledCurve.EndModify(wasModified)
 
-    def getDefaultOrientation(self, curve, curveControlPointIndex, orientation=np.zeros((4,))):
+    def getDefaultOrientation(self, curve, curveControlPointIndex, worldOrientation=np.zeros((4,))):
         # logging.debug(f"getDefaultOrientation[{curveControlPointIndex}] = ", end="")
         numberOfCurveControlPoints = curve.GetNumberOfControlPoints()
         # If the curve is not closed then the last control point has the same orientation as its previous control point
@@ -767,31 +757,23 @@ def getDefaultOrientation(self, curve, curveControlPointIndex, orientation=np.ze
         nextCurveControlPointIndex = (curveControlPointIndex + 1) % numberOfCurveControlPoints
         cameraPosition = np.zeros((3,))
         curve.GetNthControlPointPositionWorld(curveControlPointIndex, cameraPosition)
-        focalPointPosition = np.zeros((3,))
-        curve.GetNthControlPointPositionWorld(nextCurveControlPointIndex, focalPointPosition)
-        matrix3x3 = np.zeros((3, 3))
-        # camera forward
-        matrix3x3[:, 2] = focalPointPosition - cameraPosition
-        matrix3x3[:, 2] /= np.linalg.norm(matrix3x3[:, 2])
-        # camera left
-        matrix3x3[:, 0] = np.cross(self.planeNormal, matrix3x3[:, 2])
-        matrix3x3[:, 0] /= np.linalg.norm(matrix3x3[:, 0])
-        # camera up
-        matrix3x3[:, 1] = np.cross(matrix3x3[:, 2], matrix3x3[:, 0])
-        EndoscopyLogic.Matrix3x3ToOrientation(matrix3x3, orientation)
-        # logging.debug(repr(orientation))
-        return orientation
+        focalPoint = np.zeros((3,))
+        curve.GetNthControlPointPositionWorld(nextCurveControlPointIndex, focalPoint)
+        matrix3x3 = EndoscopyLogic.BuildCameraMatrix3x3(cameraPosition, focalPoint, self.planeNormal)
+        EndoscopyLogic.Matrix3x3ToOrientation(matrix3x3, worldOrientation)
+        # logging.debug(repr(worldOrientation))
+        return worldOrientation
 
     @staticmethod
-    def getWorldOrientation(curve, curveControlPointIndex, worldOrientation=np.zeros((4,))):
+    def GetWorldOrientation(curve, curveControlPointIndex, worldOrientation=np.zeros((4,))):
         curve.GetNthControlPointOrientation(curveControlPointIndex, worldOrientation)
         return worldOrientation
 
     @staticmethod
-    def setWorldOrientation(curve, curveControlPointIndex, worldOrientation):
+    def SetWorldOrientation(curve, curveControlPointIndex, worldOrientation):
         curve.SetNthControlPointOrientation(curveControlPointIndex, worldOrientation)
 
-    def getRelativeOrientation(curve, curveControlPointIndex, relativeOrientation=np.zeros((4,))):
+    def getRelativeOrientation(self, curve, curveControlPointIndex, relativeOrientation=np.zeros((4,))):
         worldOrientation = curve.GetNthControlPointOrientation(curveControlPointIndex)
         self.worldOrientationToRelative(curve, curveControlPointIndex, worldOrientation, relativeOrientation)
         return relativeOrientation
@@ -817,7 +799,7 @@ def worldOrientationToRelative(
         return relativeOrientation
 
     @staticmethod
-    def distanceAlongCurveOfNthControlPoint(curve, indexOfControlPoint):
+    def DistanceAlongCurveOfNthControlPoint(curve, indexOfControlPoint):
         controlPointPositionWorld = curve.GetNthControlPointPositionWorld(indexOfControlPoint)
         # Curve points are about 10-to-1 control points.  There are index + 1 points in {0, ..., index}.  So, typically
         # numberOfCurvePoints = 10 * indexOfControlPoint + 1.
@@ -895,6 +877,32 @@ def PlaneFit(points):
             n *= -1.0
         return p, n
 
+    @staticmethod
+    def BuildCameraMatrix3x3(cameraPosition, focalPoint, viewUp, outputMatrix3x3=np.zeros((3, 3))):
+        # Note that viewUp might be supplied as planeNormal, which might not be orthogonal to (focalPoint -
+        # cameraPosition), so this mathematics is careful to handle that case too.
+
+        # Camera forward
+        outputMatrix3x3[:, 2] = np.array(focalPoint) - np.array(cameraPosition)
+        outputMatrix3x3[:, 2] /= np.linalg.norm(outputMatrix3x3[:, 2])
+        # Camera left
+        outputMatrix3x3[:, 0] = np.cross(np.array(viewUp), outputMatrix3x3[:, 2])
+        outputMatrix3x3[:, 0] /= np.linalg.norm(outputMatrix3x3[:, 0])
+        # Camera up.  (No need to normalize because it already is normalized.)
+        outputMatrix3x3[:, 1] = np.cross(outputMatrix3x3[:, 2], outputMatrix3x3[:, 0])
+
+        return outputMatrix3x3
+
+    @staticmethod
+    def BuildCameraMatrix4x4(cameraPosition, inputMatrix3x3, outputMatrix4x4=vtk.vtkMatrix4x4()):
+        outputMatrix4x4.SetElement(3, 3, 1.0)
+        for col in range(3):
+            for row in range(3):
+                outputMatrix4x4.SetElement(row, col, inputMatrix3x3[row, col])
+            outputMatrix4x4.SetElement(3, col, 0.0)
+            outputMatrix4x4.SetElement(col, 3, cameraPosition[col])
+        return outputMatrix4x4
+
 
 class EndoscopyPathModel:
     """Create a vtkPolyData for a polyline:
@@ -932,6 +940,7 @@ def __init__(self, resampledCurve, inputCurve, outputPathNode=None, cursorType=N
         idArray.InsertNextTuple1(0)
 
         for resampledCurveControlPointIndex in range(resampledCurve.GetNumberOfControlPoints()):
+            # TODO: Rename `point` to something more informative
             point = np.zeros((3,))
             resampledCurve.GetNthControlPointPositionWorld(resampledCurveControlPointIndex, point)
             pointIndex = points.InsertNextPoint(*point)