dtrack_base.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.




from dtrack_math import *
from dtrack_logger import *

from enum import IntEnum

from vrAEBase import vrAEBase



DTRACK_LOG_ENABLED = True
DTRACK_HANDS_ENABLED = True
DTRACK_GLOBAL_SCALE = 1.0
VRED_XRMODE_ENABLED = True


#------------------------------------------------------------------------------
# TargetType
#------------------------------------------------------------------------------

class TargetType( IntEnum ):
    """Enumeration of A.R.T. target types"""

    Body = 0
    Flystick = 1
    Hand = 2
    # MeaTool = 3


#------------------------------------------------------------------------------
# DTrackData
#------------------------------------------------------------------------------

class DTrackData:
   
    def __init__(self):
        
        self.mutex = None
        self.frame = (-1, -1)  # Local and DTrack frame counter
        self.body = None
        self.flystick = None
        self.hand = None



#------------------------------------------------------------------------------
# VredDTrackState
#------------------------------------------------------------------------------

class VredDTrackState( IntEnum ):
    """Module states and error codes"""

    NotInitialized = 0
    Stopped   = 1
    Receiving = 2
    NoData    = 3
    PortInvalid = 4
    NoConnection = 5



#------------------------------------------------------------------------------
# Fingertracking
#------------------------------------------------------------------------------

class Handedness( IntEnum ):
    Undefined = -1,
    Left  = 0,
    Right = 1  

class JointT( IntEnum):
    Root   = 0,
    Middle = 1,
    Outer  = 2,
    Tip    = 3


class FingerT( IntEnum):
    Thumb  = 0,
    Index  = 1,
    Middle = 2,
    Ring   = 3,
    Pinky  = 4



class FingerJointNodes:
    nNodes = 4

    def __init__(self):
        self.node = [None] * FingerJointNodes.nNodes


class HandJointNodes:
    nFingers = 5

    def __init__(self):

        self.palm = dict()

        self.finger = [None] * HandJointNodes.nFingers
        self.finger[ FingerT.Thumb ] = FingerJointNodes()
        self.finger[ FingerT.Index ] = FingerJointNodes()
        self.finger[ FingerT.Middle] = FingerJointNodes()
        self.finger[ FingerT.Ring  ] = FingerJointNodes()
        self.finger[ FingerT.Pinky ] = FingerJointNodes()


class JointPose:

    def __init__( self, rot=None, loc=None):
        self.rot = rot
        self.loc = loc


class FingerJointPoses:
    nJoints = 4

    def __init__( self):

        self.joint = [None] * FingerJointPoses.nJoints
        self.joint[ JointT.Root  ]  = JointPose()
        self.joint[ JointT.Middle]  = JointPose()
        self.joint[ JointT.Outer ]  = JointPose()
        self.joint[ JointT.Tip   ]  = JointPose()


class HandJointPoses:
    nFingers = 5

    def __init__( self, handedness, targetID):

        self.handedness = handedness
        self.targetID   = targetID
        self.isTracked  = False

        self.palm = dict()

        self.finger = [None] * HandJointPoses.nFingers
        self.finger[ FingerT.Thumb ] = FingerJointPoses()
        self.finger[ FingerT.Index ] = FingerJointPoses()
        self.finger[ FingerT.Middle] = FingerJointPoses()
        self.finger[ FingerT.Ring  ] = FingerJointPoses()
        self.finger[ FingerT.Pinky ] = FingerJointPoses()


    def handednessTag( self):
        lr = "U"
        if   self.handedness == Handedness.Left:  lr = "L"
        elif self.handedness == Handedness.Right: lr = "R"
        return lr


class ArtHandJointPoses( HandJointPoses):

    def __init__( self, handedness, targetID ):
        super().__init__( handedness, targetID)

        # ART hand base coordinate system (at index finger root).
        # See `DTRACK3_Programmers-Guide.pdf`
        self.palm["base"]  = JointPose()



