mocapstream/MocapStreamRokokoHDK.C

/*
 * PROPRIETARY INFORMATION.  This software is proprietary to
 * Side Effects Software Inc., and is not to be reproduced,
 * transmitted, or disclosed in any way without written permission.
 *
 * NAME:        MocapStreamRokokoHDK.C ( KineFX Library, C++)
 * 
 * The parser and server receiver for motion capture data from Rokoko device.
 * This is used by the MocapStream SOP.
 *
 * Information for receiving and parsing the data can be found here:
 * https://support.rokoko.com/hc/en-us/articles/4410416376977-Custom-Streaming
 */
#include "MocapStreamRokokoHDK.h"

#include <OP/OP_Director.h>
#include <PRM/PRM_TemplateBuilder.h>

#include <MC/MC_LZ4Decompressor.h>

#include <GEO/GEO_StandardAttribs.h>
#include <FS/FS_EventGenerator.h>
#include <UT/UT_UniquePtr.h>
#include <UT/UT_NetSocket.h>
#include <UT/UT_NetFDSet.h>
#include <UT/UT_Thread.h>
#include <UT/UT_Options.h>
#include <UT/UT_ConcurrentVector.h>
#include <UT/UT_ScopeExit.h>
#include <UT/UT_JSONValue.h>
#include <UT/UT_JSONValueArray.h>
#include <UT/UT_JSONValueMap.h>
#include <SYS/SYS_Pragma.h>

SYS_PRAGMA_PUSH_WARN()
SYS_PRAGMA_DISABLE_OVERLOADED_VIRTUAL()

#include <hboost/asio.hpp>

SYS_PRAGMA_POP_WARN()

using namespace HDK_Sample;
using namespace UT::Literal;

// Initialization function. 
void
newMocapStreamDevice(void *)
{
    static const MC_MocapStreamImpl::Register<MocapStreamRokokoHDK> impl;
}

// A custom server function which establishes a UDP connection with the computer
// running Rokoko Studio.
//
// Once it receives the data, the data is decompressed and then passed onto the
// device using the MC_MocapStreamImpl::ServerState::gotPacket(...) function.
void
rokokoServer(MC_MocapStreamImpl::ServerState& state,
             const MC_MocapStreamImpl::ServerOptions& options,
             MC_MocapStreamImpl *data)
{
    using hboost::asio::ip::udp;
    using hboost::asio::ip::address;

    try
    {
        // Rokoko's data streaming protocol uses LZ4 compression so we need
        // to create an LZ4 decompressor for this.
        MC_LZ4Decompressor decompressor;

        // Open a UDP socket so that we can state receiving data.
        hboost::asio::io_service io_service;
        udp::socket socket(io_service, udp::endpoint(udp::v4(), options.myPort));

        // sizes from the unreal implementation
        UT_Array<char> buf(options.myBufferSize, options.myBufferSize);
        UT_Array<char> uncompressed_buf(options.myBufferSize*64, options.myBufferSize*64);

        int native_socket = socket.native_handle();
        UT_NetFDSet fd_set;
        fd_set.add(native_socket, UT_NetFDEvent::NETFD_READ);
        
        // Tell the ServerState that the port has been opened. If this is not
        // done, the callback for the "Connect" button will not end.
        state.setPortOpened(true);

        // Loop until the server is killed. This happens when the "Disconnect"
        // button is pressed on the MotionStream SOP.
        while (!state.killed())
        {
            udp::endpoint remote_endpoint;
            hboost::system::error_code error;

            // Synchronous boost sockets don't support timouts,
            // so we'll do it manually with the native handle.

            // Check the events on the socket to see if we have receive new
            // data.
            int res = fd_set.checkEvents(100);
            if (res <= 0)
            {
                // If no data was received within the time limit, add this
                // to the log.
                state.setError("Request timed out...");
                continue;
            }

            // Get the data from the socket.
            size_t num_read = socket.receive_from(
                hboost::asio::buffer(buf.data(), buf.size()),
                remote_endpoint, 0, error);

            // If we ran into an error getting the data, throw the error.
            // This is caught, a message is added to the log, and then the
            // connection is ended.
            if (error && error != hboost::asio::error::message_size)
                throw hboost::system::system_error(error);

            // Once we have received a packet, tell the server state that we are
            // connected.
            state.setConnected(true);

            // Decompress the packet
            size_t src_size = num_read;
            size_t dst_size = uncompressed_buf.size();
            size_t decompress_result;
            decompress_result = decompressor.decompress(
                    uncompressed_buf.data(), dst_size, buf.data(), src_size);
            
            if (decompress_result == 0)
            {
                // If we successfully decompressed the data, reset the server
                // state's error and then call ServerState::getPacket(...)
                // to queue the decompressed packet for parsing.
                state.resetError();
                state.gotPacket(UT_StringHolder(uncompressed_buf.data(), dst_size));
            }
            else
            {
                // Otherwise, we send an error to the log.
                state.setError("Could not decompress packet");
            }
        }
    }
    catch(hboost::system::system_error& e)
    {
        // If a boost error was encountered, add the error message to the log.
        MC_MocapStreamImpl::handleBoostError(e, state);
    }
}

