cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

perf: IK求解器姿态路径提速1.41×——轴夹角预筛+Set去重+免hypot，并修正牛顿收口取错行的列下标bug

Browse Source 
- 4.3牛顿收口 err2 原取 [M5[6..8]]（行主序第2行）当"列2工具轴"，正确应为列2
  [M[2],M[5],M[8]]；该节因此从未起作用。修复后 selfTest 姿态命中 5/6→6/6、
  最差 oerr 0.602°→0.024°
- scoreBranch 轴夹角预筛：θ6 只滚转不改工具轴 ⇒ oerr≥轴夹角（20万样本零违反），
  轴夹角就超 POSETOL 的候选免 orient 全矩阵+acos 直接拒
- keepBand.indexOf → Set.has（原 O(n²) 约6000×4000恒等比较/次）；evalAt 误差范数
  hypot→平方和
- A/B 40 组随机位形输出与合入前逐位一致（Δθ=0.000°、nSol 一致、oerr 零变差），
  node 六项回归全绿，浏览器冒烟已到达/残差6.4e-14mm/零报错
This commit is contained in:
cbb
2026-09-29 00:10:12 +08:00
parent a70724e8a9
commit 4fdc37da9f
5 changed files with 108 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
web/arm-ik-solver.js
+27 -6
View File
@@ -158,6 +158,18 @@ function make(cfg) {
for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
return M;
}
/* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转，
只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */
const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
function axisCol(q) {
let v;
if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]);
else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]);
v = mv3(rotAxis(D.J3, q[2] * D2R), v);
v = mv3(rotAxis(D.J2, q[1] * D2R), v);
v = mv3(rotAxis(D.J1, q[0] * D2R), v);
return v;
}
/* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
function fk(q, tool) {
@@ -242,7 +254,7 @@ function make(cfg) {
const A = target[0] - K.X0;
const BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
const tol = opt.tol || 1e-6;
const tol = opt.tol || 1e-6, tol2 = tol * tol;
const KEEP = 4000;
/* 参考位姿连续项：交互求解时以当前姿态为 ref，代价加入 lam·Σwrap(θᵢ-refᵢ)²。
等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优会随目标微动
@@ -259,6 +271,11 @@ function make(cfg) {
const POSEFINE = 0.05; // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
let poseW = 200.0; // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
// （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
/* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角
（20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒，
先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */
const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null;
const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL)
const closeRoll = (q) => { // 给定位置解，解 θ6 并返回姿态偏差（度）
const M5 = orient(q, 5);
const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6)
@@ -271,6 +288,8 @@ function make(cfg) {
const scoreBranch = (q, e) => { // 对一条位置精确支打分（位置误差 e 已由闭式保证）
let oerr = 0;
if (POSE) {
const w = axisCol(q);
if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null;
const cl = closeRoll(q);
if (cl.oerr > POSETOL) return null;
q = cl.q; oerr = cl.oerr;
@@ -288,9 +307,10 @@ function make(cfg) {
const q = sols[k];
if (!inLimit(q)) continue;
const p = fk(q, tool);
const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
if (e > tol) continue;
const c = scoreBranch(q, e);
const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
const e2 = dx * dx + dy * dy + dz * dz;
if (e2 > tol2) continue;
const c = scoreBranch(q, Math.sqrt(e2));
if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
}
return best;
@@ -328,7 +348,8 @@ function make(cfg) {
if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优）
band.sort((x, y) => x.oerr - y.oerr);
const keepBand = band.slice(0, KEEP);
const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
const keepSet = new Set(keepBand);
const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
all = keepBand.concat(rest);
trimmed = band.length + rest.length < nSol;
} else all.sort((x, y) => x.rcost - y.rcost);
@@ -406,7 +427,7 @@ function make(cfg) {
const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0]));
const v = cross3(A0, u);
const err2 = q => {
const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序：列 2 = 工具轴
const w = axisCol(q); // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行）
const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2];
return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];
web/index.html
+27 -6
View File
@@ -1374,6 +1374,18 @@ function make(cfg) {
for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
return M;
}
/* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转，
只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */
const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
function axisCol(q) {
let v;
if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]);
else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]);
v = mv3(rotAxis(D.J3, q[2] * D2R), v);
v = mv3(rotAxis(D.J2, q[1] * D2R), v);
v = mv3(rotAxis(D.J1, q[0] * D2R), v);
return v;
}
/* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
function fk(q, tool) {
@@ -1458,7 +1470,7 @@ function make(cfg) {
const A = target[0] - K.X0;
const BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
const tol = opt.tol || 1e-6;
const tol = opt.tol || 1e-6, tol2 = tol * tol;
const KEEP = 4000;
/* 参考位姿连续项：交互求解时以当前姿态为 ref，代价加入 lam·Σwrap(θᵢ-refᵢ)²。
等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优会随目标微动
@@ -1475,6 +1487,11 @@ function make(cfg) {
const POSEFINE = 0.05; // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
let poseW = 200.0; // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
// （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
/* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角
（20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒，
先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */
const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null;
const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL)
const closeRoll = (q) => { // 给定位置解，解 θ6 并返回姿态偏差（度）
const M5 = orient(q, 5);
const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6)
@@ -1487,6 +1504,8 @@ function make(cfg) {
const scoreBranch = (q, e) => { // 对一条位置精确支打分（位置误差 e 已由闭式保证）
let oerr = 0;
if (POSE) {
const w = axisCol(q);
if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null;
const cl = closeRoll(q);
if (cl.oerr > POSETOL) return null;
q = cl.q; oerr = cl.oerr;
@@ -1504,9 +1523,10 @@ function make(cfg) {
const q = sols[k];
if (!inLimit(q)) continue;
const p = fk(q, tool);
const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
if (e > tol) continue;
const c = scoreBranch(q, e);
const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
const e2 = dx * dx + dy * dy + dz * dz;
if (e2 > tol2) continue;
const c = scoreBranch(q, Math.sqrt(e2));
if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
}
return best;
@@ -1544,7 +1564,8 @@ function make(cfg) {
if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优）
band.sort((x, y) => x.oerr - y.oerr);
const keepBand = band.slice(0, KEEP);
const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
const keepSet = new Set(keepBand);
const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
all = keepBand.concat(rest);
trimmed = band.length + rest.length < nSol;
} else all.sort((x, y) => x.rcost - y.rcost);
@@ -1622,7 +1643,7 @@ function make(cfg) {
const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0]));
const v = cross3(A0, u);
const err2 = q => {
const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序：列 2 = 工具轴
const w = axisCol(q); // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行）
const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2];
return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];
web/机器人手臂3D查看器.html
+27 -6
View File
@@ -1374,6 +1374,18 @@ function make(cfg) {
for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
return M;
}
/* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转，
只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */
const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
function axisCol(q) {
let v;
if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]);
else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]);
v = mv3(rotAxis(D.J3, q[2] * D2R), v);
v = mv3(rotAxis(D.J2, q[1] * D2R), v);
v = mv3(rotAxis(D.J1, q[0] * D2R), v);
return v;
}
/* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
function fk(q, tool) {
@@ -1458,7 +1470,7 @@ function make(cfg) {
const A = target[0] - K.X0;
const BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
const tol = opt.tol || 1e-6;
const tol = opt.tol || 1e-6, tol2 = tol * tol;
const KEEP = 4000;
/* 参考位姿连续项：交互求解时以当前姿态为 ref，代价加入 lam·Σwrap(θᵢ-refᵢ)²。
等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优会随目标微动
@@ -1475,6 +1487,11 @@ function make(cfg) {
const POSEFINE = 0.05; // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
let poseW = 200.0; // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
// （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
/* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角
（20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒，
先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */
const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null;
const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL)
const closeRoll = (q) => { // 给定位置解，解 θ6 并返回姿态偏差（度）
const M5 = orient(q, 5);
const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6)
@@ -1487,6 +1504,8 @@ function make(cfg) {
const scoreBranch = (q, e) => { // 对一条位置精确支打分（位置误差 e 已由闭式保证）
let oerr = 0;
if (POSE) {
const w = axisCol(q);
if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null;
const cl = closeRoll(q);
if (cl.oerr > POSETOL) return null;
q = cl.q; oerr = cl.oerr;
@@ -1504,9 +1523,10 @@ function make(cfg) {
const q = sols[k];
if (!inLimit(q)) continue;
const p = fk(q, tool);
const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
if (e > tol) continue;
const c = scoreBranch(q, e);
const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
const e2 = dx * dx + dy * dy + dz * dz;
if (e2 > tol2) continue;
const c = scoreBranch(q, Math.sqrt(e2));
if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
}
return best;
@@ -1544,7 +1564,8 @@ function make(cfg) {
if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优）
band.sort((x, y) => x.oerr - y.oerr);
const keepBand = band.slice(0, KEEP);
const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
const keepSet = new Set(keepBand);
const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
all = keepBand.concat(rest);
trimmed = band.length + rest.length < nSol;
} else all.sort((x, y) => x.rcost - y.rcost);
@@ -1622,7 +1643,7 @@ function make(cfg) {
const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0]));
const v = cross3(A0, u);
const err2 = q => {
const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序：列 2 = 工具轴
const w = axisCol(q); // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行）
const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2];
return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];
定位图/IK姿态控制_fast合并冒烟.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 123 KiB

