Merge pull request #2231 from ey6es/joints

Send joint rotations in avatar packets, allow scripts to get/set them.

examples/crazylegs.js

@@ -0,0 +1,23 @@
+//
+//  crazylegs.js
+//  hifi
+//
+//  Created by Andrzej Kapolka on 3/6/14.
+//  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
+//
+
+var FREQUENCY = 5.0;
+
+var AMPLITUDE = 45.0;
+
+var cumulativeTime = 0.0;
+
+Script.update.connect(function(deltaTime) {
+    cumulativeTime += deltaTime;
+    MyAvatar.setJointData("joint_R_hip", Quat.fromPitchYawRoll(0.0, 0.0, AMPLITUDE * Math.sin(cumulativeTime * FREQUENCY)));
+    MyAvatar.setJointData("joint_L_hip", Quat.fromPitchYawRoll(0.0, 0.0, -AMPLITUDE * Math.sin(cumulativeTime * FREQUENCY)));
+    MyAvatar.setJointData("joint_R_knee", Quat.fromPitchYawRoll(0.0, 0.0,
+        AMPLITUDE * (1.0 + Math.sin(cumulativeTime * FREQUENCY))));
+    MyAvatar.setJointData("joint_L_knee", Quat.fromPitchYawRoll(0.0, 0.0,
+        AMPLITUDE * (1.0 - Math.sin(cumulativeTime * FREQUENCY))));
+});

interface/src/Application.cpp

