Ian

app/App.cs

@@ -1,5 +0,0 @@
-interface IApp { // <T> ?
-  void Initialize();
-  void Update();
-  void Shutdown();
-}

app/Glove.cs

@@ -10,7 +10,7 @@ public class Glove {
     this.chirality = chirality;
   }
 
-  public Pose virtualGlove;
+  public Pose virtualGlove = Pose.Identity;
   Quat projection = Quat.Identity;
   Vec3 direction {
     get { return projection * new Vec3(0, 0, -1); }
@@ -125,7 +125,7 @@ public class Glove {
     Vec3 thumb = hand.Get(FingerId.Thumb, JointId.Tip).position - con.pos;
     virtualGlove.position += Vec3.Lerp(index, thumb, 0.5f);
 
-    Render(con.Pose(), virtualGlove, wrist, stretch, twist, chirality);
+    Render(con.pose, virtualGlove, wrist, stretch, twist, chirality);
   }
 
   // decouple the rendering

app/Mono.cs

@@ -10,6 +10,11 @@ public class Mono {
   public Scene scene = new Scene();
 
   // -------------------------------------------------
+
+  dof[] dofs;
+  int dofIndex = 0;
+  dof dof => dofs[dofIndex];
+
   public Oriel oriel = new Oriel(); // -> array ?
 
   public ColorCube colorCube = new ColorCube();
@@ -27,6 +32,7 @@ public class Mono {
   public Cubic[] cubics = new Cubic[] {
     new Cubic(), new Cubic(), new Cubic(), new Cubic(), new Cubic(), new Cubic()
   };
+
   // -------------------------------------------------
 
   public MonoNet net = new MonoNet();
@@ -34,14 +40,26 @@ public class Mono {
   public Mono() {
     Renderer.SetClip(0.02f, 1000f);
 
+    dofs = new dof[] {
+      // new StretchFinger(),
+      // new Trackballer(rGlove.virtualGlove, anchor),
+      new StretchCursor() { handId = 0, deadzone = 0.01f, strength = 3f }, // can override design variables
+      new StretchCursor() { handId = 1, deadzone = 0.01f, strength = 3f }, // can override design variables
+    };
   }
+  Pose anchor = Pose.Identity;
 
   public void Init() {
+
+
+    dofs[0].Init();
+    dofs[1].Init();
+
+
     // spaceMono.Init();
     greenyard.Init();
   }
 
-
   // -------------------------------------------------
 
   // Space.Mono spaceMono = new Space.Mono();
@@ -53,13 +71,18 @@ public class Mono {
 
   // -------------------------------------------------
 
-  public void Step() {
+  public void Frame() {
 
     rig.Step();
 
     // -------------------------------------------------
 
-    rGlove.Step(); lGlove.Step();
+    // dof.Frame();    
+    dofs[0].Frame();
+    dofs[1].Frame();
+
+
+    // rGlove.Step(); lGlove.Step();
 
     // rBlock.Step(); lBlock.Step();
 
@@ -67,19 +90,19 @@ public class Mono {
 
     // colorCube.Palm(lCon.device);
 
-    oriel.Frame();
+    // oriel.Frame();
 
     scene.Step(); // after! (render scene after oriel)
 
     // -------------------------------------------------
 
     // spaceMono.Frame();
-    greenyard.Frame();
+    // greenyard.Frame();
     // board.Frame();
 
     // -------------------------------------------------
 
-    oriel.Render();
+    // oriel.Render();
 
     net.me.Step();
     net.send = true;
@@ -91,17 +114,38 @@ public class Mono {
   void ShowWindowButton() {
     UI.WindowBegin("Window Button", ref windowPoseButton);
 
-    if (UI.Button("Reset Oriel Quat")) { oriel.ori = Quat.Identity; }
     // if (UI.Button("Draw Oriel Axis")) { oriel.drawAxis = !oriel.drawAxis; }
-    if (UI.Button("Scale w/Height")) { oriel.scaleWithHeight = !oriel.scaleWithHeight; }
-    UI.HSlider("Scale", ref oriel.scale, 0.1f, 1f, 0.1f);
-    UI.HSlider("Multiplier", ref oriel.multiplier, 0.1f, 1f, 0.1f);
-    UI.Label("Player.y");
-    UI.HSlider("Player.y", ref greenyard.height, 0.1f, 1.5f, 0.1f);
+
+    // if (UI.Button("Reset Oriel Quat")) { oriel.ori = Quat.Identity; }
+    // if (UI.Button("Scale w/Height")) { oriel.scaleWithHeight = !oriel.scaleWithHeight; }
+    // UI.HSlider("Scale", ref oriel.scale, 0.1f, 1f, 0.1f);
+    // UI.HSlider("Multiplier", ref oriel.multiplier, 0.1f, 1f, 0.1f);
+    // UI.Label("Player.y");
+    // UI.HSlider("Player.y", ref greenyard.height, 0.1f, 1.5f, 0.1f);
+
+    UI.Label("trail.length");
+    UI.HSlider("trail.length", ref trailLen, 0.1f, 1f, 0.1f);
+
+    UI.Label("trail.scale");
+    UI.HSlider("trail.str", ref trailScl, 0.1f, 2f, 0.1f);
+
+    UI.Label("str");
+    UI.HSlider("str", ref stretchStr, 0.1f, 1f, 0.1f);
+
+
+    // flipIndex
+    // flipGrip
+
 
 
     UI.WindowEnd();
   }
+  public float trailLen = 0.333f;
+  public float trailScl = 1f;
+  public float stretchStr = 0.333f;
+
+
+
 }
 
 

app/MonoNet.cs

@@ -228,8 +228,8 @@ public class Peer {
     Mono mono = Mono.inst;
     color = mono.colorCube.color;
     headset = Input.Head;
-    rHand = mono.rig.rCon.Pose();
-    lHand = mono.rig.lCon.Pose();
+    rHand = mono.rig.rCon.pose;
+    lHand = mono.rig.lCon.pose;
     rCursor = mono.rGlove.virtualGlove;
     lCursor = mono.lGlove.virtualGlove;
 

app/PullRequest.cs

@@ -13,20 +13,32 @@ public static class PullRequest {
     }
   }
 
+  public static Vec3 Slerp(Vec3 a, Vec3 b, float t) {
+    float dot = Vec3.Dot(a, b);
+    dot = Clamp(dot, -1f, 1f);
+
+    float theta = MathF.Acos(dot) * t;
+    Vec3 relativeVec = b - a * dot;
+    relativeVec.Normalize();
+
+    return (a * MathF.Cos(theta)) + (relativeVec * MathF.Sin(theta));
+  }
+
   // amplify quaternions (q * q * lerp(q.i, q, %))
 
   public static Vec3 AngularDisplacement(Quat q) {
     float angle; Vec3 axis;
-    ToAngleAxis(q, out angle, out axis);
+    ToAxisAngle(q, out axis, out angle);
     return axis * angle;
     // * (float)(Math.PI / 180); // radians -> degrees
     // / Time.Elapsedf; // delta -> velocity
   }
 
-  public static void ToAngleAxis(Quat q, out float angle, out Vec3 axis) {
-    q = q.Normalized;
-    angle = 2 * (float)Math.Acos(q.w);
-    float s = (float)Math.Sqrt(1 - q.w * q.w);
+  public static void ToAxisAngle(this Quat q, out Vec3 axis, out float angle) {
+    q = q.Normalized; // q.Normalize(); ?
+    angle = 2 * MathF.Acos(q.w);
+    float s = MathF.Sqrt(1 - q.w * q.w);
+    // float s = 2 * MathF.Asin(angle * 0.5f);
     axis = Vec3.Right;
     // avoid divide by zero
     // + if s is close to zero then direction of axis not important
@@ -37,6 +49,22 @@ public static class PullRequest {
     }
   }
 
+  public static void LookDirection(this ref Quat q, Vec3 dir) {
+    Vec3 up = Vec3.Up;
+    
+    // using AxisAngle
+    Vec3 axis = Vec3.Cross(up, dir);
+    float angle = MathF.Atan2(Vec3.Dot(up, dir), axis.Length);
+    q = FromAxisAngle(axis.Normalized, angle);
+  }
+
+  public static Quat FromAxisAngle(Vec3 axis, float angle) {
+    float halfAngle = angle * 0.5f;
+    float sin = (float)Math.Sin(halfAngle);
+    float cos = (float)Math.Cos(halfAngle);
+    return new Quat(axis.x * sin, axis.y * sin, axis.z * sin, cos).Normalized;
+  }
+
   static Random r = new Random();
   public static int RandomRange(int min, int max) {
     return r.Next(min, max);
@@ -127,16 +155,16 @@ public static class PullRequest {
     return a + (b - a) * t;
   }
 
-  public static Vec3 Slerp(Vec3 a, Vec3 b, float t) {
-    // spherical linear interpolation
-    float dot = Vec3.Dot(a, b);
-    if (dot > 0.9995f) {
-      return Vec3.Lerp(a, b, t);
-    }
-    float theta = (float)Math.Acos(dot);
-    float sinTheta = (float)Math.Sin(theta);
-    return Vec3.Lerp(a * (float)Math.Sin(theta - theta * t) / sinTheta, b * (float)Math.Sin(theta * t) / sinTheta, t);
-  }
+  // public static Vec3 Slerp(Vec3 a, Vec3 b, float t) {
+  //   // spherical linear interpolation
+  //   float dot = Vec3.Dot(a, b);
+  //   if (dot > 0.9995f) {
+  //     return Vec3.Lerp(a, b, t);
+  //   }
+  //   float theta = (float)Math.Acos(dot);
+  //   float sinTheta = (float)Math.Sin(theta);
+  //   return Vec3.Lerp(a * (float)Math.Sin(theta - theta * t) / sinTheta, b * (float)Math.Sin(theta * t) / sinTheta, t);
+  // }
 
   [Serializable]
   public class Noise {

app/Rig/Rig.cs

@@ -83,22 +83,19 @@ public class Con {
   public Controller device;
   public Vec3 pos;
   public Quat ori;
+  public Pose pose;
   public Vec3 backhandDir;
   public Btn gripBtn;
   public Btn triggerBtn;
 
   public void Step(bool chirality) {
     device = Input.Controller(chirality ? Handed.Right : Handed.Left);
-    pos = device.pose.position;
-    ori = device.aim.orientation;
+    pose.position = pos = device.pose.position;
+    pose.orientation = ori = device.aim.orientation;
     backhandDir = ori * (chirality ? Vec3.Right : -Vec3.Right);
     gripBtn.Step(device.grip > 0.5f);
     triggerBtn.Step(device.trigger > 0.5f);
   }
-
-  public Pose Pose() {
-    return new Pose(pos, ori);
-  }
 }
 
 public struct Btn {

app/SpatialCursor.cs

@@ -10,23 +10,23 @@ public abstract class SpatialCursor {
   public abstract void Calibrate();
 }
 
-public class StretchCursor : SpatialCursor {
-  public StretchCursor() {
-    this.min = 1f;
-    this.str = 3f;
-    this.max = 10f;
-  }
-  public override void Step(Pose[] poses, float scalar) {
-    Pose dom = poses[0];
-    Pose sub = poses[1];
-    float stretch = (sub.position - dom.position).Magnitude;
-    stretch = Math.Max(stretch - 0.1f, 0);
-    p0 = dom.position + dom.Forward * stretch * 3;
+// public class StretchCursor : SpatialCursor {
+//   public StretchCursor() {
+//     this.min = 1f;
+//     this.str = 3f;
+//     this.max = 10f;
+//   }
+//   public override void Step(Pose[] poses, float scalar) {
+//     Pose dom = poses[0];
+//     Pose sub = poses[1];
+//     float stretch = (sub.position - dom.position).Magnitude;
+//     stretch = Math.Max(stretch - 0.1f, 0);
+//     p0 = dom.position + dom.Forward * stretch * 3;
 
-    model.Draw(Matrix.TS(p0, 0.06f));
-  }
-  public override void Calibrate() { }
-}
+//     model.Draw(Matrix.TS(p0, 0.06f));
+//   }
+//   public override void Calibrate() { }
+// }
 
 public class ReachCursor : SpatialCursor {
   bool chirality;

app/_Init.cs

@@ -8,6 +8,7 @@ SKSettings settings = new SKSettings {
   depthMode = DepthMode.D32,
   disableUnfocusedSleep = true,
   // displayPreference = DisplayMode.Flatscreen,
+  // disableFlatscreenMRSim = true,
 };
 if (!SK.Initialize(settings))
   Environment.Exit(1);
@@ -18,6 +19,6 @@ Input.HandVisible(Handed.Max, true);
 Oriels.Mono mono = Oriels.Mono.inst;
 mono.Init();
 while (SK.Step(() => {
-  mono.Step();
+  mono.Frame();
 }));
 SK.Shutdown();

app/dofs/dof.cs

@@ -0,0 +1,5 @@
+interface dof { // <T> ?
+  void Init();
+  void Frame();
+  // void Drop();
+}

app/dofs/stretch-cursor/finger/StretchFinger.cs

@@ -0,0 +1,12 @@
+namespace Oriels;
+
+class StretchFinger : dof {
+  // Pose p0, p1
+  public void Init() {
+
+  }
+
+  public void Frame() {
+    
+  }
+}

app/dofs/stretch-cursor/og/StretchCursor.cs

@@ -0,0 +1,118 @@
+namespace Oriels;
+
+class StretchCursor : dof {
+  Pose p0, p1;
+  public StretchCursor() {
+    
+  }
+
+  public Pose cursor;
+  public void Init() {
+    
+  }
+
+  bool isTracking = false;
+  public void Frame() {
+    Hand hand = Input.Hand(handId);
+
+    if (hand.tracked.IsActive() && !hand.tracked.IsJustActive()) {
+      p0.position = hand.Get(FingerId.Index, JointId.KnuckleMajor).position;
+      p1.position = hand.Get(FingerId.Index, JointId.Tip).position;
+
+      isTracking = true;
+    }
+
+
+    // Vec3   vec      = p0.position - p1.position;
+    // float  len      = vec.Length;
+    // float  stretch  = Math.Max(len - deadzone, 0f);
+    // Vec3   dir      = backhand ? vec / len : p0.orientation * Vec3.Forward;
+
+
+
+    float fI = Flexion(hand, FingerId.Index);
+    float fM = Flexion(hand, FingerId.Middle);
+    float fR = Flexion(hand, FingerId.Ring);
+    float fL = Flexion(hand, FingerId.Little);
+
+    float stretch = (fI + fI + fM + fM + fM + fR + fR + fL) / 8f; // based on finger length
+
+    Vec3 dir = PullRequest.Direction(
+      hand.Get(FingerId.Index, JointId.Tip).position,
+      hand.Get(FingerId.Index, JointId.KnuckleMajor).position
+    );
+
+    cursor.position = hand.Get(FingerId.Index, JointId.Tip).position + dir * stretch * strength * Mono.inst.stretchStr;
+
+    Mesh.Cube.Draw(Material.Default, Matrix.TS(cursor.position, 0.01f));
+
+    if (isTracking) { Demo(); }
+  }
+
+  public bool  backhand = true;
+  public float deadzone = 0.1f;
+  public float strength = 3f;
+  public int   handId   = 0;
+
+
+  float Flexion(Hand hand, FingerId finger) {
+    float flexion = (Vec3.Dot(
+      PullRequest.Direction(
+        hand.Get(finger, JointId.Tip).position,
+        hand.Get(finger, JointId.KnuckleMinor).position
+      ),
+      PullRequest.Direction(
+        hand.Get(finger, JointId.KnuckleMid).position,
+        hand.Get(finger, JointId.KnuckleMajor).position
+      )
+    ) + 1f) / 2;
+
+    return Math.Max(flexion - deadzone, 0f) / (1 - deadzone);
+  }
+
+
+
+
+
+
+
+
+
+
+
+  Vec3 smoothPos;
+  Vec3[] points = new Vec3[64];
+  void Demo() {
+
+    smoothPos = Vec3.Lerp(smoothPos, cursor.position, Time.Elapsedf * 6f);
+
+    // while (Vec3.Distance(points[0], smoothPos) > 0.03f) {
+    //   for (int i = points.Length - 1; i > 0; i--) {
+    //     points[i] = points[i - 1];
+    //   }
+    //   points[0] += PullRequest.Direction(smoothPos, points[0]) * 0.02f;
+    // }
+
+
+    points[0] = smoothPos;
+
+    int len = (int)(points.Length * Mono.inst.trailLen);
+    for (int i = 0; i < len; i++) {
+      if (i > 0) {
+        Vec3 dir = Vec3.Forward;
+        if (points[i].v != points[i - 1].v) {
+          dir = PullRequest.Direction(points[i], points[i - 1]);
+        }
+        points[i] = points[i - 1] + dir * 0.02f * Mono.inst.trailScl;
+      }
+
+      Vec3 from = i > 0 ? points[i - 1] : smoothPos;
+
+      Mesh.Cube.Draw(
+        Material.Default,
+        Matrix.TRS(points[i], Quat.LookDir(PullRequest.Direction(points[i], from)), 0.01f * Mono.inst.trailScl), 
+        Color.HSV(i / (float)len, 1, 1)
+      );
+    }
+  }
+}

app/dofs/trackballer/Trackballer.cs

@@ -0,0 +1,130 @@
+namespace Oriels;
+
+class Trackballer : dof {
+  public Pose p0, anchor = Pose.Identity;
+  public Trackballer(Pose p0, Pose anchor) {
+    this.p0 = p0;
+    this.anchor = anchor;
+  }
+
+
+  Vec3 pos = new Vec3(0, 0, -1);
+  Vec3 vel = new Vec3(0, 0, 0);
+
+
+  public Quat ori = Quat.Identity;
+  public void Init() {
+
+  }
+
+  // Vec3 pos, oldPos;
+  Quat qDelta = Quat.Identity;
+  public void Frame() {
+    p0 = Mono.inst.rGlove.virtualGlove;
+
+    // Vec3 vA = PullRequest.Direction(oldPos,      anchor.position);
+    // Vec3 vB = PullRequest.Direction(p0.position, anchor.position);
+
+    // if (Vec3.Distance(p0.position, oldPos) > 0.2f) { // (p0.position.v != oldPos.v) {
+    //   // Vec3 delta = (p0.position - oldPos);
+    //   float angle = Vec3.AngleBetween(vA, vB);
+    //   Console.WriteLine("angle: " + angle);
+    //   if (angle > 1f) {
+    //     Vec3 v = PullRequest.Slerp(vA, vB, 1 / angle).Normalized;
+    //     // Vec3 delta -> Quat delta
+    //     Quat a = Quat.LookDir(vA).Normalized;
+    //     Quat b = Quat.LookDir(v).Normalized;
+    //     // when converting from vec to quat, the up axis can get flipped causing issues with reliably scaling the qDelta by vector angle
+
+
+    //     qDelta = Quat.Difference(a, b).Normalized;
+
+    //     // qDelta scaled to one degree
+    //     // qDelta = Quat.Slerp(Quat.Identity, qDelta, 1 / angle).Normalized;
+
+    //     // ori = Quat.Slerp(Quat.Identity, Quat.FromAngles(1, 0, 0), 6 * Time.Elapsedf) * ori;
+
+    //     // and use the velocity magnitude
+    //     float test = MathF.Tau * 0.05f;
+    //   }
+
+    //   oldPos = p0.position;
+    // }
+    // ori = Quat.Slerp(Quat.Identity, qDelta, 60 * Time.Elapsedf).Normalized * ori;
+    // ori.Normalize();
+
+
+
+
+
+    
+    // Vec3 newPos = pos + vel * Time.Elapsedf;
+    // if (newPos.v != pos) {
+    //   newPos = PullRequest.Direction(newPos, anchor.position).Normalized * 1f;
+    //   vel = PullRequest.Direction(newPos, pos) * vel.Length;
+    //   pos = newPos;
+    // }
+
+    if (Input.Key(Key.Space).IsJustActive()) {
+      fromMouse = Input.Mouse.pos;
+    }
+
+    if (Input.Key(Key.Space).IsJustInactive()) {
+      Vec2 delta = (Input.Mouse.pos - fromMouse) / 32f;
+      vel = new Vec3(delta.x, -delta.y, 0);
+
+      pos = new Vec3(0, 0, 1);
+      // ori = (Quat.LookDir(pos).Normalized * ori).Normalized;
+    }
+
+    // ori.LookDirection(pos);
+
+    // ori = Quat.FromAngles(0, MathF.Sin(Time.Totalf) * 180, 0);
+    // float angle; Vec3 axis;
+    // ori.ToAxisAngle(out axis, out angle);
+    // ori = PullRequest.FromAxisAngle(axis, angle);
+
+
+
+
+
+    // pivot to just Quat.FromAngles(x, 0, 0) * Quat.FromAngles(0, y, 0) for the delta
+    Quat qDelta = (
+      Quat.FromAngles(-vel.y * 60 * Time.Elapsedf, 0, 0) 
+      * Quat.FromAngles(0, vel.x * 60 * Time.Elapsedf, 0) 
+      // * Quat.FromAngles(0, 0, -vel.z * 60 * Time.Elapsedf)
+    );
+    // apply the qDelta to the current orientation relative to the head orientation
+    Quat headOri = Input.Head.orientation;
+    ori = (headOri * qDelta * headOri.Inverse * ori).Normalized;
+    // ori = qDelta * ori;
+
+    
+    
+
+
+    // Lines.Add(anchor.position, pos, new Color(1, 0, 0), 0.01f);
+
+    Lines.Add(
+      anchor.position - ori * new Vec3(-1, 0, 0) * 0.1f,
+      anchor.position - ori * new Vec3( 1, 0, 0) * 0.1f,
+      new Color(1, 0, 0), 0.002f
+    );
+    Lines.Add(
+      anchor.position - ori * new Vec3( 0,-1, 0) * 0.1f,
+      anchor.position - ori * new Vec3( 0, 1, 0) * 0.1f,
+      new Color(0, 1, 0), 0.002f
+    );
+    Lines.Add(
+      anchor.position - ori * new Vec3( 0, 0,-1) * 0.1f,
+      anchor.position - ori * new Vec3( 0, 0, 1) * 0.1f,
+      new Color(0, 0, 1), 0.002f
+    );
+    Mesh.Cube.Draw(Material.Default, Matrix.TRS(anchor.position, ori, 0.04f));
+    // Mesh.Cube.Draw(Material.Default, Matrix.TS(p0.position, new Vec3(0.04f, 0.01f, 0.04f)));
+  }
+
+  Vec2 fromMouse = new Vec2(0, 0);
+
+  public float deadzone = 0.1f;
+}