# For now we only support the hands 'R/L_hand.osb' shipped with Vred
class VredHandJointPoses( HandJointPoses):

    def __init__( self, handedness, targetID, wristNode, meshNode):
        super().__init__( handedness, targetID)

        # The `wrist` corresponds to `L_hand_transformation`, in World CS.
        # Calculated from `ArtHandJointPoses.base` and `wrist_artBase`
        self.palm["wrist"] = JointPose()


        # Finger sockets (palm joints with connection to finger roots), in World CS

        self.palm["palmEnd"]   = JointPose()
        self.palm["thumbBall"] = JointPose()
        self.palm["pinkyBase"] = JointPose()

        self.socket = [None] * HandJointPoses.nFingers
        self.socket[ FingerT.Thumb ] = self.palm["thumbBall"]
        self.socket[ FingerT.Index ] = self.palm["palmEnd"]
        self.socket[ FingerT.Middle] = self.palm["palmEnd"]
        self.socket[ FingerT.Ring  ] = self.palm["palmEnd"]
        self.socket[ FingerT.Pinky ] = self.palm["pinkyBase"]


        # Constant relative palm joint poses from current node trafos

        self.relative = dict()

        # Constant pose of `wrist` relative to `ArtHandJointPoses.base`.
        # From VRED hand model without re-scaling.
        self.relative["wrist_artBase"]   = JointPose()

        # Constant poses of finger sockets relative to `wrist`.
        # From VRED hand model without re-scaling.
        self.relative["palmEnd_wrist"]   = JointPose()
        self.relative["thumbBall_wrist"] = JointPose()
        self.relative["pinkyBase_wrist"] = JointPose()


        wristWT       = wristNode.getWorldTransform()
        ixRootWT      = vrNodeService.findNode( self.handednessTag() + "_index_1_JNT",    root=wristNode ).getWorldTransform()
        palmEndWT     = vrNodeService.findNode( self.handednessTag() + "_palmEnd_JNT",    root=wristNode ).getWorldTransform()
        pinkyBaseWT   = vrNodeService.findNode( self.handednessTag() + "_pinkyBase_JNT",  root=wristNode ).getWorldTransform()
        thumbBallWT   = vrNodeService.findNode( self.handednessTag() + "_thumb_ball_JNT", root=wristNode ).getWorldTransform()

        wristJP     = JointPose( qmatrix4dRot( wristWT),     qmatrix4dLoc( wristWT) )
        ixRootJP    = JointPose( qmatrix4dRot( ixRootWT),    qmatrix4dLoc( ixRootWT) )
        palmEndJP   = JointPose( qmatrix4dRot( palmEndWT),   qmatrix4dLoc( palmEndWT) )
        pinkyBaseJP = JointPose( qmatrix4dRot( pinkyBaseWT), qmatrix4dLoc( pinkyBaseWT) )
        thumbBallJP = JointPose( qmatrix4dRot( thumbBallWT), qmatrix4dLoc( thumbBallWT) )


        # `artBaseJP`: The ART hand coordinate system `ArtHandJointPoses.palm["base"]` 
        # expressed through VRED joint poses. May still need some finetuning (TODO).
        flip_  =  np.eye(3,3)  if self.handedness == Handedness.Left else  Rotation.Y( 180)
        artBaseJP   = JointPose(  wristJP.rot @ flip_, ixRootJP.loc )


        self.relative["wrist_artBase"].rot    =  artBaseJP.rot.transpose()  @  wristJP.rot
        self.relative["wrist_artBase"].loc    =  artBaseJP.rot.transpose()  @  ( wristJP.loc - artBaseJP.loc )
          
        self.relative["palmEnd_wrist"].rot    =  wristJP.rot.transpose()    @  palmEndJP.rot                              
        self.relative["palmEnd_wrist"].loc    =  wristJP.rot.transpose()    @  ( palmEndJP.loc - wristJP.loc )

        self.relative["thumbBall_wrist"].rot  =  wristJP.rot.transpose()    @  thumbBallJP.rot                            
        self.relative["thumbBall_wrist"].loc  =  wristJP.rot.transpose()    @  ( thumbBallJP.loc - wristJP.loc )

        self.relative["pinkyBase_wrist"].rot  =  wristJP.rot.transpose()    @  pinkyBaseJP.rot                            
        self.relative["pinkyBase_wrist"].loc  =  wristJP.rot.transpose()    @  ( pinkyBaseJP.loc - wristJP.loc )


        # Memorize relevant nodes

        self.nodes = HandJointNodes()

        self.nodes.palm["wrist"]  = wristNode        # L/R_hand_transformation 

        for N in range( 0, 4):
            self.nodes.finger[ FingerT.Thumb ].node[N]  = vrNodeService.findNode( self.handednessTag() + "_thumb_"  + str(N+1) + "_JNT",  root=wristNode )
            self.nodes.finger[ FingerT.Index ].node[N]  = vrNodeService.findNode( self.handednessTag() + "_index_"  + str(N+1) + "_JNT",  root=wristNode )
            self.nodes.finger[ FingerT.Middle].node[N]  = vrNodeService.findNode( self.handednessTag() + "_middle_" + str(N+1) + "_JNT",  root=wristNode )
            self.nodes.finger[ FingerT.Ring  ].node[N]  = vrNodeService.findNode( self.handednessTag() + "_ring_"   + str(N+1) + "_JNT",  root=wristNode )
            self.nodes.finger[ FingerT.Pinky ].node[N]  = vrNodeService.findNode( self.handednessTag() + "_pinky_"  + str(N+1) + "_JNT",  root=wristNode )


        # Scaling

        self.scalePivot = artBaseJP.loc





#------------------------------------------------------------------------------
# DTrackRenderHook
#------------------------------------------------------------------------------

class DTrackRenderHook( vrAEBase):
    """Hook to execute code in the VRED render loop"""
    
    def __init__( self ):
        vrAEBase.__init__(self)
        
        self.lastFrame = -1
        self.logReset = False
        self.module = None       # VredDTrack instance, with access to DTrackData
        self.hooks = []
       
            
    def recEvent( self, state ):
        vrAEBase.recEvent( self, state)


    def _execute( self):

        if self.module is None:
            dtrackLog.out( "self.module is None", every=1000)
            return

        if self.module.dtrackData is None:
            dtrackLog.out( "self.module.dtrackData is None", every=1000)
            return
    
        if self.module.dtrackData.mutex.try_lock():
           
            # Check if new frame received
            if self.lastFrame >= self.module.dtrackData.frame[0]:
                self.module.dtrackData.mutex.unlock()
                if dtrackLog.enabled:
                    dtrackLog.out( "lastFrame >= dtrackData.frame ({} >= {})"
                        .format( self.lastFrame, self.module.dtrackData.frame[0]),
                        every=1000, reset=self.logReset )
                    self.logReset = False
                return
            
            self.logReset = True
            self.lastFrame = self.module.dtrackData.frame[0]

            self.execute()

            self.module.dtrackData.mutex.unlock()


    def loop( self):
        self._execute()

    # Override
    def execute( self):
        pass