@@ -121,6 +121,8 @@ void messageHandler(QtMsgType type, const QMessageLogContext& context, const QSt
 QString& Application::resourcesPath() {
 #ifdef Q_OS_MAC
     static QString staticResourcePath = QCoreApplication::applicationDirPath() + "/../Resources/";
+#elif defined Q_OS_LINUX
+    static QString staticResourcePath = "resources/";
 #else
     static QString staticResourcePath = QCoreApplication::applicationDirPath() + "/resources/";
 #endif

interface/src/avatar/Avatar.cpp

@@ -136,6 +136,12 @@ void Avatar::simulate(float deltaTime) {
     
     getHand()->simulate(deltaTime, false);
     _skeletonModel.setLODDistance(getLODDistance());
+    
+    // copy joint data to skeleton
+    for (int i = 0; i < _jointData.size(); i++) {
+        const JointData& data = _jointData.at(i);
+        _skeletonModel.setJointState(i, data.valid, data.rotation);
+    }
     glm::vec3 headPosition = _position;
     if (!_shouldRenderBillboard) {
         _skeletonModel.simulate(deltaTime);
@@ -575,6 +581,35 @@ bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float parti
     return collided;
 }
 
+glm::quat Avatar::getJointRotation(int index) const {
+    if (QThread::currentThread() != thread()) {
+        return AvatarData::getJointRotation(index);
+    }
+    glm::quat rotation;
+    _skeletonModel.getJointState(index, rotation);
+    return rotation;
+}
+
+int Avatar::getJointIndex(const QString& name) const {
+    if (QThread::currentThread() != thread()) {
+        int result;
+        QMetaObject::invokeMethod(const_cast<Avatar*>(this), "getJointIndex", Qt::BlockingQueuedConnection,
+            Q_RETURN_ARG(int, result), Q_ARG(const QString&, name));
+        return result;
+    }
+    return _skeletonModel.isActive() ? _skeletonModel.getGeometry()->getFBXGeometry().getJointIndex(name) : -1;
+}
+
+QStringList Avatar::getJointNames() const {
+    if (QThread::currentThread() != thread()) {
+        QStringList result;
+        QMetaObject::invokeMethod(const_cast<Avatar*>(this), "getJointNames", Qt::BlockingQueuedConnection,
+            Q_RETURN_ARG(QStringList, result));
+        return result;
+    }
+    return _skeletonModel.isActive() ? _skeletonModel.getGeometry()->getFBXGeometry().getJointNames() : QStringList();
+}
+
 void Avatar::setFaceModelURL(const QUrl& faceModelURL) {
     AvatarData::setFaceModelURL(faceModelURL);
     const QUrl DEFAULT_FACE_MODEL_URL = QUrl::fromLocalFile(Application::resourcesPath() + "meshes/defaultAvatar_head.fst");

interface/src/avatar/Avatar.h

@@ -122,6 +122,10 @@ public:
 
     virtual bool isMyAvatar() { return false; }
     
+    virtual glm::quat getJointRotation(int index) const;
+    virtual int getJointIndex(const QString& name) const;
+    virtual QStringList getJointNames() const;
+    
     virtual void setFaceModelURL(const QUrl& faceModelURL);
     virtual void setSkeletonModelURL(const QUrl& skeletonModelURL);
     virtual void setDisplayName(const QString& displayName);

interface/src/avatar/MyAvatar.cpp

@@ -306,6 +306,13 @@ void MyAvatar::simulate(float deltaTime) {
 
     _skeletonModel.simulate(deltaTime);
 
+    // copy out the skeleton joints from the model
+    _jointData.resize(_skeletonModel.getJointStateCount());
+    for (int i = 0; i < _jointData.size(); i++) {
+        JointData& data = _jointData[i];
+        data.valid = _skeletonModel.getJointState(i, data.rotation);
+    }
+
     Head* head = getHead();
     glm::vec3 headPosition;
     if (!_skeletonModel.getHeadPosition(headPosition)) {
@@ -665,6 +672,20 @@ glm::vec3 MyAvatar::getUprightHeadPosition() const {
     return _position + getWorldAlignedOrientation() * glm::vec3(0.0f, getPelvisToHeadLength(), 0.0f);
 }
 
+void MyAvatar::setJointData(int index, const glm::quat& rotation) {
+    Avatar::setJointData(index, rotation);
+    if (QThread::currentThread() == thread()) {
+        _skeletonModel.setJointState(index, true, rotation);
+    }
+}
+
+void MyAvatar::clearJointData(int index) {
+    Avatar::clearJointData(index);
+    if (QThread::currentThread() == thread()) {
+        _skeletonModel.setJointState(index, false);
+    }
+}
+
 void MyAvatar::setFaceModelURL(const QUrl& faceModelURL) {
     Avatar::setFaceModelURL(faceModelURL);
     _billboardValid = false;

interface/src/avatar/MyAvatar.h

@@ -81,6 +81,8 @@ public:
     void updateLookAtTargetAvatar();
     void clearLookAtTargetAvatar();
     
+    virtual void setJointData(int index, const glm::quat& rotation);
+    virtual void clearJointData(int index);
     virtual void setFaceModelURL(const QUrl& faceModelURL);
     virtual void setSkeletonModelURL(const QUrl& skeletonModelURL);
 public slots:

interface/src/avatar/SkeletonModel.cpp

@@ -30,6 +30,10 @@ void SkeletonModel::simulate(float deltaTime, bool delayLoad) {
 
     Model::simulate(deltaTime, delayLoad);
 
+    if (!_owningAvatar->isMyAvatar()) {
+        return; // only simulate for own avatar
+    }
+
     // find the left and rightmost active Leap palms
     int leftPalmIndex, rightPalmIndex;
     Hand* hand = _owningAvatar->getHand();
@@ -188,6 +192,9 @@ void SkeletonModel::updateJointState(int index) {
 }
 
 void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
+    if (!_owningAvatar->isMyAvatar()) {
+        return;
+    }
     // get the rotation axes in joint space and use them to adjust the rotation
     glm::mat3 axes = glm::mat3_cast(_rotation);
     glm::mat3 inverse = glm::mat3(glm::inverse(parentState.transform * glm::translate(state.translation) * 

interface/src/renderer/FBXReader.cpp

@@ -46,6 +46,14 @@ void Extents::addPoint(const glm::vec3& point) {
     maximum = glm::max(maximum, point);
 }
 
+QStringList FBXGeometry::getJointNames() const {
+    QStringList names;
+    foreach (const FBXJoint& joint, joints) {
+        names.append(joint.name);
+    }
+    return names;
+}
+
 static int fbxGeometryMetaTypeId = qRegisterMetaType<FBXGeometry>();
 
 template<class T> QVariant readBinaryArray(QDataStream& in) {

interface/src/renderer/FBXReader.h

@@ -81,7 +81,7 @@ public:
     glm::quat inverseDefaultRotation;
     glm::quat inverseBindRotation;
     glm::mat4 bindTransform;
-    QString name;             // temp field for debugging
+    QString name;
     glm::vec3 shapePosition;  // in joint frame
     glm::quat shapeRotation;  // in joint frame
     int shapeType;
@@ -152,7 +152,7 @@ class FBXGeometry {
 public:
 
     QVector<FBXJoint> joints;
-    QHash<QString, int> jointIndices;
+    QHash<QString, int> jointIndices; ///< 1-based, so as to more easily detect missing indices
     
     QVector<FBXMesh> meshes;
     
@@ -181,6 +181,9 @@ public:
     Extents staticExtents;
     
     QVector<FBXAttachment> attachments;
+    
+    int getJointIndex(const QString& name) const { return jointIndices.value(name) - 1; }
+    QStringList getJointNames() const;
 };
 
 Q_DECLARE_METATYPE(FBXGeometry)

interface/src/renderer/Model.cpp

@@ -378,6 +378,24 @@ Extents Model::getStaticExtents() const {
     return scaledExtents;
 }
 
+bool Model::getJointState(int index, glm::quat& rotation) const {
+    if (index == -1 || index >= _jointStates.size()) {
+        return false;
+    }
+    rotation = _jointStates.at(index).rotation;
+    const glm::quat& defaultRotation = _geometry->getFBXGeometry().joints.at(index).rotation;
+    return glm::abs(rotation.x - defaultRotation.x) >= EPSILON ||
+        glm::abs(rotation.y - defaultRotation.y) >= EPSILON ||
+        glm::abs(rotation.z - defaultRotation.z) >= EPSILON ||
+        glm::abs(rotation.w - defaultRotation.w) >= EPSILON;
+}
+
+void Model::setJointState(int index, bool valid, const glm::quat& rotation) {
+    if (index != -1 && index < _jointStates.size()) {
+        _jointStates[index].rotation = valid ? rotation : _geometry->getFBXGeometry().joints.at(index).rotation;
+    }
+}
+
 int Model::getParentJointIndex(int jointIndex) const {
     return (isActive() && jointIndex != -1) ? _geometry->getFBXGeometry().joints.at(jointIndex).parentIndex : -1;
 }
@@ -878,9 +896,10 @@ QVector<Model::JointState> Model::updateGeometry(bool delayLoad) {
             newJointStates = createJointStates(newGeometry);
             for (QHash<QString, int>::const_iterator it = oldGeometry.jointIndices.constBegin();
                     it != oldGeometry.jointIndices.constEnd(); it++) {
-                int newIndex = newGeometry.jointIndices.value(it.key());
-                if (newIndex != 0) {
-                    newJointStates[newIndex - 1] = _jointStates.at(it.value() - 1);
+                int oldIndex = it.value() - 1;
+                int newIndex = newGeometry.getJointIndex(it.key());
+                if (newIndex != -1) {
+                    newJointStates[newIndex] = _jointStates.at(oldIndex);
                 }
             }
         }

interface/src/renderer/Model.h

@@ -81,6 +81,16 @@ public:
     /// Returns a reference to the shared geometry.
     const QSharedPointer<NetworkGeometry>& getGeometry() const { return _geometry; }
     
+    /// Returns the number of joint states in the model.
+    int getJointStateCount() const { return _jointStates.size(); }
+    
+    /// 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;
+    
+    /// Sets the joint state at the specified index.
+    void setJointState(int index, bool valid, const glm::quat& rotation = glm::quat());
+    
     /// Returns the index of the left hand joint, or -1 if not found.
     int getLeftHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().leftHandJointIndex : -1; }
     

libraries/avatars/src/AvatarData.cpp

@@ -11,6 +11,7 @@
 #include <stdint.h>
 
 #include <QtCore/QDataStream>
+#include <QtCore/QThread>
 #include <QtNetwork/QNetworkAccessManager>
 #include <QtNetwork/QNetworkReply>
 #include <QtNetwork/QNetworkRequest>
@@ -156,10 +157,32 @@ QByteArray AvatarData::toByteArray() {
     
     // pupil dilation
     destinationBuffer += packFloatToByte(destinationBuffer, _headData->_pupilDilation, 1.0f);
-    
+
+    // joint data
+    *destinationBuffer++ = _jointData.size();
+    unsigned char validity = 0;
+    int validityBit = 0;
+    foreach (const JointData& data, _jointData) {
+        if (data.valid) {
+            validity |= (1 << validityBit);
+        }
+        if (++validityBit == BITS_IN_BYTE) {
+            *destinationBuffer++ = validity;
+            validityBit = validity = 0;
+        }
+    }
+    if (validityBit != 0) {
+        *destinationBuffer++ = validity;
+    }
+    foreach (const JointData& data, _jointData) {
+        if (data.valid) {
+            destinationBuffer += packOrientationQuatToBytes(destinationBuffer, data.rotation);
+        }
+    }
+        
     // hand data
     destinationBuffer += HandData::encodeData(_handData, destinationBuffer);
-
+    
     return avatarDataByteArray.left(destinationBuffer - startPosition);
 }
 
@@ -264,6 +287,25 @@ int AvatarData::parseData(const QByteArray& packet) {
     // pupil dilation
     sourceBuffer += unpackFloatFromByte(sourceBuffer, _headData->_pupilDilation, 1.0f);
     
+    // joint data
+    int jointCount = *sourceBuffer++;
+    _jointData.resize(jointCount);
+    unsigned char validity;
+    int validityBit = 0;
+    for (int i = 0; i < jointCount; i++) {
+        if (validityBit == 0) {
+            validity = *sourceBuffer++;
+        }   
+        _jointData[i].valid = validity & (1 << validityBit);
+        validityBit = (validityBit + 1) % BITS_IN_BYTE; 
+    }
+    for (int i = 0; i < jointCount; i++) {
+        JointData& data = _jointData[i];
+        if (data.valid) {
+            sourceBuffer += unpackOrientationQuatFromBytes(sourceBuffer, data.rotation);
+        }
+    }
+    
     // hand data
     if (sourceBuffer - startPosition < packet.size()) {
         // check passed, bytes match
@@ -273,6 +315,99 @@ int AvatarData::parseData(const QByteArray& packet) {
     return sourceBuffer - startPosition;
 }
 
+void AvatarData::setJointData(int index, const glm::quat& rotation) {
+    if (index == -1) {
+        return;
+    }
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(this, "setJointData", Q_ARG(int, index), Q_ARG(const glm::quat&, rotation));
+        return;
+    }
+    if (_jointData.size() <= index) {
+        _jointData.resize(index + 1);
+    }
+    JointData& data = _jointData[index];
+    data.valid = true;
+    data.rotation = rotation;
+}
+
+void AvatarData::clearJointData(int index) {
+    if (index == -1) {
+        return;
+    }
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(this, "clearJointData", Q_ARG(int, index));
+        return;
+    }
+    if (_jointData.size() <= index) {
+        _jointData.resize(index + 1);
+    }
+    _jointData[index].valid = false;
+}
+
+bool AvatarData::isJointDataValid(int index) const {
+    if (index == -1) {
+        return false;
+    }
+    if (QThread::currentThread() != thread()) {
+        bool result;
+        QMetaObject::invokeMethod(const_cast<AvatarData*>(this), "isJointDataValid", Qt::BlockingQueuedConnection,
+            Q_RETURN_ARG(bool, result), Q_ARG(int, index));
+        return result;
+    }
+    return index < _jointData.size() && _jointData.at(index).valid;
+}
+
+glm::quat AvatarData::getJointRotation(int index) const {
+    if (index == -1) {
+        return glm::quat();
+    }
+    if (QThread::currentThread() != thread()) {
+        glm::quat result;
+        QMetaObject::invokeMethod(const_cast<AvatarData*>(this), "getJointRotation", Qt::BlockingQueuedConnection,
+            Q_RETURN_ARG(glm::quat, result), Q_ARG(int, index));
+        return result;
+    }
+    return index < _jointData.size() ? _jointData.at(index).rotation : glm::quat();
+}
+
+void AvatarData::setJointData(const QString& name, const glm::quat& rotation) {
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(this, "setJointData", Q_ARG(const QString&, name),
+            Q_ARG(const glm::quat&, rotation));
+        return;
+    }
+    setJointData(getJointIndex(name), rotation);
+}
+
+void AvatarData::clearJointData(const QString& name) {
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(this, "clearJointData", Q_ARG(const QString&, name));
+        return;
+    }
+    clearJointData(getJointIndex(name));
+}
+
+bool AvatarData::isJointDataValid(const QString& name) const {
+    if (QThread::currentThread() != thread()) {
+        bool result;
+        QMetaObject::invokeMethod(const_cast<AvatarData*>(this), "isJointDataValid", Qt::BlockingQueuedConnection,
+            Q_RETURN_ARG(bool, result), Q_ARG(const QString&, name));
+        return result;
+    }
+    return isJointDataValid(getJointIndex(name));
+}
+
+glm::quat AvatarData::getJointRotation(const QString& name) const {
+    if (QThread::currentThread() != thread()) {
+        glm::quat result;
+        QMetaObject::invokeMethod(const_cast<AvatarData*>(this), "getJointRotation", Qt::BlockingQueuedConnection,
+            Q_RETURN_ARG(glm::quat, result), Q_ARG(const QString&, name));
+        return result;
+    }
+    return getJointRotation(getJointIndex(name));
+}
+
 bool AvatarData::hasIdentityChangedAfterParsing(const QByteArray &packet) {
     QDataStream packetStream(packet);
     packetStream.skipRawData(numBytesForPacketHeader(packet));

libraries/avatars/src/AvatarData.h

@@ -31,8 +31,10 @@ typedef unsigned long long quint64;
 
 #include <QtCore/QByteArray>
 #include <QtCore/QObject>
+#include <QtCore/QStringList>
 #include <QtCore/QUrl>
 #include <QtCore/QUuid>
+#include <QtCore/QVector>
 #include <QtCore/QVariantMap>
 #include <QRect>
 
@@ -70,6 +72,8 @@ const glm::vec3 vec3Zero(0.0f);
 
 class QNetworkAccessManager;
 
+class JointData;
+
 class AvatarData : public NodeData {
     Q_OBJECT
 
@@ -137,6 +141,24 @@ public:
     void setHandState(char s) { _handState = s; }
     char getHandState() const { return _handState; }
 
+    const QVector<JointData>& getJointData() const { return _jointData; }
+    void setJointData(const QVector<JointData>& jointData) { _jointData = jointData; }
+
+    Q_INVOKABLE virtual void setJointData(int index, const glm::quat& rotation);
+    Q_INVOKABLE virtual void clearJointData(int index);
+    Q_INVOKABLE bool isJointDataValid(int index) const;
+    Q_INVOKABLE virtual glm::quat getJointRotation(int index) const;
+
+    Q_INVOKABLE void setJointData(const QString& name, const glm::quat& rotation);
+    Q_INVOKABLE void clearJointData(const QString& name);
+    Q_INVOKABLE bool isJointDataValid(const QString& name) const;
+    Q_INVOKABLE glm::quat getJointRotation(const QString& name) const;
+
+    /// Returns the index of the joint with the specified name, or -1 if not found/unknown.
+    Q_INVOKABLE virtual int getJointIndex(const QString& name) const { return -1; } 
+
+    Q_INVOKABLE virtual QStringList getJointNames() const { return QStringList(); }
+
     // key state
     void setKeyState(KeyState s) { _keyState = s; }
     KeyState keyState() const { return _keyState; }
@@ -205,6 +227,8 @@ protected:
     //  Hand state (are we grabbing something or not)
     char _handState;
 
+    QVector<JointData> _jointData; ///< the state of the skeleton joints
+
     // key state
     KeyState _keyState;
 
@@ -235,4 +259,10 @@ private:
     AvatarData& operator= (const AvatarData&);
 };
 
+class JointData {
+public:
+    bool valid;
+    glm::quat rotation;
+};
+
 #endif /* defined(__hifi__AvatarData__) */

libraries/shared/src/PacketHeaders.cpp

@@ -44,6 +44,8 @@ int packArithmeticallyCodedValue(int value, char* destination) {
 
 PacketVersion versionForPacketType(PacketType type) {
     switch (type) {
+        PacketTypeAvatarData:
+            return 1;
         case PacketTypeParticleData:
             return 1;
         case PacketTypeDomainList: