diff --git a/assignment-client/src/avatars/AvatarMixer.cpp b/assignment-client/src/avatars/AvatarMixer.cpp index a109934d10..46599396ca 100644 --- a/assignment-client/src/avatars/AvatarMixer.cpp +++ b/assignment-client/src/avatars/AvatarMixer.cpp @@ -45,6 +45,9 @@ AvatarMixer::AvatarMixer(ReceivedMessage& message) : packetReceiver.registerListener(PacketType::AvatarData, this, "handleAvatarDataPacket"); packetReceiver.registerListener(PacketType::AvatarIdentity, this, "handleAvatarIdentityPacket"); packetReceiver.registerListener(PacketType::KillAvatar, this, "handleKillAvatarPacket"); + + auto nodeList = DependencyManager::get(); + connect(nodeList.data(), &NodeList::packetVersionMismatch, this, &AvatarMixer::handlePacketVersionMismatch); } AvatarMixer::~AvatarMixer() { @@ -414,7 +417,9 @@ void AvatarMixer::handleAvatarIdentityPacket(QSharedPointer mes AvatarData& avatar = nodeData->getAvatar(); // parse the identity packet and update the change timestamp if appropriate - if (avatar.hasIdentityChangedAfterParsing(message->getMessage())) { + AvatarData::Identity identity; + AvatarData::parseAvatarIdentityPacket(message->getMessage(), identity); + if (avatar.processAvatarIdentity(identity)) { QMutexLocker nodeDataLocker(&nodeData->getMutex()); nodeData->flagIdentityChange(); } @@ -509,6 +514,19 @@ void AvatarMixer::domainSettingsRequestComplete() { _broadcastThread.start(); } +void AvatarMixer::handlePacketVersionMismatch(PacketType type, const HifiSockAddr& senderSockAddr, const QUuid& senderUUID) { + // if this client is using packet versions we don't expect. + if ((type == PacketTypeEnum::Value::AvatarIdentity || type == PacketTypeEnum::Value::AvatarData) && !senderUUID.isNull()) { + // Echo an empty AvatarData packet back to that client. + // This should trigger a version mismatch dialog on their side. + auto nodeList = DependencyManager::get(); + auto node = nodeList->nodeWithUUID(senderUUID); + if (node) { + auto emptyPacket = NLPacket::create(PacketType::AvatarData, 0); + nodeList->sendPacket(std::move(emptyPacket), *node); + } + } +} void AvatarMixer::parseDomainServerSettings(const QJsonObject& domainSettings) { const QString AVATAR_MIXER_SETTINGS_KEY = "avatar_mixer"; diff --git a/assignment-client/src/avatars/AvatarMixer.h b/assignment-client/src/avatars/AvatarMixer.h index c7761a2cba..d1a9249c83 100644 --- a/assignment-client/src/avatars/AvatarMixer.h +++ b/assignment-client/src/avatars/AvatarMixer.h @@ -38,7 +38,8 @@ private slots: void handleAvatarIdentityPacket(QSharedPointer message, SharedNodePointer senderNode); void handleKillAvatarPacket(QSharedPointer message); void domainSettingsRequestComplete(); - + void handlePacketVersionMismatch(PacketType type, const HifiSockAddr& senderSockAddr, const QUuid& senderUUID); + private: void broadcastAvatarData(); void parseDomainServerSettings(const QJsonObject& domainSettings); diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index d12306a122..ccda77a44d 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -641,10 +641,6 @@ void Avatar::simulateAttachments(float deltaTime) { } } -void Avatar::updateJointMappings() { - // no-op; joint mappings come from skeleton model -} - float Avatar::getBoundingRadius() const { return getBounds().getLargestDimension() / 2.0f; } diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index 7072ce3847..79952e8f58 100644 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -236,8 +236,6 @@ protected: virtual bool shouldRenderHead(const RenderArgs* renderArgs) const; virtual void fixupModelsInScene(); - virtual void updateJointMappings() override; - virtual void updatePalms(); render::ItemID _renderItemID{ render::Item::INVALID_ITEM_ID }; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 2366d078bd..5036eefbd6 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -687,8 +687,6 @@ void MyAvatar::saveData() { settings.setValue("headPitch", getHead()->getBasePitch()); - settings.setValue("pupilDilation", getHead()->getPupilDilation()); - settings.setValue("leanScale", _leanScale); settings.setValue("scale", _targetScale); @@ -805,8 +803,6 @@ void MyAvatar::loadData() { getHead()->setBasePitch(loadSetting(settings, "headPitch", 0.0f)); - getHead()->setPupilDilation(loadSetting(settings, "pupilDilation", 0.0f)); - _leanScale = loadSetting(settings, "leanScale", 0.05f); _targetScale = loadSetting(settings, "scale", 1.0f); setScale(glm::vec3(_targetScale)); diff --git a/interface/src/ui/PreferencesDialog.cpp b/interface/src/ui/PreferencesDialog.cpp index 9b1146340e..cb68b36c24 100644 --- a/interface/src/ui/PreferencesDialog.cpp +++ b/interface/src/ui/PreferencesDialog.cpp @@ -144,11 +144,6 @@ void setupPreferences() { preference->setStep(1); preferences->addPreference(preference); } - { - auto getter = [=]()->float { return myAvatar->getHead()->getPupilDilation(); }; - auto setter = [=](float value) { myAvatar->getHead()->setPupilDilation(value); }; - preferences->addPreference(new SliderPreference(AVATAR_TUNING, "Pupil dilation", getter, setter)); - } { auto getter = []()->float { return DependencyManager::get()->getEyeClosingThreshold(); }; auto setter = [](float value) { DependencyManager::get()->setEyeClosingThreshold(value); }; diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp index 2d37be9b87..351c09beee 100644 --- a/libraries/animation/src/AnimSkeleton.cpp +++ b/libraries/animation/src/AnimSkeleton.cpp @@ -107,6 +107,18 @@ void AnimSkeleton::convertAbsolutePosesToRelative(AnimPoseVec& poses) const { } } +void AnimSkeleton::convertAbsoluteRotationsToRelative(std::vector& rotations) const { + // poses start off absolute and leave in relative frame + int lastIndex = std::min((int)rotations.size(), (int)_joints.size()); + for (int i = lastIndex - 1; i >= 0; --i) { + int parentIndex = _joints[i].parentIndex; + if (parentIndex != -1) { + rotations[i] = glm::inverse(rotations[parentIndex]) * rotations[i]; + } + } +} + + void AnimSkeleton::mirrorRelativePoses(AnimPoseVec& poses) const { convertRelativePosesToAbsolute(poses); mirrorAbsolutePoses(poses); diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h index e2cd20d63e..68cce11326 100644 --- a/libraries/animation/src/AnimSkeleton.h +++ b/libraries/animation/src/AnimSkeleton.h @@ -55,6 +55,8 @@ public: void convertRelativePosesToAbsolute(AnimPoseVec& poses) const; void convertAbsolutePosesToRelative(AnimPoseVec& poses) const; + void convertAbsoluteRotationsToRelative(std::vector& rotations) const; + void mirrorRelativePoses(AnimPoseVec& poses) const; void mirrorAbsolutePoses(AnimPoseVec& poses) const; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 67dfbec24a..9bba9ffc33 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -165,6 +165,7 @@ void Rig::destroyAnimGraph() { void Rig::initJointStates(const FBXGeometry& geometry, const glm::mat4& modelOffset) { _geometryOffset = AnimPose(geometry.offset); + _invGeometryOffset = _geometryOffset.inverse(); setModelOffset(modelOffset); _animSkeleton = std::make_shared(geometry); @@ -193,6 +194,7 @@ void Rig::initJointStates(const FBXGeometry& geometry, const glm::mat4& modelOff void Rig::reset(const FBXGeometry& geometry) { _geometryOffset = AnimPose(geometry.offset); + _invGeometryOffset = _geometryOffset.inverse(); _animSkeleton = std::make_shared(geometry); _internalPoseSet._relativePoses.clear(); @@ -272,24 +274,6 @@ void Rig::setModelOffset(const glm::mat4& modelOffsetMat) { } } -bool Rig::getJointStateRotation(int index, glm::quat& rotation) const { - if (isIndexValid(index)) { - rotation = _internalPoseSet._relativePoses[index].rot; - return !isEqual(rotation, _animSkeleton->getRelativeDefaultPose(index).rot); - } else { - return false; - } -} - -bool Rig::getJointStateTranslation(int index, glm::vec3& translation) const { - if (isIndexValid(index)) { - translation = _internalPoseSet._relativePoses[index].trans; - return !isEqual(translation, _animSkeleton->getRelativeDefaultPose(index).trans); - } else { - return false; - } -} - void Rig::clearJointState(int index) { if (isIndexValid(index)) { _internalPoseSet._overrideFlags[index] = false; @@ -1229,24 +1213,90 @@ glm::mat4 Rig::getJointTransform(int jointIndex) const { } void Rig::copyJointsIntoJointData(QVector& jointDataVec) const { + + const AnimPose geometryToRigPose(_geometryToRigTransform); + jointDataVec.resize((int)getJointStateCount()); for (auto i = 0; i < jointDataVec.size(); i++) { JointData& data = jointDataVec[i]; - data.rotationSet |= getJointStateRotation(i, data.rotation); - // geometry offset is used here so that translations are in meters. - // this is what the avatar mixer expects - data.translationSet |= getJointStateTranslation(i, data.translation); - data.translation = _geometryOffset * data.translation; + if (isIndexValid(i)) { + // rotations are in absolute rig frame. + glm::quat defaultAbsRot = geometryToRigPose.rot * _animSkeleton->getAbsoluteDefaultPose(i).rot; + data.rotation = _internalPoseSet._absolutePoses[i].rot; + data.rotationSet = !isEqual(data.rotation, defaultAbsRot); + + // translations are in relative frame but scaled so that they are in meters, + // instead of geometry units. + glm::vec3 defaultRelTrans = _geometryOffset.scale * _animSkeleton->getRelativeDefaultPose(i).trans; + data.translation = _geometryOffset.scale * _internalPoseSet._relativePoses[i].trans; + data.translationSet = !isEqual(data.translation, defaultRelTrans); + } else { + data.translationSet = false; + data.rotationSet = false; + } } } void Rig::copyJointsFromJointData(const QVector& jointDataVec) { - AnimPose invGeometryOffset = _geometryOffset.inverse(); - for (int i = 0; i < jointDataVec.size(); i++) { - const JointData& data = jointDataVec.at(i); - setJointRotation(i, data.rotationSet, data.rotation, 1.0f); - // geometry offset is used here to undo the fact that avatar mixer translations are in meters. - setJointTranslation(i, data.translationSet, invGeometryOffset * data.translation, 1.0f); + + if (_animSkeleton) { + + // transform all the default poses into rig space. + const AnimPose geometryToRigPose(_geometryToRigTransform); + std::vector overrideFlags(_internalPoseSet._overridePoses.size(), false); + + // start with the default rotations in absolute rig frame + std::vector rotations; + rotations.reserve(_animSkeleton->getAbsoluteDefaultPoses().size()); + for (auto& pose : _animSkeleton->getAbsoluteDefaultPoses()) { + rotations.push_back(geometryToRigPose.rot * pose.rot); + } + + // start translations in relative frame but scaled to meters. + std::vector translations; + translations.reserve(_animSkeleton->getRelativeDefaultPoses().size()); + for (auto& pose : _animSkeleton->getRelativeDefaultPoses()) { + translations.push_back(_geometryOffset.scale * pose.trans); + } + + ASSERT(overrideFlags.size() == rotations.size()); + + // copy over rotations from the jointDataVec, which is also in absolute rig frame + for (int i = 0; i < jointDataVec.size(); i++) { + if (isIndexValid(i)) { + const JointData& data = jointDataVec.at(i); + if (data.rotationSet) { + overrideFlags[i] = true; + rotations[i] = data.rotation; + } + if (data.translationSet) { + overrideFlags[i] = true; + translations[i] = data.translation; + } + } + } + + ASSERT(_internalPoseSet._overrideFlags.size() == _internalPoseSet._overridePoses.size()); + + // convert resulting rotations into geometry space. + const glm::quat rigToGeometryRot(glmExtractRotation(_rigToGeometryTransform)); + for (auto& rot : rotations) { + rot = rigToGeometryRot * rot; + } + + // convert all rotations from absolute to parent relative. + _animSkeleton->convertAbsoluteRotationsToRelative(rotations); + + // copy the geometry space parent relative poses into _overridePoses + for (int i = 0; i < jointDataVec.size(); i++) { + if (overrideFlags[i]) { + _internalPoseSet._overrideFlags[i] = true; + _internalPoseSet._overridePoses[i].scale = Vectors::ONE; + _internalPoseSet._overridePoses[i].rot = rotations[i]; + // scale translations from meters back into geometry units. + _internalPoseSet._overridePoses[i].trans = _invGeometryOffset.scale * translations[i]; + } + } } } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 363006d48c..891d9fdb92 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -104,12 +104,6 @@ public: void setModelOffset(const glm::mat4& modelOffsetMat); - // geometry space - bool getJointStateRotation(int index, glm::quat& rotation) const; - - // geometry space - bool getJointStateTranslation(int index, glm::vec3& translation) const; - void clearJointState(int index); void clearJointStates(); void clearJointAnimationPriority(int index); @@ -119,8 +113,6 @@ public: // geometry space void setJointTranslation(int index, bool valid, const glm::vec3& translation, float priority); - - // geometry space void setJointRotation(int index, bool valid, const glm::quat& rotation, float priority); // legacy @@ -239,6 +231,7 @@ protected: AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) + AnimPose _invGeometryOffset; struct PoseSet { AnimPoseVec _relativePoses; // geometry space relative to parent. diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index b26cecbc9e..16e4bd5437 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -37,6 +37,8 @@ #include "AvatarLogging.h" +//#define WANT_DEBUG + quint64 DEFAULT_FILTERED_LOG_EXPIRY = 2 * USECS_PER_SECOND; using namespace std; @@ -46,6 +48,52 @@ const glm::vec3 DEFAULT_LOCAL_AABOX_SCALE(1.0f); const QString AvatarData::FRAME_NAME = "com.highfidelity.recording.AvatarData"; +namespace AvatarDataPacket { + // NOTE: AvatarDataPackets start with a uint16_t sequence number that is not reflected in the Header structure. + + PACKED_BEGIN struct Header { + float position[3]; // skeletal model's position + float globalPosition[3]; // avatar's position + uint16_t localOrientation[3]; // avatar's local euler angles (degrees, compressed) relative to the thing it's attached to + uint16_t scale; // (compressed) 'ratio' encoding uses sign bit as flag. + float lookAtPosition[3]; // world space position that eyes are focusing on. + float audioLoudness; // current loundess of microphone + uint8_t flags; + } PACKED_END; + const size_t HEADER_SIZE = 49; + + // only present if HAS_REFERENTIAL flag is set in header.flags + PACKED_BEGIN struct ParentInfo { + uint8_t parentUUID[16]; // rfc 4122 encoded + uint16_t parentJointIndex; + } PACKED_END; + const size_t PARENT_INFO_SIZE = 18; + + // only present if IS_FACESHIFT_CONNECTED flag is set in header.flags + PACKED_BEGIN struct FaceTrackerInfo { + float leftEyeBlink; + float rightEyeBlink; + float averageLoudness; + float browAudioLift; + uint8_t numBlendshapeCoefficients; + // float blendshapeCoefficients[numBlendshapeCoefficients]; + } PACKED_END; + const size_t FACE_TRACKER_INFO_SIZE = 17; + + // variable length structure follows + /* + struct JointData { + uint8_t numJoints; + uint8_t rotationValidityBits[ceil(numJoints / 8)]; // one bit per joint, if true then a compressed rotation follows. + SixByteQuat rotation[numValidRotations]; // encodeded and compressed by packOrientationQuatToSixBytes() + uint8_t translationValidityBits[ceil(numJoints / 8)]; // one bit per joint, if true then a compressed translation follows. + SixByteTrans translation[numValidTranslations]; // encodeded and compressed by packFloatVec3ToSignedTwoByteFixed() + }; + */ +} + +#define ASSERT(COND) do { if (!(COND)) { abort(); } } while(0) + AvatarData::AvatarData() : SpatiallyNestable(NestableType::Avatar, QUuid()), _handPosition(0.0f), @@ -66,6 +114,10 @@ AvatarData::AvatarData() : setBodyPitch(0.0f); setBodyYaw(-90.0f); setBodyRoll(0.0f); + + ASSERT(sizeof(AvatarDataPacket::Header) == AvatarDataPacket::HEADER_SIZE); + ASSERT(sizeof(AvatarDataPacket::ParentInfo) == AvatarDataPacket::PARENT_INFO_SIZE); + ASSERT(sizeof(AvatarDataPacket::FaceTrackerInfo) == AvatarDataPacket::FACE_TRACKER_INFO_SIZE); } AvatarData::~AvatarData() { @@ -139,87 +191,70 @@ QByteArray AvatarData::toByteArray(bool cullSmallChanges, bool sendAll) { unsigned char* destinationBuffer = reinterpret_cast(avatarDataByteArray.data()); unsigned char* startPosition = destinationBuffer; - const glm::vec3& position = getLocalPosition(); - memcpy(destinationBuffer, &position, sizeof(position)); - destinationBuffer += sizeof(position); + auto header = reinterpret_cast(destinationBuffer); + header->position[0] = getLocalPosition().x; + header->position[1] = getLocalPosition().y; + header->position[2] = getLocalPosition().z; + header->globalPosition[0] = _globalPosition.x; + header->globalPosition[1] = _globalPosition.y; + header->globalPosition[2] = _globalPosition.z; - memcpy(destinationBuffer, &_globalPosition, sizeof(_globalPosition)); - destinationBuffer += sizeof(_globalPosition); - - // Body rotation glm::vec3 bodyEulerAngles = glm::degrees(safeEulerAngles(getLocalOrientation())); - destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, bodyEulerAngles.y); - destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, bodyEulerAngles.x); - destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, bodyEulerAngles.z); + packFloatAngleToTwoByte((uint8_t*)(header->localOrientation + 0), bodyEulerAngles.y); + packFloatAngleToTwoByte((uint8_t*)(header->localOrientation + 1), bodyEulerAngles.x); + packFloatAngleToTwoByte((uint8_t*)(header->localOrientation + 2), bodyEulerAngles.z); + packFloatRatioToTwoByte((uint8_t*)(&header->scale), _targetScale); + header->lookAtPosition[0] = _headData->_lookAtPosition.x; + header->lookAtPosition[1] = _headData->_lookAtPosition.y; + header->lookAtPosition[2] = _headData->_lookAtPosition.z; + header->audioLoudness = _headData->_audioLoudness; - // Body scale - destinationBuffer += packFloatRatioToTwoByte(destinationBuffer, _targetScale); - - // Lookat Position - memcpy(destinationBuffer, &_headData->_lookAtPosition, sizeof(_headData->_lookAtPosition)); - destinationBuffer += sizeof(_headData->_lookAtPosition); - - // Instantaneous audio loudness (used to drive facial animation) - memcpy(destinationBuffer, &_headData->_audioLoudness, sizeof(float)); - destinationBuffer += sizeof(float); - - // bitMask of less than byte wide items - unsigned char bitItems = 0; - - // key state - setSemiNibbleAt(bitItems,KEY_STATE_START_BIT,_keyState); + setSemiNibbleAt(header->flags, KEY_STATE_START_BIT, _keyState); // hand state bool isFingerPointing = _handState & IS_FINGER_POINTING_FLAG; - setSemiNibbleAt(bitItems, HAND_STATE_START_BIT, _handState & ~IS_FINGER_POINTING_FLAG); + setSemiNibbleAt(header->flags, HAND_STATE_START_BIT, _handState & ~IS_FINGER_POINTING_FLAG); if (isFingerPointing) { - setAtBit(bitItems, HAND_STATE_FINGER_POINTING_BIT); + setAtBit(header->flags, HAND_STATE_FINGER_POINTING_BIT); } // faceshift state if (_headData->_isFaceTrackerConnected) { - setAtBit(bitItems, IS_FACESHIFT_CONNECTED); + setAtBit(header->flags, IS_FACESHIFT_CONNECTED); } // eye tracker state if (_headData->_isEyeTrackerConnected) { - setAtBit(bitItems, IS_EYE_TRACKER_CONNECTED); + setAtBit(header->flags, IS_EYE_TRACKER_CONNECTED); } // referential state QUuid parentID = getParentID(); if (!parentID.isNull()) { - setAtBit(bitItems, HAS_REFERENTIAL); + setAtBit(header->flags, HAS_REFERENTIAL); } - *destinationBuffer++ = bitItems; + destinationBuffer += sizeof(AvatarDataPacket::Header); if (!parentID.isNull()) { + auto parentInfo = reinterpret_cast(destinationBuffer); QByteArray referentialAsBytes = parentID.toRfc4122(); - memcpy(destinationBuffer, referentialAsBytes.data(), referentialAsBytes.size()); - destinationBuffer += referentialAsBytes.size(); - memcpy(destinationBuffer, &_parentJointIndex, sizeof(_parentJointIndex)); - destinationBuffer += sizeof(_parentJointIndex); + memcpy(parentInfo->parentUUID, referentialAsBytes.data(), referentialAsBytes.size()); + parentInfo->parentJointIndex = _parentJointIndex; + destinationBuffer += sizeof(AvatarDataPacket::ParentInfo); } // If it is connected, pack up the data if (_headData->_isFaceTrackerConnected) { - memcpy(destinationBuffer, &_headData->_leftEyeBlink, sizeof(float)); - destinationBuffer += sizeof(float); + auto faceTrackerInfo = reinterpret_cast(destinationBuffer); - memcpy(destinationBuffer, &_headData->_rightEyeBlink, sizeof(float)); - destinationBuffer += sizeof(float); + faceTrackerInfo->leftEyeBlink = _headData->_leftEyeBlink; + faceTrackerInfo->rightEyeBlink = _headData->_rightEyeBlink; + faceTrackerInfo->averageLoudness = _headData->_averageLoudness; + faceTrackerInfo->browAudioLift = _headData->_browAudioLift; + faceTrackerInfo->numBlendshapeCoefficients = _headData->_blendshapeCoefficients.size(); + destinationBuffer += sizeof(AvatarDataPacket::FaceTrackerInfo); - memcpy(destinationBuffer, &_headData->_averageLoudness, sizeof(float)); - destinationBuffer += sizeof(float); - - memcpy(destinationBuffer, &_headData->_browAudioLift, sizeof(float)); - destinationBuffer += sizeof(float); - - *destinationBuffer++ = _headData->_blendshapeCoefficients.size(); - memcpy(destinationBuffer, _headData->_blendshapeCoefficients.data(), - _headData->_blendshapeCoefficients.size() * sizeof(float)); + // followed by a variable number of float coefficients + memcpy(destinationBuffer, _headData->_blendshapeCoefficients.data(), _headData->_blendshapeCoefficients.size() * sizeof(float)); destinationBuffer += _headData->_blendshapeCoefficients.size() * sizeof(float); } - // pupil dilation - destinationBuffer += packFloatToByte(destinationBuffer, _headData->_pupilDilation, 1.0f); - // joint rotation data *destinationBuffer++ = _jointData.size(); unsigned char* validityPosition = destinationBuffer; @@ -261,7 +296,7 @@ QByteArray AvatarData::toByteArray(bool cullSmallChanges, bool sendAll) { for (int i = 0; i < _jointData.size(); i ++) { const JointData& data = _jointData[ i ]; if (validity & (1 << validityBit)) { - destinationBuffer += packOrientationQuatToBytes(destinationBuffer, data.rotation); + destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, data.rotation); } if (++validityBit == BITS_IN_BYTE) { validityBit = 0; @@ -304,15 +339,11 @@ QByteArray AvatarData::toByteArray(bool cullSmallChanges, bool sendAll) { } } - if (validityBit != 0) { *destinationBuffer++ = validity; } - // TODO -- automatically pick translationCompressionRadix - int translationCompressionRadix = 12; - - *destinationBuffer++ = translationCompressionRadix; + const int TRANSLATION_COMPRESSION_RADIX = 12; validityBit = 0; validity = *validityPosition++; @@ -320,7 +351,7 @@ QByteArray AvatarData::toByteArray(bool cullSmallChanges, bool sendAll) { const JointData& data = _jointData[ i ]; if (validity & (1 << validityBit)) { destinationBuffer += - packFloatVec3ToSignedTwoByteFixed(destinationBuffer, data.translation, translationCompressionRadix); + packFloatVec3ToSignedTwoByteFixed(destinationBuffer, data.translation, TRANSLATION_COMPRESSION_RADIX); } if (++validityBit == BITS_IN_BYTE) { validityBit = 0; @@ -333,7 +364,6 @@ QByteArray AvatarData::toByteArray(bool cullSmallChanges, bool sendAll) { qDebug() << "AvatarData::toByteArray" << cullSmallChanges << sendAll << "rotations:" << rotationSentCount << "translations:" << translationSentCount << "largest:" << maxTranslationDimension - << "radix:" << translationCompressionRadix << "size:" << (beforeRotations - startPosition) << "+" << (beforeTranslations - beforeRotations) << "+" @@ -370,6 +400,12 @@ void AvatarData::doneEncoding(bool cullSmallChanges) { } bool AvatarData::shouldLogError(const quint64& now) { +#ifdef WANT_DEBUG + if (now > 0) { + return true; + } +#endif + if (now > _errorLogExpiry) { _errorLogExpiry = now + DEFAULT_FILTERED_LOG_EXPIRY; return true; @@ -377,6 +413,16 @@ bool AvatarData::shouldLogError(const quint64& now) { return false; } +#define PACKET_READ_CHECK(ITEM_NAME, SIZE_TO_READ) \ + if ((endPosition - sourceBuffer) < (int)SIZE_TO_READ) { \ + if (shouldLogError(now)) { \ + qCWarning(avatars) << "AvatarData packet too small, attempting to read " << \ + #ITEM_NAME << ", only " << (endPosition - sourceBuffer) << \ + " bytes left, " << getSessionUUID(); \ + } \ + return buffer.size(); \ + } + // read data in packet starting at byte offset and return number of bytes parsed int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { @@ -386,125 +432,76 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { } const unsigned char* startPosition = reinterpret_cast(buffer.data()); + const unsigned char* endPosition = startPosition + buffer.size(); const unsigned char* sourceBuffer = startPosition; quint64 now = usecTimestampNow(); - // The absolute minimum size of the update data is as follows: - // 36 bytes of "plain old data" { - // position = 12 bytes - // bodyYaw = 2 (compressed float) - // bodyPitch = 2 (compressed float) - // bodyRoll = 2 (compressed float) - // targetScale = 2 (compressed float) - // lookAt = 12 - // audioLoudness = 4 - // } - // + 1 byte for varying data - // + 1 byte for pupilSize - // + 1 byte for numJoints (0) - // = 39 bytes - int minPossibleSize = 39; + PACKET_READ_CHECK(Header, sizeof(AvatarDataPacket::Header)); + auto header = reinterpret_cast(sourceBuffer); + sourceBuffer += sizeof(AvatarDataPacket::Header); - int maxAvailableSize = buffer.size(); - if (minPossibleSize > maxAvailableSize) { + glm::vec3 position = glm::vec3(header->position[0], header->position[1], header->position[2]); + _globalPosition = glm::vec3(header->globalPosition[0], header->globalPosition[1], header->globalPosition[2]); + if (isNaN(position)) { if (shouldLogError(now)) { - qCDebug(avatars) << "Malformed AvatarData packet at the start; " - << " displayName = '" << _displayName << "'" - << " minPossibleSize = " << minPossibleSize - << " maxAvailableSize = " << maxAvailableSize; + qCWarning(avatars) << "Discard AvatarData packet: position NaN, uuid " << getSessionUUID(); } - // this packet is malformed so we report all bytes as consumed - return maxAvailableSize; + return buffer.size(); + } + setLocalPosition(position); + + float pitch, yaw, roll; + unpackFloatAngleFromTwoByte(header->localOrientation + 0, &yaw); + unpackFloatAngleFromTwoByte(header->localOrientation + 1, &pitch); + unpackFloatAngleFromTwoByte(header->localOrientation + 2, &roll); + if (isNaN(yaw) || isNaN(pitch) || isNaN(roll)) { + if (shouldLogError(now)) { + qCWarning(avatars) << "Discard AvatarData packet: localOriention is NaN, uuid " << getSessionUUID(); + } + return buffer.size(); } - { // Body world position, rotation, and scale - // position - glm::vec3 position; - memcpy(&position, sourceBuffer, sizeof(position)); - sourceBuffer += sizeof(position); + glm::quat currentOrientation = getLocalOrientation(); + glm::vec3 newEulerAngles(pitch, yaw, roll); + glm::quat newOrientation = glm::quat(glm::radians(newEulerAngles)); + if (currentOrientation != newOrientation) { + _hasNewJointRotations = true; + setLocalOrientation(newOrientation); + } - memcpy(&_globalPosition, sourceBuffer, sizeof(_globalPosition)); - sourceBuffer += sizeof(_globalPosition); - - if (glm::isnan(position.x) || glm::isnan(position.y) || glm::isnan(position.z)) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Discard nan AvatarData::position; displayName = '" << _displayName << "'"; - } - return maxAvailableSize; + float scale; + unpackFloatRatioFromTwoByte((uint8_t*)&header->scale, scale); + if (isNaN(scale)) { + if (shouldLogError(now)) { + qCWarning(avatars) << "Discard AvatarData packet: scale NaN, uuid " << getSessionUUID(); } - setLocalPosition(position); + return buffer.size(); + } + _targetScale = std::max(MIN_AVATAR_SCALE, std::min(MAX_AVATAR_SCALE, scale)); - // rotation (NOTE: This needs to become a quaternion to save two bytes) - float yaw, pitch, roll; - sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &yaw); - sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &pitch); - sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t*) sourceBuffer, &roll); - if (glm::isnan(yaw) || glm::isnan(pitch) || glm::isnan(roll)) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Discard nan AvatarData::yaw,pitch,roll; displayName = '" << _displayName << "'"; - } - return maxAvailableSize; + glm::vec3 lookAt = glm::vec3(header->lookAtPosition[0], header->lookAtPosition[1], header->lookAtPosition[2]); + if (isNaN(lookAt)) { + if (shouldLogError(now)) { + qCWarning(avatars) << "Discard AvatarData packet: lookAtPosition is NaN, uuid " << getSessionUUID(); } + return buffer.size(); + } + _headData->_lookAtPosition = lookAt; - // TODO is this safe? will the floats not exactly match? - // Andrew says: - // Yes, there is a possibility that the transmitted will not quite match the extracted despite being originally - // extracted from the exact same quaternion. I followed the code through and it appears the risk is that the - // avatar's SkeletonModel might fall into the CPU expensive part of Model::updateClusterMatrices() when otherwise it - // would not have required it. However, we know we can update many simultaneously animating avatars, and most - // avatars will be moving constantly anyway, so I don't think we need to worry. - glm::quat currentOrientation = getLocalOrientation(); - glm::vec3 newEulerAngles(pitch, yaw, roll); - glm::quat newOrientation = glm::quat(glm::radians(newEulerAngles)); - if (currentOrientation != newOrientation) { - _hasNewJointRotations = true; - setLocalOrientation(newOrientation); + float audioLoudness = header->audioLoudness; + if (isNaN(audioLoudness)) { + if (shouldLogError(now)) { + qCWarning(avatars) << "Discard AvatarData packet: audioLoudness is NaN, uuid " << getSessionUUID(); } - - // scale - float scale; - sourceBuffer += unpackFloatRatioFromTwoByte(sourceBuffer, scale); - if (glm::isnan(scale)) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Discard nan AvatarData::scale; displayName = '" << _displayName << "'"; - } - return maxAvailableSize; - } - _targetScale = std::max(MIN_AVATAR_SCALE, std::min(MAX_AVATAR_SCALE, scale)); - } // 20 bytes - - { // Lookat Position - glm::vec3 lookAt; - memcpy(&lookAt, sourceBuffer, sizeof(lookAt)); - sourceBuffer += sizeof(lookAt); - if (glm::isnan(lookAt.x) || glm::isnan(lookAt.y) || glm::isnan(lookAt.z)) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Discard nan AvatarData::lookAt; displayName = '" << _displayName << "'"; - } - return maxAvailableSize; - } - _headData->_lookAtPosition = lookAt; - } // 12 bytes - - { // AudioLoudness - // Instantaneous audio loudness (used to drive facial animation) - float audioLoudness; - memcpy(&audioLoudness, sourceBuffer, sizeof(float)); - sourceBuffer += sizeof(float); - if (glm::isnan(audioLoudness)) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Discard nan AvatarData::audioLoudness; displayName = '" << _displayName << "'"; - } - return maxAvailableSize; - } - _headData->_audioLoudness = audioLoudness; - } // 4 bytes + return buffer.size(); + } + _headData->_audioLoudness = audioLoudness; { // bitFlags and face data - unsigned char bitItems = *sourceBuffer++; + uint8_t bitItems = header->flags; // key state, stored as a semi-nibble in the bitItems - _keyState = (KeyState)getSemiNibbleAt(bitItems,KEY_STATE_START_BIT); + _keyState = (KeyState)getSemiNibbleAt(bitItems, KEY_STATE_START_BIT); // hand state, stored as a semi-nibble plus a bit in the bitItems // we store the hand state as well as other items in a shared bitset. The hand state is an octal, but is split @@ -521,98 +518,47 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { bool hasReferential = oneAtBit(bitItems, HAS_REFERENTIAL); if (hasReferential) { - const int sizeOfPackedUuid = 16; - QByteArray referentialAsBytes((const char*)sourceBuffer, sizeOfPackedUuid); - _parentID = QUuid::fromRfc4122(referentialAsBytes); - sourceBuffer += sizeOfPackedUuid; - memcpy(&_parentJointIndex, sourceBuffer, sizeof(_parentJointIndex)); - sourceBuffer += sizeof(_parentJointIndex); + PACKET_READ_CHECK(ParentInfo, sizeof(AvatarDataPacket::ParentInfo)); + auto parentInfo = reinterpret_cast(sourceBuffer); + sourceBuffer += sizeof(AvatarDataPacket::ParentInfo); + + QByteArray byteArray((const char*)parentInfo->parentUUID, NUM_BYTES_RFC4122_UUID); + _parentID = QUuid::fromRfc4122(byteArray); + _parentJointIndex = parentInfo->parentJointIndex; } else { _parentID = QUuid(); } if (_headData->_isFaceTrackerConnected) { - float leftEyeBlink, rightEyeBlink, averageLoudness, browAudioLift; - minPossibleSize += sizeof(leftEyeBlink) + sizeof(rightEyeBlink) + sizeof(averageLoudness) + sizeof(browAudioLift); - minPossibleSize++; // one byte for blendDataSize - if (minPossibleSize > maxAvailableSize) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Malformed AvatarData packet after BitItems;" - << " displayName = '" << _displayName << "'" - << " minPossibleSize = " << minPossibleSize - << " maxAvailableSize = " << maxAvailableSize; - } - return maxAvailableSize; - } - // unpack face data - memcpy(&leftEyeBlink, sourceBuffer, sizeof(float)); - sourceBuffer += sizeof(float); + PACKET_READ_CHECK(FaceTrackerInfo, sizeof(AvatarDataPacket::FaceTrackerInfo)); + auto faceTrackerInfo = reinterpret_cast(sourceBuffer); + sourceBuffer += sizeof(AvatarDataPacket::FaceTrackerInfo); - memcpy(&rightEyeBlink, sourceBuffer, sizeof(float)); - sourceBuffer += sizeof(float); + _headData->_leftEyeBlink = faceTrackerInfo->leftEyeBlink; + _headData->_rightEyeBlink = faceTrackerInfo->rightEyeBlink; + _headData->_averageLoudness = faceTrackerInfo->averageLoudness; + _headData->_browAudioLift = faceTrackerInfo->browAudioLift; - memcpy(&averageLoudness, sourceBuffer, sizeof(float)); - sourceBuffer += sizeof(float); - - memcpy(&browAudioLift, sourceBuffer, sizeof(float)); - sourceBuffer += sizeof(float); - - if (glm::isnan(leftEyeBlink) || glm::isnan(rightEyeBlink) - || glm::isnan(averageLoudness) || glm::isnan(browAudioLift)) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Discard nan AvatarData::faceData; displayName = '" << _displayName << "'"; - } - return maxAvailableSize; - } - _headData->_leftEyeBlink = leftEyeBlink; - _headData->_rightEyeBlink = rightEyeBlink; - _headData->_averageLoudness = averageLoudness; - _headData->_browAudioLift = browAudioLift; - - int numCoefficients = (int)(*sourceBuffer++); - int blendDataSize = numCoefficients * sizeof(float); - minPossibleSize += blendDataSize; - if (minPossibleSize > maxAvailableSize) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Malformed AvatarData packet after Blendshapes;" - << " displayName = '" << _displayName << "'" - << " minPossibleSize = " << minPossibleSize - << " maxAvailableSize = " << maxAvailableSize; - } - return maxAvailableSize; - } - - _headData->_blendshapeCoefficients.resize(numCoefficients); - memcpy(_headData->_blendshapeCoefficients.data(), sourceBuffer, blendDataSize); - sourceBuffer += numCoefficients * sizeof(float); - - //bitItemsDataSize = 4 * sizeof(float) + 1 + blendDataSize; + int numCoefficients = faceTrackerInfo->numBlendshapeCoefficients; + const int coefficientsSize = sizeof(float) * numCoefficients; + PACKET_READ_CHECK(FaceTrackerCoefficients, coefficientsSize); + _headData->_blendshapeCoefficients.resize(numCoefficients); // make sure there's room for the copy! + memcpy(_headData->_blendshapeCoefficients.data(), sourceBuffer, coefficientsSize); + sourceBuffer += coefficientsSize; } - } // 1 + bitItemsDataSize bytes - - { // pupil dilation - sourceBuffer += unpackFloatFromByte(sourceBuffer, _headData->_pupilDilation, 1.0f); - } // 1 byte - - // joint rotations - int numJoints = *sourceBuffer++; - int bytesOfValidity = (int)ceil((float)numJoints / (float)BITS_IN_BYTE); - minPossibleSize += bytesOfValidity; - if (minPossibleSize > maxAvailableSize) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Malformed AvatarData packet after JointValidityBits;" - << " displayName = '" << _displayName << "'" - << " minPossibleSize = " << minPossibleSize - << " maxAvailableSize = " << maxAvailableSize; - } - return maxAvailableSize; } - int numValidJointRotations = 0; + + PACKET_READ_CHECK(NumJoints, sizeof(uint8_t)); + int numJoints = *sourceBuffer++; + _jointData.resize(numJoints); + const int bytesOfValidity = (int)ceil((float)numJoints / (float)BITS_IN_BYTE); + PACKET_READ_CHECK(JointRotationValidityBits, bytesOfValidity); + + int numValidJointRotations = 0; QVector validRotations; validRotations.resize(numJoints); - { // rotation validity bits unsigned char validity = 0; int validityBit = 0; @@ -627,38 +573,26 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { validRotations[i] = valid; validityBit = (validityBit + 1) % BITS_IN_BYTE; } - } // 1 + bytesOfValidity bytes - - // each joint rotation component is stored in two bytes (sizeof(uint16_t)) - int COMPONENTS_PER_QUATERNION = 4; - minPossibleSize += numValidJointRotations * COMPONENTS_PER_QUATERNION * sizeof(uint16_t); - if (minPossibleSize > maxAvailableSize) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Malformed AvatarData packet after JointData rotation validity;" - << " displayName = '" << _displayName << "'" - << " minPossibleSize = " << minPossibleSize - << " maxAvailableSize = " << maxAvailableSize; - } - return maxAvailableSize; } - { // joint data - for (int i = 0; i < numJoints; i++) { - JointData& data = _jointData[i]; - if (validRotations[i]) { - _hasNewJointRotations = true; - data.rotationSet = true; - sourceBuffer += unpackOrientationQuatFromBytes(sourceBuffer, data.rotation); - } + // each joint rotation is stored in 6 bytes. + const int COMPRESSED_QUATERNION_SIZE = 6; + PACKET_READ_CHECK(JointRotations, numValidJointRotations * COMPRESSED_QUATERNION_SIZE); + for (int i = 0; i < numJoints; i++) { + JointData& data = _jointData[i]; + if (validRotations[i]) { + sourceBuffer += unpackOrientationQuatFromSixBytes(sourceBuffer, data.rotation); + _hasNewJointRotations = true; + data.rotationSet = true; } - } // numJoints * 8 bytes + } + + PACKET_READ_CHECK(JointTranslationValidityBits, bytesOfValidity); - // joint translations // get translation validity bits -- these indicate which translations were packed int numValidJointTranslations = 0; QVector validTranslations; validTranslations.resize(numJoints); - { // translation validity bits unsigned char validity = 0; int validityBit = 0; @@ -675,42 +609,36 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) { } } // 1 + bytesOfValidity bytes - // each joint translation component is stored in 6 bytes. 1 byte for translationCompressionRadix - minPossibleSize += numValidJointTranslations * 6 + 1; - if (minPossibleSize > maxAvailableSize) { - if (shouldLogError(now)) { - qCDebug(avatars) << "Malformed AvatarData packet after JointData translation validity;" - << " displayName = '" << _displayName << "'" - << " minPossibleSize = " << minPossibleSize - << " maxAvailableSize = " << maxAvailableSize; + // each joint translation component is stored in 6 bytes. + const int COMPRESSED_TRANSLATION_SIZE = 6; + PACKET_READ_CHECK(JointTranslation, numValidJointTranslations * COMPRESSED_TRANSLATION_SIZE); + const int TRANSLATION_COMPRESSION_RADIX = 12; + + for (int i = 0; i < numJoints; i++) { + JointData& data = _jointData[i]; + if (validTranslations[i]) { + sourceBuffer += unpackFloatVec3FromSignedTwoByteFixed(sourceBuffer, data.translation, TRANSLATION_COMPRESSION_RADIX); + _hasNewJointTranslations = true; + data.translationSet = true; } - return maxAvailableSize; } - int translationCompressionRadix = *sourceBuffer++; - - { // joint data - for (int i = 0; i < numJoints; i++) { - JointData& data = _jointData[i]; - if (validTranslations[i]) { - sourceBuffer += - unpackFloatVec3FromSignedTwoByteFixed(sourceBuffer, data.translation, translationCompressionRadix); - _hasNewJointTranslations = true; - data.translationSet = true; - } - } - } // numJoints * 12 bytes - #ifdef WANT_DEBUG if (numValidJointRotations > 15) { qDebug() << "RECEIVING -- rotations:" << numValidJointRotations << "translations:" << numValidJointTranslations - << "radix:" << translationCompressionRadix << "size:" << (int)(sourceBuffer - startPosition); } #endif int numBytesRead = sourceBuffer - startPosition; + + if (numBytesRead != buffer.size()) { + if (shouldLogError(now)) { + qCWarning(avatars) << "AvatarData packet size mismatch: expected " << numBytesRead << " received " << buffer.size(); + } + } + _averageBytesReceived.updateAverage(numBytesRead); return numBytesRead; } @@ -954,38 +882,33 @@ void AvatarData::clearJointsData() { } } -bool AvatarData::hasIdentityChangedAfterParsing(const QByteArray& data) { - // this is used by the avatar-mixer +void AvatarData::parseAvatarIdentityPacket(const QByteArray& data, Identity& identityOut) { QDataStream packetStream(data); - QUuid avatarUUID; - QUrl unusedModelURL; // legacy faceModel support - QUrl skeletonModelURL; - QVector attachmentData; - AvatarEntityMap avatarEntityData; - QString displayName; - packetStream >> avatarUUID >> unusedModelURL >> skeletonModelURL >> attachmentData >> displayName >> avatarEntityData; + packetStream >> identityOut.uuid >> identityOut.skeletonModelURL >> identityOut.attachmentData >> identityOut.displayName >> identityOut.avatarEntityData; +} +bool AvatarData::processAvatarIdentity(const Identity& identity) { bool hasIdentityChanged = false; - if (_firstSkeletonCheck || (skeletonModelURL != _skeletonModelURL)) { - setSkeletonModelURL(skeletonModelURL); + if (_firstSkeletonCheck || (identity.skeletonModelURL != _skeletonModelURL)) { + setSkeletonModelURL(identity.skeletonModelURL); hasIdentityChanged = true; _firstSkeletonCheck = false; } - if (displayName != _displayName) { - setDisplayName(displayName); + if (identity.displayName != _displayName) { + setDisplayName(identity.displayName); hasIdentityChanged = true; } - if (attachmentData != _attachmentData) { - setAttachmentData(attachmentData); + if (identity.attachmentData != _attachmentData) { + setAttachmentData(identity.attachmentData); hasIdentityChanged = true; } - if (avatarEntityData != _avatarEntityData) { - setAvatarEntityData(avatarEntityData); + if (identity.avatarEntityData != _avatarEntityData) { + setAvatarEntityData(identity.avatarEntityData); hasIdentityChanged = true; } @@ -998,21 +921,18 @@ QByteArray AvatarData::identityByteArray() { QUrl emptyURL(""); const QUrl& urlToSend = _skeletonModelURL.scheme() == "file" ? emptyURL : _skeletonModelURL; - QUrl unusedModelURL; // legacy faceModel support - - identityStream << QUuid() << unusedModelURL << urlToSend << _attachmentData << _displayName << _avatarEntityData; + identityStream << getSessionUUID() << urlToSend << _attachmentData << _displayName << _avatarEntityData; return identityData; } - void AvatarData::setSkeletonModelURL(const QUrl& skeletonModelURL) { const QUrl& expanded = skeletonModelURL.isEmpty() ? AvatarData::defaultFullAvatarModelUrl() : skeletonModelURL; if (expanded == _skeletonModelURL) { return; } _skeletonModelURL = expanded; - qCDebug(avatars) << "Changing skeleton model for avatar to" << _skeletonModelURL.toString(); + qCDebug(avatars) << "Changing skeleton model for avatar" << getSessionUUID() << "to" << _skeletonModelURL.toString(); updateJointMappings(); } diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 2402a052c6..61ee649273 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -53,6 +53,7 @@ typedef unsigned long long quint64; #include #include #include +#include #include "AABox.h" #include "HeadData.h" @@ -171,6 +172,7 @@ class AvatarData : public QObject, public SpatiallyNestable { Q_PROPERTY(QUuid sessionUUID READ getSessionUUID) public: + static const QString FRAME_NAME; static void fromFrame(const QByteArray& frameData, AvatarData& avatar); @@ -289,7 +291,19 @@ public: const HeadData* getHeadData() const { return _headData; } - bool hasIdentityChangedAfterParsing(const QByteArray& data); + struct Identity { + QUuid uuid; + QUrl skeletonModelURL; + QVector attachmentData; + QString displayName; + AvatarEntityMap avatarEntityData; + }; + + static void parseAvatarIdentityPacket(const QByteArray& data, Identity& identityOut); + + // returns true if identity has changed, false otherwise. + bool processAvatarIdentity(const Identity& identity); + QByteArray identityByteArray(); const QUrl& getSkeletonModelURL() const { return _skeletonModelURL; } diff --git a/libraries/avatars/src/AvatarHashMap.cpp b/libraries/avatars/src/AvatarHashMap.cpp index bd43560ae8..9084fd837b 100644 --- a/libraries/avatars/src/AvatarHashMap.cpp +++ b/libraries/avatars/src/AvatarHashMap.cpp @@ -50,26 +50,26 @@ AvatarSharedPointer AvatarHashMap::newSharedAvatar() { AvatarSharedPointer AvatarHashMap::addAvatar(const QUuid& sessionUUID, const QWeakPointer& mixerWeakPointer) { qCDebug(avatars) << "Adding avatar with sessionUUID " << sessionUUID << "to AvatarHashMap."; - + auto avatar = newSharedAvatar(); avatar->setSessionUUID(sessionUUID); avatar->setOwningAvatarMixer(mixerWeakPointer); - + _avatarHash.insert(sessionUUID, avatar); emit avatarAddedEvent(sessionUUID); - + return avatar; } AvatarSharedPointer AvatarHashMap::newOrExistingAvatar(const QUuid& sessionUUID, const QWeakPointer& mixerWeakPointer) { QWriteLocker locker(&_hashLock); - + auto avatar = _avatarHash.value(sessionUUID); - + if (!avatar) { avatar = addAvatar(sessionUUID, mixerWeakPointer); } - + return avatar; } @@ -86,14 +86,14 @@ void AvatarHashMap::processAvatarDataPacket(QSharedPointer mess // only add them if mixerWeakPointer points to something (meaning that mixer is still around) while (message->getBytesLeftToRead()) { QUuid sessionUUID = QUuid::fromRfc4122(message->readWithoutCopy(NUM_BYTES_RFC4122_UUID)); - + int positionBeforeRead = message->getPosition(); QByteArray byteArray = message->readWithoutCopy(message->getBytesLeftToRead()); - + if (sessionUUID != _lastOwnerSessionUUID) { auto avatar = newOrExistingAvatar(sessionUUID, sendingNode); - + // have the matching (or new) avatar parse the data from the packet int bytesRead = avatar->parseDataFromBuffer(byteArray); message->seek(positionBeforeRead + bytesRead); @@ -107,37 +107,12 @@ void AvatarHashMap::processAvatarDataPacket(QSharedPointer mess } void AvatarHashMap::processAvatarIdentityPacket(QSharedPointer message, SharedNodePointer sendingNode) { - // this is used by clients - // setup a data stream to parse the packet - QDataStream identityStream(message->getMessage()); + AvatarData::Identity identity; + AvatarData::parseAvatarIdentityPacket(message->getMessage(), identity); - QUuid sessionUUID; - - while (!identityStream.atEnd()) { - - QUrl faceMeshURL, skeletonURL; - QVector attachmentData; - AvatarEntityMap avatarEntityData; - QString displayName; - identityStream >> sessionUUID >> faceMeshURL >> skeletonURL >> attachmentData >> displayName >> avatarEntityData; - - // mesh URL for a UUID, find avatar in our list - auto avatar = newOrExistingAvatar(sessionUUID, sendingNode); - - if (avatar->getSkeletonModelURL().isEmpty() || (avatar->getSkeletonModelURL() != skeletonURL)) { - avatar->setSkeletonModelURL(skeletonURL); // Will expand "" to default and so will not continuously fire - } - - if (avatar->getAttachmentData() != attachmentData) { - avatar->setAttachmentData(attachmentData); - } - - avatar->setAvatarEntityData(avatarEntityData); - - if (avatar->getDisplayName() != displayName) { - avatar->setDisplayName(displayName); - } - } + // mesh URL for a UUID, find avatar in our list + auto avatar = newOrExistingAvatar(identity.uuid, sendingNode); + avatar->processAvatarIdentity(identity); } void AvatarHashMap::processKillAvatar(QSharedPointer message, SharedNodePointer sendingNode) { @@ -148,9 +123,9 @@ void AvatarHashMap::processKillAvatar(QSharedPointer message, S void AvatarHashMap::removeAvatar(const QUuid& sessionUUID) { QWriteLocker locker(&_hashLock); - + auto removedAvatar = _avatarHash.take(sessionUUID); - + if (removedAvatar) { handleRemovedAvatar(removedAvatar); } diff --git a/libraries/avatars/src/HeadData.cpp b/libraries/avatars/src/HeadData.cpp index b98112d6e0..1aee85b2cd 100644 --- a/libraries/avatars/src/HeadData.cpp +++ b/libraries/avatars/src/HeadData.cpp @@ -43,10 +43,9 @@ HeadData::HeadData(AvatarData* owningAvatar) : _averageLoudness(0.0f), _browAudioLift(0.0f), _audioAverageLoudness(0.0f), - _pupilDilation(0.0f), _owningAvatar(owningAvatar) { - + } glm::quat HeadData::getRawOrientation() const { @@ -72,7 +71,7 @@ void HeadData::setOrientation(const glm::quat& orientation) { glm::vec3 newFront = glm::inverse(bodyOrientation) * (orientation * IDENTITY_FRONT); bodyOrientation = bodyOrientation * glm::angleAxis(atan2f(-newFront.x, -newFront.z), glm::vec3(0.0f, 1.0f, 0.0f)); _owningAvatar->setOrientation(bodyOrientation); - + // the rest goes to the head glm::vec3 eulers = glm::degrees(safeEulerAngles(glm::inverse(bodyOrientation) * orientation)); _basePitch = eulers.x; @@ -186,4 +185,3 @@ void HeadData::fromJson(const QJsonObject& json) { } } } - diff --git a/libraries/avatars/src/HeadData.h b/libraries/avatars/src/HeadData.h index fef77c6f8f..535aa12847 100644 --- a/libraries/avatars/src/HeadData.h +++ b/libraries/avatars/src/HeadData.h @@ -34,7 +34,7 @@ class HeadData { public: explicit HeadData(AvatarData* owningAvatar); virtual ~HeadData() { }; - + // degrees float getBaseYaw() const { return _baseYaw; } void setBaseYaw(float yaw) { _baseYaw = glm::clamp(yaw, MIN_HEAD_YAW, MAX_HEAD_YAW); } @@ -42,7 +42,7 @@ public: void setBasePitch(float pitch) { _basePitch = glm::clamp(pitch, MIN_HEAD_PITCH, MAX_HEAD_PITCH); } float getBaseRoll() const { return _baseRoll; } void setBaseRoll(float roll) { _baseRoll = glm::clamp(roll, MIN_HEAD_ROLL, MAX_HEAD_ROLL); } - + virtual void setFinalYaw(float finalYaw) { _baseYaw = finalYaw; } virtual void setFinalPitch(float finalPitch) { _basePitch = finalPitch; } virtual void setFinalRoll(float finalRoll) { _baseRoll = finalRoll; } @@ -64,26 +64,23 @@ public: void setBlendshape(QString name, float val); const QVector& getBlendshapeCoefficients() const { return _blendshapeCoefficients; } void setBlendshapeCoefficients(const QVector& blendshapeCoefficients) { _blendshapeCoefficients = blendshapeCoefficients; } - - float getPupilDilation() const { return _pupilDilation; } - void setPupilDilation(float pupilDilation) { _pupilDilation = pupilDilation; } - + const glm::vec3& getLookAtPosition() const { return _lookAtPosition; } void setLookAtPosition(const glm::vec3& lookAtPosition) { _lookAtPosition = lookAtPosition; } - - + + float getLeanSideways() const { return _leanSideways; } float getLeanForward() const { return _leanForward; } float getTorsoTwist() const { return _torsoTwist; } virtual float getFinalLeanSideways() const { return _leanSideways; } virtual float getFinalLeanForward() const { return _leanForward; } - + void setLeanSideways(float leanSideways) { _leanSideways = leanSideways; } void setLeanForward(float leanForward) { _leanForward = leanForward; } void setTorsoTwist(float torsoTwist) { _torsoTwist = torsoTwist; } - + friend class AvatarData; - + QJsonObject toJson() const; void fromJson(const QJsonObject& json); @@ -106,9 +103,8 @@ protected: float _browAudioLift; float _audioAverageLoudness; QVector _blendshapeCoefficients; - float _pupilDilation; AvatarData* _owningAvatar; - + private: // privatize copy ctor and assignment operator so copies of this object cannot be made HeadData(const HeadData&); diff --git a/libraries/networking/src/LimitedNodeList.cpp b/libraries/networking/src/LimitedNodeList.cpp index 2c10d0627e..9efe51183e 100644 --- a/libraries/networking/src/LimitedNodeList.cpp +++ b/libraries/networking/src/LimitedNodeList.cpp @@ -176,9 +176,10 @@ bool LimitedNodeList::packetVersionMatch(const udt::Packet& packet) { bool hasBeenOutput = false; QString senderString; + const HifiSockAddr& senderSockAddr = packet.getSenderSockAddr(); + QUuid sourceID; if (NON_SOURCED_PACKETS.contains(headerType)) { - const HifiSockAddr& senderSockAddr = packet.getSenderSockAddr(); hasBeenOutput = versionDebugSuppressMap.contains(senderSockAddr, headerType); if (!hasBeenOutput) { @@ -186,7 +187,7 @@ bool LimitedNodeList::packetVersionMatch(const udt::Packet& packet) { senderString = QString("%1:%2").arg(senderSockAddr.getAddress().toString()).arg(senderSockAddr.getPort()); } } else { - QUuid sourceID = NLPacket::sourceIDInHeader(packet); + sourceID = NLPacket::sourceIDInHeader(packet); hasBeenOutput = sourcedVersionDebugSuppressMap.contains(sourceID, headerType); @@ -201,7 +202,7 @@ bool LimitedNodeList::packetVersionMatch(const udt::Packet& packet) { << senderString << "sent" << qPrintable(QString::number(headerVersion)) << "but" << qPrintable(QString::number(versionForPacketType(headerType))) << "expected."; - emit packetVersionMismatch(headerType); + emit packetVersionMismatch(headerType, senderSockAddr, sourceID); } return false; diff --git a/libraries/networking/src/LimitedNodeList.h b/libraries/networking/src/LimitedNodeList.h index 0cbe9668b3..2ab8aaab39 100644 --- a/libraries/networking/src/LimitedNodeList.h +++ b/libraries/networking/src/LimitedNodeList.h @@ -236,7 +236,9 @@ public slots: signals: void dataSent(quint8 channelType, int bytes); - void packetVersionMismatch(PacketType type); + + // QUuid might be zero for non-sourced packet types. + void packetVersionMismatch(PacketType type, const HifiSockAddr& senderSockAddr, const QUuid& senderUUID); void uuidChanged(const QUuid& ownerUUID, const QUuid& oldUUID); void nodeAdded(SharedNodePointer); diff --git a/libraries/networking/src/udt/PacketHeaders.cpp b/libraries/networking/src/udt/PacketHeaders.cpp index 915c2f44ba..fbfc17e56d 100644 --- a/libraries/networking/src/udt/PacketHeaders.cpp +++ b/libraries/networking/src/udt/PacketHeaders.cpp @@ -48,9 +48,11 @@ PacketVersion versionForPacketType(PacketType packetType) { case PacketType::EntityEdit: case PacketType::EntityData: return VERSION_ENTITIES_NO_FLY_ZONES; + case PacketType::AvatarIdentity: case PacketType::AvatarData: case PacketType::BulkAvatarData: - return static_cast(AvatarMixerPacketVersion::AvatarEntities); + case PacketType::KillAvatar: + return static_cast(AvatarMixerPacketVersion::AbsoluteSixByteRotations); case PacketType::ICEServerHeartbeat: return 18; // ICE Server Heartbeat signing case PacketType::AssetGetInfo: diff --git a/libraries/networking/src/udt/PacketHeaders.h b/libraries/networking/src/udt/PacketHeaders.h index 030b4af8c9..06e90a00d5 100644 --- a/libraries/networking/src/udt/PacketHeaders.h +++ b/libraries/networking/src/udt/PacketHeaders.h @@ -176,7 +176,8 @@ const PacketVersion VERSION_ENTITIES_NO_FLY_ZONES = 58; enum class AvatarMixerPacketVersion : PacketVersion { TranslationSupport = 17, SoftAttachmentSupport, - AvatarEntities + AvatarEntities, + AbsoluteSixByteRotations }; #endif // hifi_PacketHeaders_h diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index 9ef16b2daa..8011b0c4c8 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -726,10 +726,6 @@ glm::vec3 Model::calculateScaledOffsetPoint(const glm::vec3& point) const { return translatedPoint; } -bool Model::getJointState(int index, glm::quat& rotation) const { - return _rig->getJointStateRotation(index, rotation); -} - void Model::clearJointState(int index) { _rig->clearJointState(index); } diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h index 581184918d..6a7c9ec560 100644 --- a/libraries/render-utils/src/Model.h +++ b/libraries/render-utils/src/Model.h @@ -252,10 +252,6 @@ protected: /// Returns the scaled equivalent of a point in model space. glm::vec3 calculateScaledOffsetPoint(const glm::vec3& point) const; - /// Fetches the joint state at the specified index. - /// \return whether or not the joint state is "valid" (that is, non-default) - bool getJointState(int index, glm::quat& rotation) const; - /// Clear the joint states void clearJointState(int index); diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 784e9ce5af..556c313f95 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -135,6 +135,87 @@ int unpackOrientationQuatFromBytes(const unsigned char* buffer, glm::quat& quatO return sizeof(quatParts); } +#define HI_BYTE(x) (uint8_t)(x >> 8) +#define LO_BYTE(x) (uint8_t)(0xff & x) + +int packOrientationQuatToSixBytes(unsigned char* buffer, const glm::quat& quatInput) { + + // find largest component + uint8_t largestComponent = 0; + for (int i = 1; i < 4; i++) { + if (fabs(quatInput[i]) > fabs(quatInput[largestComponent])) { + largestComponent = i; + } + } + + // ensure that the sign of the dropped component is always negative. + glm::quat q = quatInput[largestComponent] > 0 ? -quatInput : quatInput; + + const float MAGNITUDE = 1.0f / sqrtf(2.0f); + const uint32_t NUM_BITS_PER_COMPONENT = 15; + const uint32_t RANGE = (1 << NUM_BITS_PER_COMPONENT) - 1; + + // quantize the smallest three components into integers + uint16_t components[3]; + for (int i = 0, j = 0; i < 4; i++) { + if (i != largestComponent) { + // transform component into 0..1 range. + float value = (q[i] + MAGNITUDE) / (2.0f * MAGNITUDE); + + // quantize 0..1 into 0..range + components[j] = (uint16_t)(value * RANGE); + j++; + } + } + + // encode the largestComponent into the high bits of the first two components + components[0] = (0x7fff & components[0]) | ((0x01 & largestComponent) << 15); + components[1] = (0x7fff & components[1]) | ((0x02 & largestComponent) << 14); + + buffer[0] = HI_BYTE(components[0]); + buffer[1] = LO_BYTE(components[0]); + buffer[2] = HI_BYTE(components[1]); + buffer[3] = LO_BYTE(components[1]); + buffer[4] = HI_BYTE(components[2]); + buffer[5] = LO_BYTE(components[2]); + + return 6; +} + +int unpackOrientationQuatFromSixBytes(const unsigned char* buffer, glm::quat& quatOutput) { + + uint16_t components[3]; + components[0] = ((uint16_t)(0x7f & buffer[0]) << 8) | buffer[1]; + components[1] = ((uint16_t)(0x7f & buffer[2]) << 8) | buffer[3]; + components[2] = ((uint16_t)(0x7f & buffer[4]) << 8) | buffer[5]; + + // largestComponent is encoded into the highest bits of the first 2 components + uint8_t largestComponent = ((0x80 & buffer[2]) >> 6) | ((0x80 & buffer[0]) >> 7); + + const uint32_t NUM_BITS_PER_COMPONENT = 15; + const float RANGE = (float)((1 << NUM_BITS_PER_COMPONENT) - 1); + const float MAGNITUDE = 1.0f / sqrtf(2.0f); + float floatComponents[3]; + for (int i = 0; i < 3; i++) { + floatComponents[i] = ((float)components[i] / RANGE) * (2.0f * MAGNITUDE) - MAGNITUDE; + } + + // missingComponent is always negative. + float missingComponent = -sqrtf(1.0f - floatComponents[0] * floatComponents[0] - floatComponents[1] * floatComponents[1] - floatComponents[2] * floatComponents[2]); + + for (int i = 0, j = 0; i < 4; i++) { + if (i != largestComponent) { + quatOutput[i] = floatComponents[j]; + j++; + } else { + quatOutput[i] = missingComponent; + } + } + + return 6; +} + + // Safe version of glm::eulerAngles; uses the factorization method described in David Eberly's // http://www.geometrictools.com/Documentation/EulerAngles.pdf (via Clyde, // https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Quaternion.java) diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 8b1446d4e5..ae9ec25195 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -97,6 +97,14 @@ int unpackFloatAngleFromTwoByte(const uint16_t* byteAnglePointer, float* destina int packOrientationQuatToBytes(unsigned char* buffer, const glm::quat& quatInput); int unpackOrientationQuatFromBytes(const unsigned char* buffer, glm::quat& quatOutput); +// alternate compression method that picks the smallest three quaternion components. +// and packs them into 15 bits each. An additional 2 bits are used to encode which component +// was omitted. Also because the components are encoded from the -1/sqrt(2) to 1/sqrt(2) which +// gives us some extra precision over the -1 to 1 range. The final result will have a maximum +// error of +- 4.3e-5 error per compoenent. +int packOrientationQuatToSixBytes(unsigned char* buffer, const glm::quat& quatInput); +int unpackOrientationQuatFromSixBytes(const unsigned char* buffer, glm::quat& quatOutput); + // Ratios need the be highly accurate when less than 10, but not very accurate above 10, and they // are never greater than 1000 to 1, this allows us to encode each component in 16bits int packFloatRatioToTwoByte(unsigned char* buffer, float ratio); diff --git a/libraries/shared/src/Packed.h b/libraries/shared/src/Packed.h new file mode 100644 index 0000000000..3300634b96 --- /dev/null +++ b/libraries/shared/src/Packed.h @@ -0,0 +1,12 @@ +#ifndef hifi_Packed_h +#define hifi_Packed_h + +#if defined(_MSC_VER) +#define PACKED_BEGIN __pragma(pack(push, 1)) +#define PACKED_END __pragma(pack(pop)); +#else +#define PACKED_BEGIN +#define PACKED_END __attribute__((__packed__)); +#endif + +#endif diff --git a/tests/shared/src/GLMHelpersTests.cpp b/tests/shared/src/GLMHelpersTests.cpp index afb634ecbd..a796d62ba5 100644 --- a/tests/shared/src/GLMHelpersTests.cpp +++ b/tests/shared/src/GLMHelpersTests.cpp @@ -54,4 +54,51 @@ void GLMHelpersTests::testEulerDecomposition() { } } +static void testQuatCompression(glm::quat testQuat) { + float MAX_COMPONENT_ERROR = 4.3e-5f; + + glm::quat q; + uint8_t bytes[6]; + packOrientationQuatToSixBytes(bytes, testQuat); + unpackOrientationQuatFromSixBytes(bytes, q); + if (glm::dot(q, testQuat) < 0.0f) { + q = -q; + } + QCOMPARE_WITH_ABS_ERROR(q.x, testQuat.x, MAX_COMPONENT_ERROR); + QCOMPARE_WITH_ABS_ERROR(q.y, testQuat.y, MAX_COMPONENT_ERROR); + QCOMPARE_WITH_ABS_ERROR(q.z, testQuat.z, MAX_COMPONENT_ERROR); + QCOMPARE_WITH_ABS_ERROR(q.w, testQuat.w, MAX_COMPONENT_ERROR); +} + +void GLMHelpersTests::testSixByteOrientationCompression() { + const glm::quat ROT_X_90 = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f)); + const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + + testQuatCompression(ROT_X_90); + testQuatCompression(ROT_Y_180); + testQuatCompression(ROT_Z_30); + testQuatCompression(ROT_X_90 * ROT_Y_180); + testQuatCompression(ROT_Y_180 * ROT_X_90); + testQuatCompression(ROT_Z_30 * ROT_X_90); + testQuatCompression(ROT_X_90 * ROT_Z_30); + testQuatCompression(ROT_Z_30 * ROT_Y_180); + testQuatCompression(ROT_Y_180 * ROT_Z_30); + testQuatCompression(ROT_X_90 * ROT_Y_180 * ROT_Z_30); + testQuatCompression(ROT_Y_180 * ROT_Z_30 * ROT_X_90); + testQuatCompression(ROT_Z_30 * ROT_X_90 * ROT_Y_180); + + testQuatCompression(-ROT_X_90); + testQuatCompression(-ROT_Y_180); + testQuatCompression(-ROT_Z_30); + testQuatCompression(-(ROT_X_90 * ROT_Y_180)); + testQuatCompression(-(ROT_Y_180 * ROT_X_90)); + testQuatCompression(-(ROT_Z_30 * ROT_X_90)); + testQuatCompression(-(ROT_X_90 * ROT_Z_30)); + testQuatCompression(-(ROT_Z_30 * ROT_Y_180)); + testQuatCompression(-(ROT_Y_180 * ROT_Z_30)); + testQuatCompression(-(ROT_X_90 * ROT_Y_180 * ROT_Z_30)); + testQuatCompression(-(ROT_Y_180 * ROT_Z_30 * ROT_X_90)); + testQuatCompression(-(ROT_Z_30 * ROT_X_90 * ROT_Y_180)); +} diff --git a/tests/shared/src/GLMHelpersTests.h b/tests/shared/src/GLMHelpersTests.h index 5e880899e8..40d552a07b 100644 --- a/tests/shared/src/GLMHelpersTests.h +++ b/tests/shared/src/GLMHelpersTests.h @@ -19,6 +19,7 @@ class GLMHelpersTests : public QObject { Q_OBJECT private slots: void testEulerDecomposition(); + void testSixByteOrientationCompression(); }; float getErrorDifference(const float& a, const float& b);