From d72410e401600b30d4e582ebff64ea9bcf6c9bc5 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Fri, 9 Oct 2026 06:02:02 -0300 Subject: [PATCH] Debug draw (#163, second part) aephysics.debug_draw has the reference's b3DebugDraw, its colours and material presets; physics_world.world_draw draws a world as b3World_Draw does, each joint kind as the reference draws it; the world def's debug shape callbacks give a host its own drawable per shape, made when first drawn and given back when the shape goes, its geometry changes or the world goes, and kept through an in-place restore; the player draws a replayed frame's queries. --- README.md | 1 + aephysics/debug_draw/module.ae | 322 ++++++++++++++++++++ aephysics/dynamics/module.ae | 84 +++++- aephysics/joint_solver/module.ae | 226 ++++++++++++++- aephysics/physics_world/module.ae | 275 +++++++++++++++++- aephysics/recording/module.ae | 186 +++++++++++- aephysics/snapshot/module.ae | 49 +++- design.md | 6 +- tests/test_debug_draw.ae | 467 ++++++++++++++++++++++++++++++ 9 files changed, 1600 insertions(+), 16 deletions(-) create mode 100644 aephysics/debug_draw/module.ae create mode 100644 tests/test_debug_draw.ae diff --git a/README.md b/README.md index 9dd4bfd..e289593 100644 --- a/README.md +++ b/README.md @@ -51,6 +51,7 @@ so a test written against the reference reads the same here. | `aephysics.joint_solver` | the seven joints solved: each kind's prepare (frames relative to the centres of mass, effective masses, spring softness), warm start and solve (rigid or soft constraints, speculative limits, springs, motors), the kinds' accessors (limits, springs, motors, current angles and translations, forces and torques), joint.c's dispatch with the constraint hertz clamp; beyond the reference, a spherical joint's cone may be centred off frame A's z and may be an ellipse of two half-angles (the circle stays the default, bit for bit) | `test_joint_solver.ae` (181 checks: test_joint.c's accessors on every kind, each kind solved by hand) | [ours alone](bench/RESULTS.md#joint_solver) | | `aephysics.solver` | the Soft Step over the workers, the reference's stages: the work in blocks (the bodies', each colour's joints, wide and scalar constraints) that workers claim by compare-and-swap, the stages published as sync bits by worker 0 -- the constraints prepared, per sub-step the velocities integrated (gravity, damping, the gyroscopic torque), warm start, solve, positions, relax, colour by colour with the overflow first on worker 0; restitution, the impulses stored; the bodies finalised in parallel (sleep velocities, move events, fast bodies swept for the time of impact, bounds and proxies), the joint and hit events, the trees refit, bullets, the sensors' hits, islands put to sleep | `test_solver.ae` (45 checks: free fall against the closed form, resting and sleeping, a stack, a bounce and its hit event, a joint event, a pendulum, a fast sphere stopped by a thin wall, a bullet, a sensor swept, two worlds bit for bit alike) | [1.2-1.3x the reference's step, stage by stage](bench/RESULTS.md#stage-by-stage) | | `aephysics.physics_world` | the world's face: the step (pairs, narrow phase, solve, sensors, events), the events read back, the settings and counters (the reference's whole `b3Counters`: byte count, tasks, manifold counts, time of impact iterations), the maximum capacity kept at each step's start, the memory part by part to the host's log, the world counts, version and CPU queries, queries over every shape (overlap of a box or a proxy, the mover's planes, ray, shape and mover casts, the closest ray hit) and against one body at a transform of the caller's, explosions | `test_physics_world.ae` (259 checks: test_world.c whole, among it the hull database (equal hulls one world-owned copy, a caller's hull freed at once, transformed hulls baked once, every holder's release), empty and recycled worlds, every setting, the worker count set between steps, contact, hit, move and sensor events, a compound child's hit events and materials, the overflow colour pile, the explosion near and far, the bullet and sleep flags, the compound count, an enlarged proxy destroyed; test_body_query.c whole: casts, overlaps of hull proxies and turned compounds, mover planes, and the mover's time of impact against 2,000 random sweeps; test_broad_phase.c's static rebuild and its stress test, a swarm through every proxy change with the pair set checked by brute force every step; test_id.c; the world queries; a wave pile stepping alike twice) | [the reference's own benchmark scenes with the same checksums: 1.2x its AVX2 build on the large pyramid, 1.15x on the many pyramids, 1.05x on the joint grid, ours at eight lanes in AVX2 with float records (#121)](bench/RESULTS.md#the-collide-pass-as-the-reference-keeps-it-2026-10-09-121); [the rest of its suite, nine scenes and the SAT runs, at 1.04-1.21x](bench/RESULTS.md#suite-the-references-benchmark-suite) | +| `aephysics.debug_draw` | what a host draws a world with (the reference's b3DebugDraw): callbacks for a user shape, segments, transforms, points, spheres, capsules, bounds, boxes and strings, the options, the reference's 145 named colours, material presets and graph colours; `physics_world.world_draw` walks the shapes in the drawing bounds (each coloured by its body's state as the reference picks it, or by its material's own colour) and draws what the options ask of their bodies (names, mass, sleep, each joint kind once, contacts by state or graph colour with normals, forces or features, islands); the world def's debug shape callbacks make the host's drawable for a shape when it is first drawn and give it back when the shape goes, its geometry changes (the reference keeps the stale one) or the world goes (the reference leaves those to the host), and an in-place restore keeps the drawables of the shapes still there; `recording.player_draw_frame_queries` draws a replayed frame's queries, the selected one labelled with its caller | done, `test_debug_draw.ae` (41 checks: every shape once with its colour, a drawable each made once and kept, every option counted against the world (a point per manifold point, a weld's two boxes, a label per joint), the drawing bounds and the mask culling, the drawables' lives through destroy, geometry change, restore and the world's end, a player's world and its queries drawn, the labels' number formats) | | `aephysics.snapshot` | a world as bytes and back (the reference's world_snapshot.c): `save_world` and `load_world` keep everything a step reads (the settings, the id pools, the solver sets, bodies, shapes and their geometry once each, contacts with their manifolds and mesh caches, joints, sensors, islands, the trees and pair set, the graph's colours), so a loaded world steps on as the one it was taken from, bit for bit; `restore_world` loads one into an existing world in place, its id, workers and callbacks kept (what a replay rewinds with); a snapshot of another layout or any damage to its bytes loads no world | done, `test_snapshot.ae` (63 checks: a world of every kind of thing loaded at four moments and stepped 150 steps beside its original, the same hash and events every step; a loaded world saves to the same bytes; one worker's world loaded with four steps alike; a restore in place stepping alike and saving to the same bytes, a refused restore leaving the world empty and whole; every truncation and corruption refused, nothing leaked) | | `aephysics.recording` | recording and replay (the reference's recording.c and recording_replay.c): a world records every call that changes it, 160 kinds, field by field, a create's record with the id it made, a state hash after every step and the geometry the calls name once each, and every world query with each visit and the caller's answer (a caller named by the query filter's id and name gets a key a viewer follows it by); a player replays it into a world of its own, restored in place so its id stays, checking every step's hash and every created id, asking every recorded query again with the recorded answers and comparing each visit, with keyframes under a memory budget for seeking, bodies in the order they were made, the queries of the frame last played with their hits, a pause before a step that makes bodies, and the recording's info; records it does not know are skipped, as a newer minor version may add some | `test_recording.ae` (152 checks: the snapshot test's scene recorded from an empty world and from mid stream with the host changing it between steps, played on one worker and several, every frame's hash as recorded; every one of the 160 calls recorded and replayed (the reference's AllOps); geometry kept once from two copies; seeking back and forth with and without keyframes and under a tight budget; the reference's QueryReplay and TaggedQuery, every query kind with answers that stop and clip them, a changed hit caught at its frame; the reference's NameRoundTrip, RollbackNames, GeometryHashCollision and ReservedHeaderBytes; a file round trip; every truncation, a skipped call, a changed id, a short record and a changed hash each caught, nothing leaked); `test_threads.ae` (four threads querying a recording world, every record whole and replayed alike) | | `aephysics.human` | the ragdoll: twelve capsule bones on spherical joints with cone and twist limits and revolute joints with angle limits -- a person's ranges by default (shoulders that reach overhead on a cone centred off the rest, hips on an elliptical cone through 125 degrees ahead, 25 back, 45 out and 30 across, knees and elbows that never pass straight), the reference's tight ones on request -- soles that grip, and on request (`Human.ankles`, past the reference) the feet as bones of their own on ankles, an elliptical cone through 50 degrees down, 35 up and 15 aside with 15 of roll, so a crouched figure's soles lie flat, a spring on every joint toward the reference pose, a motor whose torque limit is joint friction, filter joints for the limbs that clash; the align spring, kinematic anchors through motor or parallel joints and their targets driven over a step (the pose drive of an active ragdoll), velocity, kicks, bullets | `test_human.ae` (115 checks: the figure's shape, a fall to rest in one piece, the setters, a kick, standing under the align spring, the pose held on anchors, the anchors driven two metres and the figure walked behind them, the same drop twice bit for bit, a fitted shape, each arm reaching flexion 170, abduction 160 and extension 45 degrees standing and prone, the hips' cone through a person's four ranges and short of a split, the hinges stopping at straight and folding to their range, the soles' grip, the ankles' four ranges measured through the joint and the soles flat under shins tipped 25 degrees over them) | [the reference's rain benchmark, 300 ragdolls over 400 steps, at 0.9x](bench/RESULTS.md#human) | diff --git a/aephysics/debug_draw/module.ae b/aephysics/debug_draw/module.ae new file mode 100644 index 0000000..b485751 --- /dev/null +++ b/aephysics/debug_draw/module.ae @@ -0,0 +1,322 @@ +// What a host draws a world with (the reference's b3DebugDraw): its +// callbacks for a user shape, a segment, a transform, a point, a sphere, a +// capsule, a box, an oriented box and a string, the options of what to +// draw, and the named colours and material presets the reference's +// drawing uses. physics_world.world_draw walks a world through it; +// recording.player_draw_frame_queries a replayed frame's queries. +// +// A user shape is the host's own drawable for a shape, made when the shape +// is first drawn by the world def's create_debug_shape (and given back by +// destroy_debug_shape when the shape goes, its geometry changes, or the +// world goes). + +import std.string +import aephysics.math + +exports ( + DebugDraw, default_debug_draw, make_debug_color, graph_color, fixed, hex, + DEBUG_MATERIAL_DEFAULT, DEBUG_MATERIAL_MATTE, DEBUG_MATERIAL_SOFT, DEBUG_MATERIAL_DEAD, DEBUG_MATERIAL_GLOSSY, DEBUG_MATERIAL_METALLIC, + COLOR_ALICE_BLUE, COLOR_ANTIQUE_WHITE, COLOR_AQUA, COLOR_AQUAMARINE, COLOR_AZURE, COLOR_BEIGE, COLOR_BISQUE, + COLOR_BLACK, COLOR_BLANCHED_ALMOND, COLOR_BLUE, COLOR_BLUE_VIOLET, COLOR_BROWN, COLOR_BURLYWOOD, COLOR_CADET_BLUE, + COLOR_CHARTREUSE, COLOR_CHOCOLATE, COLOR_CORAL, COLOR_CORNFLOWER_BLUE, COLOR_CORNSILK, COLOR_CRIMSON, COLOR_CYAN, + COLOR_DARK_BLUE, COLOR_DARK_CYAN, COLOR_DARK_GOLDEN_ROD, COLOR_DARK_GRAY, COLOR_DARK_GREEN, COLOR_DARK_KHAKI, + COLOR_DARK_MAGENTA, COLOR_DARK_OLIVE_GREEN, COLOR_DARK_ORANGE, COLOR_DARK_ORCHID, COLOR_DARK_RED, + COLOR_DARK_SALMON, COLOR_DARK_SEA_GREEN, COLOR_DARK_SLATE_BLUE, COLOR_DARK_SLATE_GRAY, COLOR_DARK_TURQUOISE, + COLOR_DARK_VIOLET, COLOR_DEEP_PINK, COLOR_DEEP_SKY_BLUE, COLOR_DIM_GRAY, COLOR_DODGER_BLUE, COLOR_FIRE_BRICK, + COLOR_FLORAL_WHITE, COLOR_FOREST_GREEN, COLOR_FUCHSIA, COLOR_GAINSBORO, COLOR_GHOST_WHITE, COLOR_GOLD, + COLOR_GOLDEN_ROD, COLOR_GRAY, COLOR_GREEN, COLOR_GREEN_YELLOW, COLOR_HONEY_DEW, COLOR_HOT_PINK, COLOR_INDIAN_RED, + COLOR_INDIGO, COLOR_IVORY, COLOR_KHAKI, COLOR_LAVENDER, COLOR_LAVENDER_BLUSH, COLOR_LAWN_GREEN, + COLOR_LEMON_CHIFFON, COLOR_LIGHT_BLUE, COLOR_LIGHT_CORAL, COLOR_LIGHT_CYAN, COLOR_LIGHT_GOLDEN_ROD_YELLOW, + COLOR_LIGHT_GRAY, COLOR_LIGHT_GREEN, COLOR_LIGHT_PINK, COLOR_LIGHT_SALMON, COLOR_LIGHT_SEA_GREEN, + COLOR_LIGHT_SKY_BLUE, COLOR_LIGHT_SLATE_GRAY, COLOR_LIGHT_STEEL_BLUE, COLOR_LIGHT_YELLOW, COLOR_LIME, + COLOR_LIME_GREEN, COLOR_LINEN, COLOR_MAGENTA, COLOR_MAROON, COLOR_MEDIUM_AQUA_MARINE, COLOR_MEDIUM_BLUE, + COLOR_MEDIUM_ORCHID, COLOR_MEDIUM_PURPLE, COLOR_MEDIUM_SEA_GREEN, COLOR_MEDIUM_SLATE_BLUE, + COLOR_MEDIUM_SPRING_GREEN, COLOR_MEDIUM_TURQUOISE, COLOR_MEDIUM_VIOLET_RED, COLOR_MIDNIGHT_BLUE, COLOR_MINT_CREAM, + COLOR_MISTY_ROSE, COLOR_MOCCASIN, COLOR_NAVAJO_WHITE, COLOR_NAVY, COLOR_OLD_LACE, COLOR_OLIVE, COLOR_OLIVE_DRAB, + COLOR_ORANGE, COLOR_ORANGE_RED, COLOR_ORCHID, COLOR_PALE_GOLDEN_ROD, COLOR_PALE_GREEN, COLOR_PALE_TURQUOISE, + COLOR_PALE_VIOLET_RED, COLOR_PAPAYA_WHIP, COLOR_PEACH_PUFF, COLOR_PERU, COLOR_PINK, COLOR_PLUM, COLOR_POWDER_BLUE, + COLOR_PURPLE, COLOR_REBECCA_PURPLE, COLOR_RED, COLOR_ROSY_BROWN, COLOR_ROYAL_BLUE, COLOR_SADDLE_BROWN, + COLOR_SALMON, COLOR_SANDY_BROWN, COLOR_SEA_GREEN, COLOR_SEA_SHELL, COLOR_SIENNA, COLOR_SILVER, COLOR_SKY_BLUE, + COLOR_SLATE_BLUE, COLOR_SLATE_GRAY, COLOR_SNOW, COLOR_SPRING_GREEN, COLOR_STEEL_BLUE, COLOR_TAN, COLOR_TEAL, + COLOR_THISTLE, COLOR_TOMATO, COLOR_TURQUOISE, COLOR_VIOLET, COLOR_WHEAT, COLOR_WHITE, COLOR_WHITE_SMOKE, + COLOR_YELLOW, COLOR_YELLOW_GREEN, COLOR_BOX2D_RED, COLOR_BOX2_D_BLUE, COLOR_BOX2_D_GREEN, COLOR_BOX2_D_YELLOW +) + +struct DebugDraw { + draw_shape: fn(ptr, Transform, int, ptr) // a user shape at a transform + draw_segment: fn(Vec3, Vec3, int, ptr) + draw_transform: fn(Transform, ptr) // axes of the host's own length + draw_point: fn(Vec3, float, int, ptr) // a point of a size in pixels + draw_sphere: fn(Vec3, float, int, float, ptr) // centre, radius, colour, alpha + draw_capsule: fn(Vec3, Vec3, float, int, float, ptr) // the two centres, radius, colour, alpha + draw_bounds: fn(AABB, int, ptr) + draw_box: fn(Vec3, Transform, int, ptr) // half extents at a transform + draw_string: fn(Vec3, string, int, ptr) // text at a world point + drawing_bounds: AABB // what is drawn: the shapes whose bounds overlap it + force_scale: float // a contact force's length per newton + joint_scale: float // the joints' drawing, scaled + draw_shapes: bool + draw_joints: bool + draw_joint_extras: bool // each joint's constraint force and torque + draw_bounds_option: bool // each shape's fat bounds (the reference's drawBounds) + draw_mass: bool // the centre of mass and the mass of dynamic bodies + draw_sleep: bool // the sleep velocity against the threshold + draw_body_names: bool + draw_contacts: bool + draw_anchor_a: bool // contact points at body A's anchors, else B's + draw_graph_colors: bool // the constraint graph's colour of each contact and joint + draw_contact_features: bool + draw_contact_normals: bool + draw_contact_forces: bool + draw_islands: bool // each island's bounds + context: ptr // given to every callback +} + +no_shape(user_shape: ptr, transform: Transform, color: int, context: ptr) { } +no_segment(p1: Vec3, p2: Vec3, color: int, context: ptr) { } +no_transform(transform: Transform, context: ptr) { } +no_point(p: Vec3, size: float, color: int, context: ptr) { } +no_sphere(p: Vec3, radius: float, color: int, alpha: float, context: ptr) { } +no_capsule(p1: Vec3, p2: Vec3, radius: float, color: int, alpha: float, context: ptr) { } +no_bounds(box: AABB, color: int, context: ptr) { } +no_box(extents: Vec3, transform: Transform, color: int, context: ptr) { } +no_string(p: Vec3, text: string, color: int, context: ptr) { } + +// Callbacks that draw nothing (a host sets the ones it has), every option +// off, bounds a hundred metres out every way, unit scales +// (b3DefaultDebugDraw). +default_debug_draw() -> DebugDraw { + h = 100.0 + return DebugDraw { draw_shape: no_shape, draw_segment: no_segment, draw_transform: no_transform, draw_point: no_point, + draw_sphere: no_sphere, draw_capsule: no_capsule, draw_bounds: no_bounds, draw_box: no_box, draw_string: no_string, + drawing_bounds: AABB { lower: math.vec3(0.0 - h, 0.0 - h, 0.0 - h), upper: math.vec3(h, h, h) }, + force_scale: 1.0, joint_scale: 1.0, draw_shapes: false, draw_joints: false, draw_joint_extras: false, + draw_bounds_option: false, draw_mass: false, draw_sleep: false, draw_body_names: false, draw_contacts: false, + draw_anchor_a: false, draw_graph_colors: false, draw_contact_features: false, draw_contact_normals: false, + draw_contact_forces: false, draw_islands: false, context: null } +} + +// A preset of how a drawn colour should look, packed in a colour's high +// byte (the reference's b3DebugMaterial); 0 keeps the host's own look. +const DEBUG_MATERIAL_DEFAULT = 0 +const DEBUG_MATERIAL_MATTE = 1 +const DEBUG_MATERIAL_SOFT = 2 +const DEBUG_MATERIAL_DEAD = 3 +const DEBUG_MATERIAL_GLOSSY = 4 +const DEBUG_MATERIAL_METALLIC = 5 + +// An RGB colour with a material preset in its high byte (b3MakeDebugColor). +make_debug_color(rgb: int, material: int) -> int { return (rgb & 0x00FFFFFF) | (material << 24) } + +// The colour drawn for a constraint graph colour, the overflow last (b3GetGraphColor). +graph_color(index: int) -> int { + if index == 0 { return COLOR_RED } + if index == 1 { return COLOR_ORANGE } + if index == 2 { return COLOR_YELLOW } + if index == 3 { return COLOR_GREEN } + if index == 4 { return COLOR_CYAN } + if index == 5 { return COLOR_BLUE } + if index == 6 { return COLOR_VIOLET } + if index == 7 { return COLOR_PINK } + if index == 8 { return COLOR_CHOCOLATE } + if index == 9 { return COLOR_GOLDEN_ROD } + if index == 10 { return COLOR_CORAL } + if index == 11 { return COLOR_ROSY_BROWN } + if index == 12 { return COLOR_AQUA } + if index == 13 { return COLOR_PERU } + if index == 14 { return COLOR_LIME } + if index == 15 { return COLOR_GOLD } + if index == 16 { return COLOR_PLUM } + if index == 17 { return COLOR_SNOW } + if index == 18 { return COLOR_TEAL } + if index == 19 { return COLOR_KHAKI } + if index == 20 { return COLOR_SALMON } + if index == 21 { return COLOR_PEACH_PUFF } + if index == 22 { return COLOR_HONEY_DEW } + return COLOR_BLACK +} + +// A number with `digits` decimals, rounded half away from zero, for the +// drawing's labels (the reference's "%.2f" and its siblings). +fixed(value: float, digits: int) -> string { + scale = 1 as long + i = 0 + while i < digits { + scale = scale * (10 as long) + i = i + 1 + } + negative = value < 0.0 + magnitude = value + if negative { magnitude = 0.0 - value } + units = (magnitude * (scale as float) + 0.5) as long + whole = units / scale + sign = "" + if negative && units != (0 as long) { sign = "-" } + if digits <= 0 { return "${sign}${whole}" } + rest = "${units % scale}" + while string.length(rest) < digits { rest = "0${rest}" } + return "${sign}${whole}.${rest}" +} + +// A value in hexadecimal with the reference's "%#X" look: 0X and capitals, +// 0 alone. +hex(value: int) -> string { + if value == 0 { return "0" } + digits = "0123456789ABCDEF" + bits = value as long + if bits < (0 as long) { bits = bits + (1 as long << 32) } + text = "" + while bits > (0 as long) { + nibble = (bits % (16 as long)) as int + text = "${string.substring(digits, nibble, nibble + 1)}${text}" + bits = bits / (16 as long) + } + return "0X${text}" +} + +// The reference's named colours (b3HexColor), as 0xRRGGBB. +const COLOR_ALICE_BLUE = 0xF0F8FF +const COLOR_ANTIQUE_WHITE = 0xFAEBD7 +const COLOR_AQUA = 0x00FFFF +const COLOR_AQUAMARINE = 0x7FFFD4 +const COLOR_AZURE = 0xF0FFFF +const COLOR_BEIGE = 0xF5F5DC +const COLOR_BISQUE = 0xFFE4C4 +const COLOR_BLACK = 0x000000 +const COLOR_BLANCHED_ALMOND = 0xFFEBCD +const COLOR_BLUE = 0x0000FF +const COLOR_BLUE_VIOLET = 0x8A2BE2 +const COLOR_BROWN = 0xA52A2A +const COLOR_BURLYWOOD = 0xDEB887 +const COLOR_CADET_BLUE = 0x5F9EA0 +const COLOR_CHARTREUSE = 0x7FFF00 +const COLOR_CHOCOLATE = 0xD2691E +const COLOR_CORAL = 0xFF7F50 +const COLOR_CORNFLOWER_BLUE = 0x6495ED +const COLOR_CORNSILK = 0xFFF8DC +const COLOR_CRIMSON = 0xDC143C +const COLOR_CYAN = 0x00FFFF +const COLOR_DARK_BLUE = 0x00008B +const COLOR_DARK_CYAN = 0x008B8B +const COLOR_DARK_GOLDEN_ROD = 0xB8860B +const COLOR_DARK_GRAY = 0xA9A9A9 +const COLOR_DARK_GREEN = 0x006400 +const COLOR_DARK_KHAKI = 0xBDB76B +const COLOR_DARK_MAGENTA = 0x8B008B +const COLOR_DARK_OLIVE_GREEN = 0x556B2F +const COLOR_DARK_ORANGE = 0xFF8C00 +const COLOR_DARK_ORCHID = 0x9932CC +const COLOR_DARK_RED = 0x8B0000 +const COLOR_DARK_SALMON = 0xE9967A +const COLOR_DARK_SEA_GREEN = 0x8FBC8F +const COLOR_DARK_SLATE_BLUE = 0x483D8B +const COLOR_DARK_SLATE_GRAY = 0x2F4F4F +const COLOR_DARK_TURQUOISE = 0x00CED1 +const COLOR_DARK_VIOLET = 0x9400D3 +const COLOR_DEEP_PINK = 0xFF1493 +const COLOR_DEEP_SKY_BLUE = 0x00BFFF +const COLOR_DIM_GRAY = 0x696969 +const COLOR_DODGER_BLUE = 0x1E90FF +const COLOR_FIRE_BRICK = 0xB22222 +const COLOR_FLORAL_WHITE = 0xFFFAF0 +const COLOR_FOREST_GREEN = 0x228B22 +const COLOR_FUCHSIA = 0xFF00FF +const COLOR_GAINSBORO = 0xDCDCDC +const COLOR_GHOST_WHITE = 0xF8F8FF +const COLOR_GOLD = 0xFFD700 +const COLOR_GOLDEN_ROD = 0xDAA520 +const COLOR_GRAY = 0x808080 +const COLOR_GREEN = 0x008000 +const COLOR_GREEN_YELLOW = 0xADFF2F +const COLOR_HONEY_DEW = 0xF0FFF0 +const COLOR_HOT_PINK = 0xFF69B4 +const COLOR_INDIAN_RED = 0xCD5C5C +const COLOR_INDIGO = 0x4B0082 +const COLOR_IVORY = 0xFFFFF0 +const COLOR_KHAKI = 0xF0E68C +const COLOR_LAVENDER = 0xE6E6FA +const COLOR_LAVENDER_BLUSH = 0xFFF0F5 +const COLOR_LAWN_GREEN = 0x7CFC00 +const COLOR_LEMON_CHIFFON = 0xFFFACD +const COLOR_LIGHT_BLUE = 0xADD8E6 +const COLOR_LIGHT_CORAL = 0xF08080 +const COLOR_LIGHT_CYAN = 0xE0FFFF +const COLOR_LIGHT_GOLDEN_ROD_YELLOW = 0xFAFAD2 +const COLOR_LIGHT_GRAY = 0xD3D3D3 +const COLOR_LIGHT_GREEN = 0x90EE90 +const COLOR_LIGHT_PINK = 0xFFB6C1 +const COLOR_LIGHT_SALMON = 0xFFA07A +const COLOR_LIGHT_SEA_GREEN = 0x20B2AA +const COLOR_LIGHT_SKY_BLUE = 0x87CEFA +const COLOR_LIGHT_SLATE_GRAY = 0x778899 +const COLOR_LIGHT_STEEL_BLUE = 0xB0C4DE +const COLOR_LIGHT_YELLOW = 0xFFFFE0 +const COLOR_LIME = 0x00FF00 +const COLOR_LIME_GREEN = 0x32CD32 +const COLOR_LINEN = 0xFAF0E6 +const COLOR_MAGENTA = 0xFF00FF +const COLOR_MAROON = 0x800000 +const COLOR_MEDIUM_AQUA_MARINE = 0x66CDAA +const COLOR_MEDIUM_BLUE = 0x0000CD +const COLOR_MEDIUM_ORCHID = 0xBA55D3 +const COLOR_MEDIUM_PURPLE = 0x9370DB +const COLOR_MEDIUM_SEA_GREEN = 0x3CB371 +const COLOR_MEDIUM_SLATE_BLUE = 0x7B68EE +const COLOR_MEDIUM_SPRING_GREEN = 0x00FA9A +const COLOR_MEDIUM_TURQUOISE = 0x48D1CC +const COLOR_MEDIUM_VIOLET_RED = 0xC71585 +const COLOR_MIDNIGHT_BLUE = 0x191970 +const COLOR_MINT_CREAM = 0xF5FFFA +const COLOR_MISTY_ROSE = 0xFFE4E1 +const COLOR_MOCCASIN = 0xFFE4B5 +const COLOR_NAVAJO_WHITE = 0xFFDEAD +const COLOR_NAVY = 0x000080 +const COLOR_OLD_LACE = 0xFDF5E6 +const COLOR_OLIVE = 0x808000 +const COLOR_OLIVE_DRAB = 0x6B8E23 +const COLOR_ORANGE = 0xFFA500 +const COLOR_ORANGE_RED = 0xFF4500 +const COLOR_ORCHID = 0xDA70D6 +const COLOR_PALE_GOLDEN_ROD = 0xEEE8AA +const COLOR_PALE_GREEN = 0x98FB98 +const COLOR_PALE_TURQUOISE = 0xAFEEEE +const COLOR_PALE_VIOLET_RED = 0xDB7093 +const COLOR_PAPAYA_WHIP = 0xFFEFD5 +const COLOR_PEACH_PUFF = 0xFFDAB9 +const COLOR_PERU = 0xCD853F +const COLOR_PINK = 0xFFC0CB +const COLOR_PLUM = 0xDDA0DD +const COLOR_POWDER_BLUE = 0xB0E0E6 +const COLOR_PURPLE = 0x800080 +const COLOR_REBECCA_PURPLE = 0x663399 +const COLOR_RED = 0xFF0000 +const COLOR_ROSY_BROWN = 0xBC8F8F +const COLOR_ROYAL_BLUE = 0x4169E1 +const COLOR_SADDLE_BROWN = 0x8B4513 +const COLOR_SALMON = 0xFA8072 +const COLOR_SANDY_BROWN = 0xF4A460 +const COLOR_SEA_GREEN = 0x2E8B57 +const COLOR_SEA_SHELL = 0xFFF5EE +const COLOR_SIENNA = 0xA0522D +const COLOR_SILVER = 0xC0C0C0 +const COLOR_SKY_BLUE = 0x87CEEB +const COLOR_SLATE_BLUE = 0x6A5ACD +const COLOR_SLATE_GRAY = 0x708090 +const COLOR_SNOW = 0xFFFAFA +const COLOR_SPRING_GREEN = 0x00FF7F +const COLOR_STEEL_BLUE = 0x4682B4 +const COLOR_TAN = 0xD2B48C +const COLOR_TEAL = 0x008080 +const COLOR_THISTLE = 0xD8BFD8 +const COLOR_TOMATO = 0xFF6347 +const COLOR_TURQUOISE = 0x40E0D0 +const COLOR_VIOLET = 0xEE82EE +const COLOR_WHEAT = 0xF5DEB3 +const COLOR_WHITE = 0xFFFFFF +const COLOR_WHITE_SMOKE = 0xF5F5F5 +const COLOR_YELLOW = 0xFFFF00 +const COLOR_YELLOW_GREEN = 0x9ACD32 +const COLOR_BOX2D_RED = 0xDC3132 +const COLOR_BOX2D_BLUE = 0x30AEBF +const COLOR_BOX2D_GREEN = 0x8CC924 +const COLOR_BOX2D_YELLOW = 0xFFEE8C diff --git a/aephysics/dynamics/module.ae b/aephysics/dynamics/module.ae index a34081a..57eb1fb 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -158,7 +158,8 @@ exports ( TaskContext, task_context, task_context_at, task_sensor_hit_at, enter_world, leave_world, reset_world, get_world_count, get_max_world_count, track_capacity, contact_get_data, joint_get_constraint_tuning, shape_get_mesh_material_count, - MANIFOLD_COUNT_BUCKETS, hull_entry_at, HullEntry, OwnedGeometry + MANIFOLD_COUNT_BUCKETS, hull_entry_at, HullEntry, OwnedGeometry, + DebugShape, release_debug_shape, release_debug_shapes, set_debug_shape_callbacks ) extern memset(block: ptr, value: int, size: int) -> ptr @@ -278,7 +279,30 @@ struct WorldDef { worker_count: int // the threads a step runs over: 1 is the calling thread alone scheduler: ptr // a *Scheduler of the host's to step on (its worker count then rules), or null for the world's own user_data: ptr -} + // The host's drawable for a shape, made when the shape is first drawn + // and given back when it goes, its geometry changes or the world goes + // (b3CreateDebugShapeCallback, b3DestroyDebugShapeCallback); null from + // create_debug_shape for none. + create_debug_shape: fn(*DebugShape, ptr) -> ptr + destroy_debug_shape: fn(ptr, ptr) + user_debug_shape_context: ptr +} + +// A shape as the host's create_debug_shape sees it (b3DebugShape): its id, +// its kind and its geometry, of which the pointer for its kind is set. +struct DebugShape { + shape_id: ShapeId + kind: int // shape.SHAPE_* + sphere: *Sphere + capsule: *Capsule + hull: *HullData + mesh: *Mesh // the mesh with its scale + height_field: *HeightFieldData + compound: *CompoundData +} + +no_debug_shape(debug_shape: *DebugShape, context: ptr) -> ptr { return null } +drop_debug_shape(user_shape: ptr, context: ptr) { } struct MotionLocks { linear_x: bool @@ -359,7 +383,8 @@ default_world_def() -> WorldDef { contact_hertz: 30.0, contact_damping_ratio: 10.0, maximum_linear_speed: 400.0, friction_callback: default_friction, restitution_callback: default_restitution, enable_sleep: true, enable_continuous: true, static_body_count: 0, dynamic_body_count: 0, - static_shape_count: 0, dynamic_shape_count: 0, contact_count: 0, worker_count: 1, scheduler: null, user_data: null } + static_shape_count: 0, dynamic_shape_count: 0, contact_count: 0, worker_count: 1, scheduler: null, user_data: null, + create_debug_shape: no_debug_shape, destroy_debug_shape: drop_debug_shape, user_debug_shape_context: null } } default_body_def() -> BodyDef { @@ -493,6 +518,7 @@ struct WorldShape { materials: ptr // SurfaceMaterial[] when there are several filter: Filter user_data: ptr + user_shape: ptr // the host's drawable (create_debug_shape's), or null generation: int flags: int name_id: int // the name's id in the world's name cache, or NULL_NAME @@ -1309,6 +1335,13 @@ struct World { recording: ptr // *Recording while the world records (aephysics.recording), else null names: NameCache // the bodies' and shapes' names, each once max_capacity: Capacity // the most it has held at a step's start (b3World_GetMaxCapacity) + create_debug_shape: fn(*DebugShape, ptr) -> ptr // the host's, kept through a reset + destroy_debug_shape: fn(ptr, ptr) + debug_shape_context: ptr + debug_body_set: BitSet // world_draw's: the bodies, joints, contacts and islands drawn so far + debug_joint_set: BitSet + debug_contact_set: BitSet + debug_island_set: BitSet manifold_counts: int[8] // the last collide pass's contacts by manifold count, every worker's task_count: int // the tasks the last step ran on the scheduler } @@ -1465,6 +1498,9 @@ create_world(def: *WorldDef) -> WorldId { w.friction_callback = def.friction_callback w.restitution_callback = def.restitution_callback w.user_data = def.user_data + w.create_debug_shape = def.create_debug_shape + w.destroy_debug_shape = def.destroy_debug_shape + w.debug_shape_context = def.user_debug_shape_context init_world_contents(w, def) g_max_world_count = math.max_int(g_max_world_count, get_world_count()) return WorldId { index1: index + 1, generation: generation } @@ -1513,6 +1549,10 @@ init_world_contents(w: *World, def: *WorldDef) { w.inv_h = 0.0 w.inv_dt = 0.0 w.names = basics.name_cache_create() + w.debug_body_set = basics.bitset_create(64) + w.debug_joint_set = basics.bitset_create(64) + w.debug_contact_set = basics.bitset_create(64) + w.debug_island_set = basics.bitset_create(64) w.contact_state = basics.bitset_create(64) w.contact_begin_events = basics.buffer_create(sizeof(ContactBeginTouchEvent), 4) w.contact_end_events0 = basics.buffer_create(sizeof(ContactEndTouchEvent), 4) @@ -1609,6 +1649,8 @@ reset_world(id: WorldId, def: *WorldDef) -> bool { // What init_world_contents made, freed. free_world_contents(w: *World) { + // The host's drawables, given back (the reference leaves them to the host). + release_debug_shapes(w) // The shapes' materials and names, the bodies' names, the sensors' // lists and the contacts' caches. i = 0 @@ -1621,6 +1663,10 @@ free_world_contents(w: *World) { i = i + 1 } basics.name_cache_destroy(&w.names) + basics.bitset_destroy(&w.debug_body_set) + basics.bitset_destroy(&w.debug_joint_set) + basics.bitset_destroy(&w.debug_contact_set) + basics.bitset_destroy(&w.debug_island_set) i = 0 while i < w.sensors.count { sensor = sensor_at(w, i) @@ -2077,6 +2123,7 @@ destroy_body(id: BodyId) { s = world_shape_at(w, shape_id) if s.geometry.kind == KIND_COMPOUND { w.compound_shape_count = w.compound_shape_count - 1 } if s.geometry.kind == KIND_HULL { remove_hull_from_database(w, s.geometry.hull) } + release_debug_shape(w, s) if s.sensor_index != NULL_INDEX { destroy_sensor(w, s) } if s.proxy_key != NULL_INDEX { broad_phase.broad_phase_destroy_proxy(&w.broad_phase, s.proxy_key) @@ -2213,6 +2260,7 @@ create_shape(body_id: BodyId, def: *ShapeDef, geometry: Shape) -> ShapeId { set_shape_fat_aabb(w, shape_id, math.aabb_round_out(s.aabb)) s.generation = s.generation + 1 s.sensor_index = NULL_INDEX + s.user_shape = null s.name_id = basics.name_cache_add(&w.names, def.name) if kind == KIND_COMPOUND { w.compound_shape_count = w.compound_shape_count + 1 @@ -3381,6 +3429,7 @@ destroy_shape(id: ShapeId, update_body_mass: bool) { body.shape_count = body.shape_count - 1 if s.geometry.kind == KIND_COMPOUND { w.compound_shape_count = w.compound_shape_count - 1 } if s.geometry.kind == KIND_HULL { remove_hull_from_database(w, s.geometry.hull) } + release_debug_shape(w, s) if s.sensor_index != NULL_INDEX { destroy_sensor(w, s) } if s.proxy_key != NULL_INDEX { broad_phase.broad_phase_destroy_proxy(&w.broad_phase, s.proxy_key) @@ -3541,6 +3590,7 @@ change_shape_geometry(id: ShapeId, geometry: Shape) { if s.geometry.kind == KIND_COMPOUND || s.geometry.kind == KIND_HEIGHT_FIELD { return } // A hull the shape held goes back to the database. if s.geometry.kind == KIND_HULL { remove_hull_from_database(w, s.geometry.hull) } + release_debug_shape(w, s) density = s.geometry.density s.geometry = geometry s.geometry.density = density @@ -4488,6 +4538,34 @@ contact_is_valid(id: ContactId) -> bool { return c.contact_id == id.index1 - 1 && c.generation == id.generation } +// The host's drawable for the shape given back, if it has one: the shape +// goes, or its geometry changed and the next draw makes another. +release_debug_shape(w: *World, s: *WorldShape) { + if s.user_shape == null { return } + destroy = w.destroy_debug_shape + destroy(s.user_shape, w.debug_shape_context) + s.user_shape = null +} + +// Every drawable the world's shapes hold given back to the host. +release_debug_shapes(w: *World) { + i = 0 + while i < w.shapes.count { + s = world_shape_at(w, i) + if s.id != NULL_INDEX { release_debug_shape(w, s) } + i = i + 1 + } +} + +// The host's debug shape callbacks changed: what the old ones made is +// given back to them first, and the next draw makes new ones. +set_debug_shape_callbacks(w: *World, create: fn(*DebugShape, ptr) -> ptr, destroy: fn(ptr, ptr), context: ptr) { + release_debug_shapes(w) + w.create_debug_shape = create + w.destroy_debug_shape = destroy + w.debug_shape_context = context +} + // One contact's shapes and manifolds by its id (b3Contact_GetData): the // manifolds are the contact's own, read until the next step; an id that is // not a contact's gives none. diff --git a/aephysics/joint_solver/module.ae b/aephysics/joint_solver/module.ae index ade8f7b..1dfdc9c 100644 --- a/aephysics/joint_solver/module.ae +++ b/aephysics/joint_solver/module.ae @@ -21,6 +21,7 @@ import std.string import aephysics.math import aephysics.basics import aephysics.dynamics +import aephysics.debug_draw import aephysics.contact_solver exports ( @@ -82,7 +83,8 @@ exports ( wheel_joint_enable_steering_limit, wheel_joint_is_steering_limit_enabled, wheel_joint_get_lower_steering_limit, wheel_joint_get_upper_steering_limit, wheel_joint_set_steering_limits, wheel_joint_set_target_steering_angle, wheel_joint_get_target_steering_angle, wheel_joint_get_spin_speed, wheel_joint_get_spin_torque, wheel_joint_get_steering_angle, - wheel_joint_get_steering_torque + wheel_joint_get_steering_torque, + draw_joint ) extern sqrt(x: float) -> float @@ -2530,3 +2532,225 @@ joint_get_constraint_torque(id: JointId) -> Vec3 { w = dynamics.get_world(id.world0) return get_joint_constraint_torque(w, dynamics.get_joint_full_id(w, id)) } + +// --- drawing (the reference's b3DrawJoint and each kind's) --------------------------------------- + +// A joint drawn through the host's callbacks: its frames and anchors, its +// limits and what drives it; skipped when either body is disabled. +draw_joint(draw: *DebugDraw, w: *World, j: *Joint) { + body_a = dynamics.body_at(w, j.edge_a.body_id) + body_b = dynamics.body_at(w, j.edge_b.body_id) + if body_a.set_index == dynamics.DISABLED_SET || body_b.set_index == dynamics.DISABLED_SET { return } + sim = dynamics.get_joint_sim(w, j) + transform_a = dynamics.get_body_transform(w, j.edge_a.body_id) + transform_b = dynamics.get_body_transform(w, j.edge_b.body_id) + p_a = math.transform_point(transform_a, sim.local_frame_a.p) + p_b = math.transform_point(transform_b, sim.local_frame_b.p) + segment = draw.draw_segment + point = draw.draw_point + scale = math.max_float(0.0001, draw.joint_scale * j.draw_scale) + if j.kind == dynamics.JOINT_PARALLEL { draw_parallel_joint(draw, sim, transform_a, transform_b, scale) } + else if j.kind == dynamics.JOINT_DISTANCE { draw_distance_joint(draw, sim, transform_a, transform_b) } + else if j.kind == dynamics.JOINT_FILTER { segment(p_a, p_b, debug_draw.COLOR_GOLD, draw.context) } + else if j.kind == dynamics.JOINT_MOTOR { + segment(p_a, p_b, debug_draw.COLOR_PLUM, draw.context) + point(p_a, 8.0, debug_draw.COLOR_YELLOW_GREEN, draw.context) + point(p_b, 8.0, debug_draw.COLOR_PLUM, draw.context) + } + else if j.kind == dynamics.JOINT_PRISMATIC { draw_prismatic_joint(draw, sim, transform_a, transform_b, scale) } + else if j.kind == dynamics.JOINT_REVOLUTE { draw_revolute_joint(draw, sim, transform_a, transform_b, scale) } + else if j.kind == dynamics.JOINT_SPHERICAL { draw_spherical_joint(draw, sim, transform_a, transform_b, scale) } + else if j.kind == dynamics.JOINT_WELD { draw_weld_joint(draw, sim, transform_a, transform_b, scale) } + else if j.kind == dynamics.JOINT_WHEEL { draw_wheel_joint(draw, sim, transform_a, transform_b, scale) } + else { + segment(transform_a.p, p_a, debug_draw.COLOR_DARK_SEA_GREEN, draw.context) + segment(p_a, p_b, debug_draw.COLOR_DARK_SEA_GREEN, draw.context) + segment(transform_b.p, p_b, debug_draw.COLOR_DARK_SEA_GREEN, draw.context) + } + if draw.draw_graph_colors && j.color_index != NULL_INDEX { + point(math.lerp(p_a, p_b, 0.5), 5.0, debug_draw.graph_color(j.color_index), draw.context) + } + if draw.draw_joint_extras { + force = get_joint_constraint_force(w, j) + torque = get_joint_constraint_torque(w, j) + middle = math.lerp(p_a, p_b, 0.5) + segment(middle, math.add(middle, math.mul_sv(0.001, force)), debug_draw.COLOR_AZURE, draw.context) + label = draw.draw_string + label(middle, "f = ${debug_draw.fixed(math.length(force), 3)}, t = ${debug_draw.fixed(math.length(torque), 3)}", debug_draw.COLOR_AZURE, draw.context) + } +} + +// A segment from a frame's origin along one of its axes. +draw_axis(draw: *DebugDraw, frame: Transform, axis: Vec3, length: float, color: int) { + segment = draw.draw_segment + segment(frame.p, math.add(frame.p, math.mul_sv(length, math.rotate_vector(frame.q, axis))), color, draw.context) +} + +draw_frame_axes(draw: *DebugDraw, frame: Transform, length: float) { + draw_axis(draw, frame, math.vec3_axis_x(), length, debug_draw.COLOR_RED) + draw_axis(draw, frame, math.vec3_axis_y(), length, debug_draw.COLOR_GREEN) + draw_axis(draw, frame, math.vec3_axis_z(), length, debug_draw.COLOR_BLUE) +} + +draw_parallel_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform, scale: float) { + length = 0.1 * scale + draw_axis(draw, math.mul_transforms(transform_a, sim.local_frame_a), math.vec3_axis_z(), length, debug_draw.COLOR_GREEN) + draw_axis(draw, math.mul_transforms(transform_b, sim.local_frame_b), math.vec3_axis_z(), length, debug_draw.COLOR_BLUE) +} + +draw_distance_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform) { + joint = dynamics.distance_joint_of(sim) + segment = draw.draw_segment + point = draw.draw_point + p_a = math.transform_point(transform_a, sim.local_frame_a.p) + p_b = math.transform_point(transform_b, sim.local_frame_b.p) + axis = math.normalize(math.sub(p_b, p_a)) + if joint.min_length < joint.max_length && joint.enable_limit { + p_min = math.add(p_a, math.mul_sv(joint.min_length, axis)) + p_max = math.add(p_a, math.mul_sv(joint.max_length, axis)) + if joint.min_length > math.LINEAR_SLOP { point(p_min, 6.0, debug_draw.COLOR_LIGHT_GREEN, draw.context) } + if joint.max_length < math.HUGE { point(p_max, 6.0, debug_draw.COLOR_RED, draw.context) } + if joint.min_length > math.LINEAR_SLOP && joint.max_length < math.HUGE { segment(p_min, p_max, debug_draw.COLOR_GRAY, draw.context) } + } + segment(p_a, p_b, debug_draw.COLOR_WHITE, draw.context) + point(p_a, 4.0, debug_draw.COLOR_WHITE, draw.context) + point(p_b, 4.0, debug_draw.COLOR_WHITE, draw.context) + if joint.hertz > 0.0 && joint.enable_spring { + point(math.add(p_a, math.mul_sv(joint.length, axis)), 4.0, debug_draw.COLOR_BLUE, draw.context) + } +} + +draw_prismatic_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform, scale: float) { + frame_a = math.mul_transforms(transform_a, sim.local_frame_a) + frame_b = math.mul_transforms(transform_b, sim.local_frame_b) + r = math.make_matrix_from_quat(frame_a.q) + segment = draw.draw_segment + point = draw.draw_point + s = 0.2 * scale + segment(frame_a.p, math.add(frame_a.p, math.mul_sv(s, r.cy)), debug_draw.COLOR_GREEN, draw.context) + segment(frame_a.p, math.add(frame_a.p, math.mul_sv(s, r.cz)), debug_draw.COLOR_BLUE, draw.context) + joint = dynamics.prismatic_joint_of(sim) + if joint.enable_limit { + p1 = math.add(frame_a.p, math.mul_sv(joint.lower_translation, r.cx)) + p2 = math.add(frame_a.p, math.mul_sv(joint.upper_translation, r.cx)) + segment(p1, p2, debug_draw.COLOR_ORANGE, draw.context) + point(p1, 10.0, debug_draw.COLOR_GREEN, draw.context) + point(p2, 10.0, debug_draw.COLOR_RED, draw.context) + } else { + segment(math.add(frame_a.p, math.mul_sv(0.0 - 0.5 * scale, r.cx)), math.add(frame_a.p, math.mul_sv(0.5 * scale, r.cx)), debug_draw.COLOR_ORANGE, draw.context) + } + point(frame_b.p, 8.0, debug_draw.COLOR_VIOLET, draw.context) +} + +// A wedge of the twist range about frame A's z, and the twist now in it. +draw_twist(draw: *DebugDraw, frame_a: Transform, frame_b: Transform, lower: float, upper: float, radius: float) { + segment = draw.draw_segment + quat_b = frame_b.q + // This keeps the twist angle in [-pi, pi]. + if math.dot_quat(frame_a.q, quat_b) < 0.0 { quat_b = math.negate_quat(quat_b) } + relative = math.inv_mul_quat(frame_a.q, quat_b) + slices = 16 + index = 0 + while index < slices { + t1 = (index as float) / (slices as float) + t2 = ((index + 1) as float) / (slices as float) + alpha1 = (1.0 - t1) * lower + t1 * upper + alpha2 = (1.0 - t2) * lower + t2 * upper + cs1 = math.compute_cos_sin(alpha1) + cs2 = math.compute_cos_sin(alpha2) + vertex1 = math.transform_point(frame_a, math.vec3(radius * cs1.cosine, radius * cs1.sine, 0.0)) + vertex2 = math.transform_point(frame_a, math.vec3(radius * cs2.cosine, radius * cs2.sine, 0.0)) + if index == 0 { segment(frame_a.p, vertex1, debug_draw.COLOR_CYAN, draw.context) } + if index == slices - 1 { segment(vertex2, frame_a.p, debug_draw.COLOR_CYAN, draw.context) } + segment(vertex1, vertex2, debug_draw.COLOR_CYAN, draw.context) + index = index + 1 + } + twist = math.compute_cos_sin(math.get_twist_angle(relative)) + segment(frame_a.p, math.transform_point(frame_a, math.vec3(radius * twist.cosine, radius * twist.sine, 0.0)), debug_draw.COLOR_YELLOW, draw.context) +} + +draw_revolute_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform, scale: float) { + frame_a = math.mul_transforms(transform_a, sim.local_frame_a) + draw_frame_axes(draw, frame_a, 0.1 * scale) + frame_b = math.mul_transforms(transform_b, sim.local_frame_b) + joint = dynamics.revolute_joint_of(sim) + if joint.enable_limit { draw_twist(draw, frame_a, frame_b, joint.lower_angle, joint.upper_angle, 0.2 * scale) } +} + +draw_spherical_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform, scale: float) { + frame_a = math.mul_transforms(transform_a, sim.local_frame_a) + draw_frame_axes(draw, frame_a, 0.1 * scale) + frame_b = math.mul_transforms(transform_b, sim.local_frame_b) + draw_axis(draw, frame_b, math.vec3_axis_z(), 0.2 * scale, debug_draw.COLOR_ORANGE) + joint = dynamics.spherical_joint_of(sim) + if joint.enable_twist_limit { draw_twist(draw, frame_a, frame_b, joint.lower_twist_angle, joint.upper_twist_angle, 0.1 * scale) } + if joint.enable_cone_limit { + segment = draw.draw_segment + radius = 0.1 * scale + cone = math.compute_cos_sin(joint.cone_angle) + cone_radius = radius * cone.sine + cone_height = radius * cone.cosine + slices = 16 + index = 0 + while index < slices { + phi1 = math.compute_cos_sin(2.0 * (index as float) / (slices as float) * math.PI) + phi2 = math.compute_cos_sin(2.0 * ((index + 1) as float) / (slices as float) * math.PI) + vertex1 = math.transform_point(frame_a, math.vec3(cone_radius * phi1.cosine, cone_radius * phi1.sine, cone_height)) + vertex2 = math.transform_point(frame_a, math.vec3(cone_radius * phi2.cosine, cone_radius * phi2.sine, cone_height)) + segment(frame_a.p, vertex1, debug_draw.COLOR_CYAN, draw.context) + segment(vertex1, vertex2, debug_draw.COLOR_CYAN, draw.context) + index = index + 1 + } + } +} + +draw_weld_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform, scale: float) { + box = draw.draw_box + extents = math.vec3(0.1 * scale, 0.05 * scale, 0.025 * scale) + box(extents, math.mul_transforms(transform_a, sim.local_frame_a), debug_draw.COLOR_DARK_ORANGE, draw.context) + box(extents, math.mul_transforms(transform_b, sim.local_frame_b), debug_draw.COLOR_DARK_CYAN, draw.context) +} + +draw_wheel_joint(draw: *DebugDraw, sim: *JointSim, transform_a: Transform, transform_b: Transform, scale: float) { + joint = dynamics.wheel_joint_of(sim) + frame_a = math.mul_transforms(transform_a, sim.local_frame_a) + frame_b = math.mul_transforms(transform_b, sim.local_frame_b) + matrix_a = math.make_matrix_from_quat(frame_a.q) + matrix_b = math.make_matrix_from_quat(frame_b.q) + segment = draw.draw_segment + point = draw.draw_point + segment(frame_a.p, frame_b.p, debug_draw.COLOR_BLUE, draw.context) + if joint.enable_suspension_limit { + lower = math.add(frame_a.p, math.mul_sv(joint.lower_suspension_limit, matrix_a.cx)) + upper = math.add(frame_a.p, math.mul_sv(joint.upper_suspension_limit, matrix_a.cx)) + perp = matrix_a.cy + segment(lower, upper, debug_draw.COLOR_GRAY, draw.context) + segment(math.add(lower, math.mul_sv(0.0 - 0.1 * scale, perp)), math.add(lower, math.mul_sv(0.1 * scale, perp)), debug_draw.COLOR_GREEN, draw.context) + segment(math.add(upper, math.mul_sv(0.0 - 0.1 * scale, perp)), math.add(upper, math.mul_sv(0.1 * scale, perp)), debug_draw.COLOR_RED, draw.context) + } else { + segment(math.add(frame_a.p, math.mul_sv(0.0 - scale, matrix_a.cx)), math.add(frame_a.p, math.mul_sv(scale, matrix_a.cx)), debug_draw.COLOR_GRAY, draw.context) + } + if joint.enable_steering && joint.enable_steering_limit { + frame = Transform { p: frame_b.p, q: frame_a.q } + radius = 0.5 * scale + slices = 16 + lower_limit = joint.lower_steering_limit + upper_limit = joint.upper_steering_limit + cs = math.compute_cos_sin(lower_limit) + vertex1 = math.transform_point(frame, math.vec3(0.0, 0.0 - radius * cs.sine, radius * cs.cosine)) + index = 0 + while index < slices { + t2 = ((index as float) + 1.0) / (slices as float) + cs2 = math.compute_cos_sin(math.lerp_float(lower_limit, upper_limit, t2)) + vertex2 = math.transform_point(frame, math.vec3(0.0, 0.0 - radius * cs2.sine, radius * cs2.cosine)) + if index == 0 { segment(frame.p, vertex1, debug_draw.COLOR_CYAN, draw.context) } + if index == slices - 1 { segment(vertex2, frame.p, debug_draw.COLOR_CYAN, draw.context) } + segment(vertex1, vertex2, debug_draw.COLOR_CYAN, draw.context) + vertex1 = vertex2 + index = index + 1 + } + } + segment(math.add(frame_b.p, math.mul_sv(0.0 - 0.5 * scale, matrix_b.cz)), math.add(frame_b.p, math.mul_sv(0.5 * scale, matrix_b.cz)), debug_draw.COLOR_MAGENTA, draw.context) + point(frame_a.p, 5.0, debug_draw.COLOR_GRAY, draw.context) + point(frame_b.p, 5.0, debug_draw.COLOR_DIM_GRAY, draw.context) +} diff --git a/aephysics/physics_world/module.ae b/aephysics/physics_world/module.ae index 3021c9d..f31cc72 100644 --- a/aephysics/physics_world/module.ae +++ b/aephysics/physics_world/module.ae @@ -29,9 +29,10 @@ import aephysics.contact_solver import aephysics.joint_solver import aephysics.solver import aephysics.parallel +import aephysics.debug_draw exports ( - get_profile, get_max_capacity, world_byte_count, dump_memory_stats, Version, get_version, is_double_precision, is_avx2_available, get_world_count, get_max_world_count, + get_profile, get_max_capacity, world_byte_count, dump_memory_stats, world_draw, Version, get_version, is_double_precision, is_avx2_available, get_world_count, get_max_world_count, RayResult, BodyCastResult, BodyPlaneResult, BodyTOIResult, ExplosionDef, Counters, step, world_of, is_valid, body_event_count, body_event, contact_begin_event_count, contact_begin_event, contact_end_event_count, contact_end_event, @@ -1289,6 +1290,278 @@ cast_mover(id: WorldId, origin: Vec3, mover: Capsule, translation: Vec3, filter: return fraction } +// --- debug draw (the reference's b3World_Draw) --------------------------------------------------------- + +struct DrawContext { + w: *World + draw: *DebugDraw +} + +// A shape's colour as the reference picks it: its material's own colour +// when it has one, else its body's state as a hue and its energy as a +// material (calm matte for resting masses, glossy for fast bodies, +// metallic for a driven kinematic; diagnostic states saturated). +shape_draw_color(w: *World, s: *WorldShape, body: *Body, sim: *BodySim) -> int { + first = s.material + if s.materials != null { first = (s.materials as SurfaceMaterial[])[0] } + if first.custom_color != 0 { return first.custom_color } + flags = body.flags | sim.flags + rgb = debug_draw.COLOR_LIGHT_SLATE_GRAY + look = debug_draw.DEBUG_MATERIAL_DEAD + if body.kind == dynamics.BODY_DYNAMIC && body.mass == 0.0 { + // A dynamic body without mass. + rgb = debug_draw.COLOR_RED + look = debug_draw.DEBUG_MATERIAL_DEFAULT + } else if body.set_index == dynamics.DISABLED_SET { + rgb = debug_draw.COLOR_SLATE_GRAY + look = debug_draw.DEBUG_MATERIAL_DEFAULT + } else if s.sensor_index != NULL_INDEX { + rgb = debug_draw.COLOR_WHEAT + look = debug_draw.DEBUG_MATERIAL_DEFAULT + } else if (flags & dynamics.HAD_TIME_OF_IMPACT) != 0 { + rgb = debug_draw.COLOR_LIME + look = debug_draw.DEBUG_MATERIAL_DEFAULT + } else if (flags & dynamics.IS_BULLET) != 0 && body.set_index == dynamics.AWAKE_SET { + rgb = debug_draw.COLOR_TURQUOISE + look = debug_draw.DEBUG_MATERIAL_DEFAULT + } else if (flags & dynamics.IS_SPEED_CAPPED) != 0 { + rgb = debug_draw.COLOR_YELLOW + look = debug_draw.DEBUG_MATERIAL_DEFAULT + } else if (flags & dynamics.IS_FAST) != 0 { + rgb = debug_draw.COLOR_ORANGE + look = debug_draw.DEBUG_MATERIAL_GLOSSY + } else if body.kind == dynamics.BODY_STATIC { + rgb = debug_draw.COLOR_DARK_GRAY + look = debug_draw.DEBUG_MATERIAL_MATTE + } else if body.kind == dynamics.BODY_KINEMATIC { + if body.set_index == dynamics.AWAKE_SET { + rgb = debug_draw.COLOR_STEEL_BLUE + look = debug_draw.DEBUG_MATERIAL_METALLIC + } else { + rgb = debug_draw.COLOR_LIGHT_STEEL_BLUE + look = debug_draw.DEBUG_MATERIAL_MATTE + } + } else if body.set_index == dynamics.AWAKE_SET { + rgb = debug_draw.COLOR_TAN + look = debug_draw.DEBUG_MATERIAL_SOFT + } + return debug_draw.make_debug_color(rgb, look) +} + +// The host's drawable for the shape, made the first time the shape is drawn. +debug_shape_of(w: *World, s: *WorldShape) -> ptr { + if s.user_shape != null { return s.user_shape } + g = &s.geometry + shown = DebugShape { shape_id: dynamics.make_shape_id(w, s), kind: g.kind, sphere: null, capsule: null, hull: null, mesh: null, + height_field: null, compound: null } + if g.kind == shape.SHAPE_SPHERE { shown.sphere = &g.sphere } + else if g.kind == shape.SHAPE_CAPSULE { shown.capsule = &g.capsule } + else if g.kind == shape.SHAPE_HULL { shown.hull = g.hull } + else if g.kind == shape.SHAPE_MESH { shown.mesh = &g.mesh } + else if g.kind == shape.SHAPE_HEIGHT_FIELD { shown.height_field = g.height_field } + else if g.kind == shape.SHAPE_COMPOUND { shown.compound = g.compound } + create = w.create_debug_shape + s.user_shape = create(&shown, w.debug_shape_context) + return s.user_shape +} + +draw_query_visitor(proxy_id: int, user_data: long, context: ptr) -> bool { + dc = context as *DrawContext + w = dc.w + draw = dc.draw + s = dynamics.world_shape_at(w, user_data as int) + basics.bitset_set(&w.debug_body_set, s.body_id) + if draw.draw_shapes { + body = dynamics.body_at(w, s.body_id) + sim = dynamics.get_body_sim(w, body) + color = shape_draw_color(w, s, body, sim) + user_shape = debug_shape_of(w, s) + if user_shape != null { + show = draw.draw_shape + show(user_shape, sim.transform, color, draw.context) + } + } + if draw.draw_bounds_option { + bounds = draw.draw_bounds + bounds(math.aabb_widen(dynamics.shape_fat_aabb(w, s.id)), debug_draw.COLOR_GOLD, draw.context) + } + return true +} + +// The world drawn through the host's callbacks (b3World_Draw): the shapes +// whose bounds overlap the drawing bounds (and whose categories the mask +// takes), then for their bodies what the options ask: names, mass, sleep, +// joints, contacts (points by state or graph colour, normals with +// separations, forces, features) and islands. Nothing while the world +// steps. +world_draw(id: WorldId, draw: *DebugDraw, mask_bits: long) { + w = world_of(id) + if w == null || math.is_valid_aabb(draw.drawing_bounds) == false { return } + axis_scale = 0.3 + basics.bitset_set_count_and_clear(&w.debug_body_set, basics.idpool_capacity(&w.body_id_pool)) + basics.bitset_set_count_and_clear(&w.debug_joint_set, basics.idpool_capacity(&w.joint_id_pool)) + basics.bitset_set_count_and_clear(&w.debug_contact_set, basics.idpool_capacity(&w.contact_id_pool)) + basics.bitset_set_count_and_clear(&w.debug_island_set, basics.idpool_capacity(&w.island_id_pool)) + dc = DrawContext { w: w, draw: draw } + i = 0 + while i < BODY_KIND_COUNT { + dynamic_tree.tree_query(broad_phase.broad_phase_tree(&w.broad_phase, i), draw.drawing_bounds, mask_bits, false, draw_query_visitor, (&dc) as ptr) + i = i + 1 + } + body_id = 0 + while body_id < w.bodies.count { + if basics.bitset_get(&w.debug_body_set, body_id) { draw_body_extras(w, draw, body_id, axis_scale) } + body_id = body_id + 1 + } +} + +// What the options ask of one drawn body. +draw_body_extras(w: *World, draw: *DebugDraw, body_id: int, axis_scale: float) { + point = draw.draw_point + label = draw.draw_string + body = dynamics.body_at(w, body_id) + sim = dynamics.get_body_sim(w, body) + at_center = Transform { p: sim.center, q: sim.transform.q } + offset = math.vec3(0.05, 0.05, 0.05) + if draw.draw_body_names && body.name_id != basics.NULL_NAME { + name = basics.name_cache_find(&w.names, body.name_id) + if name != null { label(math.transform_point(at_center, offset), native.name_text(name), debug_draw.COLOR_WHITE, draw.context) } + } + if draw.draw_mass { + show_transform = draw.draw_transform + show_transform(at_center, draw.context) + if body.kind == dynamics.BODY_DYNAMIC { label(math.transform_point(at_center, offset), debug_draw.fixed(body.mass, 2), debug_draw.COLOR_WHITE, draw.context) } + } + if draw.draw_sleep && dynamics.get_body_state(w, body) != null { + color = debug_draw.COLOR_BLACK + if body.sleep_threshold > 0.0 { + index = math.clamp_int((body.sleep_velocity / body.sleep_threshold) as int, 0, 3) + if index == 0 { color = debug_draw.COLOR_BLUE } + else if index == 1 { color = debug_draw.COLOR_SKY_BLUE } + else if index == 2 { color = debug_draw.COLOR_ORANGE } + else { color = debug_draw.COLOR_RED } + } + point(sim.center, 10.0, color, draw.context) + label(math.add(sim.center, math.vec3(0.1, 0.1, 0.1)), " ${debug_draw.fixed(body.sleep_velocity, 3)}", color, draw.context) + } + if draw.draw_joints { + key = body.head_joint_key + while key != NULL_INDEX { + joint_index = key >> 1 + j = dynamics.joint_at(w, joint_index) + if basics.bitset_get(&w.debug_joint_set, joint_index) == false { + joint_solver.draw_joint(draw, w, j) + basics.bitset_set(&w.debug_joint_set, joint_index) + } + key = dynamics.joint_edge_next(w, key) + } + } + if draw.draw_contacts && body.kind == dynamics.BODY_DYNAMIC && body.set_index == dynamics.AWAKE_SET { + key = body.head_contact_key + while key != NULL_INDEX { + contact_index = key >> 1 + c = dynamics.contact_at(w, contact_index) + key = dynamics.contact_edge_next(w, key) + if c.set_index == dynamics.AWAKE_SET && c.color_index != NULL_INDEX && basics.bitset_get(&w.debug_contact_set, contact_index) == false { + draw_contact(w, draw, c, axis_scale) + basics.bitset_set(&w.debug_contact_set, contact_index) + } + } + } + if draw.draw_islands && body.island_id != NULL_INDEX && basics.bitset_get(&w.debug_island_set, body.island_id) == false { + island = dynamics.island_at(w, body.island_id) + if island.set_index != NULL_INDEX { + shapes = 0 + box = AABB { lower: math.vec3(math.HUGE, math.HUGE, math.HUGE), upper: math.vec3(0.0 - math.HUGE, 0.0 - math.HUGE, 0.0 - math.HUGE) } + members = island.bodies.data as int[] + b = 0 + while b < island.bodies.count { + shape_id = dynamics.body_at(w, members[b]).head_shape_id + while shape_id != NULL_INDEX { + box = math.aabb_union(box, math.aabb_widen(dynamics.shape_fat_aabb(w, shape_id))) + shapes = shapes + 1 + shape_id = dynamics.world_shape_at(w, shape_id).next_shape_id + } + b = b + 1 + } + if shapes > 0 { + bounds = draw.draw_bounds + bounds(box, debug_draw.COLOR_ORANGE_RED, draw.context) + } + basics.bitset_set(&w.debug_island_set, body.island_id) + } + } +} + +// A contact's manifolds: each point at an anchor of A or of B, coloured +// by whether it was added, is speculative far or near, or persists (or by +// its graph colour), with its normal and separation, its force, or its +// feature, and the friction's anchor and force. +draw_contact(w: *World, draw: *DebugDraw, c: *Contact, axis_scale: float) { + segment = draw.draw_segment + point = draw.draw_point + label = draw.draw_string + sim_a = dynamics.get_body_sim(w, dynamics.body_at(w, c.edge_a.body_id)) + sim_b = dynamics.get_body_sim(w, dynamics.body_at(w, c.edge_b.body_id)) + // Hack inv_dt for single step debugging, as the reference does. + inv_dt = w.inv_dt + if inv_dt <= 0.0 { inv_dt = 60.0 } + inv_tau = 1.0 / dynamics.SPECULATIVE_DISTANCE + m = 0 + while m < c.manifold_count { + manifold = dynamics.manifold_at(c, m) + normal = math.widen3(manifold.normal) + center = sim_b.center + if draw.draw_anchor_a { center = sim_a.center } + friction_anchor = math.vec3_zero() + total_weight = 0.0 + k = 0 + while k < manifold.point_count { + mp = dynamics.manifold_point_at(manifold, k) + anchor = math.widen3(mp.anchor_b) + if draw.draw_anchor_a { anchor = math.widen3(mp.anchor_a) } + p = math.add(center, anchor) + separation = mp.separation as float + weight = math.clamp_float(2.0 - separation * inv_tau, contact_solver.MIN_FRICTION_WEIGHT, 1.0) + friction_anchor = math.add(friction_anchor, math.mul_sv(weight, anchor)) + total_weight = total_weight + weight + if draw.draw_contact_normals { + segment(p, math.add(p, math.mul_sv(axis_scale, normal)), debug_draw.COLOR_LIGHT_GRAY, draw.context) + label(p, " ${debug_draw.fixed(100.0 * separation, 2)}", debug_draw.COLOR_WHITE, draw.context) + } else if draw.draw_contact_forces { + force = (mp.total_normal_impulse as float) * inv_dt + tip = math.add(p, math.mul_sv(draw.force_scale * force, normal)) + segment(p, tip, debug_draw.COLOR_MAGENTA, draw.context) + label(p, " ${debug_draw.fixed(force, 1)}", debug_draw.COLOR_WHITE, draw.context) + } else if draw.draw_contact_features { + label(p, " ${debug_draw.hex(mp.feature_id)}", debug_draw.COLOR_WHITE, draw.context) + } + if draw.draw_graph_colors { + size = 10.0 + if c.color_index == dynamics.OVERFLOW_INDEX { size = 15.0 } + point(p, size, debug_draw.graph_color(c.color_index), draw.context) + } else if mp.persisted == false { + point(p, 20.0, debug_draw.COLOR_LIME_GREEN, draw.context) + } else if separation > dynamics.SPECULATIVE_DISTANCE { + point(p, 10.0, debug_draw.COLOR_DARK_BLUE, draw.context) + } else if separation > 0.0 { + point(p, 10.0, debug_draw.COLOR_BLUE, draw.context) + } else { + point(p, 10.0, debug_draw.COLOR_LIGHT_BLUE, draw.context) + } + k = k + 1 + } + if draw.draw_contact_forces && total_weight > 0.0 { + p1 = math.add(center, math.mul_sv(1.0 / total_weight, friction_anchor)) + friction_force = math.mul_sv(0.5 * inv_dt, math.widen3(manifold.friction_impulse)) + segment(p1, math.add(p1, math.mul_sv(draw.force_scale, friction_force)), debug_draw.COLOR_YELLOW, draw.context) + point(p1, 5.0, debug_draw.COLOR_YELLOW, draw.context) + label(math.add(p1, math.mul_sv(0.05, normal)), debug_draw.fixed(math.length(friction_force), 2), debug_draw.COLOR_WHITE, draw.context) + } + m = m + 1 + } +} + // --- the body queries ------------------------------------------------------------------------------------------------ no_body_hit() -> BodyCastResult { diff --git a/aephysics/recording/module.ae b/aephysics/recording/module.ae index 4dbabf0..ed08ce4 100644 --- a/aephysics/recording/module.ae +++ b/aephysics/recording/module.ae @@ -51,11 +51,12 @@ import aephysics.dynamics import aephysics.joint_solver import aephysics.physics_world import aephysics.snapshot +import aephysics.debug_draw exports ( Player, PlayerInfo, QueryInfo, QueryHit, QUERY_OVERLAP_AABB, QUERY_OVERLAP_SHAPE, QUERY_CAST_RAY, QUERY_CAST_SHAPE, QUERY_CAST_RAY_CLOSEST, QUERY_CAST_MOVER, QUERY_COLLIDE_MOVER, player_frame_query_count, player_frame_query, - player_frame_query_hit, create_recording, destroy_recording, recording_data, recording_size, start_recording, + player_frame_query_hit, player_draw_frame_queries, player_set_debug_shape_callbacks, create_recording, destroy_recording, recording_data, recording_size, start_recording, stop_recording, save_recording_to_file, load_recording_from_file, create_player, destroy_player, player_step_frame, player_sub_step_frame, player_restart, player_seek_frame, player_world, player_frame, player_frame_count, player_at_end, player_at_pre_step, player_has_diverged, player_diverge_frame, player_failed, player_info, @@ -263,6 +264,7 @@ struct Player { replay_planes: Buffer // PlaneResult: a mover's recorded planes frame_queries: Buffer // FrameQuery: the queries of the frame last played frame_hits: Buffer // QueryHit + frame_proxy_points: Buffer // Vec3: the frame's proxy queries' points pending_key: long // the key of the tag record just read, for the query after it } @@ -320,6 +322,7 @@ create_player(data: ptr, size: int, worker_count: int) -> *Player { p.replay_planes = basics.buffer_create(sizeof(PlaneResult), 0) p.frame_queries = basics.buffer_create(sizeof(FrameQuery), 0) p.frame_hits = basics.buffer_create(sizeof(QueryHit), 0) + p.frame_proxy_points = basics.buffer_create(sizeof(Vec3), 0) p.keyframe_interval = DEFAULT_KEYFRAME_INTERVAL p.keyframe_min_interval = DEFAULT_KEYFRAME_INTERVAL p.keyframe_budget = DEFAULT_KEYFRAME_BUDGET as long @@ -371,6 +374,7 @@ destroy_player(p: *Player) { basics.buffer_destroy(&p.replay_planes) basics.buffer_destroy(&p.frame_queries) basics.buffer_destroy(&p.frame_hits) + basics.buffer_destroy(&p.frame_proxy_points) release_record(p) basics.free_bytes(p.strings, STRING_SLOTS * 8) basics.free_bytes(p.string_bytes, STRING_SLOTS * 4) @@ -986,6 +990,10 @@ struct FrameQuery { hit_start: int hit_count: int key: long + mover: Capsule // a mover query's capsule, relative to the origin + proxy_start: int // a proxy query's points in frame_proxy_points, relative to the origin + proxy_count: int + proxy_radius: float } // A tagged query's caller, from the recording's tag table. @@ -1065,9 +1073,25 @@ free_tags(p: *Player) { clear_frame_queries(p: *Player) { p.frame_queries.count = 0 p.frame_hits.count = 0 + p.frame_proxy_points.count = 0 p.pending_key = 0 as long } +// The proxy the query asked with, kept for drawing it. +stash_proxy(p: *Player, q: *FrameQuery, proxy: ShapeProxy) { + q.proxy_start = p.frame_proxy_points.count + q.proxy_count = proxy.count + q.proxy_radius = proxy.radius + points = proxy.points as Vec3[] + i = 0 + while i < proxy.count { + index = basics.buffer_push(&p.frame_proxy_points) + kept = p.frame_proxy_points.data as Vec3[] + kept[index] = points[i] + i = i + 1 + } +} + read_aabb(p: *Player) -> AABB { lower = read_vec3(p) upper = read_vec3(p) @@ -1264,6 +1288,10 @@ stash_query(p: *Player, kind: int, filter: QueryFilter, aabb: AABB, origin: Vec3 p.pending_key = 0 as long q.hit_start = p.frame_hits.count q.hit_count = hit_count + q.mover = Capsule { center1: math.vec3_zero(), center2: math.vec3_zero(), radius: 0.0 } + q.proxy_start = 0 + q.proxy_count = 0 + q.proxy_radius = 0.0 return q } @@ -1311,7 +1339,8 @@ replay_overlap_shape(p: *Player) { stats = physics_world.overlap_shape(p.world_id, origin, proxy, filter, replay_overlap_visit, (&rq) as ptr) check_replayed(p, &rq, stats, nodes, leaves) box = query_bounds(origin, math.vec3_zero(), proxy.points, proxy.count, proxy.radius) - stash_query(p, QUERY_OVERLAP_SHAPE, filter, box, origin, math.vec3_zero(), 1.0, n) + kept_query = stash_query(p, QUERY_OVERLAP_SHAPE, filter, box, origin, math.vec3_zero(), 1.0, n) + stash_proxy(p, kept_query, proxy) stash_recorded_hits(p, n) } @@ -1343,7 +1372,8 @@ replay_cast_shape(p: *Player) { stats = physics_world.cast_shape(p.world_id, origin, proxy, translation, filter, replay_cast_visit, (&rq) as ptr) check_replayed(p, &rq, stats, nodes, leaves) box = query_bounds(origin, translation, proxy.points, proxy.count, proxy.radius) - stash_query(p, QUERY_CAST_SHAPE, filter, box, origin, translation, 1.0, n) + kept_query = stash_query(p, QUERY_CAST_SHAPE, filter, box, origin, translation, 1.0, n) + stash_proxy(p, kept_query, proxy) stash_recorded_hits(p, n) } @@ -1394,7 +1424,8 @@ replay_cast_mover(p: *Player) { centres[0] = mover.center1 centres[1] = mover.center2 box = query_bounds(origin, translation, p.proxy_points, 2, mover.radius) - stash_query(p, QUERY_CAST_MOVER, filter, box, origin, translation, fraction, 0) + kept_query = stash_query(p, QUERY_CAST_MOVER, filter, box, origin, translation, fraction, 0) + kept_query.mover = mover } replay_collide_mover(p: *Player) { @@ -1430,14 +1461,15 @@ replay_collide_mover(p: *Player) { centres[0] = mover.center1 centres[1] = mover.center2 box = query_bounds(origin, math.vec3_zero(), p.proxy_points, 2, mover.radius) - stash_query(p, QUERY_COLLIDE_MOVER, filter, box, origin, math.vec3_zero(), 1.0, p.replay_planes.count) + kept_query = stash_query(p, QUERY_COLLIDE_MOVER, filter, box, origin, math.vec3_zero(), 1.0, p.replay_planes.count) + kept_query.mover = mover i = 0 while i < groups { h = recorded_hit_at(p, i) k = 0 while k < h.plane_count { kept_plane = replay_plane_at(p, h.plane_start + k) - stash_hit(p, h.shape, kept_plane.point, kept_plane.plane.normal, 0.0) + stash_hit(p, h.shape, math.add(origin, kept_plane.point), kept_plane.plane.normal, 0.0) k = k + 1 } i = i + 1 @@ -1495,6 +1527,148 @@ player_frame_query_hit(p: *Player, query: int, hit: int) -> QueryHit { return hits[q.hit_start + hit] } +// --- drawing ---------------------------------------------------------------------------------------- + +// The host's debug shape callbacks for the player's world, now and through +// every restore (b3RecPlayer_SetDebugShapeCallbacks): what the old ones +// made is given back to them, and the player rewinds to frame 0. +player_set_debug_shape_callbacks(p: *Player, create: fn(*DebugShape, ptr) -> ptr, destroy: fn(ptr, ptr), context: ptr) { + p.def.create_debug_shape = create + p.def.destroy_debug_shape = destroy + p.def.user_debug_shape_context = context + if dynamics.world_is_valid(p.world_id) { dynamics.set_debug_shape_callbacks(dynamics.get_world(p.world_id.index1 - 1), create, destroy, context) } + player_restart(p) +} + +// The colour of a query, or the one reserved for the selected query. +query_color(selected: bool, base: int) -> int { + if selected { return debug_draw.COLOR_PLUM } + return base +} + +// Each reported overlap's shape by its bounds; a shape destroyed since is +// skipped. +draw_hit_bounds(p: *Player, q: *FrameQuery, draw: *DebugDraw, color: int) { + bounds = draw.draw_bounds + hits = p.frame_hits.data as QueryHit[] + i = q.hit_start + while i < q.hit_start + q.hit_count { + if dynamics.shape_is_valid(hits[i].shape) { bounds(dynamics.shape_get_aabb(hits[i].shape), color, draw.context) } + i = i + 1 + } +} + +// A recorded proxy at `base`: a point without radius as a fat point, with +// one as a sphere, two with a radius as a capsule, more as their points. +draw_proxy(p: *Player, draw: *DebugDraw, base: Vec3, q: *FrameQuery, color: int) { + points = p.frame_proxy_points.data as Vec3[] + if q.proxy_count == 1 { + at = math.add(base, points[q.proxy_start]) + if q.proxy_radius > 0.0 { + sphere = draw.draw_sphere + sphere(at, q.proxy_radius, color, 0.5, draw.context) + } else { + dot = draw.draw_point + dot(at, 10.0, color, draw.context) + } + } else if q.proxy_count == 2 && q.proxy_radius > 0.0 { + capsule = draw.draw_capsule + capsule(math.add(base, points[q.proxy_start]), math.add(base, points[q.proxy_start + 1]), q.proxy_radius, color, 0.5, draw.context) + } else if q.proxy_count >= 2 { + dot = draw.draw_point + i = 0 + while i < q.proxy_count { + dot(math.add(base, points[q.proxy_start + i]), 6.0, color, draw.context) + i = i + 1 + } + } +} + +// The queries of the frame last played, drawn (b3RecPlayer_DrawFrameQueries): +// every one when `query` is below zero, else that one; the `selected` one +// in the reserved colour and labelled with its caller. +player_draw_frame_queries(p: *Player, draw: *DebugDraw, query: int, selected: int) { + segment = draw.draw_segment + dot = draw.draw_point + capsule = draw.draw_capsule + hits = p.frame_hits.data as QueryHit[] + qi = 0 + while qi < p.frame_queries.count { + if query < 0 || qi == query { + q = frame_query_at(p, qi) + chosen = qi == selected + if q.kind == QUERY_CAST_RAY || q.kind == QUERY_CAST_RAY_CLOSEST { + segment(q.origin, math.add(q.origin, q.translation), query_color(chosen, debug_draw.COLOR_YELLOW), draw.context) + i = q.hit_start + while i < q.hit_start + q.hit_count { + dot(hits[i].point, 4.0, query_color(chosen, debug_draw.COLOR_YELLOW), draw.context) + segment(hits[i].point, math.add(hits[i].point, math.mul_sv(0.2, hits[i].normal)), query_color(chosen, debug_draw.COLOR_YELLOW_GREEN), draw.context) + i = i + 1 + } + } else if q.kind == QUERY_CAST_SHAPE { + segment(q.origin, math.add(q.origin, q.translation), query_color(chosen, debug_draw.COLOR_SKY_BLUE), draw.context) + draw_proxy(p, draw, q.origin, q, query_color(chosen, debug_draw.COLOR_LIGHT_GREEN)) + i = q.hit_start + while i < q.hit_start + q.hit_count { + dot(hits[i].point, 4.0, query_color(chosen, debug_draw.COLOR_SKY_BLUE), draw.context) + segment(hits[i].point, math.add(hits[i].point, math.mul_sv(0.2, hits[i].normal)), query_color(chosen, debug_draw.COLOR_LIGHT_SKY_BLUE), draw.context) + draw_proxy(p, draw, math.add(q.origin, math.mul_sv(hits[i].fraction, q.translation)), q, query_color(chosen, debug_draw.COLOR_SKY_BLUE)) + i = i + 1 + } + } else if q.kind == QUERY_CAST_MOVER { + c1 = math.add(q.origin, q.mover.center1) + c2 = math.add(q.origin, q.mover.center2) + capsule(c1, c2, q.mover.radius, query_color(chosen, debug_draw.COLOR_LIGHT_SKY_BLUE), 0.6, draw.context) + if q.fraction > 0.01 { + moved = math.mul_sv(q.fraction, q.translation) + capsule(math.add(c1, moved), math.add(c2, moved), q.mover.radius, debug_draw.COLOR_LIGHT_SKY_BLUE, 0.3, draw.context) + } + } else if q.kind == QUERY_COLLIDE_MOVER { + capsule(math.add(q.origin, q.mover.center1), math.add(q.origin, q.mover.center2), q.mover.radius, query_color(chosen, debug_draw.COLOR_TAN), 0.6, draw.context) + i = q.hit_start + while i < q.hit_start + q.hit_count { + segment(hits[i].point, math.add(hits[i].point, math.mul_sv(0.2, hits[i].normal)), query_color(chosen, debug_draw.COLOR_ORANGE), draw.context) + i = i + 1 + } + } else if q.kind == QUERY_OVERLAP_AABB { + bounds = draw.draw_bounds + bounds(q.aabb, query_color(chosen, debug_draw.COLOR_LIME_GREEN), draw.context) + draw_hit_bounds(p, q, draw, query_color(chosen, debug_draw.COLOR_MAGENTA)) + } else if q.kind == QUERY_OVERLAP_SHAPE { + draw_proxy(p, draw, q.origin, q, query_color(chosen, debug_draw.COLOR_LIME_GREEN)) + draw_hit_bounds(p, q, draw, query_color(chosen, debug_draw.COLOR_MAGENTA)) + } + if chosen && q.key != (0 as long) { draw_query_label(p, draw, q) } + } + qi = qi + 1 + } +} + +// The selected query's caller at its origin: its name and id, its name, +// or its id; an overlap box's at its centre, a mover's past its top. +draw_query_label(p: *Player, draw: *DebugDraw, q: *FrameQuery) { + name = "" + id = 0 as long + tag_index = basics.map_get(&p.tag_index, q.key, NULL_INDEX) + if tag_index != NULL_INDEX { + tag = player_tag_at(p, tag_index) + id = tag.id + if tag.name != null { name = native.name_text(tag.name) } + } + text = "#${id}" + if string.length(name) > 0 && id != (0 as long) { text = "${name} (${id})" } + else if string.length(name) > 0 { text = name } + at = q.origin + if q.kind == QUERY_OVERLAP_AABB { at = math.aabb_center(q.aabb) } + else if q.kind == QUERY_CAST_MOVER || q.kind == QUERY_COLLIDE_MOVER { + c1 = math.add(q.origin, q.mover.center1) + c2 = math.add(q.origin, q.mover.center2) + at = math.add(c2, math.mul_sv(1.25 * q.mover.radius, math.normalize(math.sub(c2, c1)))) + } + label = draw.draw_string + label(at, text, debug_draw.COLOR_WHITE, draw.context) +} + // --- the records ------------------------------------------------------------------------------------ // The record's call made in the player's world, or its check done; false diff --git a/aephysics/snapshot/module.ae b/aephysics/snapshot/module.ae index 6f129b0..377c383 100644 --- a/aephysics/snapshot/module.ae +++ b/aephysics/snapshot/module.ae @@ -458,6 +458,7 @@ save_world(world_id: WorldId) -> (ptr, int) { shape_copy.geometry.compound = null shape_copy.materials = null shape_copy.user_data = null + shape_copy.user_shape = null w_int(&wr, indices[i]) if s.id != NULL_INDEX && s.materials != null { w_int(&wr, s.material_count) @@ -611,13 +612,52 @@ restore_world(world_id: WorldId, def: *WorldDef, data: ptr, size: int) -> bool { if data == null || size < 16 { return false } rd = SnapReader { data: data, size: size, at: 0, ok: true } if r_int(&rd) != SNAPSHOT_MAGIC || r_int(&rd) != SNAPSHOT_VERSION || r_long(&rd) != snapshot_layout() { return false } - if dynamics.reset_world(world_id, def) == false { return false } + if dynamics.world_is_valid(world_id) == false { return false } w = dynamics.get_world(world_id.index1 - 1) - if fill_world(w, &rd) == false || rd.at != rd.size || world_valid(w) == false { + if w.locked || w.recording != null { return false } + // The host's drawables are kept for the shapes still in their slot + // after the restore (the same generation: the same shape), as the + // reference keeps them, so a seek does not remake every one; the rest + // are given back. + kept_count = w.shapes.count + kept_block = basics.alloc(math.max_int(kept_count, 1) * 8) + kept = kept_block as ptr[] + generations_block = basics.alloc(math.max_int(kept_count, 1) * 4) + generations = generations_block as int[] + i = 0 + while i < kept_count { + old = dynamics.world_shape_at(w, i) + kept[i] = null + generations[i] = old.generation + if old.id == i { + kept[i] = old.user_shape + old.user_shape = null + } + i = i + 1 + } + restored = dynamics.reset_world(world_id, def) + if restored && (fill_world(w, &rd) == false || rd.at != rd.size || world_valid(w) == false) { dynamics.reset_world(world_id, def) - return false + restored = false } - return true + i = 0 + while i < kept_count { + if restored && i < w.shapes.count && kept[i] != null { + now = dynamics.world_shape_at(w, i) + if now.id == i && now.generation == generations[i] { + now.user_shape = kept[i] + kept[i] = null + } + } + if kept[i] != null { + destroy = w.destroy_debug_shape + destroy(kept[i], w.debug_shape_context) + } + i = i + 1 + } + basics.free_bytes(kept_block, math.max_int(kept_count, 1) * 8) + basics.free_bytes(generations_block, math.max_int(kept_count, 1) * 4) + return restored } // The world's own invariants over everything loaded. @@ -767,6 +807,7 @@ fill_world(w: *World, rd: *SnapReader) -> bool { s.geometry.compound = null s.materials = null s.user_data = null + s.user_shape = null if s.id != NULL_INDEX && name_known(&w.names, s.name_id) == false { rd.ok = false } geometry_index = r_int(rd) material_count = r_count(rd, sizeof(SurfaceMaterial)) diff --git a/design.md b/design.md index ce6ecbf..340c19b 100644 --- a/design.md +++ b/design.md @@ -381,7 +381,11 @@ started until its tests pass. world under the reference's 32-bit hash, the bodies and shapes holding ids, so their records copy whole and a snapshot carries each name once; where the reference gives two names on one hash the first name, the - second here takes the next free id. Two + second here takes the next free id. Debug draw came after (#163), the + reference's b3World_Draw with its colours, its joints' drawings and the + host's debug shapes, whose lives here also end with a geometry change + or the world's end, where the reference keeps a stale one or leaves them + to the host. Two functions no test called hid type errors (`body_apply_force`, `body_get_local_point_velocity`) until the replay dispatcher called every recorded one: `ae build` checks only what a program reaches diff --git a/tests/test_debug_draw.ae b/tests/test_debug_draw.ae new file mode 100644 index 0000000..26f4be8 --- /dev/null +++ b/tests/test_debug_draw.ae @@ -0,0 +1,467 @@ +// Debug draw (#163, the reference's b3World_Draw and its debug shapes): a +// world of every kind of thing drawn through counting callbacks. Every +// shape is drawn once, with the colour the reference picks for its state +// (or its material's own), its host drawable made the first time and kept +// after; the options draw bounds, names, mass, sleep, joints (each once, +// a weld as two boxes), contacts (a point per manifold point) and +// islands; the drawing bounds and the mask cull. A host drawable goes +// back to the host when its shape goes, when its geometry changes and when +// the world goes; an in-place restore keeps the ones of the shapes still +// there and gives back the rest. A player's world draws with the host's +// callbacks and its frame's queries draw, the selected one labelled with +// its caller. The labels' number formats as the reference's. + +import std.string +import aephysics.math +import aephysics.basics +import aephysics.hull +import aephysics.material +import aephysics.shape +import aephysics.manifold +import aephysics.dynamics +import aephysics.joint_solver +import aephysics.physics_world +import aephysics.snapshot +import aephysics.recording +import aephysics.debug_draw + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern exit(code: int) + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("debug draw: FAIL ${name}") + failures = failures + 1 + } +} + +// --- the host: drawables that know their shape, and counting callbacks ----------------------------- + +struct Drawable { + index1: int // the shape's + kind: int +} + +var g_made = 0 +var g_given_back = 0 +var g_live = 0 +var g_colors: ptr = null // int[256] by shape index1: the colour last drawn + +make_drawable(debug_shape: *DebugShape, context: ptr) -> ptr { + d = calloc(1, sizeof(Drawable)) as *Drawable + d.index1 = debug_shape.shape_id.index1 + d.kind = debug_shape.kind + // The pointer for its kind is the one set. + right = false + if d.kind == shape.SHAPE_SPHERE { right = debug_shape.sphere != null } + else if d.kind == shape.SHAPE_CAPSULE { right = debug_shape.capsule != null } + else if d.kind == shape.SHAPE_HULL { right = debug_shape.hull != null } + else if d.kind == shape.SHAPE_MESH { right = debug_shape.mesh != null } + else if d.kind == shape.SHAPE_HEIGHT_FIELD { right = debug_shape.height_field != null } + else if d.kind == shape.SHAPE_COMPOUND { right = debug_shape.compound != null } + if right == false { d.kind = 0 - 1 } + g_made = g_made + 1 + g_live = g_live + 1 + return d as ptr +} + +give_back_drawable(user_shape: ptr, context: ptr) { + g_given_back = g_given_back + 1 + g_live = g_live - 1 + free(user_shape) +} + +var g_shapes = 0 +var g_bad_kinds = 0 +var g_segments = 0 +var g_points = 0 +var g_transforms = 0 +var g_spheres = 0 +var g_capsules = 0 +var g_bounds = 0 +var g_boxes = 0 +var g_strings = 0 +var g_last_text = "" + +count_shape(user_shape: ptr, transform: Transform, color: int, context: ptr) { + g_shapes = g_shapes + 1 + d = user_shape as *Drawable + if d.kind < 0 { g_bad_kinds = g_bad_kinds + 1 } + if d.index1 >= 0 && d.index1 < 256 { + colors = g_colors as int[] + colors[d.index1] = color + } +} +count_segment(p1: Vec3, p2: Vec3, color: int, context: ptr) { g_segments = g_segments + 1 } +count_transform(transform: Transform, context: ptr) { g_transforms = g_transforms + 1 } +count_point(p: Vec3, size: float, color: int, context: ptr) { g_points = g_points + 1 } +count_sphere(p: Vec3, radius: float, color: int, alpha: float, context: ptr) { g_spheres = g_spheres + 1 } +count_capsule(p1: Vec3, p2: Vec3, radius: float, color: int, alpha: float, context: ptr) { g_capsules = g_capsules + 1 } +count_bounds(box: AABB, color: int, context: ptr) { g_bounds = g_bounds + 1 } +count_box(extents: Vec3, transform: Transform, color: int, context: ptr) { g_boxes = g_boxes + 1 } +count_string(p: Vec3, text: string, color: int, context: ptr) { + g_strings = g_strings + 1 + g_last_text = string.copy(text) +} + +reset_counts() { + g_shapes = 0 + g_bad_kinds = 0 + g_segments = 0 + g_points = 0 + g_transforms = 0 + g_spheres = 0 + g_capsules = 0 + g_bounds = 0 + g_boxes = 0 + g_strings = 0 +} + +counting_draw() -> DebugDraw { + draw = debug_draw.default_debug_draw() + draw.draw_shape = count_shape + draw.draw_segment = count_segment + draw.draw_transform = count_transform + draw.draw_point = count_point + draw.draw_sphere = count_sphere + draw.draw_capsule = count_capsule + draw.draw_bounds = count_bounds + draw.draw_box = count_box + draw.draw_string = count_string + return draw +} + +color_of(id: ShapeId) -> int { + colors = g_colors as int[] + return colors[id.index1] +} + +// --- the scene -------------------------------------------------------------------------------------- + +body_at(world_id: WorldId, kind: int, p: Vec3) -> BodyId { + bdef = dynamics.default_body_def() + bdef.kind = kind + bdef.position = p + return dynamics.create_body(world_id, &bdef) +} + +joint_frames(base: *JointDef, a: BodyId, b: BodyId) { + base.body_id_a = a + base.body_id_b = b + base.local_frame_a = Transform { p: math.vec3(0.0, 0.0, 0.6), q: math.quat_identity() } + base.local_frame_b = Transform { p: math.vec3(0.0, 0.0, 0.0 - 0.6), q: math.quat_identity() } +} + +var g_box: ptr = null + +pair(world_id: WorldId, x: float) -> (BodyId, BodyId) { + sdef = dynamics.default_shape_def() + a = body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3(x, 6.0, 0.0 - 8.0)) + dynamics.create_hull_shape(a, &sdef, g_box as *HullData) + b = body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3(x, 6.0, 0.0 - 6.8)) + dynamics.create_hull_shape(b, &sdef, g_box as *HullData) + return a, b +} + +main() { + g_colors = calloc(256, 4) + g_box = hull.make_box_hull(0.4, 0.4, 0.4) as ptr + wdef = dynamics.default_world_def() + wdef.create_debug_shape = make_drawable + wdef.destroy_debug_shape = give_back_drawable + world_id = dynamics.create_world(&wdef) + w = dynamics.get_world(world_id.index1 - 1) + sdef = dynamics.default_shape_def() + + ground = body_at(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0)) + floor_hull = hull.make_box_hull(20.0, 1.0, 20.0) + floor = dynamics.create_hull_shape(ground, &sdef, floor_hull) + hull.destroy_hull(floor_hull) + sensor_def = dynamics.default_shape_def() + sensor_def.is_sensor = true + sensor = dynamics.create_sphere_shape(body_at(world_id, dynamics.BODY_STATIC, math.vec3(10.0, 1.0, 10.0)), &sensor_def, manifold.sphere(math.vec3_zero(), 1.0)) + tinted_def = dynamics.default_shape_def() + tinted_def.base_material.custom_color = debug_draw.COLOR_CRIMSON + tinted = dynamics.create_hull_shape(body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3(5.0, 0.4, 5.0)), &tinted_def, g_box as *HullData) + boxes = 0 + while boxes < 12 { + b = body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.9 * ((boxes % 4) as float), 0.4 + 0.85 * ((boxes / 4) as float), 0.0)) + bdef_name = "box" + dynamics.body_set_name(b, bdef_name) + dynamics.create_hull_shape(b, &sdef, g_box as *HullData) + boxes = boxes + 1 + } + kinematic = body_at(world_id, dynamics.BODY_KINEMATIC, math.vec3(0.0 - 5.0, 1.0, 0.0)) + dynamics.body_set_linear_velocity(kinematic, math.vec3(1.0, 0.0, 0.0)) + paddle = dynamics.create_hull_shape(kinematic, &sdef, g_box as *HullData) + off = body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0 - 10.0, 3.0, 10.0)) + off_shape = dynamics.create_capsule_shape(off, &sdef, Capsule { center1: math.vec3(0.0 - 0.3, 0.0, 0.0), center2: math.vec3(0.3, 0.0, 0.0), radius: 0.2 }) + dynamics.body_disable(off) + ball_body = body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3(3.0, 3.0, 3.0)) + ball = dynamics.create_sphere_shape(ball_body, &sdef, manifold.sphere(math.vec3_zero(), 0.3)) + + // Joints of every kind, their limits on. + a, b = pair(world_id, 0.0 - 12.0) + distance_def = dynamics.default_distance_joint_def() + joint_frames(&distance_def.base, a, b) + distance_def.enable_limit = true + distance_def.min_length = 0.1 + distance_def.max_length = 1.0 + distance_def.enable_spring = true + distance_def.hertz = 2.0 + dynamics.create_distance_joint(world_id, &distance_def) + a, b = pair(world_id, 0.0 - 9.0) + revolute_def = dynamics.default_revolute_joint_def() + joint_frames(&revolute_def.base, a, b) + revolute_def.enable_limit = true + revolute_def.lower_angle = 0.0 - 0.5 + revolute_def.upper_angle = 0.5 + dynamics.create_revolute_joint(world_id, &revolute_def) + a, b = pair(world_id, 0.0 - 6.0) + spherical_def = dynamics.default_spherical_joint_def() + joint_frames(&spherical_def.base, a, b) + spherical_def.enable_cone_limit = true + spherical_def.cone_angle = 0.5 + spherical_def.enable_twist_limit = true + spherical_def.lower_twist_angle = 0.0 - 0.3 + spherical_def.upper_twist_angle = 0.3 + dynamics.create_spherical_joint(world_id, &spherical_def) + a, b = pair(world_id, 0.0 - 3.0) + weld_def = dynamics.default_weld_joint_def() + joint_frames(&weld_def.base, a, b) + dynamics.create_weld_joint(world_id, &weld_def) + a, b = pair(world_id, 3.0) + prismatic_def = dynamics.default_prismatic_joint_def() + joint_frames(&prismatic_def.base, a, b) + prismatic_def.enable_limit = true + prismatic_def.lower_translation = 0.0 - 0.2 + prismatic_def.upper_translation = 0.2 + dynamics.create_prismatic_joint(world_id, &prismatic_def) + a, b = pair(world_id, 6.0) + wheel_def = dynamics.default_wheel_joint_def() + joint_frames(&wheel_def.base, a, b) + wheel_def.enable_suspension_limit = true + wheel_def.lower_suspension_limit = 0.0 - 0.2 + wheel_def.upper_suspension_limit = 0.2 + wheel_def.enable_steering = true + wheel_def.enable_steering_limit = true + wheel_def.lower_steering_limit = 0.0 - 0.4 + wheel_def.upper_steering_limit = 0.4 + dynamics.create_wheel_joint(world_id, &wheel_def) + a, b = pair(world_id, 9.0) + motor_def = dynamics.default_motor_joint_def() + joint_frames(&motor_def.base, a, b) + dynamics.create_motor_joint(world_id, &motor_def) + a, b = pair(world_id, 12.0) + parallel_def = dynamics.default_parallel_joint_def() + joint_frames(¶llel_def.base, a, b) + dynamics.create_parallel_joint(world_id, ¶llel_def) + a, b = pair(world_id, 15.0) + filter_def = dynamics.default_filter_joint_def() + joint_frames(&filter_def.base, a, b) + dynamics.create_filter_joint(world_id, &filter_def) + i = 0 + while i < 20 { + physics_world.step(world_id, 1.0 / 60.0, 4) + i = i + 1 + } + // A disabled body's shapes are out of the broad phase: not drawn, as in the reference. + shape_count = dynamics.world_shape_count(w) - 1 + + // Shapes: each once, a drawable made for each and kept. + draw = counting_draw() + draw.draw_shapes = true + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("shapes: each drawn once (${g_shapes} of ${shape_count})", g_shapes == shape_count && g_bad_kinds == 0) + ensure("shapes: a drawable each (${g_made})", g_made == shape_count && g_live == shape_count) + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("shapes: drawn again, nothing made again", g_shapes == shape_count && g_made == shape_count) + + // Colours as the reference picks them. + ensure("colour: static matte dark grey", color_of(floor) == debug_draw.make_debug_color(debug_draw.COLOR_DARK_GRAY, debug_draw.DEBUG_MATERIAL_MATTE)) + ensure("colour: a sensor wheat", color_of(sensor) == debug_draw.COLOR_WHEAT) + ensure("colour: a material's own", color_of(tinted) == debug_draw.COLOR_CRIMSON) + ensure("colour: a driven kinematic metallic steel blue", color_of(paddle) == debug_draw.make_debug_color(debug_draw.COLOR_STEEL_BLUE, debug_draw.DEBUG_MATERIAL_METALLIC)) + ensure("colour: a disabled body's shape not drawn", color_of(off_shape) == 0) + awake = debug_draw.make_debug_color(debug_draw.COLOR_TAN, debug_draw.DEBUG_MATERIAL_SOFT) + ensure("colour: an awake body soft tan", color_of(ball) == awake || color_of(ball) == debug_draw.make_debug_color(debug_draw.COLOR_ORANGE, debug_draw.DEBUG_MATERIAL_GLOSSY)) + + // The options. + draw = counting_draw() + draw.draw_bounds_option = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("bounds: one a shape (${g_bounds})", g_bounds == shape_count && g_shapes == 0) + draw = counting_draw() + draw.draw_body_names = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("names: one a named body (${g_strings})", g_strings == 12 && string.equals(g_last_text, "box") == 1) + draw = counting_draw() + draw.draw_mass = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + + ensure("mass: a transform a body, the mass of each dynamic one (${g_transforms}, ${g_strings})", g_transforms > 20 && g_strings > 0 && g_strings < g_transforms) + draw = counting_draw() + draw.draw_joints = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("joints: the weld's two boxes, once", g_boxes == 2) + ensure("joints: frames, limits and anchors (${g_segments} segments, ${g_points} points)", g_segments > 80 && g_points > 10) + draw.draw_joint_extras = true + draw.draw_graph_colors = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("joints: a force and torque label each awake joint", g_strings == 9) + draw = counting_draw() + draw.draw_contacts = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + expected_points = 0 + k = 0 + while k < w.contacts.count { + c = dynamics.contact_at(w, k) + if c.contact_id == k && c.set_index == dynamics.AWAKE_SET && c.color_index != 0 - 1 { + m = 0 + while m < c.manifold_count { + expected_points = expected_points + dynamics.manifold_at(c, m).point_count + m = m + 1 + } + } + k = k + 1 + } + ensure("contacts: a point each manifold point (${g_points} of ${expected_points})", g_points == expected_points && expected_points > 0) + draw.draw_contact_normals = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("contacts: a normal and its separation each", g_segments == expected_points && g_strings == expected_points) + draw = counting_draw() + draw.draw_islands = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("islands: bounds each (${g_bounds})", g_bounds >= 1) + draw = counting_draw() + draw.draw_sleep = true + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("sleep: a point and a value each moving body (${g_points})", g_points > 0 && g_points == g_strings) + + // The drawing bounds and the mask cull. + draw = counting_draw() + draw.draw_shapes = true + draw.drawing_bounds = AABB { lower: math.vec3(9.9, 0.9, 9.9), upper: math.vec3(10.1, 1.1, 10.1) } + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("cull: the bounds hold the sensor alone (${g_shapes})", g_shapes == 1 && color_of(sensor) == debug_draw.COLOR_WHEAT) + draw.drawing_bounds = debug_draw.default_debug_draw().drawing_bounds + reset_counts() + physics_world.world_draw(world_id, &draw, 0 as long) + ensure("cull: a mask of nothing draws nothing", g_shapes == 0) + + // The drawables' lives: a shape goes, a geometry changes. + given = g_given_back + dynamics.destroy_shape(tinted, true) + ensure("lives: a destroyed shape's drawable given back", g_given_back == given + 1) + dynamics.shape_set_sphere(ball, manifold.sphere(math.vec3_zero(), 0.4)) + ensure("lives: a changed shape's drawable given back", g_given_back == given + 2) + made = g_made + draw = counting_draw() + draw.draw_shapes = true + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("lives: the changed shape's next draw makes another", g_made == made + 1) + + // An in-place restore keeps the drawables of the shapes still there. + block, byte_count = snapshot.save_world(world_id) + made = g_made + given = g_given_back + ensure("restore: in place", snapshot.restore_world(world_id, &wdef, block, byte_count)) + ensure("restore: nothing given back, nothing made", g_given_back == given && g_made == made) + reset_counts() + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + ensure("restore: drawn without remaking", g_made == made && g_shapes == dynamics.world_shape_count(w) - 1) + dynamics.destroy_shape(ball, true) + physics_world.world_draw(world_id, &draw, shape.default_query_filter().mask_bits) + given = g_given_back + ensure("restore: to before the ball went", snapshot.restore_world(world_id, &wdef, block, byte_count)) + ensure("restore: no handle for the ball to give back, none lost", g_given_back == given) + snapshot.free_snapshot(block, byte_count) + + // The world goes: every drawable given back. + dynamics.destroy_world(world_id) + ensure("lives: the world's going gives every drawable back (${g_live} left)", g_live == 0 && g_made == g_given_back) + + // A player's world draws with the host's callbacks, and its queries draw. + world_id = dynamics.create_world(&wdef) + ground = body_at(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0)) + floor_hull = hull.make_box_hull(20.0, 1.0, 20.0) + dynamics.create_hull_shape(ground, &sdef, floor_hull) + hull.destroy_hull(floor_hull) + i = 0 + while i < 4 { + dynamics.create_sphere_shape(body_at(world_id, dynamics.BODY_DYNAMIC, math.vec3((i as float) - 1.5, 3.0, 0.0)), &sdef, manifold.sphere(math.vec3_zero(), 0.5)) + i = i + 1 + } + r = recording.create_recording() + recording.start_recording(world_id, r) + i = 0 + while i < 10 { + tagged = shape.default_query_filter() + tagged.id = 42 as long + tagged.name = "bullet" + physics_world.cast_ray_closest(world_id, math.vec3(0.0, 6.0, 0.0), math.vec3(0.0, 0.0 - 8.0, 0.0), tagged) + physics_world.collide_mover(world_id, math.vec3(0.0, 0.2, 0.0), Capsule { center1: math.vec3_zero(), center2: math.vec3(0.0, 1.0, 0.0), radius: 0.3 }, + shape.default_query_filter(), accept_planes, null) + physics_world.step(world_id, 1.0 / 60.0, 4) + i = i + 1 + } + recording.stop_recording(world_id) + dynamics.destroy_world(world_id) + p = recording.create_player(recording.recording_data(r), recording.recording_size(r), 1) + made = g_made + recording.player_set_debug_shape_callbacks(p, make_drawable, give_back_drawable, null) + recording.player_seek_frame(p, 5) + draw = counting_draw() + draw.draw_shapes = true + reset_counts() + physics_world.world_draw(recording.player_world(p), &draw, shape.default_query_filter().mask_bits) + ensure("player: its world drawn with the host's drawables", g_shapes == 5 && g_made == made + 5) + recording.player_seek_frame(p, 2) + recording.player_seek_frame(p, 7) + ensure("player: seeks keep the drawables", g_made == made + 5) + draw = counting_draw() + reset_counts() + recording.player_draw_frame_queries(p, &draw, 0 - 1, 0) + ensure("player: the ray, its hit and its normal; the mover and its planes", g_segments >= 3 && g_points >= 1 && g_capsules == 1) + ensure("player: the selected query labelled with its caller", g_strings == 1 && string.equals(g_last_text, "bullet (42)") == 1) + reset_counts() + recording.player_draw_frame_queries(p, &draw, 1, 0 - 1) + ensure("player: one query alone, unlabelled", g_capsules == 1 && g_strings == 0) + recording.destroy_player(p) + recording.destroy_recording(r) + ensure("player: its going gives every drawable back", g_live == 0) + + // The labels' numbers, as the reference prints them. + ensure("format: two decimals", string.equals(debug_draw.fixed(3.14159, 2), "3.14") == 1 && string.equals(debug_draw.fixed(0.0 - 0.5, 2), "-0.50") == 1) + ensure("format: none and three", string.equals(debug_draw.fixed(41.6, 0), "42") == 1 && string.equals(debug_draw.fixed(0.0004, 3), "0.000") == 1) + ensure("format: a feature in hexadecimal", string.equals(debug_draw.hex(255), "0XFF") == 1 && string.equals(debug_draw.hex(0), "0") == 1) + ensure("colours: the graph's", debug_draw.graph_color(0) == debug_draw.COLOR_RED && debug_draw.graph_color(23) == debug_draw.COLOR_BLACK) + ensure("colours: a material in the high byte", debug_draw.make_debug_color(debug_draw.COLOR_WHITE, debug_draw.DEBUG_MATERIAL_METALLIC) == (0x05FFFFFF)) + + hull.destroy_hull(g_box as *HullData) + free(g_colors) + println("debug draw: ${checks} checks") + if failures > 0 { + println("debug draw: ${failures} failure(s)") + exit(1) + } + println("debug draw: all checks passed") +} + +accept_planes(shape_id: ShapeId, planes: PlaneResult[], count: int, context: ptr) -> bool { return true }