diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 25c7a788b3..b8c390297a 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 f460881a45..74f36e858e 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,30 @@ QByteArray AvatarData::toByteArray(AvatarDataDetail dataDetail, quint64 lastSent } } + // AJT: hand controller section + IF_AVATAR_SPACE(PACKET_HAS_HAND_CONTROLLERS, AvatarDataPacket::HAND_CONTROLLERS_SIZE) { + + qDebug() << "AJT: sending hand controllers!"; + + // AJT: TODO + //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); + + /* AJT: TODO + 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 +653,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 +773,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 +905,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 +955,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 +1244,26 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { _localPositionUpdateRate.increment(); } + // AJT: hand controller section + if (hasHandControllers) { + + qDebug() << "AJT: received hand controllers!"; + + // AJT: TODO + // auto startSection = sourceBuffer; + + sourceBuffer = unpackHandController(sourceBuffer, _controllerLeftHandMatrixCache); + sourceBuffer = unpackHandController(sourceBuffer, _controllerRightHandMatrixCache); + + // AJT: TODO + // int numBytesRead = sourceBuffer - startSection; + // _handControllerUpdateRate.increment(numBytesRead); + // _handControllerUpdateRate.increment(); + } else { + _controllerLeftHandMatrixCache.invalidate(); + _controllerRightHandMatrixCache.invalidate(); + } + if (hasFaceTrackerInfo) { auto startSection = sourceBuffer; @@ -1351,10 +1375,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(); diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 79c82d4f29..ccb5423c78 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -164,10 +164,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]; @@ -230,7 +231,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 @@ -273,6 +274,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 @@ -303,7 +313,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); /* @@ -327,7 +337,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 }; 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;