dtrack_hooks.py

# Copyright (c) 2025, Advanced Realtime Tracking GmbH & Co. KG
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
# 3. Neither the name of copyright holder nor the names of its contributors
#    may be used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


"""Example Hooks"""


from dtrack_math import *
from dtrack_base import *
from dtrack_logger import *

import vrNodeUtils
from vrKernelServices import vrXRealityTypes



def getRenderMode():
    mode = vrHMDService.getXRRenderMode()
    if mode == vrXRealityTypes.XRRenderMode.FullScene: return "FullScene"
    if mode == vrXRealityTypes.XRRenderMode.ModelOnly: return "ModelOnly"
    if mode == vrXRealityTypes.XRRenderMode.SeethroughBackground: return "SeethroughBackground"
    return "SeethroughOnly"




class Track6DHook( DTrackRenderHook):
    """Hook for applying DTrack pose data of a specified target to a given VRED node"""

    def __init__( self, vrdevice, node, targetType, targetID):
        super().__init__()

        self.vrdevice = vrdevice
        self.node = node
        self.targetType = targetType
        self.targetID = targetID
        self.tracked = False        # TMP, log reset switch
        self.rotation = Rotation()

        self.node.setVisibilityFlag( True)


    def execute( self):

        if self.module.isTracked( self.targetType, self.targetID ):

            translation = self.module.getTranslation( self.targetType, self.targetID, DTRACK_GLOBAL_SCALE )
            if translation is not None: self.node.setTranslation( translation)

            rotation = self.module.getRotationAsQuaternion( self.targetType, self.targetID )
            if rotation is not None: self.node.setRotationAsQuaternion( rotation)

            self.tracked = True


            # XR mode
            if VRED_XRMODE_ENABLED  and self.vrdevice is not None:

                if vrHMDService.isHmdActive():

                    # Right hand / Flystick: Pose of the laser beam
                    if self.targetType == TargetType.Flystick:
                        trafo = self.node.getWorldTransform()
                        rot = qmatrix4dRot( trafo) @ self.rotation.X45
                        loc = qmatrix4dLoc( trafo)   +  rot @ np.array([[0, -30, -20]]).transpose()
                        self.vrdevice.setTrackingMatrix( qmatrix4d( rot, loc))

                    # Left hand / Standard Body: Pose of the vrMenu
                    elif self.targetType == TargetType.Body:
                        trafo = self.node.getWorldTransform()
                        self.vrdevice.setTrackingMatrix( trafo)


                    # Object representation depending on render mode
                    renderMode = vrHMDService.getXRRenderMode()
                    if renderMode == vrKernelServices.vrXRealityTypes.XRRenderMode.FullScene:
                        self.node.setVisibilityFlag( False)
                        self.vrdevice.setVisible(True)
                        self.vrdevice.setVisualizationMode(0)   # TMP
                    else:
                        self.vrdevice.setVisible(False)
                        self.node.setVisibilityFlag( True)

                else: 
                    self.node.setVisibilityFlag( True)


        else:
            if dtrackLog.enabled:
                dtrackLog.out( "targetType {} targetID {} not tracked".format(
                    self.targetType, self.targetID ), every=1000, reset=self.tracked )
            self.tracked = False




