diff --git a/README.md b/README.md index bac6610..9b297f5 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,8 @@ 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` | shapes with mass properties, ray and shape casts per shape, compounds | next | +| `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.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 | | diff --git a/aephysics/shape/module.ae b/aephysics/shape/module.ae new file mode 100644 index 0000000..68286db --- /dev/null +++ b/aephysics/shape/module.ae @@ -0,0 +1,716 @@ +// aephysics.shape -- the shape: one of a sphere, a capsule, a hull, a +// mesh or a height field (the compound comes with its own module), with +// what every query needs regardless of kind: the bounds under a +// transform, the swept and fat bounds, the centroid, the areas, the mass +// properties, the extent for sleeping, the ray cast, the shape cast, the +// overlap, the character mover's planes and the proxy; and the sphere's +// and the capsule's own geometry, which had no module of their own. +// +// The shapes are Box3D's (Erin Catto, MIT) sphere.c, capsule.c and the +// geometric half of shape.c, the reference this engine is measured +// against: the ray against a sphere by the closest point on the ray to +// the centre (the Ray Tracing Gems form that keeps its precision far +// from the origin), the ray against a capsule by the closest points of +// the two lines with the near-parallel fallback through the axial +// circle, the initial-overlap convention (the origin, fraction zero, no +// normal), and the mover's deep-overlap directions. Names are the +// reference's without its prefix, in snake case: b3RayCastCapsule is +// ray_cast_capsule. The world-bound half of shape.c (creation on a body, +// the broad-phase proxy, materials, events, filters at run time) comes +// with the dynamics. +// +// Differences: no union; a Shape holds every kind's fields and a kind. +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.height_field + +exports ( + Shape, Filter, QueryFilter, + SHAPE_SPHERE, SHAPE_CAPSULE, SHAPE_HULL, SHAPE_MESH, SHAPE_HEIGHT_FIELD, SHAPE_COMPOUND, SHAPE_KIND_COUNT, + OVERLAP_SLOP, + sphere_shape, capsule_shape, hull_shape, mesh_shape, height_field_shape, + default_filter, default_query_filter, should_shapes_collide, should_query_collide, is_convex, + compute_sphere_mass, compute_sphere_aabb, compute_swept_sphere_aabb, overlap_sphere, + ray_cast_sphere, ray_cast_hollow_sphere, shape_cast_sphere, collide_mover_and_sphere, + compute_capsule_mass, compute_capsule_aabb, compute_swept_capsule_aabb, overlap_capsule, + ray_cast_capsule, shape_cast_capsule, collide_mover_and_capsule, + collide_mover_and_hull, + compute_shape_aabb, compute_fat_shape_aabb, compute_swept_shape_aabb, get_shape_centroid, + get_shape_area, get_shape_projected_area, compute_shape_mass, compute_shape_extent, + ray_cast_shape, shape_cast_shape, overlap_shape, collide_mover, make_shape_proxy, + farthest_point_on_aabb +) + +extern sqrt(x: float) -> float + +const SHAPE_SPHERE = 0 +const SHAPE_CAPSULE = 1 +const SHAPE_HULL = 2 +const SHAPE_MESH = 3 +const SHAPE_HEIGHT_FIELD = 4 +const SHAPE_COMPOUND = 5 +const SHAPE_KIND_COUNT = 6 + +// Closer than this is an overlap (a tenth of the linear slop). +const OVERLAP_SLOP = 0.0005 + +// A shape of one kind: the fields of the other kinds are unused. +struct Shape { + kind: int + density: float + material_count: int + sphere: Sphere + capsule: Capsule + hull: *HullData + mesh: Mesh + height_field: *HeightFieldData + compound: ptr // *CompoundData, the next layer +} + +// Who collides with whom: a shape is in the categories of its bits and +// collides with the categories in its mask; a shared positive group +// always collides, a shared negative group never. +struct Filter { + category_bits: long + mask_bits: long + group_index: int +} + +struct QueryFilter { + category_bits: long + mask_bits: long +} + +// --- construction --------------------------------------------------------------------- + +empty_shape() -> Shape { + return Shape { kind: SHAPE_SPHERE, density: 1.0, material_count: 1, + sphere: Sphere { center: math.vec3_zero(), radius: 0.0 }, + capsule: Capsule { center1: math.vec3_zero(), center2: math.vec3_zero(), radius: 0.0 }, + hull: null, mesh: Mesh { data: null, scale: math.vec3_one() }, height_field: null, compound: null } +} + +sphere_shape(s: Sphere, density: float) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_SPHERE + shape.density = density + shape.sphere = s + return shape +} + +capsule_shape(c: Capsule, density: float) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_CAPSULE + shape.density = density + shape.capsule = c + return shape +} + +hull_shape(h: *HullData, density: float) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_HULL + shape.density = density + shape.hull = h + return shape +} + +mesh_shape(m: Mesh, material_count: int) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_MESH + shape.density = 0.0 + shape.mesh = m + shape.material_count = math.max_int(material_count, 1) + return shape +} + +height_field_shape(h: *HeightFieldData, material_count: int) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_HEIGHT_FIELD + shape.density = 0.0 + shape.height_field = h + shape.material_count = math.max_int(material_count, 1) + return shape +} + +// --- filters ---------------------------------------------------------------------------- + +default_filter() -> Filter { return Filter { category_bits: 1 as long, mask_bits: all_bits(), group_index: 0 } } +default_query_filter() -> QueryFilter { return QueryFilter { category_bits: 1 as long, mask_bits: all_bits() } } + +all_bits() -> long { return (0 as long) - (1 as long) } + +should_shapes_collide(a: Filter, b: Filter) -> bool { + if a.group_index == b.group_index && a.group_index != 0 { return a.group_index > 0 } + return (a.mask_bits & b.category_bits) != (0 as long) && (a.category_bits & b.mask_bits) != (0 as long) +} + +should_query_collide(shape_filter: Filter, query_filter: QueryFilter) -> bool { + return (shape_filter.category_bits & query_filter.mask_bits) != (0 as long) && + (shape_filter.mask_bits & query_filter.category_bits) != (0 as long) +} + +is_convex(kind: int) -> bool { return kind == SHAPE_SPHERE || kind == SHAPE_CAPSULE || kind == SHAPE_HULL } + +// --- the sphere ----------------------------------------------------------------------------- + +compute_sphere_mass(s: Sphere, density: float) -> MassData { + radius = s.radius + volume = 4.0 / 3.0 * math.PI * radius * radius * radius + mass = volume * density + ixx = 0.4 * mass * radius * radius + // The inertia is about the centre of mass. + return MassData { mass: mass, center: s.center, inertia: math.make_diagonal_matrix(ixx, ixx, ixx) } +} + +compute_sphere_aabb(s: Sphere, t: Transform) -> AABB { + center = math.transform_point(t, s.center) + extent = math.vec3(s.radius, s.radius, s.radius) + return AABB { lower: math.sub(center, extent), upper: math.add(center, extent) } +} + +compute_swept_sphere_aabb(s: Sphere, xf1: Transform, xf2: Transform) -> AABB { + r = math.vec3(s.radius, s.radius, s.radius) + center1 = math.transform_point(xf1, s.center) + center2 = math.transform_point(xf2, s.center) + return AABB { lower: math.sub(math.min_vec3(center1, center2), r), upper: math.add(math.max_vec3(center1, center2), r) } +} + +var g_center: ptr = null // Vec3[2]: a sphere's centre or a capsule's two, for a proxy + +center_buffer() -> Vec3[] { + if g_center == null { g_center = core.alloc(2 * sizeof(Vec3)) } + return g_center as Vec3[] +} + +sphere_proxy(s: Sphere) -> ShapeProxy { + buffer = center_buffer() + buffer[0] = s.center + return distance.shape_proxy(g_center, 1, s.radius) +} + +capsule_proxy(c: Capsule) -> ShapeProxy { + buffer = center_buffer() + buffer[0] = c.center1 + buffer[1] = c.center2 + return distance.shape_proxy(g_center, 2, c.radius) +} + +overlap_sphere(s: Sphere, shape_transform: Transform, proxy: ShapeProxy) -> bool { + input = DistanceInput { proxy_a: sphere_proxy(s), 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 < OVERLAP_SLOP +} + +// The ray against the sphere through the closest point on the ray to the +// centre, which keeps the precision of a far origin (Ray Tracing Gems +// 2019, and codercorner.com/blog/?p=321). A ray starting inside, or of +// zero length inside, reports its origin at fraction zero with no normal. +ray_cast_sphere(s: Sphere, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + output = distance.empty_cast_output() + p = s.center + // The ray with the centre at the origin. + shifted = math.sub(origin, p) + r = s.radius + rr = r * r + length_out = length_scratch() + d = math.get_length_and_normalize(length_out, translation) + length = length_out[0] + if length == 0.0 { + if math.length_squared(shifted) < rr { + output.point = origin + output.hit = true + } + return output + } + // The closest point on the line to the centre: dot(s + t d, d) = 0. + t = 0.0 - math.dot(shifted, d) + c = math.mul_add(shifted, t, d) + cc = math.dot(c, c) + if cc > rr { return output } + h = sqrt(rr - cc) + fraction = t - h + if fraction < 0.0 || max_fraction * length < fraction { + // The intersection is off the segment; an origin inside still counts. + if math.length_squared(shifted) < rr { + output.point = origin + output.hit = true + } + return output + } + hit_point = math.mul_add(shifted, fraction, d) + output.fraction = fraction / length + if output.fraction > max_fraction { output.fraction = max_fraction } + output.normal = math.normalize(hit_point) + output.point = math.mul_add(p, s.radius, output.normal) + output.hit = true + return output +} + +// The same against the sphere's surface only, so a ray from inside hits +// the far side on its way out. +ray_cast_hollow_sphere(s: Sphere, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + output = distance.empty_cast_output() + p = s.center + shifted = math.sub(origin, p) + d = math.normalize(translation) + t = 0.0 - math.dot(shifted, d) + c = math.mul_add(shifted, t, d) + cc = math.dot(c, c) + r = s.radius + rr = r * r + if cc > rr { return output } + h = sqrt(rr - cc) + fraction = t - h + if fraction < 0.0 { fraction = t + h } + if fraction < 0.0 { return output } + if fraction > max_fraction { return output } + hit_point = math.mul_add(shifted, fraction, d) + output.fraction = fraction + output.normal = math.normalize(hit_point) + output.point = math.mul_add(p, s.radius, output.normal) + output.hit = true + return output +} + +shape_cast_sphere(s: Sphere, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + pair = ShapeCastPairInput { proxy_a: sphere_proxy(s), proxy_b: proxy, transform: math.transform_identity(), + translation_b: translation, max_fraction: max_fraction, can_encroach: can_encroach } + return distance.shape_cast(&pair) +} + +var g_length: ptr = null // float[1], the out-parameter of get_length_and_normalize + +length_scratch() -> float[] { + if g_length == null { g_length = core.alloc(8) } + return g_length as float[] +} + +// The mover's plane against a sphere: the normal from the sphere to the +// closest point of the mover's axis, or, when the axis runs through the +// centre, perpendicular to the axis. Returns how many planes (0 or 1). +collide_mover_and_sphere(planes: PlaneResult[], s: Sphere, mover: Capsule) -> int { + total_radius = mover.radius + s.radius + closest = math.point_to_segment_distance(mover.center1, mover.center2, s.center) + length_out = length_scratch() + normal = math.get_length_and_normalize(length_out, math.sub(closest, s.center)) + dist = length_out[0] + if dist > total_radius { return 0 } + if dist < math.LINEAR_SLOP { + 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(axis) } + dist = 0.0 + } + planes[0] = PlaneResult { plane: Plane { normal: normal, offset: total_radius - dist }, point: s.center, + triangle_index: 0, child_index: 0, material_index: 0 } + return 1 +} + +// --- the capsule ------------------------------------------------------------------------------ + +// 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: 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 < 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 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 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 = 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 ray_cast_sphere(Sphere { center: c1, radius: r }, origin, translation, max_fraction) } + if length < tc { return 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: 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 = 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 +} + +// --- the hull's mover ------------------------------------------------------------------------- + +// The mover's plane against a hull by GJK on the core segment: none when +// the segment is inside (a mesh could not do better, so a hull does not). +collide_mover_and_hull(planes: PlaneResult[], h: *HullData, mover: Capsule) -> int { + buffer = center_buffer() + buffer[0] = mover.center1 + buffer[1] = mover.center2 + input = DistanceInput { proxy_a: hull.hull_proxy(h), proxy_b: distance.shape_proxy(g_center, 2, mover.radius), + transform: math.transform_identity(), use_radii: false } + cache = distance.empty_cache() + output = distance.shape_distance(&input, &cache, null, 0) + if output.distance == 0.0 { return 0 } + if output.distance <= mover.radius { + planes[0] = PlaneResult { plane: Plane { normal: output.normal, offset: mover.radius - output.distance }, point: output.point_a, + triangle_index: 0, child_index: 0, material_index: 0 } + return 1 + } + return 0 +} + +// --- any shape ------------------------------------------------------------------------------------ + +compute_shape_aabb(shape: *Shape, t: Transform) -> AABB { + kind = shape.kind + if kind == SHAPE_CAPSULE { return compute_capsule_aabb(shape.capsule, t) } + if kind == SHAPE_HEIGHT_FIELD { return height_field.compute_height_field_aabb(shape.height_field, t) } + if kind == SHAPE_HULL { return hull.compute_hull_aabb(shape.hull, t) } + if kind == SHAPE_MESH { return mesh.compute_mesh_aabb(shape.mesh.data, t, shape.mesh.scale) } + if kind == SHAPE_SPHERE { return compute_sphere_aabb(shape.sphere, t) } + return AABB { lower: t.p, upper: t.p } +} + +// The bounds under a transform inflated by a margin: built in the shape's +// frame and inflated before the translation, so the margin survives far +// from the origin. +compute_fat_shape_aabb(shape: *Shape, t: Transform, extra: float) -> AABB { + r = math.vec3(extra, extra, extra) + rotation = Transform { p: math.vec3_zero(), q: t.q } + local_box = compute_shape_aabb(shape, rotation) + local_box.lower = math.sub(local_box.lower, r) + local_box.upper = math.add(local_box.upper, r) + return math.offset_aabb(local_box, t.p) +} + +// The bounds covering a convex shape from the start of a sweep to a time along it. +compute_swept_shape_aabb(shape: *Shape, sweep: Sweep, time: float) -> AABB { + xf1 = Transform { p: math.sub(sweep.c1, math.rotate_vector(sweep.q1, sweep.local_center)), q: sweep.q1 } + xf2 = distance.get_sweep_transform(sweep, time) + kind = shape.kind + if kind == SHAPE_CAPSULE { return compute_swept_capsule_aabb(shape.capsule, xf1, xf2) } + if kind == SHAPE_HULL { return hull.compute_swept_hull_aabb(shape.hull, xf1, xf2) } + if kind == SHAPE_SPHERE { return compute_swept_sphere_aabb(shape.sphere, xf1, xf2) } + return AABB { lower: xf1.p, upper: xf1.p } +} + +get_shape_centroid(shape: *Shape) -> Vec3 { + kind = shape.kind + if kind == SHAPE_CAPSULE { return math.lerp(shape.capsule.center1, shape.capsule.center2, 0.5) } + if kind == SHAPE_SPHERE { return shape.sphere.center } + if kind == SHAPE_HULL { return shape.hull.center } + if kind == SHAPE_MESH { return math.aabb_center(mesh.compute_mesh_aabb(shape.mesh.data, math.transform_identity(), shape.mesh.scale)) } + if kind == SHAPE_HEIGHT_FIELD { return math.aabb_center(height_field.compute_height_field_aabb(shape.height_field, math.transform_identity())) } + return math.vec3_zero() +} + +// The surface area, as the reference has it (a rough measure for explosions). +get_shape_area(shape: *Shape) -> float { + kind = shape.kind + if kind == SHAPE_CAPSULE { return 2.0 * math.length(math.sub(shape.capsule.center1, shape.capsule.center2)) + 2.0 * math.PI * shape.capsule.radius } + if kind == SHAPE_HULL { return shape.hull.surface_area } + if kind == SHAPE_SPHERE { return 2.0 * math.PI * shape.sphere.radius } + return 0.0 +} + +// The area of the shape's shadow on a plane. +get_shape_projected_area(shape: *Shape, plane_normal: Vec3) -> float { + kind = shape.kind + if kind == SHAPE_CAPSULE { + radius = shape.capsule.radius + axis = math.sub(shape.capsule.center2, shape.capsule.center1) + projected_length = math.length(math.cross(axis, plane_normal)) + return math.PI * radius * radius + 2.0 * radius * projected_length + } + if kind == SHAPE_HULL { return hull.compute_hull_projected_area(shape.hull, plane_normal) } + if kind == SHAPE_SPHERE { return math.PI * shape.sphere.radius * shape.sphere.radius } + return 0.0 +} + +compute_shape_mass(shape: *Shape) -> MassData { + kind = shape.kind + if kind == SHAPE_CAPSULE { return compute_capsule_mass(shape.capsule, shape.density) } + if kind == SHAPE_HULL { return hull.compute_hull_mass(shape.hull, shape.density) } + if kind == SHAPE_SPHERE { return compute_sphere_mass(shape.sphere, shape.density) } + return MassData { mass: 0.0, center: math.vec3_zero(), inertia: math.make_diagonal_matrix(0.0, 0.0, 0.0) } +} + +// The corner of a box farthest from a point. +farthest_point_on_aabb(b: AABB, p: Vec3) -> Vec3 { + x = b.upper.x + if p.x - b.lower.x > b.upper.x - p.x { x = b.lower.x } + y = b.upper.y + if p.y - b.lower.y > b.upper.y - p.y { y = b.lower.y } + z = b.upper.z + if p.z - b.lower.z > b.upper.z - p.z { z = b.lower.z } + return math.vec3(x, y, z) +} + +// The shape's reach from a local centre: the least radius (for the sleep +// and continuous thresholds) and the farthest extent along each axis. +compute_shape_extent(shape: *Shape, local_center: Vec3) -> ShapeExtent { + kind = shape.kind + if kind == SHAPE_CAPSULE { + radius = shape.capsule.radius + c1 = math.sub(shape.capsule.center1, local_center) + c2 = math.sub(shape.capsule.center2, local_center) + r = math.vec3(radius, radius, radius) + return ShapeExtent { min_extent: radius, max_extent: math.add(math.max_vec3(math.abs_vec3(c1), math.abs_vec3(c2)), r) } + } + if kind == SHAPE_SPHERE { + radius = shape.sphere.radius + h = math.abs_vec3(math.sub(shape.sphere.center, local_center)) + r = math.vec3(radius, radius, radius) + return ShapeExtent { min_extent: radius, max_extent: math.add(h, r) } + } + if kind == SHAPE_HULL { return hull.compute_hull_extent(shape.hull, local_center) } + if kind == SHAPE_MESH { + // Needed for a kinematic mesh to sleep. + return aabb_extent(mesh.compute_mesh_aabb(shape.mesh.data, math.transform_identity(), shape.mesh.scale), local_center) + } + if kind == SHAPE_HEIGHT_FIELD { + return aabb_extent(height_field.compute_height_field_aabb(shape.height_field, math.transform_identity()), local_center) + } + return ShapeExtent { min_extent: 0.0, max_extent: math.vec3_zero() } +} + +aabb_extent(box: AABB, local_center: Vec3) -> ShapeExtent { + r1 = math.length(math.sub(box.lower, local_center)) + r2 = math.length(math.sub(box.upper, local_center)) + p = farthest_point_on_aabb(box, local_center) + return ShapeExtent { min_extent: math.min_float(r1, r2), max_extent: math.abs_vec3(math.sub(p, local_center)) } +} + +// A ray in world space against the shape under its transform. +ray_cast_shape(shape: *Shape, t: Transform, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + local_origin = math.inv_transform_point(t, origin) + local_translation = math.inv_rotate_vector(t.q, translation) + output = distance.empty_cast_output() + kind = shape.kind + if kind == SHAPE_CAPSULE { output = ray_cast_capsule(shape.capsule, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_SPHERE { output = ray_cast_sphere(shape.sphere, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_HULL { output = hull.ray_cast_hull(shape.hull, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_MESH { output = mesh.ray_cast_mesh(shape.mesh, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_HEIGHT_FIELD { output = height_field.ray_cast_height_field(shape.height_field, local_origin, local_translation, max_fraction) } + else { return output } + output.point = math.transform_point(t, output.point) + output.normal = math.rotate_vector(t.q, output.normal) + return output +} + +var g_local: ptr = null // Vec3[MAX_SHAPE_CAST_POINTS], a proxy brought into the shape's frame + +local_buffer() -> ptr { + if g_local == null { g_local = core.alloc(distance.MAX_SHAPE_CAST_POINTS * sizeof(Vec3)) } + return g_local +} + +// A proxy in world space swept against the shape under its transform. +shape_cast_shape(shape: *Shape, t: Transform, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + buffer = local_buffer() + local_proxy = mesh.make_local_proxy(proxy, t, buffer) + local_translation = math.inv_rotate_vector(t.q, translation) + output = distance.empty_cast_output() + kind = shape.kind + if kind == SHAPE_CAPSULE { output = shape_cast_capsule(shape.capsule, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_HEIGHT_FIELD { output = height_field.shape_cast_height_field(shape.height_field, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_HULL { output = hull.shape_cast_hull(shape.hull, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_MESH { output = mesh.shape_cast_mesh(shape.mesh, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_SPHERE { output = shape_cast_sphere(shape.sphere, local_proxy, local_translation, max_fraction, can_encroach) } + else { return output } + output.point = math.transform_point(t, output.point) + output.normal = math.rotate_vector(t.q, output.normal) + return output +} + +overlap_shape(shape: *Shape, t: Transform, proxy: ShapeProxy) -> bool { + kind = shape.kind + if kind == SHAPE_CAPSULE { return overlap_capsule(shape.capsule, t, proxy) } + if kind == SHAPE_HEIGHT_FIELD { return height_field.overlap_height_field(shape.height_field, t, proxy) } + if kind == SHAPE_HULL { return hull.overlap_hull(shape.hull, t, proxy) } + if kind == SHAPE_MESH { return mesh.overlap_mesh(shape.mesh, t, proxy) } + if kind == SHAPE_SPHERE { return overlap_sphere(shape.sphere, t, proxy) } + return false +} + +// The planes of the shape against the character mover's capsule, in +// world space, with the material index clamped to the shape's. +collide_mover(planes: PlaneResult[], capacity: int, shape: *Shape, t: Transform, mover: Capsule) -> int { + if capacity == 0 { return 0 } + local_mover = Capsule { center1: math.inv_transform_point(t, mover.center1), center2: math.inv_transform_point(t, mover.center2), + radius: mover.radius } + plane_count = 0 + kind = shape.kind + if kind == SHAPE_CAPSULE { plane_count = collide_mover_and_capsule(planes, shape.capsule, local_mover) } + else if kind == SHAPE_SPHERE { plane_count = collide_mover_and_sphere(planes, shape.sphere, local_mover) } + else if kind == SHAPE_HULL { plane_count = collide_mover_and_hull(planes, shape.hull, local_mover) } + else if kind == SHAPE_MESH { plane_count = mesh.collide_mover_and_mesh(planes, capacity, shape.mesh, local_mover) } + else if kind == SHAPE_HEIGHT_FIELD { plane_count = height_field.collide_mover_and_height_field(planes, capacity, shape.height_field, local_mover) } + i = 0 + while i < plane_count { + planes[i].plane.normal = math.rotate_vector(t.q, planes[i].plane.normal) + planes[i].point = math.transform_point(t, planes[i].point) + planes[i].material_index = math.clamp_int(planes[i].material_index, 0, shape.material_count - 1) + i = i + 1 + } + return plane_count +} + +// A convex shape as a proxy for GJK; the sphere's and capsule's points +// live in the module's buffer until the next call. +make_shape_proxy(shape: *Shape) -> ShapeProxy { + kind = shape.kind + if kind == SHAPE_CAPSULE { return capsule_proxy(shape.capsule) } + if kind == SHAPE_SPHERE { return sphere_proxy(shape.sphere) } + if kind == SHAPE_HULL { return hull.hull_proxy(shape.hull) } + return distance.shape_proxy(null, 0, 0.0) +} diff --git a/aephysics/test_shape.ae b/aephysics/test_shape.ae new file mode 100644 index 0000000..3ef824f --- /dev/null +++ b/aephysics/test_shape.ae @@ -0,0 +1,833 @@ +// aephysics.shape against the reference's (Box3D's) test_shape.c: the +// masses of a sphere, an analytic box, translated, rotated and +// transformed boxes and a capsule bracketed by hulls, the bounds, the +// ray against a sphere (hits, misses, the clip, the interior, the +// graze), against a capsule (the side, oblique, the caps, misses, the +// interior, degenerate, the clip, near parallel), the overlap convention +// across the solids, the far origin's precision, and the cast through +// the dispatch; and beyond it the dispatch of every kind under a +// transform (bounds, fat and swept bounds, centroid, areas, mass, extent, +// ray, shape cast, overlap, the mover) and the filters. The shape name +// and flag tests need a world and come with the dynamics. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.shape + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float +extern sqrt(x: float) -> float + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("shape: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +// The reference's fixtures: a capsule along x, a sphere at x = 1, a unit cube. +test_capsule() -> Capsule { return manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 1.0) } +test_sphere() -> Sphere { return manifold.sphere(math.vec3(1.0, 0.0, 0.0), 1.0) } +// The capsule the ray tests share: along x from -2 to 2, radius 1. +ray_capsule() -> Capsule { return manifold.capsule(math.vec3(0.0 - 2.0, 0.0, 0.0), math.vec3(2.0, 0.0, 0.0), 1.0) } +unit_sphere() -> Sphere { return manifold.sphere(math.vec3_zero(), 1.0) } + +// A surface hit: the normal outward toward the ray, the point on the +// surface and on the ray at the fraction. +check_hit(name: string, out: CastOutput, origin: Vec3, translation: Vec3, point: Vec3, normal: Vec3, fraction: float, tol: float) { + ensure("${name} hit", out.hit) + small("${name} fraction", out.fraction - fraction, tol) + small("${name} point x", out.point.x - point.x, tol) + small("${name} point y", out.point.y - point.y, tol) + small("${name} point z", out.point.z - point.z, tol) + small("${name} normal x", out.normal.x - normal.x, tol) + small("${name} normal y", out.normal.y - normal.y, tol) + small("${name} normal z", out.normal.z - normal.z, tol) + on_ray = math.mul_add(origin, out.fraction, translation) + small("${name} on the ray", math.distance(out.point, on_ray), tol) +} + +// The initial-overlap convention: the origin at fraction zero with no normal. +check_initial_overlap(name: string, out: CastOutput, origin: Vec3) { + ensure("${name} hit", out.hit) + ensure("${name} fraction zero", out.fraction == 0.0) + small("${name} at the origin", math.distance(out.point, origin), math.EPSILON) + ensure("${name} no normal", out.normal.x == 0.0 && out.normal.y == 0.0 && out.normal.z == 0.0) +} + +check_inertia_equal(name: string, a: Matrix3, b: Matrix3, tol: float) { + d = math.sub_mm(a, b) + small("${name} xx", d.cx.x, tol) + small("${name} xy", d.cx.y, tol) + small("${name} xz", d.cx.z, tol) + small("${name} yx", d.cy.x, tol) + small("${name} yy", d.cy.y, tol) + small("${name} yz", d.cy.z, tol) + small("${name} zx", d.cz.x, tol) + small("${name} zy", d.cz.y, tol) + small("${name} zz", d.cz.z, tol) +} + +test_mass() { + // The sphere: its inertia is about its own centre, so the offset does not appear. + md = shape.compute_sphere_mass(test_sphere(), 1.0) + mass = 4.0 / 3.0 * math.PI + small("sphere mass", md.mass - mass, math.EPSILON) + ensure("sphere centre", md.center.x == 1.0 && md.center.y == 0.0) + inertia = 2.0 / 5.0 * mass + small("sphere inertia xx", md.inertia.cx.x - inertia, math.EPSILON) + small("sphere inertia yy", md.inertia.cy.y - inertia, math.EPSILON) + small("sphere inertia zz", md.inertia.cz.z - inertia, math.EPSILON) + + // The analytic box. + box = hull.make_box_hull(1.0, 1.0, 1.0) + md = hull.compute_hull_mass(box, 1.0) + mass = 8.0 + small("box mass", md.mass - mass, math.EPSILON) + small("box centre x", md.center.x, math.EPSILON) + small("box centre y", md.center.y, math.EPSILON) + small("box centre z", md.center.z, math.EPSILON) + inertia = (1.0 / 12.0) * mass * (4.0 + 4.0) + small("box inertia xx", md.inertia.cx.x - inertia, 2.0 * math.EPSILON) + small("box inertia yy", md.inertia.cy.y - inertia, 2.0 * math.EPSILON) + small("box inertia zz", md.inertia.cz.z - inertia, 2.0 * math.EPSILON) + + // A translated box keeps its central inertia and moves its centre. + offset = math.vec3(0.4, 0.0 - 0.7, 0.1) + t = Transform { p: offset, q: math.quat_identity() } + b1 = hull.make_box_hull(0.25, 0.5, 0.3) + b2 = hull.make_transformed_box_hull(0.25, 0.5, 0.3, t) + m1 = hull.compute_hull_mass(b1, 1.0) + m2 = hull.compute_hull_mass(b2, 1.0) + small("translated mass", m1.mass - m2.mass, math.EPSILON) + check_inertia_equal("translated inertia", b1.central_inertia, b2.central_inertia, math.EPSILON) + small("translated centre x", m2.center.x - offset.x, math.EPSILON) + small("translated centre y", m2.center.y - offset.y, math.EPSILON) + small("translated centre z", m2.center.z - offset.z, math.EPSILON) + hull.destroy_hull(b1) + hull.destroy_hull(b2) + + // A box rotated y onto z is the box with those extents swapped. + q = math.compute_quat_between_unit_vectors(math.vec3_axis_y(), math.vec3_axis_z()) + t = Transform { p: math.vec3_zero(), q: q } + b1 = hull.make_transformed_box_hull(0.25, 0.5, 0.3, t) + b2 = hull.make_box_hull(0.25, 0.3, 0.5) + m1 = hull.compute_hull_mass(b1, 1.0) + m2 = hull.compute_hull_mass(b2, 1.0) + small("rotated mass", m1.mass - m2.mass, math.EPSILON) + check_inertia_equal("rotated inertia", b1.central_inertia, b2.central_inertia, math.EPSILON) + small("rotated centre x", m1.center.x - m2.center.x, math.EPSILON) + small("rotated centre y", m1.center.y - m2.center.y, math.EPSILON) + small("rotated centre z", m1.center.z - m2.center.z, math.EPSILON) + hull.destroy_hull(b1) + hull.destroy_hull(b2) + + // Both at once. + t = Transform { p: offset, q: q } + b1 = hull.make_transformed_box_hull(0.25, 0.5, 0.3, t) + b2 = hull.make_box_hull(0.25, 0.3, 0.5) + m1 = hull.compute_hull_mass(b1, 1.0) + m2 = hull.compute_hull_mass(b2, 1.0) + small("transformed mass", m1.mass - m2.mass, math.EPSILON) + check_inertia_equal("transformed inertia", b1.central_inertia, b2.central_inertia, math.EPSILON) + small("transformed centre x", m1.center.x - offset.x, math.EPSILON) + small("transformed centre y", m1.center.y - offset.y, math.EPSILON) + small("transformed centre z", m1.center.z - offset.z, math.EPSILON) + hull.destroy_hull(b1) + hull.destroy_hull(b2) + + // The capsule between the box that contains it and the hull inside it. + c = test_capsule() + radius = c.radius + length = math.distance(c.center1, c.center2) + md = shape.compute_capsule_mass(c, 1.0) + r = hull.make_box_hull(radius + 0.5 * length, radius, radius) + md_upper = hull.compute_hull_mass(r, 1.0) + n = 4 + points_block = calloc(2 * n * n, sizeof(Vec3)) + points = points_block as Vec3[] + d = math.PI / ((n as float) - 1.0) + angle1 = 0.0 - 0.5 * math.PI + index = 0 + i = 0 + while i < n { + s1 = sin(angle1) + c1 = cos(angle1) + angle2 = 0.0 - 0.5 * math.PI + j = 0 + while j < n { + points[index] = math.vec3(1.0 + radius * c1, radius * s1 * cos(angle2), radius * s1 * sin(angle2)) + angle2 = angle2 + d + index = index + 1 + j = j + 1 + } + angle1 = angle1 + d + i = i + 1 + } + angle1 = 0.5 * math.PI + i = 0 + while i < n { + s1 = sin(angle1) + c1 = cos(angle1) + angle2 = 0.0 - 0.5 * math.PI + j = 0 + while j < n { + points[index] = math.vec3(0.0 - 1.0 + radius * c1, radius * s1 * cos(angle2), radius * s1 * sin(angle2)) + angle2 = angle2 + d + index = index + 1 + j = j + 1 + } + angle1 = angle1 + d + i = i + 1 + } + ensure("capsule sample count", index == 2 * n * n) + inner = hull.create_hull(points, 2 * n * n, 2 * n * n) + md_lower = hull.compute_hull_mass(inner, 1.0) + ensure("capsule mass bracketed", md_lower.mass < md.mass && md.mass < md_upper.mass) + ensure("capsule inertia xx bracketed", md_lower.inertia.cx.x < md.inertia.cx.x && md.inertia.cx.x < md_upper.inertia.cx.x) + ensure("capsule inertia yy bracketed", md_lower.inertia.cy.y < md.inertia.cy.y && md.inertia.cy.y < md_upper.inertia.cy.y) + ensure("capsule inertia zz bracketed", md_lower.inertia.cz.z < md.inertia.cz.z && md.inertia.cz.z < md_upper.inertia.cz.z) + hull.destroy_hull(inner) + hull.destroy_hull(r) + free(points_block) + + // A capsule along y has the analytic cylinder-plus-caps inertia about y. + cy = manifold.capsule(math.vec3(0.0, 0.0 - 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 0.5) + md = shape.compute_capsule_mass(cy, 2.0) + cylinder_mass = 2.0 * math.PI * 0.25 * 2.0 + sphere_mass = 2.0 * (4.0 / 3.0) * math.PI * 0.125 + small("capsule mass", md.mass - cylinder_mass - sphere_mass, 0.000001) + small("capsule inertia yy", md.inertia.cy.y - 0.5 * cylinder_mass * 0.25 - 0.4 * sphere_mass * 0.25, 0.000001) + small("capsule inertia xx = zz", md.inertia.cx.x - md.inertia.cz.z, 0.000001) + ensure("capsule inertia diagonal", math.abs_float(md.inertia.cx.y) < 0.000001 && math.abs_float(md.inertia.cy.z) < 0.000001) + // The same capsule along x: the roles of xx and yy swap. + cx = manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.5) + mx = shape.compute_capsule_mass(cx, 2.0) + small("capsule rotated xx", mx.inertia.cx.x - md.inertia.cy.y, 0.000001) + small("capsule rotated yy", mx.inertia.cy.y - md.inertia.cx.x, 0.000001) + + // The dispatch. + sh = shape.sphere_shape(test_sphere(), 1.0) + small("shape mass sphere exactly", shape.compute_shape_mass(&sh).mass - 4.0 / 3.0 * math.PI, math.EPSILON) + sh = shape.hull_shape(box, 2.0) + small("shape mass hull", shape.compute_shape_mass(&sh).mass - 16.0, math.EPSILON) + sh = shape.capsule_shape(cy, 2.0) + small("shape mass capsule", shape.compute_shape_mass(&sh).mass - cylinder_mass - sphere_mass, 0.000001) + hull.destroy_hull(box) +} + +test_aabb() { + b = shape.compute_sphere_aabb(test_sphere(), math.transform_identity()) + small("sphere lower x", b.lower.x, math.EPSILON) + small("sphere lower y", b.lower.y + 1.0, math.EPSILON) + small("sphere lower z", b.lower.z + 1.0, math.EPSILON) + small("sphere upper x", b.upper.x - 2.0, math.EPSILON) + small("sphere upper y", b.upper.y - 1.0, math.EPSILON) + small("sphere upper z", b.upper.z - 1.0, math.EPSILON) + b = shape.compute_capsule_aabb(test_capsule(), math.transform_identity()) + small("capsule lower x", b.lower.x + 2.0, math.EPSILON) + small("capsule lower y", b.lower.y + 1.0, math.EPSILON) + small("capsule lower z", b.lower.z + 1.0, math.EPSILON) + small("capsule upper x", b.upper.x - 2.0, math.EPSILON) + small("capsule upper y", b.upper.y - 1.0, math.EPSILON) + small("capsule upper z", b.upper.z - 1.0, math.EPSILON) + box = hull.make_box_hull(1.0, 1.0, 1.0) + b = hull.compute_hull_aabb(box, math.transform_identity()) + small("box lower x", b.lower.x + 1.0, math.EPSILON) + small("box lower y", b.lower.y + 1.0, math.EPSILON) + small("box lower z", b.lower.z + 1.0, math.EPSILON) + small("box upper x", b.upper.x - 1.0, math.EPSILON) + small("box upper y", b.upper.y - 1.0, math.EPSILON) + small("box upper z", b.upper.z - 1.0, math.EPSILON) + + // Under a transform, through the dispatch, and fat. + q = math.compute_quat_between_unit_vectors(math.vec3_axis_x(), math.vec3_axis_y()) + t = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + sh = shape.capsule_shape(test_capsule(), 1.0) + b = shape.compute_shape_aabb(&sh, t) + small("rotated capsule lower x", b.lower.x - 9.0, 0.000001) + small("rotated capsule upper x", b.upper.x - 11.0, 0.000001) + small("rotated capsule lower y", b.lower.y - 18.0, 0.000001) + small("rotated capsule upper y", b.upper.y - 22.0, 0.000001) + fat = shape.compute_fat_shape_aabb(&sh, t, 0.1) + small("fat capsule lower y", fat.lower.y - 17.9, 0.000001) + small("fat capsule upper z", fat.upper.z - 31.1, 0.000001) + sh = shape.hull_shape(box, 1.0) + b = shape.compute_shape_aabb(&sh, t) + small("moved box lower z", b.lower.z - 29.0, 0.000001) + sh = shape.sphere_shape(test_sphere(), 1.0) + b = shape.compute_shape_aabb(&sh, t) + small("rotated sphere upper y", b.upper.y - 22.0, 0.000001) + small("rotated sphere upper x", b.upper.x - 11.0, 0.000001) + + // The swept bounds cover both ends of the sweep. + sweep = Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: math.vec3(5.0, 0.0, 0.0), q1: math.quat_identity(), q2: math.quat_identity() } + b = shape.compute_swept_shape_aabb(&sh, sweep, 1.0) + small("swept sphere lower x", b.lower.x, 0.000001) + small("swept sphere upper x", b.upper.x - 7.0, 0.000001) + b = shape.compute_swept_shape_aabb(&sh, sweep, 0.5) + small("half swept sphere upper x", b.upper.x - 4.5, 0.000001) + sh = shape.capsule_shape(test_capsule(), 1.0) + b = shape.compute_swept_shape_aabb(&sh, sweep, 1.0) + small("swept capsule upper x", b.upper.x - 7.0, 0.000001) + sh = shape.hull_shape(box, 1.0) + b = shape.compute_swept_shape_aabb(&sh, sweep, 1.0) + small("swept hull upper x", b.upper.x - 6.0, 0.000001) + small("swept hull lower x", b.lower.x + 1.0, 0.000001) + + // A mesh and a height field through the dispatch. + grid = mesh.create_grid_mesh(4, 4, 1.0, 1, false) + sh = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + b = shape.compute_shape_aabb(&sh, math.transform_identity()) + small("mesh extent x", b.upper.x - b.lower.x - 4.0, 0.000001) + field = height_field.create_grid(4, 4, math.vec3(2.0, 1.0, 2.0), false) + sh = shape.height_field_shape(field, 1) + b = shape.compute_shape_aabb(&sh, t) + small("height field lower x", b.lower.x - 10.0, 0.000001) + small("height field upper y", b.upper.y - 26.0, 0.01) + mesh.destroy_mesh(grid) + height_field.destroy_height_field(field) + hull.destroy_hull(box) +} + +test_ray_cast_sphere() { + s = unit_sphere() + // Along each axis: the surface at 3 over a ray of 8. + origin = math.vec3(0.0 - 4.0, 0.0, 0.0) + translation = math.vec3(8.0, 0.0, 0.0) + check_hit("sphere -x", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(0.0 - 1.0, 0.0, 0.0), 3.0 / 8.0, 0.00001) + origin = math.vec3(0.0, 4.0, 0.0) + translation = math.vec3(0.0, 0.0 - 8.0, 0.0) + check_hit("sphere +y", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 3.0 / 8.0, 0.00001) + origin = math.vec3(0.0, 0.0, 0.0 - 4.0) + translation = math.vec3(0.0, 0.0, 8.0) + check_hit("sphere -z", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0, 0.0, 0.0 - 1.0), math.vec3(0.0, 0.0, 0.0 - 1.0), 3.0 / 8.0, 0.00001) + // An offset centre, hit partway. + s2 = manifold.sphere(math.vec3(5.0, 0.0, 0.0), 2.0) + origin = math.vec3_zero() + translation = math.vec3(10.0, 0.0, 0.0) + check_hit("sphere offset", shape.ray_cast_sphere(s2, origin, translation, 1.0), origin, translation, math.vec3(3.0, 0.0, 0.0), math.vec3(0.0 - 1.0, 0.0, 0.0), 0.3, 0.00001) + // A diagonal through the centre. + k = 0.70710678 + origin = math.vec3(0.0 - 3.0, 0.0 - 3.0, 0.0) + translation = math.vec3(6.0, 6.0, 0.0) + check_hit("sphere diagonal", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0 - k, 0.0 - k, 0.0), math.vec3(0.0 - k, 0.0 - k, 0.0), 0.382149, 0.0001) + + // Misses: pointing away, wide, stopping short. + ensure("sphere away", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(0.0 - 8.0, 0.0, 0.0), 1.0).hit == false) + ensure("sphere wide", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 3.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) + ensure("sphere short", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 0.3).hit == false) + // The clip straddling the surface at 3/8. + ensure("sphere clipped", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 0.374).hit == false) + out = shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 0.376) + ensure("sphere just reached", out.hit) + small("sphere just reached fraction", out.fraction - 3.0 / 8.0, 0.00001) + // The interior: the origin at fraction zero; a zero-length ray inside and out. + origin = math.vec3(0.3, 0.0, 0.0) + out = shape.ray_cast_sphere(s, origin, math.vec3(8.0, 0.0, 0.0), 1.0) + ensure("sphere interior hit", out.hit) + ensure("sphere interior fraction", out.fraction == 0.0) + small("sphere interior point", math.distance(out.point, origin), math.EPSILON) + origin = math.vec3(0.5, 0.0, 0.0) + out = shape.ray_cast_sphere(s, origin, math.vec3_zero(), 1.0) + ensure("sphere zero ray inside", out.hit) + small("sphere zero ray point", math.distance(out.point, origin), math.EPSILON) + ensure("sphere zero ray outside", shape.ray_cast_sphere(s, math.vec3(3.0, 0.0, 0.0), math.vec3_zero(), 1.0).hit == false) + // The graze. + ensure("sphere graze hits", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.999, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit) + ensure("sphere graze misses", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 1.001, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) + // The hollow sphere hits from inside. + out = shape.ray_cast_hollow_sphere(s, math.vec3(0.3, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0) + ensure("hollow sphere inside hit", out.hit) + small("hollow sphere inside point", out.point.x - 1.0, 0.00001) + small("hollow sphere inside normal", out.normal.x - 1.0, 0.00001) + // The hollow cast reports its fraction in length units against a unit ray. + out = shape.ray_cast_hollow_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 8.0) + ensure("hollow sphere outside hit", out.hit) + small("hollow sphere outside point", out.point.x + 1.0, 0.00001) + small("hollow sphere outside fraction", out.fraction - 3.0, 0.00001) +} + +test_ray_cast_capsule() { + c = ray_capsule() + // The side, perpendicular: the surface at 2 over a ray of 6. + origin = math.vec3(0.0, 3.0, 0.0) + translation = math.vec3(0.0, 0.0 - 6.0, 0.0) + check_hit("capsule side y", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 1.0 / 3.0, 0.00001) + origin = math.vec3(0.0, 0.0, 3.0) + translation = math.vec3(0.0, 0.0, 0.0 - 6.0) + check_hit("capsule side z", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0, 0.0, 1.0), math.vec3(0.0, 0.0, 1.0), 1.0 / 3.0, 0.00001) + origin = math.vec3(0.0 - 1.0, 3.0, 0.0) + translation = math.vec3(0.0, 0.0 - 6.0, 0.0) + check_hit("capsule side near c1", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 1.0 / 3.0, 0.00001) + // Oblique in the z = 0 plane, crossing y = 1 within the cylinder. + origin = math.vec3(0.0 - 3.0, 3.0, 0.0) + translation = math.vec3(4.0, 0.0 - 4.0, 0.0) + check_hit("capsule oblique", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 0.5, 0.0001) + // The caps: collinear onto c2, off-axis through each cap's centre. + k = 0.70710678 + origin = math.vec3(5.0, 0.0, 0.0) + translation = math.vec3(0.0 - 8.0, 0.0, 0.0) + check_hit("capsule cap collinear", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(3.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.25, 0.00001) + origin = math.vec3(4.0, 2.0, 0.0) + translation = math.vec3(0.0 - 4.0, 0.0 - 4.0, 0.0) + check_hit("capsule cap c2", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(2.0 + k, k, 0.0), math.vec3(k, k, 0.0), 0.323223, 0.0001) + origin = math.vec3(0.0 - 4.0, 2.0, 0.0) + translation = math.vec3(4.0, 0.0 - 4.0, 0.0) + check_hit("capsule cap c1", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 2.0 - k, k, 0.0), math.vec3(0.0 - k, k, 0.0), 0.323223, 0.0001) + // Misses. + ensure("capsule away", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(0.0, 4.0, 0.0), 1.0).hit == false) + ensure("capsule over", shape.ray_cast_capsule(c, math.vec3(0.0, 4.0, 2.0), math.vec3(0.0, 0.0 - 8.0, 0.0), 1.0).hit == false) + ensure("capsule short", shape.ray_cast_capsule(c, math.vec3(0.0, 5.0, 0.0), math.vec3(0.0, 0.0 - 1.0, 0.0), 1.0).hit == false) + ensure("capsule parallel outside", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) + ensure("capsule past the end", shape.ray_cast_capsule(c, math.vec3(4.0, 3.0, 0.0), math.vec3(0.0, 0.0 - 6.0, 0.0), 1.0).hit == false) + // The interior. + origin = math.vec3_zero() + out = shape.ray_cast_capsule(c, origin, math.vec3(0.0, 0.0 - 5.0, 0.0), 1.0) + ensure("capsule interior hit", out.hit) + ensure("capsule interior fraction", out.fraction == 0.0) + small("capsule interior point", math.distance(out.point, origin), math.EPSILON) + origin = math.vec3(2.5, 0.0, 0.0) + out = shape.ray_cast_capsule(c, origin, math.vec3(0.0, 0.0, 5.0), 1.0) + ensure("capsule cap interior hit", out.hit) + ensure("capsule cap interior fraction", out.fraction == 0.0) + small("capsule cap interior point", math.distance(out.point, origin), math.EPSILON) + out = shape.ray_cast_capsule(c, math.vec3_zero(), math.vec3_zero(), 1.0) + ensure("capsule zero ray inside", out.hit) + small("capsule zero ray point", math.length(out.point), math.EPSILON) + ensure("capsule zero ray outside", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3_zero(), 1.0).hit == false) + // Coincident centres are a sphere. + degenerate = manifold.capsule(math.vec3_zero(), math.vec3_zero(), 1.0) + origin = math.vec3(0.0 - 4.0, 0.0, 0.0) + translation = math.vec3(8.0, 0.0, 0.0) + check_hit("capsule degenerate", shape.ray_cast_capsule(degenerate, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(0.0 - 1.0, 0.0, 0.0), 3.0 / 8.0, 0.00001) + // The clip straddling the side hit at 1/3. + ensure("capsule clipped", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(0.0, 0.0 - 6.0, 0.0), 0.3).hit == false) + out = shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(0.0, 0.0 - 6.0, 0.0), 0.5) + ensure("capsule just reached", out.hit) + small("capsule just reached fraction", out.fraction - 1.0 / 3.0, 0.00001) + + // Near parallel: a long ray drifting in from just outside the cylinder. + axis_y = manifold.capsule(math.vec3_zero(), math.vec3(0.0, 10.0, 0.0), 1.0) + origin = math.vec3(1.0001, 100.0, 0.0) + translation = math.vec3(0.0 - 0.001, 0.0 - 200.0, 0.0) + out = shape.ray_cast_capsule(axis_y, origin, translation, 1.0) + ensure("near parallel y hit", out.hit) + on_segment = math.point_to_segment_distance(axis_y.center1, axis_y.center2, out.point) + small("near parallel y on the surface", math.distance(out.point, on_segment) - axis_y.radius, 0.001) + small("near parallel y on the ray", math.distance(out.point, math.mul_add(origin, out.fraction, translation)), 0.001) + origin = math.vec3(0.0 - 1000.0, 1.0001, 0.0) + translation = math.vec3(2000.0, 0.0 - 0.001, 0.0) + out = shape.ray_cast_capsule(c, origin, translation, 1.0) + ensure("near parallel x hit", out.hit) + on_segment = math.point_to_segment_distance(c.center1, c.center2, out.point) + small("near parallel x on the surface", math.distance(out.point, on_segment) - c.radius, 0.001) + ensure("exactly parallel outside misses", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) +} + +test_overlap_convention() { + ray = math.vec3(8.0, 0.0, 0.0) + zero = math.vec3_zero() + s = unit_sphere() + inside = math.vec3(0.2, 0.0, 0.0) + check_initial_overlap("sphere moving", shape.ray_cast_sphere(s, inside, ray, 1.0), inside) + check_initial_overlap("sphere point", shape.ray_cast_sphere(s, inside, zero, 1.0), inside) + ensure("sphere point outside", shape.ray_cast_sphere(s, math.vec3(3.0, 0.0, 0.0), zero, 1.0).hit == false) + c = ray_capsule() + inside = math.vec3_zero() + check_initial_overlap("capsule moving", shape.ray_cast_capsule(c, inside, ray, 1.0), inside) + check_initial_overlap("capsule point", shape.ray_cast_capsule(c, inside, zero, 1.0), inside) + ensure("capsule point outside", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), zero, 1.0).hit == false) + box = hull.make_box_hull(1.0, 1.0, 1.0) + inside = math.vec3(0.3, 0.2, 0.1) + check_initial_overlap("hull moving", hull.ray_cast_hull(box, inside, ray, 1.0), inside) + check_initial_overlap("hull point", hull.ray_cast_hull(box, inside, zero, 1.0), inside) + ensure("hull point outside", hull.ray_cast_hull(box, math.vec3(3.0, 0.0, 0.0), zero, 1.0).hit == false) + hull.destroy_hull(box) +} + +// The distance from a hit point to the analytic first intersection of +// the same ray with the sphere: what the method loses, since our floats +// are doubles and the reference's were floats. +sphere_hit_error(s: Sphere, origin: Vec3, translation: Vec3, point: Vec3) -> float { + r = s.radius + sv = math.sub(origin, s.center) + d = math.normalize(translation) + b = math.dot(sv, d) + cc = math.dot(sv, sv) - r * r + t = 0.0 - b - sqrt(b * b - cc) + return math.distance(point, math.mul_add(origin, t, d)) +} + +capsule_hit_error(c: Capsule, origin: Vec3, translation: Vec3, point: Vec3) -> float { + a = math.normalize(math.sub(c.center2, c.center1)) + sv = math.sub(origin, c.center1) + d = math.normalize(translation) + sp = math.mul_sub(sv, math.dot(sv, a), a) + dp = math.mul_sub(d, math.dot(d, a), a) + qa = math.dot(dp, dp) + qb = 2.0 * math.dot(sp, dp) + qc = math.dot(sp, sp) - c.radius * c.radius + tau = (0.0 - qb - sqrt(qb * qb - 4.0 * qa * qc)) / (2.0 * qa) + return math.distance(point, math.mul_add(origin, tau, d)) +} + +test_far_origin() { + s = unit_sphere() + c = ray_capsule() + // (0, 0, 1) is on the sphere and on the capsule's side; the ray dives + // in from far away along a fan of directions skewed off the normal. + h = math.vec3(0.0, 0.0, 1.0) + offsets_block = calloc(3, 8) + offsets = offsets_block as float[] + offsets[0] = 0.0 - 0.7 + offsets[1] = 0.0 + offsets[2] = 0.7 + distances_block = calloc(7, 8) + distances = distances_block as float[] + distances[0] = 10.0 + distances[1] = 100.0 + distances[2] = 1000.0 + distances[3] = 10000.0 + distances[4] = 100000.0 + distances[5] = 1000000.0 + distances[6] = 10000000.0 + ray_miss = 1000000000000.0 + println(" worst hit point error over a fan of skew rays, by origin distance (sphere, capsule):") + i = 0 + while i < 7 { + d = distances[i] + max_s = 0.0 + max_c = 0.0 + ia = 0 + while ia < 3 { + ib = 0 + while ib < 3 { + u = math.normalize(math.vec3(offsets[ia], offsets[ib], 1.0)) + origin = math.mul_add(h, d, u) + translation = math.mul_sv(0.0 - 2.0 * d, u) + os = shape.ray_cast_sphere(s, origin, translation, 1.0) + oc = shape.ray_cast_capsule(c, origin, translation, 1.0) + err_s = ray_miss + if os.hit { err_s = sphere_hit_error(s, origin, translation, os.point) } + err_c = ray_miss + if oc.hit { err_c = capsule_hit_error(c, origin, translation, oc.point) } + max_s = math.max_float(max_s, err_s) + max_c = math.max_float(max_c, err_c) + ib = ib + 1 + } + ia = ia + 1 + } + println(" ${d}: ${max_s} ${max_c}") + // The closest-point form keeps the error at the precision floor, + // growing only linearly with the distance: the reference's float + // bound is 16 * distance * FLT_EPSILON + 2e-6; ours in doubles must + // sit far below it, and at ten million units (where a float ray + // loses the hit) still within the same bound. + floor = 16.0 * d * math.EPSILON + 0.000002 + ensure("sphere error at ${d} under the float floor", max_s < floor) + ensure("capsule error at ${d} under the float floor", max_c < floor) + if d < 10000000.0 { + ensure("sphere error at ${d} at double precision", max_s < 0.001 * floor + 0.0000001) + ensure("capsule error at ${d} at double precision", max_c < 0.001 * floor + 0.0000001) + } + i = i + 1 + } + free(distances_block) + free(offsets_block) +} + +test_dispatch() { + origin = math.vec3(0.0 - 4.0, 0.0, 0.0) + translation = math.vec3(8.0, 0.0, 0.0) + // The reference's RayCastShape: the sphere at x 1, the capsule -1..1, the cube. + out = shape.ray_cast_sphere(test_sphere(), origin, translation, 1.0) + ensure("cast sphere hit", out.hit) + small("cast sphere normal x", out.normal.x + 1.0, math.EPSILON) + small("cast sphere normal y", out.normal.y, math.EPSILON) + small("cast sphere normal z", out.normal.z, math.EPSILON) + small("cast sphere fraction", out.fraction - 0.5, math.EPSILON) + out = shape.ray_cast_capsule(test_capsule(), origin, translation, 1.0) + ensure("cast capsule hit", out.hit) + small("cast capsule normal x", out.normal.x + 1.0, math.EPSILON) + small("cast capsule normal y", out.normal.y, math.EPSILON) + small("cast capsule normal z", out.normal.z, math.EPSILON) + small("cast capsule fraction", out.fraction - 0.25, math.EPSILON) + box = hull.make_box_hull(1.0, 1.0, 1.0) + out = hull.ray_cast_hull(box, origin, translation, 1.0) + ensure("cast hull hit", out.hit) + small("cast hull normal x", out.normal.x + 1.0, math.EPSILON) + small("cast hull normal y", out.normal.y, math.EPSILON) + small("cast hull normal z", out.normal.z, math.EPSILON) + small("cast hull fraction", out.fraction - 3.0 / 8.0, math.EPSILON) + + // Every kind under one transform (turned a quarter about z and moved), + // with a world ray along -y at the moved origin: the shape's local x + // now points along world y, so each hits its local +x face from above. + q = math.compute_quat_between_unit_vectors(math.vec3_axis_x(), math.vec3_axis_y()) + t = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + world_origin = math.vec3(10.0, 24.0, 30.0) + world_translation = math.vec3(0.0, 0.0 - 8.0, 0.0) + grid = mesh.create_grid_mesh(4, 4, 1.0, 1, false) + field = height_field.create_grid(4, 4, math.vec3(1.0, 1.0, 1.0), false) + shapes_block = calloc(5, sizeof(Shape)) + shapes = shapes_block as Shape[] + shapes[0] = shape.sphere_shape(unit_sphere(), 1.0) + shapes[1] = shape.capsule_shape(ray_capsule(), 1.0) + shapes[2] = shape.hull_shape(box, 1.0) + // The grid mesh lies in the local xz plane from 0 to 4: its normal (local y) faces world -x. + shapes[3] = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + shapes[4] = shape.height_field_shape(field, 1) + // The sphere and the cube reach local x = 1 (world y = 21); the capsule x = 3 (world y = 23). + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + out = shape.ray_cast_shape(&sh, t, world_origin, world_translation, 1.0) + ensure("dispatch ray ${k} hit", out.hit) + small("dispatch ray ${k} fraction", out.fraction - (24.0 - surface) / 8.0, 0.00001) + small("dispatch ray ${k} normal", out.normal.y - 1.0, 0.00001) + small("dispatch ray ${k} point", math.distance(out.point, math.vec3(10.0, surface, 30.0)), 0.00001) + k = k + 1 + } + // The mesh (centred, -2..2) and the field (0..3) lie in their local xz + // plane, their +y face toward world -x: a ray along world +x from the + // world point of local (1, 3, 1), which is t.p + q (1, 3, 1) = (7, 21, 31). + k = 3 + while k < 5 { + sh = shapes[k] + out = shape.ray_cast_shape(&sh, t, math.vec3(7.0, 21.0, 31.0), math.vec3(8.0, 0.0, 0.0), 1.0) + ensure("dispatch ray ${k} hit", out.hit) + small("dispatch ray ${k} fraction", out.fraction - 3.0 / 8.0, 0.01) + small("dispatch ray ${k} normal", out.normal.x + 1.0, 0.00001) + k = k + 1 + } + // A miss leaves the output empty. + sh = shapes[0] + out = shape.ray_cast_shape(&sh, t, math.vec3(50.0, 50.0, 50.0), math.vec3(1.0, 0.0, 0.0), 1.0) + ensure("dispatch miss", out.hit == false) + + // The shape cast of a sphere of radius 0.5 down onto each convex kind. + point_block = calloc(1, sizeof(Vec3)) + point = point_block as Vec3[] + point[0] = world_origin + proxy = distance.shape_proxy(point_block, 1, 0.5) + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + point[0] = world_origin + out = shape.shape_cast_shape(&sh, t, proxy, world_translation, 1.0, false) + ensure("dispatch shape cast ${k} hit", out.hit) + small("dispatch shape cast ${k} fraction", out.fraction - (24.0 - surface - 0.5) / 8.0, 0.001) + small("dispatch shape cast ${k} normal", out.normal.y - 1.0, 0.001) + small("dispatch shape cast ${k} point", math.distance(out.point, math.vec3(10.0, surface, 30.0)), 0.001) + k = k + 1 + } + k = 3 + while k < 5 { + sh = shapes[k] + point[0] = math.vec3(7.0, 21.0, 31.0) + out = shape.shape_cast_shape(&sh, t, proxy, math.vec3(8.0, 0.0, 0.0), 1.0, false) + ensure("dispatch shape cast ${k} hit", out.hit) + small("dispatch shape cast ${k} fraction", out.fraction - 2.5 / 8.0, 0.01) + small("dispatch shape cast ${k} normal", out.normal.x + 1.0, 0.001) + k = k + 1 + } + + // The overlap: the same sphere sunk 0.1 into each kind's surface, and 0.1 clear of it. + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + point[0] = math.vec3(10.0, surface + 0.4, 30.0) + ensure("dispatch overlap ${k}", shape.overlap_shape(&sh, t, proxy)) + point[0] = math.vec3(10.0, surface + 0.6, 30.0) + ensure("dispatch clear ${k}", shape.overlap_shape(&sh, t, proxy) == false) + k = k + 1 + } + k = 3 + while k < 5 { + sh = shapes[k] + point[0] = math.vec3(9.6, 21.0, 31.0) + ensure("dispatch overlap ${k}", shape.overlap_shape(&sh, t, proxy)) + point[0] = math.vec3(9.4, 21.0, 31.0) + ensure("dispatch clear ${k}", shape.overlap_shape(&sh, t, proxy) == false) + k = k + 1 + } + + // The mover: a capsule standing on each kind, within its radius, gets + // a plane whose normal points out of the surface in world space. + planes_block = calloc(8, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + mover = manifold.capsule(math.vec3(10.0, surface + 0.2, 30.0), math.vec3(10.0, surface + 1.2, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("dispatch mover ${k} plane", count == 1) + if count == 1 { + small("dispatch mover ${k} normal", planes[0].plane.normal.y - 1.0, 0.0001) + small("dispatch mover ${k} offset", planes[0].plane.offset - 0.1, 0.0001) + // The sphere and the capsule report their centre or axis point; the hull the surface point. + if k == 2 { small("dispatch mover ${k} point", planes[0].point.y - 21.0, 0.0001) } + if k == 0 { small("dispatch mover ${k} point", planes[0].point.y - 20.0, 0.0001) } + ensure("dispatch mover ${k} material", planes[0].material_index == 0) + } + k = k + 1 + } + // Standing on the mesh and the field: local (1, 0.2, 1) up to (1, 1.2, 1) is world (9.8, 21, 31) to (8.8, 21, 31). + mover = manifold.capsule(math.vec3(9.8, 21.0, 31.0), math.vec3(8.8, 21.0, 31.0), 0.3) + k = 3 + while k < 5 { + sh = shapes[k] + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("dispatch mover ${k} planes (${count})", count >= 1) + if count >= 1 { + small("dispatch mover ${k} normal", planes[0].plane.normal.x + 1.0, 0.0001) + small("dispatch mover ${k} offset", planes[0].plane.offset - 0.1, 0.01) + } + k = k + 1 + } + // Hovering, none; capacity zero, none. + mover = manifold.capsule(math.vec3(10.0, 25.0, 30.0), math.vec3(10.0, 26.0, 30.0), 0.3) + sh = shapes[0] + ensure("mover hovering", shape.collide_mover(planes, 8, &sh, t, mover) == 0) + mover = manifold.capsule(math.vec3(10.0, 21.2, 30.0), math.vec3(10.0, 22.2, 30.0), 0.3) + ensure("mover no capacity", shape.collide_mover(planes, 0, &sh, t, mover) == 0) + // Deep overlap: the mover's axis through the sphere's centre pushes + // perpendicular to the axis; through a hull, nothing. + mover = manifold.capsule(math.vec3(10.0, 19.5, 30.0), math.vec3(10.0, 20.5, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("mover through the sphere", count == 1) + if count == 1 { + small("mover through the sphere: perpendicular", planes[0].plane.normal.y, 0.0001) + small("mover through the sphere: full push", planes[0].plane.offset - 1.3, 0.0001) + } + sh = shapes[2] + ensure("mover through the hull", shape.collide_mover(planes, 8, &sh, t, mover) == 0) + sh = shapes[1] + mover = manifold.capsule(math.vec3(10.0, 19.5, 30.0), math.vec3(10.0, 20.5, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("mover through the capsule", count == 1) + if count == 1 { small("mover through the capsule: full push", planes[0].plane.offset - 1.3, 0.0001) } + + // The centroid, areas, extent and proxy. + sh = shapes[1] + small("capsule centroid", math.length(shape.get_shape_centroid(&sh)), 0.000001) + small("capsule area", shape.get_shape_area(&sh) - 8.0 - 2.0 * math.PI, 0.000001) + small("capsule projected area on y", shape.get_shape_projected_area(&sh, math.vec3_axis_y()) - math.PI - 8.0, 0.000001) + small("capsule projected area on x", shape.get_shape_projected_area(&sh, math.vec3_axis_x()) - math.PI, 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("capsule min extent", extent.min_extent - 1.0, 0.000001) + small("capsule max extent x", extent.max_extent.x - 3.0, 0.000001) + small("capsule max extent y", extent.max_extent.y - 1.0, 0.000001) + p = shape.make_shape_proxy(&sh) + ensure("capsule proxy", p.count == 2 && p.radius == 1.0) + sh = shapes[0] + small("sphere area", shape.get_shape_area(&sh) - 2.0 * math.PI, 0.000001) + small("sphere projected area", shape.get_shape_projected_area(&sh, math.vec3_axis_y()) - math.PI, 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3(1.0, 0.0, 0.0)) + small("sphere max extent x", extent.max_extent.x - 2.0, 0.000001) + p = shape.make_shape_proxy(&sh) + ensure("sphere proxy", p.count == 1 && p.radius == 1.0) + sh = shapes[2] + small("hull area", shape.get_shape_area(&sh) - 24.0, 0.000001) + small("hull projected area", shape.get_shape_projected_area(&sh, math.vec3_axis_y()) - 4.0, 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("hull min extent", extent.min_extent - 1.0, 0.000001) + small("hull max extent", extent.max_extent.z - 1.0, 0.000001) + p = shape.make_shape_proxy(&sh) + ensure("hull proxy", p.count == 8 && p.radius == 0.0) + sh = shapes[3] + small("mesh centroid", math.length(shape.get_shape_centroid(&sh)), 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("mesh max extent", extent.max_extent.x - 2.0, 0.000001) + small("mesh min extent", extent.min_extent - sqrt(8.0), 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3(1.0, 0.0, 0.0)) + small("mesh max extent off centre", extent.max_extent.x - 3.0, 0.000001) + ensure("mesh has no mass", shape.compute_shape_mass(&sh).mass == 0.0) + ensure("mesh has no area", shape.get_shape_area(&sh) == 0.0) + sh = shapes[4] + small("field centroid", math.distance(shape.get_shape_centroid(&sh), math.vec3(1.5, 0.0, 1.5)), 0.01) + ensure("field proxy is empty", shape.make_shape_proxy(&sh).count == 0) + + free(planes_block) + free(point_block) + free(shapes_block) + mesh.destroy_mesh(grid) + height_field.destroy_height_field(field) + hull.destroy_hull(box) +} + +test_filters() { + a = shape.default_filter() + b = shape.default_filter() + ensure("defaults collide", shape.should_shapes_collide(a, b)) + b.category_bits = 2 as long + a.mask_bits = 1 as long + ensure("mask excludes", shape.should_shapes_collide(a, b) == false) + a.mask_bits = 3 as long + ensure("mask includes", shape.should_shapes_collide(a, b)) + a.group_index = 3 + b.group_index = 3 + a.mask_bits = 0 as long + ensure("a shared positive group always collides", shape.should_shapes_collide(a, b)) + a.group_index = 0 - 3 + b.group_index = 0 - 3 + a.mask_bits = 3 as long + ensure("a shared negative group never collides", shape.should_shapes_collide(a, b) == false) + a.group_index = 0 - 3 + b.group_index = 0 - 4 + ensure("different groups fall back to the masks", shape.should_shapes_collide(a, b)) + q = shape.default_query_filter() + ensure("default query collides", shape.should_query_collide(shape.default_filter(), q)) + q.mask_bits = 2 as long + ensure("query mask excludes", shape.should_query_collide(shape.default_filter(), q) == false) + ensure("convex kinds", shape.is_convex(shape.SHAPE_SPHERE) && shape.is_convex(shape.SHAPE_CAPSULE) && shape.is_convex(shape.SHAPE_HULL)) + ensure("concave kinds", shape.is_convex(shape.SHAPE_MESH) == false && shape.is_convex(shape.SHAPE_HEIGHT_FIELD) == false && shape.is_convex(shape.SHAPE_COMPOUND) == false) +} + +main() { + before = core.alloc_count() + test_mass() + test_aabb() + test_ray_cast_sphere() + test_ray_cast_capsule() + test_overlap_convention() + test_far_origin() + test_dispatch() + test_filters() + // The scratch stays allocated: three blocks here, three in mesh, three in the height field. + ensure("every other counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before + 9) + + println("shape: ${checks} checks") + if failures == 0 { + println("shape: all checks passed") + } else { + println("shape: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 11d213c..5c6ccdd 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -245,3 +245,32 @@ straddled cell on top of distance's 1.3-1.5x; the overlap 1.85x. The field is 4.2 MB here against 1.3 MB there: the reference packs 16-bit heights and 8-bit materials and flags, which Aether cannot yet address, so they are ints. + +## shape + +`bench/shape.ae` and `bench/shape_box3d.c`: 1,000,000 rays at a unit +sphere and 1,000,000 at a capsule from a fan of origins, a third of them +missing; 100,000 casts of a sphere at a capsule under a transform; +100,000 overlaps of a sphere with it; 100,000 mover planes against it; +100,000 capsule masses. + +| phase | aephysics | Box3D | +|---|---|---| +| 1,000,000 sphere ray casts | 33.2 ms | **31.5** | +| 1,000,000 capsule ray casts | **41.0** | 49.7 | +| 100,000 shape casts | 44.6 | **18.0** | +| 100,000 overlaps | 12.2 | **5.9** | +| 100,000 mover planes | 6.9 | **4.8** | +| 100,000 capsule masses | 4.5 | **4.3** | + +The same answers: the ray hits agree to one boundary case in a million +(349,452 sphere hits here against 349,451, the same 595,654 on the +capsule) with equal fraction sums, the casts hit the same 89,582 times +with equal sums, the overlaps (51,518), the mover's planes (45,071) and +the masses agree. The sphere ray and the masses are at parity, the +capsule ray is 0.8x (the reference's float division by the axis length +against our double), the mover 1.45x. The sphere-against-capsule cast +is 2.5x and the overlap 2x: both are one GJK call on proxies with +radii, where the box-against-box work of the distance layer sat at +1.3-1.5x; the radius handling in `distance.shape_cast` and +`shape_distance` is the place to profile (aephysics#9). diff --git a/bench/shape.ae b/bench/shape.ae new file mode 100644 index 0000000..4b1f9a5 --- /dev/null +++ b/bench/shape.ae @@ -0,0 +1,134 @@ +// The shapes on the same scenes as bench/shape_box3d.c: 1,000,000 rays +// at a unit sphere and 1,000,000 at a capsule from a fan of origins (a +// third missing), 100,000 sphere shape casts at a capsule under a +// transform through the dispatch, 100,000 overlaps, 100,000 mover planes +// against a capsule, and 100,000 capsule masses. Single thread, wall +// time per phase, with the hit counts and sums as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.shape + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float + +clock() -> long { return os.now_monotonic_ns() } +ms(ns: long) -> float { return (ns as float) / 1000000.0 } + +const RAYS = 1000000 +const CASTS = 100000 +const OVERLAPS = 100000 +const MOVERS = 100000 +const MASSES = 100000 + +main() { + sphere = manifold.sphere(math.vec3_zero(), 1.0) + capsule = manifold.capsule(math.vec3(0.0 - 2.0, 0.0, 0.0), math.vec3(2.0, 0.0, 0.0), 1.0) + + t0 = clock() + sphere_hits = 0 + sphere_sum = 0.0 + i = 0 + while i < RAYS { + t = (i as float) / (RAYS as float) + a = 6.2831853 * t + origin = math.vec3(5.0 * cos(7.0 * a), 5.0 * sin(7.0 * a), 5.0 * sin(3.0 * a)) + target = math.vec3(1.5 * sin(11.0 * a), 1.5 * cos(13.0 * a), 0.0) + out = shape.ray_cast_sphere(sphere, origin, math.sub(target, origin), 1.0) + if out.hit { + sphere_hits = sphere_hits + 1 + sphere_sum = sphere_sum + out.fraction + } + i = i + 1 + } + t1 = clock() + + capsule_hits = 0 + capsule_sum = 0.0 + i = 0 + while i < RAYS { + t = (i as float) / (RAYS as float) + a = 6.2831853 * t + origin = math.vec3(6.0 * cos(7.0 * a), 5.0 * sin(7.0 * a), 5.0 * sin(3.0 * a)) + target = math.vec3(3.0 * sin(11.0 * a), 1.5 * cos(13.0 * a), 0.0) + out = shape.ray_cast_capsule(capsule, origin, math.sub(target, origin), 1.0) + if out.hit { + capsule_hits = capsule_hits + 1 + capsule_sum = capsule_sum + out.fraction + } + i = i + 1 + } + t2 = clock() + + sh = shape.capsule_shape(capsule, 1.0) + q = math.make_quat_from_axis_angle(math.normalize(math.vec3(1.0, 2.0, 3.0)), 0.7) + transform = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + start_block = calloc(1, sizeof(Vec3)) + start = start_block as Vec3[] + cast_hits = 0 + cast_sum = 0.0 + i = 0 + while i < CASTS { + t = (i as float) / (CASTS as float) + a = 6.2831853 * t + local = math.vec3(2.5 * cos(5.0 * a), 4.0, 1.2 * sin(5.0 * a)) + start[0] = math.transform_point(transform, local) + translation = math.rotate_vector(q, math.vec3(0.0, 0.0 - 8.0, 0.5 * sin(9.0 * a))) + out = shape.shape_cast_shape(&sh, transform, distance.shape_proxy(start_block, 1, 0.3), translation, 1.0, false) + if out.hit { + cast_hits = cast_hits + 1 + cast_sum = cast_sum + out.fraction + } + i = i + 1 + } + t3 = clock() + + overlap_hits = 0 + i = 0 + while i < OVERLAPS { + t = (i as float) / (OVERLAPS as float) + a = 6.2831853 * t + local = math.vec3(3.0 * cos(5.0 * a), 1.6 * sin(3.0 * a), 1.2 * sin(5.0 * a)) + start[0] = math.transform_point(transform, local) + if shape.overlap_shape(&sh, transform, distance.shape_proxy(start_block, 1, 0.5)) { overlap_hits = overlap_hits + 1 } + i = i + 1 + } + t4 = clock() + + planes_block = calloc(4, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + mover_planes = 0 + mover_sum = 0.0 + i = 0 + while i < MOVERS { + t = (i as float) / (MOVERS as float) + a = 6.2831853 * t + local = math.vec3(3.0 * cos(5.0 * a), 1.0 + 0.6 * sin(3.0 * a), 1.0 * sin(5.0 * a)) + mover = manifold.capsule(math.transform_point(transform, local), math.transform_point(transform, math.add(local, math.vec3(0.0, 1.0, 0.0))), 0.3) + count = shape.collide_mover(planes, 4, &sh, transform, mover) + mover_planes = mover_planes + count + if count > 0 { mover_sum = mover_sum + planes[0].plane.offset } + i = i + 1 + } + t5 = clock() + + mass_sum = 0.0 + i = 0 + while i < MASSES { + t = (i as float) / (MASSES as float) + c = manifold.capsule(math.vec3(0.0 - 1.0 - t, 0.0, 0.0), math.vec3(1.0, t, 0.0), 0.5 + 0.5 * t) + md = shape.compute_capsule_mass(c, 1.0) + mass_sum = mass_sum + md.mass + md.inertia.cx.x + md.inertia.cy.y + md.inertia.cz.z + i = i + 1 + } + t6 = clock() + + println("aephysics shape: ${RAYS} sphere rays ${ms(t1 - t0)} ms (${sphere_hits} hits, sum ${sphere_sum}), ${RAYS} capsule rays ${ms(t2 - t1)} ms (${capsule_hits} hits, sum ${capsule_sum}), ${CASTS} shape casts ${ms(t3 - t2)} ms (${cast_hits} hits, sum ${cast_sum}), ${OVERLAPS} overlaps ${ms(t4 - t3)} ms (${overlap_hits} hits), ${MOVERS} movers ${ms(t5 - t4)} ms (${mover_planes} planes, sum ${mover_sum}), ${MASSES} masses ${ms(t6 - t5)} ms (sum ${mass_sum})") + free(planes_block) + free(start_block) +} diff --git a/bench/shape_box3d.c b/bench/shape_box3d.c new file mode 100644 index 0000000..8caa48d --- /dev/null +++ b/bench/shape_box3d.c @@ -0,0 +1,151 @@ +// The shapes of the reference on the same scenes as bench/shape.ae: +// 1,000,000 rays at a unit sphere and 1,000,000 at a capsule from a fan +// of origins (a third missing), 100,000 sphere shape casts at a capsule +// under a transform through the dispatch, 100,000 overlaps, 100,000 +// mover planes against a capsule, and 100,000 capsule masses. Single +// thread, wall time per phase, with the hit counts and sums as the +// checksum. +#include "box3d/collision.h" +#include "box3d/math_functions.h" +#include "box3d/types.h" + +// Internal to the reference (shape.h in src) but linked from its library. +int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover ); + +#include +#include +#include + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +#define RAYS 1000000 +#define CASTS 100000 +#define OVERLAPS 100000 +#define MOVERS 100000 +#define MASSES 100000 + +int main( void ) +{ + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f }; + b3Capsule capsule = { { -2.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, 1.0f }; + + double t0 = now_ms(); + int sphereHits = 0; + double sphereSum = 0.0; + for ( int i = 0; i < RAYS; ++i ) + { + float t = (float)i / (float)RAYS; + float a = 6.2831853f * t; + b3Vec3 origin = { 5.0f * cosf( 7.0f * a ), 5.0f * sinf( 7.0f * a ), 5.0f * sinf( 3.0f * a ) }; + b3Vec3 target = { 1.5f * sinf( 11.0f * a ), 1.5f * cosf( 13.0f * a ), 0.0f }; + b3RayCastInput input = { origin, b3Sub( target, origin ), 1.0f }; + b3CastOutput out = b3RayCastSphere( &sphere, &input ); + if ( out.hit ) + { + sphereHits += 1; + sphereSum += out.fraction; + } + } + double t1 = now_ms(); + + int capsuleHits = 0; + double capsuleSum = 0.0; + for ( int i = 0; i < RAYS; ++i ) + { + float t = (float)i / (float)RAYS; + float a = 6.2831853f * t; + b3Vec3 origin = { 6.0f * cosf( 7.0f * a ), 5.0f * sinf( 7.0f * a ), 5.0f * sinf( 3.0f * a ) }; + b3Vec3 target = { 3.0f * sinf( 11.0f * a ), 1.5f * cosf( 13.0f * a ), 0.0f }; + b3RayCastInput input = { origin, b3Sub( target, origin ), 1.0f }; + b3CastOutput out = b3RayCastCapsule( &capsule, &input ); + if ( out.hit ) + { + capsuleHits += 1; + capsuleSum += out.fraction; + } + } + double t2 = now_ms(); + + // b3ShapeCastShape, b3OverlapShape and b3CollideMover are internal to the + // reference (shape.h in src); their dispatch is done here as they do it: + // the input into the shape's frame, the output back out. + b3Quat q = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 1.0f, 2.0f, 3.0f } ), 0.7f ); + b3Transform transform = { { 10.0f, 20.0f, 30.0f }, q }; + + int castHits = 0; + double castSum = 0.0; + for ( int i = 0; i < CASTS; ++i ) + { + float t = (float)i / (float)CASTS; + float a = 6.2831853f * t; + b3Vec3 local = { 2.5f * cosf( 5.0f * a ), 4.0f, 1.2f * sinf( 5.0f * a ) }; + b3Vec3 start = b3TransformPoint( transform, local ); + b3Vec3 translation = b3RotateVector( q, (b3Vec3){ 0.0f, -8.0f, 0.5f * sinf( 9.0f * a ) } ); + b3Vec3 localStart = b3InvTransformPoint( transform, start ); + b3ShapeCastInput input = { { &localStart, 1, 0.3f }, b3InvRotateVector( q, translation ), 1.0f, false }; + b3CastOutput out = b3ShapeCastCapsule( &capsule, &input ); + if ( out.hit ) + { + out.point = b3TransformPoint( transform, out.point ); + out.normal = b3RotateVector( q, out.normal ); + castHits += 1; + castSum += out.fraction; + } + } + double t3 = now_ms(); + + int overlapHits = 0; + for ( int i = 0; i < OVERLAPS; ++i ) + { + float t = (float)i / (float)OVERLAPS; + float a = 6.2831853f * t; + b3Vec3 local = { 3.0f * cosf( 5.0f * a ), 1.6f * sinf( 3.0f * a ), 1.2f * sinf( 5.0f * a ) }; + b3Vec3 center = b3TransformPoint( transform, local ); + b3ShapeProxy proxy = { ¢er, 1, 0.5f }; + if ( b3OverlapCapsule( &capsule, transform, &proxy ) ) overlapHits += 1; + } + double t4 = now_ms(); + + int moverPlanes = 0; + double moverSum = 0.0; + for ( int i = 0; i < MOVERS; ++i ) + { + float t = (float)i / (float)MOVERS; + float a = 6.2831853f * t; + b3Vec3 local = { 3.0f * cosf( 5.0f * a ), 1.0f + 0.6f * sinf( 3.0f * a ), 1.0f * sinf( 5.0f * a ) }; + b3Capsule mover = { b3TransformPoint( transform, local ), b3TransformPoint( transform, b3Add( local, (b3Vec3){ 0.0f, 1.0f, 0.0f } ) ), 0.3f }; + b3Capsule localMover = { b3InvTransformPoint( transform, mover.center1 ), b3InvTransformPoint( transform, mover.center2 ), mover.radius }; + b3PlaneResult planes[4]; + int count = b3CollideMoverAndCapsule( planes, &capsule, &localMover ); + for ( int k = 0; k < count; ++k ) + { + planes[k].plane.normal = b3RotateVector( q, planes[k].plane.normal ); + planes[k].point = b3TransformPoint( transform, planes[k].point ); + } + moverPlanes += count; + if ( count > 0 ) moverSum += planes[0].plane.offset; + } + double t5 = now_ms(); + + double massSum = 0.0; + for ( int i = 0; i < MASSES; ++i ) + { + float t = (float)i / (float)MASSES; + b3Capsule c = { { -1.0f - t, 0.0f, 0.0f }, { 1.0f, t, 0.0f }, 0.5f + 0.5f * t }; + b3MassData md = b3ComputeCapsuleMass( &c, 1.0f ); + massSum += md.mass + md.inertia.cx.x + md.inertia.cy.y + md.inertia.cz.z; + } + double t6 = now_ms(); + + printf( "box3d shape: %d sphere rays %.2f ms (%d hits, sum %.3f), %d capsule rays %.2f ms (%d hits, sum %.3f), " + "%d shape casts %.2f ms (%d hits, sum %.3f), %d overlaps %.2f ms (%d hits), %d movers %.2f ms (%d planes, sum %.3f), " + "%d masses %.2f ms (sum %.3f)\n", + RAYS, t1 - t0, sphereHits, sphereSum, RAYS, t2 - t1, capsuleHits, capsuleSum, CASTS, t3 - t2, castHits, castSum, + OVERLAPS, t4 - t3, overlapHits, MOVERS, t5 - t4, moverPlanes, moverSum, MASSES, t6 - t5, massSum ); + return 0; +} diff --git a/design.md b/design.md index 59d0c28..1a14112 100644 --- a/design.md +++ b/design.md @@ -88,20 +88,38 @@ started until its tests pass. query and the mover. The same results as the reference; the query at parity, the casts 1.4-2x. The heights, materials and flags are ints for want of 16- and 8-bit arrays, 3x the reference's bytes. -10. **shape**: shape.c (mass properties, ray and shape casts per shape, - compounds). Test: `test_shape`. -11. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +10. **shape** (done): sphere.c, capsule.c and the geometric half of + shape.c as `aephysics.shape`: the sphere's and capsule's mass, bounds, + ray casts (the closest-point forms that hold their precision far from + the origin), casts, overlap and mover planes, the hull's mover, and + the Shape of any kind with its dispatch under a transform (bounds, + swept and fat bounds, centroid, areas, mass, extent, ray and shape + casts, overlap, mover, proxy) and the collision filters. 440 checks: + test_shape.c's masses (sphere, analytic and transformed boxes, the + capsule bracketed by hulls), bounds, the sphere and capsule ray cases, + the overlap convention, the far-origin precision (our doubles sit + three orders under the reference's float floor and still hit at ten + million units), the cast through the dispatch; plus every kind under + one transform through every query, and the filters. Rays and masses + at parity; the GJK cast and overlap on proxies with radii 2-2.5x, + to profile (aephysics#9). The world-bound half of shape.c (creation + on a body, the broad-phase proxy, materials, events) comes with the + dynamics. +11. **compound**: compound.c (the baked compound: children under a + static tree, materials and hulls shared by content) with + test_compound.c, and the compound branch of the shape dispatch. +12. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), `solver_set`, `island`, `solver` (the Soft Step: sub-stepping, relax iterations, restitution), `contact_solver` (scalar first; the wide SIMD path second, measured), the joints (revolute, prismatic, distance, motor, weld, wheel, spherical), `sensor`, `mover` (the character mover), `physics_world`. Tests: `test_body`, `test_joint`, `test_world`, `test_mover`, `test_determinism`, `test_large_world`. -12. **parallel**: `parallel_for` and the scheduler over Aether's actors; +13. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -13. **recording and replay**, `world_snapshot`: last, since they are the +14. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -14. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +15. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures