From 63acd145761276ef2eee474cd18e9536418c4925 Mon Sep 17 00:00:00 2001
From: spatialfree <cadetmerchant@gmail.com>
Date: Wed, 5 Jan 2022 08:49:48 -0500
Subject: [PATCH] reach cursor deadzone

---
 app/Monolith.cs      | 40 ++++++++++++++++++++++++++++++++++++----
 app/SpatialCursor.cs |  4 ++++
 2 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/app/Monolith.cs b/app/Monolith.cs
index 444ca13..8ae5c2c 100644
--- a/app/Monolith.cs
+++ b/app/Monolith.cs
@@ -40,7 +40,7 @@ public class Monolith {
   Mesh cube = Default.MeshCube;
 
   public void Run() {
-    Renderer.SetClip(0.0f, 1000f);
+    Renderer.SetClip(0.02f, 1000f);
     // Renderer.
     // mic = new Mic();
     Vec3 pos = new Vec3(0, 0, 0);
@@ -90,6 +90,8 @@ public class Monolith {
     bool rightGripping = false, leftGripping = false;
     bool gripLeft = false;
 
+    bool rightGripDown = false, leftGripDown = false;
+
 
     float grindDir = 1f;
     bool grinding = false;
@@ -197,13 +199,43 @@ public class Monolith {
       } 
 
       if (rCon.grip > 0.5f) {
-        leftReachCursor.origin = lShoulder.orientation.Inverse * (rCon.pose.position - lShoulder.position);
+        Vec3 toPos = lShoulder.orientation.Inverse * (rCon.pose.position - lShoulder.position);
+        if (!rightGripDown) {
+          float deadzone = Vec3.Distance(leftReachCursor.origin, toPos);
+          if (deadzone < 0.1f) {
+            leftReachCursor.deadzone = deadzone;
+            rightGripDown = true;
+          }
+        }
+        
+        if (rightGripDown) {
+          leftReachCursor.origin = toPos;
+        }
+      } else {
+        leftReachCursor.deadzone = 0;
+        rightGripDown = false;
       }
       if (lCon.grip > 0.5f) {
-        rightReachCursor.origin = rShoulder.orientation.Inverse * (lCon.pose.position - rShoulder.position);
+        Vec3 toPos = rShoulder.orientation.Inverse * (lCon.pose.position - rShoulder.position);
+        if (!leftGripDown) {
+          float deadzone = Vec3.Distance(rightReachCursor.origin, toPos);
+          if (deadzone < 0.1f) {
+            rightReachCursor.deadzone = deadzone;
+            leftGripDown = true;
+          }
+        }
+
+        if (leftGripDown) {
+          rightReachCursor.origin = toPos;
+        }
+      } else {
+        rightReachCursor.deadzone = 0;
+        leftGripDown = false;
       }
 
-      
+
+
+
       rightReachCursor.Step(new Pose[] { rCon.pose }, 0.2f);
       leftReachCursor.Step(new Pose[] { lCon.pose }, 0.2f);
 
diff --git a/app/SpatialCursor.cs b/app/SpatialCursor.cs
index ac50b8c..9ce62af 100644
--- a/app/SpatialCursor.cs
+++ b/app/SpatialCursor.cs
@@ -39,9 +39,11 @@ public class ReachCursor : SpatialCursor {
     this.min = 1f;
     this.str = 3f;
     this.max = 10f;
+    this.deadzone = 0;
   }
   Vec3 pos;
   public Vec3 origin;
+  public float deadzone;
   public override void Step(Pose[] poses, float scalar) {
     pos = poses[0].position;
     Vec3 wrist = mono.Wrist(chirality).position;
@@ -52,6 +54,8 @@ public class ReachCursor : SpatialCursor {
     str = min + (scalar * max);
 
     float stretch = Vec3.Distance(from, wrist);
+    stretch = Math.Max(stretch - deadzone, 0);
+
     Vec3 dir = (pos - from).Normalized;
     p0 = pos + dir * stretch * str;