class FlyButtonHook( DTrackRenderHook):
    """Example hook for applying A.R.T. Flystick button information"""

    # Avoid repeated actions on one button press
    # TODO, use timestamps instead of frames
    class DoOnce:
        def __init__( self, deltaFrames):
            self.deltaFrames = deltaFrames
            self.frameCount  = 0
            self.startFrame  = -1

        def increment( self):
            self.frameCount += 1

        def start( self):
            self.startFrame = self.frameCount

        def available( self):
            if self.startFrame < 0:
                return True
            if self.frameCount > self.startFrame + self.deltaFrames:
                self.startFrame = -1
                return True
            return False



    def __init__( self, leftDevice, rightDevice, laserNode, flystickID, menuBtnID, touchBtnID, triggerBtnID ):
        super().__init__()

        self.leftDevice    = leftDevice
        self.rightDevice   = rightDevice

        self.laserNode     = laserNode
        self.targetType    = TargetType.Flystick
        self.targetID      = flystickID
        self.menuBtnID     = menuBtnID
        self.touchBtnID    = touchBtnID
        self.triggerBtnID  = triggerBtnID
        self.touchBtnActive = False

        self.menuDoOnce    = FlyButtonHook.DoOnce( 20)
        self.touchDoOnce   = FlyButtonHook.DoOnce( 20)
        self.triggerDoOnce = FlyButtonHook.DoOnce( 20)


    def execute( self):

        self.menuDoOnce.increment()
        self.touchDoOnce.increment()
        self.triggerDoOnce.increment()

        
        if self.module.isTracked( self.targetType, self.targetID ):

            buttons = self.module.getFlyButtons( self.targetID)

            if buttons is not None and self.triggerBtnID >= 0  and self.triggerBtnID < len( buttons):

                if dtrackLog.enabled and VRED_XRMODE_ENABLED:
                    dtrackLog.out( "vrHMDService.isHmdActive {}".format( int( vrHMDService.isHmdActive() )), every=3000)
                    dtrackLog.out( "vrHMDService.getXRRenderMode {}".format( getRenderMode() ), every=3000)


                # XR mode
                if  VRED_XRMODE_ENABLED  and vrHMDService.isHmdActive():

                    self.laserNode.setVisibilityFlag( False)

                    if self.leftDevice is not None:
                        if buttons[ self.menuBtnID] > 0  and self.menuDoOnce.available():
                            self.menuDoOnce.start()
                            dtrackLog.out( "setButtonPressed(menu)")
                            self.leftDevice.setButtonPressed( True,  "menu")
                            self.leftDevice.setButtonPressed( False, "menu")

                    if self.rightDevice is not None:

                        if buttons[ self.touchBtnID] > 0  and self.touchDoOnce.available():
                            self.touchDoOnce.start()
                            dtrackLog.out( "setButtonTouched(trigger)")
                            self.touchBtnActive = not self.touchBtnActive
                            self.rightDevice.setButtonTouched( self.touchBtnActive, "trigger")

                        if buttons[ self.triggerBtnID] > 0  and self.triggerDoOnce.available():
                            self.triggerDoOnce.start()
                            dtrackLog.out( "setButtonPressed(trigger)")
                            self.rightDevice.setButtonPressed( True,  "trigger")
                            self.rightDevice.setButtonPressed( False, "trigger")


                # Display mode
                else:

                    # Show dummy laser beam 
                    self.laserNode.setVisibilityFlag( buttons[ self.triggerBtnID] )




class FlyJoystickHook( DTrackRenderHook):
    """Example hook for applying A.R.T. Flystick joystick information"""

    def __init__( self, vrdevice, joyNode, flystickID, joyOffset, maxShift ):
        super().__init__()

        self.vrdevice      = vrdevice
        self.joyNode       = joyNode
        self.targetType    = TargetType.Flystick
        self.targetID      = flystickID
        self.maxShift      = maxShift

        # Offset from flystick origin for joystick values = 0
        self.joyOffset = np.array( [ joyOffset.x(), joyOffset.y(), joyOffset.z() ])


    def execute( self):

        if self.module.isTracked( self.targetType, self.targetID ):

            joystick = self.module.getFlyJoystick( self.targetID)

            if joystick is not None:

                # Joystick array indices: 0 horizontal, 1 vertical
                # Values are in range (-1.0,  1.0)
                jx = joystick[0] * self.maxShift
                jz = joystick[1] * self.maxShift

                # Move the joystick node along the X and Z axes 
                # in the local flystick coordinate system
                Ex = np.array( [1, 0, 0 ])
                Ez = np.array( [0, 0, -1 ])
                T  =  self.joyOffset  +  jx * Ex  + jz * Ez 
                self.joyNode.setTranslation( QVector3D( T[0], T[1], T[2] ) )



