Implemented CoR visualization

cura/NavlibClient.py

@@ -4,6 +4,7 @@
 from UM.Math.AxisAlignedBox import AxisAlignedBox
 from cura.PickingPass import PickingPass
 from UM.Scene.Iterator.DepthFirstIterator import DepthFirstIterator
+from cura.Scene.OverlayNode import OverlayNode, SceneNode
 
 class NavlibClient(pynav.NavlibNavigationModel):
 
@@ -15,6 +16,7 @@ def __init__(self, scene, renderer) -> None:
         self._was_pick = False
         self._hit_selection_only = False
         self._picking_pass = None
+        self._pivot_node = OverlayNode(node=SceneNode(), image_path="resources/images/3dx_pivot.png", size=3.)
 
     def pick(self, x, y, check_selection = False, radius = 0.):
 
@@ -218,6 +220,20 @@ def set_camera_matrix(self, matrix : pynav.NavlibMatrix):
         transformation = Matrix(data = matrix._matrix)
         self._scene.getActiveCamera().setTransformation(transformation)
 
+        active_camera = self._scene.getActiveCamera()
+        if active_camera.isPerspective():
+            camera_position = active_camera.getWorldPosition()
+            dist = (camera_position - self._pivot_node.getWorldPosition()).length()
+            scale = dist/400
+            if scale < 1.0:
+                scale = scale * scale
+        else:
+            view_width = active_camera.getViewportWidth()
+            current_size = view_width + (2 * active_camera.getZoomFactor() * view_width)
+            scale = current_size / view_width * 5
+
+        self._pivot_node.scale(scale)
+
     def set_view_extents(self, extents: pynav.NavlibBox):
         view_width = self._scene.getActiveCamera().getViewportWidth()
         new_zoom = (extents._min._x + view_width / 2.) / - view_width
@@ -235,4 +251,13 @@ def set_motion_flag(self, motion : bool):
         else:
             self._was_pick = False
             self._renderer.removeRenderPass(self._picking_pass)
-
+
+    def set_pivot_position(self, position):
+        self._pivot_node._target_node.setPosition(position=Vector(position._x, position._y, position._z), transform_space = 3)
+
+    def set_pivot_visible(self, visible):
+        if visible:
+            self._scene.getRoot().addChild(self._pivot_node)
+        else:
+            self._scene.getRoot().removeChild(self._pivot_node)
+

cura/Scene/OverlayNode.py

@@ -0,0 +1,68 @@
+# Copyright (c) 2016 Ultimaker B.V.
+# Cura is released under the terms of the AGPLv3 or higher.
+
+from UM.Scene.SceneNode import SceneNode
+from UM.View.GL.OpenGL import OpenGL
+from UM.Mesh.MeshBuilder import MeshBuilder  # To create the overlay quad
+from UM.Resources import Resources  # To find shader locations
+from UM.Math.Matrix import Matrix
+from UM.Application import Application
+
+try:
+    from PyQt6.QtGui import QImage
+except:
+    from PyQt5.QtGui import QImage
+
+class OverlayNode(SceneNode):
+    def __init__(self, node, image_path, parent=None, size=1.):
+        super().__init__(parent)
+        self._target_node = node
+        self.setCalculateBoundingBox(False)
+
+        self._overlay_mesh = self._createOverlayQuad(size)
+        self._drawed_mesh = self._overlay_mesh
+        self._shader = None
+        self._scene = Application.getInstance().getController().getScene()
+        self._scale = 1.
+        self._image_path = image_path
+
+    def scale(self, factor):
+        scale_matrix = Matrix()
+        scale_matrix.setByScaleFactor(factor)
+        self._drawed_mesh = self._overlay_mesh.getTransformed(scale_matrix)
+
+    def _createOverlayQuad(self, size):
+        mb = MeshBuilder()
+        mb.addFaceByPoints(-size / 2, -size / 2, 0, -size / 2, size / 2, 0, size / 2, -size / 2, 0)
+        mb.addFaceByPoints(size / 2, size / 2, 0, -size / 2, size / 2, 0, size / 2, -size / 2, 0)
+
+        # Set UV coordinates so a texture can be created
+        mb.setVertexUVCoordinates(0, 0, 1)
+        mb.setVertexUVCoordinates(1, 0, 0)
+        mb.setVertexUVCoordinates(4, 0, 0)
+        mb.setVertexUVCoordinates(2, 1, 1)
+        mb.setVertexUVCoordinates(5, 1, 1)
+        mb.setVertexUVCoordinates(3, 1, 0)
+
+        return mb.build()
+
+    def render(self, renderer):
+
+        if not self._shader:
+            # We now misuse the platform shader, as it actually supports textures
+            self._shader = OpenGL.getInstance().createShaderProgram(Resources.getPath(Resources.Shaders, "platform.shader"))
+            # Set the opacity to 0, so that the template is in full control.
+            self._shader.setUniformValue("u_opacity", 0)
+            self._texture = OpenGL.getInstance().createTexture()
+            texture_image = QImage(self._image_path)
+            self._texture.setImage(texture_image)
+            self._shader.setTexture(0, self._texture)
+
+        node_position = self._target_node.getWorldPosition()
+        position_matrix = Matrix()
+        position_matrix.setByTranslation(node_position)
+        camera_orientation = self._scene.getActiveCamera().getOrientation().toMatrix()
+
+        renderer.queueNode(self._scene.getRoot(), shader=self._shader, mesh=self._drawed_mesh.getTransformed(position_matrix.multiply(camera_orientation)), type=3)
+
+        return True  # This node does it's own rendering.