dymokomi/luce-geocore
Source of release 0.7.5
0.7.5 / src / splats / rotation.lucb
## Rotations for splats in f64: unit quaternions (x, y, z, w, Houdini's
## order) and 3×3 matrices, row-major. A quaternion q turns vectors as its
## matrix does; `a.multiply(b)` turns by b first, then by a.
## A quaternion x, y, z, w; a rotation when unit.
pub struct Quat:
pub var x: f64
pub var y: f64
pub var z: f64
pub var w: f64
## No rotation.
pub static func identity() -> Quat:
return Quat(0.0, 0.0, 0.0, 1.0)
## The rotation by `angle` radians about `axis` (right-handed); none when the
## axis has no direction.
pub static func about(axis: Vector3, angle: f64) -> Quat:
let length = axis.length()
if not (length > 0.0): return Quat.identity()
let s = math.sin(0.5 * angle) / length
return Quat(axis.x * s, axis.y * s, axis.z * s, math.cos(0.5 * angle))
## This rotation after `other`: turning by the result turns by `other`, then by this.
pub func multiply(other: Quat) -> Quat:
return Quat(self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z)
## The inverse rotation of a unit quaternion.
pub func conjugate() -> Quat:
return Quat(-self.x, -self.y, -self.z, self.w)
## At unit length; the identity when it has none (or is not finite).
pub func normalized() -> Quat:
let length = math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w)
if not (length > 1.0e-300 and math.is_finite(length)): return Quat.identity()
let inverse = 1.0 / length
return Quat(self.x * inverse, self.y * inverse, self.z * inverse, self.w * inverse)
## Its rotation matrix (of the normalized quaternion).
pub func matrix() -> Mat3:
let q = self.normalized()
let xx = q.x * q.x
let yy = q.y * q.y
let zz = q.z * q.z
let xy = q.x * q.y
let xz = q.x * q.z
let yz = q.y * q.z
let wx = q.w * q.x
let wy = q.w * q.y
let wz = q.w * q.z
return Mat3([1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy),
2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx),
2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)])
## `value` turned by this rotation.
pub func rotate(value: Vector3) -> Vector3:
return self.matrix().transform(value)
## The unit quaternion of rotation matrix `m` (Shepperd's method: the
## largest of w, x, y, z is found first, so it is stable for any angle).
pub static func of_matrix(m: Mat3) -> Quat:
let a = m.m
let trace = a[0] + a[4] + a[8]
if trace > 0.0:
let s = 2.0 * math.sqrt(trace + 1.0)
return Quat((a[7] - a[5]) / s, (a[2] - a[6]) / s, (a[3] - a[1]) / s, 0.25 * s).normalized()
if a[0] > a[4] and a[0] > a[8]:
let s = 2.0 * math.sqrt(1.0 + a[0] - a[4] - a[8])
return Quat(0.25 * s, (a[1] + a[3]) / s, (a[2] + a[6]) / s, (a[7] - a[5]) / s).normalized()
if a[4] > a[8]:
let s = 2.0 * math.sqrt(1.0 + a[4] - a[0] - a[8])
return Quat((a[1] + a[3]) / s, 0.25 * s, (a[5] + a[7]) / s, (a[2] - a[6]) / s).normalized()
let s = 2.0 * math.sqrt(1.0 + a[8] - a[0] - a[4])
return Quat((a[2] + a[6]) / s, (a[5] + a[7]) / s, 0.25 * s, (a[3] - a[1]) / s).normalized()
## A 3×3 matrix, row-major: `m[row * 3 + column]`.
pub struct Mat3:
pub var m: f64[9]
pub static func identity() -> Mat3:
return Mat3([1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0])
## The diagonal matrix of `values`.
pub static func diagonal(values: Vector3) -> Mat3:
return Mat3([values.x, 0.0, 0.0, 0.0, values.y, 0.0, 0.0, 0.0, values.z])
## The linear part of an affine matrix.
pub static func of_matrix4(matrix: Matrix4) -> Mat3:
return Mat3([matrix.x.x, matrix.y.x, matrix.z.x,
matrix.x.y, matrix.y.y, matrix.z.y,
matrix.x.z, matrix.y.z, matrix.z.z])
## The matrix with these columns.
pub static func of_columns(a: Vector3, b: Vector3, d: Vector3) -> Mat3:
return Mat3([a.x, b.x, d.x, a.y, b.y, d.y, a.z, b.z, d.z])
## Element (row, column).
pub func at(row: usize, column: usize) -> f64:
return self.m[row * 3 + column]
## Column `index`.
pub func column(index: usize) -> Vector3:
return Vector3(self.m[index], self.m[3 + index], self.m[6 + index])
## This matrix times `value`.
pub func transform(value: Vector3) -> Vector3:
let a = self.m
return Vector3(a[0] * value.x + a[1] * value.y + a[2] * value.z,
a[3] * value.x + a[4] * value.y + a[5] * value.z,
a[6] * value.x + a[7] * value.y + a[8] * value.z)
## This matrix times `other`.
pub func multiply(other: Mat3) -> Mat3:
var result: f64[9]
for row in 0..<(usize)3:
for column in 0..<(usize)3:
result[row * 3 + column] = self.m[row * 3] * other.m[column] + self.m[row * 3 + 1] * other.m[3 + column] + self.m[row * 3 + 2] * other.m[6 + column]
return Mat3(result)
pub func transposed() -> Mat3:
let a = self.m
return Mat3([a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8]])
pub func determinant() -> f64:
let a = self.m
return a[0] * (a[4] * a[8] - a[5] * a[7]) - a[1] * (a[3] * a[8] - a[5] * a[6]) + a[2] * (a[3] * a[7] - a[4] * a[6])
## This matrix times `scale`.
pub func scaled(scale: f64) -> Mat3:
var result = self.m
for index in 0..<(usize)9: result[index] = result[index] * scale
return Mat3(result)
## The largest absolute difference from `other`'s elements.
pub func distance(other: Mat3) -> f64:
var most: f64 = 0.0
for index in 0..<(usize)9: most = math.max(most, math.abs(self.m[index] - other.m[index]))
return most