Send avatar skeleton trait

assignment-client/src/avatars/AvatarMixerClientData.cpp

@@ -179,7 +179,13 @@ void AvatarMixerClientData::processSetTraitsMessage(ReceivedMessage& message,
             if (packetTraitVersion > _lastReceivedTraitVersions[traitType]) {
                 _avatar->processTrait(traitType, message.read(traitSize));
                 _lastReceivedTraitVersions[traitType] = packetTraitVersion;
-
+                if (traitType == AvatarTraits::SkeletonData) {
+                    qDebug() << "Sending skeleton avatar trait";
+                    auto packet = NLPacket::create(PacketType::SetAvatarTraits, -1, true);
+                    AvatarTraits::packVersionedTrait(AvatarTraits::SkeletonData, *packet, packetTraitVersion, *_avatar);
+                    auto nodeList = DependencyManager::get<NodeList>();
+                    nodeList->sendPacket(std::move(packet), sendingNode);
+                }
                 if (traitType == AvatarTraits::SkeletonModelURL) {
                     // special handling for skeleton model URL, since we need to make sure it is in the whitelist
                     checkSkeletonURLAgainstWhitelist(slaveSharedData, sendingNode, packetTraitVersion);

libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp

@@ -1449,6 +1449,28 @@ QStringList Avatar::getJointNames() const {
     return result;
 }
 
+std::vector<AvatarSkeletonTrait::UnpackedJointData> Avatar::getSkeletonDefaultData() {
+    std::vector<AvatarSkeletonTrait::UnpackedJointData> defaultSkeletonData;
+    if (_skeletonModel->isLoaded()) {
+        auto jointNames = getJointNames();
+        int sizeCount = 0;
+        for (int i = 0; i < min(43, jointNames.size()); i++) {
+            AvatarSkeletonTrait::UnpackedJointData jointData;
+            jointData.jointParent = _skeletonModel->getRig().getJointParentIndex(i);
+            jointData.jointIndex = i;
+            jointData.defaultRotation = getDefaultJointRotation(i);
+            jointData.defaultTranslation = getDefaultJointTranslation(i);
+            jointData.defaultScale = glm::length(getAbsoluteJointScaleInObjectFrame(i));
+            jointData.jointName = jointNames[i];
+            jointData.stringLength = jointNames[i].size();
+            jointData.stringStart = sizeCount;
+            sizeCount += jointNames[i].size();
+            defaultSkeletonData.push_back(jointData);
+        }
+    }
+    return defaultSkeletonData;
+}
+
 glm::vec3 Avatar::getJointPosition(int index) const {
     glm::vec3 position;
     _skeletonModel->getJointPositionInWorldFrame(index, position);
@@ -1515,6 +1537,8 @@ void Avatar::rigReady() {
     buildSpine2SplineRatioCache();
     computeMultiSphereShapes();
     buildSpine2SplineRatioCache();
+    setSkeletonData(getSkeletonDefaultData());
+    sendSkeletonData();
 }
 
 // rig has been reset.

libraries/avatars-renderer/src/avatars-renderer/Avatar.h

@@ -199,6 +199,8 @@ public:
     virtual int getJointIndex(const QString& name) const override;
     virtual QStringList getJointNames() const override;
 
+    std::vector<AvatarSkeletonTrait::UnpackedJointData> getSkeletonDefaultData();
+
     /**jsdoc
      * Gets the default rotation of a joint (in the current avatar) relative to its parent.
      * <p>For information on the joint hierarchy used, see

libraries/avatars/src/AvatarData.cpp

@@ -1990,11 +1990,98 @@ QUrl AvatarData::getWireSafeSkeletonModelURL() const {
         return QUrl();
     }
 }
+QByteArray AvatarData::packSkeletonData() const {
+    int avatarDataSize = 0;
+    QByteArray avatarDataByteArray;
+    //_avatarSkeletonDataLock.withReadLock([&] {
+        // Add header
+        AvatarSkeletonTrait::Header header;
+        header.maxScaleDimension = 0.0f;
+        header.maxTranslationDimension = 0.0f;
+        header.numJoints = (uint8_t)_avatarSkeletonData.size();
+        header.padding = 0;
+        header.stringTableLength = 0;
+        qDebug() << "Dealing with: " << _avatarSkeletonData.size() << " joints";
+        for (size_t i = 0; i < _avatarSkeletonData.size(); i++) {
+            header.stringTableLength += (uint16_t)_avatarSkeletonData[i].jointName.size();
+            auto &translation = _avatarSkeletonData[i].defaultTranslation;
+            header.maxTranslationDimension = std::max(header.maxTranslationDimension, std::max(std::max(translation.x, translation.y), translation.z));
+            header.maxScaleDimension = std::max(header.maxScaleDimension, _avatarSkeletonData[i].defaultScale);
+        }
+
+        const int byteArraySize = (int)sizeof(AvatarSkeletonTrait::Header) + (int)(header.numJoints * sizeof(AvatarSkeletonTrait::JointData)) + header.stringTableLength;
+        avatarDataByteArray = QByteArray(byteArraySize, 0);
+        unsigned char* destinationBuffer = reinterpret_cast<unsigned char*>(avatarDataByteArray.data());
+        const unsigned char* const startPosition = destinationBuffer;
+
+        memcpy(destinationBuffer, &header, sizeof(header));
+        destinationBuffer += sizeof(AvatarSkeletonTrait::Header);
+
+        QString stringTable = "";
+        for (size_t i = 0; i < _avatarSkeletonData.size(); i++) {
+            AvatarSkeletonTrait::JointData jdata;
+            jdata.boneType = _avatarSkeletonData[i].boneType;
+            jdata.jointParent = _avatarSkeletonData[i].jointParent;
+            packFloatRatioToTwoByte((uint8_t*)(&jdata.defaultScale), _avatarSkeletonData[i].defaultScale / header.maxScaleDimension);
+            packOrientationQuatToSixBytes(jdata.defaultRotation, _avatarSkeletonData[i].defaultRotation);
+            packFloatVec3ToSignedTwoByteFixed(jdata.defaultTranslation, _avatarSkeletonData[i].defaultTranslation / header.maxTranslationDimension, TRANSLATION_COMPRESSION_RADIX);
+            jdata.jointIndex = (uint16_t)i;
+            jdata.stringStart = (uint16_t)_avatarSkeletonData[i].stringStart;
+            jdata.stringLength = (uint8_t)_avatarSkeletonData[i].stringLength;
+            stringTable += _avatarSkeletonData[i].jointName;
+            memcpy(destinationBuffer, &jdata, sizeof(AvatarSkeletonTrait::JointData));
+            destinationBuffer += sizeof(AvatarSkeletonTrait::JointData);
+        }
+
+        memcpy(destinationBuffer, stringTable.toUtf8(), header.stringTableLength);
+        destinationBuffer += header.stringTableLength;
+
+        avatarDataSize = destinationBuffer - startPosition;
+        qDebug() << "Data size: " << avatarDataSize;
+    //});
+    return avatarDataByteArray.left(avatarDataSize);
+}
 
 QByteArray AvatarData::packSkeletonModelURL() const {
     return getWireSafeSkeletonModelURL().toEncoded();
 }
 
+void AvatarData::unpackSkeletonData(const QByteArray& data) {
+
+    const unsigned char* startPosition = reinterpret_cast<const unsigned char*>(data.data());
+    const unsigned char* endPosition = startPosition + data.size();
+    const unsigned char* sourceBuffer = startPosition;
+    
+    auto header = reinterpret_cast<const AvatarSkeletonTrait::Header*>(sourceBuffer);
+    sourceBuffer += sizeof(const AvatarSkeletonTrait::Header);
+
+    std::vector<AvatarSkeletonTrait::UnpackedJointData> joints;
+    for (uint8_t i = 0; i < header->numJoints; i++) {
+        auto jointData = reinterpret_cast<const AvatarSkeletonTrait::JointData*>(sourceBuffer);
+        sourceBuffer += sizeof(const AvatarSkeletonTrait::JointData);
+        AvatarSkeletonTrait::UnpackedJointData uJointData;
+        uJointData.boneType = (int)jointData->boneType;
+        uJointData.jointIndex = (int)i;
+        uJointData.stringLength = (int)jointData->stringLength;
+        uJointData.stringStart = (int)jointData->stringStart;
+        unpackOrientationQuatFromSixBytes(reinterpret_cast<const unsigned char*>(&jointData->defaultRotation), uJointData.defaultRotation);
+        unpackFloatVec3FromSignedTwoByteFixed(reinterpret_cast<const unsigned char*>(&jointData->defaultTranslation), uJointData.defaultTranslation, TRANSLATION_COMPRESSION_RADIX);
+        unpackFloatScalarFromSignedTwoByteFixed((const int16_t*)(&jointData->defaultScale), &uJointData.defaultScale, TRANSLATION_COMPRESSION_RADIX);
+        uJointData.defaultTranslation *= header->maxTranslationDimension;
+        uJointData.defaultScale *= header->maxScaleDimension;
+        joints.push_back(uJointData);
+    }
+    qDebug() << "_____length: " << header->stringTableLength;
+    QString table = QString::fromUtf8(reinterpret_cast<const char*>(sourceBuffer), (int)header->stringTableLength);
+    for (size_t i = 0; i < joints.size(); i++) {
+        QStringRef subString(&table, joints[i].stringStart, joints[i].stringLength);
+        joints[i].jointName = subString.toString();
+        qDebug() << "_____data: " << joints[i].boneType << " " << joints[i].jointIndex << " " << joints[i].stringLength << " " << joints[i].stringStart << " " << joints[i].defaultRotation << " " << joints[i].defaultScale << joints[i].defaultTranslation;
+        qDebug() << "_____JointNameReveived: " << joints[i].jointName;
+    }
+    setSkeletonData(joints);
+}
+
 void AvatarData::unpackSkeletonModelURL(const QByteArray& data) {
     auto skeletonModelURL = QUrl::fromEncoded(data);
     setSkeletonModelURL(skeletonModelURL);
@@ -2030,6 +2117,8 @@ QByteArray AvatarData::packTrait(AvatarTraits::TraitType traitType) const {
     // Call packer function
     if (traitType == AvatarTraits::SkeletonModelURL) {
         traitBinaryData = packSkeletonModelURL();
+    } else if (traitType == AvatarTraits::SkeletonData) {
+        traitBinaryData = packSkeletonData();
     }
 
     return traitBinaryData;
@@ -2051,6 +2140,8 @@ QByteArray AvatarData::packTraitInstance(AvatarTraits::TraitType traitType, Avat
 void AvatarData::processTrait(AvatarTraits::TraitType traitType, QByteArray traitBinaryData) {
     if (traitType == AvatarTraits::SkeletonModelURL) {
         unpackSkeletonModelURL(traitBinaryData);
+    } else if (traitType == AvatarTraits::SkeletonData) {
+        unpackSkeletonData(traitBinaryData);
     }
 }
 
@@ -2997,6 +3088,18 @@ AABox AvatarData::computeBubbleBox(float bubbleScale) const {
     return box;
 }
 
+void AvatarData::setSkeletonData(const std::vector<AvatarSkeletonTrait::UnpackedJointData>& skeletonData) {
+    _avatarSkeletonDataLock.withWriteLock([&] {
+        _avatarSkeletonData = skeletonData;
+    });
+}
+
+void AvatarData::sendSkeletonData() const{
+    if (_clientTraitsHandler) {
+        _clientTraitsHandler->markTraitUpdated(AvatarTraits::SkeletonData);
+    }
+}
+
 AABox AvatarData::getDefaultBubbleBox() const {
     AABox bubbleBox(_defaultBubbleBox);
     bubbleBox.translate(_globalPosition);

libraries/avatars/src/AvatarData.h

@@ -145,6 +145,39 @@ const char AVATARDATA_FLAGS_MINIMUM = 0;
 
 using SmallFloat = uint16_t; // a compressed float with less precision, user defined radix
 
+namespace AvatarSkeletonTrait {
+    PACKED_BEGIN struct Header {
+        float maxTranslationDimension;
+        float maxScaleDimension;
+        uint8_t numJoints;
+        uint8_t padding;
+        uint16_t stringTableLength;
+    } PACKED_END;
+
+    PACKED_BEGIN struct JointData {
+        uint16_t stringStart;
+        uint8_t stringLength;
+        uint8_t boneType;
+        uint8_t defaultTranslation[6];
+        uint8_t defaultRotation[6];
+        uint16_t defaultScale;
+        uint16_t jointIndex;
+        uint16_t jointParent;
+    } PACKED_END;
+
+    struct UnpackedJointData {
+        int jointParent;
+        int stringStart;
+        int stringLength;
+        int boneType;
+        glm::vec3 defaultTranslation;
+        glm::quat defaultRotation;
+        float defaultScale;
+        int jointIndex;
+        QString jointName;
+    };
+}
+
 namespace AvatarDataPacket {
 
     // NOTE: every time AvatarData is sent from mixer to client, it also includes the GUIID for the session
@@ -258,6 +291,7 @@ namespace AvatarDataPacket {
     PACKED_BEGIN struct AvatarLocalPosition {
         float localPosition[3];           // parent frame translation of the avatar
     } PACKED_END;
+
     const size_t AVATAR_LOCAL_POSITION_SIZE = 12;
     static_assert(sizeof(AvatarLocalPosition) == AVATAR_LOCAL_POSITION_SIZE, "AvatarDataPacket::AvatarLocalPosition size doesn't match.");
 
@@ -1419,6 +1453,8 @@ public:
     void setIsNewAvatar(bool isNewAvatar) { _isNewAvatar = isNewAvatar; }
     bool getIsNewAvatar() { return _isNewAvatar; }
     void setIsClientAvatar(bool isClientAvatar) { _isClientAvatar = isClientAvatar; }
+    void setSkeletonData(const std::vector<AvatarSkeletonTrait::UnpackedJointData>& skeletonData);
+    void sendSkeletonData() const;
 
 signals:
 
@@ -1597,12 +1633,13 @@ protected:
     bool hasParent() const { return !getParentID().isNull(); }
     bool hasFaceTracker() const { return _headData ? _headData->_isFaceTrackerConnected : false; }
 
+    QByteArray packSkeletonData() const;
     QByteArray packSkeletonModelURL() const;
     QByteArray packAvatarEntityTraitInstance(AvatarTraits::TraitInstanceID traitInstanceID);
     QByteArray packGrabTraitInstance(AvatarTraits::TraitInstanceID traitInstanceID);
 
     void unpackSkeletonModelURL(const QByteArray& data);
-
+    void unpackSkeletonData(const QByteArray& data);
     
     // isReplicated will be true on downstream Avatar Mixers and their clients, but false on the upstream "master"
     // Audio Mixer that the replicated avatar is connected to.
@@ -1719,6 +1756,9 @@ protected:
     AvatarGrabDataMap _avatarGrabData;
     bool _avatarGrabDataChanged { false }; // by network
 
+    mutable ReadWriteLockable _avatarSkeletonDataLock;
+    std::vector<AvatarSkeletonTrait::UnpackedJointData> _avatarSkeletonData;
+
     // used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers.
     ThreadSafeValueCache<glm::mat4> _sensorToWorldMatrixCache { glm::mat4() };
     ThreadSafeValueCache<glm::mat4> _controllerLeftHandMatrixCache { glm::mat4() };

libraries/avatars/src/AvatarTraits.h

@@ -29,7 +29,7 @@ namespace AvatarTraits {
 
         // Simple traits
         SkeletonModelURL = 0,
-
+        SkeletonData,
         // Instanced traits
         FirstInstancedTrait,
         AvatarEntity = FirstInstancedTrait,