// Define the server function and options in the contructor for the device.
//
// Either the MC_MocapStreamImpl::basicUdpReceiver or
// MC_MocapStreamImpl::basicTcpReceiver function could be used instead of
// rokokoServer for a more simple protocol.
MocapStreamRokokoHDK::MocapStreamRokokoHDK(const MC_MocapStreamImpl::ServerOptions& opts)
    : MC_MocapStreamImpl(rokokoServer, opts)
{ }

MocapStreamRokokoHDK::~MocapStreamRokokoHDK()
{ }

// Rokoko uses a pre-defined skeleton for streaming, the following arrays define
// the joints names, their parents, and their world space rest transforms.
UT_StringArray JOINTS{
    "hip",
    "leftUpLeg", "leftLeg", "leftFoot", "leftToe", "leftToeEnd",
    "rightUpLeg", "rightLeg", "rightFoot", "rightToe", "rightToeEnd",
    "spine", "chest",
    "leftShoulder", "leftUpperArm", "leftLowerArm","leftHand",
    "leftIndexProximal", "leftIndexMedial", "leftIndexDistal", "leftIndexTip",
    "leftLittleProximal", "leftLittleMedial", "leftLittleDistal", "leftLittleTip",
    "leftMiddleProximal", "leftMiddleMedial", "leftMiddleDistal", "leftMiddleTip",
    "leftRingProximal", "leftRingMedial", "leftRingDistal", "leftRingTip",
    "leftThumbProximal", "leftThumbMedial", "leftThumbDistal", "leftThumbTip",
    "neck", "head",
    "rightShoulder", "rightUpperArm", "rightLowerArm", "rightHand",
    "rightIndexProximal", "rightIndexMedial", "rightIndexDistal", "rightIndexTip",
    "rightLittleProximal", "rightLittleMedial", "rightLittleDistal", "rightLittleTip",
    "rightMiddleProximal", "rightMiddleMedial", "rightMiddleDistal", "rightMiddleTip",
    "rightRingProximal", "rightRingMedial", "rightRingDistal", "rightRingTip",
    "rightThumbProximal", "rightThumbMedial", "rightThumbDistal", "rightThumbTip",
};

UT_Array<exint> PARENTS{
    -1,             // Hips
    0, 1, 2, 3, 4,  // Left Leg
    0, 6, 7, 8, 9,  // Right Leg
    0, 11,          // Spine
    12, 13, 14, 15, // Left Arm
    16, 17, 18, 19, // Left Index Finger
    16, 21, 22, 23, // Left Litter Finger
    16, 25, 26, 27, // Left Middle Finger
    16, 29, 30, 31, // Left Ring Finder
    16, 33, 34, 35, // Left Thumb
    12, 37,         // Neck
    12, 39, 40, 41, // Right Arm 
    42, 43, 44, 45, // Right Index Finger
    42, 47, 48, 49, // Right Litter Finger
    42, 51, 52, 53, // Right Middle Finger
    42, 55, 56, 57, // Right Ring Finder
    42, 59, 60, 61, // Right Thumb
};

