const f=Math.fround; export function multiply(a,b){const r=Array(16).fill(0);for(let i=0;i<4;i++)for(let j=0;j<4;j++)for(let k=0;k<4;k++)r[i*4+j]+=a[i*4+k]*b[k*4+j];return r;} const identity=()=>[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]; export function chainPoints(config,q){let t=identity();const points=[];for(const link of config.links){t=multiply(t,link.base_matrix.flat());if(link.joint!==null){const [x,y,z]=link.axis,a=q[link.joint],c=Math.cos(a),s=Math.sin(a),v=1-c;t=multiply(t,[x*x*v+c,x*y*v-z*s,x*z*v+y*s,0,y*x*v+z*s,y*y*v+c,y*z*v-x*s,0,z*x*v-y*s,z*y*v+x*s,z*z*v+c,0,0,0,0,1]);}points.push([f(t[3]),f(t[7]),f(t[11])]);}return points;} export const fk=(config,q)=>chainPoints(config,q).at(-1); export const distance=(a,b)=>Math.hypot(...a.map((x,i)=>x-b[i])); export class Actor{ constructor(buffer){const values=new Float32Array(buffer);if(values.length!==512*29+512+512*512+512+6*512+6)throw Error('Invalid actor size');let n=0;this.parts=[512*29,512,512*512,512,6*512,6].map(size=>{const a=values.slice(n,n+size);n+=size;return a;});} predict(observation){if(observation.length!==29||!Array.from(observation).every(Number.isFinite))throw Error('Expected finite 29D observation');const [w1,b1,w2,b2,wm,bm]=this.parts;const layer=(x,w,b,rows,cols,relu)=>{const y=new Float32Array(rows);for(let i=0;i[...r,b[i]]);for(let j=0;j<3;j++){let pivot=j;for(let i=j+1;i<3;i++)if(Math.abs(a[i][j])>Math.abs(a[pivot][j]))pivot=i;[a[j],a[pivot]]=[a[pivot],a[j]];let d=a[j][j];if(Math.abs(d)<1e-12)throw Error('Singular DLS matrix');for(let k=j;k<4;k++)a[j][k]/=d;for(let i=0;i<3;i++)if(i!==j){const z=a[i][j];for(let k=j;k<4;k++)a[i][k]-=z*a[j][k];}}return a.map(r=>r[3]);} export function dls(config,o){const q=Array.from(o.slice(0,6),(v,i)=>v*config.limits[i][1]);const jac=[[],[],[]];for(let j=0;j<6;j++){const a=q.slice(),b=q.slice();a[j]+=1e-4;b[j]-=1e-4;const fa=fk(config,a),fb=fk(config,b);for(let k=0;k<3;k++)jac[k][j]=(fa[k]-fb[k])/2e-4;}const mat=Array.from({length:3},(_,i)=>Array.from({length:3},(_,j)=>jac[i].reduce((s,v,k)=>s+v*jac[j][k],0)+(i===j?.02**2:0)));const x=solve3(mat,Array.from(o.slice(11,14),(v,i)=>v-o[6+i]));return Float32Array.from({length:6},(_,i)=>Math.max(-1,Math.min(1,.8*jac.reduce((s,r,k)=>s+r[i]*x[k],0)/.05)));} export class Environment{ constructor(config,target){this.config=config;this.target=target;this.q=Float32Array.from(config.home);this.cursor=0;this.phase=0;this.steps=0;this.stagnation=0;this.errors=[];this.path=[];this.done=false;this.success=false;this.reason='Ready';} observation(){const p=fk(this.config,this.q),look=[];for(let i=0;i<5;i++)look.push(...this.target[Math.min(this.cursor+i,59)]);return Float32Array.from([...Array.from(this.q,(v,i)=>v/this.config.limits[i][1]),...p,this.cursor/60,this.phase,...this.target[this.cursor],...look]);} step(action){if(this.done)return;this.steps++;for(let i=0;i<6;i++)this.q[i]=f(Math.max(this.config.limits[i][0],Math.min(this.config.limits[i][1],f(this.q[i]+f(Math.max(-1,Math.min(1,action[i]))*.05)))));const p=fk(this.config,this.q);this.path.push(p);const dist=distance(p,this.target[this.cursor]);let error=Infinity;for(let i=Math.max(0,this.cursor-3);i.06){this.done=true;this.reason='Off path';}if(this.stagnation>150){this.done=true;this.reason='Stagnation';}}if(this.steps>=800){this.done=true;this.reason='Step limit';}if(this.done&&this.phase===1&&this.cursor===59&&distance(p,this.target[59])<.025&&this.errors.every(e=>e<=60)){this.success=true;this.reason='Strict completion';}} get rms(){return this.errors.length?Math.sqrt(this.errors.reduce((s,v)=>s+v*v,0)/this.errors.length):null;} }