dymokomi/luce-fbx
Source of release 0.12.1
0.12.1 / src / scene / rotation.lucb
## Rotations and translation-rotation-scale transforms, as FBX (and ufbx)
## compose them: Euler angles in degrees applied in a rotation order, kept
## as unit quaternions until a matrix is needed.
pub struct Quat:
pub var x: f64 = 0.0
pub var y: f64 = 0.0
pub var z: f64 = 0.0
pub var w: f64 = 1.0
## This rotation after `other` (other is applied first).
pub func times(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)
pub func inverse() -> Quat:
return Quat(-self.x, -self.y, -self.z, self.w)
pub func rotate(v: Vector3) -> Vector3:
# v + 2w(q x v) + 2 q x (q x v)
let q = Vector3(self.x, self.y, self.z)
let t = q.cross(v).multiply_scalar(2.0)
return v.add(t.multiply_scalar(self.w)).add(q.cross(t))
pub func is_identity() -> bool:
return self.x == 0.0 and self.y == 0.0 and self.z == 0.0 and self.w == 1.0
## The rotation `t` of the way from `a` to `b` (ufbx_quat_slerp).
pub func slerp(a: Quat, b: Quat, t: f64) -> Quat:
var other = b
var dot = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
if dot < 0.0:
dot = -dot
other = Quat(-b.x, -b.y, -b.z, -b.w)
let omega = math.acos(math.min(math.max(dot, 0.0), 1.0))
if omega <= 1.175494351e-38: return a
let reciprocal = 1.0 / math.sin(omega)
let af = math.sin((1.0 - t) * omega) * reciprocal
let bf = math.sin(t * omega) * reciprocal
let x = af * a.x + bf * other.x
let y = af * a.y + bf * other.y
let z = af * a.z + bf * other.z
let w = af * a.w + bf * other.w
let length = 1.0 / math.sqrt(x * x + y * y + z * z + w * w)
return Quat(x * length, y * length, z * length, w * length)
## Euler angles in degrees of rotation `q` in `order` (ufbx_quat_to_euler,
## its formulas generated for each order).
pub func quat_euler(q: Quat, order: i64) -> Vector3:
let eps = 0.999999999
let qx = q.x
let qy = q.y
let qz = q.z
let qw = q.w
var vx: f64 = 0.0
var vy: f64 = 0.0
var vz: f64 = 0.0
var t: f64 = 0.0
if order == 0:
t = 2.0 * (qw * qy - qx * qz)
if math.abs(t) < eps:
vy = math.asin(t)
vz = math.atan2(2.0 * (qw * qz + qx * qy), 2.0 * (qw * qw + qx * qx) - 1.0)
vx = -math.atan2(-2.0 * (qw * qx + qy * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
else:
vy = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
vz = math.atan2(-2.0 * t * (qw * qx - qy * qz), t * (2.0 * qw * qy + 2.0 * qx * qz))
vx = 0.0
elif order == 1:
t = 2.0 * (qw * qz + qx * qy)
if math.abs(t) < eps:
vz = math.asin(t)
vy = math.atan2(2.0 * (qw * qy - qx * qz), 2.0 * (qw * qw + qx * qx) - 1.0)
vx = -math.atan2(-2.0 * (qw * qx - qy * qz), 2.0 * (qw * qw + qy * qy) - 1.0)
else:
vz = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
vy = math.atan2(2.0 * t * (qw * qx + qy * qz), -t * (2.0 * qx * qy - 2.0 * qw * qz))
vx = 0.0
elif order == 2:
t = 2.0 * (qw * qz - qx * qy)
if math.abs(t) < eps:
vz = math.asin(t)
vx = math.atan2(2.0 * (qw * qx + qy * qz), 2.0 * (qw * qw + qy * qy) - 1.0)
vy = -math.atan2(-2.0 * (qw * qy + qx * qz), 2.0 * (qw * qw + qx * qx) - 1.0)
else:
vz = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
vx = math.atan2(-2.0 * t * (qw * qy - qx * qz), t * (2.0 * qw * qz + 2.0 * qx * qy))
vy = 0.0
elif order == 3:
t = 2.0 * (qw * qx + qy * qz)
if math.abs(t) < eps:
vx = math.asin(t)
vz = math.atan2(2.0 * (qw * qz - qx * qy), 2.0 * (qw * qw + qy * qy) - 1.0)
vy = -math.atan2(-2.0 * (qw * qy - qx * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
else:
vx = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
vz = math.atan2(2.0 * t * (qw * qy + qx * qz), -t * (2.0 * qy * qz - 2.0 * qw * qx))
vy = 0.0
elif order == 4:
t = 2.0 * (qw * qx - qy * qz)
if math.abs(t) < eps:
vx = math.asin(t)
vy = math.atan2(2.0 * (qw * qy + qx * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
vz = -math.atan2(-2.0 * (qw * qz + qx * qy), 2.0 * (qw * qw + qy * qy) - 1.0)
else:
vx = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
vy = math.atan2(-2.0 * t * (qw * qz - qx * qy), t * (2.0 * qw * qx + 2.0 * qy * qz))
vz = 0.0
elif order == 5:
t = 2.0 * (qw * qy + qx * qz)
if math.abs(t) < eps:
vy = math.asin(t)
vx = math.atan2(2.0 * (qw * qx - qy * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
vz = -math.atan2(-2.0 * (qw * qz - qx * qy), 2.0 * (qw * qw + qx * qx) - 1.0)
else:
vy = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
vx = math.atan2(2.0 * t * (qw * qz + qx * qy), -t * (2.0 * qx * qz - 2.0 * qw * qy))
vz = 0.0
return Vector3(vx * (180.0 / math.pi), vy * (180.0 / math.pi), vz * (180.0 / math.pi))
## A rotation of `degrees` about axis `axis` (0 X, 1 Y, 2 Z).
func axis_rotation(axis: usize, degrees: f64) -> Quat:
let half = degrees * (math.pi / 360.0)
let s = math.sin(half)
let c = math.cos(half)
match axis:
0: return Quat(s, 0.0, 0.0, c)
1: return Quat(0.0, s, 0.0, c)
_: return Quat(0.0, 0.0, s, c)
## The axes of each FBX rotation order, first applied first: XYZ, XZY, YZX,
## YXZ, ZXY, ZYX; spheric XYZ (6) rotates as XYZ, as ufbx does.
let order_axes: u8[18] = [0, 1, 2, 0, 2, 1, 1, 2, 0, 1, 0, 2, 2, 0, 1, 2, 1, 0]
## Euler angles in degrees as a rotation, in `order` (an FBX RotationOrder).
pub func euler(degrees: Vector3, order: i64) -> Quat:
var row: usize = 0
if order >= 0 and order < 6: row = (usize)order * 3
let angles: f64[3] = [degrees.x, degrees.y, degrees.z]
var result = Quat()
for step: usize in 0..<3:
let axis = (usize)order_axes[row + step]
if angles[axis] != 0.0: result = axis_rotation(axis, angles[axis]).times(result)
return result
## Translation, rotation and scale: the point is scaled, rotated, then moved.
pub struct Trs:
pub var translation: Vector3 = Vector3()
pub var rotation: Quat = Quat()
pub var scale: Vector3 = Vector3(1.0, 1.0, 1.0)
## Scale the whole transform (as S * T, ufbx's ufbxi_mul_scale).
pub func scaled(by: Vector3):
self.translation = Vector3(self.translation.x * by.x, self.translation.y * by.y, self.translation.z * by.z)
self.scale = Vector3(self.scale.x * by.x, self.scale.y * by.y, self.scale.z * by.z)
## Rotate the whole transform (as R * T).
pub func rotated(by: Quat):
if by.is_identity(): return
self.rotation = by.times(self.rotation)
self.translation = by.rotate(self.translation)
pub func moved(by: Vector3):
self.translation = self.translation.add(by)
pub func matrix() -> Matrix4:
let q = self.rotation
let x = q.rotate(Vector3(self.scale.x, 0.0, 0.0))
let y = q.rotate(Vector3(0.0, self.scale.y, 0.0))
let z = q.rotate(Vector3(0.0, 0.0, self.scale.z))
return Matrix4(x, y, z, self.translation)
## The matrix without the scale.
pub func unscaled() -> Matrix4:
var copy = self
copy.scale = Vector3(1.0, 1.0, 1.0)
return copy.matrix()
pub func is_identity() -> bool:
return self.translation == Vector3() and self.rotation.is_identity() and self.scale == Vector3(1.0, 1.0, 1.0)