Merge pull request #45 from overte-org/feature/emocam_head_roll

IK based head movement for emocam app
overte-org · Oct 25, 2023 · 8948944 · 8948944
2 parents b2022f3 + 4019835
commit 8948944
Show file tree

Hide file tree

Showing 2 changed files with 62 additions and 14 deletions.
diff --git a/applications/emocam/emocam1.js b/applications/emocam/emocam1.js
@@ -34,6 +34,11 @@
     var yawValue = 0;
     var forwardValue = 0;
     var sideValue = 0;
+    var handlerId = 0;
+    var pitch = 0;
+    var yaw = 0;
+    var roll = 0;
+    var lastDataArrived = Date.now();
 
     button = tablet.addButton({
         icon: ROOT + "images/face.png",
@@ -64,6 +69,19 @@
     var sideBinding = mapping.from(function() { return sideValue; }).to(Controller.Actions.TranslateX);
     mapping.enable();
 
+    var propList = ["headRotation", "headType"];
+    handlerId = MyAvatar.addAnimationStateHandler(function (props) {
+        if (Date.now() - lastDataArrived < 2000) {
+            let headTransform = Quat.fromPitchYawRollDegrees(pitch, -yaw, roll);
+            return {
+                headRotation: headTransform,
+                headType: 4
+            };
+        } else {
+            return props;
+        }
+    }, propList);
+
     function onWebEventReceived(event) {
 
         var parsed = JSON.parse(event);
@@ -170,14 +188,14 @@
                         }
                     }
                 }
-                var direction = Vec3.multiplyQbyV(Quat.fromPitchYawRollDegrees(parsed.pitch, parsed.yaw, 0 ), {x: 0, y: 0, z: 100});
-                direction = Vec3.multiplyQbyV(MyAvatar.orientation, direction);
-                direction = Vec3.sum(direction, MyAvatar.position);
-                MyAvatar.setHeadLookAt(direction);
-                print("YAW="+MyAvatar.headYaw);
+
+                yaw = parsed.yaw;
+                pitch = parsed.pitch;
+                roll = parsed.roll;
                 for (var blendshape in bend) {
                     MyAvatar.setBlendshape(blendshape, bend[blendshape]);
                 }
+                lastDataArrived = Date.now();
             }
         }
     }
@@ -199,6 +217,8 @@
 
     Script.scriptEnding.connect(function () {
 
+        MyAvatar.removeAnimationStateHandler(handlerId);
+
         if (onEmoteScreen) {
             tablet.gotoHomeScreen();
         }

diff --git a/applications/emocam/index.html b/applications/emocam/index.html
@@ -802,24 +802,52 @@
             y: normalVector.y / magnitude,
             z: normalVector.z / magnitude
         };
+
+        const sidewaysVector = {
+            x: referencePoints[2].x - referencePoints[3].x,
+            y: referencePoints[2].y - referencePoints[3].y,
+            z: referencePoints[2].z - referencePoints[3].z
+        };
+
+        const upwardVector = {
+                x: sidewaysVector.y * normalizedNormal.z - sidewaysVector.z * normalizedNormal.y,
+                y: sidewaysVector.z * normalizedNormal.x - sidewaysVector.x * normalizedNormal.z,
+                z: sidewaysVector.x * normalizedNormal.y - sidewaysVector.y * normalizedNormal.x
+            };
+
+        // Normalize the upward normal vector (optional but can give better results)
+        const upwardMagnitude = Math.sqrt(upwardVector.x ** 2 + upwardVector.y ** 2 + upwardVector.z ** 2);
+        const upwardNormal = {
+            x: upwardVector.x / upwardMagnitude,
+            y: upwardVector.y / upwardMagnitude,
+            z: upwardVector.z / upwardMagnitude
+        };
 
         // Calculate the yaw, pitch, and roll angles
         const yaw = Math.atan2(normalizedNormal.x, normalizedNormal.z);
         const pitch = Math.atan2(-normalizedNormal.y, Math.sqrt(normalizedNormal.x ** 2 + normalizedNormal.z ** 2));
-        const roll = Math.atan2(normalizedNormal.y, normalizedNormal.x);
+
+        const planeRightZ = Math.sin(yaw);
+        const planeRightX = -Math.cos(yaw);
+
+        let roll = -Math.asin(upwardNormal.x*planeRightX + upwardNormal.z*planeRightZ);
+
+        if (upwardNormal.y < 0) {
+            roll = Math.sign(roll) * Math.PI - roll;
+        }
 
-        // Convert to degrees if needed
-		if (calibration === 0){
-		    yawC = yaw * (180 / Math.PI);
-			pitchC = pitch * (180 / Math.PI);
-			rollC = roll * (180 / Math.PI);
-			calibration = 1;
-		}
+        // Convert to degrees
+        if (calibration === 0){
+            yawC = yaw * (180 / Math.PI);
+            pitchC = pitch * (180 / Math.PI);
+            rollC = roll * (180 / Math.PI);
+            //console.log("YawC: ", yawC, "PitchC: ", pitchC, "RollC: ", rollC);
+            calibration = 1;
+        }
         yawDegrees = yaw * (180 / Math.PI) - yawC;
         pitchDegrees = pitch * (180 / Math.PI) - pitchC;
         rollDegrees = roll * (180 / Math.PI) - rollC;
 
-
         //console.log("Yaw: ", yawDegrees, "Pitch: ", pitchDegrees, "Roll: ", rollDegrees);
 	}