"use strict";var _extension=require("../../core/extension");exports.__esModule=!0,exports.default=void 0;class CoordinateTransform{static createMatrix(a){return{data:a}}static matrixMultiply(c,a){const b=Array(c.data.length).fill(0).map(()=>Array(a.data[0].length).fill(0));for(let d=0;d<c.data.length;d++)for(let e=0;e<a.data[0].length;e++)for(let f=0;f<a.data.length;f++)b[d][e]+=c.data[d][f]*a.data[f][e];return{data:b}}static transpose(a){const b=a.data.length,c=a.data[0].length,d=Array(c).fill(0).map(()=>Array(b).fill(0));for(let e=0;e<b;e++)for(let b=0;b<c;b++)d[b][e]=a.data[e][b];return{data:d}}static svd(e){const[[f,a],[b,c]]=e.data,d=[[f*f+b*b,f*a+b*c],[f*a+b*c,a*a+c*c]],g=d[0][0]+d[1][1],h=d[0][0]*d[1][1]-d[0][1]*d[1][0],i=Math.sqrt(Math.max(0,g*g-4*h)),j=(g+i)/2,k=(g-i)/2,l=Math.sqrt(Math.max(0,j)),m=Math.sqrt(Math.max(0,k));let n,o;if(1e-10>Math.abs(d[0][1]))n=[1,0],o=[0,1];else{const a=Math.sqrt((j-d[1][1])**2+d[0][1]**2);n=[(j-d[1][1])/a,d[0][1]/a];const b=Math.sqrt((k-d[1][1])**2+d[0][1]**2);o=[(k-d[1][1])/b,d[0][1]/b]}const p=1e-10<l?[(f*n[0]+a*n[1])/l,(b*n[0]+c*n[1])/l]:[1,0],q=1e-10<m?[(f*o[0]+a*o[1])/m,(b*o[0]+c*o[1])/m]:[0,1],r=this.createMatrix([[p[0],q[0]],[p[1],q[1]]]),s=this.createMatrix([[n[0],n[1]],[o[0],o[1]]]);return{U:r,S:[l,m],Vt:s}}static calculateBidirectionalTransform(a,b){if(3!==a.length||3!==b.length)throw new Error("\u5FC5\u987B\u63D0\u4F9B\u4E09\u7EC4\u5BF9\u5E94\u70B9");const c=[a.reduce((a,b)=>a+b[0],0)/3,a.reduce((a,b)=>a+b[1],0)/3],d=[b.reduce((a,b)=>a+b[0],0)/3,b.reduce((a,b)=>a+b[1],0)/3],e=a.map(a=>[a[0]-c[0],a[1]-c[1]]),f=b.map(a=>[a[0]-d[0],a[1]-d[1]]),g=Math.sqrt(e.reduce((a,b)=>a+b[0]*b[0]+b[1]*b[1],0)/3),h=Math.sqrt(f.reduce((a,b)=>a+b[0]*b[0]+b[1]*b[1],0)/3),i=h/g,j=this.createMatrix([[e.reduce((a,b,c)=>a+b[0]*f[c][0],0),e.reduce((a,b,c)=>a+b[0]*f[c][1],0)],[e.reduce((a,b,c)=>a+b[1]*f[c][0],0),e.reduce((a,b,c)=>a+b[1]*f[c][1],0)]]),{U:k,Vt:l}=this.svd(j),m=this.matrixMultiply(this.transpose(l),this.transpose(k)),n=this.transpose(m),o=this.matrixMultiply(m,this.createMatrix([[c[0]],[c[1]]])),p=[d[0]-i*o.data[0][0],d[1]-i*o.data[1][0]],q=this.matrixMultiply(n,this.createMatrix([[d[0]],[d[1]]])),r=[c[0]-1/i*q.data[0][0],c[1]-1/i*q.data[1][0]];return function(a,b="AtoB"){if("AtoB"===b){const b=[i*a[0],i*a[1]],c=CoordinateTransform.matrixMultiply(m,CoordinateTransform.createMatrix([[b[0]],[b[1]]]));return[c.data[0][0]+p[0],c.data[1][0]+p[1]]}if("BtoA"===b){const b=[a[0]-p[0],a[1]-p[1]],c=CoordinateTransform.matrixMultiply(n,CoordinateTransform.createMatrix([[b[0]],[b[1]]]));return[1/i*c.data[0][0]+r[0],1/i*c.data[1][0]+r[1]]}throw new Error("Invalid direction. Use 'AtoB' or 'BtoA'")}}}var _default=exports.default=(0,_extension.definePlugin)("robot",{setup(a){if(!a.map_calibration)throw Error("[Robot Plugin] map_calibration \u53C2\u6570\u662F\u5FC5\u987B\u7684");if(a.map_calibration.robot.length!==a.map_calibration.three.length&&3>a.map_calibration.robot.length)throw Error("[Robot Plugin] map_calibration \u4E2D\u7684\u5750\u6807\u9700\u8981\u4E00\u81F4\uFF0C\u4E14\u4E0D\u5C11\u4E8E3\u4E2A\u5750\u6807\u70B9");let b=CoordinateTransform.calculateBidirectionalTransform(a.map_calibration.robot.map(a=>[a.x,a.y]),a.map_calibration.three.map(a=>[a.x,a.z||a.y]));return{update:(a,c)=>{let d=b([c.position.x,c.position.y]);a.model.position={x:d[0],z:d[1],y:a.model.position.y},a.model.update(c)}}},render(){},destroyed(){}});