half decent cursor integration

MonoNet.cs

@@ -94,6 +94,7 @@ public class MonoNet {
           peers[index].offHand = ReadPose();
           peers[index].mainHand = ReadPose();
           ReadBlock(ref peers[index].blocks);
+          ReadCubic(ref peers[index].cubics);
 
           peers[index].lastPing = Time.Totalf;
         }
@@ -123,6 +124,7 @@ public class MonoNet {
       WritePose(me.offHand);
       WritePose(me.mainHand);
       WriteBlock(me.blocks);
+      WriteCubic(me.cubics);
       socket.Send(wData);
 
       Thread.Sleep(60);
@@ -255,7 +257,7 @@ public class MonoNet {
       }
     }
   }
-  void WriteBlock(Cubic[] cubics) {
+  void WriteCubic(Cubic[] cubics) {
     for (int i = 0; i < cubics.Length; i++) {
       WriteBool(cubics[i].active);
       WriteColor(cubics[i].color);
@@ -422,6 +424,9 @@ public class MonoNet {
             Quat toRot = (con.aim.orientation * heldRot * spinRot).Normalized;
             Vec3 toPos = cursor + (con.aim.orientation * heldRot * spinRot).Normalized * offset;
             // cursor - offset;
+            if (con.stick.Magnitude > 0.1f) {
+              toRot = blocks[index].solid.GetPose().orientation;
+            }
             blocks[index].solid.Move(toPos, toRot);
 
             Quat newHeldRot = blocks[index].solid.GetPose().orientation;
@@ -460,16 +465,18 @@ public class MonoNet {
 
     public void Draw(bool body) {
       if (body){
-        Cube(Matrix.TRS(cursor0, Quat.Identity, Vec3.One * 0.05f), color);
+        // Cube(Matrix.TRS(cursor0, Quat.Identity, Vec3.One * 0.05f), color);
-        Cube(headset.ToMatrix(Vec3.One * 0.3f), color);
+        Cube(Matrix.TRS(headset.position + Input.Head.Forward * -0.15f, headset.orientation, Vec3.One * 0.3f), color);
-        Cube(offHand.ToMatrix(Vec3.One * 0.1f), color);
+        // Cube(offHand.ToMatrix(new Vec3(0.1f, 0.025f, 0.1f)), color);
-        Cube(mainHand.ToMatrix(Vec3.One * 0.1f), color);
+        // Cube(mainHand.ToMatrix(new Vec3(0.1f, 0.025f, 0.1f)), color);
 
         Bezier.Draw(cursor0, cursor1, cursor2, cursor3, Color.White);
       }
+        // Cube(offHand.ToMatrix(new Vec3(0.1f, 0.025f, 0.1f)), color);
+        // Cube(mainHand.ToMatrix(new Vec3(0.1f, 0.025f, 0.1f)), color);
     
-      Cube(Matrix.TRS(cursor0, Input.Controller(Handed.Right).pose.orientation, 0.1f), color);
+      Cube(Matrix.TRS(cursor0, mainHand.orientation, new Vec3(0.025f, 0.1f, 0.1f)), color);
-      Cube(Matrix.TRS(cursor3, Input.Controller(Handed.Right).pose.orientation, 0.1f), color);
+      Cube(Matrix.TRS(cursor3, offHand.orientation, new Vec3(0.025f, 0.1f, 0.1f)), color);
 
       for (int i = 0; i < blocks.Length; i++) {
         if (blocks[i].solid.GetPose().position.y < -10) {

Program.cs

@@ -29,7 +29,7 @@ public class Mono {
   Mesh cube = Default.MeshCube;
 
   public void Run() {
-    Renderer.SetClip(0f, 100f);
+    Renderer.SetClip(0f, 200f);
     // mic = new Mic();
     Vec3 pos = new Vec3(0, 0, 0);
     Vec3 vel = new Vec3(0, 0, 0);
@@ -63,6 +63,8 @@ public class Mono {
 
     SpatialCursor cursor = new ReachCursor();
     SpatialCursor subCursor = new ReachCursor();
+    bool domPlanted = false;
+    bool subPlanted = false;
 
     SpatialCursor cubicFlow = new CubicFlow();
 
@@ -87,6 +89,8 @@ public class Mono {
 
     Input.HandSolid(Handed.Right, false);
     Input.HandSolid(Handed.Left, false);
+    Input.HandVisible(Handed.Right, false);
+    Input.HandVisible(Handed.Left, false);
 
     while (SK.Step(() => {
       Renderer.CameraRoot = Matrix.T(pos);
@@ -103,8 +107,8 @@ public class Mono {
 
       // Shoulders
       Vec3 headPos = Input.Head.position + Input.Head.Forward * -0.15f;
-      Vec3 toSub = (subCon.aim.position.X0Z - headPos.X0Z).Normalized;
+      Vec3 toSub = (subCon.pose.position.X0Z - headPos.X0Z).Normalized;
-      Vec3 toDom = (domCon.aim.position.X0Z - headPos.X0Z).Normalized;
+      Vec3 toDom = (domCon.pose.position.X0Z - headPos.X0Z).Normalized;
       Vec3 middl = (toSub + toDom).Normalized;
 
       if (Vec3.Dot(middl, Input.Head.Forward) < 0) {
@@ -123,19 +127,28 @@ public class Mono {
       // Lines.Add(headPos + Vec3.Up * -0.2f, rShoulder, new Color(1, 0, 0), 0.01f);
       // Lines.Add(headPos + Vec3.Up * -0.2f, lShoulder, new Color(1, 0, 0), 0.01f);
 
-
+      if (domCon.IsX1JustPressed) {
-      cursor.Step(new Pose[] { domCon.aim, new Pose(rShoulder, Quat.LookDir(middl)) }, ((Input.Controller(Handed.Right).stick.y + 1) / 2));
+        domPlanted = !domPlanted;
-      if (!domCon.IsX1Pressed) {
-        cursor.Calibrate();
       }
-      subCursor.Step(new Pose[] { subCon.aim, new Pose(lShoulder, Quat.LookDir(middl)) }, ((Input.Controller(Handed.Left).stick.y + 1) / 2));
+      if (subCon.IsX1JustPressed) {
-      if (!subCon.IsX1Pressed) {
+        subPlanted = !subPlanted;
+      }
+
+
+      cursor.Step(new Pose[] { domCon.pose, new Pose(rShoulder, Quat.LookDir(middl)) }, 1);
+      if (!domPlanted) {
+        cursor.Calibrate();
+        cursor.p0 = domCon.pose.position;
+      }
+      subCursor.Step(new Pose[] { subCon.pose, new Pose(lShoulder, Quat.LookDir(middl)) }, 1); // ((Input.Controller(Handed.Left).stick.y + 1) / 2)
+      if (!subPlanted) {
         subCursor.Calibrate();
+        subCursor.p0 = subCon.pose.position;
       } 
       // cursor.p1 = subCursor.p0; // override *later change all one handed cursors to be dual wielded by default*
 
-      cubicFlow.Step(new Pose[] { domCon.aim, subCon.aim }, 1);
+      cubicFlow.Step(new Pose[] { new Pose(cursor.p0, domCon.aim.orientation), new Pose(subCursor.p0, subCon.aim.orientation) }, 1);
-      if (domCon.stick.MagnitudeSq != 0 || subCon.stick.MagnitudeSq != 0) {
+      if (domCon.stick.Magnitude > 0.1f || subCon.stick.Magnitude > 0.1f) {
         Bezier.Draw(cubicFlow.p0, cubicFlow.p1, cubicFlow.p2, cubicFlow.p3, Color.White);
         net.me.cursor0 = cubicFlow.p0; net.me.cursor1 = cubicFlow.p1; net.me.cursor2 = cubicFlow.p2; net.me.cursor3 = cubicFlow.p3;
       } else {
@@ -237,7 +250,7 @@ public class Mono {
                 };
                 for (int j = 0; j < rail.Length; j++) {
                   Vec3 point = Bezier.Sample(rail, (float)j / (rail.Length - 1f));
-                  float dist = Vec3.Distance(point, domCon.aim.position + vel.Normalized * 0.25f);
+                  float dist = Vec3.Distance(point, domCon.pose.position + vel.Normalized * 0.25f);
                   if (dist < closestDist && dist < 0.5f) {
                     closest = j;
                     closestRail = i;
@@ -256,7 +269,7 @@ public class Mono {
                 net.me.cubics[closestRail].p2,
                 net.me.cubics[closestRail].p3,
               };
-              // pos = closestPoint - (subCon.aim.position - pos);
+              // pos = closestPoint - (subCon.pose.position - pos);
               grindVel = vel;
               Vec3 fromPos = Bezier.Sample(grindRail[0], grindRail[1], grindRail[2], grindRail[3], railT);
               Vec3 toPos = Bezier.Sample(grindRail[0], grindRail[1], grindRail[2], grindRail[3], railT + 0.1f);
@@ -270,7 +283,7 @@ public class Mono {
 
             // vel += (toPos - fromPos);
 
-            pos = -(domCon.aim.position - Input.Head.position) + grindPos - (Input.Head.position - pos);
+            pos = -(domCon.pose.position - Input.Head.position) + grindPos - (Input.Head.position - pos);
             vel = Vec3.Zero;
 
             railT += Time.Elapsedf * grindVel.Magnitude * grindDir; // scale based on length of rail * calculate and cache on place
@@ -327,12 +340,13 @@ public class Mono {
 
       // COLOR CUBE
       // reveal when palm up
-      float reveal = subCon.pose.Right.y * 2;
+      float reveal = subCon.pose.Right.y * 0.666f;
+      reveal += (1 - Math.Clamp(Vec3.Dot((subCon.pose.position - Input.Head.position).Normalized, Input.Head.Forward), 0f, 1f)) * 0.666f;
       colorCube.size = colorCube.ogSize * Math.Clamp(reveal, 0, 1);
       colorCube.center = subCon.pose.position + subCon.pose.Right * 0.0666f;
       // move with grip
       if (reveal > colorCube.thicc) {
-        if (reveal > 1f && subCon.grip > 0.5f) {
+        if (reveal > 1f && subCon.trigger > 0.5f) {
           colorCube.p0 -= (subCon.pose.position - oldSubPos) / colorCube.ogSize * 2;
         } else {
           // clamp 0 - 1

SpatialCursor.cs

@@ -97,18 +97,27 @@ public class TwistCursor : SpatialCursor {
     this.str = 6f;
     this.max = 10f;
   }
+  Vec3 twistFrom = -Vec3.Right;
+  Quat quat;
   public override void Step(Pose[] poses, float scalar) {
+    // chirality = Math.Sign(scalar);
     Vec3 pos = poses[0].position;
-    Quat quat = poses[0].orientation;
+    quat = poses[0].orientation;
-    Quat rel = Quat.LookAt(Vec3.Zero, quat * Vec3.Forward);
+    Quat from = Quat.LookAt(Vec3.Zero, quat * Vec3.Forward, twistFrom);
-    float twist = (Vec3.Dot(rel * -Vec3.Right * scalar, quat * Vec3.Up) + 1) / 2;
+    float twist = 1 - ((Vec3.Dot(from * Vec3.Up, quat * Vec3.Up) + 1) / 2);
+    // float wrap = twist - twistFrom; // wrap around 0 to 1
+    // (wrap > 0.5f) ? 1 - wrap : wrap;
+    
     p0 = pos + quat * Vec3.Forward * twist * str;
-
     // model.Draw(Matrix.TS(p0, 0.02f));
-  }
-  public override void Calibrate() { }
 
-  public bool outty;
+    // Lines.Add(pos, pos + from * Vec3.Up, Color.White, 0.005f);
+    // Lines.Add(pos, pos + quat * Vec3.Up, Color.White, 0.005f);
+  }
+  public override void Calibrate() {
+
+    twistFrom = quat * Vec3.Up; // -Vec3.Right * chirality;
+  }
 }
 
 public class CubicFlow : SpatialCursor {
@@ -119,29 +128,62 @@ public class CubicFlow : SpatialCursor {
   }
   TwistCursor domTwist = new TwistCursor();
   TwistCursor subTwist = new TwistCursor();
+  bool domTwisting = false; bool domUp = false;
+  bool subTwisting = false; bool subUp = false;
   public override void Step(Pose[] poses, float scalar) {
     Pose dom = poses[0];
     Pose sub = poses[1];
+    Controller domCon = Input.Controller(Handed.Right);
+    Controller subCon = Input.Controller(Handed.Left);
+
+    if (domCon.stick.Magnitude < 0.1f) {
+      domTwist.Calibrate();
+      domTwisting = false;
+    } else {
+      if (!domTwisting) {
+        domUp = domCon.stick.y > 0;
+        domTwisting = true;
+      }
+    }
     domTwist.Step(new Pose[] { dom }, scalar);
-    subTwist.Step(new Pose[] { sub }, -scalar);
+
+    if (subCon.stick.Magnitude < 0.1f) {
+      subTwist.Calibrate();
+      subTwisting = false;
+    } else {
+      if (!subTwisting) {
+        subUp = subCon.stick.y > 0;
+        subTwisting = true;
+      }
+    }
+    subTwist.Step(new Pose[] { sub }, scalar);
 
     p0 = dom.position;
     p1 = domTwist.p0;
     p2 = subTwist.p0;
     p3 = sub.position;
 
-    Controller domCon = Input.Controller(Handed.Right);
-    Controller subCon = Input.Controller(Handed.Left);
 
-    Vec3 np0 = Vec3.Lerp(p0, p1, (1 + domCon.stick.x) / 2);
+    if (domUp) {
-    Vec3 np1 = Vec3.Lerp(p1, p0, (1 + domCon.stick.x) / 2);
+      p0 = domTwist.p0;
-    Vec3 np2 = Vec3.Lerp(p2, p3, (1 + -subCon.stick.x) / 2);
+      p1 = dom.position;
-    Vec3 np3 = Vec3.Lerp(p3, p2, (1 + -subCon.stick.x) / 2);
+    }
+
+    if (subUp) {
+      p2 = sub.position;
+      p3 = subTwist.p0;
+    }
+    // Vec3 np0 = Vec3.Lerp(p0, p1, (1 + domCon.stick.x) / 2);
+    // Vec3 np1 = Vec3.Lerp(p1, p0, (1 + domCon.stick.x) / 2);
+    // Vec3 np2 = Vec3.Lerp(p2, p3, (1 + -subCon.stick.x) / 2);
+    // Vec3 np3 = Vec3.Lerp(p3, p2, (1 + -subCon.stick.x) / 2);
+
+    // p0 = np0;
+    // p1 = np1;
+    // p2 = np2;
+    // p3 = np3;
+
 
-    p0 = np0;
-    p1 = np1;
-    p2 = np2;
-    p3 = np3;
     // if toggle
   }