机器人手臂3D查看器.html
+27 -6
View File
@@ -1374,6 +1374,18 @@ function make(cfg) {
for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
return M;
}
/* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转，
只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */
const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
function axisCol(q) {
let v;
if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]);
else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]);
v = mv3(rotAxis(D.J3, q[2] * D2R), v);
v = mv3(rotAxis(D.J2, q[1] * D2R), v);
v = mv3(rotAxis(D.J1, q[0] * D2R), v);
return v;
}
/* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
function fk(q, tool) {
@@ -1458,7 +1470,7 @@ function make(cfg) {
const A = target[0] - K.X0;
const BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
const tol = opt.tol || 1e-6;
const tol = opt.tol || 1e-6, tol2 = tol * tol;
const KEEP = 4000;
/* 参考位姿连续项：交互求解时以当前姿态为 ref，代价加入 lam·Σwrap(θᵢ-refᵢ)²。
等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优会随目标微动
@@ -1475,6 +1487,11 @@ function make(cfg) {
const POSEFINE = 0.05; // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
let poseW = 200.0; // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
// （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
/* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角
（20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒，
先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */
const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null;
const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL)
const closeRoll = (q) => { // 给定位置解，解 θ6 并返回姿态偏差（度）
const M5 = orient(q, 5);
const N = mul3(T3(M5), POSE); // 需满足 N = Rz(−θ6)
@@ -1487,6 +1504,8 @@ function make(cfg) {
const scoreBranch = (q, e) => { // 对一条位置精确支打分（位置误差 e 已由闭式保证）
let oerr = 0;
if (POSE) {
const w = axisCol(q);
if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > AXREJ) return null;
const cl = closeRoll(q);
if (cl.oerr > POSETOL) return null;
q = cl.q; oerr = cl.oerr;
@@ -1504,9 +1523,10 @@ function make(cfg) {
const q = sols[k];
if (!inLimit(q)) continue;
const p = fk(q, tool);
const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
if (e > tol) continue;
const c = scoreBranch(q, e);
const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
const e2 = dx * dx + dy * dy + dz * dz;
if (e2 > tol2) continue;
const c = scoreBranch(q, Math.sqrt(e2));
if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
}
return best;
@@ -1544,7 +1564,8 @@ function make(cfg) {
if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优）
band.sort((x, y) => x.oerr - y.oerr);
const keepBand = band.slice(0, KEEP);
const rest = all.filter(s => keepBand.indexOf(s) < 0).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
const keepSet = new Set(keepBand);
const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
all = keepBand.concat(rest);
trimmed = band.length + rest.length < nSol;
} else all.sort((x, y) => x.rcost - y.rcost);
@@ -1622,7 +1643,7 @@ function make(cfg) {
const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0]));
const v = cross3(A0, u);
const err2 = q => {
const M5 = orient(q, 5), w = [M5[6], M5[7], M5[8]]; // 行主序：列 2 = 工具轴
const w = axisCol(q); // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行）
const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2];
return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];