diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 935c92a4e0..0dd394c459 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1041,11 +1041,15 @@ void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeV assert(QThread::currentThread() == thread()); auto userInputMapper = DependencyManager::get(); controller::Pose controllerPose = userInputMapper->getPoseState(poseKey); - Transform transform; - transform.setTranslation(controllerPose.getTranslation()); - transform.setRotation(controllerPose.getRotation()); - glm::mat4 controllerMatrix = transform.getMatrix(); - matrixCache.set(controllerMatrix); + if (controllerPose.isValid()) { + Transform transform; + transform.setTranslation(controllerPose.getTranslation()); + transform.setRotation(controllerPose.getRotation()); + glm::mat4 controllerMatrix = transform.getMatrix(); + matrixCache.set(controllerMatrix); + } else { + matrixCache.invalidate(); + } } // best called at end of main loop, after physics. diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 95f46b3ddc..aea214efd7 100755 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -55,7 +55,7 @@ using namespace std; const QString AvatarData::FRAME_NAME = "com.highfidelity.recording.AvatarData"; static const int TRANSLATION_COMPRESSION_RADIX = 14; -static const int FAUX_JOINT_COMPRESSION_RADIX = 12; +static const int HAND_CONTROLLER_COMPRESSION_RADIX = 12; static const int SENSOR_TO_WORLD_SCALE_RADIX = 10; static const float AUDIO_LOUDNESS_SCALE = 1024.0f; static const float DEFAULT_AVATAR_DENSITY = 1000.0f; // density of water @@ -66,7 +66,7 @@ size_t AvatarDataPacket::maxFaceTrackerInfoSize(size_t numBlendshapeCoefficients return FACE_TRACKER_INFO_SIZE + numBlendshapeCoefficients * sizeof(float); } -size_t AvatarDataPacket::maxJointDataSize(size_t numJoints, bool hasGrabJoints) { +size_t AvatarDataPacket::maxJointDataSize(size_t numJoints) { const size_t validityBitsSize = calcBitVectorSize((int)numJoints); size_t totalSize = sizeof(uint8_t); // numJoints @@ -76,14 +76,6 @@ size_t AvatarDataPacket::maxJointDataSize(size_t numJoints, bool hasGrabJoints) totalSize += validityBitsSize; // Translations mask totalSize += sizeof(float); // maxTranslationDimension totalSize += numJoints * sizeof(SixByteTrans); // Translations - - size_t NUM_FAUX_JOINT = 2; - totalSize += NUM_FAUX_JOINT * (sizeof(SixByteQuat) + sizeof(SixByteTrans)); // faux joints - - if (hasGrabJoints) { - totalSize += sizeof(AvatarDataPacket::FarGrabJoints); - } - return totalSize; } @@ -98,9 +90,6 @@ size_t AvatarDataPacket::minJointDataSize(size_t numJoints) { totalSize += sizeof(float); // maxTranslationDimension // assume no valid translations - size_t NUM_FAUX_JOINT = 2; - totalSize += NUM_FAUX_JOINT * (sizeof(SixByteQuat) + sizeof(SixByteTrans)); // faux joints - return totalSize; } @@ -329,6 +318,7 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent // separately bool hasParentInfo = false; bool hasAvatarLocalPosition = false; + bool hasHandControllers = false; bool hasFaceTrackerInfo = false; @@ -346,7 +336,7 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent hasAvatarLocalPosition = hasParent() && (sendAll || tranlationChangedSince(lastSentTime) || parentInfoChangedSince(lastSentTime)); - + hasHandControllers = _controllerLeftHandMatrixCache.isValid() || _controllerRightHandMatrixCache.isValid(); hasFaceTrackerInfo = !dropFaceTracking && (hasFaceTracker() || getHasScriptedBlendshapes()) && (sendAll || faceTrackerInfoChangedSince(lastSentTime)); hasJointData = !sendMinimum; @@ -364,6 +354,7 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent | (hasAdditionalFlags ? AvatarDataPacket::PACKET_HAS_ADDITIONAL_FLAGS : 0) | (hasParentInfo ? AvatarDataPacket::PACKET_HAS_PARENT_INFO : 0) | (hasAvatarLocalPosition ? AvatarDataPacket::PACKET_HAS_AVATAR_LOCAL_POSITION : 0) + | (hasHandControllers ? AvatarDataPacket::PACKET_HAS_HAND_CONTROLLERS : 0) | (hasFaceTrackerInfo ? AvatarDataPacket::PACKET_HAS_FACE_TRACKER_INFO : 0) | (hasJointData ? AvatarDataPacket::PACKET_HAS_JOINT_DATA : 0) | (hasJointDefaultPoseFlags ? AvatarDataPacket::PACKET_HAS_JOINT_DEFAULT_POSE_FLAGS : 0) @@ -406,7 +397,7 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent const size_t byteArraySize = AvatarDataPacket::MAX_CONSTANT_HEADER_SIZE + NUM_BYTES_RFC4122_UUID + AvatarDataPacket::maxFaceTrackerInfoSize(_headData->getBlendshapeCoefficients().size()) + - AvatarDataPacket::maxJointDataSize(_jointData.size(), true) + + AvatarDataPacket::maxJointDataSize(_jointData.size()) + AvatarDataPacket::maxJointDefaultPoseFlagsSize(_jointData.size()); if (maxDataSize == 0) { @@ -592,7 +583,7 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent } } - IF_AVATAR_SPACE(PACKET_HAS_AVATAR_LOCAL_POSITION, sizeof(getLocalPosition()) ) { + IF_AVATAR_SPACE(PACKET_HAS_AVATAR_LOCAL_POSITION, AvatarDataPacket::AVATAR_LOCAL_POSITION_SIZE) { auto startSection = destinationBuffer; const auto localPosition = getLocalPosition(); AVATAR_MEMCPY(localPosition); @@ -603,6 +594,23 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent } } + IF_AVATAR_SPACE(PACKET_HAS_HAND_CONTROLLERS, AvatarDataPacket::HAND_CONTROLLERS_SIZE) { + auto startSection = destinationBuffer; + + Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix()); + destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, controllerLeftHandTransform.getRotation()); + destinationBuffer += packFloatVec3ToSignedTwoByteFixed(destinationBuffer, controllerLeftHandTransform.getTranslation(), HAND_CONTROLLER_COMPRESSION_RADIX); + + Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix()); + destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, controllerRightHandTransform.getRotation()); + destinationBuffer += packFloatVec3ToSignedTwoByteFixed(destinationBuffer, controllerRightHandTransform.getTranslation(), HAND_CONTROLLER_COMPRESSION_RADIX); + + int numBytes = destinationBuffer - startSection; + if (outboundDataRateOut) { + outboundDataRateOut->handControllersRate.increment(numBytes); + } + } + const auto& blendshapeCoefficients = _headData->getBlendshapeCoefficients(); // If it is connected, pack up the data IF_AVATAR_SPACE(PACKET_HAS_FACE_TRACKER_INFO, sizeof(AvatarDataPacket::FaceTrackerInfo) + (size_t)blendshapeCoefficients.size() * sizeof(float)) { @@ -638,9 +646,8 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent // include jointData if there is room for the most minimal section. i.e. no translations or rotations. IF_AVATAR_SPACE(PACKET_HAS_JOINT_DATA, AvatarDataPacket::minJointDataSize(numJoints)) { // Minimum space required for another rotation joint - - // size of joint + following translation bit-vector + translation scale + faux joints: - const ptrdiff_t minSizeForJoint = sizeof(AvatarDataPacket::SixByteQuat) + jointBitVectorSize + - sizeof(float) + AvatarDataPacket::FAUX_JOINTS_SIZE; + // size of joint + following translation bit-vector + translation scale: + const ptrdiff_t minSizeForJoint = sizeof(AvatarDataPacket::SixByteQuat) + jointBitVectorSize + sizeof(float); auto startSection = destinationBuffer; @@ -759,17 +766,6 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent } sendStatus.translationsSent = i; - // faux joints - Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix()); - destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, controllerLeftHandTransform.getRotation()); - destinationBuffer += packFloatVec3ToSignedTwoByteFixed(destinationBuffer, controllerLeftHandTransform.getTranslation(), - FAUX_JOINT_COMPRESSION_RADIX); - - Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix()); - destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, controllerRightHandTransform.getRotation()); - destinationBuffer += packFloatVec3ToSignedTwoByteFixed(destinationBuffer, controllerRightHandTransform.getTranslation(), - FAUX_JOINT_COMPRESSION_RADIX); - IF_AVATAR_SPACE(PACKET_HAS_GRAB_JOINTS, sizeof (AvatarDataPacket::FarGrabJoints)) { // the far-grab joints may range further than 3 meters, so we can't use packFloatVec3ToSignedTwoByteFixed etc auto startSection = destinationBuffer; @@ -902,12 +898,12 @@ bool AvatarData::shouldLogError(const quint64& now) { } -const unsigned char* unpackFauxJoint(const unsigned char* sourceBuffer, ThreadSafeValueCache& matrixCache) { +const unsigned char* unpackHandController(const unsigned char* sourceBuffer, ThreadSafeValueCache& matrixCache) { glm::quat orientation; glm::vec3 position; Transform transform; sourceBuffer += unpackOrientationQuatFromSixBytes(sourceBuffer, orientation); - sourceBuffer += unpackFloatVec3FromSignedTwoByteFixed(sourceBuffer, position, FAUX_JOINT_COMPRESSION_RADIX); + sourceBuffer += unpackFloatVec3FromSignedTwoByteFixed(sourceBuffer, position, HAND_CONTROLLER_COMPRESSION_RADIX); transform.setTranslation(position); transform.setRotation(orientation); matrixCache.set(transform.getMatrix()); @@ -952,6 +948,7 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { bool hasAdditionalFlags = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_ADDITIONAL_FLAGS); bool hasParentInfo = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_PARENT_INFO); bool hasAvatarLocalPosition = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_AVATAR_LOCAL_POSITION); + bool hasHandControllers = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_HAND_CONTROLLERS); bool hasFaceTrackerInfo = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_FACE_TRACKER_INFO); bool hasJointData = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_JOINT_DATA); bool hasJointDefaultPoseFlags = HAS_FLAG(packetStateFlags, AvatarDataPacket::PACKET_HAS_JOINT_DEFAULT_POSE_FLAGS); @@ -1240,6 +1237,20 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { _localPositionUpdateRate.increment(); } + if (hasHandControllers) { + auto startSection = sourceBuffer; + + sourceBuffer = unpackHandController(sourceBuffer, _controllerLeftHandMatrixCache); + sourceBuffer = unpackHandController(sourceBuffer, _controllerRightHandMatrixCache); + + int numBytesRead = sourceBuffer - startSection; + _handControllersRate.increment(numBytesRead); + _handControllersUpdateRate.increment(); + } else { + _controllerLeftHandMatrixCache.invalidate(); + _controllerRightHandMatrixCache.invalidate(); + } + if (hasFaceTrackerInfo) { auto startSection = sourceBuffer; @@ -1351,10 +1362,6 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { << "size:" << (int)(sourceBuffer - startPosition); } #endif - // faux joints - sourceBuffer = unpackFauxJoint(sourceBuffer, _controllerLeftHandMatrixCache); - sourceBuffer = unpackFauxJoint(sourceBuffer, _controllerRightHandMatrixCache); - int numBytesRead = sourceBuffer - startSection; _jointDataRate.increment(numBytesRead); _jointDataUpdateRate.increment(); @@ -1445,6 +1452,7 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { * * "globalPosition"Incoming global position. * "localPosition"Incoming local position. + * "handControllers"Incoming hand controllers. * "avatarBoundingBox"Incoming avatar bounding box. * "avatarOrientation"Incoming avatar orientation. * "avatarScale"Incoming avatar scale. @@ -1483,6 +1491,8 @@ float AvatarData::getDataRate(const QString& rateName) const { return _globalPositionRate.rate() / BYTES_PER_KILOBIT; } else if (rateName == "localPosition") { return _localPositionRate.rate() / BYTES_PER_KILOBIT; + } else if (rateName == "handControllers") { + return _handControllersRate.rate() / BYTES_PER_KILOBIT; } else if (rateName == "avatarBoundingBox") { return _avatarBoundingBoxRate.rate() / BYTES_PER_KILOBIT; } else if (rateName == "avatarOrientation") { @@ -1547,6 +1557,7 @@ float AvatarData::getDataRate(const QString& rateName) const { * * "globalPosition"Global position. * "localPosition"Local position. + * "handControllers"Hand controller positions and orientations. * "avatarBoundingBox"Avatar bounding box. * "avatarOrientation"Avatar orientation. * "avatarScale"Avatar scale. @@ -1571,6 +1582,8 @@ float AvatarData::getUpdateRate(const QString& rateName) const { return _globalPositionUpdateRate.rate(); } else if (rateName == "localPosition") { return _localPositionUpdateRate.rate(); + } else if (rateName == "handControllers") { + return _handControllersUpdateRate.rate(); } else if (rateName == "avatarBoundingBox") { return _avatarBoundingBoxUpdateRate.rate(); } else if (rateName == "avatarOrientation") { diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 2add2f9881..76fa9e0a34 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -203,10 +203,11 @@ namespace AvatarDataPacket { const HasFlags PACKET_HAS_ADDITIONAL_FLAGS = 1U << 7; const HasFlags PACKET_HAS_PARENT_INFO = 1U << 8; const HasFlags PACKET_HAS_AVATAR_LOCAL_POSITION = 1U << 9; - const HasFlags PACKET_HAS_FACE_TRACKER_INFO = 1U << 10; - const HasFlags PACKET_HAS_JOINT_DATA = 1U << 11; - const HasFlags PACKET_HAS_JOINT_DEFAULT_POSE_FLAGS = 1U << 12; - const HasFlags PACKET_HAS_GRAB_JOINTS = 1U << 13; + const HasFlags PACKET_HAS_HAND_CONTROLLERS = 1U << 10; + const HasFlags PACKET_HAS_FACE_TRACKER_INFO = 1U << 11; + const HasFlags PACKET_HAS_JOINT_DATA = 1U << 12; + const HasFlags PACKET_HAS_JOINT_DEFAULT_POSE_FLAGS = 1U << 13; + const HasFlags PACKET_HAS_GRAB_JOINTS = 1U << 14; const size_t AVATAR_HAS_FLAGS_SIZE = 2; using SixByteQuat = uint8_t[6]; @@ -269,7 +270,7 @@ namespace AvatarDataPacket { // // POTENTIAL SAVINGS - 20 bytes - SixByteQuat sensorToWorldQuat; // 6 byte compressed quaternion part of sensor to world matrix + SixByteQuat sensorToWorldQuat; // 6 byte compressed quaternion part of sensor to world matrix uint16_t sensorToWorldScale; // uniform scale of sensor to world matrix float sensorToWorldTrans[3]; // fourth column of sensor to world matrix // FIXME - sensorToWorldTrans might be able to be better compressed if it was @@ -313,6 +314,15 @@ namespace AvatarDataPacket { PARENT_INFO_SIZE + AVATAR_LOCAL_POSITION_SIZE; + PACKED_BEGIN struct HandControllers { + SixByteQuat leftHandRotation; + SixByteTrans leftHandTranslation; + SixByteQuat rightHandRotation; + SixByteTrans rightHandTranslation; + } PACKED_END; + static const size_t HAND_CONTROLLERS_SIZE = 24; + static_assert(sizeof(HandControllers) == HAND_CONTROLLERS_SIZE, "AvatarDataPacket::HandControllers size doesn't match."); + // variable length structure follows @@ -343,7 +353,7 @@ namespace AvatarDataPacket { SixByteTrans rightHandControllerTranslation; }; */ - size_t maxJointDataSize(size_t numJoints, bool hasGrabJoints); + size_t maxJointDataSize(size_t numJoints); size_t minJointDataSize(size_t numJoints); /* @@ -367,7 +377,6 @@ namespace AvatarDataPacket { static_assert(sizeof(FarGrabJoints) == FAR_GRAB_JOINTS_SIZE, "AvatarDataPacket::FarGrabJoints size doesn't match."); static const size_t MIN_BULK_PACKET_SIZE = NUM_BYTES_RFC4122_UUID + HEADER_SIZE; - static const size_t FAUX_JOINTS_SIZE = 2 * (sizeof(SixByteQuat) + sizeof(SixByteTrans)); struct SendStatus { HasFlags itemFlags { 0 }; @@ -444,6 +453,7 @@ class AvatarDataRate { public: RateCounter<> globalPositionRate; RateCounter<> localPositionRate; + RateCounter<> handControllersRate; RateCounter<> avatarBoundingBoxRate; RateCounter<> avatarOrientationRate; RateCounter<> avatarScaleRate; @@ -1716,6 +1726,7 @@ protected: RateCounter<> _parseBufferRate; RateCounter<> _globalPositionRate; RateCounter<> _localPositionRate; + RateCounter<> _handControllersRate; RateCounter<> _avatarBoundingBoxRate; RateCounter<> _avatarOrientationRate; RateCounter<> _avatarScaleRate; @@ -1733,6 +1744,7 @@ protected: RateCounter<> _parseBufferUpdateRate; RateCounter<> _globalPositionUpdateRate; RateCounter<> _localPositionUpdateRate; + RateCounter<> _handControllersUpdateRate; RateCounter<> _avatarBoundingBoxUpdateRate; RateCounter<> _avatarOrientationUpdateRate; RateCounter<> _avatarScaleUpdateRate; diff --git a/libraries/networking/src/udt/PacketHeaders.cpp b/libraries/networking/src/udt/PacketHeaders.cpp index b21c200ef2..861774b145 100644 --- a/libraries/networking/src/udt/PacketHeaders.cpp +++ b/libraries/networking/src/udt/PacketHeaders.cpp @@ -38,10 +38,10 @@ PacketVersion versionForPacketType(PacketType packetType) { return static_cast(EntityQueryPacketVersion::ConicalFrustums); case PacketType::AvatarIdentity: case PacketType::AvatarData: - return static_cast(AvatarMixerPacketVersion::FBXJointOrderChange); + return static_cast(AvatarMixerPacketVersion::HandControllerSection); case PacketType::BulkAvatarData: case PacketType::KillAvatar: - return static_cast(AvatarMixerPacketVersion::FBXJointOrderChange); + return static_cast(AvatarMixerPacketVersion::HandControllerSection); case PacketType::MessagesData: return static_cast(MessageDataVersion::TextOrBinaryData); // ICE packets diff --git a/libraries/networking/src/udt/PacketHeaders.h b/libraries/networking/src/udt/PacketHeaders.h index 1dafc561f6..274c34a268 100644 --- a/libraries/networking/src/udt/PacketHeaders.h +++ b/libraries/networking/src/udt/PacketHeaders.h @@ -331,7 +331,8 @@ enum class AvatarMixerPacketVersion : PacketVersion { AvatarTraitsAck, FasterAvatarEntities, SendMaxTranslationDimension, - FBXJointOrderChange + FBXJointOrderChange, + HandControllerSection, }; enum class DomainConnectRequestVersion : PacketVersion { diff --git a/libraries/shared/src/ThreadSafeValueCache.h b/libraries/shared/src/ThreadSafeValueCache.h index 192300abbb..048bc8db69 100644 --- a/libraries/shared/src/ThreadSafeValueCache.h +++ b/libraries/shared/src/ThreadSafeValueCache.h @@ -52,6 +52,11 @@ public: _valid = false; } + bool isValid() const { + std::lock_guard guard(_mutex); + return _valid; + } + private: mutable std::mutex _mutex; T _value; diff --git a/tools/dissectors/3-hf-avatar.lua b/tools/dissectors/3-hf-avatar.lua index bc449770f5..9b8567c55f 100644 --- a/tools/dissectors/3-hf-avatar.lua +++ b/tools/dissectors/3-hf-avatar.lua @@ -5,6 +5,33 @@ p_hf_avatar = Proto("hf-avatar", "HF Avatar Protocol") -- avatar data fields local f_avatar_id = ProtoField.guid("hf_avatar.avatar_id", "Avatar ID") + +-- sizes in bytes +local f_avatar_header_size = ProtoField.int32("hf_avatar.header_size", "Header Size", base.DEC) +local f_avatar_global_position_size = ProtoField.int32("hf_avatar.global_position_size", "Global Position Size", base.DEC) +local f_avatar_bounding_box_size = ProtoField.int32("hf_avatar.bounding_box_size", "Bounding Box Size", base.DEC) +local f_avatar_orientation_size = ProtoField.int32("hf_avatar.orientation_size", "Orientation Size", base.DEC) +local f_avatar_scale_size = ProtoField.int32("hf_avatar.scale_size", "Scale Size", base.DEC) +local f_avatar_look_at_position_size = ProtoField.int32("hf_avatar.look_at_position_size", "Look At Position Size", base.DEC) +local f_avatar_audio_loudness_size = ProtoField.int32("hf_avatar.audio_loudness_size", "Audio Loudness Size", base.DEC) +local f_avatar_sensor_to_world_size = ProtoField.int32("hf_avatar.sensor_to_world_size", "Sensor To World Matrix Size", base.DEC) +local f_avatar_additional_flags_size = ProtoField.int32("hf_avatar.additional_flags_size", "Additional Flags Size", base.DEC) +local f_avatar_parent_info_size = ProtoField.int32("hf_avatar.parent_info_size", "Parent Info Size", base.DEC) +local f_avatar_local_position_size = ProtoField.int32("hf_avatar.local_position_size", "Local Position Size", base.DEC) +local f_avatar_face_tracker_info_size = ProtoField.int32("hf_avatar.face_tracker_info_size", "Face Tracker Info Size", base.DEC) +local f_avatar_joint_data_size = ProtoField.int32("hf_avatar.joint_data_size", "Joint Data Size", base.DEC) +local f_avatar_default_pose_flags_size = ProtoField.int32("hf_avatar.default_pose_flags_size", "Default Pose Flags Size", base.DEC) +local f_avatar_grab_joints_size = ProtoField.int32("hf_avatar.grab_joints_size", "Grab Joints Size", base.DEC) + +local f_avatar_joint_data_bit_vector_size = ProtoField.int32("hf_avatar.joint_data_bit_vector_size", "Joint Data Bit Vector Size", base.DEC) +local f_avatar_joint_data_rotations_size = ProtoField.int32("hf_avatar.joint_data_rotations_size", "Joint Data Rotations Size", base.DEC) +local f_avatar_joint_data_translations_size = ProtoField.int32("hf_avatar.joint_data_translations_size", "Joint Data Translations Size", base.DEC) +local f_avatar_joint_data_faux_joints_size = ProtoField.int32("hf_avatar.joint_data_faux_joints_size", "Joint Data Faux Joints Size", base.DEC) +local f_avatar_joint_data_other_size = ProtoField.int32("hf_avatar.joint_data_other_size", "Joint Data Other Size", base.DEC) + +local f_avatar_bulk_count = ProtoField.int32("hf_avatar.bulk_count", "Bulk Count", base.DEC) + +-- contents local f_avatar_data_has_flags = ProtoField.string("hf_avatar.avatar_has_flags", "Has Flags") local f_avatar_data_position = ProtoField.string("hf_avatar.avatar_data_position", "Position") local f_avatar_data_dimensions = ProtoField.string("hf_avatar.avatar_data_dimensions", "Dimensions") @@ -19,7 +46,6 @@ local f_avatar_data_valid_rotations = ProtoField.string("hf_avatar.avatar_data_v local f_avatar_data_valid_translations = ProtoField.string("hf_avatar.avatar_data_valid_translations", "Valid Translations") local f_avatar_data_default_rotations = ProtoField.string("hf_avatar.avatar_data_default_rotations", "Valid Default") local f_avatar_data_default_translations = ProtoField.string("hf_avatar.avatar_data_default_translations", "Valid Default") -local f_avatar_data_sizes = ProtoField.string("hf_avatar.avatar_sizes", "Sizes") -- avatar trait data fields local f_avatar_trait_data = ProtoField.bytes("hf_avatar.avatar_trait_data", "Avatar Trait Data") @@ -31,7 +57,32 @@ local f_avatar_trait_instance_id = ProtoField.guid("hf_avatar.trait_instance_id" local f_avatar_trait_binary = ProtoField.bytes("hf_avatar.trait_binary", "Trait Binary Data") p_hf_avatar.fields = { - f_avatar_id, f_avatar_data_parent_id, + f_avatar_id, + f_avatar_data_parent_id, + + f_avatar_header_size, + f_avatar_global_position_size, + f_avatar_bounding_box_size, + f_avatar_orientation_size, + f_avatar_scale_size, + f_avatar_look_at_position_size, + f_avatar_audio_loudness_size, + f_avatar_sensor_to_world_size, + f_avatar_additional_flags_size, + f_avatar_parent_info_size, + f_avatar_local_position_size, + f_avatar_face_tracker_info_size, + f_avatar_joint_data_size, + f_avatar_default_pose_flags_size, + f_avatar_grab_joints_size, + f_avatar_bulk_count, + + f_avatar_joint_data_bit_vector_size, + f_avatar_joint_data_rotations_size, + f_avatar_joint_data_translations_size, + f_avatar_joint_data_faux_joints_size, + f_avatar_joint_data_other_size, + f_avatar_trait_data, f_avatar_trait_type, f_avatar_trait_id, f_avatar_trait_version, f_avatar_trait_binary, @@ -55,7 +106,34 @@ function p_hf_avatar.dissector(buf, pinfo, tree) local avatar_data + -- sizes + local avatar_data_sizes = { + header_size = 0, + global_position_size = 0, + bounding_box_size = 0, + orientation_size = 0, + scale_size = 0, + look_at_position_size = 0, + audio_loudness_size = 0, + sensor_to_world_size = 0, + additional_flags_size = 0, + parent_info_size = 0, + local_position_size = 0, + face_tracker_info_size = 0, + joint_data_size = 0, + default_pose_flags_size = 0, + grab_joints_size = 0, + bulk_count = 0, + joint_data_bit_vector_size = 0, + joint_data_rotations_size = 0, + joint_data_translations_size = 0, + joint_data_faux_joints_size = 0, + joint_data_other_size = 0 + } + + local packet_type = packet_type_extractor().value + avatar_data_sizes.header_size = avatar_data_sizes.header_size + 1 if packet_type == 6 then -- AvatarData packet @@ -63,12 +141,14 @@ function p_hf_avatar.dissector(buf, pinfo, tree) -- uint16 sequence_number local sequence_number = buf(i, 2):le_uint() i = i + 2 + avatar_data_sizes.header_size = avatar_data_sizes.header_size + 2 local avatar_data_packet_len = buf:len() - i - avatar_data = decode_avatar_data_packet(buf(i, avatar_data_packet_len)) + avatar_data = decode_avatar_data_packet(buf(i, avatar_data_packet_len), avatar_data_sizes) i = i + avatar_data_packet_len add_avatar_data_subtrees(avatar_data) + add_avatar_data_sizes(avatar_data_sizes) elseif packet_type == 11 then -- BulkAvatarData packet @@ -76,13 +156,18 @@ function p_hf_avatar.dissector(buf, pinfo, tree) -- avatar_id is first 16 bytes avatar_subtree:add(f_avatar_id, buf(i, 16)) i = i + 16 + avatar_data_sizes.header_size = avatar_data_sizes.header_size + 16 local avatar_data_packet_len = buf:len() - i - avatar_data = decode_avatar_data_packet(buf(i, avatar_data_packet_len)) - i = i + avatar_data_packet_len + avatar_data = decode_avatar_data_packet(buf(i, avatar_data_packet_len), avatar_data_sizes) + i = i + avatar_data.bytes_consumed add_avatar_data_subtrees(avatar_data) + avatar_data_sizes.bulk_count = avatar_data_sizes.bulk_count + 1 end + + add_avatar_data_sizes(avatar_data_sizes) + elseif packet_type == 100 then -- BulkAvatarTraits packet @@ -209,9 +294,74 @@ function add_avatar_data_subtrees(avatar_data) if avatar_data["default_translations"] then avatar_subtree:add(f_avatar_data_default_translations, avatar_data["default_translations"]) end - if avatar_data["sizes"] then - avatar_subtree:add(f_avatar_data_sizes, avatar_data["sizes"]) +end + +function add_avatar_data_sizes(avatar_data_sizes) + if avatar_data_sizes.header_size then + avatar_subtree:add(f_avatar_header_size, avatar_data_sizes.header_size) end + if avatar_data_sizes.global_position_size then + avatar_subtree:add(f_avatar_global_position_size, avatar_data_sizes.global_position_size) + end + if avatar_data_sizes.bounding_box_size then + avatar_subtree:add(f_avatar_bounding_box_size, avatar_data_sizes.bounding_box_size) + end + if avatar_data_sizes.orientation_size then + avatar_subtree:add(f_avatar_orientation_size, avatar_data_sizes.orientation_size) + end + if avatar_data_sizes.scale_size then + avatar_subtree:add(f_avatar_scale_size, avatar_data_sizes.scale_size) + end + if avatar_data_sizes.look_at_position_size then + avatar_subtree:add(f_avatar_look_at_position_size, avatar_data_sizes.look_at_position_size) + end + if avatar_data_sizes.audio_loudness_size then + avatar_subtree:add(f_avatar_audio_loudness_size, avatar_data_sizes.audio_loudness_size) + end + if avatar_data_sizes.sensor_to_world_size then + avatar_subtree:add(f_avatar_sensor_to_world_size, avatar_data_sizes.sensor_to_world_size) + end + if avatar_data_sizes.additional_flags_size then + avatar_subtree:add(f_avatar_additional_flags_size, avatar_data_sizes.additional_flags_size) + end + if avatar_data_sizes.parent_info_size then + avatar_subtree:add(f_avatar_parent_info_size, avatar_data_sizes.parent_info_size) + end + if avatar_data_sizes.local_position_size then + avatar_subtree:add(f_avatar_local_position_size, avatar_data_sizes.local_position_size) + end + if avatar_data_sizes.face_tracker_info_size then + avatar_subtree:add(f_avatar_face_tracker_info_size, avatar_data_sizes.face_tracker_info_size) + end + if avatar_data_sizes.joint_data_size then + avatar_subtree:add(f_avatar_joint_data_size, avatar_data_sizes.joint_data_size) + end + if avatar_data_sizes.default_pose_flags_size then + avatar_subtree:add(f_avatar_default_pose_flags_size, avatar_data_sizes.default_pose_flags_size) + end + if avatar_data_sizes.grab_joints_size then + avatar_subtree:add(f_avatar_grab_joints_size, avatar_data_sizes.grab_joints_size) + end + if avatar_data_sizes.bulk_count then + avatar_subtree:add(f_avatar_bulk_count, avatar_data_sizes.bulk_count) + end + + if avatar_data_sizes.joint_data_bit_vector_size then + avatar_subtree:add(f_avatar_joint_data_bit_vector_size, avatar_data_sizes.joint_data_bit_vector_size) + end + if avatar_data_sizes.joint_data_rotations_size then + avatar_subtree:add(f_avatar_joint_data_rotations_size, avatar_data_sizes.joint_data_rotations_size) + end + if avatar_data_sizes.joint_data_translations_size then + avatar_subtree:add(f_avatar_joint_data_translations_size, avatar_data_sizes.joint_data_translations_size) + end + if avatar_data_sizes.joint_data_faux_joints_size then + avatar_subtree:add(f_avatar_joint_data_faux_joints_size, avatar_data_sizes.joint_data_faux_joints_size) + end + if avatar_data_sizes.joint_data_other_size then + avatar_subtree:add(f_avatar_joint_data_other_size, avatar_data_sizes.joint_data_other_size) + end + end function decode_vec3(buf) @@ -251,16 +401,33 @@ function decode_validity_bits(buf, num_bits) return result end -function decode_avatar_data_packet(buf) +function validity_bits_to_string(buf, num_bits) + -- first pass, decode each bit into an array of booleans + local i = 0 + local bit = 0 + local booleans = {} + for n = 1, num_bits do + local value = (bit32.band(buf(i, 1):uint(), bit32.lshift(1, bit)) ~= 0) + booleans[#booleans + 1] = (value and 1 or 0) + bit = bit + 1 + if bit == 8 then + i = i + 1 + bit = 0 + end + end + + return table.concat(booleans, "") +end + +function decode_avatar_data_packet(buf, avatar_data_sizes) local i = 0 local result = {} - result["sizes"] = "" - -- uint16 has_flags local has_flags = buf(i, 2):le_uint() i = i + 2 + avatar_data_sizes.header_size = avatar_data_sizes.header_size + 2 local has_global_position = (bit32.band(has_flags, 1) ~= 0) local has_bounding_box = (bit32.band(has_flags, 2) ~= 0) @@ -272,9 +439,11 @@ function decode_avatar_data_packet(buf) local has_additional_flags = (bit32.band(has_flags, 128) ~= 0) local has_parent_info = (bit32.band(has_flags, 256) ~= 0) local has_local_position = (bit32.band(has_flags, 512) ~= 0) - local has_face_tracker_info = (bit32.band(has_flags, 1024) ~= 0) - local has_joint_data = (bit32.band(has_flags, 2048) ~= 0) - local has_joint_default_pose_flags = (bit32.band(has_flags, 4096) ~= 0) + local has_hand_controllers = (bit32.band(has_flags, 1024) ~= 0) + local has_face_tracker_info = (bit32.band(has_flags, 2048) ~= 0) + local has_joint_data = (bit32.band(has_flags, 4096) ~= 0) + local has_joint_default_pose_flags = (bit32.band(has_flags, 8192) ~= 0) + local has_grab_joints = (bit32.band(has_flags, 16384) ~= 0) result["has_flags"] = string.format("HasFlags: 0x%x", has_flags) @@ -282,6 +451,7 @@ function decode_avatar_data_packet(buf) local position = decode_vec3(buf(i, 12)) result["position"] = string.format("Position: %.3f, %.3f, %.3f", position[1], position[2], position[3]) i = i + 12 + avatar_data_sizes.global_position_size = avatar_data_sizes.global_position_size + 12 end if has_bounding_box then @@ -289,6 +459,7 @@ function decode_avatar_data_packet(buf) i = i + 12 local offset = decode_vec3(buf(i, 12)) i = i + 12 + avatar_data_sizes.bounding_box_size = avatar_data_sizes.bounding_box_size + (12 + 12) result["dimensions"] = string.format("Dimensions: %.3f, %.3f, %.3f", dimensions[1], dimensions[2], dimensions[3]) result["offset"] = string.format("Offset: %.3f, %.3f, %.3f", offset[1], offset[2], offset[3]) end @@ -296,34 +467,40 @@ function decode_avatar_data_packet(buf) if has_orientation then -- TODO: orientation is hard to decode... i = i + 6 + avatar_data_sizes.orientation_size = avatar_data_sizes.orientation_size + 6 end if has_scale then -- TODO: scale is hard to decode... i = i + 2 + avatar_data_sizes.scale_size = avatar_data_sizes.scale_size + 2 end if has_look_at_position then local look_at = decode_vec3(buf(i, 12)) i = i + 12 + avatar_data_sizes.look_at_position_size = avatar_data_sizes.look_at_position_size + 12 result["look_at_position"] = string.format("Look At Position: %.3f, %.3f, %.3f", look_at[1], look_at[2], look_at[3]) end if has_audio_loudness then local loudness = buf(i, 1):uint() i = i + 1 + avatar_data_sizes.audio_loudness_size = avatar_data_sizes.audio_loudness_size + 1 result["audio_loudness"] = string.format("Audio Loudness: %d", loudness) end if has_sensor_to_world_matrix then -- TODO: sensor to world matrix is hard to decode i = i + 20 + avatar_data_sizes.sensor_to_world_size = avatar_data_sizes.sensor_to_world_size + 20 end if has_additional_flags then - local flags = buf(i, 1):uint() - i = i + 1 + local flags = buf(i, 2):uint() + i = i + 2 result["additional_flags"] = string.format("Additional Flags: 0x%x", flags) + avatar_data_sizes.additional_flags_size = avatar_data_sizes.additional_flags_size + 2 end if has_parent_info then @@ -331,6 +508,7 @@ function decode_avatar_data_packet(buf) i = i + 16 local parent_joint_index = buf(i, 2):le_int() i = i + 2 + avatar_data_sizes.parent_info_size = avatar_data_sizes.parent_info_size + 18 result["parent_id"] = parent_id result["parent_joint_index"] = string.format("Parent Joint Index: %d", parent_joint_index) end @@ -338,9 +516,16 @@ function decode_avatar_data_packet(buf) if has_local_position then local local_pos = decode_vec3(buf(i, 12)) i = i + 12 + avatar_data_sizes.local_position_size = avatar_data_sizes.local_position_size + 12 result["local_position"] = string.format("Local Position: %.3f, %.3f, %.3f", local_pos[1], local_pos[2], local_pos[3]) end + if has_hand_controllers then + -- TODO: skip faux joint data + i = i + (2 * 12) + avatar_data_sizes.joint_data_faux_joints_size = avatar_data_sizes.joint_data_faux_joints_size + (2 * 12) + end + if has_face_tracker_info then local left_eye_blink = buf(i, 4):le_float() i = i + 4 @@ -357,6 +542,7 @@ function decode_avatar_data_packet(buf) blendshape_coefficients[n] = buf(i, 4):le_float() i = i + 4 end + avatar_data_sizes.face_tracker_info_size = avatar_data_sizes.face_tracker_info_size + 17 + (num_blendshape_coefficients * 4) -- TODO: insert blendshapes into result end @@ -366,30 +552,42 @@ function decode_avatar_data_packet(buf) local num_joints = buf(i, 1):uint() i = i + 1 + avatar_data_sizes.joint_data_other_size = avatar_data_sizes.joint_data_other_size + 1 + local num_validity_bytes = math.ceil(num_joints / 8) local indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + local s = validity_bits_to_string(buf(i, num_validity_bytes), num_joints) i = i + num_validity_bytes - result["valid_rotations"] = "Valid Rotations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + avatar_data_sizes.joint_data_bit_vector_size = avatar_data_sizes.joint_data_bit_vector_size + num_validity_bytes + + result["valid_rotations"] = "Valid Rotations: " .. string.format("(%d/%d) {", #indices, num_joints) .. s .. "}" -- TODO: skip rotations for now - i = i + #indices * 6 + i = i + (#indices * 6) + avatar_data_sizes.joint_data_rotations_size = avatar_data_sizes.joint_data_rotations_size + (#indices * 6) indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + s = validity_bits_to_string(buf(i, num_validity_bytes), num_joints) i = i + num_validity_bytes - result["valid_translations"] = "Valid Translations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + avatar_data_sizes.joint_data_bit_vector_size = avatar_data_sizes.joint_data_bit_vector_size + num_validity_bytes + result["valid_translations"] = "Valid Translations: " .. string.format("(%d/%d) {", #indices, num_joints) .. s .. "}" -- TODO: skip maxTranslationDimension i = i + 4 + avatar_data_sizes.joint_data_other_size = avatar_data_sizes.joint_data_other_size + 4 -- TODO: skip translations for now - i = i + #indices * 6 + i = i + (#indices * 6) + avatar_data_sizes.joint_data_translations_size = avatar_data_sizes.joint_data_translations_size + (#indices * 6) - -- TODO: skip hand controller data - i = i + 24 - - result["sizes"] = result["sizes"] .. " Poses: " .. (i - joint_poses_start) + avatar_data_sizes.joint_data_size = avatar_data_sizes.joint_data_size + (i - joint_poses_start) + end + if has_grab_joints then + -- TODO: skip grab joints + i = i + 84 + avatar_data_sizes.grab_joints_size = avatar_data_sizes.grab_joints_size + 84 end if has_joint_default_pose_flags then @@ -398,15 +596,19 @@ function decode_avatar_data_packet(buf) local num_validity_bytes = math.ceil(num_joints / 8) local indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + local s = validity_bits_to_string(buf(i, num_validity_bytes), num_joints) i = i + num_validity_bytes - result["default_rotations"] = "Default Rotations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + result["default_rotations"] = "Default Rotations: " .. string.format("(%d/%d) {", #indices, num_joints) .. s .. "}" indices = decode_validity_bits(buf(i, num_validity_bytes), num_joints) + s = validity_bits_to_string(buf(i, num_validity_bytes), num_joints) i = i + num_validity_bytes - result["default_translations"] = "Default Translations: " .. string.format("(%d/%d) {", #indices, num_joints) .. table.concat(indices, ", ") .. "}" + result["default_translations"] = "Default Translations: " .. string.format("(%d/%d) {", #indices, num_joints) .. s .. "}" + + avatar_data_sizes.default_pose_flags_size = avatar_data_sizes.default_pose_flags_size + 2 * num_validity_bytes end - result["sizes"] = result["sizes"] .. " Total: " .. i + result["bytes_consumed"] = i return result end