# (Temporary hack to) set the camera to a neutral relative pose when switching to XR mode.
# TODO, reset the camera after changing the view direction by dragging in the viewport.
# TODO, this assumes the dtrack room CS has Z-upwards (with Vred camera Z-backwards).
class XRCamHook( DTrackRenderHook):
    def __init__( self, hmdID ):
        super().__init__()
        self.rotation        = Rotation()
        self.targetType      = TargetType.Body
        self.targetID        = hmdID
        self.hmdState        = -1

    def execute( self):

        if VRED_XRMODE_ENABLED  and self.module.isTracked( self.targetType, self.targetID ):

            hmdState = vrHMDService.isHmdActive()
            if hmdState == True  and self.hmdState == False:

                loc = np.array( [[0,0,0]]).transpose()
                rot = self.rotation.X90

                cam = vrCameraService.getActiveCamera()
                cam.setWorldTransform(  qmatrix4d( rot, loc) )

            self.hmdState = hmdState
            


class HandHook( DTrackRenderHook):
    """Example hook for applying A.R.T. Fingertracking information"""

    def __init__( self, handRootNode, targetID, handedness = Handedness.Undefined, scaleFactor = 1.0):
        super().__init__()

        self.tracked = False        # TMP, log reset switch
        self.valid = True
        self.scaleFactor = scaleFactor

        if handedness is Handedness.Undefined:
            self.valid = False
            dtrackLog.out("handedness is Undefined")

        elif handedness is Handedness.Left:
            self.wristNode = vrNodeService.findNode( "L_hand_transformation", root=handRootNode)
            self.meshNode  = vrNodeService.findNode( "L_hand_mesh", root=handRootNode)

        else:
            self.wristNode = vrNodeService.findNode( "R_hand_transformation", root=handRootNode)
            self.meshNode  = vrNodeService.findNode( "R_hand_mesh", root=handRootNode)

        if self.wristNode is None or self.meshNode is None:
            self.valid = False
            dtrackLog.out("wristNode or meshNode is None")

        self.targetID = targetID
        self.vrHand  =  VredHandJointPoses( handedness, targetID, self.wristNode, self.meshNode)

#        vrNodeUtils.setTransformNodeScale( self.meshNode, self.scaleFactor, self.scaleFactor, self.scaleFactor)


    def execute( self):

        if self.valid is False:
            return

        # Hands currently not supported in XR
        if not VRED_XRMODE_ENABLED  or not vrHMDService.isHmdActive():

            self.meshNode.setVisibilityFlag( True)

            if self.vrHand.handedness == Handedness.Undefined or self.vrHand.handedness != self.module.getHandedness( self.targetID):
                if dtrackLog.enabled: dtrackLog.out( f"Wrong handedness for hand id {self.targetID}", every=1000)
                return


            # Update VredHandJointPoses model and apply the update to nodes

            if self.module.updateHandJointPoses( self.vrHand ):

               
                # Shift wrist so that effectively scaling tooks place around pivot
                wrist  = self.vrHand.palm["wrist"]
                wshift = self.vrHand.scalePivot * ( 1 / self.scaleFactor - 1 )
                self.vrHand.nodes.palm["wrist"].setTransform( qmatrix4d( wrist.rot,  wrist.loc + wshift )) 


                for F in range( 0, HandJointPoses.nFingers):
                #F = FingerT.Index
                #if True:

                    jointFinger = self.vrHand.finger[ F ]
                    nodeFinger  = self.vrHand.nodes.finger[ F ]
                    socket      = self.vrHand.socket[ F ]  

                    rot  =  socket.rot.transpose()   @  jointFinger.joint[ JointT.Root ].rot 
                    qmat =  nodeFinger.node[ JointT.Root ].getTransform()
                    loc  =  qmatrix4dLoc( qmat)
                    nodeFinger.node[ JointT.Root ].setTransform( qmatrix4d( rot, loc) )


                    for J in [JointT.Middle, JointT.Outer]:
                    #J = JointT.Outer
                    #if False:
                        rot  =  jointFinger.joint[ J-1 ].rot.transpose()  @  jointFinger.joint[ J ].rot 
                        qmat = nodeFinger.node[ J ].getTransform()
                        loc  = qmatrix4dLoc( qmat)
                        nodeFinger.node[ J ].setTransform( qmatrix4d( rot, loc) )

                self.tracked = True

            else:
                if dtrackLog.enabled: dtrackLog.out( "Hand id {} not tracked".format( self.targetID), every=1000, reset=self.tracked )
                self.tracked = False

        else:
            self.meshNode.setVisibilityFlag( False)