From 1701d0537284006ecbcc03aff904fd3378e8f448 Mon Sep 17 00:00:00 2001 From: spatialfree Date: Fri, 3 Dec 2021 15:17:56 -0500 Subject: [PATCH] shoulders! --- MonoNet.cs | 29 ++++++---- Program.cs | 138 +++++++++++++++++++++++++++++++---------------- SpatialCursor.cs | 51 +++++++++++++----- 3 files changed, 146 insertions(+), 72 deletions(-) diff --git a/MonoNet.cs b/MonoNet.cs index ff1b1b7..be1e423 100644 --- a/MonoNet.cs +++ b/MonoNet.cs @@ -326,14 +326,18 @@ public class MonoNet { } } - Quat conRotDelta = con.aim.orientation * blockCon.oldConRot.Inverse; + Quat conRotDelta = (con.aim.orientation * blockCon.oldConRot.Inverse).Normalized; if (con.grip > 0.5f) { if (blockCon.index < 0) { + // loop over peer blocks as well + // disable theirs ? (id of the peer, index of block) + // wait for their block to be disabled + // recycle one of yours to replace it + for (int i = 0; i < blocks.Length; i++) { Pose blockPose = blocks[i].solid.GetPose(); Bounds bounds = new Bounds(Vec3.Zero, Vec3.One); - // Quat.Difference() !! this is useful **delta** if (blocks[i].active && bounds.Contains(blockPose.orientation.Inverse * (cursor - blockPose.position))) { blockCon.index = i; if (otherBlockCon.index == i) { @@ -342,8 +346,10 @@ public class MonoNet { // block.color = colorCube.color; // clear blockCon.spinRot = blockCon.spinDelta = Quat.Identity; + blocks[i].solid.SetAngularVelocity(Vec3.Zero); + blocks[i].solid.SetVelocity(Vec3.Zero); // set - blockCon.heldRot = Quat.Difference(con.aim.orientation, blockPose.orientation); + blockCon.heldRot = (con.aim.orientation.Inverse * blockPose.orientation).Normalized; blockCon.offset = blockPose.orientation.Inverse * (blockPose.position - cursor); // @@ -353,26 +359,26 @@ public class MonoNet { } if (blockCon.index >= 0) { - Quat newRot = con.aim.orientation * blockCon.heldRot * blockCon.spinRot; + Quat newRot = (con.aim.orientation * blockCon.heldRot * blockCon.spinRot).Normalized; // trackballer if (con.trigger > 0.75f) { blockCon.spinDelta = Quat.Slerp( - blockCon.spinDelta, - ((newRot.Inverse * conRotDelta) * newRot).Normalized, + blockCon.spinDelta.Normalized, + (newRot.Inverse * conRotDelta * newRot).Normalized, Time.Elapsedf / 0.1f ); } blockCon.spinRot *= blockCon.spinDelta; - Quat toRot = con.aim.orientation * blockCon.heldRot * blockCon.spinRot; - Vec3 toPos = cursor + con.aim.orientation * blockCon.heldRot * blockCon.spinRot * blockCon.offset; + Quat toRot = (con.aim.orientation * blockCon.heldRot * blockCon.spinRot).Normalized; + Vec3 toPos = cursor + (con.aim.orientation * blockCon.heldRot * blockCon.spinRot).Normalized * blockCon.offset; // cursor - blockCon.offset; blocks[blockCon.index].solid.Move(toPos, toRot); Quat newHeldRot = blocks[blockCon.index].solid.GetPose().orientation; - blockCon.angularMomentum = Vec3.Lerp(blockCon.angularMomentum, AngularDisplacement(Quat.Difference(blockCon.oldHeldRot, newHeldRot)), Time.Elapsedf / 0.1f); + blockCon.angularMomentum = Vec3.Lerp(blockCon.angularMomentum, AngularDisplacement((newHeldRot * blockCon.oldHeldRot.Inverse).Normalized), Time.Elapsedf / 0.1f); blockCon.oldHeldRot = newHeldRot; - blockCon.delta = (cursor + con.aim.orientation * blockCon.heldRot * blockCon.spinRot * blockCon.offset) - blocks[blockCon.index].solid.GetPose().position; + blockCon.delta = (cursor + (con.aim.orientation * blockCon.heldRot * blockCon.spinRot).Normalized * blockCon.offset) - blocks[blockCon.index].solid.GetPose().position; blockCon.momentum = Vec3.Lerp(blockCon.momentum, blockCon.delta, Time.Elapsedf / 0.1f); } } else { @@ -390,7 +396,8 @@ public class MonoNet { float angleInDegrees; Vec3 rotationAxis; ToAngleAxis(rotDelta, out angleInDegrees, out rotationAxis); - return rotationAxis * angleInDegrees * (float)(Math.PI / 180); + return rotationAxis * angleInDegrees; + // (float)(Math.PI / 180); } public void ToAngleAxis(Quat q1, out float angle, out Vec3 axis) { diff --git a/Program.cs b/Program.cs index 7cbe378..f41b9fb 100644 --- a/Program.cs +++ b/Program.cs @@ -19,8 +19,8 @@ class Program { public class Mono { public Mic mic; public Controller domCon, subCon; public bool lefty; - public float movePress; - public Vec3 dragStart, posStart; + + public Vec3 domDragStart, subDragStart; public float railT; Mesh ball = Default.MeshSphere; @@ -58,70 +58,114 @@ public class Mono { SpatialCursor cursor = new ReachCursor(); SpatialCursor subCursor = new ReachCursor(); + Tex camTex = new Tex(TexType.Rendertarget); + camTex.SetSize(600, 400); + Material camMat = new Material(Shader.Unlit); + camMat.SetTexture("diffuse", camTex); + Mesh quad = Default.MeshQuad; + while (SK.Step(() => { if (lefty) { domCon = Input.Controller(Handed.Left); subCon = Input.Controller(Handed.Right); } else { domCon = Input.Controller(Handed.Right); subCon = Input.Controller(Handed.Left); } - // if (subCon.IsX2JustPressed) { lefty = !lefty; } + // if (subCon.IsX2JustPressed) { lefty = !lefty; } // ball.Draw(ballMat, Matrix.TS(pos, 0.1f)); // SpatialCursor cursor = cursors.Step(domCon.aim, subCon.aim); - cursor.Step(new Pose[] { domCon.aim, Input.Head }); - if (domCon.IsStickJustClicked) { + + // Shoulders + Vec3 headPos = Input.Head.position + Input.Head.Forward * -0.15f; + Vec3 toSub = (subCon.aim.position.X0Z - headPos.X0Z).Normalized; + Vec3 toDom = (domCon.aim.position.X0Z - headPos.X0Z).Normalized; + Vec3 middl = (toSub + toDom).Normalized; + + if (Vec3.Dot(middl, Input.Head.Forward) < 0) { + middl = -middl; + } + + // Lines.Add(headPos.X0Z, headPos.X0Z + toSub.X0Z, Color.White, 0.005f); + // Lines.Add(headPos.X0Z, headPos.X0Z + toDom.X0Z, Color.White, 0.005f); + // Lines.Add(headPos.X0Z, headPos.X0Z + middl.X0Z, Color.White, 0.005f); + + // cube.Draw(mat, Matrix.TRS(headPos, Input.Head.orientation, new Vec3(0.3f, 0.3f, 0.3f))); + + Vec3 rShoulder = headPos + Quat.LookDir(middl) * new Vec3(0.2f, -0.2f, 0); + Vec3 lShoulder = headPos + Quat.LookDir(middl) * new Vec3(-0.2f, -0.2f, 0); + // cube.Draw(mat, Matrix.TRS(headPos, Input.Head.orientation, new Vec3(0.25f, 0.3f, 0.3f)), new Color(1,0,0)); + 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); + + + cursor.Step(new Pose[] { domCon.aim, new Pose(rShoulder, Quat.LookDir(middl)) }, ((Input.Controller(Handed.Right).stick.y + 1) / 2)); + if (domCon.trigger > 0.5f) { cursor.Calibrate(); } - subCursor.Step(new Pose[] { subCon.aim, Input.Head }); - if (subCon.IsStickJustClicked) { + subCursor.Step(new Pose[] { subCon.aim, new Pose(lShoulder, Quat.LookDir(middl)) }, ((Input.Controller(Handed.Left).stick.y + 1) / 2)); + if (subCon.trigger > 0.5f) { subCursor.Calibrate(); } cursor.p1 = subCursor.p0; // override *later change all one handed cursors to be dual wielded by default* - // fullstick + + // FULLSTICK // 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; - Vec3[] rail = new Vec3[] { - new Vec3(0, 0, -1), - new Vec3(0, 0, -2), - new Vec3(1, 2, -3), - new Vec3(0, 1, -4), - }; + // CUBIC BEZIER RAIL + // 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 = Vec3.Lerp(pos, Bezier.Sample(rail, railT) - (subCon.aim.position - pos), Time.Elapsedf * 6f); - railT += Time.Elapsedf * 0.1f; - // how to reliably determine and control which direction to go? (velocity) - } + // 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 = Vec3.Lerp(pos, Bezier.Sample(rail, railT) - (subCon.aim.position - pos), Time.Elapsedf * 6f); + // railT += Time.Elapsedf * 0.1f; + // // how to reliably determine and control which direction to go? (velocity) + // } // Console.WriteLine(World.RefreshInterval.ToString()); - Vec3 p00 = domCon.aim.position; + // DRAG DRIFT + Vec3 domPos = domCon.aim.position; if (domCon.IsX1JustPressed) { // movePress = Time.Totalf; - dragStart = p00; + domDragStart = domPos; } if (domCon.IsX1Pressed) { - vel = -((p00 - dragStart) / Time.Elapsedf); - dragStart = p00; + vel += -(domPos - domDragStart) * 6; + domDragStart = domPos; } + + Vec3 subPos = subCon.aim.position; + if (subCon.IsX1JustPressed) { + // movePress = Time.Totalf; + subDragStart = subPos; + } + if (subCon.IsX1Pressed) { + vel += -(subPos - subDragStart) * 6; + subDragStart = subPos; + } + // if (domCon.IsX1JustUnPressed && Time.Totalf - movePress < 0.2f) { // pos = p00 - (Input.Head.position - pos); // } @@ -137,10 +181,9 @@ public class Mono { float preZ = pos.z; pos.z = Math.Clamp(pos.z, -16f, 16f); if (pos.z != preZ) { vel.z = 0; } Renderer.CameraRoot = Matrix.T(pos); - float friction = 1 - Math.Clamp(vel.Magnitude / Time.Elapsedf / 45f, 0f, 1f); - friction = friction * friction * friction; - vel = Vec3.Lerp(Vec3.Zero, vel, 1 - (friction * Time.Elapsedf)); + vel *= 1 - Time.Elapsedf; + // COLOR CUBE // reveal when palm up float reveal = subCon.pose.Right.y * 2; colorCube.size = colorCube.ogSize * Math.Clamp(reveal, 0, 1); @@ -159,11 +202,6 @@ public class Mono { } oldSubPos = subCon.pose.position; - // how to extend the buttons!!! as we only have 2 T-T - - - cube.Draw(mat, floor.GetPose().ToMatrix(floorScale)); - // for (int i = 0; i < net.me.blocks.Length; i++) { // cube.Draw(mat, net.me.blocks[i].solid.GetPose().ToMatrix(), net.me.blocks[i].color); // } @@ -190,6 +228,12 @@ public class Mono { // Default.MaterialHand["color"] = cube.color; // cursor.Draw(Matrix.S(0.1f)); + + cube.Draw(mat, floor.GetPose().ToMatrix(floorScale), Color.White * 0.666f); + + + // Renderer.RenderTo(camTex, Matrix.TR(Input.Head.position + Vec3.Up * 10, Quat.FromAngles(-90f, 0, 0)), Matrix.Orthographic(2f, 2f, 0.1f, 100f), RenderLayer.All, RenderClear.All); + // quad.Draw(camMat, Matrix.TR(Input.Head.Forward, Quat.FromAngles(0, 180, 0))); })) ; SK.Shutdown(); } diff --git a/SpatialCursor.cs b/SpatialCursor.cs index 80ac85f..7183dd1 100644 --- a/SpatialCursor.cs +++ b/SpatialCursor.cs @@ -7,7 +7,7 @@ public abstract class SpatialCursor { public Model model = Model.FromFile("cursor.glb", Shader.Default); - public abstract void Step(Pose[] poses); + public abstract void Step(Pose[] poses, float scalar); public abstract void Calibrate(); } @@ -21,7 +21,7 @@ public class Cursors { public SpatialCursor Step(Pose domHand, Pose subHand) { SpatialCursor cursor = oneHanded[oneIndex]; - cursor.Step(new Pose[] { domHand, subHand }); + cursor.Step(new Pose[] { domHand, subHand }, 0); return cursor; } } @@ -32,7 +32,7 @@ public class StretchCursor : SpatialCursor { this.str = 3f; this.max = 10f; } - public override void Step(Pose[] poses) { + public override void Step(Pose[] poses, float scalar) { Pose dom = poses[0]; Pose sub = poses[1]; float stretch = (sub.position - dom.position).Magnitude; @@ -53,18 +53,41 @@ public class ReachCursor : SpatialCursor { } Vec3 pos; Vec3 origin; - public override void Step(Pose[] poses) { + Pose shoulder; + // Vec3 yaw; + public override void Step(Pose[] poses, float scalar) { pos = poses[0].position; - float stretch = Vec3.Distance(origin, pos); - Vec3 dir = (pos - origin).Normalized; + shoulder = poses[1]; + // just the yaw of the head Quaternion + // yaw = Input.Head.Forward; yaw.y = 0; yaw = yaw.Normalized; + // Quat q = Quat.LookDir(yaw); + Vec3 from = (shoulder.orientation * origin) + shoulder.position; + + str = min + (scalar * max); + + float stretch = Vec3.Distance(from, pos); + Vec3 dir = (pos - from).Normalized; p0 = pos + dir * stretch * str; model.Draw(Matrix.TS(p0, 0.06f)); - Lines.Add(origin, p0, Color.White, 0.01f); - model.Draw(Matrix.TS(origin, 0.04f)); + model.Draw(Matrix.TS(shoulder.position, 0.06f)); + Lines.Add(from, p0, Color.White, 0.005f); + + // model.Draw(Matrix.TS(from, 0.04f)); + // Pose mainHand = poses[0]; + // Pose offHand = poses[1]; + + // Vec2 mid = Vec2.Lerp(lHand.position.XZ, rHand.position.XZ, 0.5f); + + Lines.Add(from, p0, Color.White, 0.005f); + + // Vec3 calib = shoulder.orientation.Inverse * (pos - shoulder.position); + // if (calib.z > origin.z) { + // Calibrate(); + // } } public override void Calibrate() { - origin = pos; + origin = shoulder.orientation.Inverse * (pos - shoulder.position); } } @@ -74,7 +97,7 @@ public class TwistCursor : SpatialCursor { this.str = 3f; this.max = 10f; } - public override void Step(Pose[] poses) { + public override void Step(Pose[] poses, float scalar) { Vec3 pos = poses[0].position; Quat quat = poses[0].orientation; Quat rel = Quat.LookAt(Vec3.Zero, quat * Vec3.Forward); @@ -94,11 +117,11 @@ public class CubicFlow : SpatialCursor { } TwistCursor domTwist = new TwistCursor(); TwistCursor subTwist = new TwistCursor(); - public override void Step(Pose[] poses) { + public override void Step(Pose[] poses, float scalar) { Pose dom = poses[0]; Pose sub = poses[1]; - domTwist.Step(new Pose[] { dom }); - subTwist.Step(new Pose[] { sub }); + domTwist.Step(new Pose[] { dom }, scalar); + subTwist.Step(new Pose[] { sub }, scalar); p0 = dom.position; p1 = domTwist.p0; @@ -123,7 +146,7 @@ public class SupineCursor : SpatialCursor { float calibStr; Quat calibQuat; Pose dom, sub; - public override void Step(Pose[] poses) { + public override void Step(Pose[] poses, float scalar) { dom = poses[0]; sub = poses[1];