Swift
Add the motion math that turns the case into a pointer.
Andy Gaoe8c871f15329
diff --git a/ClosedCase/Orientation.js b/ClosedCase/Orientation.jsnew file mode 100644index 0000000..cc3ae95--- /dev/null+++ b/ClosedCase/Orientation.js@@ -0,0 +1,139 @@+// Raw body-to-reference orientation for the closed-case pointer, and the grip that maps it onto the screen.+export const dot=(a,b)=>a.reduce((sum,x,i)=>sum+x*b[i],0);+export const norm=a=>Math.sqrt(dot(a,a));+export const unit=a=>a.map(x=>x/norm(a));+export const cross=(a,b)=>[a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]];+export const inverse=q=>[q[0],-q[1],-q[2],-q[3]];+export function multiply(a,b){return [+ a[0]*b[0]-a[1]*b[1]-a[2]*b[2]-a[3]*b[3],+ a[0]*b[1]+a[1]*b[0]+a[2]*b[3]-a[3]*b[2],+ a[0]*b[2]-a[1]*b[3]+a[2]*b[0]+a[3]*b[1],+ a[0]*b[3]+a[1]*b[2]-a[2]*b[1]+a[3]*b[0]];}+export const rotate=(q,v)=>multiply(multiply(q,[0,...v]),inverse(q)).slice(1);+export function rotation(q){+ if(q[0]<0)q=q.map(x=>-x);+ const n=norm(q.slice(1));return n<1e-10?[0,0,0]:q.slice(1).map(x=>x*2*Math.atan2(n,q[0])/n);+}++// Still stretches measure the gyro's resting offset. The first, before any offset is known, allows a+// hand-held hold; later ones keep a warm sensor's offset current, so they must be steadier.+const firstStill={span:1.2,gyro:.8,accel:.03},steadyStill={span:1.8,gyro:.35,accel:.015};++export class Orientation {+ // A saved offset barely moves between connections, so it is used in any pose without a wait.+ constructor(seed=null){+ const valid=a=>Array.isArray(a)&&a.length===3&&a.every(Number.isFinite);+ this.seed=seed&&valid(seed.bias)&&norm(seed.bias)<5?{bias:[...seed.bias]}:null;+ this.measured=0;this.reset();+ }+ reset(){+ this.q=[1,0,0,0];this.bias=[0,0,0];this.accel=null;this.referenceGravity=null;+ this.last={};this.window=[];this.ready=false;this.seedUsed=false;this.generation=(this.generation||0)+1;+ }+ settling(){+ const span=firstStill.span;+ if(this.ready)return {seconds:span,span,progress:1,message:'Ready'};+ if(this.window.length<2)return {seconds:0,span,progress:0,message:'Set the case down'};+ const mean=key=>[0,1,2].map(i=>this.window.reduce((sum,x)=>sum+x[key][i],0)/this.window.length);+ const bias=mean('vector'),gravity=mean('g');+ const spread=(key,center)=>Math.sqrt(this.window.reduce((sum,x)=>sum+norm(x[key].map((v,i)=>v-center[i]))**2,0)/this.window.length);+ const stable=spread('vector',bias)<=firstStill.gyro&&spread('g',gravity)<=firstStill.accel;+ const seconds=stable?Math.min(span,(this.window.at(-1).stamp-this.window[0].stamp)*1e-6):0;+ return {seconds,span,progress:seconds/span,message:stable?'Keep it still':'Movement detected — keep it still'};+ }+ // The mean of a still stretch is the gyro's resting offset; any movement starts the stretch over.+ still(stamp,vector,g,magnitude,limits){+ if(magnitude<.85||magnitude>1.15||norm(vector)>5){this.window=[];return null;}+ this.window.push({stamp,vector,g});+ while(this.window.length>1&&stamp-this.window[0].stamp>(limits.span+.4)*1e6)this.window.shift();+ if(stamp-this.window[0].stamp<limits.span*1e6)return null;+ const mean=key=>[0,1,2].map(i=>this.window.reduce((s,x)=>s+x[key][i],0)/this.window.length);+ const bias=mean('vector'),gravity=mean('g');+ const spread=(key,center)=>Math.sqrt(this.window.reduce((s,x)=>s+norm(x[key].map((v,i)=>v-center[i]))**2,0)/this.window.length);+ if(spread('vector',bias)>limits.gyro||spread('g',gravity)>limits.accel)return null;+ this.window=[];return {bias,gravity};+ }+ feed(row){+ const [,service,stamp,host,...raw]=row;+ if(![11,12].includes(service)||!Number.isFinite(stamp)||stamp<=0||+ !Number.isFinite(host)||raw.length!==3||!raw.every(Number.isFinite))return false;+ const previous=this.last[service];if(previous&&stamp<=previous.stamp)return false;+ const dt=previous?(stamp-previous.stamp)*1e-6:0;+ // A drop under two seconds holds the last pose rather than integrating across it; a longer one starts over.+ if(previous&&dt>.2){if(this.ready&&dt<2)this.last[service]={stamp,host};else this.reset();return false;}+ this.last[service]={stamp,host};+ const vector=raw.map(x=>x*1e-5);+ if(service===11){this.accel={vector,stamp};return false;}+ if(!this.accel||Math.abs(stamp-this.accel.stamp)>100000)return false;+ const g=this.accel.vector, magnitude=norm(g);+ if(!this.ready){+ // The reference pose comes from the first calm sample, so the wait is only for a first offset.+ if(this.seed&&magnitude>.95&&magnitude<1.05&&norm(vector.map((v,i)=>v-this.seed.bias[i]))<20){+ this.bias=[...this.seed.bias];this.referenceGravity=unit(g);this.ready=true;this.seedUsed=true;this.window=[];return true;+ }+ const still=this.still(stamp,vector,g,magnitude,firstStill);+ if(!still)return false;+ this.bias=still.bias;this.referenceGravity=unit(still.gravity);this.seed={bias:still.bias};this.measured++;this.ready=true;+ return true;+ }+ // A steadier stretch moves the offset halfway, at most 0.1°/s, so a slow steady turn can't pass for it.+ const still=this.still(stamp,vector,g,magnitude,steadyStill);+ if(still){+ const step=still.bias.map((x,i)=>x-this.bias[i]),k=Math.min(.5,.1/Math.max(norm(step),1e-9));+ this.bias=this.bias.map((x,i)=>x+step[i]*k);this.seed={bias:[...this.bias]};this.measured++;+ }+ if(dt<=0)return false;+ const omega=vector.map((x,i)=>(x-this.bias[i])*Math.PI/180);+ const advance=rate=>{+ const speed=norm(rate),angle=speed*dt;+ const delta=speed<1e-10?[1,0,0,0]:[Math.cos(angle/2),...rate.map(x=>x/speed*Math.sin(angle/2))];+ this.q=unit(multiply(this.q,delta));+ };+ advance(omega);+ if(magnitude>.85&&magnitude<1.15){+ const expected=rotate(inverse(this.q),this.referenceGravity);+ const correction=cross(unit(g),expected);+ advance(correction.map(x=>x*.5));+ }+ return true;+ }+}++export class Grip {+ // Edges saved from an earlier calibration still hold in the case's own frame, so after a reconnect+ // only the center is new. Gravity in that frame tells whether the case is held the same way.+ constructor(saved=null){+ this.center=null;this.right=null;this.up=null;this.gravity=null;+ const valid=(a,low,high)=>Array.isArray(a)&&a.length===3&&a.every(Number.isFinite)&&norm(a)>=low&&norm(a)<=high;+ const edge=a=>valid(a,5*Math.PI/180,80*Math.PI/180);+ if(saved&&edge(saved.right)&&edge(saved.up)&&valid(saved.gravity,.99,1.01)&&+ Math.abs(dot(saved.right,saved.up)/(norm(saved.right)*norm(saved.up)))<=.88){+ this.right=[...saved.right];this.up=[...saved.up];this.gravity=[...saved.gravity];+ }+ }+ get edges(){return this.right&&this.up&&this.gravity?{right:this.right,up:this.up,gravity:this.gravity}:null;}+ relative(q){return rotation(multiply(inverse(this.center),q));}+ setCenter(q,gravity=null){this.center=[...q];this.right=this.up=null;this.gravity=gravity?unit(rotate(inverse(q),gravity)):null;}+ // A new center for saved edges. False when the case is held differently, and the edges need aiming again.+ // A connection that points by itself asks to keep them; Recalibrate is there if the aim is off.+ recenter(q,gravity,keep=false){+ const held=unit(rotate(inverse(q),gravity)),kept=this.edges&&(keep||dot(held,this.gravity)>=Math.cos(35*Math.PI/180));+ this.center=[...q];if(!kept){this.right=this.up=null;this.gravity=held;}+ return !!kept;+ }+ setAxis(name,q){+ if(!this.center)throw Error('Set the center first.');+ const axis=this.relative(q),degrees=norm(axis)*180/Math.PI;+ if(degrees<5||degrees>80)throw Error('Aim about 10–30° away from center, then try again.');+ const other=name==='right'?this.up:this.right;+ if(other&&Math.abs(dot(axis,other)/(norm(axis)*norm(other)))>.88)+ throw Error('Aim in a different direction. Return to center, then aim toward the top.');+ this[name]=axis;+ }+ point(q){+ if(!this.center||!this.right||!this.up)return null;+ const v=this.relative(q),r=this.right,u=this.up,rr=dot(r,r),uu=dot(u,u),ru=dot(r,u),d=rr*uu-ru*ru;+ const x=(dot(v,r)*uu-dot(v,u)*ru)/d,y=(dot(v,u)*rr-dot(v,r)*ru)/d;+ return [Math.max(0,Math.min(1,.5+x/2)),Math.max(0,Math.min(1,.5-y/2))];+ }+}diff --git a/MotionLearning.swift b/MotionLearning.swiftnew file mode 100644index 0000000..506f70b--- /dev/null+++ b/MotionLearning.swift@@ -0,0 +1,45 @@+import Foundation++struct V3 {+ var x: Double; var y: Double; var z: Double+ static let zero = V3(x: 0, y: 0, z: 0)+ var length: Double { sqrt(dot(self)) }+ func dot(_ b: V3) -> Double { x*b.x + y*b.y + z*b.z }+ static func * (a: V3, b: Double) -> V3 { V3(x: a.x*b, y: a.y*b, z: a.z*b) }+}+struct Pose {+ var w: Double; var x: Double; var y: Double; var z: Double+ init(w: Double, x: Double, y: Double, z: Double) { self.w=w; self.x=x; self.y=y; self.z=z }+ var inverse: Pose { Pose(w:w, x:-x, y:-y, z:-z) }+ func dot(_ b: Pose) -> Double { w*b.w + x*b.x + y*b.y + z*b.z }+ func multiplied(by b: Pose) -> Pose {+ Pose(w:w*b.w-x*b.x-y*b.y-z*b.z, x:w*b.x+x*b.w+y*b.z-z*b.y,+ y:w*b.y-x*b.z+y*b.w+z*b.x, z:w*b.z+x*b.y-y*b.x+z*b.w)+ }+ func rotation(from center: Pose) -> V3 {+ var q = center.inverse.multiplied(by: self)+ if q.w < 0 { q = Pose(w:-q.w, x:-q.x, y:-q.y, z:-q.z) }+ let s = sqrt(q.x*q.x+q.y*q.y+q.z*q.z)+ guard s > 1e-8 else { return .zero }+ return V3(x:q.x, y:q.y, z:q.z) * (2*atan2(s, max(0, q.w))/s)+ }+ static func mean(_ poses: [Pose]) -> Pose? {+ guard let first = poses.first else { return nil }+ var total = [0.0, 0.0, 0.0, 0.0]+ for p in poses {+ let sign = p.dot(first) < 0 ? -1.0 : 1.0+ total[0] += sign*p.w; total[1] += sign*p.x; total[2] += sign*p.y; total[3] += sign*p.z+ }+ let n = sqrt(total.reduce(0) { $0 + $1*$1 })+ guard n > 1e-8 else { return nil }+ return Pose(w:total[0]/n, x:total[1]/n, y:total[2]/n, z:total[3]/n)+ }+}+enum LessonError: LocalizedError {+ case message(String)+ var errorDescription: String? { if case .message(let text)=self { return text }; return nil }+}+struct MotionFrame {+ let t: Double+ let pose: Pose+}