pub type Mat4 = [f64; 16];
pub const IDENTITY: Mat4 = [
1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
];
#[derive(Debug, Clone, Default, PartialEq)]
pub struct HelperGeometry {
pub lines: Vec<f32>,
pub colors: Vec<f32>,
pub mesh: Vec<f32>,
}
type V3 = [f64; 3];
type Quat = [f64; 4];
fn sub(a: V3, b: V3) -> V3 {
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
fn cross(a: V3, b: V3) -> V3 {
[
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0],
]
}
fn length_sq(a: V3) -> f64 {
a[0] * a[0] + a[1] * a[1] + a[2] * a[2]
}
fn normalize(a: V3) -> V3 {
let l = length_sq(a).sqrt();
let l = if l == 0.0 { 1.0 } else { l };
[a[0] / l, a[1] / l, a[2] / l]
}
pub fn multiply(a: &Mat4, b: &Mat4) -> Mat4 {
let mut out = [0.0; 16];
for row in 0..4 {
for col in 0..4 {
out[col * 4 + row] = a[row] * b[col * 4]
+ a[4 + row] * b[col * 4 + 1]
+ a[8 + row] * b[col * 4 + 2]
+ a[12 + row] * b[col * 4 + 3];
}
}
out
}
pub fn invert(te: &Mat4) -> Mat4 {
let (n11, n21, n31, n41) = (te[0], te[1], te[2], te[3]);
let (n12, n22, n32, n42) = (te[4], te[5], te[6], te[7]);
let (n13, n23, n33, n43) = (te[8], te[9], te[10], te[11]);
let (n14, n24, n34, n44) = (te[12], te[13], te[14], te[15]);
let t11 =
n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44
+ n22 * n33 * n44;
let t12 =
n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44
- n12 * n33 * n44;
let t13 =
n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44
+ n12 * n23 * n44;
let t14 =
n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34
- n12 * n23 * n34;
let det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;
if det == 0.0 {
return [0.0; 16];
}
let d = 1.0 / det;
[
t11 * d,
(n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44
- n21 * n33 * n44)
* d,
(n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44
+ n21 * n32 * n44)
* d,
(n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43
- n21 * n32 * n43)
* d,
t12 * d,
(n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44
+ n11 * n33 * n44)
* d,
(n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44
- n11 * n32 * n44)
* d,
(n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43
+ n11 * n32 * n43)
* d,
t13 * d,
(n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44
- n11 * n23 * n44)
* d,
(n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44
+ n11 * n22 * n44)
* d,
(n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43
- n11 * n22 * n43)
* d,
t14 * d,
(n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34
+ n11 * n23 * n34)
* d,
(n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34
- n11 * n22 * n34)
* d,
(n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33
+ n11 * n22 * n33)
* d,
]
}
pub fn apply(e: &Mat4, p: V3) -> V3 {
let [x, y, z] = p;
let w = 1.0 / (e[3] * x + e[7] * y + e[11] * z + e[15]);
[
(e[0] * x + e[4] * y + e[8] * z + e[12]) * w,
(e[1] * x + e[5] * y + e[9] * z + e[13]) * w,
(e[2] * x + e[6] * y + e[10] * z + e[14]) * w,
]
}
pub fn compose(p: V3, q: Quat, s: V3) -> Mat4 {
let [x, y, z, w] = q;
let (x2, y2, z2) = (x + x, y + y, z + z);
let (xx, xy, xz) = (x * x2, x * y2, x * z2);
let (yy, yz, zz) = (y * y2, y * z2, z * z2);
let (wx, wy, wz) = (w * x2, w * y2, w * z2);
[
(1.0 - (yy + zz)) * s[0],
(xy + wz) * s[0],
(xz - wy) * s[0],
0.0,
(xy - wz) * s[1],
(1.0 - (xx + zz)) * s[1],
(yz + wx) * s[1],
0.0,
(xz + wy) * s[2],
(yz - wx) * s[2],
(1.0 - (xx + yy)) * s[2],
0.0,
p[0],
p[1],
p[2],
1.0,
]
}
fn extract_rotation(m: &Mat4) -> Mat4 {
let col = |c: usize| {
(m[c * 4] * m[c * 4] + m[c * 4 + 1] * m[c * 4 + 1] + m[c * 4 + 2] * m[c * 4 + 2]).sqrt()
};
let (sx, sy, sz) = (1.0 / col(0), 1.0 / col(1), 1.0 / col(2));
[
m[0] * sx,
m[1] * sx,
m[2] * sx,
0.0,
m[4] * sy,
m[5] * sy,
m[6] * sy,
0.0,
m[8] * sz,
m[9] * sz,
m[10] * sz,
0.0,
0.0,
0.0,
0.0,
1.0,
]
}
fn quat_from_rotation(te: &Mat4) -> Quat {
let (m11, m12, m13) = (te[0], te[4], te[8]);
let (m21, m22, m23) = (te[1], te[5], te[9]);
let (m31, m32, m33) = (te[2], te[6], te[10]);
let trace = m11 + m22 + m33;
if trace > 0.0 {
let s = 0.5 / (trace + 1.0).sqrt();
[(m32 - m23) * s, (m13 - m31) * s, (m21 - m12) * s, 0.25 / s]
} else if m11 > m22 && m11 > m33 {
let s = 2.0 * (1.0 + m11 - m22 - m33).sqrt();
[0.25 * s, (m12 + m21) / s, (m13 + m31) / s, (m32 - m23) / s]
} else if m22 > m33 {
let s = 2.0 * (1.0 + m22 - m11 - m33).sqrt();
[(m12 + m21) / s, 0.25 * s, (m23 + m32) / s, (m13 - m31) / s]
} else {
let s = 2.0 * (1.0 + m33 - m11 - m22).sqrt();
[(m13 + m31) / s, (m23 + m32) / s, 0.25 * s, (m21 - m12) / s]
}
}
fn quat_mul(a: Quat, b: Quat) -> Quat {
let [qax, qay, qaz, qaw] = a;
let [qbx, qby, qbz, qbw] = b;
[
qax * qbw + qaw * qbx + qay * qbz - qaz * qby,
qay * qbw + qaw * qby + qaz * qbx - qax * qbz,
qaz * qbw + qaw * qbz + qax * qby - qay * qbx,
qaw * qbw - qax * qbx - qay * qby - qaz * qbz,
]
}
fn look_at_matrix(eye: V3, target: V3, up: V3) -> Mat4 {
let mut z = sub(eye, target);
if length_sq(z) == 0.0 {
z[2] = 1.0;
}
z = normalize(z);
let mut x = cross(up, z);
if length_sq(x) == 0.0 {
if up[2].abs() == 1.0 {
z[0] += 0.0001;
} else {
z[2] += 0.0001;
}
z = normalize(z);
x = cross(up, z);
}
x = normalize(x);
let y = cross(z, x);
let mut te = IDENTITY;
te[0] = x[0];
te[4] = y[0];
te[8] = z[0];
te[1] = x[1];
te[5] = y[1];
te[9] = z[1];
te[2] = x[2];
te[6] = y[2];
te[10] = z[2];
te
}
fn position_of(m: &Mat4) -> V3 {
[m[12], m[13], m[14]]
}
const UP: V3 = [0.0, 1.0, 0.0];
fn child_looking_at(parent: &Mat4, position: V3, scale: V3, target: V3) -> Mat4 {
let world_pos = position_of(&multiply(
parent,
&compose(position, [0.0, 0.0, 0.0, 1.0], scale),
));
let mut q = quat_from_rotation(&look_at_matrix(target, world_pos, UP));
let pq = quat_from_rotation(&extract_rotation(parent));
q = quat_mul([-pq[0], -pq[1], -pq[2], pq[3]], q);
multiply(parent, &compose(position, q, scale))
}
fn apply_quat(v: V3, q: Quat) -> V3 {
let [vx, vy, vz] = v;
let [qx, qy, qz, qw] = q;
let tx = 2.0 * (qy * vz - qz * vy);
let ty = 2.0 * (qz * vx - qx * vz);
let tz = 2.0 * (qx * vy - qy * vx);
[
vx + qw * tx + qy * tz - qz * ty,
vy + qw * ty + qz * tx - qx * tz,
vz + qw * tz + qx * ty - qy * tx,
]
}
fn f32v(v: V3) -> V3 {
[v[0] as f32 as f64, v[1] as f32 as f64, v[2] as f32 as f64]
}
fn push(out: &mut Vec<f32>, p: V3) {
out.extend_from_slice(&[p[0] as f32, p[1] as f32, p[2] as f32]);
}
fn strip(out: &mut Vec<f32>, m: &Mat4, points: &[V3]) {
for w in points.windows(2) {
push(out, apply(m, f32v(w[0])));
push(out, apply(m, f32v(w[1])));
}
}
fn segments(out: &mut Vec<f32>, m: &Mat4, points: &[V3]) {
for p in points {
push(out, apply(m, f32v(*p)));
}
}
fn wireframe(out: &mut Vec<f32>, m: &Mat4, positions: &[V3], triangles: &[[usize; 3]]) {
for &[a, b, c] in triangles {
for (i, j) in [(a, b), (b, c), (c, a)] {
push(out, apply(m, positions[i]));
push(out, apply(m, positions[j]));
}
}
}
pub fn directional_light(light_world: &Mat4, target_world: V3, size: f64) -> HelperGeometry {
let v1 = position_of(light_world);
let v3 = sub(target_world, v1);
let mut lines = Vec::new();
let plane = child_looking_at(light_world, [0.0; 3], [1.0; 3], target_world);
strip(
&mut lines,
&plane,
&[
[-size, size, 0.0],
[size, size, 0.0],
[size, -size, 0.0],
[-size, -size, 0.0],
[-size, size, 0.0],
],
);
let target_line = child_looking_at(
light_world,
[0.0; 3],
[1.0, 1.0, length_sq(v3).sqrt()],
target_world,
);
strip(
&mut lines,
&target_line,
&[[0.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
);
HelperGeometry {
lines,
..Default::default()
}
}
fn octahedron(radius: f64) -> Vec<V3> {
let v: [V3; 6] = [
[1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, -1.0, 0.0],
[0.0, 0.0, 1.0],
[0.0, 0.0, -1.0],
];
let faces = [
[0, 2, 4],
[0, 4, 3],
[0, 3, 5],
[0, 5, 2],
[1, 2, 5],
[1, 5, 3],
[1, 3, 4],
[1, 4, 2],
];
let mut out = Vec::with_capacity(24);
for [a, b, c] in faces {
for i in [b, c, a] {
let n = normalize(v[i]);
out.push(f32v([n[0] * radius, n[1] * radius, n[2] * radius]));
}
}
out
}
pub fn hemisphere_light(
light_world: &Mat4,
size: f64,
sky: [f32; 3],
ground: [f32; 3],
) -> HelperGeometry {
let (s, c) = (std::f64::consts::PI * 0.5).sin_cos();
let positions: Vec<V3> = octahedron(size)
.into_iter()
.map(|[x, y, z]| f32v([c * x + s * z, y, -s * x + c * z]))
.collect();
let n = positions.len();
let lp = position_of(light_world);
let mesh_world = child_looking_at(light_world, [0.0; 3], [1.0; 3], [-lp[0], -lp[1], -lp[2]]);
let triangles: Vec<[usize; 3]> = (0..n / 3).map(|t| [3 * t, 3 * t + 1, 3 * t + 2]).collect();
let mut lines = Vec::new();
wireframe(&mut lines, &mesh_world, &positions, &triangles);
let color = |i: usize| if i < n / 2 { sky } else { ground };
let mut colors = Vec::new();
for &[a, b, cc] in &triangles {
for (i, j) in [(a, b), (b, cc), (cc, a)] {
colors.extend_from_slice(&color(i));
colors.extend_from_slice(&color(j));
}
}
HelperGeometry {
lines,
colors,
mesh: Vec::new(),
}
}
fn sphere(radius: f64, ws: usize, hs: usize) -> (Vec<V3>, Vec<[usize; 3]>) {
use std::f64::consts::PI;
let (phi_len, theta_len) = (PI * 2.0, PI);
let mut positions = Vec::new();
let mut grid = Vec::new();
for iy in 0..=hs {
let v = iy as f64 / hs as f64;
let mut row = Vec::new();
for ix in 0..=ws {
let u = ix as f64 / ws as f64;
positions.push(f32v([
-radius * (u * phi_len).cos() * (v * theta_len).sin(),
radius * (v * theta_len).cos(),
radius * (u * phi_len).sin() * (v * theta_len).sin(),
]));
row.push(positions.len() - 1);
}
grid.push(row);
}
let mut tris = Vec::new();
for iy in 0..hs {
for ix in 0..ws {
let a = grid[iy][ix + 1];
let b = grid[iy][ix];
let c = grid[iy + 1][ix];
let d = grid[iy + 1][ix + 1];
if iy != 0 {
tris.push([a, b, d]);
}
if iy != hs - 1 {
tris.push([b, c, d]);
}
}
}
(positions, tris)
}
pub fn point_light(light_world: &Mat4, sphere_size: f64) -> HelperGeometry {
let (positions, tris) = sphere(sphere_size, 4, 2);
let mut lines = Vec::new();
wireframe(&mut lines, light_world, &positions, &tris);
HelperGeometry {
lines,
..Default::default()
}
}
pub fn spot_light(
light_world: &Mat4,
target_world: V3,
distance: f64,
angle: f64,
) -> HelperGeometry {
let mut pts: Vec<V3> = vec![
[0.0, 0.0, 0.0],
[0.0, 0.0, 1.0],
[0.0, 0.0, 0.0],
[1.0, 0.0, 1.0],
[0.0, 0.0, 0.0],
[-1.0, 0.0, 1.0],
[0.0, 0.0, 0.0],
[0.0, 1.0, 1.0],
[0.0, 0.0, 0.0],
[0.0, -1.0, 1.0],
];
let l = 32;
for i in 0..l {
let p1 = i as f64 / l as f64 * std::f64::consts::PI * 2.0;
let p2 = (i + 1) as f64 / l as f64 * std::f64::consts::PI * 2.0;
pts.push([p1.cos(), p1.sin(), 1.0]);
pts.push([p2.cos(), p2.sin(), 1.0]);
}
let cone_length = if distance != 0.0 { distance } else { 1000.0 };
let cone_width = cone_length * angle.tan();
let cone = child_looking_at(
light_world,
[0.0; 3],
[cone_width, cone_width, cone_length],
target_world,
);
let mut lines = Vec::new();
segments(&mut lines, &cone, &pts);
HelperGeometry {
lines,
..Default::default()
}
}
pub fn camera_default_colors() -> [[f32; 3]; 5] {
let lin = |h: u32| {
let c = crate::math::Color::from_hex(h);
[c.r, c.g, c.b]
};
[
lin(0xffaa00),
lin(0xff0000),
lin(0x00aaff),
lin(0xffffff),
lin(0x333333),
]
}
pub fn camera(
projection_inverse: &Mat4,
camera_world: &Mat4,
colors: [[f32; 3]; 5],
) -> HelperGeometry {
let (w, h) = (1.0, 1.0);
let point = |id: &str| -> V3 {
let ndc = match id {
"c" => [0.0, 0.0, -1.0],
"t" => [0.0, 0.0, 1.0],
"n1" => [-w, -h, -1.0],
"n2" => [w, -h, -1.0],
"n3" => [-w, h, -1.0],
"n4" => [w, h, -1.0],
"f1" => [-w, -h, 1.0],
"f2" => [w, -h, 1.0],
"f3" => [-w, h, 1.0],
"f4" => [w, h, 1.0],
"u1" => [w * 0.7, h * 1.1, -1.0],
"u2" => [-w * 0.7, h * 1.1, -1.0],
"u3" => [0.0, h * 2.0, -1.0],
"cf1" => [-w, 0.0, 1.0],
"cf2" => [w, 0.0, 1.0],
"cf3" => [0.0, -h, 1.0],
"cf4" => [0.0, h, 1.0],
"cn1" => [-w, 0.0, -1.0],
"cn2" => [w, 0.0, -1.0],
"cn3" => [0.0, -h, -1.0],
"cn4" => [0.0, h, -1.0],
_ => return [0.0; 3],
};
f32v(apply(&IDENTITY, apply(projection_inverse, ndc)))
};
let [frustum, cone, up, target, cross_c] = colors;
let pairs: [(&str, &str, [f32; 3]); 25] = [
("n1", "n2", frustum),
("n2", "n4", frustum),
("n4", "n3", frustum),
("n3", "n1", frustum),
("f1", "f2", frustum),
("f2", "f4", frustum),
("f4", "f3", frustum),
("f3", "f1", frustum),
("n1", "f1", frustum),
("n2", "f2", frustum),
("n3", "f3", frustum),
("n4", "f4", frustum),
("p", "n1", cone),
("p", "n2", cone),
("p", "n3", cone),
("p", "n4", cone),
("u1", "u2", up),
("u2", "u3", up),
("u3", "u1", up),
("c", "t", target),
("p", "c", cross_c),
("cn1", "cn2", cross_c),
("cn3", "cn4", cross_c),
("cf1", "cf2", cross_c),
("cf3", "cf4", cross_c),
];
let mut lines = Vec::with_capacity(pairs.len() * 6);
let mut cols = Vec::with_capacity(pairs.len() * 6);
for (a, b, c) in pairs {
push(&mut lines, apply(camera_world, point(a)));
push(&mut lines, apply(camera_world, point(b)));
cols.extend_from_slice(&c);
cols.extend_from_slice(&c);
}
HelperGeometry {
lines,
colors: cols,
mesh: Vec::new(),
}
}
#[allow(clippy::too_many_arguments)]
pub fn perspective_projection(
fov: f64,
aspect: f64,
near: f64,
far: f64,
zoom: f64,
film_offset: f64,
film_width: f64,
view: Option<[f64; 6]>,
) -> Mat4 {
let mut top = near * (std::f64::consts::PI / 180.0 * 0.5 * fov).tan() / zoom;
let mut height = 2.0 * top;
let mut width = aspect * height;
let mut left = -0.5 * width;
if let Some([full_w, full_h, ox, oy, vw, vh]) = view {
left += ox * width / full_w;
top -= oy * height / full_h;
width *= vw / full_w;
height *= vh / full_h;
}
if film_offset != 0.0 {
left += near * film_offset / film_width;
}
make_perspective(left, left + width, top, top - height, near, far)
}
fn make_perspective(left: f64, right: f64, top: f64, bottom: f64, near: f64, far: f64) -> Mat4 {
let x = 2.0 * near / (right - left);
let y = 2.0 * near / (top - bottom);
let a = (right + left) / (right - left);
let b = (top + bottom) / (top - bottom);
let c = -(far + near) / (far - near);
let d = (-2.0 * far * near) / (far - near);
[
x, 0.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, a, b, c, -1.0, 0.0, 0.0, d, 0.0,
]
}
#[allow(clippy::too_many_arguments)]
pub fn orthographic_projection(
left: f64,
right: f64,
top: f64,
bottom: f64,
near: f64,
far: f64,
zoom: f64,
view: Option<[f64; 6]>,
) -> Mat4 {
let dx = (right - left) / (2.0 * zoom);
let dy = (top - bottom) / (2.0 * zoom);
let cx = (right + left) / 2.0;
let cy = (top + bottom) / 2.0;
let (mut l, mut r, mut t, mut b) = (cx - dx, cx + dx, cy + dy, cy - dy);
if let Some([full_w, full_h, ox, oy, vw, vh]) = view {
let scale_w = (right - left) / full_w / zoom;
let scale_h = (top - bottom) / full_h / zoom;
l += scale_w * ox;
r = l + scale_w * vw;
t -= scale_h * oy;
b = t - scale_h * vh;
}
let w = 1.0 / (r - l);
let h = 1.0 / (t - b);
let p = 1.0 / (far - near);
let x = (r + l) * w;
let y = (t + b) * h;
let z = (far + near) * p;
let z_inv = -2.0 * p;
[
2.0 * w,
0.0,
0.0,
0.0,
0.0,
2.0 * h,
0.0,
0.0,
0.0,
0.0,
z_inv,
0.0,
-x,
-y,
-z,
1.0,
]
}
pub fn arrow_quaternion(dir: V3) -> Quat {
if dir[1] > 0.99999 {
[0.0, 0.0, 0.0, 1.0]
} else if dir[1] < -0.99999 {
[1.0, 0.0, 0.0, 0.0]
} else {
let axis = normalize([dir[2], 0.0, -dir[0]]);
let radians = dir[1].acos();
let half = radians / 2.0;
let s = half.sin();
[axis[0] * s, axis[1] * s, axis[2] * s, half.cos()]
}
}
fn arrow_cone() -> (Vec<V3>, Vec<[usize; 3]>) {
let (radius_top, radius_bottom, height, radial, hs) = (0.0f64, 0.5f64, 1.0f64, 5usize, 1usize);
let half = height / 2.0;
let mut positions: Vec<V3> = Vec::new();
let mut tris = Vec::new();
let mut rows = Vec::new();
for y in 0..=hs {
let v = y as f64 / hs as f64;
let radius = v * (radius_bottom - radius_top) + radius_top;
let mut row = Vec::new();
for x in 0..=radial {
let u = x as f64 / radial as f64;
let theta = u * std::f64::consts::PI * 2.0;
positions.push(f32v([
radius * theta.sin(),
-v * height + half,
radius * theta.cos(),
]));
row.push(positions.len() - 1);
}
rows.push(row);
}
for x in 0..radial {
for y in 0..hs {
let a = rows[y][x];
let b = rows[y + 1][x];
let c = rows[y + 1][x + 1];
let d = rows[y][x + 1];
tris.push([a, b, d]);
tris.push([b, c, d]);
}
}
let center_start = positions.len();
for _ in 1..=radial {
positions.push([0.0, -half, 0.0]);
}
let center_end = positions.len();
for x in 0..=radial {
let theta = x as f64 / radial as f64 * std::f64::consts::PI * 2.0;
positions.push(f32v([
radius_bottom * theta.sin(),
-half,
radius_bottom * theta.cos(),
]));
}
for x in 0..radial {
let c = center_start + x;
let i = center_end + x;
tris.push([i + 1, i, c]);
}
let positions = positions
.into_iter()
.map(|[x, y, z]| f32v([x, y - 0.5, z]))
.collect();
(positions, tris)
}
pub fn arrow(
helper_world: &Mat4,
length: f64,
head_length: f64,
head_width: f64,
) -> HelperGeometry {
let line = multiply(
helper_world,
&compose(
[0.0; 3],
[0.0, 0.0, 0.0, 1.0],
[1.0, (length - head_length).max(0.0001), 1.0],
),
);
let mut lines = Vec::new();
strip(&mut lines, &line, &[[0.0, 0.0, 0.0], [0.0, 1.0, 0.0]]);
let cone = multiply(
helper_world,
&compose(
[0.0, length, 0.0],
[0.0, 0.0, 0.0, 1.0],
[head_width, head_length, head_width],
),
);
let (positions, tris) = arrow_cone();
let mut mesh = Vec::with_capacity(tris.len() * 9);
for t in tris {
for i in t {
push(&mut mesh, apply(&cone, positions[i]));
}
}
HelperGeometry {
lines,
colors: Vec::new(),
mesh,
}
}
pub fn plane(normal: V3, constant: f64, size: f64) -> HelperGeometry {
let scale = [0.5 * size, 0.5 * size, 1.0];
let q = quat_from_rotation(&look_at_matrix(normal, [0.0; 3], UP));
let z = apply_quat([0.0, 0.0, 1.0], q);
let position = [z[0] * -constant, z[1] * -constant, z[2] * -constant];
let world = compose(position, q, scale);
let mut lines = Vec::new();
strip(
&mut lines,
&world,
&[
[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, 1.0, 0.0],
[-1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, -1.0, 0.0],
[1.0, 1.0, 0.0],
],
);
let mut mesh = Vec::new();
segments(
&mut mesh,
&world,
&[
[1.0, 1.0, 0.0],
[-1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, -1.0, 0.0],
],
);
HelperGeometry {
lines,
colors: Vec::new(),
mesh,
}
}
pub fn skeleton(root_world: &Mat4, bone_worlds: &[Mat4], parents: &[i32]) -> HelperGeometry {
let inv = invert(root_world);
let mut lines = Vec::new();
let mut colors = Vec::new();
for (i, world) in bone_worlds.iter().enumerate() {
let Some(&p) = parents.get(i) else { continue };
if p < 0 || p as usize >= bone_worlds.len() {
continue;
}
for m in [world, &bone_worlds[p as usize]] {
let local = f32v(position_of(&multiply(&inv, m)));
push(&mut lines, apply(root_world, local));
}
colors.extend_from_slice(&[0.0, 0.0, 1.0, 0.0, 1.0, 0.0]);
}
HelperGeometry {
lines,
colors,
mesh: Vec::new(),
}
}
const BOX_EDGES: [usize; 24] = [
0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4, 0, 4, 1, 5, 2, 6, 3, 7,
];
pub fn box_lines(min: V3, max: V3) -> HelperGeometry {
if max[0] < min[0] || max[1] < min[1] || max[2] < min[2] {
return HelperGeometry::default();
}
let corners: [V3; 8] = [
[max[0], max[1], max[2]],
[min[0], max[1], max[2]],
[min[0], min[1], max[2]],
[max[0], min[1], max[2]],
[max[0], max[1], min[2]],
[min[0], max[1], min[2]],
[min[0], min[1], min[2]],
[max[0], min[1], min[2]],
];
let mut lines = Vec::with_capacity(72);
for i in BOX_EDGES {
push(&mut lines, f32v(corners[i]));
}
HelperGeometry {
lines,
..Default::default()
}
}
pub fn object_bounds(worlds: &[Mat4], positions: &[&[f32]]) -> Option<(V3, V3)> {
let mut min = [f64::INFINITY; 3];
let mut max = [f64::NEG_INFINITY; 3];
for (world, pos) in worlds.iter().zip(positions) {
if pos.len() < 3 {
continue;
}
let mut lo = [f64::INFINITY; 3];
let mut hi = [f64::NEG_INFINITY; 3];
for p in pos.as_chunks::<3>().0 {
for k in 0..3 {
lo[k] = lo[k].min(p[k] as f64);
hi[k] = hi[k].max(p[k] as f64);
}
}
for i in 0..8 {
let c = [
if i & 1 == 0 { lo[0] } else { hi[0] },
if i & 2 == 0 { lo[1] } else { hi[1] },
if i & 4 == 0 { lo[2] } else { hi[2] },
];
let w = apply(world, c);
for k in 0..3 {
min[k] = min[k].min(w[k]);
max[k] = max[k].max(w[k]);
}
}
}
(min[0] <= max[0]).then_some((min, max))
}
pub fn box3(min: V3, max: V3) -> HelperGeometry {
if max[0] < min[0] || max[1] < min[1] || max[2] < min[2] {
return HelperGeometry::default();
}
let center = [
(min[0] + max[0]) * 0.5,
(min[1] + max[1]) * 0.5,
(min[2] + max[2]) * 0.5,
];
let half = [
(max[0] - min[0]) * 0.5,
(max[1] - min[1]) * 0.5,
(max[2] - min[2]) * 0.5,
];
let world = compose(center, [0.0, 0.0, 0.0, 1.0], half);
let unit: [V3; 8] = [
[1.0, 1.0, 1.0],
[-1.0, 1.0, 1.0],
[-1.0, -1.0, 1.0],
[1.0, -1.0, 1.0],
[1.0, 1.0, -1.0],
[-1.0, 1.0, -1.0],
[-1.0, -1.0, -1.0],
[1.0, -1.0, -1.0],
];
let mut lines = Vec::with_capacity(72);
for i in BOX_EDGES {
push(&mut lines, apply(&world, unit[i]));
}
HelperGeometry {
lines,
..Default::default()
}
}
pub fn vertex_normals(
object_world: &Mat4,
positions: &[f32],
normals: &[f32],
size: f64,
) -> HelperGeometry {
let m = object_world;
let (n11, n21, n31) = (m[0], m[1], m[2]);
let (n12, n22, n32) = (m[4], m[5], m[6]);
let (n13, n23, n33) = (m[8], m[9], m[10]);
let t11 = n33 * n22 - n32 * n23;
let t12 = n32 * n13 - n33 * n12;
let t13 = n23 * n12 - n22 * n13;
let det = n11 * t11 + n21 * t12 + n31 * t13;
let inv: [f64; 9] = if det == 0.0 {
[0.0; 9]
} else {
let d = 1.0 / det;
[
t11 * d,
(n31 * n23 - n33 * n21) * d,
(n32 * n21 - n31 * n22) * d,
t12 * d,
(n33 * n11 - n31 * n13) * d,
(n31 * n12 - n32 * n11) * d,
t13 * d,
(n21 * n13 - n23 * n11) * d,
(n22 * n11 - n21 * n12) * d,
]
};
let e = [
inv[0], inv[3], inv[6], inv[1], inv[4], inv[7], inv[2], inv[5], inv[8],
];
let count = (positions.len() / 3).min(normals.len() / 3);
let mut lines = Vec::with_capacity(count * 6);
for j in 0..count {
let p = apply(
object_world,
[
positions[j * 3] as f64,
positions[j * 3 + 1] as f64,
positions[j * 3 + 2] as f64,
],
);
let (x, y, z) = (
normals[j * 3] as f64,
normals[j * 3 + 1] as f64,
normals[j * 3 + 2] as f64,
);
let n = normalize([
e[0] * x + e[3] * y + e[6] * z,
e[1] * x + e[4] * y + e[7] * z,
e[2] * x + e[5] * y + e[8] * z,
]);
push(&mut lines, p);
push(
&mut lines,
[n[0] * size + p[0], n[1] * size + p[1], n[2] * size + p[2]],
);
}
HelperGeometry {
lines,
..Default::default()
}
}
pub fn rect_area_light(light_world: &Mat4, width: f64, height: f64) -> HelperGeometry {
let mut world = extract_rotation(light_world);
let scale = [0.5 * width, 0.5 * height, 1.0];
for c in 0..3 {
for r in 0..4 {
world[c * 4 + r] *= scale[c];
}
}
world[12] = light_world[12];
world[13] = light_world[13];
world[14] = light_world[14];
let mut lines = Vec::new();
strip(
&mut lines,
&world,
&[
[1.0, 1.0, 0.0],
[-1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, -1.0, 0.0],
[1.0, 1.0, 0.0],
],
);
let mut mesh = Vec::new();
segments(
&mut mesh,
&world,
&[
[1.0, 1.0, 0.0],
[-1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, 1.0, 0.0],
[-1.0, -1.0, 0.0],
[1.0, -1.0, 0.0],
],
);
HelperGeometry {
lines,
colors: Vec::new(),
mesh,
}
}
pub fn rect_area_light_color(color: [f32; 3], intensity: f32) -> [f32; 3] {
let mut c = [
color[0] * intensity,
color[1] * intensity,
color[2] * intensity,
];
let max = c[0].max(c[1]).max(c[2]);
if max > 1.0 {
let s = 1.0 / max;
c = [c[0] * s, c[1] * s, c[2] * s];
}
c
}
#[cfg(test)]
mod tests;