dymokomi/luce-geocore
Source of release 0.7.4
0.7.4 / src / splats / covariance.lucb
## A splat's covariance Σ = R S Sᵀ Rᵀ (R from `orient`, S = diag(`scale`)),
## and back: a symmetric 3×3 eigensolver turns any covariance into an orient
## and a scale, which is how non-uniform placements stay exact.
## A symmetric matrix's eigenvectors (the columns of a proper rotation) and
## its eigenvalues, in the columns' order, unsorted.
pub struct Eigen:
pub let vectors: Mat3
pub let values: Vector3
## The eigen decomposition of symmetric `matrix` (only its upper triangle is
## read) by cyclic Jacobi rotations in f64: orthonormal vectors even for
## repeated values, converged to rounding in at most a few sweeps.
pub func symmetric_eigen(matrix: Mat3) -> Eigen:
var a = matrix.m
a[3] = a[1]
a[6] = a[2]
a[7] = a[5]
var v = Mat3.identity().m
let scale = math.abs(a[0]) + math.abs(a[4]) + math.abs(a[8]) + math.abs(a[1]) + math.abs(a[2]) + math.abs(a[5])
for _ in 0..<(usize)24:
let off = math.abs(a[1]) + math.abs(a[2]) + math.abs(a[5])
if not (off > scale * 1.0e-17): break
jacobi_turn(a, v, 0, 1)
jacobi_turn(a, v, 0, 2)
jacobi_turn(a, v, 1, 2)
var vectors = Mat3(v)
# A reflection of one axis makes the vectors a proper rotation.
if vectors.determinant() < 0.0:
for row in 0..<(usize)3: vectors.m[row * 3 + 2] = -vectors.m[row * 3 + 2]
return Eigen(vectors, Vector3(a[0], a[4], a[8]))
## One Jacobi rotation zeroing a[p][q] (Numerical Recipes' form): a becomes
## Jᵀ a J and v becomes v J.
func jacobi_turn(a: f64[], v: f64[], p: usize, q: usize):
let apq = a[p * 3 + q]
if apq == 0.0: return
let theta = (a[q * 3 + q] - a[p * 3 + p]) / (2.0 * apq)
var t = 0.5 / theta
if math.abs(theta) < 1.0e150:
t = 1.0 / (math.abs(theta) + math.sqrt(theta * theta + 1.0))
if theta < 0.0: t = -t
let cosine = 1.0 / math.sqrt(t * t + 1.0)
let sine = t * cosine
for k in 0..<(usize)3:
let akp = a[k * 3 + p]
let akq = a[k * 3 + q]
a[k * 3 + p] = cosine * akp - sine * akq
a[k * 3 + q] = sine * akp + cosine * akq
for k in 0..<(usize)3:
let apk = a[p * 3 + k]
let aqk = a[q * 3 + k]
a[p * 3 + k] = cosine * apk - sine * aqk
a[q * 3 + k] = sine * apk + cosine * aqk
a[p * 3 + q] = 0.0
a[q * 3 + p] = 0.0
for k in 0..<(usize)3:
let vkp = v[k * 3 + p]
let vkq = v[k * 3 + q]
v[k * 3 + p] = cosine * vkp - sine * vkq
v[k * 3 + q] = sine * vkp + cosine * vkq
## Σ = R diag(scale²) Rᵀ, R the rotation of `orient`.
pub func compose_covariance(orient: Quat, scale: Vector3) -> Mat3:
let r = orient.matrix()
let squares = Vector3(scale.x * scale.x, scale.y * scale.y, scale.z * scale.z)
return r.multiply(Mat3.diagonal(squares)).multiply(r.transposed())
## A splat's orient and scale.
pub struct Ellipsoid:
pub let orient: Quat
pub let scale: Vector3
## The orient and scale of covariance `sigma`: its eigenvectors and the square
## roots of its eigenvalues (negative rounding is clamped to 0). The axes come
## in no particular order, and each up to sign: compose_covariance gives the
## same Σ back.
pub func decompose_covariance(sigma: Mat3) -> Ellipsoid:
let eigen = symmetric_eigen(sigma)
let values = eigen.values
return Ellipsoid(Quat.of_matrix(eigen.vectors), Vector3(math.sqrt(math.max(values.x, 0.0)), math.sqrt(math.max(values.y, 0.0)), math.sqrt(math.max(values.z, 0.0))))
## The rotation of a linear map with a nonnegative determinant: Q of its polar
## decomposition L = Q P (P symmetric, positive semidefinite), the rotation
## nearest to L. Axes L flattens are completed to a right-handed frame.
pub func rotation_part(linear: Mat3) -> Mat3:
let eigen = symmetric_eigen(linear.transposed().multiply(linear))
let values = eigen.values
let largest = math.max(values.x, math.max(values.y, values.z))
if not (largest > 0.0): return Mat3.identity()
var columns: Vector3[3] = [Vector3(), Vector3(), Vector3()]
var kept: bool[3] = [false, false, false]
var count: usize = 0
for axis in 0..<(usize)3:
let value = values.coordinate(axis)
if not (value > largest * 1.0e-20): continue
columns[axis] = linear.transform(eigen.vectors.column(axis)).multiply_scalar(1.0 / math.sqrt(value))
kept[axis] = true
count += 1
if count == 1:
# Any axis perpendicular to the one kept, then the third by the right hand.
let single = 0 if kept[0] else (1 if kept[1] else 2)
let known = columns[single]
let helper = Vector3(1.0, 0.0, 0.0) if math.abs(known.x) < 0.9 else Vector3(0.0, 1.0, 0.0)
let next = (single + 1) % 3
columns[next] = unit_or(known.cross(helper), Vector3(0.0, 0.0, 1.0))
kept[next] = true
count = 2
if count == 2:
let missing = 0 if not kept[0] else (1 if not kept[1] else 2)
columns[missing] = unit_or(columns[(missing + 1) % 3].cross(columns[(missing + 2) % 3]), Vector3(0.0, 0.0, 1.0))
let u = Mat3.of_columns(columns[0], columns[1], columns[2])
return u.multiply(eigen.vectors.transposed())
func unit_or(value: Vector3, fallback: Vector3) -> Vector3:
return value.normalized() catch failure: return fallback