Rotations, transforms and small linear algebra (motion)
Rotation matrices, roll-pitch-yaw angles, rotation vectors, rigid transforms and the dense solve used by the robot and linkage calculations.
How the precision is kept
All 64-bit and closed form. Inverse trigonometric arguments are clamped to [−1, 1]. The dense solve treats a pivot below 1e-14 as singular, an absolute threshold. Singular values are taken from the Gram matrix, which halves the digits available for the small ones.
Open item. Compute singular values from the matrix itself (one-sided Jacobi) instead of from the Gram matrix.
Calculations under this policy
7 calculations in 1 source file. The code is read from the released app when this page loads. It is the calculation people run, not a copy kept for this page.
math.jsShared shell, every app64-bit7 calculationsReviewed
The axis is normalized first. 1 − cos θ is formed by subtraction; its absolute error stays at rounding level, so each matrix entry is within a few units of 1e-16 of the exact value.
math.js
lines 25 to 31
/** Rotation by `angle` about unit-ised `axis` (Rodrigues). */
export function axisAngle(axis, angle) {
const a = unit(axis), c = Math.cos(angle), s = Math.sin(angle), t = 1 - c;
return [t * a[0] * a[0] + c, t * a[0] * a[1] - s * a[2], t * a[0] * a[2] + s * a[1],
t * a[0] * a[1] + s * a[2], t * a[1] * a[1] + c, t * a[1] * a[2] - s * a[0],
t * a[0] * a[2] - s * a[1], t * a[1] * a[2] + s * a[0], t * a[2] * a[2] + c];
}
The asin argument is clamped to [−1, 1]. Within 1e-9 of ±1 the roll is set to 0 and the yaw takes the whole rotation. At that point asin is ill-conditioned: a rounding of 1e-16 in the matrix entry moves the pitch by up to about 1e-8 rad.
math.js
lines 37 to 42
/** Inverse of rpy() for |pitch| < 90°; at the gimbal lock roll is set to 0. */
export function toRpy(R) {
const sp = -R[6], pitch = Math.asin(Math.max(-1, Math.min(1, sp)));
if (Math.abs(sp) > 1 - 1e-9) return [0, pitch, Math.atan2(-R[1], R[4])];
return [Math.atan2(R[7], R[8]), pitch, Math.atan2(R[3], R[0])];
}
Rotation vector from a matrix
64-bit
Axis times angle of a rotation, used for orientation error.
θ = acos((tr R − 1) ÷ 2); v = θ ÷ (2 sin θ) × (R₃₂ − R₂₃, R₁₃ − R₃₁, R₂₁ − R₁₂)
The acos argument is clamped. Below 1e-9 rad the vector is half the antisymmetric part; within 1e-6 rad of 180° the axis comes from the diagonal. Just outside that window the axis is built from differences of entries whose size is 2 sin θ, so their relative rounding error grows as θ nears 180°.
math.js
lines 43 to 54
/** Rotation vector (axis × angle) of R, angle in [0, π]. */
export function rotvec(R) {
const tr = R[0] + R[4] + R[8], c = Math.max(-1, Math.min(1, (tr - 1) / 2)), ang = Math.acos(c);
const w = [R[7] - R[5], R[2] - R[6], R[3] - R[1]];
if (ang < 1e-9) return scale(w, 0.5);
if (Math.PI - ang < 1e-6) { // near 180°: axis from the symmetric part
const d = [Math.sqrt(Math.max(0, (R[0] + 1) / 2)), Math.sqrt(Math.max(0, (R[4] + 1) / 2)), Math.sqrt(Math.max(0, (R[8] + 1) / 2))];
if (R[1] + R[3] < 0) d[1] = -d[1]; if (R[2] + R[6] < 0) d[2] = -d[2];
return scale(unit(d), ang);
}
return scale(w, ang / (2 * Math.sin(ang)));
}
Rigid transforms
64-bit
Product, inverse and application of a rotation with a translation.
Products and sums in 64-bit. The inverse uses the transpose, which is the true inverse only while R stays orthonormal; a long chain of products is not re-orthonormalized.
Solves the small systems of inverse kinematics and linkages.
Gaussian elimination with partial pivoting
A pivot smaller than 1e-14 in absolute value returns null. The threshold is not scaled to the matrix, so a well-posed system with small entries can be refused and a nearly singular one with large entries accepted.
math.js
lines 67 to 79
/** Solve A x = b (Gaussian elimination, partial pivoting). Returns null when singular. */
export function solve(A, b) {
const n = A.length, M = A.map((r, i) => [...r, b[i]]);
for (let c = 0; c < n; c++) {
let p = c; for (let r = c + 1; r < n; r++) if (Math.abs(M[r][c]) > Math.abs(M[p][c])) p = r;
if (Math.abs(M[p][c]) < 1e-14) return null;
[M[c], M[p]] = [M[p], M[c]];
for (let r = c + 1; r < n; r++) { const f = M[r][c] / M[c][c]; if (f) for (let k = c; k <= n; k++) M[r][k] -= f * M[c][k]; }
}
const x = new Array(n).fill(0);
for (let i = n - 1; i >= 0; i--) { let s = M[i][n]; for (let j = i + 1; j < n; j++) s -= M[i][j] * x[j]; x[i] = s / M[i][i]; }
return x;
}
Positive-definite test
64-bit
Whether a symmetric matrix, such as a mass matrix, is positive definite.
Cholesky factorization succeeds
The factorization fails when a diagonal term is at or below 1e-14, an absolute value not scaled to the matrix.
math.js
lines 80 to 88
/** Cholesky test for a symmetric positive-definite matrix. */
export function isSPD(A) {
const n = A.length, L = zeros(n, n);
for (let i = 0; i < n; i++) for (let j = 0; j <= i; j++) {
let s = A[i][j]; for (let k = 0; k < j; k++) s -= L[i][k] * L[j][k];
if (i === j) { if (s <= 1e-14) return false; L[i][i] = Math.sqrt(s); } else L[i][j] = s / L[j][j];
}
return true;
}
Singular values
64-bit
The singular values of a Jacobian, used to detect singular poses and to count degrees of freedom.
σᵢ = √(eigenvalues of J Jᵀ or Jᵀ J)
Forming the Gram matrix squares the condition number: an eigenvalue carries an absolute error of about 1e-16 × σ_max², so a singular value σ has a relative error of about 1e-16 × (σ_max ÷ σ)² ÷ 2. Negative eigenvalues from rounding are clamped to 0 before the root.
math.js
lines 89 to 105
/** Singular values of an m×n matrix by the eigenvalues of the Gram matrix (Jacobi), descending. Fine for 6×n Jacobians. */
export function singularValues(J) {
const m = J.length, n = m ? J[0].length : 0;
const G = m <= n ? matmul(J, transpose(J)) : matmul(transpose(J), J), k = G.length;
const A = G.map((r) => r.slice());
for (let sweep = 0; sweep < 60; sweep++) {
let off = 0; for (let i = 0; i < k; i++) for (let j = i + 1; j < k; j++) off += A[i][j] * A[i][j];
if (off < 1e-26) break;
for (let p = 0; p < k; p++) for (let q = p + 1; q < k; q++) {
if (Math.abs(A[p][q]) < 1e-30) continue;
const th = (A[q][q] - A[p][p]) / (2 * A[p][q]), t = Math.sign(th || 1) / (Math.abs(th) + Math.sqrt(th * th + 1)), c = 1 / Math.sqrt(t * t + 1), s = t * c;
for (let r = 0; r < k; r++) { const x = A[r][p], y = A[r][q]; A[r][p] = c * x - s * y; A[r][q] = s * x + c * y; }
for (let r = 0; r < k; r++) { const x = A[p][r], y = A[q][r]; A[p][r] = c * x - s * y; A[q][r] = s * x + c * y; }
}
}
return A.map((r, i) => Math.sqrt(Math.max(0, r[i]))).sort((a, b) => b - a);
}
In detail
Method
Rodrigues formula; roll-pitch-yaw in the order Rz Ry Rx; Gaussian elimination with partial pivoting; Jacobi eigenvalue sweeps on J Jᵀ or Jᵀ J.
Number types
64-bit numbers in plain arrays.
Tolerances
Gimbal lock when |sin pitch| > 1 − 1e-9. Rotation vector: small-angle branch below 1e-9 rad, near-180° branch within 1e-6 rad of π. Solve: a pivot below 1e-14 (absolute) is singular. Cholesky test: a diagonal term at or below 1e-14 fails. Jacobi: stops when the off-diagonal sum of squares is below 1e-26, 60 sweeps at most.
Limits
Singular values below about 1e-8 of the largest cannot be told from zero.
Approach: 64-bit, direct
Every value is a 64-bit number from input to result. Formulas are closed form or a direct solve, and every comparison uses a stated tolerance.