Point mass matrix without skew's zero products; box hulls and the block hash

aephysics/basics/module.ae

@@ -374,15 +374,38 @@ map_bytes(m: *LongMap) -> int { return m.capacity * sizeof(MapItem) }
 // never zero. The reference uses rapidhash for the same purpose: identity
 // of hull and mesh data.
 hash_bytes(block: ptr, byte_count: int) -> long {
-    words = block as long[]
+    // Four lanes take every fourth word, so their mixes (each a chain of
+    // multiplies) overlap instead of waiting on one another: a word at a
+    // time through one chain was most of building a box hull.
     count = byte_count / 8
-    h = 0x9E3779B9 as long
+    m32 = 4294967295 as long
+    a = 0x9E3779B9 as long
+    b = 0x85EBCA6B as long
+    c = 0xC2B2AE35 as long
+    d = 0x27D4EB2F as long
     i = 0
+    while i + 4 <= count {
+        w0 = native.load_word(block, i as long)
+        w1 = native.load_word(block, (i + 1) as long)
+        w2 = native.load_word(block, (i + 2) as long)
+        w3 = native.load_word(block, (i + 3) as long)
+        a = mix32(mix32(a ^ (w0 & m32)) ^ lsr(w0, 32))
+        b = mix32(mix32(b ^ (w1 & m32)) ^ lsr(w1, 32))
+        c = mix32(mix32(c ^ (w2 & m32)) ^ lsr(w2, 32))
+        d = mix32(mix32(d ^ (w3 & m32)) ^ lsr(w3, 32))
+        i = i + 4
+    }
     while i < count {
-        h = key_hash(h ^ words[i]) + (i as long)
+        w = native.load_word(block, i as long)
+        a = mix32(mix32(a ^ (w & m32)) ^ lsr(w, 32))
         i = i + 1
     }
-    h = key_hash(h)
+    // The lanes and the length folded into two halves of the result.
+    low = mix32(mix32(a ^ (count as long)) ^ b)
+    high = mix32(mix32(c ^ low) ^ d)
+    // high kept to 31 bits: a shift into the sign bit is undefined in C, and
+    // GCC compiled it differently where the hash was inlined.
+    h = ((high & (2147483647 as long)) << 32) | low
     if h == (0 as long) { return 1 as long }
     return h
 }

aephysics/hull/module.ae

@@ -1703,26 +1703,9 @@ create_rock(radius: float) -> *HullData {
 
 // --- the box hull ----------------------------------------------------------------
 
-// The box's topology: 8 vertices, 24 half-edges, 6 faces, as the
-// reference bakes it; the geometry is filled in per box.
-box_edge_table(index: int, field: int) -> int {
-    // next, twin, origin, face per half-edge
-    table = [ 2, 1, 2, 0,  17, 0, 1, 5,  4, 3, 1, 0,  20, 2, 5, 3,  6, 5, 5, 0,  23, 4, 6, 4,  0, 7, 6, 0,  18, 6, 2, 2,
-              10, 9, 0, 1,  21, 8, 3, 5,  12, 11, 3, 1,  16, 10, 7, 2,  14, 13, 7, 1,  19, 12, 4, 4,  8, 15, 4, 1,  22, 14, 0, 3,
-              7, 17, 3, 2,  9, 16, 2, 5,  11, 19, 6, 2,  5, 18, 7, 4,  15, 21, 1, 3,  1, 20, 0, 5,  3, 23, 4, 3,  13, 22, 5, 4 ]
-    return table[index * 4 + field]
-}
-
-box_vertex_edge(index: int) -> int {
-    table = [ 8, 1, 0, 9, 13, 3, 5, 11 ]
-    return table[index]
-}
-
-box_face_edge(index: int) -> int {
-    table = [ 0, 8, 16, 20, 19, 21 ]
-    return table[index]
-}
-
+// The signs of a box corner's coordinates, in the order of the box's
+// vertices (8 vertices, 24 half-edges, 6 faces, as the reference bakes
+// them; the geometry is filled in per box).
 box_corner_sign(index: int) -> Vec3 {
     if index == 0 { return math.vec3(1.0, 1.0, 1.0) }
     if index == 1 { return math.vec3(0.0 - 1.0, 1.0, 1.0) }
@@ -1752,23 +1735,53 @@ make_transformed_box_hull(hx: float, hy: float, hz: float, t: Transform) -> *Hul
     faces = hull_faces(box)
     points = hull_points(box)
     planes = hull_planes(box)
+    // The box's topology, as the reference bakes it, written out (a table
+    // local to a helper called per entry was copied onto the stack on every
+    // call, most of a box's build).
+    vertices[0] = 8
+    vertices[1] = 1
+    vertices[2] = 0
+    vertices[3] = 9
+    vertices[4] = 13
+    vertices[5] = 3
+    vertices[6] = 5
+    vertices[7] = 11
+    // Half-edges: next, twin, origin, face.
+    edges[0] = HalfEdge { next: 2, twin: 1, origin: 2, face: 0 }
+    edges[1] = HalfEdge { next: 17, twin: 0, origin: 1, face: 5 }
+    edges[2] = HalfEdge { next: 4, twin: 3, origin: 1, face: 0 }
+    edges[3] = HalfEdge { next: 20, twin: 2, origin: 5, face: 3 }
+    edges[4] = HalfEdge { next: 6, twin: 5, origin: 5, face: 0 }
+    edges[5] = HalfEdge { next: 23, twin: 4, origin: 6, face: 4 }
+    edges[6] = HalfEdge { next: 0, twin: 7, origin: 6, face: 0 }
+    edges[7] = HalfEdge { next: 18, twin: 6, origin: 2, face: 2 }
+    edges[8] = HalfEdge { next: 10, twin: 9, origin: 0, face: 1 }
+    edges[9] = HalfEdge { next: 21, twin: 8, origin: 3, face: 5 }
+    edges[10] = HalfEdge { next: 12, twin: 11, origin: 3, face: 1 }
+    edges[11] = HalfEdge { next: 16, twin: 10, origin: 7, face: 2 }
+    edges[12] = HalfEdge { next: 14, twin: 13, origin: 7, face: 1 }
+    edges[13] = HalfEdge { next: 19, twin: 12, origin: 4, face: 4 }
+    edges[14] = HalfEdge { next: 8, twin: 15, origin: 4, face: 1 }
+    edges[15] = HalfEdge { next: 22, twin: 14, origin: 0, face: 3 }
+    edges[16] = HalfEdge { next: 7, twin: 17, origin: 3, face: 2 }
+    edges[17] = HalfEdge { next: 9, twin: 16, origin: 2, face: 5 }
+    edges[18] = HalfEdge { next: 11, twin: 19, origin: 6, face: 2 }
+    edges[19] = HalfEdge { next: 5, twin: 18, origin: 7, face: 4 }
+    edges[20] = HalfEdge { next: 15, twin: 21, origin: 1, face: 3 }
+    edges[21] = HalfEdge { next: 1, twin: 20, origin: 0, face: 5 }
+    edges[22] = HalfEdge { next: 3, twin: 23, origin: 4, face: 3 }
+    edges[23] = HalfEdge { next: 13, twin: 22, origin: 5, face: 4 }
+    faces[0] = 0
+    faces[1] = 8
+    faces[2] = 16
+    faces[3] = 20
+    faces[4] = 19
+    faces[5] = 21
     i = 0
     while i < 8 {
-        vertices[i] = box_vertex_edge(i)
         points[i] = math.transform_point(t, math.mul(box_corner_sign(i), h))
         i = i + 1
     }
-    i = 0
-    while i < 24 {
-        edges[i] = HalfEdge { next: box_edge_table(i, 0), twin: box_edge_table(i, 1),
-                              origin: box_edge_table(i, 2), face: box_edge_table(i, 3) }
-        i = i + 1
-    }
-    i = 0
-    while i < 6 {
-        faces[i] = box_face_edge(i)
-        i = i + 1
-    }
     lower = math.neg(h)
     planes[0] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.neg(math.vec3_axis_x()), lower))
     planes[1] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.vec3_axis_x(), h))

aephysics/joint_solver/module.ae

@@ -24,7 +24,7 @@ import aephysics.dynamics
 import aephysics.contact_solver
 
 exports (
-    prepare_joint, warm_start_joint, solve_joint, dummy_state, prepare_joints, warm_start_joints, solve_joints, prepare_joints_range, warm_start_joints_range, solve_joints_range,
+    prepare_joint, warm_start_joint, solve_joint, dummy_state, point_mass_matrix, prepare_joints, warm_start_joints, solve_joints, prepare_joints_range, warm_start_joints_range, solve_joints_range,
     joint_get_constraint_force, joint_get_constraint_torque, get_joint_constraint_force, get_joint_constraint_torque,
     prepare_distance_joint, warm_start_distance_joint, solve_distance_joint,
     prepare_motor_joint, warm_start_motor_joint, solve_motor_joint,
@@ -146,12 +146,28 @@ perp_axis(quat_a: Quat, rel_q: Quat, axis: Vec3) -> Vec3 {
     return math.mul_sv(0.5, math.rotate_vector(quat_a, math.add(math.mul_sv(rel_q.w, axis), math.cross(math.qv(rel_q), axis))))
 }
 
+// skew(r) i skew(r), the products with skew's zeros left out: each
+// surviving term is the one mul_mm forms, in the order it adds them, and
+// a product with zero added to a sum is exact, so the result is
+// mul_mm(skew(r), mul_mm(i, skew(r))) to the bit at a quarter of the work.
+skew_sandwich(i: *Matrix3, r: Vec3) -> Matrix3 {
+    // m = i skew(r), column by column.
+    mx = math.vec3(i.cy.x * r.z + i.cz.x * (0.0 - r.y), i.cy.y * r.z + i.cz.y * (0.0 - r.y), i.cy.z * r.z + i.cz.z * (0.0 - r.y))
+    my = math.vec3(i.cx.x * (0.0 - r.z) + i.cz.x * r.x, i.cx.y * (0.0 - r.z) + i.cz.y * r.x, i.cx.z * (0.0 - r.z) + i.cz.z * r.x)
+    mz = math.vec3(i.cx.x * r.y + i.cy.x * (0.0 - r.x), i.cx.y * r.y + i.cy.y * (0.0 - r.x), i.cx.z * r.y + i.cy.z * (0.0 - r.x))
+    // skew(r) m.
+    return Matrix3 { cx: skew_times(r, mx), cy: skew_times(r, my), cz: skew_times(r, mz) }
+}
+
+// skew(r) c: mul_mv's terms without its zero ones.
+skew_times(r: Vec3, c: Vec3) -> Vec3 {
+    return math.vec3((0.0 - r.z) * c.y + r.y * c.z, r.z * c.x + (0.0 - r.x) * c.z, (0.0 - r.y) * c.x + r.x * c.y)
+}
+
 // The point constraint's mass matrix: (mA + mB) I - skew(rA) iA skew(rA) - skew(rB) iB skew(rB).
 point_mass_matrix(m_a: float, i_a: Matrix3, r_a: Vec3, m_b: float, i_b: Matrix3, r_b: Vec3) -> Matrix3 {
-    s_a = math.skew(r_a)
-    s_b = math.skew(r_b)
-    k_a = math.mul_mm(s_a, math.mul_mm(i_a, s_a))
-    k_b = math.mul_mm(s_b, math.mul_mm(i_b, s_b))
+    k_a = skew_sandwich(&i_a, r_a)
+    k_b = skew_sandwich(&i_b, r_b)
     k = math.negate_mat3(math.add_mm(k_a, k_b))
     k.cx.x = k.cx.x + m_a + m_b
     k.cy.y = k.cy.y + m_a + m_b

aephysics/native/aephysics_inline.h

@@ -43,4 +43,15 @@ static inline int aephysics_atomic_cas(void *p, int expected, int desired)
     return __atomic_compare_exchange_n((int *)p, &expected, desired, 0, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST);
 }
 
+// The 8-byte word at `index` words into `p`, read through memcpy so a
+// block written as doubles, ints and structs can be read as words without
+// breaking C's aliasing rule (a plain int64_t* read let GCC move the read
+// ahead of the stores it follows once the hash was inlined).
+static inline long long aephysics_load_word(const void *p, long long index)
+{
+    long long word;
+    __builtin_memcpy(&word, (const char *)p + 8 * index, 8);
+    return word;
+}
+
 #endif

aephysics/native/module.ae

@@ -11,7 +11,7 @@ exports (
     MAX_WORKERS, worker_index, set_worker_index, yield_thread, pause, processor_count, scratch,
     atomic_load, atomic_store, atomic_add, atomic_or, atomic_cas,
     semaphore_new, semaphore_free, semaphore_wait, semaphore_signal,
-    name_copy, name_text, name_free, prefetch
+    name_copy, name_text, name_free, prefetch, load_word
 )
 
 // The most workers a world can have: every module's scratch is a block
@@ -33,13 +33,18 @@ extern aephysics_semaphore_free(s: ptr)
 extern aephysics_semaphore_wait(s: ptr)
 extern aephysics_semaphore_signal(s: ptr, count: int)
 extern aephysics_prefetch(p: ptr) @c_import
+extern aephysics_load_word(p: ptr, index: long) -> long @c_import
 extern aephysics_name_copy(s: string) -> ptr
 extern aephysics_name_text(name: ptr) -> string
 extern free(p: ptr)
 
 // The cache line at `p` fetched ahead of its use; harmless on any pointer.
 prefetch(p: ptr) { aephysics_prefetch(p) }
 
+// The word at `index` words into `p`, safe to read whatever the block was
+// written as (C's aliasing rule).
+load_word(p: ptr, index: long) -> long { return aephysics_load_word(p, index) }
+
 // A name the world keeps: a plain copy of `s` (null for none), its text
 // (empty for none), and its release.
 name_copy(s: string) -> ptr { return aephysics_name_copy(s) }

bench/RESULTS.md

@@ -120,14 +120,24 @@ on both.
 |---|---|---|
 | 200 sphere hulls, 64 points to 32 | 4.7 ms | **2.9** |
 | 20 cube hulls, 4,096 points to 8 | 5.2 | **2.5** |
-| 2,000 box hulls | 2.0 | **0.17** |
+| 2,000 box hulls | 0.62 (2.0 before #108) | **0.17** |
 
 Both produce 6,554 vertices and 12,106 faces in total: the same hulls.
 The builder runs at 1.6-2x the reference's time (doubles, indices in
-place of pointers, structs passed by value); the box hull is a heap block
-here where the reference's is a stack value, so its cost is the
-allocation and the hash. Hull construction is a load-time cost, not a
-per-step one, so this is recorded rather than chased.
+place of pointers, structs passed by value). Hull construction is a
+load-time cost, not a per-step one.
+
+The box hulls were not the allocation (#108, 2026-10-05). The box's
+topology came from helpers holding their table as a local array, so each
+call copied the whole table onto the stack, 96 times a box. And the block
+hash ran every word through one chain of multiplies. Now the topology is
+written out and the hash runs four lanes: 2.9 ms to 0.62 ms for the 2,000
+boxes. The faster hash also took about 10% off the mesh and height field
+builds (median mesh 234-260 to 207-224 ms, height field 172-181 to
+152-161). The hash now reads its words through `memcpy`
+(`native.load_word`). Read through an integer pointer, as before, GCC
+moved the reads ahead of the writes once the hash was inlined, and the
+compound benchmark's material table found 253 materials instead of 2.
 
 ## distance
 
@@ -660,6 +670,25 @@ instead of five. That took the joint prepare from 1.43-1.49 to 1.26-1.37 ms
 and the many pyramids' wide prepare from 3.53-3.60 to 3.38-3.49. The
 large pyramid was about even.
 
+Then #108: the profile put a sixth of the joint grid in
+`point_mass_matrix`, a real call (GCC didn't inline it) forming
+`skew(r) i skew(r)` through two full 3x3 products per body. It now leaves
+out the products with skew's zeros. Every surviving term is the one
+`mul_mm` forms, added in the same order, and adding an exact zero product
+changes nothing, so the matrix is the same to the bit
+(`tests/test_joint_solver.ae` checks 2,000 random cases). Joint grid,
+interleaved, 4 rounds:
+
+| stage | before | after | Box3D `e77352c` |
+|---|---|---|---|
+| solve | 3.87-4.23 ms | 3.09-3.38 | 3.33 |
+| relax | 3.87-4.22 | 3.01-3.22 | 3.22 |
+| step | 13.6-14.5 | 11.5-12.7 | 10.4-10.6 |
+
+The joint solve and relax are now level with the reference's. The warm
+start (1.9-2.2 ms against 1.31) and the prepare (1.2-1.4 against 0.61)
+are what's left.
+
 What's left of the gap is the records' size in double precision. Our
 `JointSim` is about 900 bytes, against the reference's float union. A
 `BodyState` is 112 bytes against 64.

tests/test_compound.ae

@@ -942,10 +942,49 @@ test_brute_force_rays() {
     hull.destroy_hull(box)
 }
 
+// Many children over two materials keep two slots, build after build, the
+// benchmark's scene. The material table hashes each material right after
+// writing its words; when the hash read them through an integer pointer,
+// GCC moved the reads ahead of the writes in the benchmark (not in this
+// program: the inlining differs) and most children got a slot of their own.
+test_two_materials_many_children() {
+    mat_a = make_material(0.6, 0)
+    mat_b = make_material(0.3, 0)
+    box = hull.make_box_hull(0.4, 0.4, 0.4)
+    d = make_defs(500, 500, 0, 1000)
+    spheres = d.spheres as CompoundSphereDef[]
+    capsules = d.capsules as CompoundCapsuleDef[]
+    hulls = d.hulls as CompoundHullDef[]
+    i = 0
+    while i < 2000 {
+        c = math.vec3((2 * (i % 20)) as float, (2 * ((i / 20) % 10)) as float, (2 * (i / 200)) as float)
+        mat = mat_a
+        if (i & 1) != 0 { mat = mat_b }
+        if i < 1000 {
+            spheres[i] = compound.compound_sphere_def(manifold.sphere(c, 0.5), mat)
+        } else if i < 1500 {
+            capsules[i - 1000] = compound.compound_capsule_def(manifold.capsule(math.vec3(c.x - 0.4, c.y, c.z), math.vec3(c.x + 0.4, c.y, c.z), 0.3), mat)
+        } else {
+            hulls[i - 1500] = compound.compound_hull_def(box, Transform { p: c, q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.3) }, mat)
+        }
+        i = i + 1
+    }
+    k = 0
+    while k < 3 {
+        data = compound.create_compound(&d.def)
+        ensure("two materials over two thousand children (${data.material_count})", data.material_count == 2)
+        compound.destroy_compound(data)
+        k = k + 1
+    }
+    free_defs(d)
+    hull.destroy_hull(box)
+}
+
 main() {
     before = basics.alloc_count()
     test_create()
     test_materials()
+    test_two_materials_many_children()
     test_sharing()
     test_children_and_bounds()
     test_casts()

tests/test_joint_solver.ae

@@ -713,6 +713,54 @@ test_parallel_motor_wheel() {
     dynamics.destroy_world(f.world_id)
 }
 
+// The point constraint's mass matrix leaves out skew's zero products; it
+// must still be the full product to the bit, over a spread of anchors,
+// masses and (symmetric and not) inertias.
+var g_seed = 12345
+
+next_unit() -> float {
+    g_seed = (g_seed * 1103515245 + 12345) & 2147483647
+    return (g_seed as float) / 1073741824.0 - 1.0
+}
+
+random_vec3(scale: float) -> Vec3 { return math.vec3(scale * next_unit(), scale * next_unit(), scale * next_unit()) }
+
+full_point_mass_matrix(m_a: float, i_a: Matrix3, r_a: Vec3, m_b: float, i_b: Matrix3, r_b: Vec3) -> Matrix3 {
+    s_a = math.skew(r_a)
+    s_b = math.skew(r_b)
+    k = math.negate_mat3(math.add_mm(math.mul_mm(s_a, math.mul_mm(i_a, s_a)), math.mul_mm(s_b, math.mul_mm(i_b, s_b))))
+    k.cx.x = k.cx.x + m_a + m_b
+    k.cy.y = k.cy.y + m_a + m_b
+    k.cz.z = k.cz.z + m_a + m_b
+    return k
+}
+
+same_vec3(a: Vec3, b: Vec3) -> bool { return a.x == b.x && a.y == b.y && a.z == b.z }
+
+test_point_mass_matrix() {
+    mismatches = 0
+    n = 0
+    while n < 2000 {
+        scale = 1.0
+        if n % 3 == 1 { scale = 1000.0 }
+        if n % 3 == 2 { scale = 0.001 }
+        i_a = math.matrix3(random_vec3(scale), random_vec3(scale), random_vec3(scale))
+        i_b = math.matrix3(random_vec3(scale), random_vec3(scale), random_vec3(scale))
+        r_a = random_vec3(1.0 / scale)
+        r_b = random_vec3(2.0)
+        m_a = next_unit() + 1.0
+        m_b = 0.0
+        if n % 2 == 0 { m_b = next_unit() + 1.0 }
+        got = joint_solver.point_mass_matrix(m_a, i_a, r_a, m_b, i_b, r_b)
+        want = full_point_mass_matrix(m_a, i_a, r_a, m_b, i_b, r_b)
+        if same_vec3(got.cx, want.cx) == false || same_vec3(got.cy, want.cy) == false || same_vec3(got.cz, want.cz) == false {
+            mismatches = mismatches + 1
+        }
+        n = n + 1
+    }
+    ensure("the point mass matrix is the full skew product to the bit (${mismatches} of 2000 differ)", mismatches == 0)
+}
+
 main() {
     dynamics.reserve_scratch()
     before = basics.alloc_count()
@@ -723,6 +771,7 @@ main() {
     test_prismatic()
     test_spherical()
     test_parallel_motor_wheel()
+    test_point_mass_matrix()
     hull.destroy_hull(g_cube as *HullData)
     // The scratch stays allocated: the worlds' block, the masses, the dummy state (the modules' scratch is reserved before the count).
     ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 3)