HDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 <OP/OP_Director.h>
#include <UT/UT_NetFDSet.h>
#include <UT/UT_Thread.h>
#include <UT/UT_Options.h>
#include <SYS/SYS_Pragma.h>
#include <hboost/asio.hpp>
using namespace HDK_Sample;
using namespace UT::Literal;
// Initialization function.
void
{
}
// 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
{
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> 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)
// 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");
}
}
}
{
// If a boost error was encountered, add the error message to the log.
}
}
// 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)
{ }
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.
"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",
};
-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_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)
{
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;
{
// 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(
// 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);
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);
}
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);
}
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_RWHandleS actor_name_h(gdp->addStringTuple(
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
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
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]));
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,
{
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)
// 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));
}
}
{
// If a boost error was encountered, add the error message to the log.
}
}
// 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,
{
// Open a connection
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();
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