compound: the baked compound, with the sphere, capsule and material split out

README.md

@@ -34,8 +34,11 @@ so a test written against the reference reads the same here.
 | `aephysics.triangle_manifold` | one mesh triangle against a sphere, capsule or hull: back-side cull with hysteresis, GJK shallow, the separating axis test deep with the triangle's edges as zero-area faces, the feature recorded for the mesh contact's ghost-collision reduction | done, `test_triangle_manifold.ae` (1.5k checks); [same manifolds as the reference, within 10% on hulls](bench/RESULTS.md#triangle_manifold) |
 | `aephysics.mesh` | the triangle mesh: a BVH by binned SAH or median split with the triangles in depth-first order, vertex welding, edge flags, any scale including mirrored; overlap, ray cast, shape cast, the mover's planes, a box query | done, `test_mesh.ae` (1.6k checks); [same trees as the reference, traversals 1.7-2x](bench/RESULTS.md#mesh) |
 | `aephysics.height_field` | the height field: quantised heights on a fixed diagonal, materials and holes per cell, edge flags per triangle, either winding; overlap, ray and shape casts by a walk along the grid, the mover's planes, a box query | done, `test_height_field.ae` (113 checks, casts against a brute force over a wave); [same results as the reference, query at parity, casts 1.4-2x](bench/RESULTS.md#height_field) |
-| `aephysics.shape` | the shape of any kind: sphere and capsule geometry (mass, bounds, ray casts with the far-origin precision, casts, overlap, mover), the dispatch over every kind under a transform (bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy), the collision filters | done, `test_shape.ae` (440 checks); [same results as the reference, rays and masses at parity, radius casts 2.5x](bench/RESULTS.md#shape) |
-| `aephysics.compound` | the baked compound of child shapes under a static tree, with material and hull sharing | next |
+| `aephysics.material` | a surface's material (friction, restitution, rolling resistance, tangent velocity, user id) and its default | done |
+| `aephysics.sphere` | the sphere: mass, bounds, the ray cast in the closest-point form that keeps its precision far from the origin, the shape cast, overlap, the mover's plane | done, in `test_shape.ae` |
+| `aephysics.capsule` | the capsule: mass by cylinder and caps, bounds, the ray cast by the closest points of the lines with the near-parallel fallback, the shape cast, overlap, the mover's plane | done, in `test_shape.ae` |
+| `aephysics.compound` | the baked compound: capsules, hulls, meshes and spheres in one block under a static tree, hulls and meshes shared by content, materials by value; bounds, overlap, ray and shape casts, the box query, the mover's planes | done, `test_compound.ae` (159 checks); [same results as the reference, build 0.7x, queries 1.3-1.6x](bench/RESULTS.md#compound) |
+| `aephysics.shape` | the shape of any kind (sphere, capsule, hull, mesh, height field, compound) with the dispatch over every kind under a transform: bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy; the collision filters | done, `test_shape.ae` (440 checks); [same results as the reference, rays and masses at parity, radius casts 2.5x](bench/RESULTS.md#shape) |
 | `aephysics.dynamics` | bodies, contacts, the constraint graph, islands, the Soft Step solver, joints (spherical, revolute, prismatic, distance, motor, weld, wheel), sensors, the character mover, the world | |
 | `aephysics` | the public API | |
 

aephysics/capsule/module.ae

@@ -0,0 +1,197 @@
+// aephysics.capsule -- the capsule: its mass (a cylinder and its two
+// caps, the caps carried out by Steiner, turned onto the axis), bounds,
+// the ray against it by the closest points of the ray's line and the
+// axis with the near-parallel fallback through the axial circle (Real-
+// Time Collision Detection 5.1.9), the shape cast, the overlap, and the
+// character mover's plane against it.
+//
+// Box3D's capsule.c (Erin Catto, MIT), the reference this engine is
+// measured against. Names are the reference's without its prefix, in
+// snake case: b3RayCastCapsule is ray_cast_capsule.
+import std.string
+import aephysics.math
+import aephysics.core
+import aephysics.distance
+import aephysics.manifold
+import aephysics.mesh
+import aephysics.sphere
+
+exports (
+    compute_capsule_mass, compute_capsule_aabb, compute_swept_capsule_aabb, overlap_capsule,
+    ray_cast_capsule, shape_cast_capsule, collide_mover_and_capsule
+)
+
+extern sqrt(x: float) -> float
+
+// The capsule's mass: a cylinder and a sphere (the two caps) about the
+// midpoint, the caps carried out to the ends by Steiner, then turned
+// from the y axis onto the capsule's.
+compute_capsule_mass(c: Capsule, density: float) -> MassData {
+    c1 = c.center1
+    c2 = c.center2
+    r = c.radius
+    cylinder_height = math.distance(c1, c2)
+    cylinder_volume = math.PI * r * r * cylinder_height
+    cylinder_mass = cylinder_volume * density
+    sphere_volume = (4.0 / 3.0) * math.PI * r * r * r
+    sphere_mass = sphere_volume * density
+    inertia = math.add_mm(math.cylinder_inertia(cylinder_mass, r, cylinder_height), math.sphere_inertia(sphere_mass, r))
+    steiner = 0.125 * sphere_mass * (3.0 * r + 2.0 * cylinder_height) * cylinder_height
+    inertia.cx.x = inertia.cx.x + steiner
+    inertia.cz.z = inertia.cz.z + steiner
+    rotation = math.mat3_identity()
+    if cylinder_height * cylinder_height > 1000.0 * math.TINY {
+        direction = math.normalize(math.sub(c2, c1))
+        q = math.compute_quat_between_unit_vectors(math.vec3_axis_y(), direction)
+        rotation = math.make_matrix_from_quat(q)
+    }
+    return MassData { mass: sphere_mass + cylinder_mass, center: math.mul_sv(0.5, math.add(c1, c2)),
+                      inertia: math.mul_mm(rotation, math.mul_mm(inertia, math.transpose(rotation))) }
+}
+
+compute_capsule_aabb(c: Capsule, t: Transform) -> AABB {
+    center1 = math.transform_point(t, c.center1)
+    center2 = math.transform_point(t, c.center2)
+    extent = math.vec3(c.radius, c.radius, c.radius)
+    return AABB { lower: math.sub(math.min_vec3(center1, center2), extent), upper: math.add(math.max_vec3(center1, center2), extent) }
+}
+
+compute_swept_capsule_aabb(c: Capsule, xf1: Transform, xf2: Transform) -> AABB {
+    r = math.vec3(c.radius, c.radius, c.radius)
+    a = math.transform_point(xf1, c.center1)
+    b = math.transform_point(xf1, c.center2)
+    cc = math.transform_point(xf2, c.center1)
+    d = math.transform_point(xf2, c.center2)
+    return AABB { lower: math.sub(math.min_vec3(math.min_vec3(a, b), math.min_vec3(cc, d)), r),
+                  upper: math.add(math.max_vec3(math.max_vec3(a, b), math.max_vec3(cc, d)), r) }
+}
+
+overlap_capsule(c: Capsule, shape_transform: Transform, proxy: ShapeProxy) -> bool {
+    input = DistanceInput { proxy_a: sphere.capsule_proxy(c), proxy_b: proxy,
+                            transform: math.inv_mul_transforms(shape_transform, math.transform_identity()), use_radii: true }
+    cache = distance.empty_cache()
+    output = distance.shape_distance(&input, &cache, null, 0)
+    return output.distance < sphere.OVERLAP_SLOP
+}
+
+// The ray against the capsule: an origin inside the infinite cylinder is
+// inside the capsule, or casts against the nearer cap; otherwise the
+// closest points of the ray's line and the axis give the side hit (the
+// axial circle when they are nearly parallel), handed to a cap when the
+// hit falls beyond an end. Real-Time Collision Detection 5.1.9.
+ray_cast_capsule(c: Capsule, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput {
+    c1 = c.center1
+    c2 = c.center2
+    r = c.radius
+    output = distance.empty_cast_output()
+    d = math.sub(c2, c1)
+    // A short capsule is a sphere.
+    tol = 0.01 * math.LINEAR_SLOP
+    length_squared = math.length_squared(d)
+    if length_squared < tol * tol {
+        return sphere.ray_cast_sphere(Sphere { center: math.mul_sv(0.5, math.add(c1, c2)), radius: r }, origin, translation, max_fraction)
+    }
+    // The ray's origin from the first centre, projected onto the axis.
+    s = math.sub(origin, c1)
+    length = sqrt(length_squared)
+    axis = math.mul_sv(1.0 / length, d)
+    u = math.dot(s, axis)
+    c_axis = math.mul_sv(u, axis)
+    sc = math.sub(s, c_axis)
+    sc2 = math.length_squared(sc)
+    if sc2 < r * r {
+        // Inside the infinite cylinder: inside the capsule, or facing a cap.
+        u_clamped = math.clamp_float(u, 0.0, length)
+        cp = math.mul_sv(u_clamped, axis)
+        scp = math.sub(s, cp)
+        if math.length_squared(scp) < r * r {
+            output.hit = true
+            output.point = origin
+            return output
+        }
+        return sphere.ray_cast_sphere(Sphere { center: math.add(c1, cp), radius: r }, origin, translation, max_fraction)
+    }
+    // A zero-length ray out here starts outside, so it misses.
+    length_out = sphere.length_scratch()
+    ray_axis = math.get_length_and_normalize(length_out, translation)
+    ray_length = length_out[0]
+    if ray_length == 0.0 { return output }
+    // The projected ray entirely beyond an end misses.
+    v = u + max_fraction * math.dot(translation, axis)
+    if (u < 0.0 - r && v < 0.0 - r) || (length + r < u && length + r < v) { return output }
+
+    a1 = axis
+    a2 = ray_axis
+    a12 = math.dot(a1, a2)
+    // The ray's distance to the near intersection with the infinite cylinder, in length units.
+    tr = 0.0
+    det = 1.0 - a12 * a12
+    if det < math.EPSILON {
+        // Nearly parallel: the ray against the circle that is the cylinder
+        // seen along its axis, from the ray's origin.
+        perp = math.mul_sub(a2, a12, a1)
+        perp2 = math.length_squared(perp)
+        beta = math.dot(sc, perp)
+        gamma = sc2 - r * r
+        disc = beta * beta - perp2 * gamma
+        // Casting away from the axis, or never closing to the radius.
+        if beta >= 0.0 || disc < 0.0 { return output }
+        // The near root in the form that avoids the cancellation as the ray nears parallel.
+        tr = gamma / (0.0 - beta + sqrt(disc))
+    } else {
+        // The closest points of the infinite ray and the infinite axis.
+        inv_det = 1.0 / det
+        sa1 = u
+        sa2 = math.dot(s, a2)
+        t1 = (sa1 - a12 * sa2) * inv_det
+        t2 = (a12 * sa1 - sa2) * inv_det
+        p1 = math.mul_sv(t1, a1)
+        p2 = math.mul_add(s, t2, a2)
+        g = math.sub(p2, p1)
+        g2 = math.length_squared(g)
+        if g2 > r * r { return output }
+        // The ray into the cylinder, relative to the closest point (like the sphere).
+        h = sqrt((r * r - g2) * inv_det)
+        tr = t2 - h
+    }
+    if tr < 0.0 || max_fraction * ray_length < tr { return output }
+    // The hit's place along the axis.
+    tc = u + tr * a12
+    if tc < 0.0 { return sphere.ray_cast_sphere(Sphere { center: c1, radius: r }, origin, translation, max_fraction) }
+    if length < tc { return sphere.ray_cast_sphere(Sphere { center: c2, radius: r }, origin, translation, max_fraction) }
+    // On the side, relative to c1.
+    p = math.mul_add(s, tr, ray_axis)
+    output.point = math.add(c1, p)
+    output.normal = math.normalize(math.mul_sub(p, tc, axis))
+    output.fraction = math.clamp_float(tr / ray_length, 0.0, max_fraction)
+    output.hit = true
+    return output
+}
+
+shape_cast_capsule(c: Capsule, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput {
+    pair = ShapeCastPairInput { proxy_a: sphere.capsule_proxy(c), proxy_b: proxy, transform: math.transform_identity(),
+                                translation_b: translation, max_fraction: max_fraction, can_encroach: can_encroach }
+    return distance.shape_cast(&pair)
+}
+
+// The mover's plane against a capsule: the normal between the closest
+// points of the two axes, or, when they cross, perpendicular to the
+// mover's axis.
+collide_mover_and_capsule(planes: PlaneResult[], c: Capsule, mover: Capsule) -> int {
+    total_radius = mover.radius + c.radius
+    approach = math.segment_distance(c.center1, c.center2, mover.center1, mover.center2)
+    length_out = sphere.length_scratch()
+    normal = math.get_length_and_normalize(length_out, math.sub(approach.point2, approach.point1))
+    dist = length_out[0]
+    if dist > total_radius { return 0 }
+    if dist < math.LINEAR_SLOP {
+        mover_axis = math.get_length_and_normalize(length_out, math.sub(mover.center2, mover.center1))
+        normal = math.vec3_axis_y()
+        if length_out[0] > math.LINEAR_SLOP { normal = math.perp(mover_axis) }
+        dist = 0.0
+    }
+    planes[0] = PlaneResult { plane: Plane { normal: normal, offset: total_radius - dist }, point: approach.point1,
+                              triangle_index: 0, child_index: 0, material_index: 0 }
+    return 1
+}
+