added interpolation implementation without visualization

mediapipe/examples/android/solutions/posetracking-lindera/src/main/java/com/google/mediapipe/examples/posetracking_lindera/ComputerVisionPluginImpl.java

@@ -1,5 +1,7 @@
 package com.google.mediapipe.examples.posetracking_lindera;
 
+import static java.lang.Math.min;
+
 import com.google.mediapipe.solutions.lindera.BodyJoints;
 import com.google.mediapipe.solutions.lindera.ComputerVisionPlugin;
 import com.google.mediapipe.solutions.lindera.XYZPointWithConfidence;
@@ -9,8 +11,10 @@ import org.json.JSONException;
 import org.json.JSONObject;
 
 import java.lang.reflect.Field;
+import java.util.HashMap;
 import java.util.LinkedList;
 import java.util.Locale;
+import java.util.Map;
 
 public class ComputerVisionPluginImpl implements ComputerVisionPlugin {
 
@@ -84,6 +88,7 @@ public class ComputerVisionPluginImpl implements ComputerVisionPlugin {
 
                 }
             }
+            bodyJointsString = bodyJointsString.concat(interpolateJoints(bodyJoints));
             // remove the last equal sign
             bodyJointsString = bodyJointsString.substring(0,bodyJointsString.length()-1);
 
@@ -101,9 +106,72 @@ public class ComputerVisionPluginImpl implements ComputerVisionPlugin {
 
     }
 
+    String interpolateJoints(BodyJoints bodyJoints){
+
+        Map<String,XYZPointWithConfidence> pts = new HashMap<>();
+        pts.put("PE", getPelvis(bodyJoints));
+        pts.put("NN",bodyJoints.nose);
+        // Assuming Thorax is 1/3 of distance between shoulders and pelvis
+        XYZPointWithConfidence thorax = getSpinePoint(bodyJoints,1/3f);
+        // Assuming spine/middle back is 2/3 of distance between shoulders and pelvis
+        XYZPointWithConfidence spine = getSpinePoint(bodyJoints,2/3f);
+        pts.put("TH",thorax);
+        pts.put("SP",spine);
+        pts.put("HT",getHeadTop(bodyJoints));
+        final String[] bodyJointsString = {""};
+        pts.forEach((key,data)->{
+            bodyJointsString[0] = bodyJointsString[0].concat(String.format(key+":%f,%f,%f=",data.x,data.y,data.z));
+
+        });
+
+        return bodyJointsString[0];
+
+
+
+
+
+    }
+    XYZPointWithConfidence getPelvis(BodyJoints bodyJoints){
+        return getMiddleJoint(bodyJoints.leftHip,bodyJoints.rightHip);
+
+    }
+
+    XYZPointWithConfidence getJointBetweenPoints(XYZPointWithConfidence pt1,XYZPointWithConfidence pt2,float distance){
+        XYZPointWithConfidence midpt = new XYZPointWithConfidence();
+        midpt.x  = pt1.x + (pt2.x-pt1.x)*distance;
+        midpt.y  = pt1.y + (pt2.y-pt1.y)*distance;
+        midpt.z  = pt1.z + (pt2.z-pt1.z)*distance;
+
+
+        midpt.presence = min(pt1.presence,pt2.presence);
+        midpt.confidence = min(pt1.confidence,pt2.confidence);
+        return midpt;
+    }
+    XYZPointWithConfidence getMiddleJoint(XYZPointWithConfidence pt1,XYZPointWithConfidence pt2) {
+        XYZPointWithConfidence midpt = new XYZPointWithConfidence();
+        midpt.x  = (pt1.x + pt2.x)/2;
+        midpt.y  = (pt1.y + pt2.y)/2;
+        midpt.z  = (pt1.z + pt2.z)/2;
+
+        midpt.presence = min(pt1.presence,pt2.presence);
+        midpt.confidence = min(pt1.confidence,pt2.confidence);
+        return midpt;
+    }
+        XYZPointWithConfidence getSpinePoint(BodyJoints bodyJoints, float distanceFromShoulders){
+            XYZPointWithConfidence midShoulder = getMiddleJoint(bodyJoints.leftShoulder, bodyJoints.rightShoulder);
+            XYZPointWithConfidence pelvis = getPelvis(bodyJoints);
+            return getJointBetweenPoints(midShoulder,pelvis,distanceFromShoulders);
+    }
+
+    XYZPointWithConfidence getHeadTop(BodyJoints bodyJoints){
+        XYZPointWithConfidence middleEye = getMiddleJoint(bodyJoints.leftEye,bodyJoints.rightEye);
+        return getJointBetweenPoints(middleEye,bodyJoints.nose,2);
+    }
+
+
 
     @Override
-    public void bodyJoints(int timestamp, BodyJoints bodyJoints) {
+    public void bodyJoints(long timestamp, BodyJoints bodyJoints) {
         if (isLogging){
             this.bodyJointsEventList.add(new BodyJointsEvent(timestamp,bodyJoints));