UT_Array<UT_Matrix4F> REST_XFORMS{
    UT_Matrix4F(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0.9449999928474426, 0.02500000037252903, 1),
    UT_Matrix4F(0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0.08799999952316284, 0.9449999928474426, 0.02500000037252903, 1),
    UT_Matrix4F(0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0.08799999952316284, 0.4740000069141388, 0.02500000037252903, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, 0.08900000154972076, 0.002000000094994903, 0.02500000037252903, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, 0.08900000154972076, -0.06599999964237213, 0.1679999977350235, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, 0.08900000154972076, -0.07999999821186066, 0.2489999979734421, 1),
    UT_Matrix4F(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0, -0.08900000154972076, 0.9449999928474426, 0.02500000037252903, 1),
    UT_Matrix4F(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0, -0.08900000154972076, 0.4740000069141388, 0.02500000037252903, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -0.08900000154972076, 0.002000000094994903, 0.02500000037252903, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -0.08900000154972076, -0.06599999964237213, 0.1679999977350235, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -0.08900000154972076, -0.07999999821186066, 0.2489999979734421, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 1.036999940872192, -0.01799999922513962, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 1.202999949455261, -0.02099999971687794, 1),
    UT_Matrix4F(0, 1, 0, 0, -0.9661348462104797, 0, 0.258037656545639, 0, 0.258037656545639, 0, 0.9661348462104797, 0, 0.07699999958276749, 1.478000044822693, 0, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.221000000834465, 1.478000044822693, -0.03799999877810478, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.5049999952316284, 1.478000044822693, -0.03799999877810478, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.8190000057220459, 1.478000044822693, -0.03799999877810478, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9070000052452087, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9589999914169312, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9900000095367432, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 1.021000027656555, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.8960000276565552, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9369999766349792, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9639999866485596, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9900000095367432, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9079999923706055, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9599999785423279, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9950000047683716, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 1.027999997138977, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9049999713897705, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9539999961853027, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0.9850000143051147, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 1.016000032424927, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(-0.3532975912094116, -0.7077658176422119, 0.6117584705352783, 0, -0.8660871982574463, 0.0002441254764562473, -0.4998929500579834, 0, 0.3536577820777893, -0.7064471244812012, -0.6130731105804443, 0, 0.8420000076293945, 1.476999998092651, -0.00800000037997961, 1),
    UT_Matrix4F(0, -0.7061478495597839, 0.7080644369125366, 0, -1, 0, 0, 0, 0, -0.7080644369125366, -0.7061478495597839, 0, 0.8790000081062317, 1.476999998092651, 0.01400000043213367, 1),
    UT_Matrix4F(0, -0.7061478495597839, 0.7080644369125366, 0, -1, 0, 0, 0, 0, -0.7080644369125366, -0.7061478495597839, 0, 0.9120000004768372, 1.476999998092651, 0.01400000043213367, 1),
    UT_Matrix4F(0, -0.7061478495597839, 0.7080644369125366, 0, -1, 0, 0, 0, 0, -0.7080644369125366, -0.7061478495597839, 0, 0.949999988079071, 1.476999998092651, 0.01400000043213367, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 1.564000010490417, -0.02099999971687794, 1),
    UT_Matrix4F(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 1.677999973297119, -0.02099999971687794, 1),
    UT_Matrix4F(0, -1, 0, 0, 0.9661348462104797, 0, 0.258037656545639, 0, -0.258037656545639, 0, 0.9661348462104797, 0, -0.07599999755620956, 1.478000044822693, 0, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.2199999988079071, 1.478000044822693, -0.03799999877810478, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.5040000081062317, 1.478000044822693, -0.03799999877810478, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.8180000185966492, 1.478000044822693, -0.03799999877810478, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9070000052452087, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9580000042915344, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9890000224113464, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -1.019999980926514, 1.476999998092651, -0.009999999776482582, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.8949999809265137, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9369999766349792, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9629999995231628, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9890000224113464, 1.476999998092651, -0.07299999892711639, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9079999923706055, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9599999785423279, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9940000176429749, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -1.026999950408936, 1.476999998092651, -0.03200000151991844, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.9039999842643738, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.953000009059906, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -0.984000027179718, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, -1.014999985694885, 1.476999998092651, -0.05400000140070915, 1),
    UT_Matrix4F(-0.3532975912094116, 0.7077658176422119, -0.6117584705352783, 0, 0.8660871982574463, 0.0002441254764562473, -0.4998929500579834, 0, -0.3536577820777893, -0.7064471244812012, -0.6130731105804443, 0, -0.8410000205039978, 1.476999998092651, -0.00800000037997961, 1),
    UT_Matrix4F(0, 0.7061478495597839, -0.7080644369125366, 0, 1, 0, 0, 0, 0, -0.7080644369125366, -0.7061478495597839, 0, -0.878000020980835, 1.476999998092651, 0.01400000043213367, 1),
    UT_Matrix4F(0, 0.7061478495597839, -0.7080644369125366, 0, 1, 0, 0, 0, 0, -0.7080644369125366, -0.7061478495597839, 0, -0.9120000004768372, 1.476999998092651, 0.01400000043213367, 1),
    UT_Matrix4F(0, 0.7061478495597839, -0.7080644369125366, 0, 1, 0, 0, 0, 0, -0.7080644369125366, -0.7061478495597839, 0, -0.9490000009536743, 1.476999998092651, 0.01400000043213367, 1),
};

// The function which parses the decompressed packets defined earlier.
// Rokoko studio uses JSON to define its skeleton and rigid body data.
//
// This should return true if the packet was successfully parsed and contained
// skeleton or rigid body data. In any other situation, it should return false.
bool
MocapStreamRokokoHDK::parsePacket(
    UT_StringHolder& packet,
    fpreal packet_time,
    bool &parse_incomplete)
{
    UT_JSONValue json;
    if (!json.parseValue(packet))
    {
        return false;
    }
    
    UT_JSONValueMap *root = json.getMap();
    UT_JSONValueMap *scene = root->getObject("scene");
    UT_JSONValueArray *actors = scene->getArray("actors");

    fpreal version = 2.0f;
    const UT_JSONValue *version_val = scene->get("version");
    if (version_val)
        version = version_val->getF();

    // Set the packet time based upon the timestamp of the pose sent by
    // Rokoko Studio. This is used when recording the stream and storing
    // the poses in a MotionClip.
    UT_JSONValue *timestamp_val = scene->get("timestamp");
    if (timestamp_val)
        setPacketTime(timestamp_val->getF());

    bool is_missing_joints = false;
    int num_actors = actors->entries();
    int num_joints = JOINTS.size();
    for (int i = 0; i < num_actors; i++)
    {
        UT_JSONValueMap *actor = actors->getObject(i);
        UT_JSONValueMap *body = actor->getObject("body");
        const UT_JSONValue *actor_name = actor->get("name");
        if (!actor_name)
            continue;

        const char *name = actor_name->getS();
        // Always make sure to note when a skeleton or other kind of motion
        // capture object was received. This tells the Mocap Stream SOP
        // to update its skeleton.
        receivedSkeleton(name);

        // Get the meta-data for the skeleton
        bool has_left_glove = true;
        bool has_right_glove = true;
        bool has_body = true;
        bool has_face = true;
        UT_JSONValueMap *meta = actor->getObject("meta");
        if (version >= 3.0f && meta)
        {
            const UT_JSONValue *has_gloves_value = meta->get("hasGloves");
            const UT_JSONValue *has_left_glove_value = meta->get("hasLeftGolve");
            const UT_JSONValue *has_right_glove_value = meta->get("hasRightGlove");
            const UT_JSONValue *has_body_value = meta->get("hasBody");
            const UT_JSONValue *has_face_value = meta->get("hasFace");

            bool has_gloves = (has_gloves_value && has_gloves_value->getB());
            has_left_glove = has_gloves && has_left_glove_value
                             && has_left_glove_value->getB();
            has_right_glove = has_gloves && has_right_glove_value
                              && has_right_glove_value->getB();
            has_body = has_body_value && has_body_value->getB();
            has_face = has_face_value && has_face_value->getB();
        }

        // Get/initialise  skeletona
        Actor *skeleton = mySkeletons.findActor(name);
        if (!skeleton)
        {
            skeleton = mySkeletons.addActor(name);
            skeleton->myPoints.setSize(num_joints);
        }
        skeleton->myIsDirty = true;

        for (int j = 0; j < num_joints; j++)
        {
            ActorPoint &joint = skeleton->myPoints[j];
        
            const UT_StringHolder& joint_name = JOINTS[j];
            UT_JSONValueMap *joint_data = body->getObject(joint_name);
            UT_JSONValueMap *joint_pos, *joint_rot;
            const UT_JSONValue *pos_x, *pos_y, *pos_z;
            const UT_JSONValue *rot_x, *rot_y, *rot_z, *rot_w;
            if (joint_data)
            {
                joint_pos = joint_data->getObject("position");
                joint_rot = joint_data->getObject("rotation");
                if (joint_pos && joint_rot)
                {
                    pos_x = joint_pos->get("x");
                    pos_y = joint_pos->get("y");
                    pos_z = joint_pos->get("z");

                    rot_x = joint_rot->get("x");
                    rot_y = joint_rot->get("y");
                    rot_z = joint_rot->get("z");
                    rot_w = joint_rot->get("w");
                }
            }

            if (!joint_data || !joint_pos || !joint_rot
                || !pos_x || !pos_y || !pos_z
                || !rot_x || !rot_y || !rot_z || !rot_w)
            {
                joint.myDidReceive = false;

                if (joint_name.fcontain("Index")
                    || joint_name.fcontain("Little")
                    || joint_name.fcontain("Middle")
                    || joint_name.fcontain("Ring")
                    || joint_name.fcontain("Thumb"))
                {
                    bool is_right_hand = joint_name.fcontain("right");
                    is_missing_joints = (is_right_hand && has_right_glove)
                                        || (!is_right_hand && has_left_glove);
                }
                else if (has_body)
                {
                    is_missing_joints = true;
                }

                continue;
            }

            UT_Vector3F pos(-pos_x->getF(), pos_y->getF(), pos_z->getF());
            UT_QuaternionF rot(-rot_x->getF(),  rot_y->getF(),
                                rot_z->getF(), -rot_w->getF());

            UT_Matrix3F rot_mat;
            rot.getRotationMatrix(rot_mat);

            joint.myWorldXform = rot_mat;
            joint.myWorldXform.setTranslates(pos);

            joint.myDidReceive = true;
        }

        // We'll just throw the face blendShapes right onto detail attributes.
        UT_JSONValueMap *face = actor->getObject("face");
        if (face && has_face)
        {
            skeleton->myBlendShapes.clear();
            for (const auto& item : *face)
            {
                UT_JSONValue *val = item.second;
                if (val->getType() == UT_JSONValue::JSON_REAL)
                {
                    // Ensure the blend shapes match the 0-1 range supported
                    // by Houdini
                    skeleton->myBlendShapes[item.first] = val->getF()/100.f;
                }
            }
        }
    }

    if (is_missing_joints)
        addParseWarning("Did not receive data for every joint.");

    // we'll treat a prop as a skeleton with a single point
    UT_JSONValueArray *props = scene->getArray("props");
    int num_props = props->entries();
    for (int i = 0; i < num_props; i++)
    {
        UT_JSONValueMap *prop = props->getObject(i);
        const UT_JSONValue *prop_name = prop->get("name");
        if (!prop_name)
            continue;

        const char *name = prop_name->getS();

        UT_JSONValueMap *joint_pos = prop->getObject("position");
        UT_JSONValueMap *joint_rot = prop->getObject("rotation");
        if (!joint_pos || !joint_rot)
            continue;

        const UT_JSONValue *pos_x = joint_pos->get("x");
        const UT_JSONValue *pos_y = joint_pos->get("y");
        const UT_JSONValue *pos_z = joint_pos->get("z");

        const UT_JSONValue *rot_x = joint_rot->get("x");
        const UT_JSONValue *rot_y = joint_rot->get("y");
        const UT_JSONValue *rot_z = joint_rot->get("z");
        const UT_JSONValue *rot_w = joint_rot->get("w");
        if (!pos_x || !pos_y || !pos_z
            || !rot_x || !rot_y || !rot_z || !rot_w)
            continue;

        receivedSkeleton(name);

        // Get/initialise a prop
        Actor *device_prop = myProps.findActor(name);
        if (!device_prop)
        {
            device_prop = myProps.addActor(name);
            device_prop->myPoints.setSize(1);
        }

        UT_Vector3F pos(-pos_x->getF(), pos_y->getF(), pos_z->getF());
        UT_QuaternionF rot(-rot_x->getF(),  rot_y->getF(),
                            rot_z->getF(), -rot_w->getF());

        UT_Matrix3F rot_mat;
        rot.getRotationMatrix(rot_mat);

        ActorPoint &point = device_prop->myPoints[0];
        point.myWorldXform = rot_mat;
        point.myWorldXform.setTranslates(pos);

        point.myDidReceive = true;
    }

    return true;
}

// This function computes the local transforms of all of the joints that were
// just received and uses the previously received local transforms of the other
// joints to compute the world space transform of each joints.
//
// Rokoko Studio can be configured to not send fingure data. This ensures that
// the fingures are always place appropriately on the skeleton.
void
MocapStreamRokokoHDK::computeSkeletonTransforms()
{
    int num_skeletons = mySkeletons.size();
    int num_joints = JOINTS.size();
    for (int i = 0; i < num_skeletons; i++)
    {
        Actor &skeleton = mySkeletons(i);
        if (!skeleton.myIsDirty)
            continue;
 
        skeleton.myIsDirty = false;       
        if (skeleton.myPoints.size() != JOINTS.size())
            continue;

        bool received_all_points = true;

        // Compute the local transforms of the joints
        for (int j = 0; j < num_joints; j++)
        {
            // If a joint and its parent were received, compute the joint's
            // local transform. Otherwise, we use the last received local
            // transform.
            ActorPoint &joint = skeleton.myPoints[j];
            if (!joint.myDidReceive)
            {   
                received_all_points = false;
                continue;
            }

            if (PARENTS[j] < 0)
            {
                joint.myLocalXform = joint.myWorldXform;
                continue;
            }

            ActorPoint &parent = skeleton.myPoints[PARENTS[j]];
            if (!parent.myDidReceive)
                continue;

            UT_Matrix4F parent_xform_inv = parent.myWorldXform;
            parent_xform_inv.invert();

            joint.myLocalXform = joint.myWorldXform * parent_xform_inv;
        }

        if (received_all_points)
            continue;
        
        // Compute the world transforms of the joints
        for (int j = 0; j < num_joints; j++)
        {
            ActorPoint &joint = skeleton.myPoints[j];
            if (PARENTS[j] < 0)
            {
                joint.myWorldXform = joint.myLocalXform;
                continue;
            }

            ActorPoint &parent = skeleton.myPoints[PARENTS[j]];
            joint.myWorldXform = joint.myLocalXform * parent.myWorldXform;
        }
    }
}

// This function is called by MC_MocapStreamImpl to update the joints of the 
// streamed skeletons on the geometry for the MocapStream SOP.
void
MocapStreamRokokoHDK::updateJoints(
    GU_Detail *gdp,
    const MC_MocapStreamCookParms &cookparms,
    const MC_MocapStreamCookParms &prev_cookparms)
{
    GA_ROHandleS actor_name_h(gdp->addStringTuple(
        GA_ATTRIB_POINT, theActorAttribName, 1));
    GA_RWHandleM3D transform_h = GEO_StandardAttribs::createTransform3(*gdp);

    // Compute the local and world transforms of the skeletons
    computeSkeletonTransforms();

    // Reset the number of times we've seen a point for each skeleton to 0
    mySkeletons.resetOccurances();
    myProps.resetOccurances();

    // Keep a record of the first actor we see that has blendshapes.
    // This will be used to define the blendshapes for the output geometry.
    // Since we do this on updateJoints, the actor filter will work to select
    // the blendshapes for different skeletons.
    MocapStreamRokokoHDK::Actor *first_blendshapes_actor = nullptr;

    // Set the world spaces transforms for each joint
    GA_Offset ptoff;
    GA_FOR_ALL_PTOFF(gdp, ptoff)
    {
        const auto& actor_name = actor_name_h.get(ptoff);
        
        // Get the actor that corresponds to the point
        MocapStreamRokokoHDK::Actor *actor = mySkeletons.findActor(actor_name);

        if (!actor)
            actor = myProps.findActor(actor_name);
        else if (!first_blendshapes_actor && actor->myBlendShapes.size() > 0)
            first_blendshapes_actor = actor;

        if (!actor)
            continue;

        // Get the number of times we've seen this actor. This tells us which
        // point to set.
        int i = actor->myOccurances;
        actor->myOccurances++;
        if (i >= actor->myPoints.entries())
            continue;

        // Set the transform of the point.
        MocapStreamRokokoHDK::ActorPoint &point = actor->myPoints(i);
        
        UT_Matrix3F transform(point.myWorldXform);
        transform_h.set(ptoff, transform);

        UT_Vector3F pos;
        point.myWorldXform.getTranslates(pos);
        gdp->setPos3(ptoff, pos);
    }
    gdp->getP()->bumpDataId();
    transform_h.bumpDataId();

    // Get our previous list of blendshapes for this node
    UT_StringArray &prev_blendshapes = myNodeBlendShapes[cookparms.myNodePath];

    // If no skeletons with blendshapes were seen, remove the blendshapes
    // attributes and exit early.
    if (!first_blendshapes_actor)
    {
        for (const auto &blendshape : prev_blendshapes)
        {
            UT_StringHolder atr_name = prev_cookparms.myFacialAttribs;
            atr_name.substitute("*", blendshape);
            gdp->destroyAttribute(GA_ATTRIB_DETAIL, GA_SCOPE_PUBLIC, atr_name);
        }
        prev_blendshapes.clear();
        return;
    }

    // Destroy any blendshapes attributes which were on the previous geometry
    // but not on this one.
    for (const auto &blendshape : prev_blendshapes)
    {
        if (first_blendshapes_actor->myBlendShapes.contains(blendshape))
            continue;
        
        UT_StringHolder atr_name = prev_cookparms.myFacialAttribs;
        atr_name.substitute("*", blendshape);
        gdp->destroyAttribute(GA_ATTRIB_DETAIL, GA_SCOPE_PUBLIC, atr_name);
    }
    prev_blendshapes.clear();

    // Set the blendshapes of the geometry to the blendshapes
    // of the first seen actor that had blendshapes
    for (auto& item : first_blendshapes_actor->myBlendShapes)
    {
        // Call the MC_MocapStreamImpl::updateAttribName function to update
        // the name of the blend shapes's detail attribute names based upon
        // the Facial Attributes parameter.
        UT_StringHolder attrib_name =
            updateAttribName(gdp, item.first, cookparms.myFacialAttribs, 
                             prev_cookparms.myFacialAttribs);
        gdp->setDetailAttributeF(attrib_name, item.second);

        // Added the non-renamed blendshape to our list for this node
        prev_blendshapes.append(item.first);
    }
}

// This function is used by MC_MocapStreamImpl to clear the recorded data
// for a skeleton. The most common use case for this would be when the data for
// a particular skeleton stops being streamed.
void
MocapStreamRokokoHDK::removeSkeleton(const UT_StringHolder& name)
{
    if (mySkeletons.removeActor(name))
        return;

    myProps.removeActor(name);
}

// This function is used by MC_MocapStreamImpl to add any new skeletons or
// rigid bodies to the MocapStream SOPs geometry.
void
MocapStreamRokokoHDK::buildSkeleton(GU_Detail *gdp, UT_StringHolder name)
{
    GA_RWHandleS joint_name_h(gdp->addStringTuple(
        GA_ATTRIB_POINT, GA_Names::name, 1));
    GA_RWHandleS actor_name_h(gdp->addStringTuple(
        GA_ATTRIB_POINT, theActorAttribName, 1));
    GA_RWHandleM4 rest_transform_h(gdp->addFloatTuple(
        GA_ATTRIB_POINT, "rest_transform", 16));

    MocapStreamRokokoHDK::Actor *actor = myProps.findActor(name);    
    if (actor)
    {
        GA_Offset ptoff = gdp->appendPoint();
        joint_name_h.set(ptoff, name);
        actor_name_h.set(ptoff, name);
    }
    else
    {
        actor = mySkeletons.findActor(name);
        if (!actor)
            return;
        
        // update the rest pose bone lengths to match the received pose
        UT_Array<UT_Matrix4F> rest_xforms(REST_XFORMS);

        int num_joints = JOINTS.size();
        if (actor->myPoints.size() == num_joints)
        {
            for (int i = 0; i < num_joints; i++)
            {
                if (PARENTS[i] < 0)
                    continue;

                auto& xform = rest_xforms[i];
                MocapStreamRokokoHDK::ActorPoint &joint
                        = actor->myPoints[i];
                MocapStreamRokokoHDK::ActorPoint &parent
                        = actor->myPoints[PARENTS[i]];

                // This works because the joints are sorted such that the parent
                // will always be processed first
                UT_Vector3F pt;
                xform.getTranslates(pt);
                xform = UT_Matrix3F(xform);
                UT_Vector3F old_origin(0.0);
                UT_Vector3F new_origin(0.0);

                fpreal bone_length;
                REST_XFORMS[PARENTS[i]].getTranslates(old_origin);
                rest_xforms[PARENTS[i]].getTranslates(new_origin);
                if (joint.myDidReceive && parent.myDidReceive)
                {
                    UT_Vector3F joint_pos, parent_pos;
                    joint.myWorldXform.getTranslates(joint_pos);
                    parent.myWorldXform.getTranslates(parent_pos);
                    bone_length = (joint_pos - parent_pos).length();
                }
                else
                {
                    bone_length = (pt - old_origin).length();
                }

                UT_Vector3F offset = pt - old_origin;
                
                pt = new_origin + offset*bone_length/offset.length();
                xform.setTranslates(pt);
            }
        }

        GA_Offset ptoff = gdp->appendPointBlock(JOINTS.size());

        for (int i = 0; i < num_joints; i++)
        {
            MocapStreamRokokoHDK::ActorPoint &joint
                    = actor->myPoints[i];

            joint_name_h.set(ptoff+i, JOINTS[i]);
            actor_name_h.set(ptoff+i, name);
            rest_transform_h.set(ptoff+i, UT_Matrix4F(rest_xforms[i]));

            UT_Vector3F pt;
            rest_xforms[i].getTranslates(pt);
            gdp->setPos3(ptoff + i, pt);

            if (PARENTS[i] != -1)
            {
                GA_Offset vtx_off;
                gdp->appendPrimitivesAndVertices(GEO_PRIMPOLY, 1,2, vtx_off, false);
                gdp->setVertexPoint(vtx_off, ptoff+PARENTS[i]);
                gdp->setVertexPoint(vtx_off + 1, ptoff+i);

                // Set the default local transform of the skeleton
                UT_Matrix4F parent_xform_inv = rest_xforms[PARENTS[i]];
                parent_xform_inv.invert();
                joint.myLocalXform = rest_xforms[i] * parent_xform_inv;
            }
            else
            {
                // Set the default local transform of the skeleton
                joint.myLocalXform = rest_xforms[i];
            }
        }


    }
    gdp->bumpDataIdsForAddOrRemove(true, true, true);
}

#if 0
// An implementation of the MC_MocapStreamImpl::basicUdpReceiver.
// This is an example of the most basic setup to receive motion capture data
// through a UDP connection and pass it onto your device to parse.
//
// This function can be used as the server function for your state instead
// of defining your own.
void basicUdpReceiver(ServerState& state, const ServerOptions& options,
                      MC_MocapStreamImpl *data)
{
    using hboost::asio::ip::udp;
    using hboost::asio::ip::address;

    try
    {
        // Open a socket so that we are prepared to receive data via the udp
        // connection.
        hboost::asio::io_service io_service;
        udp::socket socket(io_service, udp::endpoint(udp::v4(), options.myPort));
        UT_Array<char> buf(options.myBufferSize, options.myBufferSize);

        int native_socket = socket.native_handle();
        UT_NetFDSet fd_set;
        fd_set.add(native_socket, UT_NetFDEvent::NETFD_READ);
 
        // Tell the ServerState that the port has been opened. If this is not
        // done, the callback for the "Connect" button will not end.       
        state.setPortOpened(true);

        // Loop until the server is killed. This happens when the "Disconnect"
        // button is pressed on the MotionStream SOP.
        while (!state.killed())
        {
            udp::endpoint remote_endpoint;
            hboost::system::error_code error;

            // Synchronous boost sockets don't support timouts,
            // so we'll do it manually with the native handle.

            // Check the events on the socket to see if we have receive new
            // data.
            int res = fd_set.checkEvents(100);
            if (res <= 0)
            {
                // If no data was received within the time limit, add this
                // to the log.
                state.setError("Request timed out...");
                continue;
            }

            // Get the data from the socket.
            size_t num_read = socket.receive_from(
                hboost::asio::buffer(buf.data(), buf.size()),
                remote_endpoint, 0, error);

            // If we ran into an error getting the data, throw the error.
            // This is caught, a message is added to the log, and then the
            // connection is ended.
            if (error && error != hboost::asio::error::message_size)
                throw hboost::system::system_error(error);

            // Once we have received a packet, tell the server state that we are
            // connected, clear the error buffer, and call
            // ServerState::gotPacket to queue the packet for parsing.
            state.setConnected(true);
            state.resetError();
            state.gotPacket(UT_StringHolder(buf.data(), num_read));
        }
    }
    catch(hboost::system::system_error& e)
    {
        // If a boost error was encountered, add the error message to the log.
        MC_MocapStreamImpl::handleBoostError(e, state);
    }
}

// An implementation of the MC_MocapStreamImpl::basicTcpReceiver.
// This is an example of the most basic setup to receive motion capture data
// through a UDP connection and pass it onto your device to parse.
//
// This function can be used as the server function for your state instead
// of defining your own.
void basicTcpClient(ServerState& state, const ServerOptions& options,
                    MC_MocapStreamImpl *data)
{
    // Open a connection
    UT_UniquePtr<UT_NetSocket> client = UT_NetSocket::newSocketFromAddr(
        options.myHostIP, options.myPort, true);
    UT_Array<char> buf(options.myBufferSize, options.myBufferSize);

    // Tell the ServerState that the port has been opened. If this is not
    // done, the callback for the "Connect" button will not end.
    state.setPortOpened(true);

    int num_read;
    // Ensure that the client is valid. If not, send a message to the log and
    // then end the server function.
    if (client->isValid())
    {
        // Loop until the server is killed. This happens when the "Disconnect"
        // button is pressed on the MotionStream SOP.
        while (!state.killed())
        {
            // Establish a connection with the host computer.
            int cond = client->connect(500);

            // Tell the ServerState whether the connection was successful
            state.setConnected(cond == UT_NetSocket::UT_CONNECT_SUCCESS);

            if (!state.connected())
            {
                // If the connection failed, send an error to the log.
                state.setError("Could not connect");
            }

            // Loop while we are still connected and while the server has not
            // been killed.
            while (state.connected() && !state.killed())
            {
                // Attempt to receive data from the host.
                int status = client->read(buf.data(), buf.size(), &num_read, 200);
                if (status == UT_NetSocket::UT_CONNECT_SUCCESS && num_read > 0)
                {
                    // If a packet was received, call ServerState::gotPacket
                    // to queue the packet for parsing.
                    state.gotPacket(UT_StringHolder(buf.data(), num_read));
                }
                else if (status != UT_NetSocket::UT_WOULD_BLOCK)
                {
                    state.setError("Error receiving data");
                    //state.setError(UT_NetSocket::getErrorName(status));
                    state.setConnected(false);
                    client->close();
                    client = UT_NetSocket::newSocketFromAddr(
                        options.myHostIP, options.myPort, true);
                }
#if UT_ASSERT_LEVEL > 1
                else
                {
                    state.setError("No data received within 500ms");
                }
#endif
            }
        }
    }
    else
    {
        state.setError("Invalid request");
    }
}
#endif