From a832b4c8bf81679bd99f410ca6b1d74d31ea4b7d Mon Sep 17 00:00:00 2001
From: spatialfree <cadetmerchant@gmail.com>
Date: Mon, 29 Nov 2021 09:36:50 -0500
Subject: [PATCH] roughed out, sk bugged

---
 Program.cs       | 55 +++++++++++++++++++++++++++++++++++++++++++++---
 README.md        |  4 ++--
 SpatialCursor.cs | 45 +++++++++++++++++++++++++++------------
 3 files changed, 85 insertions(+), 19 deletions(-)

diff --git a/Program.cs b/Program.cs
index 550c987..c48643f 100644
--- a/Program.cs
+++ b/Program.cs
@@ -19,6 +19,12 @@ class Program {
 public class Mono {
   public Mic mic;
   public Controller domCon, subCon; public bool lefty;
+  public float movePress;
+  public Vec3 dragStart, posStart;
+  public float railT;
+
+  Mesh ball = Default.MeshSphere;
+  Material ballMat = Default.Material;
 
   public void Run() {
     // mic = new Mic();
@@ -42,17 +48,60 @@ public class Mono {
       else { domCon = Input.Controller(Handed.Right); subCon = Input.Controller(Handed.Left); }
       if (subCon.IsX2JustPressed) { lefty = !lefty; }
 
+      // ball.Draw(ballMat, Matrix.TS(pos, 0.1f));
+
+      Renderer.CameraRoot = Matrix.T(pos);
+
+      SpatialCursor cursor = cursors.Step(domCon.aim, subCon.aim);
+
       Quat rot = Quat.FromAngles(subCon.stick.y * -90, 0, subCon.stick.x * 90);
       Vec3 dir = Vec3.Up * (subCon.IsStickClicked ? -1 : 1);
       Vec3 fullstick = subCon.aim.orientation * rot * dir;
       pos += fullstick * subCon.trigger * Time.Elapsedf;
 
-      SpatialCursor cursor = cursors.Step(domCon.aim, subCon.aim);
+      Vec3[] rail = new Vec3[] {
+        new Vec3(0, 0, -1),
+        new Vec3(0, 0, -2),
+        new Vec3(1, 2, -3),
+        new Vec3(0, 1, -4),
+      };
+      Bezier.Draw(rail);
+      if (subCon.IsX1JustPressed) {
+        int closest = 0;
+        float closestDist = float.MaxValue;
+        Vec3 closestPoint = Vec3.Zero;
+        for (int i = 0; i < rail.Length; i++) {
+          Vec3 point = Bezier.Sample(rail, (float)i / (rail.Length - 1f));
+          float dist = Vec3.Distance(point, subCon.aim.position);
+          if (dist < closestDist) {
+            closest = i;
+            closestDist = dist;
+            closestPoint = point;
+            railT = (float)i / (rail.Length - 1f);
+          }
+        }
+        // pos = closestPoint - (subCon.aim.position - pos);
+      }
+      if (subCon.IsX1Pressed) {
+        pos = Bezier.Sample(rail, railT) - (subCon.aim.position - pos);
+        railT += Time.Elapsedf * 0.1f;
+        // how to reliably determine and control which direction to go?
+      }
+
       if (domCon.IsX1JustPressed) {
-        pos = cursor.p0;
+        movePress = Time.Totalf;
+        dragStart = cursor.p0;
+        posStart = pos;
+      }
+      if (domCon.IsX1Pressed) {
+        pos -= cursor.p0 - dragStart;
+        dragStart = cursor.p0;
+      }
+      if (domCon.IsX1JustUnPressed && Time.Totalf - movePress < 0.2f) {
+        pos = cursor.p0 - (Input.Head.position - pos);
       }
       // pos.x = (float)Math.Sin(Time.Total * 0.1f) * 0.5f;
-      Renderer.CameraRoot = Matrix.T(pos);
+      
 
       // cursor.Step(lHand.aim, rHand.aim); cursor.DrawSelf();
       // net.me.cursorA = Vec3.Up * (float)Math.Sin(Time.Total);
diff --git a/README.md b/README.md
index 825f70d..4450212 100644
--- a/README.md
+++ b/README.md
@@ -25,8 +25,8 @@
 switch between cursors with a button (quick press(b) on both hands to switch/cycle through the two handed ones) long press(b) on one hand to swap mainhand
 
 movement:
-- fullstick
-- ~~teleport~~ and drag
+- ~~fullstick~~
+- ~~teleport and drag~~
 - bezier rails
 
 blocks you can manipulate with spatial cursors (trackballer)
diff --git a/SpatialCursor.cs b/SpatialCursor.cs
index 794c742..ec321ef 100644
--- a/SpatialCursor.cs
+++ b/SpatialCursor.cs
@@ -107,20 +107,7 @@ public class CubicFlow : SpatialCursor {
 
     // if toggle
 
-    Draw(this.p0, this.p1, this.p2, this.p3);
-  }
-
-  LinePoint[] bezier = new LinePoint[64];
-  public void Draw(Vec3 p0, Vec3 p1, Vec3 p2, Vec3 p3) {
-    for (int i = 0; i < bezier.Length; i++) {
-      float t = i / ((float)bezier.Length - 1);
-      Vec3 a = Vec3.Lerp(p0, p1, t);
-      Vec3 b = Vec3.Lerp(p1, p2, t);
-      Vec3 c = Vec3.Lerp(p2, p3, t);
-      Vec3 pos = Vec3.Lerp(Vec3.Lerp(a, b, t), Vec3.Lerp(b, c, t), t);
-      bezier[i] = new LinePoint(pos, Color.White, 0.01f);
-    }
-    Lines.Add(bezier);
+    Bezier.Draw(this.p0, this.p1, this.p2, this.p3);
   }
 
   public override void Calibrate() {}
@@ -155,6 +142,36 @@ public class SupineCursor : SpatialCursor {
   }
 }
 
+public static class Bezier {
+  static int detail = 64;
+  public static void Draw(Vec3 p0, Vec3 p1, Vec3 p2, Vec3 p3) {
+    LinePoint[] bezier = new LinePoint[detail];
+    for (int i = 0; i < bezier.Length; i++) {
+      float t = i / ((float)bezier.Length - 1);
+      Vec3 a = Vec3.Lerp(p0, p1, t);
+      Vec3 b = Vec3.Lerp(p1, p2, t);
+      Vec3 c = Vec3.Lerp(p2, p3, t);
+      Vec3 pos = Vec3.Lerp(Vec3.Lerp(a, b, t), Vec3.Lerp(b, c, t), t);
+      bezier[i] = new LinePoint(pos, Color.White, 0.01f);
+    }
+    Lines.Add(bezier);
+  }
+  public static void Draw(Vec3[] points) {
+    Draw(points[0], points[1], points[2], points[3]);
+  }
+
+  public static Vec3 Sample(Vec3 p0, Vec3 p1, Vec3 p2, Vec3 p3, float t) {
+    Vec3 a = Vec3.Lerp(p0, p1, t);
+    Vec3 b = Vec3.Lerp(p1, p2, t);
+    Vec3 c = Vec3.Lerp(p2, p3, t);
+    Vec3 pos = Vec3.Lerp(Vec3.Lerp(a, b, t), Vec3.Lerp(b, c, t), t);
+    return pos;
+  }
+  public static Vec3 Sample(Vec3[] points, float t) {
+    return Sample(points[0], points[1], points[2], points[3], t);
+  }
+}
+
 // for fun
 // public class ClawCursor : SpatialCursor {