Merge pull request #10609 from 1P-Cusack/21317

WL21317 helper functions for getting into avatar coordinate space

interface/src/avatar/MyAvatar.cpp

@@ -794,6 +794,77 @@ controller::Pose MyAvatar::getRightHandTipPose() const {
     return pose;
 }
 
+glm::vec3 MyAvatar::worldToJointPoint(const glm::vec3& position, const int jointIndex) const {
+    glm::vec3 jointPos = getPosition();//default value if no or invalid joint specified
+    glm::quat jointRot = getRotation();//default value if no or invalid joint specified
+    if (jointIndex != -1) {
+        if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) {
+            _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot);
+        } else {
+            qWarning() << "Invalid joint index specified: " << jointIndex;
+        }
+    }
+    glm::vec3 modelOffset = position - jointPos;
+    glm::vec3 jointSpacePosition = glm::inverse(jointRot) * modelOffset;
+
+    return jointSpacePosition;
+}
+
+glm::vec3 MyAvatar::worldToJointDirection(const glm::vec3& worldDir, const int jointIndex) const {
+    glm::quat jointRot = getRotation();//default value if no or invalid joint specified
+    if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
+        qWarning() << "Invalid joint index specified: " << jointIndex;
+    }
+
+    glm::vec3 jointSpaceDir = glm::inverse(jointRot) * worldDir;
+    return jointSpaceDir;
+}
+
+glm::quat MyAvatar::worldToJointRotation(const glm::quat& worldRot, const int jointIndex) const {
+    glm::quat jointRot = getRotation();//default value if no or invalid joint specified
+    if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
+        qWarning() << "Invalid joint index specified: " << jointIndex;
+    }
+    glm::quat jointSpaceRot = glm::inverse(jointRot) * worldRot;
+    return jointSpaceRot;
+}
+
+glm::vec3 MyAvatar::jointToWorldPoint(const glm::vec3& jointSpacePos, const int jointIndex) const {
+    glm::vec3 jointPos = getPosition();//default value if no or invalid joint specified
+    glm::quat jointRot = getRotation();//default value if no or invalid joint specified
+
+    if (jointIndex != -1) {
+        if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) {
+            _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot);
+        } else {
+            qWarning() << "Invalid joint index specified: " << jointIndex;
+        }
+    }
+
+    glm::vec3 worldOffset = jointRot * jointSpacePos;
+    glm::vec3 worldPos = jointPos + worldOffset;
+
+    return worldPos;
+}
+
+glm::vec3 MyAvatar::jointToWorldDirection(const glm::vec3& jointSpaceDir, const int jointIndex) const {
+    glm::quat jointRot = getRotation();//default value if no or invalid joint specified
+    if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
+        qWarning() << "Invalid joint index specified: " << jointIndex;
+    }
+    glm::vec3 worldDir = jointRot * jointSpaceDir;
+    return worldDir;
+}
+
+glm::quat MyAvatar::jointToWorldRotation(const glm::quat& jointSpaceRot, const int jointIndex) const {
+    glm::quat jointRot = getRotation();//default value if no or invalid joint specified
+    if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
+        qWarning() << "Invalid joint index specified: " << jointIndex;
+    }
+    glm::quat worldRot = jointRot * jointSpaceRot;
+    return worldRot;
+}
+
 // virtual
 void MyAvatar::render(RenderArgs* renderArgs) {
     // don't render if we've been asked to disable local rendering

interface/src/avatar/MyAvatar.h

@@ -378,6 +378,15 @@ public:
     Q_INVOKABLE controller::Pose getLeftHandTipPose() const;
     Q_INVOKABLE controller::Pose getRightHandTipPose() const;
 
+    // world-space to avatar-space rigconversion functions
+    Q_INVOKABLE glm::vec3 worldToJointPoint(const glm::vec3& position, const int jointIndex = -1) const;
+    Q_INVOKABLE glm::vec3 worldToJointDirection(const glm::vec3& direction, const int jointIndex = -1) const;
+    Q_INVOKABLE glm::quat worldToJointRotation(const glm::quat& rotation, const int jointIndex = -1) const;
+
+    Q_INVOKABLE glm::vec3 jointToWorldPoint(const glm::vec3& position, const int jointIndex = -1) const;
+    Q_INVOKABLE glm::vec3 jointToWorldDirection(const glm::vec3& direction, const int jointIndex = -1) const;
+    Q_INVOKABLE glm::quat jointToWorldRotation(const glm::quat& rotation, const int jointIndex = -1) const;
+
     AvatarWeakPointer getLookAtTargetAvatar() const { return _lookAtTargetAvatar; }
     void updateLookAtTargetAvatar();
     void clearLookAtTargetAvatar();

scripts/developer/tests/avatarToWorldTests.js

@@ -0,0 +1,127 @@
+var AVATAR_SELF_ID = "{00000000-0000-0000-0000-000000000001}";
+
+var debugSphereBaseProperties = {
+               type: "Sphere",
+               dimensions: { x: 0.2, y: 0.2, z: 0.2 },
+               dynamic: false,
+               collisionless: true,
+               gravity: { x: 0, y: 0, z: 0 },
+               lifetime: 10.0,
+               userData: "{ \"grabbableKey\": { \"grabbable\": false, \"kinematic\": false } }"
+};
+
+var debugBoxBaseProperties = {
+               type: "Box",
+               dimensions: { x: 0.2, y: 0.2, z: 0.2 },
+               dynamic: false,
+               collisionless: true,
+               gravity: { x: 0, y: 0, z: 0 },
+               lifetime: 10.0,
+               userData: "{ \"grabbableKey\": { \"grabbable\": false, \"kinematic\": false } }"
+};
+
+//jointToWorldPoint
+// create sphere for finger on left hand
+//	each frame, calculate world position of finger, with some offset
+//	update sphere to match this position
+var jointToWorldPointTest_sphereEntity;
+function jointToWorldPointTest() {
+   var jointIndex = MyAvatar.getJointIndex("LeftHandPinky4");
+   var jointOffset_WorldSpace = { x: 0.1, y: 0, z: 0 };
+   var worldPos = MyAvatar.jointToWorldPoint(jointOffset_WorldSpace, jointIndex);
+       
+   var jointSphereProps = Object.create(debugSphereBaseProperties);
+   jointSphereProps.name = "jointToWorldPointTest_Sphere";
+   jointSphereProps.color =  { blue: 240, green: 150, red: 150 };
+   jointSphereProps.position = worldPos;
+   jointToWorldPointTest_sphereEntity = Entities.addEntity(jointSphereProps);
+}
+function jointToWorldPointTest_update(deltaTime) {
+   var jointIndex = MyAvatar.getJointIndex("LeftHandPinky4");
+   var jointOffset_WorldSpace = { x: 0.1, y: 0, z: 0 };
+   var worldPos = MyAvatar.jointToWorldPoint(jointOffset_WorldSpace, jointIndex);
+   var newProperties = { position: worldPos };   
+   Entities.editEntity(jointToWorldPointTest_sphereEntity, newProperties);
+}
+
+//jointToWorldDirection
+//	create line in world space
+//	each frame calculate world space direction of players head z axis
+//	update line to match
+var jointToWorldDirectionTest_lineEntity;
+function jointToWorldDirectionTest() {
+   var jointIndex = MyAvatar.getJointIndex("Head");
+   var avatarPos = MyAvatar.getJointPosition(jointIndex);
+
+   var jointDir = { x: 1, y: 0, z: 1 };
+   var worldDir = MyAvatar.jointToWorldDirection(jointDir, jointIndex);
+   print(worldDir.x);
+   print(worldDir.y);
+   print(worldDir.z);
+   jointToWorldDirectionTest_lineEntity = Entities.addEntity({
+      type: "Line",
+      color: {red: 250, green: 0, blue: 0},
+      dimensions: {x: 5, y: 5, z: 5},
+      lifetime: 10.0,
+      linePoints: [{
+        x: 0,
+        y: 0,
+        z: 0
+       }, worldDir
+       ],
+      position : avatarPos,
+   });
+}
+function jointToWorldDirection_update(deltaTime) {
+   var jointIndex = MyAvatar.getJointIndex("Head");
+   var avatarPos = MyAvatar.getJointPosition(jointIndex);
+   var jointDir = { x: 1, y: 0, z: 0 };
+   var worldDir = MyAvatar.jointToWorldDirection(jointDir, jointIndex);
+   var newProperties = {
+      linePoints: [{
+        x: 0,
+        y: 0,
+        z: 0
+      }, worldDir
+      ],
+      position : avatarPos
+   };
+   
+   Entities.editEntity(jointToWorldDirectionTest_lineEntity, newProperties);
+}
+
+//jointToWorldRotation
+//	create box in world space
+//	each frame calculate world space rotation of players head
+//	update box rotation to match	
+var jointToWorldRotationTest_boxEntity;
+function jointToWorldRotationTest() {
+   var jointIndex = MyAvatar.getJointIndex("Head");
+   var jointPosition_WorldSpace =  MyAvatar.getJointPosition(jointIndex);
+   var jointRot = MyAvatar.getJointRotation(jointIndex);
+   var jointRot_WorldSpace = MyAvatar.jointToWorldRotation(jointRot, jointIndex);
+       
+   var boxProps = Object.create(debugBoxBaseProperties);
+   boxProps.name = "jointToWorldRotationTest_Box";
+   boxProps.color =  { blue: 250, green: 250, red: 250 };
+   boxProps.position = jointPosition_WorldSpace;
+   boxProps.rotation = jointRot_WorldSpace;
+   jointToWorldRotationTest_boxEntity = Entities.addEntity(boxProps);
+}
+function jointToWorldRotationTest_update(deltaTime) {
+   var jointIndex = MyAvatar.getJointIndex("Head");
+   var jointPosition_WorldSpace =  MyAvatar.getJointPosition(jointIndex);
+   var jointRot = MyAvatar.getJointRotation(jointIndex);
+   var jointRot_WorldSpace = MyAvatar.jointToWorldRotation(jointRot, jointIndex);
+   var newProperties = { position: jointPosition_WorldSpace, rotation: jointRot_WorldSpace };   
+   Entities.editEntity(jointToWorldRotationTest_boxEntity, newProperties);
+}
+
+jointToWorldPointTest();
+Script.update.connect(jointToWorldPointTest_update);
+
+jointToWorldDirectionTest();
+Script.update.connect(jointToWorldDirection_update);
+
+jointToWorldRotationTest();
+Script.update.connect(jointToWorldRotationTest_update);

scripts/developer/tests/worldToAvatarTests.js

@@ -0,0 +1,134 @@
+var AVATAR_SELF_ID = "{00000000-0000-0000-0000-000000000001}";
+
+var debugSphereBaseProperties = {
+               type: "Sphere",
+               dimensions: { x: 0.2, y: 0.2, z: 0.2 },
+               dynamic: false,
+               collisionless: true,
+               gravity: { x: 0, y: 0, z: 0 },
+               lifetime: 10.0,
+               userData: "{ \"grabbableKey\": { \"grabbable\": false, \"kinematic\": false } }"
+};
+
+var debugBoxBaseProperties = {
+               type: "Box",
+               dimensions: { x: 0.2, y: 0.2, z: 0.2 },
+               dynamic: false,
+               collisionless: true,
+               gravity: { x: 0, y: 0, z: 0 },
+               lifetime: 10.0,
+               userData: "{ \"grabbableKey\": { \"grabbable\": false, \"kinematic\": false } }"
+};
+
+//worldToJointPoint
+//	calculate position offset from joint using getJointPosition
+//	pass through worldToJointPoint to get offset in joint space of players joint
+//	create a blue sphere and attach it to players joint using the joint space offset
+// The two spheres should appear in the same place, but the blue sphere will follow the avatar
+function worldToJointPointTest() {
+   var jointIndex = MyAvatar.getJointIndex("LeftHandPinky4");
+   var avatarPos = MyAvatar.position;
+
+   var jointPosition_WorldSpace =  MyAvatar.getJointPosition(jointIndex);
+   var jointOffset_WorldSpace = { x: 0.1, y: 0, z: 0 };
+   var jointPosition_WorldSpaceOffset = Vec3.sum(jointPosition_WorldSpace, jointOffset_WorldSpace);
+   var jointPosition_JointSpaceOffset = MyAvatar.worldToJointPoint(jointPosition_WorldSpaceOffset, jointIndex);
+       
+   var jointSphereProps = Object.create(debugSphereBaseProperties);
+   jointSphereProps.name = "worldToJointPointTest_Joint";
+   jointSphereProps.color =  { blue: 240, green: 150, red: 150 };
+   jointSphereProps.localPosition = jointPosition_JointSpaceOffset;
+   jointSphereProps.parentID = AVATAR_SELF_ID;           
+   jointSphereProps.parentJointIndex = jointIndex;
+   Entities.addEntity(jointSphereProps);
+   
+   var worldSphereProps = Object.create(debugSphereBaseProperties);
+   worldSphereProps.name = "worldToJointPointTest_World";
+   worldSphereProps.color =  { blue: 150, green: 250, red: 150 };
+   worldSphereProps.position = jointPosition_WorldSpaceOffset;
+   Entities.addEntity(worldSphereProps);
+}
+
+//worldToJointDirection
+//	create line and attach to avatars head
+//	each frame calculate direction of world x axis in joint space of players head
+//	update arrow orientation to match
+var worldToJointDirectionTest_lineEntity;
+function worldToJointDirectionTest() {
+   var jointIndex = MyAvatar.getJointIndex("Head");
+
+   var jointPosition_WorldSpace =  MyAvatar.getJointPosition(jointIndex);
+   var jointOffset_WorldSpace = { x: 0, y: 0, z: 0 };
+   var jointPosition_WorldSpaceOffset = Vec3.sum(jointPosition_WorldSpace, jointOffset_WorldSpace);
+   var jointPosition_JointSpaceOffset = MyAvatar.worldToJointPoint(jointPosition_WorldSpaceOffset, jointIndex);
+
+   var worldDir = { x: 1, y: 0, z: 0 };
+   var avatarDir = MyAvatar.worldToJointDirection(worldDir, jointIndex);
+   
+   worldToJointDirectionTest_lineEntity = Entities.addEntity({
+      type: "Line",
+      color: {red: 200, green: 250, blue: 0},
+      dimensions: {x: 5, y: 5, z: 5},
+      lifetime: 10.0,
+      linePoints: [{
+        x: 0,
+        y: 0,
+        z: 0
+       }, avatarDir
+       ],
+      localPosition : jointOffset_WorldSpace,
+      parentID : AVATAR_SELF_ID,         
+      parentJointIndex : jointIndex      
+   });
+}
+
+function worldToJointDirectionTest_update(deltaTime) {  
+   var jointIndex = MyAvatar.getJointIndex("Head");
+   var worldDir = { x: 1, y: 0, z: 0 };
+   var avatarDir = MyAvatar.worldToJointDirection(worldDir, jointIndex);
+   var newProperties = { linePoints: [{
+        x: 0,
+        y: 0,
+        z: 0
+       }, avatarDir
+       ]};
+   
+   Entities.editEntity(worldToJointDirectionTest_lineEntity, newProperties);
+}
+
+//worldToJointRotation
+//	create box and parent to some player joint
+//	convert world identity rotation to joint space rotation
+//	each frame, update box with new orientation
+var worldToJointRotationTest_boxEntity;
+function worldToJointRotationTest() {
+   var jointIndex = MyAvatar.getJointIndex("RightHandPinky4");
+   var avatarPos = MyAvatar.position;
+
+   var jointPosition_WorldSpace =  MyAvatar.getJointPosition(jointIndex);
+   var jointOffset_WorldSpace = { x: 0.0, y: 0, z: 0 };
+   var jointPosition_WorldSpaceOffset = Vec3.sum(jointPosition_WorldSpace, jointOffset_WorldSpace);
+   var jointPosition_JointSpaceOffset = MyAvatar.worldToJointPoint(jointPosition_WorldSpaceOffset, jointIndex);
+       
+   var jointBoxProps = Object.create(debugBoxBaseProperties);
+   jointBoxProps.name = "worldToJointRotationTest_Box";
+   jointBoxProps.color =  { blue: 0, green: 0, red: 250 };
+   jointBoxProps.localPosition = jointPosition_JointSpaceOffset;
+   jointBoxProps.parentID = AVATAR_SELF_ID;           
+   jointBoxProps.parentJointIndex = jointIndex;
+   worldToJointRotationTest_boxEntity = Entities.addEntity(jointBoxProps);
+}
+function worldToJointRotationTest_update(deltaTime) {
+   var jointIndex = MyAvatar.getJointIndex("RightHandPinky4");
+   var worldRot = Quat.fromPitchYawRollDegrees(0,0,0);
+   var avatarRot = MyAvatar.worldToJointRotation(worldRot, jointIndex);
+   var newProperties = { localRotation: avatarRot };   
+   Entities.editEntity(worldToJointRotationTest_boxEntity, newProperties);
+}
+
+worldToJointPointTest();
+worldToJointDirectionTest();
+worldToJointRotationTest();
+
+Script.update.connect(worldToJointDirectionTest_update);
+Script.update.connect(worldToJointRotationTest_update);