diff --git a/ALGORITHMS.md b/ALGORITHMS.md index ab50778..d931820 100644 --- a/ALGORITHMS.md +++ b/ALGORITHMS.md @@ -8,6 +8,8 @@ 6. [Heightfield](#heightfield) 7. [Continuous collision](#continuous-collision) 8. [Queries](#queries) +9. [Kinematic bodies](#kinematic-bodies) +10. [Characters](#characters) ## Broad phase @@ -23,11 +25,17 @@ awake body. The pairs of bodies whose stored AABBs overlap are kept from a step to the next (the persistent pairs of Box2D v3): only a body put in a tree since the last step (a dynamic body out of its enlarged AABB, a static body moved by the game, a body added) queries the trees for its pairs, and a pair is dropped when its stored AABBs no longer overlap. -A resting scene costs nothing, an awake one only pays for the bodies which left their enlarged AABB (2000 bodies -settling: 1.6 ms for a traversal of the trees against each other, 0.3 ms with the pairs kept). The pairs of the step -are those whose exact AABBs overlap, with an awake dynamic body, sorted by the index of their first body (a counting -sort): the list is the same as a search from scratch, and the same as the former uniform grid gave, so the solver keeps -its order and its results bit for bit. +A resting scene costs nothing (but for the bodies asleep in a trigger, [Triggers](#triggers)), an awake one only pays +for the bodies which left their enlarged AABB (2000 bodies settling: 1.6 ms for a traversal of the trees against each +other, 0.3 ms with the pairs kept). The pairs of the step are those whose exact AABBs overlap, with an awake dynamic +body, or a trigger and a dynamic or kinematic body awake or asleep ([Triggers](#triggers)), sorted by the index of +their first body (a counting sort), then the pairs of each body by the index of the other body (the sort of the +standard library, `slices.SortFunc`: pdqsort, an insertion sort under 12 elements; the keys of a body are unique, so +the order doesn't depend on the sort): the list is the same as a search from scratch, and the same as the former +uniform grid gave, so the solver keeps its order and its results bit for bit. The pairs of a body were sorted by +insertion up to v0.4.0, quadratic for a body with hundreds of pairs (a zone or a ground box under 400 bodies: their +pairs come from the records nearly in reverse order): 400 crates asleep in a zone cost 341 µs per step with it, 103 µs +with `slices.SortFunc` ([Triggers](#triggers)). ## Collision filtering @@ -96,6 +104,83 @@ separations measured again, instead of running GJK/EPA (the body pair cache of J impulses of the previous step warm start the new points: each point takes the impulses of the closest previous point in the local space of body A, within 2 cm (as the contact cache of Jolt; Box2D matches the points by feature id). +## Triggers + +`tree.go` (`detectsTrigger`), `world.go` (`overlapTrigger`, `overlapKept`), `event.go`. A pair of a trigger and a body +ends when their shapes no longer overlap, or when one of them leaves the world: never because one of them falls +asleep. Up to v0.4.0 the pair of a static trigger and a body left the broad phase when the body fell asleep, and the +missing pair was taken for an exit (the body entered again when it woke up); a kinematic body never entered a static +trigger. + +What the references do: + +| Engine | A body asleep in a trigger | Removal | +|--------|----------------------------|---------| +| Box2D v3.1.1 | every sensor queries the trees at each step, "Sensors do not consider sleep" (`docs/simulation.md`, `b2SensorTask`): the overlap holds | `b2SensorEndTouchEvent` "if the sensor or visitor are destroyed" (`types.h`) | +| PhysX | a trigger pair is not tested while both actors sleep, "the overlap state can not change if both objects are sleeping"; it is tested again when an actor is created or its pose set (`ScTriggerInteraction.cpp`, `onActivate`, `onDeactivate`, `PROCESS_THIS_FRAME`) | `eNOTIFY_TOUCH_LOST` when an object is deleted (`PxTriggerPair`, since 3.1.1) | +| Jolt 5.6 | a static sensor loses the contact of a body which falls asleep (`OnContactRemoved`, `ContactListener.h`); an active kinematic or dynamic sensor never sleeps and detects the sleeping bodies (`Body::SetIsSensor`, `Body::UpdateSleepStateInternal`) | `OnContactRemoved` at the next update | +| Godot 4 | `GodotAreaPair3D` keeps its state, tested only when its area moves or its body is active (`godot_step_3d.cpp`); its Jolt module makes every `Area3D` a kinematic sensor, active while it monitors (`JoltArea3D::_get_motion_type`, `_should_sleep`) | `body_exited` when the pair is destroyed | +| Unity | `OnTriggerStay` "while a collider remains touching the trigger", `OnTriggerExit` "when a collider stops touching a trigger" | no `OnTriggerExit` when a collider is destroyed or deactivated | + +Feather follows Box2D for the pairs and PhysX for their cost: +- The broad phase emits the pair of a trigger and a body which moves (dynamic or kinematic) whatever the sleep + (`detectsTrigger`): a static trigger the game moves away from a sleeping body ends the pair, moved over it starts one. + 2 static bodies never pair. +- A kinematic body enters and leaves a static trigger, and a kinematic trigger detects the static and the kinematic + bodies, without any contact: Box2D v3 tests every sensor against its 3 trees, static, kinematic and dynamic + (`b2SensorTask`); Jolt: "These sensors will only detect collisions with active Dynamic or Kinematic bodies" for a + static sensor (`Body.h`), a kinematic sensor detects the static bodies only with `SetCollideKinematicVsNonDynamic`; + Unity: "A dynamic or kinematic trigger collider collides with any collider type. A static trigger collider collides + with any dynamic or Kinematic collider" (Manual, Collider types interaction), the rule of Feather. A moved kinematic + proxy queries the static tree and the planes too: its pairs without a trigger are kept in the records of the broad + phase, never emitted, as a filtered pair. +- A pair with a trigger has no contact: its narrow phase only tells whether the shapes overlap, by the distance between + their cores (GJK), not over the sum of their radii, as `World.Overlap` (Box2D: `b2ShapeDistance` with the radii, an + overlap under `10 * FLT_EPSILON`, `b2SensorQueryCallback`). A body in contact with a trigger is in it. A plane, a + heightfield or a triangle mesh keeps its real contacts: it has no core for GJK. +- A pair whose bodies both rest (static or asleep) keeps the overlap of its last test, while the AABB of each body is + the same, bit for bit, since that test (`overlapKept`: the box and the step of its last change in the proxy of each + body, the overlap in the record of the pair): the overlap can't have changed, as PhysX. A body moved by the game, its + AABB updated, is tested again. A rotation which keeps the AABB of a body bit for bit is not seen, as the broad phase + doesn't see it for a static body. +- The events: an enter when the pair starts, a stay at each step while one of the bodies is awake (none while both rest, + as between 2 sleeping bodies), an exit when it ends. `RemoveBody` ends the pairs of the body: their exits are sent + with the next events (at the end of the next step, or in the flush running if a listener removed the body), as Box2D, + PhysX and Jolt. + +**The contacts follow the same rules.** A contact whose bodies both rest, a body asleep on a static body (the ground, +a plane) or on another sleeping body, is no longer detected but its bodies still touch: the events keep it, with no +stay, and its exit comes when the bodies stop touching or one of them is removed. Up to v0.4.0 only a contact between 2 +sleeping bodies was kept: a crate falling asleep on the ground got a `CollisionExit`, and a `CollisionEnter` when it +woke up. Box2D v3 keeps the touching contacts of an island which falls asleep in its sleeping set (`b2TrySleepIsland`, +`solver_set.c`) and sends `b2ContactEndTouchEvent` when a touching contact is destroyed (`b2DestroyContact`, +`contact.c`); PhysX sends `eNOTIFY_TOUCH_LOST` when an actor is removed; Unity: "Collision stay events are not sent for +sleeping Rigidbodies". Jolt alone documents the other way: "as soon as a body goes to sleep the contacts between that +body and all other bodies will receive an OnContactRemoved callback" (`ContactListener.h`). A pair of the events keeps +its kind (contact or trigger): a body made a trigger by the game ends its contacts and starts its trigger pairs. + +Cost of a step, 1 worker, crates asleep in a static zone of 20 x 20 m and listened to (µs, median of 5, machine at +rest): + +| Crates | With the overlap kept | A GJK per pair at each step | The manifolds of the former narrow phase | Before (out of the zone) | +|--------|-----------------------|-----------------------------|------------------------------------------|--------------------------| +| 25 | 8 | 26 | 91 | 4 | +| 100 | 29 | 104 | 363 | 8 | +| 400 | 120 | 429 | 1463 | 25 | + +Before, the sleeping crates were out of the broad phase, and out of the zone too; their contact with the ground left +the events when they fell asleep. With the overlap kept, a crate at rest costs about 0.24 µs per step: its trigger +pair, its place in the pairs of the step (the scan of its record, the sort), `overlapKept`, and in the events its +trigger pair and its contact with the ground, kept, and their comparison with the previous step (0.04 µs of it for +the contact). The pairs of the zone sorted by insertion, as up to v0.4.0, cost 341 µs at 400 crates instead of 103 +without the kept contacts. PhysX costs nothing for a pair whose actors both sleep (the interaction is deactivated), +Box2D a `b2ShapeDistance` per pair at each step (the column of a GJK per pair). + +The kinematic bodies: 64 kinematic cubes going round over a floor of 400 static tiles (1 worker, median of 5), 95 µs +per step before and 96 µs with their pairs with the static bodies kept in the records for the triggers; with 16 +static zones on the floor, 95 µs before (no kinematic cube entered them) and 118 µs: the cubes in the zones are tested +at each step and send their events. + ## GJK Algorithm GJK tests if two convex shapes overlap: they overlap if their Minkowski difference `A - B` contains the origin. The shapes only need a `Support(direction)` function, the farthest point in a direction. @@ -435,9 +520,8 @@ wherever the ball is). A box which can't turn nor move along X still rubs along (`TestFullyLockedBodyStaysDynamic`). It is the behaviour of Unity (`RigidbodyConstraints.FreezeAll`), of PhysX and of Box3D; Jolt forbids it ("No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead", `AllowedDOFs.h:12`; the assertion of `MotionProperties::SetMassProperties`, `MotionProperties.cpp:60`). -One limit: it is not a static body for the continuous collision. A fast body which is not a bullet is only stopped by -the static bodies (`stopAtImpact`): a ball of 5 cm at 80 m/s goes through a wall of 4 cm with all its axes locked, and -stops on the same static wall. A wall is a static body; otherwise the fast body is a bullet (`IsBullet`). +It is a dynamic body for the continuous collision: a fast body is stopped by it as by any body (`findImpact`), from +their speculative contact when they had one, from the time of impact otherwise (`TestFastBodyStopsOnEveryBody`). **Setting the locks.** `RigidBody.SetLocks` clears the velocities along the new locked axes (`b3Body_SetMotionLocks`, `box3d/src/body.c:2379-2408`), returns at once without change (`:2362-2365`), and wakes the body up. @@ -509,39 +593,306 @@ joint (as in PhysX). point. Angular: the twist row, the cone if both swings are limited, else 1 row per swing (`atan2(-p.z, p.x)` around Y, `atan2(p.y, p.x)` around Z), all locked = the 3 angular rows. -## Heightfield +## Heightfield and triangle mesh +A heightfield and a mesh are surfaces of triangles. They differ by the triangles they give under the AABB of a body (the +cells of a grid, the leaves of a tree) and by nothing else: one generator of contacts per triangle (`collideTriangle`) +and one grouping in patches (`collideTriangles`, `collision_triangles.go`) serve both, as `b3ComputeMeshManifolds` of +Box3D takes its triangles from `b3QueryMeshTriangles` or `b3QueryHeightFieldTriangles` (`src/mesh_contact.c:51-73`), +`CollideConvexVsTriangles` of Jolt is the visitor of its `MeshShape` and of its `HeightFieldShape`, and +`PCMConvexVsMeshContactGeneration` of PhysX serves its mesh and its height field. The sweeps (`sweepTriangle`), the +overlaps (`overlapsTriangle`) and the continuous collision (`triangleImpact`) share their test of a triangle the same +way. The edges of a mesh are classified as the ones of the heightfield (active, convex), by the triangles sharing them. +No interface of "source of triangles": the enumeration of the triangles is a `switch` on the shape, the loop over the +cells or over the leaves writes in the buffers of the scratch (an interface or a closure per triangle would cost an +indirect call per triangle, and could allocate). + The terrain is a grid of heights, split in 2 triangles per cell (along the diagonal from (x, z) to (x+1, z+1)). The body is tested against the triangles under its AABB, one by one: the grid is cut in blocks of 16x16 cells with -their lowest and highest heights, to skip the blocks far from the body. - -Each triangle is tested with GJK/EPA: -- **Face**: always, the contact of the body with the plane of the triangle (`CollideWithPlane`), limited to the - points above the triangle. On a flat terrain, a body behaves exactly as on a plane. -- **Inner edges**: a body sliding on the terrain must not hit the edges between the triangles. Each edge is active if it - is on a border or a hole, or if it bends down (convex) by more than 5° (like Jolt, `ActiveEdges.h`). A contact on an inactive - edge (or vertex) takes the normal of its triangle. If the body is beside the triangle, above the edge, it keeps the - witness point of EPA, only if no other triangle has a contact with this normal. -- **Active edges** (a ridge, a border): also the contact of EPA, with its normal. A capsule lying across a ridge touches - the ridge, and its ends can fall on both faces. - -The contacts are then grouped by normal: the contacts of triangles with less than 5° between their normals form a patch, -a manifold of 4 points. A body touches the terrain with 8 patches at most (`MaxManifoldsPerPair`): a box in a valley -gets one patch per slope. The patches of the deepest contacts are kept, the others are dropped (like Jolt). +their lowest and highest heights, to skip the blocks far from the body. Jolt keeps a hierarchy of such blocks +(`HeightFieldShape`, `RangeBlock`), PhysX and Box3D a flat grid (`b3QueryHeightField`); none uses a tree of triangles +for a terrain. The mesh is described below (Triangle mesh). + +**Edges** (`Heightfield.cellEdges`). An edge is *convex* if the triangle on its other side bends down, or if there is +none (a border, a hole), and *active* if it is convex by more than 5° (`ActiveEdges::IsEdgeActive` of Jolt with +`mActiveEdgeCosThresholdAngle`, `HeightFieldShape.h:110`; `cos5Deg` of `b3CreateHeightField`, Box3D +`src/height_field.c:192`). Only an active edge can push a body sideways. + +**Each triangle** (`collideTriangle`), with GJK/EPA as the only detection: +1. **Seen from below, it is ignored**: the center of the body is under its plane (Jolt + `CollideConvexVsTriangles.cpp:52-54`, Box3D `triangle_manifold.c:380-385`, PhysX + `GuPCMContactConvexCommon.cpp:238-240`). The terrain is a surface, without thickness (`CollidePoint` of the height + field of Jolt is empty, `thickness = 0` in `GuHeightField.h:847` of PhysX): a body whose center went through it is + not pushed back, it falls under the terrain. The continuous collision is what keeps a fast body above it. +2. **Face**: the vertices of the body over the triangle, along the normal of the triangle. The face of the body is the + one it shows to a plane of this normal (`CollideWithPlane` without limit of distance): the 4 corners of a face of a + box, both ends of a capsule. It is taken whatever its tilt, as the supporting face of Jolt + (`BoxShape::GetSupportingFace`); a capsule gives both ends at any tilt, where Jolt asks them within 2 cm along the + normal (`cCapsuleProjectionSlop`, `CapsuleShape.cpp:204-212`). Seen along the normal, the face is cut by the 3 + sides of the triangle (Sutherland-Hodgman): the part of the face over the triangle, the polygon + `ManifoldBetweenTwoFaces` of Jolt gets by cutting the triangle by the face (`ManifoldBetweenTwoFaces.cpp:194`); + a segment is cut the same way (`b3CollideTriangleAndCapsule` of Box3D: `ClipPolyVsEdge` of Jolt, `ClipPoly.h`, can + give a point out of the triangle when the capsule ends beside it). Each point is at its distance to the plane of the + triangle along the normal, kept under the margin of the pair (Jolt adds `mManifoldTolerance`, 1 mm). On a flat + terrain these points are the points of the body on a plane. +3. **Flat ridges**: the points where the face of the body crosses a convex edge which is not active (a ridge folded by + less than 5°), along the normal of the triangle. The side of a capsule, the face of a box laid across such a ridge + touch it there, with none of their vertices: before, the ridge went in the body by `half length x tan(fold / 2)`, + up to 10.7 mm for a body of 0.5 m. The points cut on a flat edge or on an edge bent inwards are not kept (Jolt keeps + all the points of the polygon): they are between vertices of the body, and say nothing more. +4. **Closest point** (GJK/EPA: the closest points of the triangle and of the body, their normal, their depth). Its + normal follows the rule of `ActiveEdges::FixNormal` of Jolt (`ActiveEdges.h:42-111`), without its part on the + motion of the body: the normal of EPA is kept if the 3 edges of the triangle are active, if the triangle has an + active edge and both normals are closer than 1°, or if the point is on an active edge or on a vertex of an active + edge (barycentric coordinates under 1e-4); else the normal is the one of the triangle. The point and its depth are + kept in both cases (Jolt), the separation is measured along the normal kept (`ContactConstraintManager.cpp:69`). + Jolt also keeps the normal of EPA when it brakes the motion of the body over the terrain less than the normal of + the triangle (the hint `mActiveEdgeMovementDirection` of `PhysicsSystem.cpp`: a body leaving a triangle is not held + by its plane). Feather does not: the margin of Jolt is fixed (`mSpeculativeContactDistance`, 2 cm, `PhysicsSettings.h:50`, the + `mMaxSeparationDistance` of its collisions, `PhysicsSystem.cpp:1108`), + the speculative margin of Feather grows with the speed of the body (`reach`: 10 cm at 5 m/s), and brings the + vertices and the edges of the triangles around within it, where every normal tilted from the vertical brakes a fall + less than the plane. A box falling flat on a flat terrain, straight on a vertex of the grid, had 5 contacts at the + same corner: one straight up, 4 on the edges of the triangles around, tilted by 10 to 60°; nearly parallel to the + first, they took impulse from it over the sub-steps (9.4 N·s, their planes 2 mm under the ground), and the box landed + 2 mm deep (0.3 mm now, the deepest of 560 boxes dropped alone on a flat terrain). PhysX and Box3D have no such + rule (`PCMContactConvexMesh`, `b3ComputeMeshManifolds`). What the rule gives in Jolt, a box sliding across an active + ridge not held by the plane it leaves, comes from the points cut on an active edge, never points of the face + (below). + - With the normal of the triangle, the closest point is a point of its face (a sphere has no other), kept as the + vertices of the body are, or a point of its edge, the body beside the triangle, above this edge: kept if it is + deeper than the other points of the triangle (the end of a capsule right above a ridge is over neither of its + triangles, along their normals). + - With another normal, it is the contact of an **edge**: the points where the face of the body crosses the active + edges of the triangle, along this normal, or the closest point alone (a sphere, an edge of a box across the + ridge). The points cut on an active edge are never points of the face: the plane of a triangle stops at its edge, + a box sliding across a ridge must not be held by the plane of the slope it leaves. + +**Patches** (`collideHeightfield`). The contacts are sorted, the deepest first, and grouped by normal: the contacts whose +normals differ by less than 5° form a patch, a manifold of 4 points (`mContactNormalCosMaxDeltaRotation` of Jolt, +`PhysicsSettings.h:74`; `clusterThreshold` 0.996 of Box3D, `mesh_contact.c:856`). The normal of a patch is the one of +its deepest contact (Jolt takes the mean of the normals, `PhysicsSystem.cpp:1186`). 3 choices of Feather: +- **The vertices of the body and the edges of the terrain don't share a manifold**: one patch per normal for the points + of the faces, one for the points on the edges (flat ridges, active edges). Jolt and Box3D group by normal alone and + keep 4 points out of all of them (`PruneContactPoints`, `b3ReduceCluster`). With 4 points for both, a point on an + edge takes the place of a corner: a plate resting flat on a slope lost 2 corners to 2 points of its face over an edge + of the grid, tipped, and sank by 10 mm while the pair cache kept its contact; boxes dropped alone on a flat terrain + landed up to 21 mm deep (0.3 mm with their corners, as on a plane). +- **A closest point on an edge is kept if it is the deepest point along its normal** (no patch within 5° has a point + as deep). A body resting on a triangle near an edge also has a closest point with each triangle around, on this + edge: one more point per triangle, which moves the friction center (a sphere would not roll on a flat terrain as on + a plane). Box3D and PhysX drop such a point when a triangle with a contact of its face owns the edge + (`b3ComputeMeshManifolds`, `mesh_contact.c:654-832`; `PCMConvexVsMeshContactGeneration::generateLastContacts`). + Feather compares the depths: with the ownership alone, the deepest point of a body above a ridge is dropped when the + rest of the body gives a contact to the faces around (the cause of the bodies found 5 to 19 cm in the terrain: a + point 56 mm deep dropped for a point 0.1 mm deep). A closest point inside a triangle is a point of its face: kept + whatever its depth, as the vertices of a body are (Box3D and PhysX keep it too, `mesh_contact.c:653-660`, + `GuPCMContactSphereMesh.cpp:305-319`: a point of a face owns no edge). Dropped as the others, it left a sphere rolling + to a triangle folded by less than 5° without a contact with it until it was the deepest, and the sphere sank by + 3 mm: 70 spheres out of 2800 dropped alone on the hills landed deeper than 2 mm, 22 now. +- **8 patches at most** per pair (`MaxManifoldsPerPair`; 32 manifolds in Jolt, `PhysicsSystem.cpp:1133`): the deepest + are kept. Measured with 32: the same depths for the spheres and the capsules, 47 boxes out of 800 landing deeper + than 5 mm instead of 52. + +The 4 points of a patch (`reducePatch`) follow the steps of `Reduce`: the deepest, the furthest from it, the points +adding the most area. Among the points as deep as the deepest, the first one is an end of the contact (the furthest +along a tangent, as the first step of `b3ReduceManifoldPoints`), and a point is added only if it widens the contact by +`SpeculativeDistance`. + +Measured (`bench`, the depth of a box counts the terrain in its faces): a capsule and a box across a ridge folded by 0 +to 10° rest less than 0.1 mm in it (2.2 to 10.8 mm before, under 5°); 1000 piles of 60 bodies on hills rest at 0.28 mm +(median of the deepest body; 3.3 mm before) and land at 7.9 mm (15.9 mm before); the same piles on a flat terrain rest +at 0.13 mm and land at 1.7 mm, as on a plane. What is left at the landing is the limit of the contacts computed once +per step, as Jolt, Box3D and PhysX compute them (see the limitations in ARCHITECTURE.md): a point of a tumbling body +moves over another triangle during the step, and has no contact with it before the next step. With the contacts +computed at each of the 8 sub-steps, the deepest of 60 bodies dropped alone lands at 3.7 mm instead of 8.1 mm, and +no sphere deeper than 1 mm. + +## Convex hull +`actor.ConvexHull`, `actor/convexhull.go`. Sources read: Jolt v5.3.0 (`Geometry/ConvexHullBuilder.cpp`, +`Physics/Collision/Shape/ConvexHullShape.cpp`), Box3D (commit `9f998c8`, `src/hull.c`), PhysX (`main`, 5.11.0, +`GuCookingQuickHullConvexHullLib.cpp`, `PxConvexMeshDesc.h`), Bullet 3.25 (`btConvexHullComputer`), Barber, Dobkin & +Huhdanpaa (1996), Dirk Gregorius, "Implementing QuickHull" (GDC 2014). + +**Quickhull** (`hullBuilder`). The points are taken around the center of their bounds (Box3D shifts them to an origin +too, `b3HullBuilder_Construct`). The tolerance is the rounding error of the distance of a point to a plane, from the +size of the cloud: `3 (|x| + |y| + |z|)max ε` (Gregorius; `DetermineCoplanarDistance` of Jolt, `ConvexHullBuilder.cpp:240`; +`b3HullBuilder_ComputeTolerance` of Box3D, `hull.c:632`), with the ε of a float64 where both use the one of a float. A +point is out of the hull if it is further than 8 tolerances in front of a face (`minOutside` of Box3D, `hull.c:644-645`), +a face is seen by a point if the point is more than 4 tolerances in front of its plane (`minRadius`, `hull.c:868`). +1. **The initial tetrahedron**: the 2 points the furthest apart along the axis of the largest extent, the point the + furthest from their line, the point the furthest from their plane (`b3HullBuilder_BuildInitialHull`, `hull.c:649`; + `QuickHull::findSimplex` of PhysX). Fewer than 4 points, a line or a plane: `ErrHullDegenerate` (Jolt builds a flat + hull of 2 faces, `ConvexHullBuilder2D`; Box3D fails as Feather). A hull is a volume: a flat one has no mass. +2. **The outside sets** (Barber et al.; the conflict lists of Jolt and Box3D): each point out of the hull is given to the + face it is the furthest in front of, the furthest point of a face last (`AssignPointToFace`, `ConvexHullBuilder.cpp:196`). +3. **A point at a time**: the furthest point of the face with the furthest point (`b3HullBuilder_NextConflictVertex`, + `hull.c:782`). The faces which see it, from its face through their neighbors (the horizon of Barber et al., + `b3HullBuilder_BuildHorizon`, `hull.c:839`), are removed, a cone of triangles is built from the horizon to the point + (`b3HullBuilder_BuildCone`, `hull.c:881`; `AddPoint` of Jolt, `:622`), and the points of the removed faces are given to + the cone. The horizon must be a loop, each vertex starting one edge: if the rounding breaks it, `ErrHullFailed` + (Jolt returns an error when the hull is inconsistent, `ConvexHullShape.cpp:60-78`). +4. **The vertex limit**: the construction stops at `maxVertices` (`MaxHullVertices` = 256 at most, `cMaxPointsInHull` of + Jolt, `B3_HULL_MAX_COUNT` of Box3D; 255 in PhysX, `PxConvexMeshDesc::vertexLimit`). The points left out are then + outside the hull, the closest to it: the furthest were added first. Jolt accepts a hull stopped this way + (`MaxVerticesReached`, `ConvexHullShape.cpp:61`), Box3D runs on a budget (`hull.c:1450-1458`); PhysX expands the + hull by its planes instead (`expandHull`, plane shifting). Measured: 32 vertices out of a cloud of 500 points on a + sphere keep 83 % of its volume. +5. **The faces**: once the triangulation is complete, 2 triangles across an edge are merged when the edge is not convex: + the centroid of one is not behind the plane of the other by more than the tolerance (coplanar, or bent inwards by the + rounding: `b3IsEdgeConvex` of Box3D, `hull.c:466`, both ways as `MergeCoplanarOrConcaveFaces` of Jolt, `:975`). The + boundary of each group of triangles is chained in a polygon, its vertices on the segment of their neighbors within + the tolerance are dropped (`b3HullBuilder_ResolveVertices`, `RemoveInvalidEdges` of Jolt), and its plane is Newell's + normal through its centroid (`b3NewellPlane`, `hull.c:529`). Jolt and Box3D merge after each cone, on a hull still + growing; Feather merges once, at the end: the triangulation is the quickhull of Barber et al. as it is, the merge a + pass over it. The face of a cube is 1 polygon of 4 vertices: a cube resting on the ground has the manifold of the Box + (4 corners), where 2 triangles would give 3. + +**Mass properties** (`massProperties`): the volume and the center of mass are the sum of the tetrahedra between the +faces (fans of triangles) and a point inside (the mean of the vertices), as `GetCenterOfMassAndVolume` of Jolt +(`ConvexHullBuilder.cpp:1258`); the inertia is the covariance of the canonical tetrahedron carried by each tetrahedron +from the center of mass (Jolt, `ConvexHullShape.cpp:84-121`, after Blow & Binstock), for a density of 1, scaled to the +mass. The points are then translated so that the center of mass is at the origin of the hull: a body is at its center +of mass, `CenterOfMass()` gives it in the space of the points (Jolt centers its shapes the same way, +`mCenterOfMass`). Measured: the hull of the corners of a box has its mass and its inertia within 1e-6, its center of +mass within 1e-12. + +**The shape**: the support point is the vertex the furthest along the direction, among all of them +(`HullNoConvex::GetSupport` of Jolt, `b3FindHullSupportVertex` of Box3D, `hull.c:1644`); the contact feature is the face +the most aligned with the direction (`GetSupportingFace` of Jolt, `ConvexHullShape.cpp:674`; `b3FindHullSupportFace`), +8 vertices at most, one out of k on a larger face (Jolt skips vertices the same way, `:703-706`); against a plane, the +vertices of this face, as the Box; the AABB is the one of the 8 corners of the local bounds (`b3ComputeHullAABB`, +`hull.c:2614`; the local bounds of Jolt), not of the vertices; the ray is clipped by the planes of the faces, in the hull +between the last plane it enters and the first one it leaves (`b3RayCastHull`, `hull.c:2641`; `CastRayHelper` of Jolt, +`:883`). Measured (a hull against a box, per pair, `Collide`): 8 vertices 5.1 µs, 32 vertices 6.9 µs, 64 vertices +7.3 µs, 256 vertices 16.3 µs; a box against a box 6.7 µs, the hull of a cube 7.2 µs. The build of a hull of 4000 +points takes 0.5 ms. + +## Triangle mesh +`actor.TriangleMesh`, `actor/trianglemesh.go`. Sources read: Box3D (`src/mesh.c`, `src/mesh_contact.c`), Jolt v5.3.0 +(`MeshShape.cpp`, `AABBTree/AABBTreeBuilder.cpp`, `TriangleSplitter/TriangleSplitterBinning.cpp`, `Geometry/Indexify.h`, +`Geometry/RayTriangle.h`, `Physics/Collision/ActiveEdges.h`), PhysX (`GuBV4Build.h`, `GuEdgeList.cpp`), Bullet 3.25 +(`btQuantizedBvh.cpp`), Ericson 6.2.1. + +**Build** (`NewTriangleMesh`, outside `Step`; 100 000 triangles take 0.3 s). The vertices closer than 0.1 mm are +welded (`MeshWeldDistance`: the weld distance of `Indexify` of Jolt, `Indexify.h:14`; `weldTolerance` of `b3MeshDef`), in +a grid of cells of this size: a mesh exported as a soup of triangles has its edges shared again, else every edge would be +a border, and active. The triangles under 0.01 LinearSlop² of area are degenerate (`minArea` of `b3CreateMesh`, +`mesh.c:1590`): kept in `Indices` (the indices of the game don't move), out of the tree, without edge. The triangles +are counterclockwise seen from outside, the right-hand rule: the side of the normal is the side a body touches (Box3D +and Jolt take the same winding). + +**Edges** (`findEdges`): each edge is looked up by its 2 vertices in a map (`b3IdentifyEdges`, `mesh.c:1065`; +`sFindActiveEdges` of Jolt, `MeshShape.cpp:248`). An edge of 1 triangle (a border) is convex and active; an edge of 2 +triangles is convex if the opposite vertex of the other triangle is under the plane, and active if it is convex and bent +by more than 5° (the rule of the heightfield, `cellEdges`), or if the 2 triangles are back to back, closer than 1° to +opposite (`ActiveEdges::IsEdgeActive`, `ActiveEdges.h:21`: a sheet); an edge of 3 triangles or more is active (Jolt, +`MeshShape.cpp:313-319`). Bits per triangle as the heightfield: active, then convex. + +**Tree** (`buildNode`, `sahSplit`): a binary tree of AABBs built top-down, the triangles of a node split in 2 by the +surface area heuristic over binned centroids (`b3SplitBinnedSah`, `mesh.c:570`; `TriangleSplitterBinning` of Jolt; +the `BV4_SAH` strategy of PhysX, `GuBV4Build.h:62-67`, whose default splits at the center): on each axis the centroids +of the triangles fall in 8 bins (`B3_BIN_COUNT`), and the cost of a split after a bin is the count of the triangles on +each side times the area of their AABB; the cheapest split of the 3 axes wins. A leaf holds 4 triangles at most when a +split is found (`B3_DESIRED_TRIANGLES_PER_LEAF`; 8 in Jolt), 8 when none separates them (the centroids are the same, +`B3_MAXIMUM_TRIANGLES_PER_LEAF`), halves above. Added to Box3D: a split leaving less than a third of the triangles on a +side is replaced by the median along its axis, the rule of `btQuantizedBvh::sortAndCalcSplittingIndex` of Bullet +("if the splitIndex causes unbalanced trees, fix this by using the center"): the height of the tree is then bounded by +`log(triangles) / log(1.5)`, 52 for 2^31 triangles, and the traversals keep their nodes in an array of 64 on the stack +(`meshStackSize`; Box3D keeps 256 and asserts). The nodes are in depth-first order, the left child follows its parent; +the triangles of a leaf are contiguous in `order`. Not taken: the quantized nodes of Jolt (`NodeCodecQuadTreeHalfFloat`) +and of Bullet, the 4-wide nodes of PhysX (BV4): less memory and SIMD, for another day. Measured: 20 000 triangles of +hills give 12 053 nodes, 6 027 leaves, a height of 14 (bound 26). + +**Queries** (`OverlapTriangles`, `Cast`, `MeshCast`): the triangles whose AABB overlaps a box, by the nodes it overlaps +(`b3QueryMesh`, `mesh.c:2416`); the triangles a moving box meets, in the order of its motion: the segment of its center +against the AABBs of the nodes enlarged by its half sizes, by slabs, the closest child first, the nodes entered after +the limit dropped (`b3ShapeCastMesh`, `mesh.c:2070`, which orders the children by the axis of the split; the ray through +the trees of the broad phase of Feather, `aabbTree.cast`). `MeshCast` is an iterator with its stack inside, as `CellWalk` +of the heightfield: the sweeps and the rays run it without a callback. + +**Ray** (`CastRay`): `MeshCast` without size, Möller-Trumbore on each triangle (`RayTriangle.h` of Jolt, +`b3RayCastMesh` of Box3D), the side of the normal only (the determinant positive), a point up to `footprintSlack` out +of its triangle in barycentric coordinates so that a ray on an edge hits the triangles on both sides, and at the very +same fraction the triangle of the lowest index, as the heightfield. Measured on 100 352 triangles of hills: 1.2 µs per +ray (100 000 slanted rays, 96 % hits), 0.85 µs for vertical rays, against the 5 µs asked. + +**Collisions**: the triangles under the AABB of the body come from `OverlapTriangles`, then the generator of the +heightfield (above). Measured: 150 bodies (boxes, spheres, capsules) falling on the same hills as a heightfield of 41x41 +samples and as a mesh of 3 200 triangles: the narrow phase takes 2.5 ms per step on the heightfield, 2.4 ms on the mesh +(the step 6.9 and 6.5 ms). A sphere rolls on a tilted grid mesh within 3 µm of a sphere on a plane; a cube on a tread of +a staircase mesh, a cube overhanging the edge of a tread and a plank lying on the edges of 3 treads (active edges alone) +rest for 10 s without drifting by a micrometer, and sleep. ## Continuous collision -**Speculative contacts**: against a static body, the contacts are created up to `SpeculativeDistance` + the relative -speed of the bodies * dt (the speculative CCD of PhysX, the "Continuous Speculative" mode of Unity): the solver stops the -bodies before they touch. Between 2 dynamic bodies, only up to `SpeculativeDistance` (as Box2D v3): a fast impact is -absorbed by the spring of the contact over a few substeps. A rigid stop in one substep throws the light body of a -sandwich (a heavy body falling on a light one resting on the ground) and turns both bodies. Their known limits: a contact can be found by a body which will not touch it (a ghost contact), and a body -accelerated by the solver during the step can go further than its margin. - -**Time of impact** (as in Box2D v3): after the solver, a body which moved more than half of its smallest extent is moved -back to its first impact with a static body (a plane, a terrain...) along its motion, its velocity is kept. A bullet -(`IsBullet`) is also stopped by the dynamic bodies. The time of impact is found by conservative advancement -(Mirtich, as in Bullet): the body moves forward by its distance to the other body (GJK) divided by the fastest approach -of its points, until it is `LinearSlop` away. If it already touches at the start, only its core (a sphere of 1/4 of its -smallest extent, as in Box2D) is stopped. +Sources read: Box2D v3.1.0 (`src/solver.c`, `docs/simulation.md`, `src/constants.h`), Box3D (commit `9f998c8`, +`src/solver.c`, `include/box3d/types.h`), Jolt v5.3.0 (`Physics/PhysicsSystem.cpp`, `PhysicsSettings.h`, +`Body/MotionQuality.h`, `Docs/Architecture.md`), PhysX 5.6.0 (`ScCCD.cpp`, `PxsCCD.cpp`, `PxRigidBody.h`, +`PxSceneDesc.h`, the guide Advanced Collision Detection). + +**Speculative contacts**: against a static or a kinematic body, and for every pair of a fast body (below), the contacts +are created up to `SpeculativeDistance` + the relative speed of the bodies * dt: the solver stops the bodies before they +touch. It is the speculative CCD of PhysX, where the contact distance of a body is its linear speed * dt + its contact +offset + its angular speed * dt * the radius of its bounds (`Sc::BodySim::updateContactDistance`, `ScCCD.cpp:64-96`), +and the "Continuous Speculative" mode of Unity. PhysX applies it to the bodies flagged `eENABLE_SPECULATIVE_CCD` +(`PxRigidBody.h:99-105`); Feather to the static and kinematic bodies, and to the fast bodies, without a flag. Between 2 +dynamic bodies which are not fast, only up to `SpeculativeDistance` (as Box2D v3: `B2_SPECULATIVE_DISTANCE`, 4 linear +slops for every pair, `constants.h:38`): an impact is absorbed by the spring of the contact over a few substeps, where a +speculative row stops the bodies rigidly in the substep they touch. Their known limits: a contact can be found by a body +which will not touch it (a ghost contact), and a body accelerated by the solver during the step can go further than its +margin (PhysX documents it: "if the constraint solver accelerates an actor [...] such that the actor passes entirely +through objects during that time-step, speculative CCD can result in tunneling"). + +**Fast body**: a body which moves more than half of its smallest extent during the step, by the speed of its farthest +point (translation + rotation * its extent): the fast body of Box2D v3 (`maxVelocity * timeStep > 0.5f * +sim->minExtent`, `solver.c:584` & `:622`), of Box3D with its `safetyFactor` of 0.5 by default (`types.h:316-318`, +`solver.c:780`); Jolt casts a body which moves more than 0.75 of its inner radius (`mLinearCastThreshold`, +`PhysicsSettings.h:53`, `PhysicsSystem.cpp:1572-1573`). Feather judges it twice: from the velocities at the start of +the step for the margin of the narrow phase (`isFast`), from the motion the step made for the time of impact (as Box2D, +from its velocity at the end of the step). + +**Time of impact**: after the solver, a fast body is moved back to its first impact along its motion, its velocity is +kept: the contact of the next step stops it (Box2D v3, `b2SolveContinuous`; Jolt applies an impulse at the impact at +once, `sSolveCCDContact`, `PhysicsSystem.cpp:2093`, not followed: the solver of the next step has the contact, with its +friction and its restitution). The time of impact is found by conservative advancement (Mirtich, as in Bullet): the body +moves forward by its distance to the other body (GJK) divided by the fastest approach of its points, until it is +`LinearSlop` away. If it already touches at the start, only its core (a sphere of 1/4 of its smallest extent, as in +Box2D) is stopped. The fast body is swept against every body it collides with: the static bodies (Box2D), the kinematic +and the dynamic bodies too (the `LinearCast` motion quality of Jolt casts against every body; Box2D only for its +bullets, "Bullets will perform CCD with all body types, but not other bullets", `simulation.md:377-378`), where they +are at the end of the step (Jolt: "Body has already moved", `PhysicsSystem.cpp:1724`). A dynamic or a kinematic body +the fast body has a contact with this step is left to the solver: the speculative row holds the pair where they touch, +and a sweep of a solved pair would lift the fast body off the overlap the soft contact allows, every step. Measured: +swept too, the links of a swinging chain of 20 capsules (`joint chain` of the bench) stretched its joints by 214 mm +instead of 9 mm, the small cubes under the slab of `high mass ratio 2` drifted by 34 mm instead of 18. A static body +is always swept (Box2D), its speculative contact being the one of the start of the step: a resting body touches it at +the start and is only stopped by its core. **Two fast bodies** are swept once, by the one with the smallest state +index, with their relative motion against the start pose of the other (Jolt: "Get relative movement of these two +bodies", `direction = mShapeCast.mDirection - sCalculateBodyMotion(body2, ...)`, `PhysicsSystem.cpp:1884`), the +rotation of the other during the step ignored (as the linear cast of Jolt); both are stopped at the fraction found +(Jolt: "the other body will shorten its distance traveled", `:2054-2060`). The fractions of all the fast bodies are +found before any body moves: the result doesn't depend on the order of the bodies (Jolt sorts its CCD bodies by +fraction for the same reason, `:2017`). Feather has no bullet flag (`IsBullet` removed): every fast body is stopped by +every body, as the motion quality of Jolt stops the bodies which have it; Box2D keeps `isBullet` because its continuous +collision is against the static bodies by default. + +Measured (`TestFastSpheresFaceToFaceBounce`, `TestFastPlateNeverCrossesARestingPlate`, +`TestKickedBodyIsStoppedByTheContinuousCollision`): before, 2 spheres of 10 cm went through each other from 7.5 m/s of +relative speed (12.5 cm per step at 60 Hz: their diameter + the margin of 2 cm), 2 plates of 1 cm from 2.4 m/s (4 cm +per step: twice their thickness + the margin). With the margin of the fast pairs, 2 spheres at 20 m/s each come within +0.07 mm and bounce back at 10 m/s each (restitution 0.5), a plate at 30 m/s pushes the resting plate at 15 m/s without +overlap. The margin follows the speed of the start of the step: a light plate kicked at 40 m/s by a heavy ball during +the step goes through the plate 30 cm behind it (their pair had the margin of 2 resting bodies); the time of impact +stops it 0.8 mm from it. Cost: nothing measurable on the falling pile of 2 000 bodies of `BenchmarkWorldStep` (no body +of it is fast: 380 ms for 60 steps before and after, 1 worker, A/B interleaved); on the mixed scene of the determinism +tests (200 bodies, 42 of them fast at every step, bouncing in a pile), the step goes from 1.80 to 2.43 ms: the narrow +phase doubles (331 to 695 µs, the speculative contacts of the fast pairs), the continuous collision goes from 58 to +89 µs, the solver takes the rest (more contacts). PhysX documents the same price for its CCD: "As the objects' +velocities increase, the CCD overhead will increase, especially if there are a lot of high-speed objects in close +proximity". On the bench, the fingerprints of 13 scenes change (slope pile, terrain piles, rain on terrain, joint +chain, locked bodies, and card house, double domino, far chain, far stack, high mass ratio 2 and 3, joint grid, rush +of Solver2D): the worst stretch of the joint chain goes from 23.7 to 9.1 mm and the small cubes of high mass ratio 2 +and 3 sink 12 mm less into the ground, while they drift by 17.8 mm instead of 16.5 and the least push of the locked +bodies goes from -0.054 to -0.051 m, accepted; the reference was regenerated machine at rest. **GJK distance**: the distance and the closest points of 2 convex shapes. The simplex is reduced to its feature closest to the origin (Voronoi regions, Ericson 5.1 & 9.5). @@ -662,15 +1013,17 @@ should. Box3D gives no hit then (`distance.c:1044-1172`). whole AABB of the motion; the walk ends when the band enters cells after the best hit. A triangle is skipped if the shape moves along its normal: it comes from behind, or leaves (`b3ShapeCastHeightField` skips the triangles whose plane is over the center of the shape instead). +- **Mesh**: the triangles of the leaves of `MeshCast`, the nodes entered after the best hit dropped + (`b3ShapeCastMesh`); the same test of a triangle. - **Not taken**: the GJK ray cast of van den Bergen, of Jolt (`GJKClosestPoint.h:506`) and PhysX (`GuGJKRaycast.h:55`): one loop instead of a GJK per iteration, faster, but a new algorithm in `gjk/`. **Overlap** (`World.Overlap`): the candidates of the trees for the AABB of the shape, then the distance of the cores by -GJK against the sum of the radii; a plane by the lowest point of the shape; a heightfield by its triangles under the -shape, from any side. The contact counts. +GJK against the sum of the radii; a plane by the lowest point of the shape; a heightfield or a mesh by its triangles +under the shape, from any side. The contact counts. -**Order.** At the same fraction, the body of the lowest index in `World.Bodies` is hit, and on a heightfield the -triangle of the lowest index; `RaycastAll` is sorted by +**Order.** At the same fraction, the body of the lowest index in `World.Bodies` is hit, and on a heightfield or a mesh +the triangle of the lowest index; `RaycastAll` is sorted by fraction then by index, `Overlap` by index (Unity: "the order of the results is undefined"). The results don't depend on the shape of the trees, nor on `Workers`. @@ -680,3 +1033,331 @@ Box2D and Box3D a sensor is a shape filtered by its category. **Concurrency.** No lock: several readers or one writer, as the queries without lock of Jolt (`GetNarrowPhaseQueryNoLock`, `PhysicsSystem.h:124-125`). A query during a step panics, as Box3D refuses a locked world (`b3GetUnlockedWorldFromId`, `src/physics_world.c:95-109`). + +## Kinematic bodies + +A kinematic body goes where the game puts it, with the velocity of this motion, and pushes the dynamic bodies it meets; +nothing pushes it back. `kinematic.go`. + +**A target per step.** `RigidBody.SetKinematicTarget` stores the pose the body reaches at the end of the next step. At +the start of the step, the body takes the velocity of the motion to its target (`World.moveKinematics`): +```` +v = (target.position - position) / dt +ω = axis * angle / dt // of the rotation target ⊗ rotation⁻¹, on the shortest arc (W ≥ 0) +```` +It is the model of PhysX (`Sc::BodySim::calculateKinematicVelocity`, `ScKinematics.cpp:44-99`, 5.6.1: the same two +formulas, "we simply determine the distance moved since the last simulation frame and assign the appropriate delta to +the velocity. This vel will be used to shove dynamic objects in the solver"), whose kinematic actor has no velocity for +a step without target (`:97-98`) and is put on its target at the end of the step (`updateKinematicPose`, `:193-217`; +`PxRigidDynamic::setKinematicTarget`: "After the move is carried out during a single time step, the velocity is +returned to zero. Thus, you must continuously call this in every time step"). Jolt (`Body::MoveKinematic`, +`Body.cpp:81-95`, `MotionProperties::MoveKinematic`, `MotionProperties.inl:9-24`, v5.3.0) and Box2D +(`b2Body_SetTargetTransform`, `body.c:808-852`, v3.1.0) compute the same velocities, then keep them from a step to the +next and integrate the body with them: without a new call the body goes on, and a first order integration of the +rotation doesn't land exactly on the target. Feather follows PhysX: the target is dropped once reached, a body without +target stays, and the last sub-step writes the target bit for bit (`solver.finalizeBody`). A target on a body which is +not kinematic is refused with `actor.ErrNotKinematic`, never ignored (PhysX asserts "Body must be kinematic"). + +**Interpolation over the sub-steps.** The contacts are solved at each sub-step, and read the position of the body +(`currentSeparation`, from its `deltaPosition` and `deltaRotation`): at the sub-step k of n, the body is at +```` +Δp = (target.position - position) * k / n +Δq = slerp(identity, target ⊗ rotation⁻¹, k / n) = rotation of angle * k / n around the axis +```` +(`solver.moveKinematic`, `rotationFraction`). The slerp from the identity turns at a constant angular velocity: the +velocity ω the rows read is the velocity of the motion at every sub-step, and the body sweeps exactly the arc its +velocity says. The last sub-step takes the target itself, not the interpolation at k = n (cos and sin don't give the +quaternion back bit for bit). A body without target doesn't move: its deltas stay the identity. + +**No mass.** The state of a kinematic body in the solver (`bodyState`, `dynamic` false) has no inverse mass, no inverse +inertia and no lock: an impulse changes its velocity by λ M⁻¹ d = 0 ("Static and kinematic sims have zero mass", Box2D +`body.c:535-536`; "static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0", +`MotionProperties.h:94-95` of Jolt). The rows read its velocity and its motion (a contact against it is a contact +against a moving wall, a joint to it pulls the other body with it), and never write it: `jacobian.apply`, +`applyTwist`, `applyRolling`, `applyLinear` and `applyAngular` write a body only if it is dynamic, where they wrote +every body with a state before. Nothing else changes for a dynamic body: the arithmetic of a world without kinematic +bodies is the same bit for bit (the fingerprints of `bench/baseline.json` are unchanged). + +Not written, a kinematic body can be shared by the rows of a color: the graph coloring takes it as a static body +(`solver.dynamicIndex`). Box2D v3 colors it as a dynamic body, for a reason Feather doesn't have: "Unlike static bodies, +we cannot use a dummy solver body for kinematic bodies. We cannot access a kinematic body from multiple threads +efficiently because the SIMD solver body scatter would write to the same kinematic body from multiple threads" +(`constraint_graph.c:20-23`, v3.1.0, the same in Box3D). A kinematic body touching 200 bodies would otherwise put its +200 contacts in 200 colors, 16 colors and the overflow here. + +The trees of joints (`articulation.go`) take it as a static body too: a joint to a kinematic body is a leaf, and two +chains hanging from the same kinematic body are two trees (the kinematic body couples nothing: its mass terms are 0). +The contacts with a kinematic body keep the softness of the contacts between dynamic bodies, as Box2D v3.1 +(`contact_solver.c:1514`: the stiffer softness is for a body without solver state) and Box3D (`b3_contactStaticFlag`, +`contact.c:241`, for a static body only). + +**Pairs.** A kinematic body has no contact with a static or a kinematic body, as in every engine (Jolt +`Body::sFindCollidingPairsCanCollide`, `Body.inl:36-44`: "One of the bodies must be dynamic to collide"; Box2D +`simulation.md`: "A shape on a kinematic body can only collide with a dynamic body"; PhysX `PxRigidBody.h:56`: +"Kinematics will not collide with static or other kinematic objects"). The broad phase keeps it in the tree of the +dynamic bodies, with a proxy of its own kind (`proxyKinematic`): moved, it queries the dynamic tree, and the static +tree and the planes for the triggers only, and a pair is emitted if it has a dynamic body and an awake body which moves +(`needsSolving`), or a trigger ([Triggers](#triggers)). Box2D v3 gives the kinematic bodies a third tree for the same +rule ("Only dynamic proxies collide with kinematic and static proxies", `broad_phase.c:351`), its sensors query the 3 +trees; two trees and a kind on the proxy do the same job here, and a body which changes type (below) keeps its leaf, +its pairs and their contacts. + +**Speculative contacts from its speed.** The margin of a contact against a static body follows the relative speed of +the bodies (the speculative CCD of PhysX, see Continuous collision); the margin against a kinematic body does the same +(`World.collide`), where the pairs of dynamic bodies keep the 2 cm of Box2D. PhysX: "unlike the sweep-based CCD, it is +legal to enable speculative CCD on kinematic actors" (the guide, Advanced Collision Detection). A leg at 5 m/s moves +8 cm per step at 60 Hz: without it, the leg enters the tail it meets by 6 cm before the contact exists, and the spring +of the contact throws the tail. With it, the contact exists a step ahead, the speculative row brings the tail to the +speed of the leg within the sub-step they touch, and the leg enters it by 0.25 mm (`TestFastKinematicDoesNotGoThroughARestingBody`, +capsules and boxes). The kinematic body itself is never stopped by the continuous collision ("Kinematic bodies cannot be +stopped", Jolt `PhysicsSystem.cpp:1564`); a fast dynamic body meeting it is held by their speculative contact, and +stopped by the time of impact when they had none (see Continuous collision; Box2D sweeps its bullets only against the +kinematic bodies, `solver.c:440-446`). + +**Sleep.** A kinematic body is in the island of the bodies it touches, as in Box2D ("dynamic and kinematic bodies that +are enabled need a island", `body.c:313-317`) and Jolt (`IslandBuilder::LinkBodies` for every contact constraint, +`PhysicsSystem.cpp:1278`). It rests when its velocity is exactly 0: no target, or a target at its pose. On its way it +keeps its island awake, however slowly it goes; stopped, its timer runs like the others and the island sleeps together; +a target away from its pose wakes it up (`SetKinematicTarget`), and the island with it at the step. Box2D and Jolt apply +their sleep threshold to the kinematic bodies too (`b2FinalizeBodiesTask`, `solver.c:617-651`; +`MotionProperties::AccumulateSleepTime`): a platform slower than 5 cm/s falls asleep with its riders, and stops, since +their kinematic body is moved by a velocity. With a target per step, the platform would wake up at the next step: a +sleep and a wake event at every half second, and the riders losing their velocity. PhysX keeps a kinematic actor +awake as long as a target is set ("A kinematic actor is asleep unless a target pose has been set", +`PxRigidDynamic.h:150-152`): so does Feather. + +**SetBodyType**, between kinematic and dynamic, in place: the body keeps its index, its leaf in the tree (the proxy +changes kind at the next step, the pairs and their contacts stay), its joints, its filters and its island; it wakes up +with its island. It is `Body::SetMotionType` of Jolt (`Body.cpp:37-77`: the forces dropped for a kinematic body, the +velocities for a static one), which needs `mAllowDynamicOrKinematic` at the creation to switch a static body; PhysX +toggles `PxRigidBodyFlag::eKINEMATIC` on a `PxRigidDynamic`, a static actor being another class ("you do need to +provide a mass for the kinematic actor"); Box2D `b2Body_SetType` destroys the contacts of the body and recreates its +proxies (`body.c:1023-1260`). Feather refuses the static bodies (`ErrStaticBody`): a static body is the shape of the world. To +dynamic, the body keeps the velocity of its last motion (the ragdoll of a creature thrown by its animation) and needs +the mass of its shape (`ErrMasslessBody` without it): a kinematic body created with a density keeps it (`NewRigidBody`), +the solver never reads it while it is kinematic. To kinematic, the body stops and drops its forces; its locks wait for its next dynamic life. + +**Teleport** places any body without velocity (`World.Teleport`): the target is dropped, a kinematic body stops, a +dynamic body keeps its velocity; the body and the sleeping bodies at the new place wake up, and the trees take it at +the next step or at `SyncQueries`. A kinematic body teleported against a resting body pushes nothing +(`TestTeleportGivesNoVelocity`), brought there by a target it pushes. PhysX on `setGlobalPose`: "the kinematic actor +would not push away other dynamic actors in its path, instead it would go right through them. The setGlobalPose() +function can still be used though, if one simply wants to teleport a kinematic actor to a new position". + +**Measured** (`TestKinematicPushesABox`, `TestFastKinematicDoesNotGoThroughARestingBody`, 60 Hz, 8 sub-steps): a cube +pushed at 1 m/s on the ground (µ = 0.6) stays against the pusher within 0.00 mm, 0.12 mm deep at worst, at 1.000 m/s; a +leg at 5 m/s enters a resting capsule by 0.25 mm at worst, a box by 0.22 mm, and never goes through. + +## Changing a body + +**SetShape** gives a body of the world another shape, in place (`World.SetShape`, `shape.go`), as +`BodyInterface::SetShape` of Jolt (`BodyInterface.cpp:298-326`, v5.3.0, with the mass properties updated and the body +activated): `RigidBody.SetShape` replaces the shape, recomputes the mass and the inertia of a dynamic or a kinematic +body at the density of its material (`Body::SetShapeInternal`, `MotionProperties::SetMassProperties`; a static body +keeps its infinite mass) and the AABB; the world then puts the body in `changed`, the list of the heightfields updated +during the step, so that its pairs skip the pair cache at the next step (`BodyManager::InvalidateContactCacheForBody` +of Jolt: the cache of the body is ignored for one step; the pair cache of Feather would move the four corners of a cube +made a sphere with the bodies, `TestSetShapeComputesTheContactsAgain`), wakes the body with its island and the sleeping +bodies within the union of its old and its new AABB (the rule of `Teleport` and `UpdateHeightfield`: a static pedestal +shrunk under a pile lets it fall, a slab grown under a crate lifts it, a trigger made its inscribed sphere tests again +the overlap of the crate asleep in its corner; Jolt leaves the neighbours asleep). The broad phase moves the proxy to +the tree of its new kind at the next synchronization (`Tree.update`: a plane made a box leaves the planes for the +static tree). The index, the joints, the layers, the ignored pairs, the velocities, the target and the locks are kept. + +The shapes of Feather are centered on their origin (the hull on its center of mass): the body stays where it is, +where Jolt moves it by the difference of the centers of mass (`Body::UpdateCenterOfMassInternal`); the caller places +the body where the new shape belongs. A body overlapping something with its new shape is pushed out by the spring of +its contacts over the next steps, at `ContactSpeed` at most: a resting cube made a box twice as big rises to its new +height without going through the ground (`TestSetShapeOfARestingBodyKeepsItStable`). PhysX (`PxShape::setGeometry`, +`PxShape.h:122-133`, v5.6.0) keeps the type of the geometry ("It is not allowed to change the geometry type of a +shape") and "does not guarantee correct/continuous behavior when objects are resting on top of old or new geometry"; +Feather changes the kind and keeps the resting bodies stable. A plane, a heightfield or a mesh on a dynamic or a +kinematic body is refused (`ErrShapeNotConvex`): a surface has no support point for GJK and no volume for a mass (the +complex collision of Unreal "cannot simulate the object", the concave shape of Godot is "intended to work with static +CollisionShape3D nodes"). No shape is refused (`ErrNoShape`); the same shape again does nothing. + +**SetDensity** gives a body another density, in place (`World.SetDensity`): the mass and the inertia of its shape at +this density (`PxRigidBodyExt::updateMassAndInertia` of PhysX, `ExtRigidBodyExt.cpp:282-290`, which the game calls after +a change of density or of geometry, where Jolt recomputes them in `SetShape`), the body woken up with its island; a +static body keeps the density written, for the day it becomes dynamic, and its infinite mass; a density which is not +positive on a dynamic body is refused (`ErrMasslessBody`). The solver reads the mass and the inertia of each body at +`prepare` (`solver.go`): nothing else is copied, a change is seen by the next step. + +Neither call allocates once the world has stepped; the steps after a change allocate nothing +(`TestStepAfterSetShapeAllocatesNothing`); the mixed scene with shapes and densities changed during the run gives the +same bits with 1, 4 and 8 workers (`TestSetShapeIsDeterministic`). No scene of the bench changes a shape: the 33 +fingerprints are unchanged. + +## Characters + +A character is a capsule moved by the game between 2 steps, with the queries of the world: a virtual character, the +`CharacterVirtual` of Jolt (`Jolt/Physics/Character/CharacterVirtual.cpp`, v5.3.0), the mover of Box2D v3 +(`src/mover.c`, `samples/sample_character.cpp`, v3.1.0). `PxController` of PhysX +(`physxcharacterkinematic/src/CctCharacterController.cpp`, 5.11) and `CharacterBody3D` of Godot +(`scene/3d/physics/character_body_3d.cpp`, 4.4) are of the same kind. `charactervirtual.go`, `charactervirtual_move.go`. +The name is the one of Jolt: Jolt keeps `Character` for its character on a rigid body, moved by the solver, which +Feather doesn't have. + +**Why virtual.** A dynamic body as a character is pushed by the solver: it bounces, it is thrown by a contact, it +needs its rotations locked and a friction tuned; a kinematic body goes through the walls. A virtual character goes +exactly where the game decides, at once, and is stopped by the walls: every engine offers one. Feather follows Jolt, +whose character slides its velocity along the planes of its contacts by time of impact and checks the path by a +sweep, where PhysX moves by 3 sweeps (up, side, down: the up pass makes the steps, `moveCharacter`, `:1682-2005`), +Box2D v3 solves the planes in position (`b2SolvePlanes`, Gauss-Seidel on the pushes, then `b2ClipVector` on the +velocity) and Godot slides the remainder of a motion test (`_move_and_slide_grounded`, 6 slides at most). + +**The inner body.** The character stands in the world through an inner kinematic body at its capsule (the +`mInnerBodyShape` of Jolt, `CharacterVirtual.h:50-55`; the kinematic actor of PhysX, `CctController.cpp:111-170`), +teleported there after each update without velocity (`UpdateInnerBodyTransform`, `:165-169`: `SetPositionAndRotation`, +`DontActivate`). The rays, the sweeps and the overlaps see it; a dynamic body which runs into it is stopped by the +solver (infinite mass); the other characters gather it in their contacts and hit it in their sweeps, with the velocity +of its character (`sFillCharacterContactProperties`, `:232-245`): the `CharacterVsCharacterCollision` list of Jolt is +not needed. It pushes nothing by itself: PhysX moves its actor by `setKinematicTarget` (`:2545-2555`), so that it +shoves the dynamic bodies with an infinite force; Feather bounds the push by the strength of the character, as Jolt. + +**Padding.** The capsule is inflated by `CharacterPadding` (2 cm, `mCharacterPadding`) for its contacts and its +sweeps: the character keeps 2 cm from everything, "this ensures that the sweep will hit as little as possible lowering +the collision cost and reducing the risk of getting stuck" (`CharacterVirtual.h:46`). Jolt collides the real shape +and removes the padding from the distances (`GetContactsAtPosition`, `:430-434`), then casts the shrunken shape and +corrects the fraction (`GetFirstContactForSweep`, `:552-640`): for a capsule, inflating the radius is the same thing, +exactly. The inner body has the real capsule. PhysX stops its sweeps `contactOffset` before the obstacle (0.1 m, +`doSweepTest`, `:1638-1641`). + +**Contacts.** `collectContacts`: the candidates of the trees for the AABB of the padded capsule enlarged by the +predictive distance (10 cm, `mPredictiveContactDistance`: "A value of 0 will most likely cause the character to get +stuck as it cannot properly calculate a sliding direction anymore", `.h:41`), filtered as the pairs of the world +(`World.ShouldCollide`), the triggers skipped, then the contact generator of the world (`collideAllMoving`: GJK/EPA, +the analytic pairs, the patches of the surfaces) with the predictive distance as margin: one contact per manifold, its +deepest point, its normal towards the character, its distance (the padding removed), the velocity of its body at the +point (the velocity of the other character). The contacts are sorted by the index of their body (Jolt sorts them, +`:420-424`): the same whatever the shape of the trees. On an inactive edge of a surface, the normal of EPA is kept +when it brakes the motion less than the normal of the triangle (`contactNormal`, the hint of `ActiveEdges::FixNormal` +of Jolt: the character passes the direction of its velocity, `CheckCollision`, `:377-404`): a capsule level with the +top edge of a riser has its closest point on the flat diagonal of the riser, 10 cm away along the normal of EPA; with +the normal of the riser it was a wall at a distance of 0, and the character stuck at the edge of every step. The pairs +of a step give no direction: their arithmetic is unchanged. + +**Conflicting contacts.** 2 contacts of the same body deeper than 1.25 × padding with opposed normals (the character +squeezed in the body) can't both be solved: the shallower one is discarded, and the body ignored by the sweep +(`RemoveConflictingContacts`, `:436-473`). + +**Constraints.** Each contact is a plane `n · x + d ≥ 0` with the velocity of its body (`DetermineConstraints`, +`:642-682`); a contact which enters the body (`d < 0`) gets the velocity `-n d / dt` out of it (the penetration +recovery, all of it in one update); a contact too steep (`n · up < cos(MaxSlopeAngle)`, `IsSlopeTooSteep`, +`CharacterBase.h:73-78`) gets a second, vertical plane (its normal in the horizontal plane, its distance the one to +travel horizontally to the contact) which blocks the way up the slope. + +**Solve** (`solveConstraints`, `SolveConstraints` of Jolt, `:755-1010`): 15 iterations at most. The time of impact of +the velocity on each plane is `toi = (n · displacement + d) / (n · (v_plane - v))`, infinite when the character moves +away or would enter the plane by less than 0.1 mm during the time left; the planes are sorted by it (at `toi = 0`, +the one which pushes the most first; at a tie, a static body first). The character goes to the first plane, hits it +(`handleContact`: the body gets its impulse), and its velocity is projected on the plane: `v - ((v - v_plane) · n) n`; +on a steep slope the velocity towards it is removed first, else the character climbs a little and jitters. If the new +velocity would enter a plane hit earlier in this solve (the planes closer than 10° apart excepted), the character +slides along the edge of both: `v · (n₁ × n₂)` on the edge, plus the velocities of both planes perpendicular to it, +each plane losing its push into the other (no ping-pong). The solve stops when nothing pushes and no velocity is left, +or when the velocity reversed. Standing without horizontal velocity on a walkable, still surface, the character stops +dead instead of creeping down it (`OnContactSolve` of the samples of Jolt, `CharacterVirtualTest.cpp:383-391`; +`floor_stop_on_slope` of Godot): the velocity of the game then holds the gravity of the step alone. + +**Move** (`moveShape`, `MoveShape` of Jolt, `:1203-1287`): contacts, constraints, solve, then a sweep of the padded +capsule along the displacement (`sweep`): the bodies it starts in or touches are passed through (their planes were +solved: the hits at the fraction 0 are ignored, `ContactCastCollector::AddHit`, `:328-376`; `b2World_CastMover` of +Box2D ignores the overlapping shapes too, `world.c:2446-2451`), a hit which would enter the body by less than the +collision tolerance (1 mm) is ignored, the others shorten the displacement. 5 loops at most, while time remains +(`characterMinTimeRemaining`, 1e-4 s) and the character moves. The sweep is the one of the world (conservative +advancement on the cores), through the bodies the character collides with. + +**Push** (`handleContact`, `HandleContact` of Jolt, `:684-753`): a dynamic body hit receives the impulse which brings +it to the velocity of the character along the normal, damped by 0.9, plus 0.4 of the penetration per update: +`P = Δv / (1/m + (r × n) · I⁻¹ (r × n))`, bounded by `MaxStrength × dt` (100 N), its downward part removed (the gravity +is the world's). `Update` adds the weight of the character to a dynamic ground, `-Mass (n · g) / |g| dt g` at the +ground point (`:1404-1416`). The body woken by the impulse wakes its island at the step. + +**Ground** (`updateSupportingContact`, `UpdateSupportingContact` of Jolt, `:1012-1190`): a contact closer than the +collision tolerance which the character doesn't leave is a collision; a contact holds the character if its point is +in the lower sphere of the capsule (the supporting volume of the samples of Jolt, `Plane(Y, -radius)`: "Accept +contacts that touch the lower sphere of the capsule", `CharacterVirtualTest.cpp:35`). On the ground if a holding +contact is under the slope limit; on steep ground with steep holding contacts only, unless they block the character +together (the solve of a fall of 1 s moves it by less than 0.6 × dt: a crevice holds); not supported with a contact +which doesn't hold; in the air without any. The ground normal and velocity are the average of the contacts within +85° of up; the ground body is the one of the most upright holding contact, else the deepest. On a kinematic platform +the ground velocity is the one of the point under the character over the last step, on its arc when the platform turns +(`CalculateCharacterGroundVelocity`, `:190-207`); `UpdateGroundVelocity` reads it again without any collision test. + +**Stick to the floor** (`stickToFloor`, `StickToFloor`, `:1686-1712`): leaving the ground without going up +(`ExtendedUpdate`, `:1740-1750`), the character sweeps `StickToFloor` down (0.5 m by default, the +`mStickToFloorStepDown` of the `ExtendedUpdateSettings` of Jolt; `floor_snap_length` of Godot is 0.1 m, a setting of +the body too) and lands on the floor found, its contacts read there. 0 sticks to nothing: off a platform of 1 m the +default doesn't reach the floor and the character falls, 1.5 m puts it on the floor at once. + +**Stairs** (`walkStairs`, `WalkStairs`, `:1545-1684`; `ExtendedUpdate`, `:1752-1792`): when the horizontal step +achieved is shorter than the step wanted and a contact too steep is pushed into (`canWalkStairs`, `:1523-1543`), the +character sweeps up by `StepHeight`, moves forward by what it lacked (2 cm at least, `mWalkStairsMinStepForward`), +cancels if it gained less than 5 % of the step against the steep contacts, sweeps down by as much as it went up, +cancels without floor, and when the floor found is too steep (the edge of the step) tests it 15 cm further along the +ground normal in the horizontal plane (or along the walk when they differ by more than 75°, +`mWalkStairsStepForwardTest`, `mWalkStairsCosAngleForwardContact`). Then it lands on the floor, on the ground. PhysX +makes its steps of its up pass (`stepOffset`); Godot has none. + +**Steep slopes.** Before the move, the horizontal velocity towards the steep contacts of the last update is removed +(`CancelVelocityTowardsSteepSlopes`, `:1297-1323`) when the character is on steep ground or not supported: it doesn't +try to climb. On a slope too steep, the gravity of the recipe slides it down (`OnSteepGround`: "The caller should +start applying downward velocity if sliding from the slope is desired", `CharacterBase.h:86`; the +`ePREVENT_CLIMBING_AND_FORCE_SLIDING` of PhysX, `PxController.h:60-67`). + +**Constants** (`charactervirtual.go`), those of Jolt: padding 0.02 m, predictive distance 0.1 m, collision tolerance +1e-3 m, accepted penetration 1e-4 m, 5 collision loops, 15 constraint iterations, 1e-4 s left, penetration recovery 1, +minimal step forward 0.02 m, forward test 0.15 m at cos 75°, push damping 0.9 and penetration 0.4, ground normals +within cos 0.08, planes apart by cos 0.984, progress 5 %. Defaults of the settings: a slope of 50° (45° in PhysX, +0.707, and in Godot), a step of 0.4 m (0.5 in PhysX), 100 N, 70 kg, the floor kept within 0.5 m (`StickToFloor`). A +normal within 1e-6 of up gets no vertical plane (the rounding of a vertical normal under a slope limit of 0). + +**Measured** (`charactervirtual_test.go`, 60 Hz, a capsule of 0.3 × 1.7 m at 2 m/s): +- on a plane, a bumpy terrain of 0.5 m cells and a hilly mesh: the real capsule is never in the ground (0.000 mm over + 480 frames), the character stays supported, and stopped it doesn't move (0.0000 mm over 2 s); +- against a wall at 45°: 20.00 mm from it (the padding), 5.657 m along it in 4 s (2 × 4 / √2 = 5.657, nothing lost); +- a slope of 30°: climbed 2.48 m in 3 s (2 × 3 × sin 30° × cos 30° = 2.60, less the gravity of the recipe); 49°: + climbed; 51° and 60°: not climbed, slid down. Steps of 0.2, 0.3, 0.39 and 0.41 m: walked (0.41: the sphere of the + capsule rolls over the edge); 0.6 m: blocked at the riser; +- down a slope of 30° at 3 m/s: 0 frame off the ground of 180; off a step of 0.3 m: 0 frame in the air of 150; +- a platform at 1 m/s sideways: carried by 3.000 m in 3 s, bit for bit; at 0.5 m/s up: 1.508 m, a frame ahead (the + first step put the platform 8 mm in the padding, the penetration recovery added its push to the velocity of the plane); +- a ball of 11 kg and a crate of 22 kg are pushed at 1.99 m/s; a crate of 8 t stops the character (0.18 m: the walk to + it); a crate of 8 t thrown at 4 m/s at a standing character is stopped by its inner body; +- 2 characters head on stop 53 mm apart (2 paddings + the accepted penetration); crossing, they pass each other at + 21 mm (2 paddings); +- the same bits with 1 and 8 workers (24 characters crossing a crowded scene); 0 allocation per update once the + buffers of the characters have grown (the scene walked again by the same characters, 2 collections before each + frame): the buffers of the contacts and of the sweeps of a character are its own, not those of a `sync.Pool`, which + the garbage collector empties, and an update which found it empty grew its buffers again; +- cost, one core (`BenchmarkCharacters`, machine at rest): 24 characters walking alone on the terrain 0.27 ms per + frame (11 µs each), 24 characters crowded against walls and crates 1.3 ms (54 µs each), the frame of the crowd with + its step 1.4 ms; 200 characters: 6.3 and 9.8 ms — before the work below, which takes 13 % off. + +**Where an update spends its time**, counted on the bumpy terrain (24 characters, per update): alone, 1.5 gatherings +of contacts testing 14 triangles with GJK (6.4 give a contact), 1.9 sweeps over 7.8 triangles (1.5 hit); against the +walls of the crowd, 2.0 gatherings of 23 triangles (10 contacts), 2.3 sweeps over 6.9 triangles. These counts are +the ones of Jolt: against a wall, `ExtendedUpdate` tries `WalkStairs` at every update too (a sweep up, a move with its +contacts, and the walk is cancelled); PhysX makes its up, side and down passes at every move. The cost is the one of +GJK per triangle within reach, and of the contact it makes. Three things were taken off it, each identical to the +bit (the 33 fingerprints of the bench, the character tests): the supports of GJK skip the two rotations of a shape +at the identity (a triangle of a surface, a standing capsule: `Proxy.upright`), the cores of GJK are made once per +pair instead of once per triangle (`penetrationOfCores`), and the proxy of the triangle of a sweep or an overlap once +per scratch. Measured against the version before them, interleaved on the same loaded machine (A/B, 3 rounds, the +best of each): the 24 characters alone 787 → 677 µs per frame (−14 %), the crowd of 24 1 063 → 925 µs (−13 %), its +frame with the step 1 218 → 1 073 µs (−12 %), 200 characters alone 7.64 → 6.58 ms (−14 %), the crowd of 200 +9.98 → 9.07 ms (−9 %). + +Two things the references do were measured and not kept. The rejection of a triangle by the exact distance of the +segment of the capsule before any contact (`PCMCapsuleVsMeshContactGeneration::processTriangle` of PhysX, by +`pcmDistanceSegmentTriangleSquared` against the inflated radius; `rejectTriangle` of `sweepCapsuleTriangles` for the +sweeps): it rejected 7.6 of the 14 triangles alone and 10 of the 23 in the crowd before GJK, and gained nothing +(0 to −2 %): GJK on a triangle out of reach converges in 1 or 2 iterations, as cheap as the exact distance (5 closest +points) in scalar Go. The geometry gathered once per move (`findTouchedGeometry` of `CctCharacterController` into +`mGeomStream`, within the swept volume grown by `mVolumeGrowth`): the triangles around the character gathered once +per update for all its gatherings and sweeps gathered 22 to 24 triangles where the queries fetched about 30 (2 +gatherings and 2.3 sweeps: a reuse of 1.3), and the gathering cost what the fetching saved (12 % against 14 %, within +the noise of the bench). What remains is GJK on the 6 to 10 triangles which touch the capsule and the 2 to 3 the +sweeps hit, 1 to 1.5 µs each: the floor of a contact generator which is GJK/EPA only. PhysX goes 3 to 5 times lower +on a capsule against a mesh with its analytic contact of a segment and a triangle, a second mechanism of contact. diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md index c7ab0cd..d36c230 100644 --- a/ARCHITECTURE.md +++ b/ARCHITECTURE.md @@ -8,21 +8,26 @@ feather/ ├── graph.go # graph coloring of the contacts and the joints, for the parallel solver ├── pool.go # workers of the step ├── island.go # sleep islands +├── kinematic.go # kinematic bodies: the motion to their target, SetBodyType, Teleport +├── shape.go # the shape or the density of a body changed in place: SetShape, SetDensity +├── charactervirtual.go # characters: Update (slide, ground, stick to the floor, stairs), the inner body +├── charactervirtual_move.go # the move of a character: contacts made planes, solver by time of impact, sweep, push ├── joint.go # joints: distance, ball, hinge, fixed ├── joint_configurable.go # configurable joint: each axis locked, limited or free ├── articulation.go # the anchors of the trees of joints, solved together (Baraff 1996) ├── collision.go # BroadPhase, NarrowPhase, Collide ├── collision_capsule.go# spheres & capsules: closest points of segments -├── collision_heightfield.go # heightfields: triangles, inner edges, patches +├── collision_triangles.go # heightfields & meshes: the triangles under the body (faces, edges), patches ├── ccd.go # continuous collision: time of impact of the fast bodies ├── tree.go # broad phase: dynamic AABB trees, the pairs kept from a step to the next ├── filter.go # collision filtering: layers & masks, ignored pairs, linked bodies, the filter of the queries ├── query.go # queries: Raycast, RaycastAll, SyncQueries, the guard, the traversal of the trees by a segment ├── query_sweep.go # Sweep: conservative advancement on the cores, against a plane, a convex body, a triangle ├── query_overlap.go # Overlap -├── query_heightfield.go# a sweep & an overlap against the triangles of a heightfield +├── query_triangles.go # a sweep & an overlap against the triangles of a heightfield or of a mesh ├── event.go # collision, trigger & sleep events -├── actor/ # RigidBody, Material, Transform, shapes (Sphere, Box, Plane, Capsule, Heightfield), their CastRay +├── actor/ # RigidBody, Material, Transform, shapes (Sphere, Box, Plane, Capsule, Heightfield, TriangleMesh, +│ # ConvexHull), their CastRay; convexhull.go (quickhull), trianglemesh.go (the tree of a mesh) ├── constraint/ # Manifold, ContactPoint, friction & restitution mixing ├── gjk/ # GJK (overlap test with margin, distance) ├── epa/ # EPA (penetration depth) & contact points (manifold) @@ -32,15 +37,19 @@ feather/ ## World.Step ``` Step(dt) -├── wake the sleeping bodies touched by a moving body +├── the kinematic bodies take the velocity of the motion to their target +├── wake the sleeping bodies touched by a moving body (dynamic, or a kinematic body on its way) ├── Phase 1: collision detection (once per step) │ ├── AABBs enlarged by the distance each body can travel during dt │ ├── broad phase: pairs of overlapping AABBs (AABB trees) which pass the collision filters │ ├── narrow phase: manifold of each pair (parallel, Workers goroutines) │ ├── a sleeping body touched by an awake body wakes up: the detection runs again -│ ├── events: pairs touching or overlapping (triggers are not solved) +│ ├── events: pairs touching or overlapping (triggers are not solved: a pair with a trigger is tested whatever the +│ │ sleep, its overlap kept while both bodies rest and their AABBs don't change; a contact whose bodies both rest is +│ │ kept until they move apart or one is removed) │ └── warm start: each point takes the impulses of the closest point of the pair in the previous step -├── Phase 2: solver (substeps: articulations, then contacts and joints by color), then restitution +├── Phase 2: solver (substeps: articulations, then contacts and joints by color; the kinematic bodies interpolated to +│ their target), then restitution ├── continuous collision: the fast bodies are moved back to their first impact (with the bodies they collide with) └── Phase 3: sleep islands, the trees updated for the queries, then the events ``` @@ -49,7 +58,8 @@ Step(dt) | Pair | Method | |------|--------| | any shape - plane | `CollideWithPlane` of the shape | -| any shape - heightfield | each triangle under the body: GJK + EPA, inner edges, patches (up to 8 manifolds) | +| any shape - heightfield or mesh | each triangle under the body, seen from the side of its normal: the vertices of the body over its face, GJK + EPA for its edges; patches by normal (up to 8 manifolds). One generator for both surfaces (`collideTriangles`), which differ by the triangles they give: the cells of the grid, the leaves of the tree | +| convex hull - any shape | GJK + EPA on its vertices (the support point among all of them), its faces for the contact points | | sphere / capsule - sphere / capsule | closest points of the segments (a sphere is a segment of length 0) | | sphere / capsule - other shape | GJK distance between the core (a point, a segment) and the shape, plus the radius; EPA only if the core is inside | | other pairs | GJK + EPA, then clipping of the contact points | @@ -59,9 +69,10 @@ points were computed, the previous contact points are moved with the bodies, the The contacts of a pair are kept by its pair in the broad phase: no lookup. Contacts are kept up to a margin: `SpeculativeDistance` (2 cm), + the relative speed of the bodies * dt against a -static body. -Each manifold has a normal (from A to B) and up to 4 points. A pair has 1 manifold, up to 8 against a heightfield -(the manifolds of a pair follow each other). Each point has its own separation (< 0 when the bodies overlap). +static or a kinematic body, and for the pairs of a fast body (a body which can move more than half of its smallest +extent during the step, `isFast`). +Each manifold has a normal (from A to B) and up to 4 points. A pair has 1 manifold, up to 8 against a heightfield or a +mesh (the manifolds of a pair follow each other). Each point has its own separation (< 0 when the bodies overlap). ## Collision filtering `filter.go`. A pair is emitted by the broad phase (`Tree.scan`) only if it passes 2 filters, in this order: @@ -79,8 +90,8 @@ in the records of the broad phase (its stored AABBs still overlap) and is emitte | Where | What the filter does | |-------|----------------------| | broad phase | a filtered pair is not emitted: no narrow phase, no manifold, no contact | -| triggers | a trigger is a body like the others in the broad phase: a filtered pair sends no trigger event | -| continuous collision | `stopAtImpact` skips the bodies the fast body doesn't collide with (`World.ShouldCollide`) | +| triggers | a trigger is a body like the others in the broad phase: a filtered pair sends no trigger event. A pair of a trigger and a dynamic or kinematic body is emitted whatever the sleep (ALGORITHMS.md#triggers) | +| continuous collision | `findImpact` skips the bodies the fast body doesn't collide with (`World.ShouldCollide`) | | sleep | a filtered pair has no contact: it wakes nobody up and links no island. `RemoveBody` and `UpdateHeightfield` only wake the sleeping bodies which collide with the body | | queries | `QueryFilter{Mask, Excluded}.Accepts(body)`: the layer of the body is in the mask of the query, and the body is not excluded. The mask of the body is not read | @@ -93,9 +104,9 @@ A body of empty mask (`actor.NoLayers`) collides with nothing but keeps its leaf | Query | Planes & heightfields (in no tree) | Trees (static, then dynamic) | A body | |---|---|---|---| -| `Raycast`, `RaycastAll` | each one, first | the segment against the AABB of the nodes, the closest child first | `CastRay` of its shape, in its local space | -| `Sweep` | each one, first | the same, the AABBs enlarged by the half sizes of the shape | conservative advancement on the cores (GJK distance) | -| `Overlap` | each one | `Tree.queryCandidates` with the AABB of the shape | GJK distance of the cores against the radii | +| `Raycast`, `RaycastAll` | each one, first | the segment against the AABB of the nodes, the closest child first | `CastRay` of its shape, in its local space (a mesh: the same walk through its own tree) | +| `Sweep` | each one, first | the same, the AABBs enlarged by the half sizes of the shape | conservative advancement on the cores (GJK distance); a mesh: its triangles along the path, through its tree | +| `Overlap` | each one | `Tree.queryCandidates` with the AABB of the shape | GJK distance of the cores against the radii; a mesh: its triangles under the shape | **The trees are up to date between 2 steps.** A step updates them at its start, for its pairs, and once more at its end, after the continuous collision and the sleep: the AABB stored for each body then contains its AABB. The end of a @@ -115,9 +126,45 @@ care"). A query started while `Step` runs panics with `feather: query during Ste refuses a locked world the same way). The listeners of the events run after the trees are updated and the guard is lifted, on the goroutine of `Step`: they can run queries, and see the end of the step. +## Kinematic bodies +`kinematic.go`, see [ALGORITHMS.md](ALGORITHMS.md#kinematic-bodies). A kinematic body (`actor.BodyTypeKinematic`) is +moved by a target pose per step (`RigidBody.SetKinematicTarget`), reached at the end of the step; its velocity is the +one of this motion. Where it appears: + +| Where | What | +|-------|------| +| start of the step | `World.moveKinematics`: the velocity of the motion to the target, 0 without target | +| broad phase | in the tree of the dynamic bodies, with a proxy of its own kind: it queries the static tree and the planes for the triggers only. A pair needs a dynamic body and an awake body which moves (`needsSolving`), or a trigger (`detectsTrigger`): no contact with a static or a kinematic body | +| narrow phase | the speculative margin of its pairs follows the relative speed, as against a static body | +| solver | a `bodyState` without mass (`dynamic` false): the rows read its velocity and its motion, never write it; colored as a static body | +| sub-steps | `solver.moveKinematic`: position interpolated linearly, rotation along the shortest arc; the last sub-step is the target, bit for bit | +| islands & sleep | in the island of the bodies it touches; resting when its velocity is exactly 0 (no target, or a target at its pose) | +| continuous collision | never stopped; a fast dynamic body meeting it is held by their speculative contact, and stopped by the time of impact when they had none | +| events, queries | the collision events with the dynamic bodies, the trigger events with every trigger (a static trigger, a kinematic trigger and the static or kinematic bodies); seen by the queries like any body | +| `World.SetBodyType` | kinematic ↔ dynamic in place: index, proxy, pairs and contacts, joints, filters and island kept; a static body or a body without mass is refused with an error (`ErrStaticBody`, `ErrMasslessBody`), as `SetKinematicTarget` on a body which is not kinematic (`actor.ErrNotKinematic`): a misuse is never silent | +| `World.Teleport` | any body placed without velocity, the sleeping bodies at the new place woken up | +| `World.SetShape`, `World.SetDensity` | another shape or density in place (`shape.go`): index, proxy, joints, filters and island kept, the mass and the inertia of the shape at the density, the pairs computed again at the next step, the body and its sleeping neighbours woken up; a surface (plane, heightfield, mesh) on a dynamic or a kinematic body is refused (`ErrShapeNotConvex`), no shape too (`ErrNoShape`), a density which is not positive on a dynamic body too (`ErrMasslessBody`) | + +## Characters +`charactervirtual.go`, `charactervirtual_move.go`, see [ALGORITHMS.md](ALGORITHMS.md#characters). A `CharacterVirtual` is a capsule moved +by the game between 2 steps (`CharacterVirtual.Update`), with the queries of the world: no rigid body moves it (the +`CharacterVirtual` of Jolt). Where it appears: + +| Where | What | +|-------|------| +| `World.AddCharacter` | an inner kinematic body at the capsule, in `World.Bodies` and in `World.characters` (by body): the queries, the dynamic bodies and the other characters see the character through it | +| `CharacterVirtual.Update` | between 2 steps, as a query (it panics during a step). `cancelVelocityTowardsSteepSlopes`, `moveShape`, `updateSupportingContact`, the weight on a dynamic ground, `stickToFloor`, `walkStairs`, then `placeBody` | +| `moveShape` | 5 loops at most: `collectContacts` (the candidates of `Tree.queryCandidates` for the padded capsule within the predictive distance, `World.ShouldCollide`, the contact generator with the direction of the motion, `collideAllMoving`), `removeConflictingContacts`, `determineConstraints`, `solveConstraints`, `sweep` | +| contact generator | `collideAllMoving` gives the direction of the motion to `collideTriangles`: on an inactive edge the normal of EPA is kept when it brakes the motion less than the normal of the triangle (the hint of `ActiveEdges::FixNormal` of Jolt, `contactNormal`). The pairs of a step give no direction: their bits are unchanged | +| `sweep` | `sweepQuery` with `treeQuery.character`: the bodies of the pair rules, the excluded ones (the inner body, the discarded contacts), the hits at the fraction 0 and the ones which enter by less than the collision tolerance ignored | +| `placeBody` | the inner body at the capsule, without velocity, its leaf of the broad phase updated (`Tree.update` at its index, checked), woken with its island when it moved | +| `World.RemoveBody` | forgets the character of the body | +| the step | the inner body is a kinematic body without target: no velocity, no pair with a static or a kinematic body, in the island of the dynamic bodies it touches, which are stopped by it (infinite mass) | + ## Solver -See [ALGORITHMS.md](ALGORITHMS.md#solver). The solver works on copies of the dynamic bodies (`bodyState`): -the static and sleeping bodies share a state with no mass. +See [ALGORITHMS.md](ALGORITHMS.md#solver). The solver works on copies of the awake dynamic and kinematic bodies +(`bodyState`): the static and sleeping bodies share a state with no mass; a kinematic body has its own state, read by +the rows (its velocity, its motion) and never written (no mass). The axis locks (`actor.Axes`, `RigidBody.LinearLock` & `AngularLock`) live in the state too: its inverse mass by axis is null along the locked axes, its inverse inertia in world space is the one of the body held around them (`lock.go`, see @@ -129,10 +176,29 @@ else knows about the locks, apart from the integration (gravity, gyroscopic torq `World.Close()` stops them (they are also stopped when the World is garbage collected). - The broad phase, the narrow phase, the preparation of the contacts and the integration of the bodies: each body, pair or contact writes its result at its own index, the order of execution doesn't matter. -- The pairs are sorted (index of the first body, then of the second body). +- The order of the bodies is the one of `World.Bodies`: `AddBody` puts a body last, `RemoveBody` moves the following + bodies up by one (the trees, the pairs and the contacts follow, `Tree.removed`), a body added again is last whatever + its former index. The same bodies added and removed in the same order give the same result. +- The pairs are sorted by the indices of their bodies (`Tree.sortPairs`: the first body by a counting sort, then the few + pairs of a body, its planes first, by an insertion sort). The pair search already gives them in the same order whatever + the workers (its chunks are read in their order): the sort makes it the order of a search from scratch, whatever the + history of the trees, and keeps the contacts of a body next to each other for the solver. It costs 4.4 ns per pair on + one goroutine: 3.8 µs for the 856 pairs of 500 bodies landing, 15 µs for the 3471 pairs of 2000 bodies, 0.1 to 0.2 % + of their step with 1 worker and 0.4 to 1 % with 8 (`go test -run xxx -bench SortPairs`, on a Ryzen 7 5800X). + Box2D states the rule (`contact.c` in v3.1, "Contacts and determinism": the contacts must exist in the same order + whatever the thread count, the Gauss-Seidel solver is order dependent) and now sorts the keys of its new pairs + (`b2UpdateBroadPhasePairs`, `broad_phase.c`, read on 02/10/2026: "Pairs arrive in deterministic order but scrambled + relative to body and shape order, sorting them here improves solver performance"); Jolt sorts the constraints and the + contacts of each island (`PhysicsSettings::mDeterministicSimulation`, on by default in 5.5.0: off, it runs "faster + but it will no longer be deterministic"). - The solver is a Gauss-Seidel: a constraint uses the result of the previous one. The contacts and the joints are colored (like Box2D v3): the constraints of a color don't share any dynamic body, so a color is solved in parallel. The colors are always solved in the same order: the result is the same bit for bit, whatever the number of workers. +- It is tested (`determinism_test.go`): the bits of the bodies and of the contacts after each of the 1000 steps of 200 + bodies (a pile bouncing on chains and on linked boxes), and of a pile of 600 bodies, are the same between two runs and + with 1, 4 and 8 workers, also when bodies are removed and added again during the run. Each scene must reach the + parallel path of every stage, or its test fails. The result is the same on a given GOARCH only: Go may fuse + `x*y + z` into a single rounding on some architectures. - A step doesn't allocate memory after the first steps: the buffers are reused. ## Tests & benchmarks @@ -191,17 +257,37 @@ restitution, continuous collision, islands), without allocation. ## Current limitations - The broad phase is a pair of dynamic AABB trees (static and dynamic bodies), the dynamic AABBs enlarged by a margin: a sleeping body costs nothing (the planes & the heightfields are not in the trees, they are tested with every awake body). -- A heightfield is a surface: a body entirely under it is not pushed up. -- The contacts are computed once per step: on a rough terrain, a corner of a tumbling body can slide over another - triangle during the step, and sink by a few mm before the next step. +- A heightfield and a mesh are surfaces, without thickness: a triangle whose plane is above the center of a body is + ignored (as Jolt, Box3D and PhysX do). A body whose center went under the surface is not pushed up, even if it still + crosses it: it falls under the terrain, through the wall of the mesh. The continuous collision keeps the fast bodies + on the right side; a body must not be placed with its center under a terrain or in the solid of a mesh. A mesh is + static, in the static tree, and never changes: build another one. +- A convex hull has 256 vertices at most, its support point is found among all of them (no hill climbing): a hull of + 256 vertices against a box costs 16 µs per pair, a hull of 64 vertices 7 µs, as a box. A face of more than 8 vertices + gives 8 of them to the contact clipping (one out of k): a wide cylinder resting on its cap has 8 of its 16 corners as + candidates, 4 kept. When the vertex limit is reached, the points left out are the closest to the hull, outside it + (Jolt and Box3D stop the same way, PhysX expands the hull by its planes). A flat cloud is refused. +- The contacts are computed once per step: on a rough terrain or on a mesh, a point of a tumbling body moves over + another triangle during the step, and has no contact with it before the next step. On hills folded by 18° between 2 triangles (the + median of the terrain of the bench), the deepest of 60 bodies dropped alone lands 8 mm deep (the median; under + 0.3 mm on a flat terrain), and 3.7 mm with the contacts computed at each sub-step (480 Hz with 1 sub-step); at rest + it is 0.3 mm deep. Jolt, Box3D and PhysX compute their contacts once per step too. +- A body has 8 manifolds at most against a terrain: the deepest ones. - The friction around the normal comes from the lever arms of the points: a ball spinning on itself on its single point of contact never stops (no sleep), unless its material has a `SpinningResistance` (0 by default). -- A capsule resting across a bump of a terrain can stay a few mm in the terrain: the contact of a triangle comes from - the feature of the body above the triangle, the middle of the capsule is missed. - The restitution is applied once per step, with the approach velocity of the impact: a body not round (box, capsule), bouncy (`e` over 0.5) and spinning fast (10-20 rad/s) can bounce higher than it fell. Measured at 60 Hz with 8 substeps: up to +21 % of energy at `e = 1`, never up to `e = 0.5`. Jolt documents the same limit. -- No kinematic bodies (moving platforms): a body is static or dynamic. +- A kinematic body is moved by a target per step: a velocity written on it is overwritten by the step (Box2D and Jolt + move theirs by their velocity). A kinematic body pushing a dynamic body against a static body squeezes it, as in every + engine (PhysX documents it). The planes and the heightfields stay static. +- A character is a standing capsule which never turns, with one padding and one supporting volume (its lower sphere), + up along +Y. Its inner body enters and leaves the triggers as a kinematic body, which never block it (its contacts + and its sweeps skip them). Pushing a body, it stands in the padding + of the body after its update (its plane moves at the velocity of the body) until the step moves the body: a frame + rendered between the two shows it up to 2 cm closer than the padding. Against a wall, every update tries a stair + walk and cancels it, as Jolt: an update costs 54 µs there, 11 µs on the open terrain (24 characters, before the + 13 % taken off, see ALGORITHMS.md), and the cost is GJK on the triangles within reach. - The queries test every plane and every heightfield of the world: they are in no tree. A terrain cut in dozens of heightfields would need one. - `SyncQueries` costs a test per body, even for a static body which never moves: Feather doesn't know what the game @@ -210,7 +296,18 @@ restitution, continuous collision, islands), without allocation. the pile is asleep. - A sweep doesn't turn the shape, gives its first hit only, and no depth when the shape starts in a body. The back side of a heightfield is never hit. -- A fast body which is not a bullet goes through a dynamic body with all its axes locked: the continuous collision of - the other bodies only looks at the static ones. -- The continuous collision stops the fast bodies against the static bodies (and the bullets against all the bodies), - not the other pairs: 2 fast dynamic bodies rely on their speculative contacts (2 cm) and on the spring of the contact. +- The continuous collision leaves to the solver the dynamic and kinematic bodies the fast body has a contact with: + along the normal of the contact the speculative row holds them, but a fast body whose closest feature changes during + the step (a corner passed, a body which turns) can slip past its contact, as with every speculative contact. The + static bodies are always swept. +- A fast body stopped by the continuous collision keeps its velocity and loses the rest of its step: it seems slower + for a step (the time stealing Jolt documents for its `LinearCast`), and its joints see the position it was moved to. + The impact of 2 fast bodies is found with the rotation of the other body ignored, and both are stopped there even if + one of them is stopped earlier by a third body (the other then stops short of a body which never came, as in Jolt). +- A fast body changes the shocks: its contacts with the dynamic bodies are speculative up to its speed * dt, and stop + it in the substep it touches, where a slower body enters the other by up to its speed * dt and is pushed out by the + spring of the contact. Measured on the bench: the slab of `high mass ratio 2` (28 cm per step, fast) bounces at + 1.97 m/s instead of 2.04 and its cubes drift by 17.8 mm instead of 16.5; a swinging chain of 20 capsules stretches by + 9 mm instead of 24. Many fast bodies close to each other cost: their speculative contacts double the narrow phase of + a scene of 200 bodies where 42 are fast at every step (the step from 1.8 to 2.4 ms); a pile without a fast body + pays nothing measurable. diff --git a/PHYSICS_GUIDE.md b/PHYSICS_GUIDE.md index 43e9c74..ced6fad 100644 --- a/PHYSICS_GUIDE.md +++ b/PHYSICS_GUIDE.md @@ -140,8 +140,8 @@ body.SetLocks(actor.NoAxes, actor.AxisX|actor.AxisZ) // later: it cle - `SetLocks` wakes the body up: a body freed in the air falls. Writing the fields of a sleeping body doesn't. - A body with all its axes locked stays dynamic: it never moves, carries what rests on it, sleeps and wakes up, and collides with the static bodies (events). A static body costs less: prefer it for what never moves. -- A fast body which is not a bullet goes through a body with all its axes locked: its continuous collision only looks - at the static bodies. A thin wall is a static body, or what is thrown at it is a bullet (`IsBullet`). +- A fast body is stopped by a body with all its axes locked as by any body (speculative contact, then time of impact). + A static body still costs less for a wall which never moves. - A static body has no lock: `SetLocks` does nothing on it. - A joint which needs a locked axis can't be satisfied: its other rows are still solved (a body which only turns around Y, held by a fixed joint, doesn't turn), the impossible ones are left out. Nothing explodes. @@ -152,6 +152,131 @@ body.SetLocks(actor.NoAxes, actor.AxisX|actor.AxisZ) // later: it cle leaning by 45° which only turns around Y is as heavy to spin as it looks). Jolt, Rapier, PhysX and Box3D make it lighter than it is; see the [algorithms](ALGORITHMS.md#axis-locks). +### Kinematic bodies +A kinematic body is moved by the game, not by the forces: a lift, a moving platform, a door, a character, the bones of +a creature which follow its animation while its tail is simulated. It pushes the dynamic bodies it meets with the +velocity of its motion, carries what rests on it, and nothing pushes it back. It is the kinematic body of PhysX +(`setKinematicTarget`), Unity (`isKinematic` + `MovePosition`), Jolt (`MoveKinematic`) and Unreal. + +```go +lift := actor.NewRigidBody(actor.Transform{Position: start, Rotation: mgl64.QuatIdent()}, &actor.Box{HalfExtents: mgl64.Vec3{1, 0.1, 1}}, actor.BodyTypeKinematic, 500) +lift.Material.StaticFriction, lift.Material.DynamicFriction = 0.6, 0.6 // the friction of its surface, for the riders +world.AddBody(lift) + +// each frame, before Step: where the lift is at the end of the step +next := lift.Transform +next.Position = next.Position.Add(mgl64.Vec3{0, 0.5, 0}.Mul(dt)) +lift.SetKinematicTarget(next) +world.Step(dt) +``` +- **One target per step.** `SetKinematicTarget` gives the pose the body reaches at the end of the next step: the body + goes there over the sub-steps (position linearly, rotation along the shortest arc), its velocity is the one of this + motion (`Velocity`, `AngularVelocity`: (target - pose) / dt, read after the step), and the target is dropped. Give one + before each step to move the body along a path; without target the body stays where it is, with no velocity. A + velocity written on a kinematic body is ignored: move it by its target. +- **What it carries follows it.** A body resting on a platform moves with it, by friction (give the platform a + friction); a body it meets is pushed at its speed. The contact exists before they touch, from the speed of the + kinematic body: a leg at 5 m/s doesn't enter the tail on its path. A kinematic body pushing a body against a wall + squeezes it: the body enters the wall, as in every engine. +- **No contact with the static and the kinematic bodies**: a kinematic body in the ground, through a wall or through + another kinematic body is not pushed out, and sends no collision event. It enters and leaves the triggers, static or + not, and a kinematic trigger detects every body ([Triggers](#triggers)). +- **Sleep.** On its way, a kinematic body keeps awake the bodies it touches, however slowly it goes. Stopped (no + target), it falls asleep with them half a second later; its next target wakes them all up. +- **Teleport.** `world.Teleport(body, transform)` places a body without any velocity: a kinematic body brought there by + a target pushes what is on its path, a teleported one goes through it (it overlaps it, and the contact pushes them + apart at the next steps). For a cut scene, a respawn; `Teleport` works on every body, and wakes the sleeping bodies at + the new place. +- **Forces, torques and impulses** are ignored, so is the gravity. The locks (`LinearLock`, `AngularLock`) are kept for + the day the body becomes dynamic. +- **The animated bones of a creature.** Each bone is a kinematic body: at each step, give it the pose of its bone in the + animation. The simulated parts (a tail, a bag, hair) are dynamic bodies linked by joints, which hit the kinematic + bones and are pushed by them. +- **Ragdoll.** `world.SetBodyType(bone, actor.BodyTypeDynamic)`: the bone falls with the velocity of its last motion, + and keeps its contacts, its joints, its layers and the pairs it ignores; `SetBodyType(bone, actor.BodyTypeKinematic)` + stops it and gives it back to the animation. Create the bones with their density: a body without mass can't become + dynamic (`ErrMasslessBody`, or give it one by `SetDensity`). A static body stays static (`ErrStaticBody`): it is the + shape of the world. +- **The continuous collision** never stops a kinematic body. A fast dynamic body meeting it is held by their speculative + contact (from their relative speed), and stopped by the time of impact when they had none: a thin moving wall is a + kinematic body. +- A plane or a heightfield stays static: a moving terrain is not supported. + +### Characters +A character is a capsule which walks: the player, a creature of the game. It is virtual (the `CharacterVirtual` of +Jolt, whose name it takes, the `CharacterController` of Unity, the mover of Box2D v3): the game gives its velocity and calls its update, +which slides the capsule along the walls, climbs the slopes and the steps, finds the ground and pushes the dynamic +bodies; no force, no contact of the solver moves it, so it goes exactly where the game decides, at once. + +```go +hero := feather.NewCharacterVirtual(actor.Capsule{HalfHeight: 0.55, Radius: 0.3}, feet.Add(mgl64.Vec3{0, 0.85, 0})) +hero.MaxSlopeAngle, hero.StepHeight = 45*math.Pi/180, 0.3 // the defaults: 50°, 0.4 m, 100 N, 70 kg, StickToFloor 0.5 m (Jolt) +world.AddCharacter(hero) +world.SetFilter(hero.Body(), layerCharacters, actor.AllLayers) + +// each frame +world.Step(dt) +velocity := mgl64.Vec3{0, hero.Velocity.Y(), 0} // in the air: the vertical velocity of the fall is kept +if hero.GroundState() == feather.CharacterOnGround { // on the ground: the velocity of the ground (a platform) + hero.UpdateGroundVelocity() + velocity = hero.GroundVelocity() + if jump { + velocity = velocity.Add(mgl64.Vec3{0, 4, 0}) + } +} +hero.Velocity = velocity.Add(world.Gravity.Mul(dt)).Add(input) // + the gravity of the step, + the input (m/s) +hero.Update(dt) +model.SetPosition(hero.Position.Sub(mgl64.Vec3{0, 0.85, 0})) // the feet are 0.85 m under the center of the capsule +``` +- **The velocity is the game's.** The recipe above is the one of the samples of Jolt (`CharacterVirtualTest::HandleInput`): + the gravity of the step is added every frame, so that the character presses on the ground and falls when it leaves + it; on the ground the vertical velocity is reset to the one of the ground, in the air it accumulates. PhysX, Box2D + and Godot leave the gravity to the game the same way. After the update, `Velocity` is what the character took: the + part towards a slope too steep removed. +- **Update after the step.** The velocity of a platform is the one of the step just done: updated after it, the + character follows the platform without lag (`UpdateGroundVelocity` reads the velocity of the platform again before + the recipe, for the frame the platform starts). Updated before the step, it is one step behind the platform. During + a step an update panics, as a query. +- **It walks on everything**: a plane, a terrain, a mesh, the boxes, the hulls, the capsules, the dynamic and the + kinematic bodies. It keeps 2 cm from them (`CharacterPadding`): a model drawn at the capsule stands 2 cm above the + ground. The contacts are gathered 10 cm around the capsule: no body is entered, whatever the speed. +- **Slopes.** Under `MaxSlopeAngle` a surface is a ground: the horizontal velocity is projected on it (the character + climbs at `speed × cos(angle)` along the slope, as in Jolt; Godot keeps the speed with `floor_constant_speed`). + Steeper, it is a wall: the character doesn't climb it, and slides down it under the gravity of the recipe + (`CharacterOnSteepGround`; the `ePREVENT_CLIMBING_AND_FORCE_SLIDING` of PhysX). Standing still (no horizontal + velocity) on a ground, the character doesn't creep down (the stop on slope of the samples of Jolt and of Godot). +- **Steps.** A step lower than `StepHeight` is walked without jumping; higher, it is a wall. A step is walked up, + forward and down onto its floor, which must not be too steep where the character lands (the edge of a step is tested + 15 cm further). A step of 0.6 m blocks a character of radius 0.3: the sphere of its capsule stops against the riser. +- **Downhill**, the character sticks to the floor: leaving the ground without going up, it looks for the floor within + `StickToFloor` under it (0.5 m by default; 0 sticks to nothing) and lands on it (a slope, a step down). It walks down + a 30° slope at 3 m/s without a frame in the air. Over a crest it was going up: it leaves the ground for a frame, and + lands at the next one. Off a drop longer than `StickToFloor`, it falls. +- **Platforms.** Standing on a kinematic body, `GroundVelocity` is the velocity of the point under the character over + the last step (its arc when the platform turns, as `CalculateCharacterGroundVelocity` of Jolt); the recipe adds it. A + platform going up lifts the character by its contact. Drive the platform by `SetKinematicTarget`: its velocity is + the one of its motion. +- **Pushing.** A dynamic body met gets an impulse which brings it to the speed of the character along the contact (damped + by 0.9), at most `MaxStrength × dt` (100 N: a ball of 11 kg or a crate of 22 kg is pushed at the walking speed), and + never downwards; a crate of 8 t doesn't move and stops the character. A dynamic body which hits a character is + stopped by its inner kinematic body: it never goes through it. The weight of the character (`Mass × g`) presses a + dynamic body it stands on. +- **Characters** block each other head on (they stop 2 paddings apart), slide along each other when they cross, and + never go through each other. The inner body of a character is the body a ray, a sweep or an overlap hits: + `world.CharacterOf(hit.Body)` gives the character. +- **Filtering.** The character collides with the bodies its inner body collides with: its layer and its mask + (`world.SetFilter(hero.Body(), ...)`), the pairs ignored (`IgnoreCollision`). Give the characters a layer of their + own to let the rays of the game skip them. The inner body enters and leaves the triggers as a kinematic body does + (`EventTriggerEnter` and `EventTriggerExit` with the body; `CharacterOf` gives the character); a trigger never + blocks the character: its contacts and its sweeps skip the triggers. +- **Teleport** places the character with its inner body and reads its contacts there; the next update pushes it out of + a body it was placed in. `RemoveCharacter` removes it with its inner body. +- **A crouch**: write `Capsule.HalfHeight` between 2 updates; the inner body shares the capsule. +- **Cost.** On a bumpy terrain, 11 µs per update for a character walking alone (24 characters: 0.27 ms per frame), 54 µs + in a crowd pushing against walls and crates (24: 1.3 ms; 200: 9.8 ms), measured on one core, then 13 % less (the + supports and the cores of GJK made once, see ALGORITHMS.md). An update is GJK on the 6 to 10 triangles under the + capsule and the 2 or 3 its sweeps hit. + ### Collision filtering Who collides with whom is decided by 3 things, tested in the broad phase before any contact is computed. @@ -236,7 +361,7 @@ how to tilt the foot. hit, ok := world.Raycast(knee, mgl64.Vec3{0, -0.8, 0}, filter) if ok { ground := hit.Point // knee + hit.Fraction * translation - slope := hit.Normal // out of the ground; hit.Triangle is the triangle of a terrain (2*cell + t), else -1 + slope := hit.Normal // out of the ground; hit.Triangle is the triangle of a terrain (2*cell + t) or of a mesh, else -1 } ``` A ray has no thickness: on stairs or on rubble, a sphere of the size of the sole gives a steadier answer. @@ -316,15 +441,64 @@ world.AddBody(terrain) world.UpdateHeightfield(terrain, minX, minZ, maxX, maxZ) ``` - A heightfield is static. The body is at the center of the grid, the heights along its Y axis. +- The terrain is a surface, without thickness: a body is held while its center is above it. Placed with its center + under the surface, it falls under the terrain. - The terrain is made of triangles: a finer grid gives finer contacts (a 2048x2048 grid follows the ground better than 512x512), the bodies slide on the flat parts without hitting the edges between the triangles. - `Holes[x*(zSamples-1)+z]`: a cell without triangles (a cave, a tunnel entrance). - `World.UpdateHeightfield` wakes up the bodies above the changed region, and computes their contacts again. +### Decor meshes and convex hulls +```go +// the decor: the triangles of a model (indices 3 per triangle, counterclockwise seen from outside), built once +mesh, err := actor.NewTriangleMesh(vertices, indices) +rock := actor.NewRigidBody(actor.Transform{Position: place, Rotation: orientation}, mesh, actor.BodyTypeStatic, 0) +world.AddBody(rock) + +// a dynamic object: the convex hull of the vertices of its model, 64 vertices at most +hull, err := actor.NewConvexHull(vertices, 64) +// the hull is centered on its center of mass: the body is there, not at the origin of the model +crate := actor.NewRigidBody(actor.Transform{Position: place.Add(hull.CenterOfMass()), Rotation: mgl64.QuatIdent()}, hull, actor.BodyTypeDynamic, 300) +``` +- A mesh is static, and a surface: its triangles are touched from the side of their normal (the right-hand rule on the + winding), a body whose center went through a wall is not pushed back. Give the triangles their outward winding, and + close the solids the bodies can fall into. The vertices closer than 0.1 mm are welded: a soup of triangles is fine. + A degenerate triangle (no area) is kept out, at its index. +- A mesh is built outside `Step`: 100 000 triangles take 0.3 s. Build it once per model and share it between bodies (a + shape has no state); a mesh never changes, build another one for another decor. +- `hit.Triangle` of a ray or a sweep on a mesh is the index of the triangle: `mesh.Triangle(hit.Triangle)` gives its + vertices, `mesh.Indices[3*hit.Triangle:]` its vertices in the model, for a material per triangle on your side. +- A hull is a convex volume: the mass and the inertia come from its volume and the density. Its local space is + centered on its center of mass, `hull.CenterOfMass()` is the offset from the origin of the points. The limit of + vertices (256 at most) bounds the cost of a contact (a hull of 64 vertices costs as a box, 256 twice as much); when it + is reached, the hull is the one of the vertices the furthest out, the points left out are the closest to it. A + flat cloud (a quad, a line) has no hull: use a Box. +- A hull slides, rolls and rests on a mesh or a terrain as the other shapes, with the same edges rules. + ### Moving a body A shape has no state: several bodies can share the same shape. Each body keeps its AABB: after moving a body by hand (its `Transform`), call `UpdateAABB`. +### Changing a body +```go +err := world.SetShape(crate, &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}) // a box twice as big, in place +err = world.SetDensity(crate, 700) // oak instead of pine +``` +- **The body stays in the world** with its index, its contacts, its joints, its layers, the pairs it ignores and its + island; it wakes up, and so do the sleeping bodies around its old and its new shape: a crate resting on a plate + made thinner falls on it, a pile on a pedestal shrunk under it falls to the ground. +- **The mass and the inertia follow**: those of the new shape at the density of the body (`SetShape`), or of its + shape at the new density (`SetDensity`). A static body keeps its infinite mass; `SetDensity` on it keeps the + density written, for the day it becomes dynamic. +- **The body stays where it is.** The shapes are centered on their origin: a cube made a box twice as big is half in + the ground, and the spring of its contacts pushes it out over the next steps (at `ContactSpeed` at most). Place the + body where the new shape belongs before the change if you want no push: its `Transform` and `UpdateAABB`, or + `Teleport`. A hull is centered on its center of mass: `hull.CenterOfMass()` is the offset to add to the pose. +- **A surface stays static.** A plane, a heightfield or a mesh on a dynamic or a kinematic body is refused + (`ErrShapeNotConvex`), no shape too (`ErrNoShape`), a density which is not positive on a dynamic body too + (`ErrMasslessBody`): the body is left as it is, never in silence. +- Both calls run between two steps, like `AddBody`; they allocate nothing. + ### Timestep & substeps ```go world := feather.World{ @@ -352,18 +526,40 @@ With 8 substeps at 60 Hz, a stack of 10 boxes of 50 cm sinks by ~32 mm (Box2D v3 more. ### Fast bodies -The contacts with a static body are created before the body touches it (speculative contacts), from the distance it can -travel during the step. Between 2 dynamic bodies, from 2 cm only: a fast body can enter another one during a step, the -spring of the contact pushes it out. -A fast body is also moved back to its first impact with a static body (continuous collision). Set `IsBullet` on a small -fast body (a projectile) to stop it on the dynamic bodies too. -A ball at 40 m/s does not go through a 4 cm wall at 60 Hz (nor at 80 m/s). +The contacts with a static or a kinematic body are created before the body touches it (speculative contacts), from the +distance it can travel during the step; so are the contacts of a fast body (a body which can move more than half of its +smallest extent during the step) with every body. Between 2 slower dynamic bodies, from 2 cm only: a body can enter +another one by what it moves in a step, the spring of the contact pushes it out. +A fast body is also moved back to its first impact with any body it had no contact with (continuous collision): +nothing marks a projectile, there is no bullet flag. +A ball at 40 m/s does not go through a 4 cm wall at 60 Hz (nor at 80 m/s); 2 balls of 10 cm thrown at 20 m/s each +bounce off each other; a plate of 1 cm at 30 m/s pushes the resting plate it meets. ### Sleep The bodies touching each other form an island. An island resting for 0.5 s (all its bodies under 0.05 m/s and 0.05 rad/s) falls asleep: it is not simulated anymore. The whole island wakes up with `AddForce`, `AddTorque` or `WakeUp` on one of its bodies, when a moving body touches it, -or when a body under it is removed. +or when a body under it is removed. A body asleep in a trigger stays in it ([Triggers](#triggers)). + +### Triggers +A trigger (`IsTrigger`) is not solved: it detects the bodies which overlap it. `EventTriggerEnter` when a body starts to +overlap it, `EventTriggerStay` at each step while one of them is awake, `EventTriggerExit` when they no longer overlap, or +when one of them is removed (`RemoveBody`: the exit is sent with the events of the next step). +- A body asleep in a trigger stays in it: no exit, and no stay while both rest. Counting the enters and the exits gives + the bodies in a zone. +- A body in contact with a trigger is in it (as `World.Overlap`). +- The game moves a static trigger, or a sleeping body: set its `Transform` and call `UpdateAABB` (or `Teleport`). The + overlap is tested again when the AABB of one of the bodies changed. +- A kinematic body enters and leaves a static trigger, and a kinematic trigger detects the static and the kinematic + bodies: a door zone sees the platform which carries the player. 2 static bodies never pair. +- Each awake body in a trigger costs a test per step; a body asleep in a trigger costs its place in the pairs of the + step, without test while nothing moves. + +### Collision events +`EventCollisionEnter` when 2 bodies start to touch, `EventCollisionStay` at each step while one of them is awake, +`EventCollisionExit` when they stop touching, or when one of them is removed (`RemoveBody`: the exit is sent with the +events of the next step). A body asleep on the ground, or on another body, still touches it: no exit, and no stay +while both rest. A body made a trigger ends its contacts and starts its trigger pairs. ### Determinism & threads The same scene gives the same result, bit for bit, whatever the number of `Workers`. @@ -379,4 +575,12 @@ Call `World.Close()` when the world is not used anymore, to stop its workers. | Stacks wobble | Increase `Substeps` | | No bounce | Restitution on both bodies, impact faster than 1 m/s | | A body does not move | It may be asleep: call `WakeUp`. It may be locked along this axis: `LinearLock`, `AngularLock` | -| A character falls over | Lock its rotations around X and Z: `SetLocks(actor.NoAxes, actor.AxisX\|actor.AxisZ)` | +| A character falls over | Use a `CharacterVirtual` (a capsule which never turns); for a dynamic body, lock its rotations around X and Z: `SetLocks(actor.NoAxes, actor.AxisX\|actor.AxisZ)` | +| A character falls through the floor, or flies | Add the gravity of the step to its velocity every frame, and reset the vertical velocity on the ground (the recipe of [Characters](#characters)) | +| A character creeps down a slope | It has a horizontal velocity: give it exactly 0 to stand still. Steeper than `MaxSlopeAngle`, a slope is slid down | +| A character stops at a small step | `StepHeight` is 0, or lower than the step | +| A character lags behind its platform | Update it after `World.Step`, and call `UpdateGroundVelocity` before reading `GroundVelocity` | +| A character can't push a crate | `MaxStrength` (N) × dt is the impulse of a frame: 100 N moves 20 kg; a heavy crate stops the character by design | +| A kinematic body doesn't move | Give it a target before each step: `SetKinematicTarget`. A velocity written on it is ignored | +| A kinematic body goes through the bodies | It was teleported or placed by hand: move it by `SetKinematicTarget`, which pushes with the velocity of the motion | +| A platform falls asleep with its riders | It had no target: a kinematic body without target rests. Give it a target each step while it must move | diff --git a/README.md b/README.md index a24b937..41e9adb 100644 --- a/README.md +++ b/README.md @@ -20,9 +20,30 @@ All shapes live in the `actor` package and implement `actor.ShapeInterface`. | `Plane` | `Normal`, `Distance` (static only) | analytic | | `Capsule` | `HalfHeight`, `Radius`, axis along local Y | analytic against planes, spheres and capsules; its segment against the other shapes (GJK distance + radius) | | `Heightfield` | a grid of heights (static only), 2 triangles per cell | GJK/EPA against each triangle under the body | +| `TriangleMesh` | triangles with a tree of AABBs (static only), for the decor: `NewTriangleMesh(vertices, indices)` | GJK/EPA against each triangle under the body, as the heightfield | +| `ConvexHull` | the convex hull of a cloud of points, by quickhull: `NewConvexHull(points, maxVertices)`, 256 vertices at most | GJK/EPA, its faces for the contact points; dynamic, with the mass and the inertia of its volume | Each shape also casts a ray on itself, in its local space (`CastRay`): the world queries are built on it. +```go +// the decor: a mesh of triangles, counterclockwise seen from outside, built once (outside Step) +mesh, err := actor.NewTriangleMesh(vertices, indices) // vertices []mgl64.Vec3, indices []int32, 3 per triangle +rock := actor.NewRigidBody(actor.Transform{Rotation: mgl64.QuatIdent()}, mesh, actor.BodyTypeStatic, 0) + +// a dynamic object: the hull of the vertices of its mesh, centered on its center of mass +hull, err := actor.NewConvexHull(vertices, 64) +crate := actor.NewRigidBody(actor.Transform{Position: hull.CenterOfMass(), Rotation: mgl64.QuatIdent()}, hull, actor.BodyTypeDynamic, 300) +``` +A mesh and a heightfield are surfaces: their triangles are seen from the side of their normal, a body whose center went +through is not pushed back. See the [physics guide](PHYSICS_GUIDE.md#decor-meshes-and-convex-hulls) and the +[algorithms](ALGORITHMS.md#convex-hull). + +A body in the world changes its shape or its density in place: `world.SetShape(body, shape)` and +`world.SetDensity(body, density)` keep its index, its pairs, its joints, its layers and its island, give it the mass and +the inertia of the shape, wake it up with the sleeping bodies around it; a plane, a heightfield or a mesh on a dynamic +or a kinematic body is refused (`ErrShapeNotConvex`). See the [physics guide](PHYSICS_GUIDE.md#changing-a-body) and the +[algorithms](ALGORITHMS.md#changing-a-body). + ```go body := actor.NewRigidBody( actor.Transform{Position: mgl64.Vec3{0, 1, 0}, Rotation: mgl64.QuatIdent()}, @@ -92,7 +113,8 @@ world.SyncQueries() // after the game added, moved or reshaped bodies, if a quer - A ray is exact on each shape. A moving shape (a sphere, a capsule, a box) stops within 1 µm of the body it hits (0.1 mm between 2 boxes), never in it. - A ray or a shape which starts in a body hits it at the fraction 0, the normal against its direction. -- The top side of a heightfield only is hit; a hole is not. +- The top side of a heightfield only is hit; a hole is not. The side of the normal of the triangles of a mesh; its + `hit.Triangle` is the index of the triangle. - At the same fraction, the body of the lowest index in `World.Bodies` is hit: the results don't depend on the workers nor on the shape of the trees. - No allocation, no write: any number of goroutines can run queries together while nothing writes the world. A query @@ -115,6 +137,118 @@ a locked body has its exact inertia, the one of a body on an axle. A body withou before. See the [physics guide](PHYSICS_GUIDE.md#axis-locks) and the [algorithms](ALGORITHMS.md#axis-locks). +## Kinematic bodies +A kinematic body goes where the game puts it and pushes the dynamic bodies it meets; nothing pushes it back: a moving +platform, a character, the bones of a creature which follow its animation while its tail is simulated. It is the +kinematic body of PhysX, Jolt, Box2D, Unity and Unreal. + +```go +platform := actor.NewRigidBody(transform, &actor.Box{HalfExtents: mgl64.Vec3{2, 0.1, 2}}, actor.BodyTypeKinematic, 500) +world.AddBody(platform) + +// each step: the pose the platform reaches at the end of the step; it goes there over the sub-steps +next := platform.Transform +next.Position = next.Position.Add(mgl64.Vec3{1, 0, 0}.Mul(dt)) +err := platform.SetKinematicTarget(next) // actor.ErrNotKinematic on a body which is not kinematic +world.Step(dt) // the platform is at next, bit for bit; platform.Velocity is (next - previous) / dt + +world.Teleport(platform, start) // placed without velocity: it pushes nothing +err = world.SetBodyType(bone, actor.BodyTypeDynamic) // a ragdoll: the bone falls, with its contacts, its joints and its island +err = world.SetBodyType(bone, actor.BodyTypeKinematic) // and follows its targets again; ErrStaticBody, ErrMasslessBody refuse +err = world.SetShape(crate, &actor.Sphere{Radius: 0.3}) // another shape in place: mass and inertia follow, contacts, joints and island kept +err = world.SetDensity(crate, 700) // another density in place: the mass and the inertia of the shape at 700 kg/m³ +``` +- One target per step: it is reached at the end of the step and dropped. Without target the body stays, with no + velocity (the kinematic actors of PhysX). Its velocity is the one of its motion: the contacts push with it. +- Infinite mass for the solver: no force, no gravity, no impulse, no contact moves it. A dynamic body it meets takes its + velocity, friction included (a stack rides a platform, a leg pushes a tail). The contact exists before they touch, + from the speed of the kinematic body: a leg at 5 m/s doesn't enter the resting body on its path. +- No contact between a kinematic body and a static or a kinematic body: no narrow phase, no collision event. A + kinematic body enters and leaves the triggers, static or not, and a kinematic trigger detects every body + ([Triggers](#triggers)). +- On its way, a kinematic body keeps awake the bodies it touches; stopped, it sleeps with them; its next target wakes + them all up. +- The planes and the heightfields stay static. + +See the [physics guide](PHYSICS_GUIDE.md#kinematic-bodies) and the [algorithms](ALGORITHMS.md#kinematic-bodies). + +## Triggers +A trigger (`IsTrigger`) is not solved: it reports the bodies which overlap it, with the events of the world. A pair of +a trigger and a body enters once, stays, and exits once: when the shapes no longer overlap, when one of the bodies +leaves the world, never because one of them falls asleep. Counting the enters and the exits tells who is in a zone. + +```go +zone := actor.NewRigidBody(transform, &actor.Box{HalfExtents: mgl64.Vec3{2, 1, 2}}, actor.BodyTypeStatic, 0) +zone.IsTrigger = true +world.AddBody(zone) + +occupants := 0 +world.Events.Subscribe(feather.EventTriggerEnter, func(feather.Event) { occupants++ }) +world.Events.Subscribe(feather.EventTriggerExit, func(feather.Event) { occupants-- }) +``` +- A body in contact with a trigger is in it: the overlap is the one of `World.Overlap`. +- A body asleep in a trigger stays in it, without stay events while both rest (the overlap can't change). Its pair is + kept in the broad phase whatever the sleep, as the sensors of Box2D v3, and is tested again when the game moves one of + the bodies (its AABB changes), as the trigger pairs of PhysX. +- `RemoveBody` on a body in a trigger, or on the trigger, ends their pairs: the exits are sent with the events of the + next step (or in the events running, if a listener removed the body). +- A kinematic body enters and leaves a static trigger, and a kinematic trigger detects the static and the kinematic + bodies (as Box2D v3 and Unity), without any contact. 2 static bodies never pair. +- A trigger is filtered like any body ([Collision filtering](#collision-filtering)). +- The collision events follow the same rules: a contact ends when the bodies stop touching or one of them is removed + (`EventCollisionExit` at the next events), never because a body falls asleep, on the ground or on another body, and + sends no stay while both rest. + +See the [physics guide](PHYSICS_GUIDE.md#triggers) and the [algorithms](ALGORITHMS.md#triggers). + +## Characters +A character is a capsule the game moves at a velocity: it walks on the ground, the terrains and the meshes, slides +along the walls, climbs the slopes under its limit and the steps under its height, stays on the floor downhill, is +carried by a moving platform, pushes the dynamic bodies with a bounded force, and is blocked by the heavy ones and by +the other characters. It is virtual, as `CharacterVirtual` of Jolt and the mover of Box2D v3: no rigid body moves it, +it moves itself between 2 steps with the queries of the world, so that it goes exactly where the game decides. It is +named as Jolt names it (Jolt keeps `Character` for its character on a rigid body, which Feather doesn't have). + +```go +hero := feather.NewCharacterVirtual(actor.Capsule{HalfHeight: 0.55, Radius: 0.3}, feet.Add(mgl64.Vec3{0, 0.85, 0})) +hero.MaxSlopeAngle = 45 * math.Pi / 180 // the defaults are those of Jolt: 50°, a step of 0.4 m, 100 N, 70 kg, the floor kept within 0.5 m +world.AddCharacter(hero) // its inner kinematic body stands in the world (hero.Body()) + +// each frame, after Step: the velocity the character wants, then its update +world.Step(dt) +velocity := mgl64.Vec3{0, hero.Velocity.Y(), 0} // in the air: the fall goes on +if hero.GroundState() == feather.CharacterOnGround { // on the ground: the velocity of the ground (a platform) + hero.UpdateGroundVelocity() + velocity = hero.GroundVelocity() +} +hero.Velocity = velocity.Add(world.Gravity.Mul(dt)).Add(input) // the gravity of the step, the input of the player +hero.Update(dt) +``` +- **Position** is the center of the capsule; **Velocity** is written by the game and read back after the update: the + part towards a slope too steep is removed, the rest is kept (a fall accumulates the gravity). The gravity is the + game's, as in Jolt, PhysX, Box2D and Godot. +- **Slide.** The contacts within 10 cm of the capsule (inflated by a padding of 2 cm it keeps from everything) are + planes with the velocity of their body; the velocity is slid along them by time of impact, and a sweep checks the + path: the character never enters a body, and never gets stuck in a corner. +- **Slopes and steps.** `MaxSlopeAngle`: steeper, a surface is a wall, not climbed, slid down. `StepHeight`: a step + lower than it is walked (up, forward, down onto the floor). Leaving the ground downhill without going up, the + character sticks to the floor within `StickToFloor` (0.5 m). +- **Ground.** `GroundState` (in the air, not supported, on steep ground, on the ground), `GroundNormal`, + `GroundVelocity`, `GroundBody`, `GroundPosition`: the contacts in the lower sphere of the capsule hold it. +- **Bodies.** A dynamic body met receives an impulse which brings it to the speed of the character, at most + `MaxStrength` × dt and never downwards; a heavy body stops the character. The weight `Mass` × g goes to a dynamic + ground. A dynamic body which runs into a character is stopped by its inner body (the solver): it never goes + through it. A kinematic platform carries the character (its velocity in `GroundVelocity`, its arc when it turns). +- **Characters** find each other through their inner bodies: 2 characters block each other head on, slide along + each other when they cross, and never go through each other. +- **Filtering.** The layer and the mask of the inner body (`world.SetFilter(hero.Body(), layer, mask)`), the pairs + ignored: the rules of the pairs. Its inner body enters and leaves the triggers (`EventTriggerEnter`, + `EventTriggerExit` with the body: `CharacterOf` gives the character); a trigger never blocks the character. +- `Teleport` places the character and reads its contacts there; `RemoveCharacter` removes it with its inner body; + `CharacterOf` gives the character of a body hit by a ray. + +See the [physics guide](PHYSICS_GUIDE.md#characters) and the [algorithms](ALGORITHMS.md#characters). + ## TGS Soft TGS Soft (or "Soft Step") is the solver of Box2D v3, described by Erin Catto in Solver2D. It is made of substeps, soft constraints, warm starting and relaxation: @@ -255,6 +389,14 @@ cd bench && go run . -queries # the cost of a ray, a sweep, an overlap https://dev.epicgames.com/documentation/en-us/unreal-engine/API/Runtime/Engine/FHitResult) - John Amanatides & Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing (Eurographics 1987): the walk of a grid by a ray +- Triggers, contacts & sleep: Box2D v3.1.1 (`src/sensor.c`: `b2SensorTask`, `b2SensorQueryCallback`; + `b2SensorEndTouchEvent`; docs/simulation.md, "Sensors do not consider sleep"; `src/solver_set.c`: + `b2TrySleepIsland`; `src/contact.c`: `b2DestroyContact`, `b2ContactEndTouchEvent`), PhysX (`ScTriggerInteraction.cpp`: + `onActivate`, `onDeactivate`, `PROCESS_THIS_FRAME`; `PxTriggerPair`, `eNOTIFY_TOUCH_LOST`), Jolt 5.6 + (`Body::SetIsSensor`, `Body::SetCollideKinematicVsNonDynamic`, `Body::UpdateSleepStateInternal`, + `ContactListener::OnContactRemoved`), Godot 4 (`Area3D.body_exited`, `GodotAreaPair3D`, `JoltArea3D`), Unity + (`MonoBehaviour.OnTriggerStay`, `MonoBehaviour.OnTriggerExit`, `MonoBehaviour.OnCollisionStay`; Manual, Collider types + interaction) - Axis locks: Jolt 5.3 (`EAllowedDOFs`, `MotionProperties::GetInverseInertiaForRotation`), Rapier 0.22 (the inverse mass by axis, `effective_inv_mass`), Box3D & Box2D (`b3MotionLocks`, `b2MotionLocks`), PhysX 5.6 (`PxRigidDynamicLockFlag`), Unity `Rigidbody.constraints` @@ -264,19 +406,60 @@ cd bench && go run . -queries # the cost of a ray, a sweep, an overlap - Dynamic AABB tree, collision margin & conservative advancement: Bullet 3.25 (`btDbvt` & `btDbvtBroadphase`; the margin of `btSphereShape` & `btCapsuleShape`, a point & a segment with their radius, added by `btGjkPairDetector`; `btContinuousConvexCollision`) -- PhysX speculative CCD & Unity "Continuous Speculative": https://nvidia-omniverse.github.io/PhysX/physx/5.6.0/docs/AdvancedCollisionDetection.html +- PhysX speculative CCD & Unity "Continuous Speculative": https://nvidia-omniverse.github.io/PhysX/physx/5.6.0/docs/AdvancedCollisionDetection.html, + PhysX 5.6 (`Sc::BodySim::updateContactDistance` in `ScCCD.cpp`, `PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD`) +- Continuous collision of the fast bodies: Box2D v3.1 (`b2SolveContinuous`, the fast body of `b2FinalizeBodiesTask`, + `src/solver.c`; the bullets of `docs/simulation.md`), Box3D (`b3SolveContinuous`, `safetyFactor`), Jolt 5.3 + (`EMotionQuality::LinearCast`, `JobFindCCDContacts` & `JobResolveCCDContacts` in `PhysicsSystem.cpp`, + `mLinearCastThreshold`), PhysX 5.6 (the sweep-based CCD of `PxsCCD.cpp`, the guide Advanced Collision Detection) +- Convex hull: Barber, Dobkin & Huhdanpaa, The Quickhull Algorithm for Convex Hulls (ACM TOMS, 1996); Dirk + Gregorius, Implementing QuickHull (GDC 2014: the tolerance, the merging of the faces); Jolt 5.3 + (`ConvexHullBuilder`, `ConvexHullShape`: the inertia by the covariance of the tetrahedra, `GetSupportingFace`), Box3D + (`b3CreateHull`, `src/hull.c`: the initial tetrahedron, the thresholds, `b3RayCastHull`, commit 9f998c8), PhysX + (`QuickHullConvexHullLib`, `PxConvexMeshDesc::vertexLimit`), Bullet 3.25 (`btConvexHullComputer`); Jonathan Blow & + Atman Binstock, How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle + mesh (the covariance of the canonical tetrahedron) +- Triangle mesh: Box3D (`b3CreateMesh`, `b3SplitBinnedSah`, `b3IdentifyEdges`, `b3RayCastMesh`, `b3ShapeCastMesh`, + `b3QueryMesh`, `src/mesh.c`; `b3ComputeMeshManifolds`, `src/mesh_contact.c`), Jolt 5.3 (`MeshShape`, + `AABBTreeBuilder`, `TriangleSplitterBinning`, `ActiveEdges`, `Indexify`, `RayTriangle.h`), PhysX (`BV4_AABBTree`, + `EdgeList::computeActiveEdges`, `PCMConvexVsMeshContactGeneration`), Bullet 3.25 (`btQuantizedBvh::buildTree`: the + balance of the split), Ericson 6.2.1 (top-down construction of a bounding volume hierarchy), Möller & Trumbore, Fast, + Minimum Storage Ray/Triangle Intersection (1997) +- Characters: Jolt 5.3 (`CharacterVirtual`: `GetContactsAtPosition`, `DetermineConstraints`, `SolveConstraints`, + `MoveShape`, `UpdateSupportingContact`, `HandleContact`, `WalkStairs`, `StickToFloor`, `ExtendedUpdate`, + `CancelVelocityTowardsSteepSlopes`, `CalculateCharacterGroundVelocity`; `ActiveEdges::FixNormal`; the samples + `CharacterVirtualTest::HandleInput` & `OnContactSolve`), PhysX 5.11 (`PxController`, `PxControllerDesc`, + `SweepTest::moveCharacter` & `doSweepTest`, `Controller::rideOnTouchedObject` in + `physxcharacterkinematic/src/CctCharacterController.cpp`), Box2D v3.1 (`b2World_CastMover`, `b2World_CollideMover`, + `b2SolvePlanes`, `b2ClipVector`, `samples/sample_character.cpp`), Godot 4.4 (`CharacterBody3D::move_and_slide`, + `_move_and_slide_grounded`, `_snap_on_floor`, `_set_collision_direction`, `scene/3d/physics/character_body_3d.cpp`) +- Kinematic bodies: PhysX 5.6 (`PxRigidDynamic::setKinematicTarget`, `Sc::BodySim::calculateKinematicVelocity` & + `updateKinematicPose` in `ScKinematics.cpp`, the guide Rigid Body Dynamics > Kinematic Actors, + https://nvidia-omniverse.github.io/PhysX/physx/5.6.0/docs/RigidBodyDynamics.html), Jolt 5.3 (`Body::MoveKinematic`, + `Body::SetMotionType`, `Body::sFindCollidingPairsCanCollide`, `MotionProperties::MoveKinematic`), Box2D v3.1 & Box3D + (`b2Body_SetTargetTransform`, `b2Body_SetType`, the kinematic tree of `broad_phase.c`, the kinematic bodies of + `constraint_graph.c`, the sleep of `b2FinalizeBodiesTask`) ## Acknowledgements Feather is written from the publications, the documentation and the source code of these projects: - [Box2D](https://github.com/erincatto/box2d), by Erin Catto: the TGS Soft solver (Solver2D, Soft Constraints), the graph coloring, the continuous collision, the category & mask bits of the collision filter - [Box3D](https://github.com/erincatto/box3d), by Erin Catto: the reference of the bench, the friction center and its - lever arms, the queries (an origin and a translation, the ray through a tree, the ray on a sphere) -- [Jolt Physics](https://github.com/jrouwe/JoltPhysics), by Jorrit Rouwe: the active edges of the terrains, - the contact patches, the body pair cache, the allowed degrees of freedom (the axis locks) + lever arms, the queries (an origin and a translation, the ray through a tree, the ray on a sphere), the thresholds of + quickhull and the ray on a hull, the tree of a mesh (the binned surface area heuristic, the leaves of 4 triangles, + the casts through it) and one generator of contacts for the meshes and the height fields +- [Jolt Physics](https://github.com/jrouwe/JoltPhysics), by Jorrit Rouwe: the active edges of the terrains and of + the meshes, the contact patches, the body pair cache, the allowed degrees of freedom (the axis locks), the cast of a + fast body against every body and the relative cast of two fast bodies, the hull centered on its center of mass with + the inertia of its volume, the supporting face of a hull, the weld of the vertices of a mesh, the virtual character + (its contacts made planes, its solver by time of impact, its stairs, its floor, its padding, its strength) - [Bullet](https://github.com/bulletphysics/bullet3), by Erwin Coumans: the spinning friction (the spinning resistance), the dynamic AABB tree (`btDbvt`), the collision margin (the cores of the rounded shapes), - the conservative advancement of the time of impact + the conservative advancement of the time of impact, the balance of the splits of the tree of a mesh +- [PhysX](https://github.com/NVIDIA-Omniverse/PhysX), by NVIDIA: the speculative CCD, the kinematic actors (a target + per step, reached at the end of the step, no velocity without target), the vertex limit of a convex hull, the + kinematic actor under a character controller +- [Godot](https://github.com/godotengine/godot): a character standing still on a slope stays there (`floor_stop_on_slope`) ## Contributing Guidelines diff --git a/actor/convexhull.go b/actor/convexhull.go new file mode 100644 index 0000000..3052353 --- /dev/null +++ b/actor/convexhull.go @@ -0,0 +1,794 @@ +package actor + +import ( + "errors" + "math" + "slices" + + "github.com/go-gl/mathgl/mgl64" +) + +// ========== CONVEX HULL ========== +// The convex hull of a cloud of points (the vertices of a mesh), built by quickhull (Barber, Dobkin & Huhdanpaa, "The +// Quickhull Algorithm for Convex Hulls", 1996; Dirk Gregorius, "Implementing QuickHull", GDC 2014): a tetrahedron of +// the points the furthest apart, then the point the furthest out of the hull is added at each step, the faces it sees +// are replaced by a cone of triangles from the horizon to this point, and the points out of the removed faces are given +// to the new faces. The ConvexHullBuilder of Jolt and b3CreateHull of Box3D (src/hull.c) build their hulls this way. +// Once every point is in the hull, the triangles coplanar within the tolerance are merged in polygons (the faces), +// each with its plane: the face of a cube is 1 polygon of 4 vertices, not 2 triangles, and the manifold of a cube +// resting on the ground has its 4 corners. Jolt and Box3D merge the faces during the construction (MergeCoplanarOrConcaveFaces, +// b3HullBuilder_MergeFaces); Feather merges once at the end, on a complete triangulation. +// +// The hull is a convex polytope: GJK/EPA takes it by its support point (the vertex the furthest along a direction, +// found among all the vertices, as HullNoConvex::GetSupport of Jolt and b3FindHullSupportVertex of Box3D), the contact +// clipping by its faces. Its mass, its center of mass and its inertia come from the integral of its volume: the sum of +// the tetrahedra between its faces and a point inside (ConvexHullShape of Jolt, after Blow & Binstock, "How to find the +// inertia tensor of a 3D solid body represented by a triangle mesh"). The points are then translated so that the center +// of mass is at the origin of the local space of the hull, where a body has its center of mass (Jolt does the same: +// its shapes are centered on their center of mass). + +const ( + // MaxHullVertices: a hull has at most this many vertices (256 in Jolt, cMaxPointsInHull, and in Box3D, + // B3_HULL_MAX_COUNT; 255 in PhysX, PxConvexMeshDesc::vertexLimit). Its support point is found among all of them + MaxHullVertices = 256 + + // hullToleranceFactor: the points within tolerance of a plane are on it. The tolerance is the rounding error of + // the distance of a point to a plane, from the size of the cloud: 3 x (|x| + |y| + |z|)max x epsilon (Gregorius, + // "Implementing QuickHull"; DetermineCoplanarDistance of Jolt, b3HullBuilder_ComputeTolerance of Box3D, in + // single precision; here in double) + hullToleranceFactor = 3 + + // hullMinOutsideFactor: a point is out of the hull if it is further than this many tolerances in front of a face + // (minOutside of Box3D: 8 tolerances, src/hull.c:644-645; points closer are on the hull, within its tolerance) + hullMinOutsideFactor = 8 + + // hullMinRadiusFactor: a face is seen by a point if the point is more than this many tolerances in front of its + // plane (minRadius of Box3D: 4 tolerances, src/hull.c:644, :868) + hullMinRadiusFactor = 4 + + // hullFeatureVertices: a face returned by GetContactFeature has at most this many vertices, the size of the + // buffer of the contact clipping. A face with more vertices gives one vertex out of k (GetSupportingFace of Jolt + // skips vertices the same way) + hullFeatureVertices = 8 +) + +var ( + // ErrHullDegenerate: the points are fewer than 4, or on a plane: no hull + ErrHullDegenerate = errors.New("feather: a convex hull needs 4 points not on a plane") + + // ErrHullVertexLimit: maxVertices is not in [4, MaxHullVertices] + ErrHullVertexLimit = errors.New("feather: the vertex limit of a convex hull is in [4, MaxHullVertices]") + + // ErrHullFailed: the construction lost the shape of the hull (the horizon of a point is not a loop): the points + // are too close to coplanar for the tolerance + ErrHullFailed = errors.New("feather: the convex hull could not be built from these points") +) + +// ConvexHull is a convex polytope: the convex hull of a cloud of points, built by NewConvexHull. In its local space +// the center of mass is at the origin: a body carrying the hull is at the center of mass of the cloud (CenterOfMass). +// A hull can be dynamic +type ConvexHull struct { + // Points: the vertices of the hull, in its local space (the center of mass at the origin) + Points []mgl64.Vec3 + + // faces: the polygons, their vertices in faceVertices (indices in Points, counterclockwise seen from outside) + faces []hullFace + faceVertices []int32 + // centerOfMass of the hull in the space of the points given to NewConvexHull + centerOfMass mgl64.Vec3 + volume float64 + // inertia for a density of 1, about the center of mass, in the local space + inertia mgl64.Mat3 + bounds AABB +} + +// hullFace: a polygon of the hull and its plane (Normal·p + Distance = 0, the normal out of the hull) +type hullFace struct { + normal mgl64.Vec3 + distance float64 + first int32 + count int32 +} + +// NewConvexHull builds the convex hull of the points, with maxVertices vertices at most (in [4, MaxHullVertices]). +// The points inside the hull, or on its surface within the rounding, are not vertices. If the limit is reached, the +// hull stops growing: the points left out are the closest to the hull, since the furthest point is added first +// (Jolt and Box3D stop the same way, PhysX expands the hull by its planes instead). The points are read, not kept +func NewConvexHull(points []mgl64.Vec3, maxVertices int) (*ConvexHull, error) { + if maxVertices < 4 || maxVertices > MaxHullVertices { + return nil, ErrHullVertexLimit + } + if len(points) < 4 { + return nil, ErrHullDegenerate + } + var b hullBuilder + if err := b.build(points, maxVertices); err != nil { + return nil, err + } + return b.finish() +} + +// CenterOfMass of the hull in the space of the points given to NewConvexHull. The local space of the hull is centered +// on it: a body carrying the hull of a mesh is at transform.ToWorld(CenterOfMass()) of the mesh +func (h *ConvexHull) CenterOfMass() mgl64.Vec3 { + return h.centerOfMass +} + +// Volume of the hull (m³) +func (h *ConvexHull) Volume() float64 { + return h.volume +} + +// FaceCount: the number of faces (polygons) of the hull +func (h *ConvexHull) FaceCount() int { + return len(h.faces) +} + +// Face i: its plane (Normal·p + Distance = 0, the normal out of the hull) and the indices of its vertices in Points, +// counterclockwise seen from outside. The slice is read-only +func (h *ConvexHull) Face(i int) (normal mgl64.Vec3, distance float64, vertices []int32) { + face := &h.faces[i] + return face.normal, face.distance, h.faceVertices[face.first : face.first+face.count] +} + +// ComputeAABB: the local bounds of the hull turned and moved: the AABB of their 8 corners, as b3ComputeHullAABB of +// Box3D (b3AABB_Transform of the local AABB) and the local bounds of ConvexHullShape of Jolt, not the AABB of the +// vertices (tighter, but a rotation of every vertex at each step) +func (h *ConvexHull) ComputeAABB(transform Transform) AABB { + q := &transform.Rotation + min := mgl64.Vec3{math.Inf(1), math.Inf(1), math.Inf(1)} + max := mgl64.Vec3{math.Inf(-1), math.Inf(-1), math.Inf(-1)} + for i := 0; i < 8; i++ { + corner := h.bounds.Min + if i&1 != 0 { + corner[0] = h.bounds.Max[0] + } + if i&2 != 0 { + corner[1] = h.bounds.Max[1] + } + if i&4 != 0 { + corner[2] = h.bounds.Max[2] + } + world := Rotate(q, corner).Add(transform.Position) + for k := 0; k < 3; k++ { + min[k] = math.Min(min[k], world[k]) + max[k] = math.Max(max[k], world[k]) + } + } + return AABB{Min: min, Max: max} +} + +// ComputeMass: the density times the volume of the hull +func (h *ConvexHull) ComputeMass(density float64) float64 { + return density * h.volume +} + +// ComputeInertia about the center of mass, in the local space: the inertia of the volume, scaled to the mass +func (h *ConvexHull) ComputeInertia(mass float64) mgl64.Mat3 { + if h.volume <= 0 { + return mgl64.Mat3{} + } + scale := mass / h.volume + var inertia mgl64.Mat3 + for i := range inertia { + inertia[i] = h.inertia[i] * scale + } + return inertia +} + +// Support: the vertex the furthest along the direction, among all of them (HullNoConvex::GetSupport of Jolt, +// b3FindHullSupportVertex of Box3D). At the same distance, the first one +func (h *ConvexHull) Support(direction mgl64.Vec3) mgl64.Vec3 { + best, bestDot := 0, math.Inf(-1) + for i, p := range h.Points { + if dot := p.Dot(direction); dot > bestDot { + best, bestDot = i, dot + } + } + return h.Points[best] +} + +// supportingFace: the face whose normal is the most aligned with the direction (GetSupportingFace of Jolt, +// b3FindHullSupportFace of Box3D) +func (h *ConvexHull) supportingFace(direction mgl64.Vec3) int { + best, bestDot := 0, math.Inf(-1) + for i := range h.faces { + if dot := h.faces[i].normal.Dot(direction); dot > bestDot { + best, bestDot = i, dot + } + } + return best +} + +// GetContactFeature: the vertices of the face the most aligned with the direction, hullFeatureVertices at most: a face +// with more vertices gives one out of k, around the polygon (GetSupportingFace of Jolt) +func (h *ConvexHull) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { + face := &h.faces[h.supportingFace(direction)] + vertices := h.faceVertices[face.first : face.first+face.count] + step := (len(vertices) + hullFeatureVertices - 1) / hullFeatureVertices + *count = 0 + for i := 0; i < len(vertices) && *count < hullFeatureVertices; i += step { + output[*count] = h.Points[vertices[i]] + *count++ + } +} + +// CollideWithPlane returns the vertices of the supporting face of the hull (the face the most opposed to the normal of +// the plane) closer to the plane than the margin, as the Box does +func (h *ConvexHull) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { + var face [8]mgl64.Vec3 + var count int + q := &myTransform.Rotation + h.GetContactFeature(RotateInverse(q, planeNormal.Mul(-1)), &face, &count) + for _, vertex := range face[:count] { + worldVertex := myTransform.Position.Add(Rotate(q, vertex)) + separation := worldVertex.Dot(planeNormal) + planeDistance + if separation > margin { + continue + } + contacts = append(contacts, ContactPoint{ + Position: worldVertex.Sub(planeNormal.Mul(separation / 2)), + Separation: separation, + }) + } + return contacts +} + +// CastRay against the planes of the faces: the ray is in the hull between the last plane it enters and the first one +// it leaves (b3RayCastHull of Box3D, CastRayHelper of ConvexHullShape of Jolt). The normal is the one of the face it +// enters by +func (h *ConvexHull) CastRay(origin, translation mgl64.Vec3, maxFraction float64) (RayHit, bool) { + if !finiteRay(origin, translation) { + return RayHit{}, false + } + enter, exit, face := 0.0, maxFraction, -1 + inside := true + for i := range h.faces { + plane := &h.faces[i] + distance := origin.Dot(plane.normal) + plane.distance + if distance > 0 { + inside = false + } + descent := translation.Dot(plane.normal) + if descent == 0 { + // along the plane: in front of it all the way, or never + if distance > 0 { + return RayHit{}, false + } + continue + } + fraction := -distance / descent + if descent < 0 { + // the ray enters this plane + if fraction > enter { + enter, face = fraction, i + } + } else if fraction < exit { + exit = fraction + } + if enter > exit { + return RayHit{}, false + } + } + if inside { + return startInside(translation), true + } + if face < 0 { + return RayHit{}, false + } + return RayHit{Fraction: enter, Normal: h.faces[face].normal, Triangle: NoTriangle}, true +} + +// ========== QUICKHULL ========== + +// qhFace: a triangle of the hull being built +type qhFace struct { + vertices [3]int32 + // neighbors[e]: the face across the edge from vertices[e] to vertices[e+1] + neighbors [3]int32 + normal mgl64.Vec3 // unit + distance float64 // normal·p + distance = 0 + centroid mgl64.Vec3 + // conflicts: the points in front of the face, further than minOutside, the furthest last + conflicts []int32 + furthest float64 + alive bool + // visited stamp of the horizon search + visited uint32 +} + +// hullBuilder: the state of quickhull +type hullBuilder struct { + points []mgl64.Vec3 + // origin: the center of the cloud, taken out of the points for the precision + origin mgl64.Vec3 + tolerance float64 + minRadius float64 + minOutside float64 + faces []qhFace + // vertices in the hull + count int + stamp uint32 + // buffers of a step + visible []int32 + horizon []horizonEdge + cone []int32 +} + +// horizonEdge: an edge of a visible face whose other face is not visible: from a to b, seen from the visible face +type horizonEdge struct { + a, b int32 + outside int32 // the face across, not visible + edge int32 // the edge of outside from b to a +} + +// build the triangulated hull +func (b *hullBuilder) build(input []mgl64.Vec3, maxVertices int) error { + // the points around their center: the tolerance follows the size of the cloud, not its distance to the origin + bounds := AABB{Min: input[0], Max: input[0]} + for _, p := range input[1:] { + for k := 0; k < 3; k++ { + bounds.Min[k] = math.Min(bounds.Min[k], p[k]) + bounds.Max[k] = math.Max(bounds.Max[k], p[k]) + } + } + b.origin = bounds.Min.Add(bounds.Max).Mul(0.5) + b.points = make([]mgl64.Vec3, len(input)) + largest := mgl64.Vec3{} + for i, p := range input { + b.points[i] = p.Sub(b.origin) + for k := 0; k < 3; k++ { + largest[k] = math.Max(largest[k], math.Abs(b.points[i][k])) + } + } + b.tolerance = hullToleranceFactor * (largest[0] + largest[1] + largest[2]) * epsilon64 + b.minRadius = hullMinRadiusFactor * b.tolerance + b.minOutside = hullMinOutsideFactor * b.tolerance + + if !b.initialTetrahedron() { + return ErrHullDegenerate + } + for i := range b.points { + b.assign(int32(i), b.faces) + } + + for b.count < maxVertices { + face := b.furthestFace() + if face < 0 { + break + } + apex := b.faces[face].conflicts[len(b.faces[face].conflicts)-1] + b.faces[face].conflicts = b.faces[face].conflicts[:len(b.faces[face].conflicts)-1] + if !b.addPoint(int32(face), apex) { + return ErrHullFailed + } + } + return nil +} + +// epsilon64: the relative rounding error of a float64 (DBL_EPSILON) +const epsilon64 = 0x1p-52 + +// initialTetrahedron: the 2 points the furthest apart along the axis of the largest extent, the point the furthest +// from their line, the point the furthest from their plane (b3HullBuilder_BuildInitialHull of Box3D, the initial +// simplex of PhysX, QuickHull::findSimplex). False if the cloud is flat, within the tolerance +func (b *hullBuilder) initialTetrahedron() bool { + var low, high [3]int + for i, p := range b.points { + for k := 0; k < 3; k++ { + if p[k] < b.points[low[k]][k] { + low[k] = i + } + if p[k] > b.points[high[k]][k] { + high[k] = i + } + } + } + axis := 0 + for k := 1; k < 3; k++ { + if b.points[high[k]][k]-b.points[low[k]][k] > b.points[high[axis]][axis]-b.points[low[axis]][axis] { + axis = k + } + } + i1, i2 := low[axis], high[axis] + if b.points[i2][axis]-b.points[i1][axis] <= 2*b.tolerance { + return false + } + + // the furthest from the line + a, line := b.points[i1], b.points[i2].Sub(b.points[i1]) + i3, best := -1, 4*b.tolerance*b.tolerance*line.LenSqr() + for i, p := range b.points { + if i == i1 || i == i2 { + continue + } + if cross := p.Sub(a).Cross(line).LenSqr(); cross > best { + i3, best = i, cross + } + } + if i3 < 0 { + return false + } + + // the furthest from the plane + normal := line.Cross(b.points[i3].Sub(a)).Normalize() + i4, bestDistance := -1, 2*b.tolerance + for i, p := range b.points { + if i == i1 || i == i2 || i == i3 { + continue + } + if distance := math.Abs(p.Sub(a).Dot(normal)); distance > bestDistance { + i4, bestDistance = i, distance + } + } + if i4 < 0 { + return false + } + // the 4th point behind the face (i1, i2, i3): its normal points out + if b.points[i4].Sub(a).Dot(normal) > 0 { + i2, i3 = i3, i2 + } + v1, v2, v3, v4 := int32(i1), int32(i2), int32(i3), int32(i4) + b.faces = append(b.faces, + b.triangle(v1, v2, v3, [3]int32{1, 2, 3}), + b.triangle(v2, v1, v4, [3]int32{0, 3, 2}), + b.triangle(v3, v2, v4, [3]int32{0, 1, 3}), + b.triangle(v1, v3, v4, [3]int32{0, 2, 1}), + ) + b.count = 4 + return true +} + +// triangle: a face from 3 vertices, counterclockwise seen from outside, with its neighbors across its 3 edges +func (b *hullBuilder) triangle(v0, v1, v2 int32, neighbors [3]int32) qhFace { + p0, p1, p2 := b.points[v0], b.points[v1], b.points[v2] + normal := p1.Sub(p0).Cross(p2.Sub(p0)).Normalize() + centroid := p0.Add(p1).Add(p2).Mul(1.0 / 3) + return qhFace{vertices: [3]int32{v0, v1, v2}, neighbors: neighbors, normal: normal, distance: -normal.Dot(centroid), centroid: centroid, alive: true} +} + +// separation of the point from the plane of the face, positive in front +func (f *qhFace) separation(p mgl64.Vec3) float64 { + return p.Dot(f.normal) + f.distance +} + +// assign the point to the face it is the furthest in front of, among faces, if it is out of the hull (further than +// minOutside): the conflict lists of Barber et al. (the outside sets), kept with the furthest point last +func (b *hullBuilder) assign(point int32, faces []qhFace) { + p := b.points[point] + best, bestSeparation := -1, b.minOutside + for i := range faces { + if !faces[i].alive { + continue + } + if separation := faces[i].separation(p); separation > bestSeparation { + best, bestSeparation = i, separation + } + } + if best < 0 { + return + } + face := &faces[best] + if bestSeparation > face.furthest { + face.furthest = bestSeparation + face.conflicts = append(face.conflicts, point) + return + } + // not the furthest: before the last one + face.conflicts = append(face.conflicts, 0) + copy(face.conflicts[len(face.conflicts)-1:], face.conflicts[len(face.conflicts)-2:]) + face.conflicts[len(face.conflicts)-2] = point +} + +// furthestFace: the face with the furthest point in front of it, -1 if no point is out of the hull +func (b *hullBuilder) furthestFace() int { + best, furthest := -1, 0.0 + for i := range b.faces { + face := &b.faces[i] + if face.alive && len(face.conflicts) > 0 && face.furthest > furthest { + best, furthest = i, face.furthest + } + } + return best +} + +// addPoint adds the apex to the hull: the faces which see it (from the face it is in front of, through their +// neighbors) are removed, a cone of triangles is built from their horizon to the apex, and the points of the removed +// faces are given to the cone. False if the horizon is not a loop: the hull is lost +func (b *hullBuilder) addPoint(seed, apex int32) bool { + p := b.points[apex] + b.stamp++ + b.visible, b.horizon = b.visible[:0], b.horizon[:0] + b.faces[seed].visited = b.stamp + b.visible = append(b.visible, seed) + for i := 0; i < len(b.visible); i++ { + face := &b.faces[b.visible[i]] + for e := 0; e < 3; e++ { + other := face.neighbors[e] + neighbor := &b.faces[other] + if neighbor.visited == b.stamp { + continue + } + if neighbor.separation(p) > b.minRadius { + neighbor.visited = b.stamp + b.visible = append(b.visible, other) + continue + } + // the edge of the neighbor from vertices[e+1] to vertices[e] + a, c := face.vertices[e], face.vertices[(e+1)%3] + edge := int32(-1) + for k := 0; k < 3; k++ { + if neighbor.vertices[k] == c && neighbor.vertices[(k+1)%3] == a { + edge = int32(k) + } + } + if edge < 0 { + return false + } + b.horizon = append(b.horizon, horizonEdge{a: a, b: c, outside: other, edge: edge}) + } + } + // a face which is not visible is tested from each visible face beside it, and gives one horizon edge each time + + // the horizon as a loop: each vertex starts one edge + b.cone = b.cone[:0] + for range b.horizon { + b.cone = append(b.cone, -1) + } + for i := range b.horizon { + h := &b.horizon[i] + for j := range b.horizon { + if b.horizon[j].a == h.b { + if b.cone[i] >= 0 { + return false + } + b.cone[i] = int32(j) + } + } + if b.cone[i] < 0 { + return false + } + } + + // the cone: a triangle per horizon edge, in the slots of the visible faces, then appended + first := len(b.faces) + for range b.horizon { + b.faces = append(b.faces, qhFace{}) + } + for i, h := range b.horizon { + index := int32(first + i) + face := b.triangle(h.a, h.b, apex, [3]int32{h.outside, int32(first) + b.cone[i], 0}) + b.faces[index] = face + b.faces[h.outside].neighbors[h.edge] = index + } + for i := range b.horizon { + // the previous edge of the loop ends at this one: across the edge apex -> a + next := int32(first) + b.cone[i] + b.faces[next].neighbors[2] = int32(first + i) + } + for i := range b.horizon { + if normal := b.faces[first+i].normal; math.IsNaN(normal[0]) || normal == (mgl64.Vec3{}) { + return false + } + } + + // the points of the removed faces go to the cone + cone := b.faces[first:] + for _, index := range b.visible { + face := &b.faces[index] + face.alive = false + for _, point := range face.conflicts { + if point != apex { + b.assign(point, cone) + } + } + face.conflicts = nil + } + b.count++ + return true +} + +// ========== THE FACES ========== + +// finish merges the coplanar triangles in polygons, keeps the vertices used, computes the mass properties and +// centers the hull on its center of mass +func (b *hullBuilder) finish() (*ConvexHull, error) { + // the alive faces, by index; group[i]: the group of the face i (union-find) + group := make([]int32, len(b.faces)) + for i := range group { + group[i] = int32(i) + } + find := func(i int32) int32 { + for group[i] != i { + group[i] = group[group[i]] + i = group[i] + } + return i + } + // 2 faces across an edge are merged when the edge is not convex: the centroid of one is not behind the plane of + // the other by more than the tolerance (coplanar, or bent inwards by the rounding: b3IsEdgeConvex of Box3D, both + // ways as MergeCoplanarOrConcaveFaces of Jolt) + for i := range b.faces { + face := &b.faces[i] + if !face.alive { + continue + } + for e := 0; e < 3; e++ { + other := &b.faces[face.neighbors[e]] + if face.separation(other.centroid) >= -b.tolerance || other.separation(face.centroid) >= -b.tolerance { + a, c := find(int32(i)), find(face.neighbors[e]) + if a != c { + group[a] = c + } + } + } + } + + // the polygon of each group: its boundary edges, chained + h := &ConvexHull{} + used := make([]int32, len(b.points)) + for i := range used { + used[i] = -1 + } + type boundary struct{ a, c int32 } + var edges []boundary + var loop []int32 + for i := range b.faces { + if !b.faces[i].alive || find(int32(i)) != int32(i) { + continue + } + edges = edges[:0] + for j := range b.faces { + face := &b.faces[j] + if !face.alive || find(int32(j)) != int32(i) { + continue + } + for e := 0; e < 3; e++ { + if find(face.neighbors[e]) != int32(i) { + edges = append(edges, boundary{face.vertices[e], face.vertices[(e+1)%3]}) + } + } + } + if len(edges) < 3 { + return nil, ErrHullFailed + } + loop = loop[:0] + loop = append(loop, edges[0].a) + for next := edges[0].c; next != edges[0].a; { + found := false + for _, edge := range edges { + if edge.a == next { + loop = append(loop, next) + next, found = edge.c, true + break + } + } + if !found || len(loop) > len(edges) { + return nil, ErrHullFailed + } + } + if len(loop) != len(edges) { + return nil, ErrHullFailed + } + loop = b.dropCollinear(loop) + if len(loop) < 3 { + return nil, ErrHullFailed + } + // the plane of the polygon: Newell's normal, through its centroid + normal, centroid := mgl64.Vec3{}, mgl64.Vec3{} + for k, v := range loop { + p, q := b.points[v], b.points[loop[(k+1)%len(loop)]] + normal = normal.Add(mgl64.Vec3{(p[1] - q[1]) * (p[2] + q[2]), (p[2] - q[2]) * (p[0] + q[0]), (p[0] - q[0]) * (p[1] + q[1])}) + centroid = centroid.Add(p) + } + if normal.Len() == 0 { + return nil, ErrHullFailed + } + normal = normal.Normalize() + centroid = centroid.Mul(1 / float64(len(loop))) + face := hullFace{normal: normal, distance: -normal.Dot(centroid), first: int32(len(h.faceVertices)), count: int32(len(loop))} + for _, v := range loop { + if used[v] < 0 { + used[v] = int32(len(h.Points)) + h.Points = append(h.Points, b.points[v]) + } + h.faceVertices = append(h.faceVertices, used[v]) + } + h.faces = append(h.faces, face) + } + if len(h.faces) < 4 || len(h.Points) < 4 { + return nil, ErrHullFailed + } + + h.massProperties() + h.centerOfMass = h.centerOfMass.Add(b.origin) + h.bounds = AABB{Min: h.Points[0], Max: h.Points[0]} + for _, p := range h.Points[1:] { + for k := 0; k < 3; k++ { + h.bounds.Min[k] = math.Min(h.bounds.Min[k], p[k]) + h.bounds.Max[k] = math.Max(h.bounds.Max[k], p[k]) + } + } + return h, nil +} + +// dropCollinear removes from the loop the vertices on the segment between their neighbors, within the tolerance (a +// vertex of merged triangles in the middle of a side of the polygon), and the repeated vertices +func (b *hullBuilder) dropCollinear(loop []int32) []int32 { + for changed := true; changed && len(loop) > 3; { + changed = false + for k := 0; k < len(loop); k++ { + previous, current, next := b.points[loop[(k+len(loop)-1)%len(loop)]], b.points[loop[k]], b.points[loop[(k+1)%len(loop)]] + side := next.Sub(previous) + if loop[k] == loop[(k+1)%len(loop)] || current.Sub(previous).Cross(side).Len() <= b.tolerance*side.Len() { + loop = slices.Delete(loop, k, k+1) + changed = true + break + } + } + } + return loop +} + +// massProperties: the volume, the center of mass and the inertia of the hull, from the tetrahedra between its faces +// (fans of triangles) and a point inside (the mean of the vertices). The inertia for a density of 1 is the covariance +// of the canonical tetrahedron carried by each tetrahedron (ConvexHullShape of Jolt, after Blow & Binstock). The +// points are then centered on the center of mass +func (h *ConvexHull) massProperties() { + inside := mgl64.Vec3{} + for _, p := range h.Points { + inside = inside.Add(p) + } + inside = inside.Mul(1 / float64(len(h.Points))) + + volume, center := 0.0, mgl64.Vec3{} + for _, face := range h.faces { + vertices := h.faceVertices[face.first : face.first+face.count] + v1 := h.Points[vertices[0]].Sub(inside) + for k := 1; k+1 < len(vertices); k++ { + v2, v3 := h.Points[vertices[k]].Sub(inside), h.Points[vertices[k+1]].Sub(inside) + // 6 times the volume, 4 times the centroid: divided at the end + tetra := v1.Dot(v2.Cross(v3)) + volume += tetra + center = center.Add(v1.Add(v2).Add(v3).Mul(tetra)) + } + } + if volume > 0 { + center = center.Mul(1 / (4 * volume)) + } + center = center.Add(inside) + h.volume = volume / 6 + h.centerOfMass = center + for i := range h.Points { + h.Points[i] = h.Points[i].Sub(center) + } + for i := range h.faces { + h.faces[i].distance += h.faces[i].normal.Dot(center) + } + + // the covariance of the canonical tetrahedron (0, e1, e2, e3): 1/60 on the diagonal, 1/120 elsewhere, for a + // density of 1. A tetrahedron (0, v1, v2, v3) is its image by A = [v1 v2 v3]: covariance det(A) A C Aᵀ + canonical := mgl64.Mat3{1.0 / 60, 1.0 / 120, 1.0 / 120, 1.0 / 120, 1.0 / 60, 1.0 / 120, 1.0 / 120, 1.0 / 120, 1.0 / 60} + var covariance mgl64.Mat3 + for _, face := range h.faces { + vertices := h.faceVertices[face.first : face.first+face.count] + v1 := h.Points[vertices[0]] + for k := 1; k+1 < len(vertices); k++ { + v2, v3 := h.Points[vertices[k]], h.Points[vertices[k+1]] + a := mgl64.Mat3{v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], v3[0], v3[1], v3[2]} + det := Det3(&a) + ac := Mul3(&a, &canonical) + at := Transpose3(&a) + c := Mul3(&ac, &at) + for i := range covariance { + covariance[i] += det * c[i] + } + } + } + trace := covariance[0] + covariance[4] + covariance[8] + h.inertia = mgl64.Mat3{ + trace - covariance[0], -covariance[1], -covariance[2], + -covariance[3], trace - covariance[4], -covariance[5], + -covariance[6], -covariance[7], trace - covariance[8], + } +} diff --git a/actor/convexhull_test.go b/actor/convexhull_test.go new file mode 100644 index 0000000..58a39c4 --- /dev/null +++ b/actor/convexhull_test.go @@ -0,0 +1,376 @@ +package actor + +import ( + "errors" + "math" + "math/rand" + "testing" + + "github.com/go-gl/mathgl/mgl64" +) + +// cubeCorners of half size half, around the center +func cubeCorners(half float64, center mgl64.Vec3) []mgl64.Vec3 { + corners := make([]mgl64.Vec3, 0, 8) + for i := 0; i < 8; i++ { + corners = append(corners, mgl64.Vec3{float64(i&1)*2 - 1, float64(i>>1&1)*2 - 1, float64(i>>2&1)*2 - 1}.Mul(half).Add(center)) + } + return corners +} + +// hullEdges: the count of edges of the hull, each shared by 2 faces +func hullEdges(h *ConvexHull) int { + sides := 0 + for i := 0; i < h.FaceCount(); i++ { + _, _, vertices := h.Face(i) + sides += len(vertices) + } + return sides / 2 +} + +// checkHull: the hull is convex and closed: every point of the cloud is behind every face (within slack), every vertex +// is a point of the cloud, every face is a polygon turning counterclockwise around its normal, and V - E + F = 2 +func checkHull(t *testing.T, h *ConvexHull, cloud []mgl64.Vec3, slack float64) { + t.Helper() + offset := h.CenterOfMass() + for i := 0; i < h.FaceCount(); i++ { + normal, distance, vertices := h.Face(i) + if len(vertices) < 3 { + t.Fatalf("face %d has %d vertices", i, len(vertices)) + } + if math.Abs(normal.Len()-1) > 1e-12 { + t.Fatalf("face %d: normal %v is not unit", i, normal) + } + for _, p := range cloud { + if separation := p.Sub(offset).Dot(normal) + distance; separation > slack { + t.Fatalf("face %d: the point %v is %.3g out of the hull", i, p, separation) + } + } + for k, v := range vertices { + if separation := h.Points[v].Dot(normal) + distance; math.Abs(separation) > slack { + t.Fatalf("face %d: its vertex %d is %.3g off its plane", i, k, separation) + } + // the polygon turns counterclockwise seen from outside: each corner turns towards the inside + previous, current, next := h.Points[vertices[(k+len(vertices)-1)%len(vertices)]], h.Points[v], h.Points[vertices[(k+1)%len(vertices)]] + if current.Sub(previous).Cross(next.Sub(current)).Dot(normal) < 0 { + t.Fatalf("face %d turns clockwise at its vertex %d", i, k) + } + } + } + for i, p := range h.Points { + found := false + for _, c := range cloud { + if c.Sub(offset).Sub(p).Len() <= slack { + found = true + break + } + } + if !found { + t.Fatalf("the vertex %d (%v) is not a point of the cloud", i, p.Add(offset)) + } + } + if euler := len(h.Points) - hullEdges(h) + h.FaceCount(); euler != 2 { + t.Fatalf("V - E + F = %d - %d + %d = %d, want 2", len(h.Points), hullEdges(h), h.FaceCount(), euler) + } + // the support point is the furthest point of the cloud + r := rand.New(rand.NewSource(int64(len(cloud)))) + for i := 0; i < 200; i++ { + direction := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()} + support := h.Support(direction).Dot(direction) + for _, p := range cloud { + if dot := p.Sub(offset).Dot(direction); dot > support+slack*direction.Len() { + t.Fatalf("direction %v: the support %.9f is under the point %v (%.9f)", direction, support, p, dot) + } + } + } +} + +// Criterion 1: the hull of the corners of a cube, with points inside it, on its faces and on its edges, is the cube: +// 8 vertices, 6 faces of 4 vertices, the center of mass at its center; its mass and its inertia are those of the Box, +// within 1e-6 +func TestConvexHullOfANoisyCubeIsTheCube(t *testing.T) { + r := rand.New(rand.NewSource(1)) + center := mgl64.Vec3{1, 2, 3} + half := mgl64.Vec3{0.3, 0.5, 0.2} + var points []mgl64.Vec3 + for i := 0; i < 8; i++ { + points = append(points, mgl64.Vec3{(float64(i&1)*2 - 1) * half[0], (float64(i>>1&1)*2 - 1) * half[1], (float64(i>>2&1)*2 - 1) * half[2]}.Add(center)) + } + for i := 0; i < 1000; i++ { + points = append(points, mgl64.Vec3{(r.Float64()*2 - 1) * half[0], (r.Float64()*2 - 1) * half[1], (r.Float64()*2 - 1) * half[2]}.Add(center)) + } + // on the faces and on the edges, exactly + points = append(points, center.Add(mgl64.Vec3{half[0], 0, 0}), center.Add(mgl64.Vec3{0, -half[1], 0.1}), center.Add(mgl64.Vec3{half[0], half[1], 0}), center.Add(mgl64.Vec3{-half[0], 0.2, half[2]})) + r.Shuffle(len(points), func(i, j int) { points[i], points[j] = points[j], points[i] }) + + h, err := NewConvexHull(points, MaxHullVertices) + if err != nil { + t.Fatal(err) + } + if len(h.Points) != 8 || h.FaceCount() != 6 { + t.Fatalf("%d vertices and %d faces, want 8 and 6", len(h.Points), h.FaceCount()) + } + for i := 0; i < h.FaceCount(); i++ { + if _, _, vertices := h.Face(i); len(vertices) != 4 { + t.Errorf("face %d has %d vertices, want 4", i, len(vertices)) + } + } + if gap := h.CenterOfMass().Sub(center).Len(); gap > 1e-12 { + t.Errorf("center of mass %v, want %v", h.CenterOfMass(), center) + } + checkHull(t, h, points, 1e-12) + + box := Box{HalfExtents: half} + if mass, want := h.ComputeMass(700), box.ComputeMass(700); math.Abs(mass-want) > 1e-6*want { + t.Errorf("mass %.9f, want %.9f", mass, want) + } + inertia, want := h.ComputeInertia(50), box.ComputeInertia(50) + for i := range inertia { + if math.Abs(inertia[i]-want[i]) > 1e-6*want[0] { + t.Errorf("inertia %v, want %v", inertia, want) + break + } + } + // the bounds are the half extents, the support points the corners + if aabb := h.ComputeAABB(NewTransform()); aabb.Min.Add(half).Len() > 1e-12 || aabb.Max.Sub(half).Len() > 1e-12 { + t.Errorf("AABB %v, want ±%v", aabb, half) + } + if support := h.Support(mgl64.Vec3{1, -1, 1}); support.Sub(mgl64.Vec3{half[0], -half[1], half[2]}).Len() > 1e-12 { + t.Errorf("support %v, want a corner", support) + } +} + +// The hull of a random cloud contains all its points, its vertices are points of the cloud, it is convex and closed; a +// cloud far from the origin and a cloud on a sphere too. The hull of 250 points on a sphere has them all as vertices, +// its volume is the one of the ball within 15 %, below it, its center of mass and its inertia are the ones of the ball +func TestConvexHullOfRandomClouds(t *testing.T) { + r := rand.New(rand.NewSource(2)) + for i := 0; i < 20; i++ { + count := 10 + r.Intn(300) + center := mgl64.Vec3{r.NormFloat64() * 100, r.NormFloat64() * 100, r.NormFloat64() * 100} + cloud := make([]mgl64.Vec3, count) + for k := range cloud { + cloud[k] = mgl64.Vec3{r.NormFloat64() * 0.5, r.NormFloat64() * 0.2, r.NormFloat64()}.Add(center) + } + h, err := NewConvexHull(cloud, MaxHullVertices) + if err != nil { + t.Fatalf("cloud %d: %v", i, err) + } + checkHull(t, h, cloud, 1e-9) + if h.Volume() <= 0 { + t.Errorf("cloud %d: volume %v", i, h.Volume()) + } + } + + cloud := make([]mgl64.Vec3, 250) + for k := range cloud { + cloud[k] = mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize().Mul(2) + } + h, err := NewConvexHull(cloud, MaxHullVertices) + if err != nil { + t.Fatal(err) + } + checkHull(t, h, cloud, 1e-9) + ball := 4.0 / 3 * math.Pi * 8 + t.Logf("sphere of 250 points: %d vertices, %d faces, volume %.4f (ball %.4f)", len(h.Points), h.FaceCount(), h.Volume(), ball) + if len(h.Points) != 250 { + t.Errorf("%d vertices, want the 250 points of the sphere", len(h.Points)) + } + if h.Volume() > ball || h.Volume() < 0.85*ball { + t.Errorf("volume %.4f, want the ball %.4f within 15 %%, below it", h.Volume(), ball) + } + if h.CenterOfMass().Len() > 0.05 { + t.Errorf("center of mass %v, want the center of the sphere", h.CenterOfMass()) + } + // the inertia of the ball: 2/5 m r², within 15 % + inertia := h.ComputeInertia(1) + if want := 0.4 * 4; math.Abs(inertia[0]-want) > 0.15*want || math.Abs(inertia[4]-want) > 0.15*want || math.Abs(inertia[8]-want) > 0.15*want { + t.Errorf("inertia %v, want %.3f on the diagonal", inertia, want) + } +} + +// The vertex limit stops the hull: at most this many vertices, the hull inside the full one, and the points the +// furthest out added first: the hull of 32 vertices of a cloud on a sphere keeps 80 % of the volume of the full one. +// A limit out of [4, MaxHullVertices] is refused +func TestConvexHullVertexLimit(t *testing.T) { + r := rand.New(rand.NewSource(3)) + cloud := make([]mgl64.Vec3, 500) + for k := range cloud { + cloud[k] = mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize() + } + full, err := NewConvexHull(cloud, MaxHullVertices) + if err != nil { + t.Fatal(err) + } + limited, err := NewConvexHull(cloud, 32) + if err != nil { + t.Fatal(err) + } + if len(limited.Points) > 32 || len(limited.Points) < 30 { + t.Errorf("%d vertices, want 32", len(limited.Points)) + } + for i := 0; i < full.FaceCount(); i++ { + normal, distance, _ := full.Face(i) + for _, p := range limited.Points { + if separation := p.Add(limited.CenterOfMass()).Sub(full.CenterOfMass()).Dot(normal) + distance; separation > 1e-9 { + t.Fatalf("a vertex of the limited hull is %.3g out of the full one", separation) + } + } + } + t.Logf("32 vertices: %d faces, volume %.4f of %.4f", limited.FaceCount(), limited.Volume(), full.Volume()) + if limited.Volume() < 0.8*full.Volume() { + t.Errorf("the limited hull keeps %.0f %% of the volume, want 80 %%", 100*limited.Volume()/full.Volume()) + } + for _, limit := range []int{0, 3, MaxHullVertices + 1} { + if _, err := NewConvexHull(cloud, limit); !errors.Is(err, ErrHullVertexLimit) { + t.Errorf("limit %d: %v, want ErrHullVertexLimit", limit, err) + } + } +} + +// Fewer than 4 points, points on a line or on a plane: no hull +func TestConvexHullDegenerate(t *testing.T) { + cases := map[string][]mgl64.Vec3{ + "3 points": {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}}, + "a line": {{0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {-1, 0, 0}}, + "a plane": {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}, {0.3, 0.7, 0}, {2, -1, 0}}, + "a point": {{1, 1, 1}, {1, 1, 1}, {1, 1, 1}, {1, 1, 1}}, + } + for name, points := range cases { + if _, err := NewConvexHull(points, 64); !errors.Is(err, ErrHullDegenerate) { + t.Errorf("%s: %v, want ErrHullDegenerate", name, err) + } + } + // a tetrahedron is the smallest hull + h, err := NewConvexHull([]mgl64.Vec3{{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, 4) + if err != nil || len(h.Points) != 4 || h.FaceCount() != 4 { + t.Fatalf("tetrahedron: %v, %d vertices, %d faces", err, len(h.Points), h.FaceCount()) + } + if math.Abs(h.Volume()-1.0/6) > 1e-12 { + t.Errorf("volume %.9f, want 1/6", h.Volume()) + } +} + +// A ray on the hull of a cube gives the hit of the Box: the planes of the faces. A ray from inside starts inside +func TestConvexHullCastRayIsTheBox(t *testing.T) { + half := mgl64.Vec3{0.3, 0.5, 0.2} + corners := cubeCorners(1, mgl64.Vec3{}) + for i := range corners { + corners[i] = mgl64.Vec3{corners[i][0] * half[0], corners[i][1] * half[1], corners[i][2] * half[2]} + } + h, err := NewConvexHull(corners, 8) + if err != nil { + t.Fatal(err) + } + box := Box{HalfExtents: half} + r := rand.New(rand.NewSource(4)) + hits := 0 + for i := 0; i < 2000; i++ { + origin := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize().Mul(1 + r.Float64()) + target := mgl64.Vec3{r.NormFloat64() * 0.4, r.NormFloat64() * 0.4, r.NormFloat64() * 0.4} + translation := target.Sub(origin).Mul(0.5 + r.Float64()) + want, wantOk := box.CastRay(origin, translation, 1) + hit, ok := h.CastRay(origin, translation, 1) + if ok != wantOk { + t.Fatalf("ray %d: hit %v, the box says %v", i, ok, wantOk) + } + if !ok { + continue + } + hits++ + if math.Abs(hit.Fraction-want.Fraction) > 1e-12 || hit.Normal.Sub(want.Normal).Len() > 1e-12 || hit.Triangle != NoTriangle { + t.Fatalf("ray %d: %v, the box says %v", i, hit, want) + } + } + if hits < 500 { + t.Fatalf("only %d hits", hits) + } + if hit, ok := h.CastRay(mgl64.Vec3{0.1, 0.1, 0.1}, mgl64.Vec3{1, 0, 0}, 1); !ok || hit.Fraction != 0 || hit.Normal != (mgl64.Vec3{-1, 0, 0}) { + t.Errorf("from inside: %v %v, want the fraction 0 against the ray", hit, ok) + } + if _, ok := h.CastRay(mgl64.Vec3{1, 0, 0}, mgl64.Vec3{0, 1, 0}, 1); ok { + t.Errorf("a ray beside the hull hits it") + } + if _, ok := h.CastRay(mgl64.Vec3{1, 0, 0}, mgl64.Vec3{-0.5, 0, 0}, 1); ok { + t.Errorf("a ray stopping before the hull hits it") + } +} + +// The contact feature is the face the most aligned with the direction, and a face of more than 8 vertices gives 8 of +// them around the polygon; the points against a plane are the vertices of the supporting face, as for the Box +func TestConvexHullFeatures(t *testing.T) { + var points []mgl64.Vec3 + for i := 0; i < 16; i++ { + a := float64(i) / 16 * 2 * math.Pi + points = append(points, mgl64.Vec3{0.3 * math.Cos(a), -0.2, 0.3 * math.Sin(a)}, mgl64.Vec3{0.3 * math.Cos(a), 0.2, 0.3 * math.Sin(a)}) + } + cylinder, err := NewConvexHull(points, 64) + if err != nil { + t.Fatal(err) + } + if len(cylinder.Points) != 32 || cylinder.FaceCount() != 18 { + t.Fatalf("%d vertices, %d faces, want 32 and 18", len(cylinder.Points), cylinder.FaceCount()) + } + var feature [8]mgl64.Vec3 + var count int + cylinder.GetContactFeature(mgl64.Vec3{0, -1, 0}, &feature, &count) + if count != 8 { + t.Fatalf("%d vertices of the bottom cap, want 8", count) + } + for k, p := range feature[:count] { + if p[1] != -0.2 { + t.Errorf("vertex %d of the bottom cap at %v", k, p) + } + // one vertex out of 2, around the cap: the neighbors are 45° apart + if k > 0 { + if cos := feature[k-1].Sub(mgl64.Vec3{0, -0.2, 0}).Normalize().Dot(p.Sub(mgl64.Vec3{0, -0.2, 0}).Normalize()); math.Abs(cos-math.Cos(math.Pi/4)) > 1e-9 { + t.Errorf("vertices %d and %d of the cap are at %.1f°, want 45", k-1, k, math.Acos(cos)*180/math.Pi) + } + } + } + cylinder.GetContactFeature(mgl64.Vec3{1, 0.1, 0}, &feature, &count) + if count != 4 { + t.Errorf("%d vertices of a side, want 4", count) + } + + // against a plane: the 4 corners of a face of a cube, the bottom ones only + cube, err := NewConvexHull(cubeCorners(0.25, mgl64.Vec3{}), 8) + if err != nil { + t.Fatal(err) + } + contacts := cube.CollideWithPlane(mgl64.Vec3{0, 1, 0}, 0, Transform{Position: mgl64.Vec3{0, 0.26, 0}, Rotation: mgl64.QuatIdent()}, 1, nil) + if len(contacts) != 4 { + t.Fatalf("%d points against the plane, want 4", len(contacts)) + } + for _, c := range contacts { + if math.Abs(c.Separation-0.01) > 1e-12 || math.Abs(c.Position[1]-0.005) > 1e-12 { + t.Errorf("point %v", c) + } + } + // the AABB of a turned hull contains its turned vertices + transform := Transform{Position: mgl64.Vec3{1, 2, 3}, Rotation: mgl64.QuatRotate(0.7, mgl64.Vec3{1, 2, 0.5}.Normalize())} + aabb := cylinder.ComputeAABB(transform) + for _, p := range cylinder.Points { + if !aabb.ContainsPoint(transform.ToWorld(p)) { + t.Fatalf("the vertex %v is out of the AABB %v", transform.ToWorld(p), aabb) + } + } +} + +// A hull is built by NewRigidBody as any shape: the mass and the inertia of its volume +func TestConvexHullBody(t *testing.T) { + h, err := NewConvexHull(cubeCorners(0.5, mgl64.Vec3{3, 3, 3}), 8) + if err != nil { + t.Fatal(err) + } + body := NewRigidBody(Transform{Position: h.CenterOfMass(), Rotation: mgl64.QuatIdent()}, h, BodyTypeDynamic, 1000) + if math.Abs(body.Material.GetMass()-1000) > 1e-6 { + t.Errorf("mass %v, want 1000", body.Material.GetMass()) + } + if want := 1000.0 / 6; math.Abs(body.InertiaLocal[0]-want) > 1e-6 || math.Abs(body.InverseInertiaLocal[0]-1/want) > 1e-9 { + t.Errorf("inertia %v, want %.3f", body.InertiaLocal, want) + } + if aabb := body.AABB(); aabb.Min != (mgl64.Vec3{2.5, 2.5, 2.5}) || aabb.Max != (mgl64.Vec3{3.5, 3.5, 3.5}) { + t.Errorf("AABB %v", aabb) + } +} diff --git a/actor/kinematic_test.go b/actor/kinematic_test.go new file mode 100644 index 0000000..dfff1cf --- /dev/null +++ b/actor/kinematic_test.go @@ -0,0 +1,90 @@ +package actor + +import ( + "errors" + "math" + "testing" + + "github.com/go-gl/mathgl/mgl64" +) + +// A kinematic body has no inverse mass nor inverse inertia for the solver, but keeps the mass of its shape and +// density, for the day it becomes dynamic. Its forces are ignored +func TestNewRigidBody_Kinematic(t *testing.T) { + box := &Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}} + body := NewRigidBody(NewTransform(), box, BodyTypeKinematic, 1000) + if body.BodyType != BodyTypeKinematic { + t.Fatalf("type %v, want kinematic", body.BodyType) + } + if mass := body.Material.GetMass(); mass != box.ComputeMass(1000) { + t.Errorf("mass %v, want the mass of the shape %v", mass, box.ComputeMass(1000)) + } + if body.InverseMass() != 0 { + t.Errorf("inverse mass %v, want 0", body.InverseMass()) + } + if body.GetInverseInertiaWorld() != (mgl64.Mat3{}) || body.GetFreeInverseInertiaWorld() != (mgl64.Mat3{}) { + t.Errorf("inverse inertia %v, want 0", body.GetInverseInertiaWorld()) + } + if body.InertiaLocal == (mgl64.Mat3{}) { + t.Errorf("the inertia of the shape is kept for a switch to dynamic") + } + body.AddForce(mgl64.Vec3{1, 1, 1}) + body.AddTorque(mgl64.Vec3{1, 1, 1}) + body.AddImpulse(mgl64.Vec3{1, 1, 1}) + body.AddAngularImpulse(mgl64.Vec3{1, 1, 1}) + if body.Force() != (mgl64.Vec3{}) || body.Torque() != (mgl64.Vec3{}) || body.Velocity != (mgl64.Vec3{}) || body.AngularVelocity != (mgl64.Vec3{}) { + t.Errorf("a kinematic body took a force %v, a torque %v or a velocity %v %v", body.Force(), body.Torque(), body.Velocity, body.AngularVelocity) + } + + // without density: no mass + massless := NewRigidBody(NewTransform(), box, BodyTypeKinematic, 0) + if mass := massless.Material.GetMass(); mass != 0 || math.IsNaN(mass) { + t.Errorf("mass %v without density, want 0", mass) + } +} + +// The target of a kinematic body is kept until the step takes it; it wakes the body up if it moves it. A body which +// is not kinematic refuses it with ErrNotKinematic, never in silence; neither call allocates +func TestKinematicTarget(t *testing.T) { + body := NewRigidBody(NewTransform(), &Sphere{Radius: 0.1}, BodyTypeKinematic, 1000) + if _, ok := body.KinematicTarget(); ok { + t.Fatalf("a new body has a target") + } + body.Sleep() + if err := body.SetKinematicTarget(body.Transform); err != nil { + t.Fatalf("a target on a kinematic body: %v", err) + } + if !body.IsSleeping { + t.Errorf("a target at the pose of the body woke it up") + } + target := Transform{Position: mgl64.Vec3{1, 2, 3}, Rotation: mgl64.QuatRotate(0.5, mgl64.Vec3{0, 1, 0})} + if err := body.SetKinematicTarget(target); err != nil { + t.Fatalf("a target on a kinematic body: %v", err) + } + if got, ok := body.KinematicTarget(); !ok || got != target { + t.Errorf("target %v %v, want %v", got, ok, target) + } + if body.IsSleeping { + t.Errorf("a target away from the body didn't wake it up") + } + body.ClearKinematicTarget() + if _, ok := body.KinematicTarget(); ok { + t.Errorf("the target was not cleared") + } + + for _, bodyType := range []BodyType{BodyTypeDynamic, BodyTypeStatic} { + other := NewRigidBody(NewTransform(), &Sphere{Radius: 0.1}, bodyType, 1000) + if err := other.SetKinematicTarget(target); !errors.Is(err, ErrNotKinematic) { + t.Errorf("a target on a body of type %v: error %v, want ErrNotKinematic", bodyType, err) + } + if _, ok := other.KinematicTarget(); ok { + t.Errorf("a body of type %v took a kinematic target", bodyType) + } + if allocs := testing.AllocsPerRun(10, func() { _ = other.SetKinematicTarget(target) }); allocs > 0 { + t.Errorf("the refusal of a target allocates %.1f, want 0", allocs) + } + } + if allocs := testing.AllocsPerRun(10, func() { _ = body.SetKinematicTarget(target) }); allocs > 0 { + t.Errorf("a target allocates %.1f, want 0", allocs) + } +} diff --git a/actor/rigidbody.go b/actor/rigidbody.go index a922420..a58ed00 100644 --- a/actor/rigidbody.go +++ b/actor/rigidbody.go @@ -1,6 +1,7 @@ package actor import ( + "errors" "math" "sync/atomic" @@ -18,6 +19,13 @@ const ( // BodyTypeStatic bodies are immovable and have infinite mass // They are not affected by forces or gravity (e.g., ground, walls) BodyTypeStatic + + // BodyTypeKinematic bodies go where the game puts them (SetKinematicTarget: a pose per step), with the velocity + // of this motion, and push the dynamic bodies they meet: infinite mass for the solver, no force, no gravity (a + // moving platform, a character, the animated bones of a creature). They don't collide with the static and the + // kinematic bodies. World.SetBodyType changes a body between kinematic and dynamic, World.Teleport places it + // without velocity + BodyTypeKinematic ) const ( @@ -84,10 +92,7 @@ type RigidBody struct { accumulatedForce mgl64.Vec3 accumulatedTorque mgl64.Vec3 - IsTrigger bool - // IsBullet: a fast body is stopped at its first impact with the dynamic bodies too, not only with the static ones - // (continuous collision). For small fast bodies: projectiles - IsBullet bool + IsTrigger bool IsSleeping bool SleepTimer float64 @@ -111,13 +116,18 @@ type RigidBody struct { aabb AABB // serial: a unique number, given by NewRigidBody serial uint64 + + // target: the pose a kinematic body reaches at the end of the next step (SetKinematicTarget), if targeted + target Transform + targeted bool } // serials of the bodies created by NewRigidBody var serials atomic.Uint64 // NewRigidBody creates a new rigid body with the given properties -// density is used to calculate mass for dynamic bodies (ignored for static) +// density is used to calculate mass for dynamic and kinematic bodies (ignored for static): a kinematic body keeps the +// mass of its shape for the day it becomes dynamic (World.SetBodyType), the solver never reads it func NewRigidBody(transform Transform, shape ShapeInterface, bodyType BodyType, density float64) *RigidBody { transform.Rotation = transform.Rotation.Normalize() rb := &RigidBody{ @@ -140,7 +150,7 @@ func NewRigidBody(transform Transform, shape ShapeInterface, bodyType BodyType, DynamicFriction: 0.0, } } else { - // Dynamic bodies compute mass from shape and density + // Dynamic and kinematic bodies compute mass from shape and density rb.Material = Material{ Density: density, mass: shape.ComputeMass(density), @@ -152,7 +162,8 @@ func NewRigidBody(transform Transform, shape ShapeInterface, bodyType BodyType, } } - // a static body has no inertia (its inverse inertia is 0, it never turns) + // a static body has no inertia (its inverse inertia is 0, it never turns); a kinematic body keeps the one of its + // shape for a switch to dynamic (GetInverseInertiaWorld gives 0 while it is kinematic) if bodyType != BodyTypeStatic { rb.InertiaLocal = shape.ComputeInertia(rb.Material.mass) rb.InverseInertiaLocal = rb.InertiaLocal.Inv() @@ -162,6 +173,40 @@ func NewRigidBody(transform Transform, shape ShapeInterface, bodyType BodyType, return rb } +// SetShape gives the body another shape, in place: the mass and the inertia of a dynamic or a kinematic body become +// those of the shape at the density of its material (a static body keeps its infinite mass), the AABB follows; the +// velocities, the locks, the material and the target are kept. The shapes of Feather are centered on their origin (the +// hull on its center of mass): the body stays where it is, where Body::SetShapeInternal of Jolt (Body.cpp:117-141, +// v5.3.0) moves the body by the difference of the centers of mass. A body in a World changes its shape by +// World.SetShape, which refuses what cannot be simulated and tells the world +func (rb *RigidBody) SetShape(shape ShapeInterface) { + rb.Shape = shape + rb.updateMassProperties() + rb.UpdateAABB() +} + +// SetDensity gives the body another density, in place: the mass and the inertia of a dynamic or a kinematic body are +// computed again from its shape (PxRigidBodyExt::updateMassAndInertia of PhysX, which the game calls after a change of +// density or of geometry; Jolt recomputes them in SetShape); a static body keeps the density, for the day it becomes +// dynamic, and its infinite mass. A body in a World changes its density by World.SetDensity, which wakes it up +func (rb *RigidBody) SetDensity(density float64) { + rb.Material.Density = density + rb.updateMassProperties() +} + +// updateMassProperties: the mass and the inertia of the shape at the density of the material, for a dynamic or a +// kinematic body (the formulas of NewRigidBody); a static body has an infinite mass and no inertia +func (rb *RigidBody) updateMassProperties() { + if rb.BodyType == BodyTypeStatic { + rb.Material.mass = math.Inf(1) + rb.InertiaLocal, rb.InverseInertiaLocal = mgl64.Mat3{}, mgl64.Mat3{} + return + } + rb.Material.mass = rb.Shape.ComputeMass(rb.Material.Density) + rb.InertiaLocal = rb.Shape.ComputeInertia(rb.Material.mass) + rb.InverseInertiaLocal = rb.InertiaLocal.Inv() +} + // Serial is a unique number of the body, given by NewRigidBody func (rb *RigidBody) Serial() uint64 { return rb.serial @@ -177,8 +222,9 @@ func (rb *RigidBody) UpdateAABB() { rb.aabb = rb.Shape.ComputeAABB(rb.Transform) } +// InverseMass: 1 / mass for a dynamic body, 0 for a static or a kinematic body (nothing moves them) func (rb *RigidBody) InverseMass() float64 { - if rb.BodyType == BodyTypeStatic { + if rb.BodyType != BodyTypeDynamic { return 0 } return 1 / rb.Material.mass @@ -191,13 +237,17 @@ func (rb *RigidBody) InverseMassAxes() mgl64.Vec3 { } // SetLocks changes the locked axes of a dynamic body: its velocities along the locked axes are cleared, and it wakes -// up (a body freed in the air must fall). A static body never moves: it has no lock +// up (a body freed in the air must fall). A static body never moves: it has no lock. A kinematic body goes where its +// target is: it keeps the locks for the day it becomes dynamic (World.SetBodyType) func (rb *RigidBody) SetLocks(linear, angular Axes) { linear, angular = linear&AllAxes, angular&AllAxes if rb.BodyType == BodyTypeStatic || (linear == rb.LinearLock && angular == rb.AngularLock) { return } rb.LinearLock, rb.AngularLock = linear, angular + if rb.BodyType != BodyTypeDynamic { + return + } rb.Velocity = linear.LockVector(rb.Velocity) rb.AngularVelocity = angular.LockVector(rb.AngularVelocity) rb.WakeUp() @@ -218,9 +268,10 @@ func (rb *RigidBody) WakeUp() { rb.SleepTimer = 0.0 } -// AddForce in N, during the next step +// AddForce in N, during the next step. A static or a kinematic body ignores the forces, the torques and the impulses +// (PhysX refuses them on a kinematic actor, Jolt drops them: "Cancel forces" in Body::SetMotionType) func (rb *RigidBody) AddForce(force mgl64.Vec3) { - if rb.BodyType != BodyTypeStatic { + if rb.BodyType == BodyTypeDynamic { rb.WakeUp() rb.accumulatedForce = rb.accumulatedForce.Add(force) } @@ -228,7 +279,7 @@ func (rb *RigidBody) AddForce(force mgl64.Vec3) { // AddTorque in N·m (world space), during the next step func (rb *RigidBody) AddTorque(torque mgl64.Vec3) { - if rb.BodyType != BodyTypeStatic { + if rb.BodyType == BodyTypeDynamic { rb.WakeUp() rb.accumulatedTorque = rb.accumulatedTorque.Add(torque) } @@ -242,7 +293,7 @@ func (rb *RigidBody) AddForceAtPoint(force mgl64.Vec3, point mgl64.Vec3) { // AddImpulse in N·s: the velocity changes immediately (a hit, a jump) func (rb *RigidBody) AddImpulse(impulse mgl64.Vec3) { - if rb.BodyType != BodyTypeStatic { + if rb.BodyType == BodyTypeDynamic { rb.WakeUp() rb.Velocity = rb.LinearLock.LockVector(rb.Velocity.Add(impulse.Mul(rb.InverseMass()))) } @@ -256,7 +307,7 @@ func (rb *RigidBody) AddImpulseAtPoint(impulse mgl64.Vec3, point mgl64.Vec3) { // AddAngularImpulse in N·m·s (world space): the angular velocity changes immediately func (rb *RigidBody) AddAngularImpulse(impulse mgl64.Vec3) { - if rb.BodyType != BodyTypeStatic { + if rb.BodyType == BodyTypeDynamic { rb.WakeUp() rb.AngularVelocity = rb.AngularVelocity.Add(rb.GetInverseInertiaWorld().Mul3x1(impulse)) } @@ -294,9 +345,10 @@ func (rb *RigidBody) GetInverseInertiaWorld() mgl64.Mat3 { return inverseInertia } -// GetFreeInverseInertiaWorld: the inverse inertia in world space of the body without its locks +// GetFreeInverseInertiaWorld: the inverse inertia in world space of the body without its locks, 0 for a static or a +// kinematic body func (rb *RigidBody) GetFreeInverseInertiaWorld() mgl64.Mat3 { - if rb.BodyType == BodyTypeStatic { + if rb.BodyType != BodyTypeDynamic { return mgl64.Mat3{} } // the rotation matrix of mgl64 (Quat.Mat4), and R I⁻¹ Rᵀ without copying the matrices: the same arithmetic @@ -311,3 +363,35 @@ func (rb *RigidBody) GetFreeInverseInertiaWorld() mgl64.Mat3 { rt := Transpose3(&r) return Mul3(&ri, &rt) } + +// ErrNotKinematic: SetKinematicTarget was called on a body which is not kinematic. The target is not stored +var ErrNotKinematic = errors.New("actor: SetKinematicTarget on a body which is not kinematic") + +// SetKinematicTarget: the pose a kinematic body reaches at the end of the next step. The step brings it there over its +// sub-steps (its position linearly, its rotation along the shortest arc), with the velocity of this motion, and drops +// the target: give one per step to move the body along a path, as PxRigidDynamic::setKinematicTarget of PhysX. Without +// target the body stays, with no velocity. A target away from the pose of the body wakes it up, with its island at the +// step. On a body which is not kinematic the target is refused: ErrNotKinematic, the only error (PhysX refuses it too, +// "Body must be kinematic"); the call allocates nothing. To place the body without pushing anything, see World.Teleport +func (rb *RigidBody) SetKinematicTarget(target Transform) error { + if rb.BodyType != BodyTypeKinematic { + return ErrNotKinematic + } + target.Rotation = target.Rotation.Normalize() + rb.target, rb.targeted = target, true + if target != rb.Transform { + rb.WakeUp() + } + return nil +} + +// KinematicTarget: the target of the next step, if the body has one (the step drops it once reached) +func (rb *RigidBody) KinematicTarget() (Transform, bool) { + return rb.target, rb.targeted +} + +// ClearKinematicTarget: the body has no target anymore, it stays where it is at the next step. The World calls it when +// the body reaches its target, and when the body is teleported or changes type +func (rb *RigidBody) ClearKinematicTarget() { + rb.target, rb.targeted = Transform{}, false +} diff --git a/actor/shapechange_test.go b/actor/shapechange_test.go new file mode 100644 index 0000000..c93d16e --- /dev/null +++ b/actor/shapechange_test.go @@ -0,0 +1,71 @@ +package actor + +import ( + "math" + "testing" + + "github.com/go-gl/mathgl/mgl64" +) + +// RigidBody.SetShape and RigidBody.SetDensity, without a world: the shape, the mass, the inertia and the AABB of the +// body follow; a static body keeps its infinite mass and the density written; the velocities and the locks stay + +func TestSetShapeOfABody(t *testing.T) { + box := &Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}} + sphere := &Sphere{Radius: 0.5} + transform := Transform{Position: mgl64.Vec3{1, 2, 3}, Rotation: mgl64.QuatIdent()} + for _, bodyType := range []BodyType{BodyTypeDynamic, BodyTypeKinematic, BodyTypeStatic} { + body := NewRigidBody(transform, box, bodyType, 1000) + body.Velocity, body.AngularVelocity = mgl64.Vec3{1, 0, 0}, mgl64.Vec3{0, 1, 0} + body.SetLocks(AxisY, AxisX) + body.SetShape(sphere) + if body.Shape != sphere { + t.Fatalf("%v: shape %T, want the sphere", bodyType, body.Shape) + } + if body.AABB() != sphere.ComputeAABB(transform) { + t.Errorf("%v: AABB %v, want the one of the sphere", bodyType, body.AABB()) + } + if bodyType == BodyTypeStatic { + if !math.IsInf(body.Material.GetMass(), 1) || body.InertiaLocal != (mgl64.Mat3{}) { + t.Errorf("a static body: mass %v, inertia %v, want +Inf and none", body.Material.GetMass(), body.InertiaLocal) + } + } else { + if want := sphere.ComputeMass(1000); body.Material.GetMass() != want { + t.Errorf("%v: mass %v, want %v", bodyType, body.Material.GetMass(), want) + } + if want := sphere.ComputeInertia(body.Material.GetMass()); body.InertiaLocal != want || body.InverseInertiaLocal != want.Inv() { + t.Errorf("%v: inertia %v, want %v", bodyType, body.InertiaLocal, want) + } + } + if body.Velocity != (mgl64.Vec3{1, 0, 0}) || body.AngularVelocity != (mgl64.Vec3{0, 1, 0}) { + if bodyType != BodyTypeStatic { + t.Errorf("%v: the velocities changed: %v %v", bodyType, body.Velocity, body.AngularVelocity) + } + } + if bodyType != BodyTypeStatic && (body.LinearLock != AxisY || body.AngularLock != AxisX) { + t.Errorf("%v: the locks changed: %v %v", bodyType, body.LinearLock, body.AngularLock) + } + } +} + +func TestSetDensityOfABody(t *testing.T) { + box := &Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}} + dynamic := NewRigidBody(NewTransform(), box, BodyTypeDynamic, 1000) + dynamic.SetDensity(250) + if dynamic.Material.Density != 250 || dynamic.Material.GetMass() != box.ComputeMass(250) { + t.Errorf("density %v, mass %v, want 250 and %v", dynamic.Material.Density, dynamic.Material.GetMass(), box.ComputeMass(250)) + } + if want := box.ComputeInertia(box.ComputeMass(250)); dynamic.InertiaLocal != want || dynamic.InverseInertiaLocal != want.Inv() { + t.Errorf("inertia %v, want %v", dynamic.InertiaLocal, want) + } + static := NewRigidBody(NewTransform(), box, BodyTypeStatic, 0) + static.SetDensity(800) + if static.Material.Density != 800 || !math.IsInf(static.Material.GetMass(), 1) || static.InverseMass() != 0 { + t.Errorf("a static body: density %v, mass %v, want 800 kept and +Inf", static.Material.Density, static.Material.GetMass()) + } + kinematic := NewRigidBody(NewTransform(), box, BodyTypeKinematic, 0) + kinematic.SetDensity(800) + if kinematic.Material.GetMass() != box.ComputeMass(800) || kinematic.InverseMass() != 0 { + t.Errorf("a kinematic body: mass %v, inverse mass %v, want %v and 0", kinematic.Material.GetMass(), kinematic.InverseMass(), box.ComputeMass(800)) + } +} diff --git a/actor/trianglemesh.go b/actor/trianglemesh.go new file mode 100644 index 0000000..15d46c4 --- /dev/null +++ b/actor/trianglemesh.go @@ -0,0 +1,656 @@ +package actor + +import ( + "errors" + "math" + "slices" + + "github.com/go-gl/mathgl/mgl64" +) + +// ========== TRIANGLE MESH ========== +// A static decor of triangles (a rock, a staircase, a building), with a bounding volume hierarchy: a binary tree of +// AABBs, a few triangles at each leaf, built once, top-down, by the surface area heuristic over binned centroids +// (b3CreateMesh of Box3D, src/mesh.c; TriangleSplitterBinning & AABBTreeBuilder of Jolt; Ericson 6.2.1, "Top-down +// Construction"). A body or a query takes the triangles under its AABB from the tree (OverlapTriangles), a ray or a +// moving shape walks the tree front to back (Cast), as b3QueryMesh, b3RayCastMesh and b3ShapeCastMesh of Box3D. +// +// The mesh is a surface, as the heightfield: its triangles are seen from the side of their normal (the vertices +// counterclockwise seen from outside, the right-hand rule), a body whose center went through is not pushed back. The +// edges between its triangles are classified as the ones of the heightfield (active, convex): a body slides over the +// edges of a flat part without hitting them. + +const ( + // MeshWeldDistance: 2 vertices closer than this distance (m) are the same vertex (the weld distance of Indexify + // of Jolt, Geometry/Indexify.h:14). A mesh exported as a soup of triangles has its edges shared again: without it, + // every edge would be a border, and active + MeshWeldDistance = 1e-4 + + // meshLeafTriangles: a leaf of the tree holds this many triangles at most, when a split is found + // (B3_DESIRED_TRIANGLES_PER_LEAF of Box3D, src/mesh.c:24; 8 in Jolt, mMaxTrianglesPerLeaf) + meshLeafTriangles = 4 + + // meshLeafMaxTriangles: a leaf holds up to this many triangles when no split separates them (their centroids are + // the same); above, they are split in halves (B3_MAXIMUM_TRIANGLES_PER_LEAF, src/mesh.c:26) + meshLeafMaxTriangles = 8 + + // meshBins of the surface area heuristic, per axis (B3_BIN_COUNT of Box3D, src/mesh.c:23) + meshBins = 8 + + // meshBalance: a split leaving less than a third of the triangles on a side is replaced by the median, so that the + // height of the tree is bounded by log(triangles)/log(1.5) (the rule of btQuantizedBvh::sortAndCalcSplittingIndex + // of Bullet: "if the splitIndex causes unbalanced trees, fix this by using the center") + meshBalance = 3 + + // meshStackSize: the nodes a traversal keeps for later. The height of the tree is bounded by meshBalance: 52 for + // 2^31 triangles + meshStackSize = 64 + + // meshMinTriangleArea: a triangle of a smaller area (m²) is degenerate, out of the tree (the minArea of + // b3CreateMesh of Box3D, src/mesh.c: 0.01 x B3_LINEAR_SLOP², with the LinearSlop of Feather, 5 mm) + meshMinTriangleArea = 0.01 * 0.005 * 0.005 + + // backToBackCos: 2 triangles whose normals are closer than 1° to opposite are back to back (a sheet): their edge + // is active (cos(179°), ActiveEdges::IsEdgeActive of Jolt) + backToBackCos = -0.999848 +) + +var ( + // ErrMeshIndices: the indices are not 3 per triangle, or out of the vertices + ErrMeshIndices = errors.New("feather: a triangle mesh needs 3 indices per triangle, within its vertices") + + // ErrMeshDegenerate: no triangle of the mesh has an area + ErrMeshDegenerate = errors.New("feather: a triangle mesh needs a triangle which is not degenerate") +) + +// TriangleMesh is a static surface of triangles with a tree of AABBs, built by NewTriangleMesh. Hit.Triangle of a +// query on a mesh is the index of the triangle (Indices[3*i:3*i+3]) +type TriangleMesh struct { + // Vertices given to NewTriangleMesh, shared without copy + Vertices []mgl64.Vec3 + // Indices of the vertices of each triangle, 3 per triangle, in the order given to NewTriangleMesh: the vertices + // closer than MeshWeldDistance share one index + Indices []int32 + + // edges of each triangle: bit e if the edge from the vertex e to the vertex e+1 is active, bit 3+e if it is + // convex (0 for a degenerate triangle) + edges []uint8 + nodes []meshNode + // order: the triangles of the leaves, the ones of a leaf from its first + order []int32 + bounds AABB +} + +// meshNode: a node of the tree: its AABB and its 2 children (the left one follows it), or its triangles (a leaf) +type meshNode struct { + min, max mgl64.Vec3 + right int32 // index of the right child, -1 for a leaf + first int32 // leaf: index in order of its first triangle + count int32 +} + +// meshPrimitive: a triangle being sorted in the tree +type meshPrimitive struct { + aabb AABB + center mgl64.Vec3 + index int32 +} + +// NewTriangleMesh builds the mesh of the triangles (indices[3*i], indices[3*i+1], indices[3*i+2] are the vertices of the +// triangle i, counterclockwise seen from outside) and its tree. The vertices closer than MeshWeldDistance are welded, +// the degenerate triangles are kept out of the tree (Triangle gives them without edge). The vertices are shared, the +// indices are copied +func NewTriangleMesh(vertices []mgl64.Vec3, indices []int32) (*TriangleMesh, error) { + if len(indices) == 0 || len(indices)%3 != 0 { + return nil, ErrMeshIndices + } + for _, index := range indices { + if index < 0 || int(index) >= len(vertices) { + return nil, ErrMeshIndices + } + } + m := &TriangleMesh{Vertices: vertices, Indices: weldIndices(vertices, indices)} + count := len(indices) / 3 + m.edges = make([]uint8, count) + + primitives := make([]meshPrimitive, 0, count) + for i := 0; i < count; i++ { + triangle := m.vertices(int32(i)) + if triangle[1].Sub(triangle[0]).Cross(triangle[2].Sub(triangle[0])).Len() < 2*meshMinTriangleArea { + continue + } + bounds := triangleBounds(triangle) + primitives = append(primitives, meshPrimitive{aabb: bounds, center: bounds.Min.Add(bounds.Max).Mul(0.5), index: int32(i)}) + } + if len(primitives) == 0 { + return nil, ErrMeshDegenerate + } + m.findEdges(primitives) + m.nodes = make([]meshNode, 0, 2*len(primitives)) + m.order = make([]int32, 0, len(primitives)) + m.buildNode(primitives) + m.bounds = AABB{Min: m.nodes[0].min, Max: m.nodes[0].max} + return m, nil +} + +// weldIndices: the indices with the vertices closer than MeshWeldDistance merged on the first of them, found in a grid +// of cells of the weld distance (a vertex is compared to the vertices of the 27 cells around it) +func weldIndices(vertices []mgl64.Vec3, indices []int32) []int32 { + type cell [3]int64 + cells := map[cell][]int32{} + welded := make([]int32, len(vertices)) + for i := range welded { + welded[i] = -1 + } + cellOf := func(p mgl64.Vec3) cell { + return cell{int64(math.Floor(p[0] / MeshWeldDistance)), int64(math.Floor(p[1] / MeshWeldDistance)), int64(math.Floor(p[2] / MeshWeldDistance))} + } + out := make([]int32, len(indices)) + for k, index := range indices { + if welded[index] >= 0 { + out[k] = welded[index] + continue + } + p := vertices[index] + c := cellOf(p) + found := int32(-1) + for dx := int64(-1); dx <= 1 && found < 0; dx++ { + for dy := int64(-1); dy <= 1 && found < 0; dy++ { + for dz := int64(-1); dz <= 1 && found < 0; dz++ { + for _, other := range cells[cell{c[0] + dx, c[1] + dy, c[2] + dz}] { + if vertices[other].Sub(p).LenSqr() <= MeshWeldDistance*MeshWeldDistance { + found = other + break + } + } + } + } + } + if found < 0 { + found = index + cells[c] = append(cells[c], index) + } + welded[index] = found + out[k] = found + } + return out +} + +// vertices of the triangle i +func (m *TriangleMesh) vertices(i int32) [3]mgl64.Vec3 { + return [3]mgl64.Vec3{m.Vertices[m.Indices[3*i]], m.Vertices[m.Indices[3*i+1]], m.Vertices[m.Indices[3*i+2]]} +} + +func triangleBounds(triangle [3]mgl64.Vec3) AABB { + aabb := AABB{Min: triangle[0], Max: triangle[0]} + for _, v := range triangle[1:] { + for k := 0; k < 3; k++ { + aabb.Min[k] = math.Min(aabb.Min[k], v[k]) + aabb.Max[k] = math.Max(aabb.Max[k], v[k]) + } + } + return aabb +} + +// findEdges classifies the edges of the triangles in the tree, by the triangles sharing each edge (the edge map of +// sFindActiveEdges of Jolt, of b3IdentifyEdges of Box3D): +// - 1 triangle (a border): convex and active +// - 2 triangles: convex if the opposite vertex of the other triangle is under the plane, active if convex and bent +// by more than 5° (the rule of the heightfield, cellEdges), or if the triangles are back to back (Jolt) +// - 3 triangles or more: active (Jolt) +func (m *TriangleMesh) findEdges(primitives []meshPrimitive) { + type side struct { + triangle int32 + edge uint8 + } + type shared struct { + count int + sides [2]side + } + edges := make(map[[2]int32]*shared, 3*len(primitives)) + for _, primitive := range primitives { + t := primitive.index + for e := int32(0); e < 3; e++ { + a, c := m.Indices[3*t+e], m.Indices[3*t+(e+1)%3] + key := [2]int32{min(a, c), max(a, c)} + s := edges[key] + if s == nil { + s = &shared{} + edges[key] = s + } + if s.count < 2 { + s.sides[s.count] = side{t, uint8(e)} + } else { + // a 3rd triangle on the edge: active for it, and for the 2 others below + m.edges[t] |= (1 | 1<<3) << e + } + s.count++ + } + } + for _, s := range edges { + if s.count != 2 { + for _, side := range s.sides[:min(s.count, 2)] { + m.edges[side.triangle] |= (1 | 1<<3) << side.edge + } + continue + } + for k, side := range s.sides { + triangle, other := m.vertices(side.triangle), s.sides[1-k] + normal := triangleNormal(triangle) + otherTriangle := m.vertices(other.triangle) + opposite := otherTriangle[(other.edge+2)%3] + cos := normal.Dot(triangleNormal(otherTriangle)) + if cos < backToBackCos { + m.edges[side.triangle] |= (1 | 1<<3) << side.edge + continue + } + if opposite.Sub(triangle[side.edge]).Dot(normal) >= 0 { + continue + } + m.edges[side.triangle] |= 1 << (3 + side.edge) + if cos < activeEdgeCos { + m.edges[side.triangle] |= 1 << side.edge + } + } + } +} + +// buildNode builds the node of the primitives and its subtree, and returns its index. The primitives are sorted in +// place: the ones of a leaf are contiguous in order +func (m *TriangleMesh) buildNode(primitives []meshPrimitive) int32 { + index := int32(len(m.nodes)) + bounds := primitives[0].aabb + for _, p := range primitives[1:] { + for k := 0; k < 3; k++ { + bounds.Min[k] = math.Min(bounds.Min[k], p.aabb.Min[k]) + bounds.Max[k] = math.Max(bounds.Max[k], p.aabb.Max[k]) + } + } + m.nodes = append(m.nodes, meshNode{min: bounds.Min, max: bounds.Max, right: -1}) + + split := 0 + if len(primitives) > meshLeafTriangles { + split = sahSplit(primitives) + if split == 0 && len(primitives) > meshLeafMaxTriangles { + // no split by the heuristic (the centroids are the same): halves (b3SplitHalf of Box3D) + split = len(primitives) / 2 + } + } + if split == 0 { + m.nodes[index].first, m.nodes[index].count = int32(len(m.order)), int32(len(primitives)) + for _, p := range primitives { + m.order = append(m.order, p.index) + } + return index + } + m.buildNode(primitives[:split]) + m.nodes[index].right = m.buildNode(primitives[split:]) + return index +} + +// sahSplit partitions the primitives in place by the surface area heuristic over binned centroids, and returns the +// count of the left part, 0 if no split separates them. On each axis the centroids fall in meshBins bins, and the cost +// of a split after a bin is the count of the triangles on each side times the area of their AABB (b3SplitBinnedSah of +// Box3D, src/mesh.c:570-681; TriangleSplitterBinning of Jolt). A split leaving less than a third of the triangles on +// a side is replaced by the median along its axis (meshBalance) +func sahSplit(primitives []meshPrimitive) int { + centroids := AABB{Min: primitives[0].center, Max: primitives[0].center} + for _, p := range primitives[1:] { + for k := 0; k < 3; k++ { + centroids.Min[k] = math.Min(centroids.Min[k], p.center[k]) + centroids.Max[k] = math.Max(centroids.Max[k], p.center[k]) + } + } + type bin struct { + count int + bounds AABB + } + bestAxis, bestBin, bestCost := -1, -1, math.Inf(1) + for axis := 0; axis < 3; axis++ { + extent := centroids.Max[axis] - centroids.Min[axis] + if extent <= 0 { + continue + } + var bins [meshBins]bin + factor := meshBins * (1 - epsilon64) / extent + for _, p := range primitives { + b := &bins[int(factor*(p.center[axis]-centroids.Min[axis]))] + if b.count == 0 { + b.bounds = p.aabb + } else { + b.bounds = unionAABB(b.bounds, p.aabb) + } + b.count++ + } + for i := 0; i < meshBins-1; i++ { + leftCount, rightCount := 0, 0 + var left, right AABB + for k := 0; k <= i; k++ { + if bins[k].count > 0 { + if leftCount == 0 { + left = bins[k].bounds + } else { + left = unionAABB(left, bins[k].bounds) + } + leftCount += bins[k].count + } + } + for k := i + 1; k < meshBins; k++ { + if bins[k].count > 0 { + if rightCount == 0 { + right = bins[k].bounds + } else { + right = unionAABB(right, bins[k].bounds) + } + rightCount += bins[k].count + } + } + if leftCount == 0 || rightCount == 0 { + continue + } + if cost := float64(leftCount)*area(left) + float64(rightCount)*area(right); cost < bestCost { + bestAxis, bestBin, bestCost = axis, i, cost + } + } + } + if bestAxis < 0 { + return 0 + } + // the partition: the primitives of the bins up to bestBin first + factor := meshBins * (1 - epsilon64) / (centroids.Max[bestAxis] - centroids.Min[bestAxis]) + split := 0 + for i := range primitives { + if int(factor*(primitives[i].center[bestAxis]-centroids.Min[bestAxis])) <= bestBin { + primitives[i], primitives[split] = primitives[split], primitives[i] + split++ + } + } + if split < len(primitives)/meshBalance || len(primitives)-split < len(primitives)/meshBalance { + slices.SortStableFunc(primitives, func(a, b meshPrimitive) int { + switch { + case a.center[bestAxis] < b.center[bestAxis]: + return -1 + case a.center[bestAxis] > b.center[bestAxis]: + return 1 + } + return int(a.index - b.index) + }) + split = len(primitives) / 2 + } + return split +} + +func unionAABB(a, b AABB) AABB { + for k := 0; k < 3; k++ { + a.Min[k] = math.Min(a.Min[k], b.Min[k]) + a.Max[k] = math.Max(a.Max[k], b.Max[k]) + } + return a +} + +// area: the surface area of the AABB +func area(a AABB) float64 { + d := a.Max.Sub(a.Min) + return 2 * (d[0]*d[1] + d[1]*d[2] + d[2]*d[0]) +} + +// TriangleCount: the triangles of the mesh, degenerate or not +func (m *TriangleMesh) TriangleCount() int { + return len(m.Indices) / 3 +} + +// Triangle i, in the local space, with its edges: bit e if the edge from the vertex e to the vertex e+1 is active, bit +// 3+e if it is convex. A degenerate triangle has no edge +func (m *TriangleMesh) Triangle(i int32) ([3]mgl64.Vec3, uint8) { + return m.vertices(i), m.edges[i] +} + +// Bounds of the mesh, in its local space +func (m *TriangleMesh) Bounds() AABB { + return m.bounds +} + +// Height of the tree (0 for a single leaf) +func (m *TriangleMesh) Height() int { + return m.height(0) +} + +func (m *TriangleMesh) height(node int32) int { + if m.nodes[node].right < 0 { + return 0 + } + return 1 + max(m.height(node+1), m.height(m.nodes[node].right)) +} + +// OverlapTriangles appends to triangles the indices of the triangles whose AABB overlaps the local bounds, in the order +// of the tree, and returns the slice: no allocation if its capacity is enough +func (m *TriangleMesh) OverlapTriangles(bounds AABB, triangles []int32) []int32 { + return m.overlapNode(0, bounds, triangles) +} + +func (m *TriangleMesh) overlapNode(index int32, bounds AABB, out []int32) []int32 { + node := &m.nodes[index] + if !bounds.Overlaps(AABB{Min: node.min, Max: node.max}) { + return out + } + if node.right < 0 { + for _, t := range m.order[node.first : node.first+node.count] { + if triangleBounds(m.vertices(t)).Overlaps(bounds) { + out = append(out, t) + } + } + return out + } + out = m.overlapNode(index+1, bounds, out) + return m.overlapNode(node.right, bounds, out) +} + +// ========== CAST ========== + +// castNode: a node kept for later by a cast, with the fraction where the segment enters it +type castNode struct { + node int32 + enter float64 +} + +// MeshCast: the triangles a moving box meets, from TriangleMesh.Cast: the nodes of the tree in the order the segment +// of the center enters them (the closest child first, the nodes entered after the limit dropped), the triangles of +// each leaf in their order +type MeshCast struct { + mesh *TriangleMesh + origin, translation mgl64.Vec3 + // inverse of the translation, finite: 0 x infinity would be a NaN for a segment starting in the plane of a face + inverse mgl64.Vec3 + extents mgl64.Vec3 + stack [meshStackSize]castNode + count int + // the triangles left in the leaf being walked + next, end int32 +} + +// Cast: the triangles a box of half sizes extents can meet while its center moves from origin by maxFraction * +// translation, in the local space of the mesh, in the order of the motion (a ray is a box without size). The AABBs of +// the nodes are enlarged by the extents (b3ShapeCastMesh of Box3D) +func (m *TriangleMesh) Cast(origin, translation, extents mgl64.Vec3, maxFraction float64) MeshCast { + var c MeshCast + c.start(m, origin, translation, extents, maxFraction) + return c +} + +func (c *MeshCast) start(m *TriangleMesh, origin, translation, extents mgl64.Vec3, maxFraction float64) { + *c = MeshCast{mesh: m, origin: origin, translation: translation, extents: extents} + for k := 0; k < 3; k++ { + if translation[k] != 0 { + c.inverse[k] = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, 1/translation[k])) + } + } + if enter, ok := c.enters(&m.nodes[0], maxFraction); ok { + c.stack[0] = castNode{0, enter} + c.count = 1 + } +} + +// enters: the fraction where the segment, cut at limit, enters the AABB of the node enlarged by the extents, false if +// it doesn't. The faces belong to the AABB +func (c *MeshCast) enters(node *meshNode, limit float64) (float64, bool) { + enter, exit := 0.0, limit + for k := 0; k < 3; k++ { + low, high := node.min[k]-c.extents[k]-c.origin[k], node.max[k]+c.extents[k]-c.origin[k] + if c.translation[k] == 0 { + if low > 0 || high < 0 { + return 0, false + } + continue + } + near, far := low*c.inverse[k], high*c.inverse[k] + if near > far { + near, far = far, near + } + enter, exit = math.Max(enter, near), math.Min(exit, far) + } + return enter, enter <= exit +} + +// Next triangle of the cast, false after the last one. The nodes entered after the limit are dropped: the caller +// lowers the limit to its best hit. A node entered at the limit is walked: a triangle of a lower index can be hit at +// the same fraction +func (c *MeshCast) Next(limit float64) (int32, bool) { + m := c.mesh + for { + if c.next < c.end { + t := m.order[c.next] + c.next++ + return t, true + } + if c.count == 0 { + return 0, false + } + c.count-- + entry := c.stack[c.count] + if entry.enter > limit { + continue + } + node := &m.nodes[entry.node] + if node.right < 0 { + c.next, c.end = node.first, node.first+node.count + continue + } + near, far := castNode{node: entry.node + 1}, castNode{node: node.right} + var nearOk, farOk bool + near.enter, nearOk = c.enters(&m.nodes[near.node], limit) + far.enter, farOk = c.enters(&m.nodes[far.node], limit) + if farOk && (!nearOk || far.enter < near.enter) { + near, far, nearOk, farOk = far, near, farOk, nearOk + } + // the further child under the closer one (the node just left the stack: 2 children fit, the height of the + // tree is bounded by meshBalance) + if farOk { + c.stack[c.count] = far + c.count++ + } + if nearOk { + c.stack[c.count] = near + c.count++ + } + } +} + +// ========== SHAPE ========== + +// ComputeAABB: the bounds of the mesh turned and moved: the AABB of their 8 corners +func (m *TriangleMesh) ComputeAABB(transform Transform) AABB { + q := &transform.Rotation + min := mgl64.Vec3{math.Inf(1), math.Inf(1), math.Inf(1)} + max := mgl64.Vec3{math.Inf(-1), math.Inf(-1), math.Inf(-1)} + for i := 0; i < 8; i++ { + corner := m.bounds.Min + if i&1 != 0 { + corner[0] = m.bounds.Max[0] + } + if i&2 != 0 { + corner[1] = m.bounds.Max[1] + } + if i&4 != 0 { + corner[2] = m.bounds.Max[2] + } + world := Rotate(q, corner).Add(transform.Position) + for k := 0; k < 3; k++ { + min[k] = math.Min(min[k], world[k]) + max[k] = math.Max(max[k], world[k]) + } + } + return AABB{Min: min, Max: max} +} + +// ComputeMass: a mesh is static +func (m *TriangleMesh) ComputeMass(density float64) float64 { + return math.Inf(1) +} + +func (m *TriangleMesh) ComputeInertia(mass float64) mgl64.Mat3 { + return mgl64.Mat3{} +} + +// Support: a mesh is not convex, its triangles are tested one by one +func (m *TriangleMesh) Support(direction mgl64.Vec3) mgl64.Vec3 { + return mgl64.Vec3{} +} + +func (m *TriangleMesh) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { + output[0] = mgl64.Vec3{} + *count = 1 +} + +// CollideWithPlane - Mesh/Plane collision (not supported) +func (m *TriangleMesh) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { + return contacts +} + +// CastRay: the ray walks the tree front to back (Cast) and meets each triangle by Möller-Trumbore (the ray cast of the +// meshes of Jolt, RayTriangle.h, and of Box3D, b3RayCastMesh). Only the side of the normal is hit: a ray from behind +// goes through. A point may be footprintSlack out of its triangle (in barycentric coordinates): a ray on an edge hits +// the triangles on both sides, and never leaks between them. At the very same fraction, the triangle of the lowest +// index is named. A ray without length hits nothing: a mesh has no inside +func (m *TriangleMesh) CastRay(origin, translation mgl64.Vec3, maxFraction float64) (RayHit, bool) { + if !finiteRay(origin, translation) { + return RayHit{}, false + } + hit, found := RayHit{Fraction: maxFraction, Triangle: NoTriangle}, false + var normal mgl64.Vec3 + var cast MeshCast + cast.start(m, origin, translation, mgl64.Vec3{}, maxFraction) + for { + t, ok := cast.Next(hit.Fraction) + if !ok { + break + } + triangle := m.vertices(t) + edge1, edge2 := triangle[1].Sub(triangle[0]), triangle[2].Sub(triangle[0]) + p := translation.Cross(edge2) + // the determinant is positive for a ray coming from the side of the normal + determinant := edge1.Dot(p) + if determinant <= 0 { + continue + } + s := origin.Sub(triangle[0]) + u := s.Dot(p) / determinant + if u < -footprintSlack || u > 1+footprintSlack { + continue + } + q := s.Cross(edge1) + v := translation.Dot(q) / determinant + if v < -footprintSlack || u+v > 1+footprintSlack { + continue + } + fraction := edge2.Dot(q) / determinant + if fraction < 0 || fraction > hit.Fraction || (found && fraction == hit.Fraction && t > hit.Triangle) { + continue + } + hit.Fraction, hit.Triangle, found = fraction, t, true + normal = edge1.Cross(edge2) + } + if found { + hit.Normal = normal.Normalize() + } + return hit, found +} diff --git a/actor/trianglemesh_test.go b/actor/trianglemesh_test.go new file mode 100644 index 0000000..7e67d77 --- /dev/null +++ b/actor/trianglemesh_test.go @@ -0,0 +1,462 @@ +package actor + +import ( + "errors" + "math" + "math/rand" + "testing" + + "github.com/go-gl/mathgl/mgl64" +) + +// gridMesh: n x n cells of size cell, at the height of the function, the vertices shared +func gridMesh(n int, cell float64, height func(x, z float64) float64) *TriangleMesh { + vertices := make([]mgl64.Vec3, 0, (n+1)*(n+1)) + for x := 0; x <= n; x++ { + for z := 0; z <= n; z++ { + px, pz := (float64(x)-float64(n)/2)*cell, (float64(z)-float64(n)/2)*cell + vertices = append(vertices, mgl64.Vec3{px, height(px, pz), pz}) + } + } + indices := make([]int32, 0, 6*n*n) + for x := 0; x < n; x++ { + for z := 0; z < n; z++ { + a, b, c, d := int32(x*(n+1)+z), int32(x*(n+1)+z+1), int32((x+1)*(n+1)+z+1), int32((x+1)*(n+1)+z) + indices = append(indices, a, b, c, a, c, d) + } + } + m, err := NewTriangleMesh(vertices, indices) + if err != nil { + panic(err) + } + return m +} + +// cubeSoup: the 12 triangles of a cube as a soup of 36 vertices, counterclockwise seen from outside +func cubeSoup(half float64) ([]mgl64.Vec3, []int32) { + faces := [6][4]mgl64.Vec3{ + {{-1, -1, -1}, {-1, -1, 1}, {-1, 1, 1}, {-1, 1, -1}}, // -x + {{1, -1, -1}, {1, 1, -1}, {1, 1, 1}, {1, -1, 1}}, // +x + {{-1, -1, -1}, {1, -1, -1}, {1, -1, 1}, {-1, -1, 1}}, // -y + {{-1, 1, -1}, {-1, 1, 1}, {1, 1, 1}, {1, 1, -1}}, // +y + {{-1, -1, -1}, {-1, 1, -1}, {1, 1, -1}, {1, -1, -1}}, // -z + {{-1, -1, 1}, {1, -1, 1}, {1, 1, 1}, {-1, 1, 1}}, // +z + } + var vertices []mgl64.Vec3 + var indices []int32 + for _, f := range faces { + i := int32(len(vertices)) + for _, v := range f { + vertices = append(vertices, v.Mul(half)) + } + indices = append(indices, i, i+1, i+2, i, i+2, i+3) + } + return vertices, indices +} + +// A cube given as a soup of triangles is welded to 8 vertices; its 12 edges are convex and active, the diagonals of +// its faces are neither; a flat grid has inactive inner edges and active borders; a fold inwards is not convex; an +// edge shared by 3 triangles is active; a sheet of 2 triangles back to back has active edges +func TestTriangleMeshWeldsAndClassifiesEdges(t *testing.T) { + vertices, indices := cubeSoup(0.5) + m, err := NewTriangleMesh(vertices, indices) + if err != nil { + t.Fatal(err) + } + distinct := map[int32]bool{} + for _, i := range m.Indices { + distinct[i] = true + } + if len(distinct) != 8 { + t.Fatalf("%d distinct vertices after the weld, want 8", len(distinct)) + } + for i := 0; i < m.TriangleCount(); i++ { + triangle, edges := m.Triangle(int32(i)) + for e := 0; e < 3; e++ { + diagonal := triangle[e].Sub(triangle[(e+1)%3]).LenSqr() > 1.5 + active, convex := edges&(1< meshLeafMaxTriangles { + t.Fatalf("leaf %d has %d triangles", i, node.count) + } + for _, tr := range m.order[node.first : node.first+node.count] { + inLeaf[tr]++ + triangle, _ := m.Triangle(tr) + for _, v := range triangle { + if !box.ContainsPoint(v) { + t.Fatalf("leaf %d doesn't contain its triangle %d", i, tr) + } + } + } + continue + } + for _, child := range [2]int32{int32(i) + 1, node.right} { + c := &m.nodes[child] + for k := 0; k < 3; k++ { + if c.min[k] < node.min[k] || c.max[k] > node.max[k] { + t.Fatalf("node %d doesn't contain its child %d", i, child) + } + } + } + } + for tr, count := range inLeaf { + if count != 1 { + t.Fatalf("triangle %d is in %d leaves", tr, count) + } + } + bound := int(math.Ceil(math.Log(float64(m.TriangleCount()))/math.Log(1.5))) + 1 + t.Logf("%d triangles, %d nodes, %d leaves, height %d (bound %d)", m.TriangleCount(), len(m.nodes), leaves, m.Height(), bound) + if m.Height() > bound || m.Height() > meshStackSize-2 { + t.Errorf("height %d over the bound %d", m.Height(), bound) + } +} + +// bruteForceMeshRay: the first triangle hit, seen from the side of its normal, by Möller-Trumbore on every triangle +func bruteForceMeshRay(m *TriangleMesh, origin, translation mgl64.Vec3, slack float64) (float64, int32, bool) { + best, triangle, found := 1.0, NoTriangle, false + for i := 0; i < m.TriangleCount(); i++ { + vertices, _ := m.Triangle(int32(i)) + if degenerate(vertices) { + continue + } + edge1, edge2 := vertices[1].Sub(vertices[0]), vertices[2].Sub(vertices[0]) + p := translation.Cross(edge2) + determinant := edge1.Dot(p) + if determinant <= 0 { + continue + } + s := origin.Sub(vertices[0]) + u := s.Dot(p) / determinant + q := s.Cross(edge1) + v := translation.Dot(q) / determinant + fraction := edge2.Dot(q) / determinant + if u < -slack || v < -slack || u+v > 1+slack || fraction < 0 || fraction > best || (found && fraction == best && int32(i) > triangle) { + continue + } + best, triangle, found = fraction, int32(i), true + } + return best, triangle, found +} + +// A ray on a mesh gives the triangle of the brute force, at its fraction, with the normal of the triangle; a ray from +// behind goes through; a ray on an edge of the grid hits the triangle of the lowest index; a ray without length hits +// nothing +func TestTriangleMeshCastRay(t *testing.T) { + m := hillsMesh() + r := rand.New(rand.NewSource(6)) + hits := 0 + for i := 0; i < 3000; i++ { + origin := mgl64.Vec3{(r.Float64() - 0.5) * 30, r.Float64()*6 - 1, (r.Float64() - 0.5) * 30} + translation := mgl64.Vec3{r.NormFloat64() * 5, -r.Float64() * 10, r.NormFloat64() * 5} + if i%7 == 0 { + translation[1] = -translation[1] // from below: most of them miss + } + wantFraction, wantTriangle, wantOk := bruteForceMeshRay(m, origin, translation, footprintSlack) + hit, ok := m.CastRay(origin, translation, 1) + if ok != wantOk { + t.Fatalf("ray %d from %v: hit %v, the brute force says %v (triangle %d)", i, origin, ok, wantOk, wantTriangle) + } + if !ok { + continue + } + hits++ + if hit.Triangle != wantTriangle || math.Abs(hit.Fraction-wantFraction) > 1e-12 { + t.Fatalf("ray %d: triangle %d at %.12f, the brute force says %d at %.12f", i, hit.Triangle, hit.Fraction, wantTriangle, wantFraction) + } + vertices, _ := m.Triangle(hit.Triangle) + if normal := triangleNormal(vertices); hit.Normal.Sub(normal).Len() > 1e-12 { + t.Fatalf("ray %d: normal %v, want %v", i, hit.Normal, normal) + } + } + if hits < 800 { + t.Fatalf("only %d hits", hits) + } + + // on the lines of a flat grid: a hit, the lowest triangle at the same fraction + flat := gridMesh(10, 1, func(x, z float64) float64 { return 0 }) + for i := 0; i < 200; i++ { + x, z := float64(r.Intn(11)-5), (r.Float64()-0.5)*10 + if i%2 == 0 { + x, z = z, x + } + if i%3 == 0 { + x = math.Round(x) + z = math.Round(z) + } + hit, ok := flat.CastRay(mgl64.Vec3{x, 1, z}, mgl64.Vec3{0, -2, 0}, 1) + if !ok || math.Abs(hit.Fraction-0.5) > 1e-12 { + t.Fatalf("ray on the line (%v, %v): %v %v", x, z, hit, ok) + } + // the rule of the lowest index holds at the very same fraction: the brute force finds the lowest triangle at + // its best fraction, the mesh must give it, or a triangle at a fraction closer by the rounding + fraction, want, _ := bruteForceMeshRay(flat, mgl64.Vec3{x, 1, z}, mgl64.Vec3{0, -2, 0}, footprintSlack) + if hit.Triangle != want && hit.Fraction >= fraction { + t.Fatalf("ray on the line (%v, %v): triangle %d at %.17g, want the lowest %d at %.17g", x, z, hit.Triangle, hit.Fraction, want, fraction) + } + } + if _, ok := flat.CastRay(mgl64.Vec3{0.3, -1, 0.3}, mgl64.Vec3{0, 2, 0}, 1); ok { + t.Errorf("a ray from below hits the grid") + } + if _, ok := flat.CastRay(mgl64.Vec3{0.3, 0, 0.3}, mgl64.Vec3{}, 1); ok { + t.Errorf("a ray without length hits the grid") + } + if _, ok := flat.CastRay(mgl64.Vec3{0.3, 1, 0.3}, mgl64.Vec3{0, -2, 0}, 0.4); ok { + t.Errorf("a ray cut before the grid hits it") + } + if _, ok := flat.CastRay(mgl64.Vec3{math.NaN(), 1, 0.3}, mgl64.Vec3{0, -2, 0}, 1); ok { + t.Errorf("a ray which is not finite hits the grid") + } +} + +// segmentEntersBox: the segment, cut at limit, enters the AABB enlarged by the extents (by slabs) +func segmentEntersBox(box AABB, origin, translation, extents mgl64.Vec3, limit float64) bool { + enter, exit := 0.0, limit + for k := 0; k < 3; k++ { + low, high := box.Min[k]-extents[k]-origin[k], box.Max[k]+extents[k]-origin[k] + if translation[k] == 0 { + if low > 0 || high < 0 { + return false + } + continue + } + near, far := low/translation[k], high/translation[k] + if near > far { + near, far = far, near + } + enter, exit = math.Max(enter, near), math.Min(exit, far) + } + return enter <= exit +} + +// The cast of a box through the tree gives every triangle whose AABB, enlarged by the box, is entered by the segment +// of its center, each once; cut at a limit, every triangle entered before the limit +func TestTriangleMeshCast(t *testing.T) { + m := hillsMesh() + r := rand.New(rand.NewSource(8)) + given := map[int32]bool{} + total := 0 + for i := 0; i < 300; i++ { + origin := mgl64.Vec3{(r.Float64() - 0.5) * 30, r.Float64()*6 - 1, (r.Float64() - 0.5) * 30} + translation := mgl64.Vec3{r.NormFloat64() * 5, -r.Float64() * 10, r.NormFloat64() * 5} + extents := mgl64.Vec3{r.Float64() * 0.5, r.Float64() * 0.5, r.Float64() * 0.5} + if i%4 == 0 { + extents = mgl64.Vec3{} + } + limit := 1.0 + if i%2 == 1 { + limit = r.Float64() + } + cast := m.Cast(origin, translation, extents, 1) + clear(given) + for { + tr, ok := cast.Next(limit) + if !ok { + break + } + if given[tr] { + t.Fatalf("cast %d: the triangle %d is given twice", i, tr) + } + given[tr] = true + } + for tr := 0; tr < m.TriangleCount(); tr++ { + triangle, _ := m.Triangle(int32(tr)) + if !degenerate(triangle) && segmentEntersBox(triangleBounds(triangle), origin, translation, extents, limit) && !given[int32(tr)] { + t.Fatalf("cast %d cut at %.3f: the triangle %d is not given", i, limit, tr) + } + } + total += len(given) + } + if total < 3000 { + t.Fatalf("only %d triangles given", total) + } +} + +// The shape of a mesh: static (infinite mass, no inertia), its AABB turned and moved +func TestTriangleMeshShape(t *testing.T) { + vertices, indices := cubeSoup(0.5) + m, err := NewTriangleMesh(vertices, indices) + if err != nil { + t.Fatal(err) + } + if !math.IsInf(m.ComputeMass(1000), 1) || m.ComputeInertia(1) != (mgl64.Mat3{}) { + t.Errorf("a mesh has a mass or an inertia") + } + transform := Transform{Position: mgl64.Vec3{1, 2, 3}, Rotation: mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0})} + aabb := m.ComputeAABB(transform) + half := math.Sqrt(0.5) + if aabb.Min.Sub(mgl64.Vec3{1 - half, 1.5, 3 - half}).Len() > 1e-12 || aabb.Max.Sub(mgl64.Vec3{1 + half, 2.5, 3 + half}).Len() > 1e-12 { + t.Errorf("AABB %v", aabb) + } + body := NewRigidBody(transform, m, BodyTypeStatic, 0) + if body.AABB() != aabb { + t.Errorf("the body has the AABB %v", body.AABB()) + } +} diff --git a/articulation.go b/articulation.go index 51834d8..c68b764 100644 --- a/articulation.go +++ b/articulation.go @@ -50,15 +50,16 @@ func (s *solver) buildArticulations() { a.bodyJoints = resizeSlices(a.bodyJoints, bodies) a.visited = resizeBools(a.visited, bodies) a.position = resizeInts(a.position, len(s.joints)) + // a kinematic body is a wall for the trees, as a static body: its joints are leaves (dynamicIndex) for i, joint := range s.joints { j := joint.base() j.inArticulation = false a.position[i] = -1 - if j.indexA >= 0 { - a.bodyJoints[j.indexA] = append(a.bodyJoints[j.indexA], i) + if indexA := s.dynamicIndex(j.indexA); indexA >= 0 { + a.bodyJoints[indexA] = append(a.bodyJoints[indexA], i) } - if j.indexB >= 0 { - a.bodyJoints[j.indexB] = append(a.bodyJoints[j.indexB], i) + if indexB := s.dynamicIndex(j.indexB); indexB >= 0 { + a.bodyJoints[indexB] = append(a.bodyJoints[indexB], i) } } @@ -130,7 +131,7 @@ func (s *solver) buildArticulations() { func (s *solver) attachedToStatic(body int) bool { for _, i := range s.articulations.bodyJoints[body] { - if j := s.joints[i].base(); j.indexA < 0 || j.indexB < 0 { + if j := s.joints[i].base(); s.dynamicIndex(j.indexA) < 0 || s.dynamicIndex(j.indexB) < 0 { return true } } @@ -155,13 +156,13 @@ func (s *solver) orderTree(root int) bool { continue } j := s.joints[i].base() - other := j.indexA + other := s.dynamicIndex(j.indexA) if other == body { - other = j.indexB + other = s.dynamicIndex(j.indexB) } switch { case other < 0: - // attached to a static body: a leaf + // attached to a static or a kinematic body: a leaf s.place(i) case !a.visited[other]: a.visited[other] = true diff --git a/bench/baseline.json b/bench/baseline.json index 9ecf2c7..c4c0c07 100644 --- a/bench/baseline.json +++ b/bench/baseline.json @@ -3,33 +3,33 @@ "machine": "AMD Ryzen 7 5800X 8-Core Processor, 16 CPUs", "scenes": { "joint chain": { - "fingerprint": "df487b66cbb2b4cc", + "fingerprint": "e375fb3eb4ce24d4", "quality": { "energy gain": { "value": 0, "unit": "%" }, "worst stretch": { - "value": 23.65572439277502, + "value": 9.119298737745858, "unit": "mm" } }, - "stepMs": 0.22009185000000003, + "stepMs": 0.22301199000000002, "phasesMs": { - "broad phase": 0.018945903333333333, - "continuous": 0.006522506666666667, - "islands": 0.00038624333333333336, - "narrow phase": 0.00135221, - "prepare": 0.00715956, - "restitution": 0.0024266333333333332, - "substeps": 0.18228492333333332 + "broad phase": 0.0192974, + "continuous": 0.006813083333333334, + "islands": 0.00043123666666666674, + "narrow phase": 0.0015468133333333333, + "prepare": 0.007279990000000001, + "restitution": 0.002465933333333333, + "substeps": 0.18380237333333332 } }, "locked bodies": { - "fingerprint": "de903bb2f7759ece", + "fingerprint": "442ffb0e1d20e289", "quality": { "least push": { - "value": -0.05361700907189492, + "value": -0.050598055941050234, "unit": "m" }, "worst lean": { @@ -41,7 +41,7 @@ "unit": "mm" }, "worst stretch": { - "value": 9.304773733705368, + "value": 9.136657420948604, "unit": "mm" }, "worst turn off plane": { @@ -49,15 +49,15 @@ "unit": "" } }, - "stepMs": 0.9530423916666667, + "stepMs": 1.0974901291666666, "phasesMs": { - "broad phase": 0.07955472499999999, - "continuous": 0.019992470833333335, - "islands": 0.002952229166666667, - "narrow phase": 0.23069107083333332, - "prepare": 0.09510173333333333, - "restitution": 0.020720166666666665, - "substeps": 0.5025578833333334 + "broad phase": 0.08469075416666666, + "continuous": 0.029372766666666668, + "islands": 0.003589795833333333, + "narrow phase": 0.2770253041666667, + "prepare": 0.10668754999999999, + "restitution": 0.02166410833333333, + "substeps": 0.5727411625 } }, "pile of 500": { @@ -68,15 +68,15 @@ "unit": "mm" } }, - "stepMs": 0.69534875, + "stepMs": 0.7004177111111111, "phasesMs": { - "broad phase": 0.09051061666666665, - "continuous": 0.006043966666666667, - "islands": 0.0022648777777777777, - "narrow phase": 0.17847557777777778, - "prepare": 0.0682592, - "restitution": 0.020380722222222224, - "substeps": 0.32546101111111114 + "broad phase": 0.09606705555555554, + "continuous": 0.005705511111111111, + "islands": 0.0024900944444444443, + "narrow phase": 0.17642735555555555, + "prepare": 0.06974563333333333, + "restitution": 0.020286266666666667, + "substeps": 0.32840634444444444 } }, "pyramid": { @@ -87,15 +87,15 @@ "unit": "mm" } }, - "stepMs": 0.06100135, + "stepMs": 0.06087862, "phasesMs": { - "broad phase": 0.004403163333333333, - "continuous": 0.00023557, - "islands": 0.00021229999999999998, - "narrow phase": 0.009503561666666665, - "prepare": 0.005879049999999999, - "restitution": 0.0010754766666666667, - "substeps": 0.039461013333333336 + "broad phase": 0.004601518333333334, + "continuous": 0.00021938333333333332, + "islands": 0.000209, + "narrow phase": 0.00921067, + "prepare": 0.00593702, + "restitution": 0.0010863433333333334, + "substeps": 0.039382375000000004 } }, "queries scattered": { @@ -110,15 +110,15 @@ "unit": "" } }, - "stepMs": 59.75905133333333, + "stepMs": 62.447808, "phasesMs": { - "all hits level 50 m": 14.695996000000001, - "capsule level 2 m": 15.354263333333334, - "overlap sphere 1 m": 7.1512243333333325, - "ray down 2 m": 3.9181373333333336, - "ray level 50 m": 9.039622333333334, - "sphere down 2 m": 6.602841000000001, - "sync queries": 2.893712666666667 + "all hits level 50 m": 14.77555, + "capsule level 2 m": 16.204857333333333, + "overlap sphere 1 m": 7.315029, + "ray down 2 m": 3.977032, + "ray level 50 m": 9.608064, + "sphere down 2 m": 6.808702666666666, + "sync queries": 2.9357323333333336 } }, "queries terrain": { @@ -133,15 +133,15 @@ "unit": "mm" } }, - "stepMs": 46.364793666666664, + "stepMs": 53.99426833333334, "phasesMs": { - "ray down on the terrain": 2.5992893333333336, - "ray of 1000 m across the terrain": 2.5271486666666667, - "sphere down on the terrain": 41.23835566666667 + "ray down on the terrain": 3.423365, + "ray of 1000 m across the terrain": 2.8693519999999997, + "sphere down on the terrain": 47.701551333333335 } }, "rain on terrain": { - "fingerprint": "a0ca7a7320e1608e", + "fingerprint": "fb4b0670fc06c4b4", "quality": { "fell through": { "value": 0, @@ -150,52 +150,52 @@ }, "draws": { "landing depth": { - "median": 17.218281823949532, + "median": 17.89686076639699, "unit": "mm", "variants": 32, - "tolerance": 10.413571779183126, - "worst": 82.58796734171223 + "tolerance": 11.151256954913668, + "worst": 47.96395980042037 } }, - "stepMs": 8.877046893333333, + "stepMs": 9.894548166666667, "phasesMs": { - "broad phase": 0.11768916, - "continuous": 3.3405569266666664, - "islands": 0.005727843333333333, - "narrow phase": 3.4047865966666664, - "prepare": 0.29774268000000004, - "restitution": 0.046472846666666665, - "substeps": 1.6427679066666667 + "broad phase": 0.12317079, + "continuous": 3.8663490866666668, + "islands": 0.006995226666666667, + "narrow phase": 3.6217038133333337, + "prepare": 0.35218296666666665, + "restitution": 0.047496616666666665, + "substeps": 1.8031240566666669 } }, "slope pile": { - "fingerprint": "b6adc79f5ef0cde8", + "fingerprint": "ebee9691aa060267", "quality": {}, "draws": { "landing depth": { - "median": 2.053984329581815, + "median": 2.210456217812524, "unit": "mm", "variants": 32, - "tolerance": 1.6952357849386868, - "worst": 8.322796739766602 + "tolerance": 1.165747096459242, + "worst": 8.322796739766714 }, "resting depth": { - "median": 0.1725802194769699, + "median": 0.14554989867798795, "unit": "mm", "variants": 32, - "tolerance": 0.3105487207868407, - "worst": 0.7472149206435574 + "tolerance": 0.11278823844551589, + "worst": 0.5109107633280985 } }, - "stepMs": 0.5205421291666666, + "stepMs": 0.5994101000000001, "phasesMs": { - "broad phase": 0.0761192, - "continuous": 0.026549345833333335, - "islands": 0.0008678833333333333, - "narrow phase": 0.0715856, - "prepare": 0.03613751666666667, - "restitution": 0.010129362499999999, - "substeps": 0.2956359125 + "broad phase": 0.082532625, + "continuous": 0.034937358333333335, + "islands": 0.0012235125, + "narrow phase": 0.08759932916666666, + "prepare": 0.042588920833333335, + "restitution": 0.0110939125, + "substeps": 0.3368199041666667 } }, "solver2d ball and chain": { @@ -210,15 +210,15 @@ "unit": "mm" } }, - "stepMs": 0.31214675666666664, + "stepMs": 0.31453573333333334, "phasesMs": { - "broad phase": 0.020797556666666665, - "continuous": 0.004592016666666666, - "islands": 0.0003969, - "narrow phase": 0.00023123666666666667, - "prepare": 0.007940486666666666, - "restitution": 0.0025251, - "substeps": 0.2753173033333333 + "broad phase": 0.021276466666666664, + "continuous": 0.005579096666666667, + "islands": 0.00042987666666666663, + "narrow phase": 0.00025607, + "prepare": 0.007935850000000001, + "restitution": 0.0025125666666666667, + "substeps": 0.27528864 } }, "solver2d bridge": { @@ -237,38 +237,38 @@ "unit": "mm" } }, - "stepMs": 0.5716243866666666, + "stepMs": 0.5867270866666666, "phasesMs": { - "broad phase": 0.006406896666666667, - "continuous": 0.0017383466666666665, - "islands": 0.0008251766666666667, - "narrow phase": 0.000047636666666666664, - "prepare": 0.014963296666666667, - "restitution": 0.008951076666666667, - "substeps": 0.53789056 + "broad phase": 0.007899423333333332, + "continuous": 0.0018044866666666666, + "islands": 0.0009190766666666666, + "narrow phase": 0.00004963333333333334, + "prepare": 0.015226720000000001, + "restitution": 0.009307059999999999, + "substeps": 0.5504318133333334 } }, "solver2d card house": { - "fingerprint": "118e5849133d9d71", + "fingerprint": "23fa14fda802263f", "quality": { "levels": { "value": 3, "unit": "" }, "worst drift": { - "value": 0.14162818993755702, + "value": 0.13354162921420484, "unit": "mm" } }, - "stepMs": 0.08459638095238095, + "stepMs": 0.08905327619047618, "phasesMs": { - "broad phase": 0.0017992523809523809, - "continuous": 0.000547247619047619, - "islands": 0.0001906238095238095, - "narrow phase": 0.025572080952380952, - "prepare": 0.005574409523809523, - "restitution": 0.0009038761904761906, - "substeps": 0.04982627619047619 + "broad phase": 0.001960295238095238, + "continuous": 0.0006018666666666667, + "islands": 0.0002086285714285714, + "narrow phase": 0.02774414761904762, + "prepare": 0.005847085714285715, + "restitution": 0.0009232952380952382, + "substeps": 0.051478671428571425 } }, "solver2d centered impact": { @@ -283,15 +283,15 @@ "unit": "mm" } }, - "stepMs": 0.006988361111111111, + "stepMs": 0.00894856111111111, "phasesMs": { - "broad phase": 0.00034295, - "continuous": 0.00010066666666666667, - "islands": 0.00011433888888888887, - "narrow phase": 0.0009121166666666667, - "prepare": 0.0007290111111111111, - "restitution": 0.00029872777777777777, - "substeps": 0.004325644444444444 + "broad phase": 0.0004703944444444444, + "continuous": 0.00012800000000000002, + "islands": 0.0001535, + "narrow phase": 0.0011600722222222222, + "prepare": 0.0009429555555555555, + "restitution": 0.00038700555555555555, + "substeps": 0.005448927777777778 } }, "solver2d circle stack": { @@ -306,15 +306,15 @@ "unit": "mm" } }, - "stepMs": 0.0043214, + "stepMs": 0.004443641666666666, "phasesMs": { - "broad phase": 0.0008298229166666666, - "continuous": 0.00011043750000000001, - "islands": 0.00011383541666666667, - "narrow phase": 0.0003176875, - "prepare": 0.0005613187499999999, - "restitution": 0.00018441666666666666, - "substeps": 0.002071233333333333 + "broad phase": 0.00087118125, + "continuous": 0.00010941875, + "islands": 0.00012127291666666666, + "narrow phase": 0.00032208541666666667, + "prepare": 0.0005773145833333333, + "restitution": 0.0001876875, + "substeps": 0.0021001479166666667 } }, "solver2d confined": { @@ -333,19 +333,19 @@ "unit": "" } }, - "stepMs": 0.59223589, + "stepMs": 0.62159325, "phasesMs": { - "broad phase": 0.05154194333333333, - "continuous": 0.0036594766666666664, - "islands": 0.0018215333333333331, - "narrow phase": 0.11584889333333334, - "prepare": 0.09548596333333334, - "restitution": 0.010471023333333333, - "substeps": 0.31178197 + "broad phase": 0.05546579, + "continuous": 0.0035981200000000002, + "islands": 0.0020239933333333336, + "narrow phase": 0.12046342, + "prepare": 0.10479641666666667, + "restitution": 0.010654786666666666, + "substeps": 0.3216453166666667 } }, "solver2d double domino": { - "fingerprint": "8c191c63323d47e2", + "fingerprint": "2a2779b9e8f22068", "quality": { "fallen": { "value": 15, @@ -354,29 +354,29 @@ }, "draws": { "time of the last": { - "median": 7.233333333333313, + "median": 7.241666666666646, "unit": "s", "variants": 32, - "tolerance": 0.04999999999999982, + "tolerance": 0.06250000000000044, "worst": 7.349999999999979 } }, - "stepMs": 0.10574514375, + "stepMs": 0.1088928375, "phasesMs": { - "broad phase": 0.0019995947916666666, - "continuous": 0.0004542322916666666, - "islands": 0.00023536666666666665, - "narrow phase": 0.019646505208333335, - "prepare": 0.008866291666666666, - "restitution": 0.001902140625, - "substeps": 0.071482978125 + "broad phase": 0.0022813072916666667, + "continuous": 0.00045727916666666663, + "islands": 0.00026416041666666666, + "narrow phase": 0.020021432291666665, + "prepare": 0.009129710416666666, + "restitution": 0.002000271875, + "substeps": 0.07454536875 } }, "solver2d far chain": { - "fingerprint": "920bc3620f9389f9", + "fingerprint": "b5f6970dbc7f0c8d", "quality": { "far deviation": { - "value": 1.0814722984127874e-7, + "value": 1.079942530743785e-7, "unit": "mm" }, "finite": { @@ -388,15 +388,15 @@ "unit": "mm" } }, - "stepMs": 0.6374967283333334, + "stepMs": 0.6394512283333333, "phasesMs": { - "broad phase": 0.04901382, - "continuous": 0.016537715, - "islands": 0.0007044716666666667, - "narrow phase": 0.007148178333333333, - "prepare": 0.01498872, - "restitution": 0.00485565, - "substeps": 0.544017615 + "broad phase": 0.050001135, + "continuous": 0.023344856666666667, + "islands": 0.000740175, + "narrow phase": 0.008228735000000001, + "prepare": 0.015145771666666667, + "restitution": 0.0048663583333333335, + "substeps": 0.5365265283333333 } }, "solver2d far pyramid": { @@ -416,15 +416,15 @@ "worst": 116.7944523183155 } }, - "stepMs": 3.4349718183333335, + "stepMs": 3.4155556933333333, "phasesMs": { - "broad phase": 0.08412530333333333, - "continuous": 0.007053413333333333, - "islands": 0.0037678716666666666, - "narrow phase": 0.6581630033333333, - "prepare": 0.25687519666666664, - "restitution": 0.04229622, - "substeps": 2.3775142283333333 + "broad phase": 0.08951543833333334, + "continuous": 0.006593758333333333, + "islands": 0.003941895, + "narrow phase": 0.6560169983333334, + "prepare": 0.2565347866666667, + "restitution": 0.04261444666666667, + "substeps": 2.3559591083333333 } }, "solver2d far recovery": { @@ -439,22 +439,22 @@ "unit": "m/s" } }, - "stepMs": 0.49591346111111106, + "stepMs": 0.5086142055555556, "phasesMs": { - "broad phase": 0.006394630555555556, - "continuous": 0.0006849805555555556, - "islands": 0.0005735055555555555, - "narrow phase": 0.15683636944444443, - "prepare": 0.029888416666666664, - "restitution": 0.0040781555555555555, - "substeps": 0.2971797 + "broad phase": 0.007088475, + "continuous": 0.0006917888888888889, + "islands": 0.0006035944444444444, + "narrow phase": 0.16135723333333332, + "prepare": 0.031161680555555554, + "restitution": 0.0041253194444444445, + "substeps": 0.3032203972222222 } }, "solver2d far stack": { - "fingerprint": "11ea1fce583a161e", + "fingerprint": "b3d0390ea3e0a953", "quality": { "far deviation": { - "value": 0.8869082560041711, + "value": 0.886908174954758, "unit": "mm" }, "worst drift": { @@ -462,15 +462,15 @@ "unit": "mm" } }, - "stepMs": 0.016295401666666667, + "stepMs": 0.01669714, "phasesMs": { - "broad phase": 0.0005315550000000001, - "continuous": 0.00012548333333333334, - "islands": 0.00013293666666666667, - "narrow phase": 0.003260023333333333, - "prepare": 0.0014563933333333332, - "restitution": 0.00044187, - "substeps": 0.010128463333333334 + "broad phase": 0.000615855, + "continuous": 0.000127635, + "islands": 0.000139955, + "narrow phase": 0.003399483333333333, + "prepare": 0.0015074216666666666, + "restitution": 0.0004595816666666667, + "substeps": 0.010311103333333333 } }, "solver2d friction ramp": { @@ -485,15 +485,15 @@ "unit": "mm" } }, - "stepMs": 0.027777786666666665, + "stepMs": 0.02921856, "phasesMs": { - "broad phase": 0.0009969466666666668, - "continuous": 0.00017273333333333332, - "islands": 0.00013646666666666666, - "narrow phase": 0.0105704, - "prepare": 0.0023008333333333335, - "restitution": 0.0006961533333333333, - "substeps": 0.01265182 + "broad phase": 0.0011450000000000002, + "continuous": 0.00018513333333333332, + "islands": 0.00016126666666666667, + "narrow phase": 0.011161693333333333, + "prepare": 0.0024384666666666666, + "restitution": 0.0007389333333333333, + "substeps": 0.01316322 } }, "solver2d high mass ratio 1": { @@ -517,109 +517,109 @@ "worst": 138.8252610057865 } }, - "stepMs": 1.82044033, + "stepMs": 1.9166499700000001, "phasesMs": { - "broad phase": 0.015397880000000001, - "continuous": 0.0024659566666666664, - "islands": 0.0018550233333333333, - "narrow phase": 0.49505928666666665, - "prepare": 0.13516982000000002, - "restitution": 0.01625481, - "substeps": 1.15018284 + "broad phase": 0.01826786, + "continuous": 0.0026056933333333332, + "islands": 0.0022142233333333336, + "narrow phase": 0.5235609133333333, + "prepare": 0.14516094, + "restitution": 0.01783967666666667, + "substeps": 1.2060733533333332 } }, "solver2d high mass ratio 2": { - "fingerprint": "35f80b29687cde2f", + "fingerprint": "17d467c15d1c48c4", "quality": { "bounce": { - "value": 2.0355465723505186, + "value": 1.9698686743722047, "unit": "m/s" }, "slab sag": { - "value": 67.63919875990432, + "value": 67.25136634365825, "unit": "mm" }, "small cubes drift": { - "value": 16.468452377556567, + "value": 17.817296209507198, "unit": "mm" }, "small cubes into ground": { - "value": 530.0366269782514, + "value": 517.6818473019647, "unit": "mm" } }, - "stepMs": 0.015596443333333333, + "stepMs": 0.016494306666666667, "phasesMs": { - "broad phase": 0.0005016399999999999, - "continuous": 0.0001576, - "islands": 0.0001263, - "narrow phase": 0.004176989999999999, - "prepare": 0.0012847833333333334, - "restitution": 0.0004263366666666666, - "substeps": 0.00869783 + "broad phase": 0.0005804133333333334, + "continuous": 0.00015480666666666665, + "islands": 0.00013893333333333334, + "narrow phase": 0.004469506666666666, + "prepare": 0.00134159, + "restitution": 0.00044283333333333333, + "substeps": 0.009166323333333332 } }, "solver2d high mass ratio 3": { - "fingerprint": "16fc3d89762c7f2b", + "fingerprint": "32bed6809aa7f3dd", "quality": { "bounce": { - "value": 2.035546593114035, + "value": 1.969868836978326, "unit": "m/s" }, "slab sag": { - "value": 67.63920297683335, + "value": 67.24738390279583, "unit": "mm" }, "small cubes drift": { - "value": 16.468453893118642, + "value": 17.821486554040813, "unit": "mm" }, "small cubes into ground": { - "value": 530.0366268830902, + "value": 517.6818552126193, "unit": "mm" } }, - "stepMs": 0.01627555, + "stepMs": 0.01787919, "phasesMs": { - "broad phase": 0.00047926666666666664, - "continuous": 0.00014606666666666665, - "islands": 0.00012530333333333332, - "narrow phase": 0.00492815, - "prepare": 0.0012756566666666668, - "restitution": 0.0004244133333333333, - "substeps": 0.008671823333333334 + "broad phase": 0.0005689800000000001, + "continuous": 0.0001452, + "islands": 0.00013636666666666668, + "narrow phase": 0.006021806666666667, + "prepare": 0.00134356, + "restitution": 0.00045063333333333335, + "substeps": 0.009069643333333332 } }, "solver2d joint grid": { - "fingerprint": "8c1a571cd246fa78", + "fingerprint": "3f9690b3dff51bba", "quality": { "finite": { "value": 1, "unit": "" }, "max speed": { - "value": 29.678005033185606, + "value": 29.61888312944309, "unit": "m/s" } }, "draws": { "worst gap": { - "median": 79.91985555188126, + "median": 80.24446346671454, "unit": "mm", "variants": 32, - "tolerance": 3.5446750905452618, - "worst": 85.46939822389496 + "tolerance": 2.0588018734757156, + "worst": 108.15144278745207 } }, - "stepMs": 4.101770872222223, + "stepMs": 4.167568188888889, "phasesMs": { - "broad phase": 0.6989104777777778, - "continuous": 0.045079122222222226, - "islands": 0.008062616666666666, - "narrow phase": 0.10386971111111111, - "prepare": 0.22165523888888888, - "restitution": 0.029008688888888887, - "substeps": 2.9808608666666663 + "broad phase": 0.6918038666666667, + "continuous": 0.056684, + "islands": 0.010288244444444445, + "narrow phase": 0.10751988333333333, + "prepare": 0.23365088333333334, + "restitution": 0.030347894444444447, + "substeps": 3.014188538888889 } }, "solver2d overlap recovery": { @@ -638,15 +638,15 @@ "unit": "m/s" } }, - "stepMs": 0.31637187333333333, + "stepMs": 0.3131749166666667, "phasesMs": { - "broad phase": 0.00493636, - "continuous": 0.00045361, - "islands": 0.00042744000000000007, - "narrow phase": 0.09771449, - "prepare": 0.01854371, - "restitution": 0.00249156, - "substeps": 0.18450917 + "broad phase": 0.005369573333333334, + "continuous": 0.00045757, + "islands": 0.00043120333333333333, + "narrow phase": 0.09785309333333334, + "prepare": 0.01950180333333333, + "restitution": 0.0025782666666666664, + "substeps": 0.18674677333333334 } }, "solver2d pyramid": { @@ -661,19 +661,19 @@ "unit": "mm" } }, - "stepMs": 1.1750501633333332, + "stepMs": 1.2664069466666665, "phasesMs": { - "broad phase": 0.0252278, - "continuous": 0.00128922, - "islands": 0.0012174533333333334, - "narrow phase": 0.31549095, - "prepare": 0.09302091666666668, - "restitution": 0.008838996666666666, - "substeps": 0.7172435966666667 + "broad phase": 0.02772171666666667, + "continuous": 0.00158789, + "islands": 0.0014767633333333335, + "narrow phase": 0.32916048, + "prepare": 0.09662408666666666, + "restitution": 0.011342203333333334, + "substeps": 0.77432627 } }, "solver2d rush": { - "fingerprint": "c3d7fed02d4135f1", + "fingerprint": "fa2086f693949d26", "quality": { "max speed": { "value": 12.944602038140784, @@ -682,29 +682,29 @@ }, "draws": { "final overlap": { - "median": 2.115334050644291, + "median": 2.1376571400363864, "unit": "mm", "variants": 32, - "tolerance": 0.414584819145275, - "worst": 2.5749926407723223 + "tolerance": 0.6183781580575698, + "worst": 3.0599492765966962 }, "final speed": { - "median": 0.22430994395373535, + "median": 0.20800663777451026, "unit": "m/s", "variants": 32, - "tolerance": 0.4801212922824684, - "worst": 1.2316129803361309 + "tolerance": 0.286175640047562, + "worst": 0.6861658272995366 } }, - "stepMs": 0.06680301666666666, + "stepMs": 0.07139497, "phasesMs": { - "broad phase": 0.017867220000000003, - "continuous": 0.004094346666666667, - "islands": 0.0008140433333333333, - "narrow phase": 0.00008163333333333334, - "prepare": 0.007779333333333333, - "restitution": 0.00654039, - "substeps": 0.029011603333333334 + "broad phase": 0.020713969999999998, + "continuous": 0.0039762, + "islands": 0.0010267533333333334, + "narrow phase": 0.00009946666666666667, + "prepare": 0.008146933333333333, + "restitution": 0.006789236666666666, + "substeps": 0.030446010000000003 } }, "solver2d single box": { @@ -719,15 +719,15 @@ "unit": "mm" } }, - "stepMs": 0.002997588888888889, + "stepMs": 0.005444461111111111, "phasesMs": { - "broad phase": 0.00022744444444444442, - "continuous": 0.00007744444444444444, - "islands": 0.00010177777777777777, - "narrow phase": 0.00017077777777777778, - "prepare": 0.0003103888888888889, - "restitution": 0.0001725611111111111, - "substeps": 0.0018186222222222222 + "broad phase": 0.0004331166666666667, + "continuous": 0.00012933333333333332, + "islands": 0.00018327777777777776, + "narrow phase": 0.000266, + "prepare": 0.0005651722222222222, + "restitution": 0.0002769, + "substeps": 0.0033888277777777776 } }, "solver2d stretched chain": { @@ -750,15 +750,15 @@ "unit": "mm" } }, - "stepMs": 0.04360726, + "stepMs": 0.043379481666666664, "phasesMs": { - "broad phase": 0.0033470116666666667, - "continuous": 0.00040148833333333335, - "islands": 0.00022413833333333332, - "narrow phase": 0.00010278666666666666, - "prepare": 0.0019959183333333333, - "restitution": 0.00043622, - "substeps": 0.03681105666666667 + "broad phase": 0.0033856433333333334, + "continuous": 0.00038864, + "islands": 0.00020358666666666665, + "narrow phase": 0.00011363333333333334, + "prepare": 0.0019641866666666665, + "restitution": 0.000427275, + "substeps": 0.03657348 } }, "solver2d vertical stack": { @@ -773,15 +773,15 @@ "unit": "" } }, - "stepMs": 0.02214192, + "stepMs": 0.022106533333333334, "phasesMs": { - "broad phase": 0.0011675733333333332, - "continuous": 0.00018533666666666667, - "islands": 0.00016870333333333335, - "narrow phase": 0.00350281, - "prepare": 0.0021247, - "restitution": 0.0007701066666666666, - "substeps": 0.01353484 + "broad phase": 0.0013727566666666667, + "continuous": 0.0002101, + "islands": 0.0001649, + "narrow phase": 0.0035674933333333337, + "prepare": 0.0021222666666666666, + "restitution": 0.00078755, + "substeps": 0.013235543333333334 } }, "solver2d warm start energy": { @@ -792,15 +792,15 @@ "unit": "mm" } }, - "stepMs": 0.0037998066666666665, + "stepMs": 0.00388577, "phasesMs": { - "broad phase": 0.00033557, - "continuous": 0.00009076666666666667, - "islands": 0.00010963333333333335, - "narrow phase": 0.0003098033333333333, - "prepare": 0.0005864033333333334, - "restitution": 0.00015683333333333334, - "substeps": 0.0020723266666666665 + "broad phase": 0.0003668733333333333, + "continuous": 0.00008963333333333333, + "islands": 0.0001169, + "narrow phase": 0.00031520666666666664, + "prepare": 0.00059258, + "restitution": 0.00016037, + "substeps": 0.0021129133333333337 } }, "spinning tops": { @@ -819,45 +819,45 @@ "unit": "mm" } }, - "stepMs": 0.4575982833333333, + "stepMs": 0.44964708333333336, "phasesMs": { - "broad phase": 0.017041905555555556, - "continuous": 0.010158511111111112, - "islands": 0.000981622222222222, - "narrow phase": 0.02932755, - "prepare": 0.041461066666666664, - "restitution": 0.009315577777777778, - "substeps": 0.34743975 + "broad phase": 0.017533533333333334, + "continuous": 0.00961046111111111, + "islands": 0.0010412444444444446, + "narrow phase": 0.028630177777777776, + "prepare": 0.04141895555555555, + "restitution": 0.009267427777777778, + "substeps": 0.34150290555555557 } }, "terrain piles": { - "fingerprint": "171ddf25f205ed46", + "fingerprint": "b23884faf0885dc4", "quality": {}, "draws": { "landing depth": { - "median": 8.387453558916441, + "median": 7.228981890187681, "unit": "mm", "variants": 32, - "tolerance": 5.102519961444935, - "worst": 38.89905371202081 + "tolerance": 9.029772686571398, + "worst": 29.280296074383454 }, "resting depth": { - "median": 0.39821435581023923, + "median": 0.29688152007436436, "unit": "mm", "variants": 32, - "tolerance": 3.065757102671334, - "worst": 24.309573266263815 + "tolerance": 0.12488629816916322, + "worst": 1.3423865310179806 } }, - "stepMs": 3.0284619412500002, + "stepMs": 3.1626479191666665, "phasesMs": { - "broad phase": 0.02969977166666667, - "continuous": 1.4339314841666668, - "islands": 0.0014310208333333334, - "narrow phase": 0.9080190854166666, - "prepare": 0.08607033624999999, - "restitution": 0.012563102916666668, - "substeps": 0.5447917120833333 + "broad phase": 0.03233981416666667, + "continuous": 1.4324611783333334, + "islands": 0.0018735670833333333, + "narrow phase": 0.9542110433333333, + "prepare": 0.09691033666666667, + "restitution": 0.013233257916666666, + "substeps": 0.6033585795833333 } } } diff --git a/bench/regression.go b/bench/regression.go index eb16664..fb67fe8 100644 --- a/bench/regression.go +++ b/bench/regression.go @@ -575,6 +575,7 @@ func underTerrain(field *actor.Heightfield, b *actor.RigidBody) float64 { } depth = math.Max(depth, sunk(field, b.Transform.ToWorld(corner), 0)) } + depth = math.Max(depth, terrainInBox(field, b, shape)) case *actor.Sphere: depth = sunk(field, b.Transform.Position, shape.Radius) case *actor.Capsule: @@ -586,6 +587,57 @@ func underTerrain(field *actor.Heightfield, b *actor.RigidBody) float64 { return depth } +// terrainInBox: how deep the terrain is in the box (identity transform of the terrain), 0 if it stays out. The corners +// of a box can all be above the terrain while a ridge or a peak is in one of its faces. A point in the box is as deep +// as its distance to the closest face; the terrain is made of planes, its deepest point in a face of the box is on an +// edge of a triangle: the edges of the triangles under the box are followed, each from end to end +func terrainInBox(field *actor.Heightfield, b *actor.RigidBody, box *actor.Box) float64 { + depth := 0.0 + cellsZ := field.ZSamples - 1 + for _, cell := range field.OverlapCells(b.AABB(), nil) { + for t := 0; t < 2; t++ { + triangle, _ := field.Triangle(int(cell)/cellsZ, int(cell)%cellsZ, t) + for e := 0; e < 3; e++ { + start, end := b.Transform.ToLocal(triangle[e]), b.Transform.ToLocal(triangle[(e+1)%3]) + depth = math.Max(depth, segmentInBox(start, end, box.HalfExtents)) + } + } + } + return depth +} + +// segmentInBox: the deepest point of the segment in the box (in its local space): the highest value, over the segment, +// of the distance to the closest face, 0 if the segment stays out. Along the segment the distance to each of the 6 +// faces is linear; the closest face changes where 2 of these distances are equal: the deepest point is at an end, or +// at one of these crossings +func segmentInBox(start, end, halfExtents mgl64.Vec3) float64 { + // the distance to the face i at the fraction f of the segment: at[i] + f*along[i] + var at, along [6]float64 + for k := 0; k < 3; k++ { + at[2*k], along[2*k] = halfExtents[k]-start[k], start[k]-end[k] + at[2*k+1], along[2*k+1] = halfExtents[k]+start[k], end[k]-start[k] + } + closest := func(fraction float64) float64 { + distance := math.Inf(1) + for i := range at { + distance = math.Min(distance, at[i]+fraction*along[i]) + } + return distance + } + depth := math.Max(0, math.Max(closest(0), closest(1))) + for i := range at { + for j := i + 1; j < len(at); j++ { + if along[i] == along[j] { + continue + } + if fraction := (at[j] - at[i]) / (along[i] - along[j]); fraction > 0 && fraction < 1 { + depth = math.Max(depth, closest(fraction)) + } + } + } + return depth +} + // sunkReach: a point under the terrain is measured against the triangles 1 m around it at most const sunkReach = 1.0 diff --git a/bench/regression_test.go b/bench/regression_test.go index 5bfd89c..d99b7a6 100644 --- a/bench/regression_test.go +++ b/bench/regression_test.go @@ -7,6 +7,9 @@ import ( "math/rand" "slices" "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" ) // The draw of an ensemble: its median, its worst variant, and a tolerance of 3 standard deviations of the median, @@ -178,3 +181,79 @@ func TestCompareDraws(t *testing.T) { t.Error("the reference holds one value of a measure which is now a draw: want a regression") } } + +// ridgeField: a ridge along Z at x = 0 and y = 0, both slopes going down by the angle, 9x9 samples every 0.5 m +func ridgeField(angle float64) *actor.Heightfield { + const samples = 9 + heights := make([]float32, samples*samples) + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + heights[x*samples+z] = float32(-math.Abs(float64(x)-4) * 0.5 * math.Tan(angle)) + } + } + return actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{0.5, 1, 0.5}) +} + +// The depth of a box in a terrain counts the terrain in its faces: a ridge 10 mm in the bottom face of a box, its 8 +// corners above the slopes. Between 2 rows of samples no sample is in the box: the ridge is an edge of the triangles +func TestUnderTerrainSeesTheTerrainInTheFacesOfABox(t *testing.T) { + field := ridgeField(10 * math.Pi / 180) + box := &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.15, 0.15}} + for _, c := range []struct { + name string + position mgl64.Vec3 + rotation mgl64.Quat + want float64 + }{ + {"across the ridge, between 2 rows of samples", mgl64.Vec3{0, 0.14, 0.25}, mgl64.QuatIdent(), 0.010}, + {"turned around the vertical", mgl64.Vec3{0, 0.14, 0.25}, mgl64.QuatRotate(0.5, mgl64.Vec3{0, 1, 0}), 0.010}, + {"over a sample of the ridge", mgl64.Vec3{0, 0.147, 0.5}, mgl64.QuatIdent(), 0.003}, + {"above the ridge", mgl64.Vec3{0, 0.151, 0.25}, mgl64.QuatIdent(), 0}, + {"a corner under a slope, deeper than the ridge in the face", mgl64.Vec3{1, 0.15 - math.Tan(10*math.Pi/180) - 0.02, 0.25}, mgl64.QuatIdent(), 0}, + } { + b := actor.NewRigidBody(actor.Transform{Position: c.position, Rotation: c.rotation}, box, actor.BodyTypeDynamic, 500) + got := underTerrain(field, b) + if c.want == 0 && c.name != "above the ridge" { + // the corners: their distance to the slope, as before + corners := 0.0 + for k := 0; k < 8; k++ { + corner := box.HalfExtents + for axis := 0; axis < 3; axis++ { + if k&(1< 1e-6 { + t.Errorf("%s: the terrain is %.4f mm in the box, want %.4f", c.name, got*1000, c.want*1000) + } + } +} + +// The deepest point of a segment in a box: at an end, or where the closest face changes +func TestSegmentInBox(t *testing.T) { + half := mgl64.Vec3{1, 1, 1} + for _, c := range []struct { + name string + start, end mgl64.Vec3 + want float64 + }{ + {"through 2 opposite faces, along a face at 0.5", mgl64.Vec3{-2, 0.5, 0}, mgl64.Vec3{2, 0.5, 0}, 0.5}, + {"from a corner edge to the opposite one, through the center", mgl64.Vec3{-1, 1, 0}, mgl64.Vec3{1, -1, 0}, 1}, + {"out of the box", mgl64.Vec3{-2, 2, 0}, mgl64.Vec3{2, 2, 0}, 0}, + {"an end in the box", mgl64.Vec3{0.2, 0.3, 0.6}, mgl64.Vec3{3, 0.3, 0.6}, 0.4}, + {"a point", mgl64.Vec3{0.2, 0.3, 0.6}, mgl64.Vec3{0.2, 0.3, 0.6}, 0.4}, + {"across a corner", mgl64.Vec3{0.8, 2, 0}, mgl64.Vec3{2, 0.8, 0}, 0}, + {"cutting a corner", mgl64.Vec3{0.4, 1.2, 0}, mgl64.Vec3{1.2, 0.4, 0}, 0.2}, + } { + if got := segmentInBox(c.start, c.end, half); math.Abs(got-c.want) > 1e-12 { + t.Errorf("%s: %.6f, want %.6f", c.name, got, c.want) + } + } +} diff --git a/ccd.go b/ccd.go index 09da760..40840cb 100644 --- a/ccd.go +++ b/ccd.go @@ -5,16 +5,24 @@ import ( "sync" "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" "github.com/akmonengine/feather/gjk" "github.com/go-gl/mathgl/mgl64" ) // ========== CONTINUOUS COLLISION ========== // The speculative contacts stop most of the fast bodies. They can miss a body accelerated by the solver during the step: -// as in Box2D v3, after the solver, a fast body is moved back to its first impact with a static body (or with any body -// for a bullet), found along its motion. Its velocity is kept: the contact of the next step stops it. A body is only -// stopped by the bodies it collides with (World.ShouldCollide, as the continuous collision of Box2D skips the filtered -// shapes and bodies). +// after the solver, a fast body is moved back to its first impact with any body, found along its motion (the time of +// impact of Box2D v3, against every body as the LinearCast motion quality of Jolt). Its velocity is kept: the contact of +// the next step stops it. A body is only stopped by the bodies it collides with (World.ShouldCollide, as the continuous +// collision of Box2D skips the filtered shapes and bodies). +// +// A static or a kinematic body, and a dynamic body which is not fast, are where the step left them: the fast body is +// swept against their final pose (Box2D, Jolt: "Body has already moved"). Two fast bodies are swept once, by their +// relative motion against the start pose of the other (Jolt: the cast of a body against a body which casts too takes +// "the relative movement of these two bodies", PhysicsSystem.cpp:1884), and both are stopped at the fraction found +// (Jolt: "the other body will shorten its distance traveled", :2054-2060). The fractions are all found before a body +// moves: the result doesn't depend on the order of the bodies. const ( // continuousSafetyFactor: a body is fast when it moves more than half of its smallest extent during a step (Box2D) @@ -54,64 +62,181 @@ func (s *sweep) at(t float64) actor.Transform { } } +// fastBody: a body which moved more than half of its smallest extent during the step, to be stopped at its first impact +type fastBody struct { + // index of its state in the solver + state int32 + motion sweep + // the distance of the farthest point of the body from its center (m), and the fraction of its motion kept (1: all) + radius float64 + fraction float64 +} + // ccdScratch: the buffers of the continuous collision, reused to avoid the allocations type ccdScratch struct { stack []int32 candidates []int32 cells []int32 + triangles []int32 shape triangleShape core actor.Sphere + // fast: the fast bodies of the step, in the order of the states; fastOf[state] is the index of a state in fast, -1 + // if the body is not fast + fast []fastBody + fastOf []int32 + // the bodies in contact with each fast body this step: partners[offsets[k]:offsets[k+1]] for the fast body k; + // touched[body] is k+1 while the fast body k is swept + offsets []int32 + partners []int32 + touched []int32 } var ccdPool = sync.Pool{New: func() any { return &ccdScratch{} }} -// continuous collision of the fast bodies: first the bodies against the static bodies, then the bullets against all -// the bodies (at their final position). The result doesn't depend on the order of the bodies -func (w *World) continuous(s *solver, dt float64) { +// continuous collision of the fast bodies: their fractions are all found first, against the bodies where the step left +// them (the start pose of another fast body), then the bodies are moved. The result doesn't depend on the order of the +// bodies. The manifolds are the contacts of the step +func (w *World) continuous(s *solver, manifolds []constraint.Manifold, dt float64) { scratch := ccdPool.Get().(*ccdScratch) defer ccdPool.Put(scratch) - for _, bullets := range [2]bool{false, true} { - for i := range s.states { - state := &s.states[i] - body := state.body - if body.IsBullet != bullets || body.IsTrigger { - continue - } - minExtent, maxExtent := shapeExtents(body.Shape) - motion := sweep{ - start: actor.Transform{Position: body.Transform.Position.Sub(state.deltaPosition), Rotation: s.starts[i].rotation}, - end: body.Transform, - } - motion.angle = rotationAngle(state.deltaRotation) - // the farthest point of the body moves at most by the translation + the rotation * its extent - if state.deltaPosition.Len()+motion.angle*maxExtent <= continuousSafetyFactor*minExtent { - continue + scratch.fast = scratch.fast[:0] + if cap(scratch.fastOf) < len(s.states) { + scratch.fastOf = make([]int32, len(s.states)) + } + scratch.fastOf = scratch.fastOf[:len(s.states)] + for i := range s.states { + scratch.fastOf[i] = -1 + state := &s.states[i] + body := state.body + // a kinematic body is never stopped ("Kinematic bodies cannot be stopped", Jolt PhysicsSystem.cpp:1564) + if !state.dynamic || body.IsTrigger { + continue + } + minExtent, maxExtent := shapeExtents(body.Shape) + motion := sweep{ + start: actor.Transform{Position: body.Transform.Position.Sub(state.deltaPosition), Rotation: s.starts[i].rotation}, + end: body.Transform, + } + motion.angle = rotationAngle(state.deltaRotation) + // the farthest point of the body moves at most by the translation + the rotation * its extent + if state.deltaPosition.Len()+motion.angle*maxExtent <= continuousSafetyFactor*minExtent { + continue + } + scratch.fastOf[i] = int32(len(scratch.fast)) + scratch.fast = append(scratch.fast, fastBody{state: int32(i), motion: motion, radius: maxExtent, fraction: 1}) + } + if len(scratch.fast) == 0 { + return + } + w.contactPartners(s, manifolds, scratch) + for k := range scratch.fast { + fast := &scratch.fast[k] + scratch.core.Radius = coreFraction * shapeMinExtent(s.states[fast.state].body.Shape) + w.findImpact(s, k, scratch) + } + for k := range scratch.fast { + fast := &scratch.fast[k] + if fast.fraction < 1 { + body := s.states[fast.state].body + body.Transform = fast.motion.at(fast.fraction) + if body.AngularLock == actor.AllAxes { + // it didn't turn: the interpolation of 2 equal rotations is not the rotation bit for bit + body.Transform.Rotation = fast.motion.end.Rotation } - scratch.core.Radius = coreFraction * minExtent - w.stopAtImpact(body, &motion, maxExtent, scratch) + body.UpdateAABB() } } } -// stopAtImpact moves the body back to its first impact during its motion -func (w *World) stopAtImpact(body *actor.RigidBody, motion *sweep, radius float64, scratch *ccdScratch) { - // the bodies around the motion - swept := body.Shape.ComputeAABB(motion.start) - end := body.Shape.ComputeAABB(motion.end) - for k := 0; k < 3; k++ { - swept.Min[k] = math.Min(swept.Min[k], end.Min[k]) - swept.Max[k] = math.Max(swept.Max[k], end.Max[k]) +// contactPartners lists, for each fast body, the bodies it has a contact with this step (the manifolds of the pair, found +// by the narrow phase up to the speculative margin) +func (w *World) contactPartners(s *solver, manifolds []constraint.Manifold, scratch *ccdScratch) { + if cap(scratch.offsets) < len(scratch.fast)+1 { + scratch.offsets = make([]int32, len(scratch.fast)+1) + } + scratch.offsets = scratch.offsets[:len(scratch.fast)+1] + clear(scratch.offsets) + fastOfBody := func(index int32) int32 { + if state := s.stateIndex[index]; state >= 0 { + return scratch.fastOf[state] + } + return -1 + } + // the count of partners of each fast body, then the offsets + for i := range manifolds { + m := &manifolds[i] + if k := fastOfBody(m.IndexA); k >= 0 { + scratch.offsets[k+1]++ + } + if k := fastOfBody(m.IndexB); k >= 0 { + scratch.offsets[k+1]++ + } + } + for k := 1; k < len(scratch.offsets); k++ { + scratch.offsets[k] += scratch.offsets[k-1] + } + total := int(scratch.offsets[len(scratch.offsets)-1]) + if cap(scratch.partners) < total { + scratch.partners = make([]int32, total) + } + scratch.partners = scratch.partners[:total] + if cap(scratch.touched) < len(w.Bodies) { + scratch.touched = make([]int32, len(w.Bodies)) + } + scratch.touched = scratch.touched[:len(w.Bodies)] + clear(scratch.touched) + // the partners, each fast body filling its range from its offset + for i := range manifolds { + m := &manifolds[i] + if k := fastOfBody(m.IndexA); k >= 0 { + scratch.partners[scratch.offsets[k]] = m.IndexB + scratch.offsets[k]++ + } + if k := fastOfBody(m.IndexB); k >= 0 { + scratch.partners[scratch.offsets[k]] = m.IndexA + scratch.offsets[k]++ + } + } + // the offsets are back to the start of each range + for k := len(scratch.offsets) - 1; k > 0; k-- { + scratch.offsets[k] = scratch.offsets[k-1] } + scratch.offsets[0] = 0 +} + +// findImpact finds the first impact of the fast body k during its motion, with every body it collides with, and +// shortens its fraction (and the one of another fast body it meets). A dynamic or a kinematic body the fast body has +// a contact with this step is left to the solver: the contact already holds them apart, and the sweep would undo the +// overlap the soft contact allows +func (w *World) findImpact(s *solver, k int, scratch *ccdScratch) { + fast := &scratch.fast[k] + body := s.states[fast.state].body + motion := &fast.motion + // the bodies around the motion + swept := sweptAABB(body.Shape, motion) scratch.stack, scratch.candidates = w.tree.queryCandidates(swept, scratch.stack, scratch.candidates[:0]) + for _, partner := range scratch.partners[scratch.offsets[k]:scratch.offsets[k+1]] { + scratch.touched[partner] = int32(k + 1) + } - fraction := 1.0 for _, index := range scratch.candidates { other := w.Bodies[index] if other == body || other.IsTrigger || !w.ShouldCollide(body, other) { continue } - // the bullets against all the bodies, the other bodies against the static bodies only - if other.BodyType != actor.BodyTypeStatic && (!body.IsBullet || other.IsBullet) { + if other.BodyType != actor.BodyTypeStatic && scratch.touched[index] == int32(k+1) { + continue + } + otherFast := -1 + if state := s.stateIndex[index]; state >= 0 { + otherFast = int(scratch.fastOf[state]) + } + if otherFast >= 0 { + // 2 fast bodies are swept once, by the one with the smallest index + if otherFast < k { + continue + } + w.fastPairImpact(s, k, otherFast, scratch) continue } if !swept.Overlaps(other.AABB()) { @@ -119,23 +244,49 @@ func (w *World) stopAtImpact(body *actor.RigidBody, motion *sweep, radius float6 } switch shape := other.Shape.(type) { case *actor.Plane: - fraction = math.Min(fraction, impact(body.Shape, motion, radius, nil, shape, fraction, scratch)) - case *actor.Heightfield: - fraction = math.Min(fraction, w.heightfieldImpact(body.Shape, motion, radius, other, shape, swept, fraction, scratch)) + fast.fraction = math.Min(fast.fraction, impact(body.Shape, motion, fast.radius, nil, shape, fast.fraction, scratch)) + case *actor.Heightfield, *actor.TriangleMesh: + fast.fraction = math.Min(fast.fraction, surfaceImpact(body.Shape, motion, fast.radius, other, swept, fast.fraction, scratch)) default: proxy := gjk.NewProxy(other) - fraction = math.Min(fraction, impact(body.Shape, motion, radius, &proxy, nil, fraction, scratch)) + fast.fraction = math.Min(fast.fraction, impact(body.Shape, motion, fast.radius, &proxy, nil, fast.fraction, scratch)) } } +} - if fraction < 1 { - body.Transform = motion.at(fraction) - if body.AngularLock == actor.AllAxes { - // it didn't turn: the interpolation of 2 equal rotations is not the rotation bit for bit - body.Transform.Rotation = motion.end.Rotation - } +// fastPairImpact: the first impact of 2 fast bodies, found by sweeping the body k with their relative motion against +// the start pose of the body j, as Jolt (PhysicsSystem.cpp:1884). The fraction is given to both, so that both are +// stopped where they meet (Jolt, :2054-2060) +func (w *World) fastPairImpact(s *solver, k, j int, scratch *ccdScratch) { + fast, other := &scratch.fast[k], &scratch.fast[j] + bodyA, bodyB := s.states[fast.state].body, s.states[other.state].body + relative := sweep{ + start: fast.motion.start, + end: actor.Transform{ + Position: fast.motion.end.Position.Sub(other.motion.end.Position.Sub(other.motion.start.Position)), + Rotation: fast.motion.end.Rotation, + }, + angle: fast.motion.angle, + } + if !sweptAABB(bodyA.Shape, &relative).Overlaps(bodyB.Shape.ComputeAABB(other.motion.start)) { + return + } + proxy := gjk.NewProxyAt(other.motion.start, bodyB.Shape) + maxFraction := math.Max(fast.fraction, other.fraction) + fraction := impact(bodyA.Shape, &relative, fast.radius, &proxy, nil, maxFraction, scratch) + fast.fraction = math.Min(fast.fraction, fraction) + other.fraction = math.Min(other.fraction, fraction) +} + +// sweptAABB: the AABB of the shape at the start and at the end of the motion +func sweptAABB(shape actor.ShapeInterface, motion *sweep) actor.AABB { + swept := shape.ComputeAABB(motion.start) + end := shape.ComputeAABB(motion.end) + for k := 0; k < 3; k++ { + swept.Min[k] = math.Min(swept.Min[k], end.Min[k]) + swept.Max[k] = math.Max(swept.Max[k], end.Max[k]) } - body.UpdateAABB() + return swept } // impact: the fraction of the motion at the first impact with the convex shape or the plane, 1 if there is none. @@ -189,28 +340,55 @@ func timeOfImpact(shape actor.ShapeInterface, motion *sweep, radius float64, oth return t } -// heightfieldImpact: the first impact with the triangles under the motion -func (w *World) heightfieldImpact(shape actor.ShapeInterface, motion *sweep, radius float64, terrain *actor.RigidBody, field *actor.Heightfield, swept actor.AABB, maxFraction float64, scratch *ccdScratch) float64 { - scratch.cells = field.OverlapCells(localBounds(terrain.Transform, swept), scratch.cells[:0]) +// surfaceImpact: the first impact with the triangles of the surface (a heightfield, a mesh) under the motion +func surfaceImpact(shape actor.ShapeInterface, motion *sweep, radius float64, surface *actor.RigidBody, swept actor.AABB, maxFraction float64, scratch *ccdScratch) float64 { + local := localBounds(surface.Transform, swept) fraction := maxFraction - cellsZ := field.ZSamples - 1 - for _, cell := range scratch.cells { - x, z := int(cell)/cellsZ, int(cell)%cellsZ - for t := 0; t < 2; t++ { - local, _ := field.Triangle(x, z, t) - for i := range local { - scratch.shape.vertices[i] = terrain.Transform.ToWorld(local[i]) - } - if !triangleAABB(scratch.shape.vertices).Overlaps(swept) { - continue + switch other := surface.Shape.(type) { + case *actor.Heightfield: + scratch.cells = other.OverlapCells(local, scratch.cells[:0]) + cellsZ := other.ZSamples - 1 + for _, cell := range scratch.cells { + x, z := int(cell)/cellsZ, int(cell)%cellsZ + for t := 0; t < 2; t++ { + vertices, _ := other.Triangle(x, z, t) + fraction = triangleImpact(shape, motion, radius, surface, vertices, swept, fraction, scratch) } - proxy := gjk.NewProxyAt(actor.NewTransform(), &scratch.shape) - fraction = math.Min(fraction, impact(shape, motion, radius, &proxy, nil, fraction, scratch)) + } + case *actor.TriangleMesh: + scratch.triangles = other.OverlapTriangles(local, scratch.triangles[:0]) + for _, t := range scratch.triangles { + vertices, _ := other.Triangle(t) + fraction = triangleImpact(shape, motion, radius, surface, vertices, swept, fraction, scratch) } } return fraction } +// triangleImpact: the impact with the triangle of the surface, given in its local space, before the fraction +func triangleImpact(shape actor.ShapeInterface, motion *sweep, radius float64, surface *actor.RigidBody, vertices [3]mgl64.Vec3, swept actor.AABB, fraction float64, scratch *ccdScratch) float64 { + for i := range vertices { + scratch.shape.vertices[i] = surface.Transform.ToWorld(vertices[i]) + } + if !triangleAABB(scratch.shape.vertices).Overlaps(swept) { + return fraction + } + proxy := gjk.NewProxyAt(actor.NewTransform(), &scratch.shape) + return math.Min(fraction, impact(shape, motion, radius, &proxy, nil, fraction, scratch)) +} + +// isFast: the body can move more than half of its smallest extent during the step, from its velocities at the start +// of the step (the fast body of Box2D v3: maxVelocity * timeStep > 0.5 * minExtent, with the velocity of the farthest +// point of the body, src/solver.c:584 & :622). The narrow phase gives the pairs of a fast body the speculative margin +// of its speed; the continuous collision judges the motion the step made +func isFast(body *actor.RigidBody, dt float64) bool { + if body.BodyType != actor.BodyTypeDynamic || body.IsSleeping { + return false + } + minExtent, maxExtent := shapeExtents(body.Shape) + return (body.Velocity.Len()+body.AngularVelocity.Len()*maxExtent)*dt > continuousSafetyFactor*minExtent +} + // shapeExtents: the smallest half size of the shape, and the distance of its farthest point from its center func shapeExtents(shape actor.ShapeInterface) (float64, float64) { switch shape := shape.(type) { @@ -227,6 +405,12 @@ func shapeExtents(shape actor.ShapeInterface) (float64, float64) { return math.Min(size.X(), math.Min(size.Y(), size.Z())), size.Len() } +// shapeMinExtent: the smallest half size of the shape +func shapeMinExtent(shape actor.ShapeInterface) float64 { + minExtent, _ := shapeExtents(shape) + return minExtent +} + // rotationAngle of a unit quaternion (rad) func rotationAngle(q mgl64.Quat) float64 { return 2 * math.Acos(math.Min(1, math.Abs(q.W))) diff --git a/ccd_test.go b/ccd_test.go index 85cdcc7..92e6a66 100644 --- a/ccd_test.go +++ b/ccd_test.go @@ -41,25 +41,327 @@ func TestTimeOfImpact(t *testing.T) { } } -// A normal body goes through the dynamic bodies during the continuous collision, a bullet stops on them -func TestBulletStopsOnDynamicBodies(t *testing.T) { - for _, bullet := range []bool{false, true} { +// moveThrough gives the body the motion of a step, as if the solver had accelerated it: the body is at the end of the +// motion, its state in the solver remembers the start. Then the continuous collision runs on the world +func moveThrough(w *World, body *actor.RigidBody, motion sweep) { + s := &w.solver + i := stateOf(w, body) + s.states[i].deltaPosition = motion.end.Position.Sub(motion.start.Position) + s.states[i].deltaRotation = motion.end.Rotation.Mul(motion.start.Rotation.Conjugate()).Normalize() + s.starts[i].rotation = motion.start.Rotation + body.Transform = motion.end + body.UpdateAABB() + w.continuous(s, w.Contacts(), sceneDt) +} + +// stateOf: the index of the state of the body in the solver, after a step +func stateOf(w *World, body *actor.RigidBody) int { + for i, b := range w.Bodies { + if b == body { + if state := w.solver.stateIndex[i]; state >= 0 { + return int(state) + } + panic("the body has no state: step the world first") + } + } + panic("the body is not in the world") +} + +// A fast body is stopped by every body it collides with: a static body, a kinematic body, a dynamic body awake or asleep, +// a dynamic body with all its axes locked. Nothing marks the body: every fast body is the bullet of Box2D +func TestFastBodyStopsOnEveryBody(t *testing.T) { + plate := &actor.Box{HalfExtents: mgl64.Vec3{0.01, 1, 1}} + cases := []struct { + name string + wall func(w *World) *actor.RigidBody + }{ + {"static", func(w *World) *actor.RigidBody { + return addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeStatic, 0.5, 0) + }}, + {"kinematic", func(w *World) *actor.RigidBody { + return addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeKinematic, 0.5, 0) + }}, + {"dynamic", func(w *World) *actor.RigidBody { + return addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + }}, + {"sleeping", func(w *World) *actor.RigidBody { + wall := addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + wall.IsSleeping = true + return wall + }}, + {"locked", func(w *World) *actor.RigidBody { + wall := addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + wall.LinearLock, wall.AngularLock = actor.AllAxes, actor.AllAxes + return wall + }}, + } + for _, c := range cases { w := newScene(1) w.Gravity = mgl64.Vec3{} - addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.01, 1, 1}}, actor.BodyTypeDynamic, 0.5, 0) + wall := c.wall(w) ball := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.02}, actor.BodyTypeDynamic, 0.5, 0) - ball.IsBullet = bullet w.Step(sceneDt) + moveThrough(w, ball, sweep{start: ball.Transform, end: actor.Transform{Position: mgl64.Vec3{10, 0, 0}, Rotation: mgl64.QuatIdent()}}) + x := ball.Transform.Position.X() + if x > 5 || x < 4.9 { + t.Errorf("%s: the ball is at x=%.3f, want it stopped before the wall at 4.98", c.name, x) + } + if wall.Transform.Position.X() != 5 { + t.Errorf("%s: the wall moved to x=%.3f", c.name, wall.Transform.Position.X()) + } + } +} + +// 2 fast bodies which meet are stopped together, where they meet: a sweep of their relative motion against the start +// pose of the other, as the LinearCast of Jolt. Each stops at the same fraction of its own motion +func TestTwoFastBodiesAreStoppedTogether(t *testing.T) { + const r = 0.05 + w := newScene(1) + w.Gravity = mgl64.Vec3{} + a := addBody(w, mgl64.Vec3{-0.1, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: r}, actor.BodyTypeDynamic, 0.5, 0) + b := addBody(w, mgl64.Vec3{0.1, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: r}, actor.BodyTypeDynamic, 0.5, 0) + w.Step(sceneDt) + // the motions cross: each sphere ends beyond the other + s := &w.solver + for _, body := range []*actor.RigidBody{a, b} { + i := stateOf(w, body) + start := body.Transform + end := actor.Transform{Position: start.Position.Mul(-1).Add(start.Position.Mul(-1).Normalize().Mul(0.1333)), Rotation: mgl64.QuatIdent()} + s.states[i].deltaPosition = end.Position.Sub(start.Position) + s.states[i].deltaRotation = mgl64.QuatIdent() + s.starts[i].rotation = start.Rotation + body.Transform = end + body.UpdateAABB() + } + w.continuous(s, w.Contacts(), sceneDt) + gap := b.Transform.Position.X() - a.Transform.Position.X() - 2*r + if gap < toiTarget-1e-9 || gap > toiTarget+toiTolerance { + t.Errorf("the spheres are %.5f m apart after the continuous collision, want %.5f (a at %.4f, b at %.4f)", gap, toiTarget, a.Transform.Position.X(), b.Transform.Position.X()) + } + if a.Transform.Position.X() != -b.Transform.Position.X() { + t.Errorf("the spheres stopped at %.6f and %.6f, want the same fraction of symmetric motions", a.Transform.Position.X(), b.Transform.Position.X()) + } +} + +// Criterion: 2 spheres of 10 cm thrown face to face at 20 m/s each, at 60 Hz with 8 sub-steps, touch and bounce, and +// never go through each other (today they go through from 7.5 m/s of relative speed) +func TestFastSpheresFaceToFaceBounce(t *testing.T) { + const r, speed = 0.05, 20.0 + for _, workers := range []int{1, 8} { + w := newScene(workers) + w.parallelFrom = 2 + w.Gravity = mgl64.Vec3{} + a := addBody(w, mgl64.Vec3{-2, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: r}, actor.BodyTypeDynamic, 0.5, 0.5) + b := addBody(w, mgl64.Vec3{2, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: r}, actor.BodyTypeDynamic, 0.5, 0.5) + a.Velocity, b.Velocity = mgl64.Vec3{speed, 0, 0}, mgl64.Vec3{-speed, 0, 0} + minGap := math.Inf(1) + simulate(w, 1, func() { + gap := b.Transform.Position.X() - a.Transform.Position.X() - 2*r + minGap = math.Min(minGap, gap) + if a.Transform.Position.X() > b.Transform.Position.X() { + t.Fatalf("workers=%d: the spheres went through each other: a at %.3f, b at %.3f", workers, a.Transform.Position.X(), b.Transform.Position.X()) + } + }) + t.Logf("workers=%d: closest %.2f mm, a at %.2f m/s, b at %.2f m/s", workers, minGap*1000, a.Velocity.X(), b.Velocity.X()) + if minGap > SpeculativeDistance { + t.Errorf("workers=%d: the spheres never came closer than %.1f mm: they didn't touch", workers, minGap*1000) + } + if minGap < -landingDepth { + t.Errorf("workers=%d: the spheres overlapped by %.2f mm", workers, -minGap*1000) + } + // the bounce: each sphere comes back at half its speed (restitution 0.5), within 1 % + if math.Abs(a.Velocity.X()+speed/2) > speed/200 || math.Abs(b.Velocity.X()-speed/2) > speed/200 { + t.Errorf("workers=%d: after the bounce a goes at %.3f m/s and b at %.3f, want ∓%.1f", workers, a.Velocity.X(), b.Velocity.X(), speed/2) + } + } +} + +// Criterion: a plate of 1 cm crossing at 30 m/s a plate of 1 cm resting (dynamic) is stopped or pushes it, never on +// the other side (today it goes through from 2.4 m/s) +func TestFastPlateNeverCrossesARestingPlate(t *testing.T) { + plate := &actor.Box{HalfExtents: mgl64.Vec3{0.005, 0.5, 0.5}} + w := newScene(1) + w.Gravity = mgl64.Vec3{} + moving := addBody(w, mgl64.Vec3{-2, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + resting := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + moving.Velocity = mgl64.Vec3{30, 0, 0} + worst := 0.0 + simulate(w, 1, func() { + worst = math.Max(worst, boxOverlap(moving, resting)) + if moving.Transform.Position.X() > resting.Transform.Position.X() { + t.Fatalf("the moving plate is on the other side: at %.3f, the resting plate at %.3f", moving.Transform.Position.X(), resting.Transform.Position.X()) + } + }) + t.Logf("worst overlap %.2f mm, the plates at %.3f and %.3f m, going at %.2f and %.2f m/s", worst*1000, moving.Transform.Position.X(), resting.Transform.Position.X(), moving.Velocity.X(), resting.Velocity.X()) + if worst > landingDepth { + t.Errorf("the plates overlapped by %.2f mm", worst*1000) + } + if resting.Velocity.X() <= 0 { + t.Errorf("the resting plate was not pushed: %.3f m/s", resting.Velocity.X()) + } +} + +// A body accelerated by the solver during the step is caught by the continuous collision: a heavy ball at 40 m/s hits +// a light plate at rest, which flies at the speed of the ball within the step (its pair with the plate behind it had +// the margin of 2 resting bodies); the plate is stopped on the plate behind it, never beyond +func TestKickedBodyIsStoppedByTheContinuousCollision(t *testing.T) { + plate := &actor.Box{HalfExtents: mgl64.Vec3{0.005, 0.5, 0.5}} + w := newScene(1) + w.Gravity = mgl64.Vec3{} + ball := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{-3, 0, 0}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.2}, actor.BodyTypeDynamic, 8000) + ball.Material.Restitution = 0 + w.AddBody(ball) + first := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, plate, actor.BodyTypeDynamic, 50) + second := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0.3, 0, 0}, Rotation: mgl64.QuatIdent()}, plate, actor.BodyTypeDynamic, 50) + w.AddBody(first) + w.AddBody(second) + ball.Velocity = mgl64.Vec3{40, 0, 0} + kicked, worst := 0.0, 0.0 + simulate(w, 0.5, func() { + kicked = math.Max(kicked, first.Velocity.X()) + worst = math.Max(worst, boxOverlap(first, second)) + if first.Transform.Position.X() > second.Transform.Position.X() { + t.Fatalf("the kicked plate went through the plate behind it: at %.3f, the other at %.3f", first.Transform.Position.X(), second.Transform.Position.X()) + } + }) + t.Logf("the first plate was kicked at %.1f m/s, worst overlap with the second %.2f mm", kicked, worst*1000) + if kicked < 20 { + t.Errorf("the first plate was kicked at %.1f m/s only: the scene doesn't accelerate it enough during a step", kicked) + } + if worst > landingDepth { + t.Errorf("the plates overlapped by %.2f mm", worst*1000) + } +} + +// A dynamic body the fast body has a contact with is left to the solver: the sweep goes on to the next body. The +// contact holds them (the speculative row stops the fast body where they touch); swept too, the fast body would be +// lifted off the overlap the soft contact allows, each step, and its joints torn (a swinging chain of 20 links stretched +// by 214 mm instead of 9) +func TestContinuousLeavesTheContactsToTheSolver(t *testing.T) { + plate := &actor.Box{HalfExtents: mgl64.Vec3{0.01, 1, 1}} + w := newScene(1) + w.Gravity = mgl64.Vec3{} + ball := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.05}, actor.BodyTypeDynamic, 0.5, 0) + near := addBody(w, mgl64.Vec3{0.3, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + far := addBody(w, mgl64.Vec3{3, 0, 0}, mgl64.QuatIdent(), plate, actor.BodyTypeDynamic, 0.5, 0) + ball.Velocity = mgl64.Vec3{20, 0, 0} + w.Step(sceneDt) + // the fast ball has a contact with the near plate (25 cm away, within the margin of its speed), none with the far one + contacts := map[*actor.RigidBody]bool{} + for _, m := range w.Contacts() { + contacts[m.BodyA], contacts[m.BodyB] = true, true + } + if !contacts[near] || contacts[far] { + t.Fatalf("contacts with the near plate %v and the far plate %v, want true and false", contacts[near], contacts[far]) + } + // a motion through both plates, as if the solver had accelerated the ball: stopped by the far plate only + moveThrough(w, ball, sweep{start: actor.Transform{Position: mgl64.Vec3{0.1, 0, 0}, Rotation: mgl64.QuatIdent()}, end: actor.Transform{Position: mgl64.Vec3{5, 0, 0}, Rotation: mgl64.QuatIdent()}}) + x := ball.Transform.Position.X() + if x < 2.9 || x > 3 { + t.Errorf("the ball is at x=%.3f, want it stopped before the far plate at 2.94 (the near plate is left to its contact)", x) + } +} + +// The pairs of a fast body get the speculative margin of its speed: the contact with a resting body exists a step ahead, +// where 2 slow bodies at the same distance have none +func TestFastPairsGetTheSpeculativeMargin(t *testing.T) { + const r = 0.05 + for _, speed := range []float64{1, 20} { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + a := addBody(w, mgl64.Vec3{-0.2, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: r}, actor.BodyTypeDynamic, 0.5, 0) + addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: r}, actor.BodyTypeDynamic, 0.5, 0) + a.Velocity = mgl64.Vec3{speed, 0, 0} + w.Step(sceneDt) + // the gap of 10 cm is beyond SpeculativeDistance: a body at 1 m/s (1.7 cm per step) has no contact, a body at + // 20 m/s (33 cm per step) has one + want := isFast(a, sceneDt) + if got := len(w.Contacts()) > 0; got != want { + t.Errorf("at %.0f m/s: contact %v, want %v (fast %v)", speed, got, want, want) + } + } +} - // a motion through the plate, as if the solver had accelerated the ball - motion := sweep{start: ball.Transform, end: actor.Transform{Position: mgl64.Vec3{10, 0, 0}, Rotation: mgl64.QuatIdent()}} - ball.Transform = motion.end - scratch := ccdPool.Get().(*ccdScratch) - scratch.core.Radius = coreFraction * 0.02 - w.stopAtImpact(ball, &motion, 0.02, scratch) - stopped := ball.Transform.Position.X() < 5 - if stopped != bullet { - t.Errorf("bullet %v: the ball is at x=%.3f", bullet, ball.Transform.Position.X()) +// isFast: a body which can move more than half of its smallest extent during the step, by its translation or its +// rotation; never a kinematic or a sleeping body +func TestIsFast(t *testing.T) { + cube := func() *actor.RigidBody { + return actor.NewRigidBody(actor.NewTransform(), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.5}}, actor.BodyTypeDynamic, 500) + } + slow := cube() + slow.Velocity = mgl64.Vec3{7, 0, 0} // 11.7 cm per step, the half of the smallest extent is 12.5 cm + fast := cube() + fast.Velocity = mgl64.Vec3{8, 0, 0} // 13.3 cm + spinning := cube() + spinning.AngularVelocity = mgl64.Vec3{0, 0, 15} // 0.25 rad per step, the farthest corner at 0.612 m moves 15.3 cm + kinematic := cube() + kinematic.BodyType, kinematic.Velocity = actor.BodyTypeKinematic, mgl64.Vec3{8, 0, 0} + sleeping := cube() + sleeping.IsSleeping, sleeping.Velocity = true, mgl64.Vec3{8, 0, 0} + for _, c := range []struct { + name string + body *actor.RigidBody + want bool + }{{"slow", slow, false}, {"fast", fast, true}, {"spinning", spinning, true}, {"kinematic", kinematic, false}, {"sleeping", sleeping, false}} { + if got := isFast(c.body, sceneDt); got != c.want { + t.Errorf("%s: fast %v, want %v", c.name, got, c.want) + } + } +} + +// fastScene: projectiles thrown at 20 to 40 m/s into a pile of crates, in a closed static box, bouncing: fast bodies +// at every step +func fastScene(workers int) *World { + w := newScene(workers) + w.parallelFrom = 2 + addGround(w, 0.5) + for _, x := range []float64{-4, 4} { + addBody(w, mgl64.Vec3{x, 2, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.1, 2, 4}}, actor.BodyTypeStatic, 0.5, 1) + } + for _, z := range []float64{-4, 4} { + addBody(w, mgl64.Vec3{0, 2, z}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{4, 2, 0.1}}, actor.BodyTypeStatic, 0.5, 1) + } + for i := 0; i < 12; i++ { + addBody(w, mgl64.Vec3{float64(i%3-1) * 0.6, 0.25 + float64(i/3)*0.5, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + } + for i := 0; i < 16; i++ { + angle := float64(i) * 2 * math.Pi / 16 + ball := addBody(w, mgl64.Vec3{3 * math.Cos(angle), 0.5 + 0.2*float64(i%4), 3 * math.Sin(angle)}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.03}, actor.BodyTypeDynamic, 0.5, 1) + speed := 20 + 1.25*float64(i) + ball.Velocity = mgl64.Vec3{-speed * math.Cos(angle), 2, -speed * math.Sin(angle)} + } + return w +} + +// The continuous collision of the fast bodies gives the same bits with 1 and 8 workers, and allocates nothing +func TestContinuousIsDeterministicAndDoesNotAllocate(t *testing.T) { + single, parallel := fastScene(1), fastScene(8) + defer single.Close() + defer parallel.Close() + fast := 0 + for step := 0; step < 240; step++ { + single.Step(sceneDt) + parallel.Step(sceneDt) + for _, body := range single.Bodies { + if isFast(body, sceneDt) { + fast++ + } + } + } + for i := range single.Bodies { + if single.Bodies[i].Transform != parallel.Bodies[i].Transform { + t.Fatalf("body %d: %v with 1 worker, %v with 8", i, single.Bodies[i].Transform, parallel.Bodies[i].Transform) + } + } + if fast < 240*8 { + t.Errorf("%d fast bodies over 240 steps, want at least 8 per step: the scene is not fast enough", fast) + } + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + for _, w := range []*World{single, parallel} { + if allocs := testing.AllocsPerRun(50, func() { w.Step(sceneDt) }); allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", w.Workers, allocs) } } } diff --git a/charactervirtual.go b/charactervirtual.go new file mode 100644 index 0000000..a0e3c65 --- /dev/null +++ b/charactervirtual.go @@ -0,0 +1,764 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== CHARACTER ========== +// A character is a capsule the game moves at a velocity, which walks on the ground and the decor, slides along the +// walls, climbs the slopes under its limit and the steps under its height, stays on the floor downhill, is carried by +// a moving platform, pushes the dynamic bodies with a bounded force and is blocked by the heavy ones and by the other +// characters. It is virtual, as CharacterVirtual of Jolt (Jolt/Physics/Character/CharacterVirtual.cpp, v5.3.0) and +// the mover of Box2D v3 (src/mover.c, samples/sample_character.cpp, v3.1.0): no rigid body moves it, it moves itself +// between 2 steps with the queries of the world (the contacts of its capsule and the sweeps), so that it goes exactly +// where the game decides. PxController of PhysX (physxcharacterkinematic/src/CctCharacterController.cpp, 5.11) and +// CharacterBody3D of Godot (scene/3d/physics/character_body_3d.cpp, 4.4) are of the same kind. +// +// An inner kinematic body stands in the world at the capsule of the character (World.AddCharacter), teleported there +// after each Update without velocity (the inner body of CharacterVirtual, UpdateInnerBodyTransform; the kinematic +// actor of PxController): the rays and the sweeps see it, a dynamic body which runs into it is stopped by the solver +// (it never goes through a character), and the other characters find it in their contacts and their sweeps, so that +// 2 characters block each other or slide along each other (the CharacterVsCharacterCollision list of Jolt is not +// needed). It pushes nothing by itself: the character pushes the dynamic bodies with impulses it bounds. +// +// The game gives the velocity (CharacterVirtual.Velocity) and calls Update after the step of the world. The gravity is the +// game's, as in all four references: the recipe of the samples of Jolt (CharacterVirtualTest::HandleInput) is, on the +// ground, velocity = the velocity of the ground + the input, else the vertical velocity kept + the horizontal input, +// plus gravity * dt in both cases. See PHYSICS_GUIDE.md. +// +// Update follows CharacterVirtual::ExtendedUpdate: the velocity towards the slopes too steep is cancelled, the +// capsule is moved (character_move.go: the contacts around it become planes, a solver by time of impact slides the +// velocity along them, a sweep checks the path), the ground is found among the contacts, the character sticks to the +// floor when it leaves it downhill, and walks the stairs when a step blocked it. Up is +Y: Feather is Y-up. + +const ( + // CharacterPadding: the character keeps this distance from everything (m): its capsule is inflated by it for its + // contacts and its sweeps, so that the sweeps rarely hit and the character never gets stuck in the rounding of a + // contact (mCharacterPadding of Jolt, 0.02; contactOffset of PhysX plays the same role, 0.1) + CharacterPadding = 0.02 + + // DefaultCharacterMaxSlopeAngle: the slope a character climbs, in rad (50° in Jolt, CharacterBaseSettings; 45° in + // PhysX, slopeLimit 0.707, and in Godot, floor_max_angle) + DefaultCharacterMaxSlopeAngle = 50 * math.Pi / 180 + + // DefaultCharacterStepHeight: the step a character climbs without jumping, in m (mWalkStairsStepUp of Jolt, 0.4; + // stepOffset of PhysX, 0.5) + DefaultCharacterStepHeight = 0.4 + + // DefaultCharacterMaxStrength: the force a character pushes the dynamic bodies with, in N (mMaxStrength of Jolt) + DefaultCharacterMaxStrength = 100.0 + + // DefaultCharacterMass: the mass of a character, in kg, its weight on the dynamic body it stands on (mMass of Jolt) + DefaultCharacterMass = 70.0 + + // characterPredictiveDistance: the contacts are gathered this far around the character (m), so that the planes it + // slides along are known before it touches them (mPredictiveContactDistance of Jolt, 0.1: "A value of 0 will most + // likely cause the character to get stuck as it cannot properly calculate a sliding direction") + characterPredictiveDistance = 0.1 + + // characterCollisionTolerance: how deep the character accepts to enter a body (m): a contact closer than this is + // a collision, a sweep which would enter by less is ignored (mCollisionTolerance of Jolt) + characterCollisionTolerance = 1e-3 + + // characterAcceptedPenetration: a plane the velocity would enter by less than this (m) during the time left is not + // solved (the "too little penetration" of SolveConstraints of Jolt) + characterAcceptedPenetration = 1e-4 + + // characterMaxCollisionIterations: the loops of contacts, solve and sweep of a move (mMaxCollisionIterations) + characterMaxCollisionIterations = 5 + + // characterMaxConstraintIterations: the planes solved at most during a move (mMaxConstraintIterations of Jolt) + characterMaxConstraintIterations = 15 + + // characterMinTimeRemaining: a move stops when less than this remains to simulate (s, mMinTimeRemaining of Jolt) + characterMinTimeRemaining = 1e-4 + + // characterPenetrationRecovery: the part of a penetration resolved in one update (mPenetrationRecoverySpeed of + // Jolt, 1: everything) + characterPenetrationRecovery = 1.0 + + // DefaultCharacterStickToFloor: how far down the floor is looked for when the character leaves the ground without + // going up, in m (mStickToFloorStepDown of the ExtendedUpdateSettings of Jolt, 0.5; floor_snap_length of Godot is + // 0.1) + DefaultCharacterStickToFloor = 0.5 + + // characterMinStepForward: the least horizontal step of a stair walk, whatever the frame rate (m, + // mWalkStairsMinStepForward of Jolt) + characterMinStepForward = 0.02 + + // characterStepForwardTest: how far ahead the floor of a step is tested when the first test lands on its edge (m, + // mWalkStairsStepForwardTest of Jolt) + characterStepForwardTest = 0.15 + + // characterSteepContactPenetration: the conflicting contacts of a body (opposed normals, both this deep) are + // reduced to the deepest one, so that a character squeezed in a body can get out (1.25 * padding, Jolt + // RemoveConflictingContacts) + characterConflictDepth = 1.25 * CharacterPadding + + // characterPushDamping and characterPushPenetration: the impulse on a dynamic body brings it to the velocity of + // the character damped by 0.9, plus 0.4 of the penetration per update (cDamping & cPenetrationResolution of + // CharacterVirtual::HandleContact) + characterPushDamping = 0.9 + characterPushPenetration = 0.4 + + // characterGroundNormalCos: a contact tilted by less than 85° from up counts in the normal and the velocity of the + // ground (0.08 in CharacterVirtual::UpdateSupportingContact) + characterGroundNormalCos = 0.08 + + // characterParallelCos: 2 planes closer than about 10° are not slid along their edge (0.984 in + // CharacterVirtual::SolveConstraints) + characterParallelCos = 0.984 + + // characterVerticalEpsilon: a steep contact whose normal has a horizontal part shorter than this (the rounding of + // a vertical normal under a slope limit of 0) gets no vertical plane + characterVerticalEpsilon = 1e-6 + + // characterProgressFraction: a stair walk which moved the character against the steep contact by less than this + // part of the step forward made no progress and is cancelled (5 % in CharacterVirtual::WalkStairs) + characterProgressFraction = 0.05 +) + +// characterCosAngleForwardContact: the ground normal is used for the forward test of a stair walk when it is within +// 75° of the walk, else the walk itself (mWalkStairsCosAngleForwardContact of Jolt) +var characterCosAngleForwardContact = math.Cos(75 * math.Pi / 180) + +// characterUp: the up direction of every character: Feather is Y-up +var characterUp = mgl64.Vec3{0, 1, 0} + +// CharacterGroundState: what a character stands on (EGroundState of Jolt) +type CharacterGroundState uint8 + +const ( + // CharacterInAir: the character touches nothing + CharacterInAir CharacterGroundState = iota + // CharacterNotSupported: the character touches a body, but it doesn't hold it (a wall, a ceiling): it falls + CharacterNotSupported + // CharacterOnSteepGround: the character stands on a slope too steep: it can't climb it, it slides down under its + // gravity + CharacterOnSteepGround + // CharacterOnGround: the character stands on the ground and can walk + CharacterOnGround +) + +func (s CharacterGroundState) String() string { + switch s { + case CharacterInAir: + return "in the air" + case CharacterNotSupported: + return "not supported" + case CharacterOnSteepGround: + return "on steep ground" + } + return "on the ground" +} + +// CharacterVirtual: a capsule which walks. Create it with NewCharacterVirtual, add it to a world with World.AddCharacter, +// give it its Velocity and call Update after each step. Position is the center of the capsule. The name is the one of +// Jolt, which keeps Character for its character on a rigid body, moved by the solver; Feather has the virtual one +// only (PxController of PhysX, the mover of Box2D and CharacterBody3D of Godot are virtual too) +type CharacterVirtual struct { + // Id is for the game (an entity id); the inner body keeps its own + Id any + + // Capsule of the character, standing (its axis is up). Its radius and its half height can change between 2 + // updates (a crouch) + Capsule actor.Capsule + + // Position of the center of the capsule, moved by Update. Write it through Teleport + Position mgl64.Vec3 + + // Velocity the character wants, written by the game before Update (its input, the gravity of the step and the + // velocity of the ground: see the recipe in PHYSICS_GUIDE.md). Update cancels the part of it towards the slopes + // too steep, and leaves the rest: the vertical velocity of a fall accumulates in it + Velocity mgl64.Vec3 + + // MaxSlopeAngle: the slope the character climbs (rad). Steeper, a surface is a wall: it is not climbed, the + // character slides down it + MaxSlopeAngle float64 + + // StepHeight: the step the character climbs without jumping (m), 0 to climb no step + StepHeight float64 + + // MaxStrength: the force the character pushes the dynamic bodies with (N): the impulse of an update is at most + // MaxStrength * dt. 0 pushes nothing + MaxStrength float64 + + // Mass of the character (kg): its weight on the dynamic body it stands on. 0 weighs nothing + Mass float64 + + // StickToFloor: how far down the floor is looked for when the character leaves the ground without going up (m): + // it is moved down onto it, instead of flying off a step or a slope it walks down. 0 sticks to nothing + StickToFloor float64 + + world *World + // body: the inner kinematic body, at the capsule of the character; index: its index in World.Bodies, checked + // before use (RemoveBody moves the bodies) + body *actor.RigidBody + index int + + // padded: the capsule inflated by the padding, the shape of the contacts and the sweeps; mover: a body for the + // contact generator, at the padded capsule + padded actor.Capsule + mover actor.RigidBody + // cosMaxSlope: cos(MaxSlopeAngle), computed at each update + cosMaxSlope float64 + // allowSliding: the character may slide along a walkable surface during this update: it has a horizontal + // velocity, or it is not supported. Without it, it stops dead on the walkable surfaces it hits (the character + // of the samples of Jolt, CharacterVirtualTest::OnContactSolve: no creeping down a slope while standing; the + // floor_stop_on_slope of Godot). The velocity of the game then holds the gravity of the step alone + allowSliding bool + + // the ground, after the last update, and the dt of the last update (the velocity of a turning platform is the one + // of its arc over dt) + ground characterGround + lastDt float64 + + // contacts: the active contacts of the last move (the contacts of the capsule, those which blocked it marked); + // gathered: the contacts of the current position during a move + contacts, gathered []characterContact + // the buffers of a move (character_move.go) + constraints []characterConstraint + sorted []int32 + previous []int32 + excluded []*actor.RigidBody + steep []mgl64.Vec3 + candidates []int32 + stack []int32 + manifolds [MaxManifoldsPerPair]constraint.Manifold + // the buffers of its contacts and of its sweeps, its own and not those of the pools of the world: a pool emptied by + // the garbage collector gives new buffers, which grow again, and an update would allocate (made by AddCharacter) + contactBuffers *contactScratch + queryBuffers *queryScratch +} + +// characterGround: what the character stands on (the ground properties of CharacterVirtual) +type characterGround struct { + state CharacterGroundState + normal mgl64.Vec3 + velocity mgl64.Vec3 + body *actor.RigidBody + position mgl64.Vec3 +} + +// NewCharacterVirtual: a character of the capsule, standing at the position (the center of the capsule), with the +// defaults of Jolt: a slope of 50°, a step of 0.4 m, a strength of 100 N, a mass of 70 kg, the floor kept within 0.5 m. +// Add it to a world with World.AddCharacter +func NewCharacterVirtual(capsule actor.Capsule, position mgl64.Vec3) *CharacterVirtual { + return &CharacterVirtual{ + Capsule: capsule, + Position: position, + MaxSlopeAngle: DefaultCharacterMaxSlopeAngle, + StepHeight: DefaultCharacterStepHeight, + MaxStrength: DefaultCharacterMaxStrength, + Mass: DefaultCharacterMass, + StickToFloor: DefaultCharacterStickToFloor, + } +} + +// AddCharacter adds the character to the world: its inner kinematic body is created at its capsule (Body), on the +// default layer with every layer in its mask (World.SetFilter changes them: the character collides with the bodies +// the pair rules let it collide with, as #800). A character of a zero capsule panics. The character is seen by the +// queries at the next Step or SyncQueries, as any body added +func (w *World) AddCharacter(c *CharacterVirtual) { + if c.world != nil { + panic("feather: the character is already in a world") + } + if !(c.Capsule.Radius > 0) { + panic("feather: a character needs a capsule of a positive radius") + } + density := 0.0 + if volume := c.Capsule.ComputeMass(1); volume > 0 { + density = c.Mass / volume + } + c.body = actor.NewRigidBody(actor.Transform{Position: c.Position, Rotation: mgl64.QuatIdent()}, &c.Capsule, actor.BodyTypeKinematic, density) + c.world = w + c.index = len(w.Bodies) + c.mover = actor.RigidBody{Transform: actor.NewTransform(), BodyType: actor.BodyTypeStatic, Shape: &c.padded} + c.ground = characterGround{} + c.contacts = c.contacts[:0] + if c.contactBuffers == nil { + c.contactBuffers, c.queryBuffers = newContactScratch(), newQueryScratch() + c.queryBuffers.epa = &c.contactBuffers.epa + } + w.AddBody(c.body) + if w.characters == nil { + w.characters = make(map[*actor.RigidBody]*CharacterVirtual) + } + w.characters[c.body] = c +} + +// RemoveCharacter removes the character and its inner body from the world +func (w *World) RemoveCharacter(c *CharacterVirtual) { + if c.world != w { + return + } + w.RemoveBody(c.body) + c.world, c.body = nil, nil +} + +// CharacterOf: the character whose inner body this is, nil for any other body (the body hit by a ray) +func (w *World) CharacterOf(body *actor.RigidBody) *CharacterVirtual { + return w.characters[body] +} + +// Body: the inner kinematic body of the character, nil before AddCharacter. Its layer and its mask are the ones of +// the character (World.SetFilter); don't move it, the character does +func (c *CharacterVirtual) Body() *actor.RigidBody { + return c.body +} + +// GroundState after the last update +func (c *CharacterVirtual) GroundState() CharacterGroundState { + return c.ground.state +} + +// IsSupported: the character stands on something, the ground or a slope too steep (IsSupported of Jolt) +func (c *CharacterVirtual) IsSupported() bool { + return c.ground.state == CharacterOnGround || c.ground.state == CharacterOnSteepGround +} + +// GroundNormal: the normal of the ground, the average of the contacts which hold the character, zero in the air +func (c *CharacterVirtual) GroundNormal() mgl64.Vec3 { + return c.ground.normal +} + +// GroundVelocity: the velocity of the ground under the character (a platform), zero on a static ground or in the air. +// The game adds it to the velocity of the character to be carried +func (c *CharacterVirtual) GroundVelocity() mgl64.Vec3 { + return c.ground.velocity +} + +// UpdateGroundVelocity reads again the velocity of the ground from its body, without any collision test: the platform +// the character stands on may have changed its velocity since the last update (UpdateGroundVelocity of Jolt, called +// by its samples before they compute the velocity of the character: the character then follows a platform which +// starts without a frame of lag) +func (c *CharacterVirtual) UpdateGroundVelocity() { + ground := c.ground.body + if ground == nil || ground.BodyType == actor.BodyTypeStatic { + return + } + if other := c.world.characters[ground]; other != nil { + c.ground.velocity = other.Velocity + return + } + c.ground.velocity = c.groundVelocityOn(ground, c.lastDt) +} + +// GroundBody: the body the character stands on (or touches, when not supported), nil in the air +func (c *CharacterVirtual) GroundBody() *actor.RigidBody { + return c.ground.body +} + +// GroundPosition: the point of the ground the character stands on +func (c *CharacterVirtual) GroundPosition() mgl64.Vec3 { + return c.ground.position +} + +// IsSlopeTooSteep: a surface of this normal is steeper than MaxSlopeAngle: a wall for the character +func (c *CharacterVirtual) IsSlopeTooSteep(normal mgl64.Vec3) bool { + return normal.Dot(characterUp) < math.Cos(c.MaxSlopeAngle) +} + +// mustBeInWorld: the character was added to a world +func (c *CharacterVirtual) mustBeInWorld() { + if c.world == nil { + panic("feather: the character is in no world: World.AddCharacter first") + } +} + +// prepare the capsules and the cosine of the update +func (c *CharacterVirtual) prepare() { + c.cosMaxSlope = math.Cos(c.MaxSlopeAngle) + c.padded = actor.Capsule{HalfHeight: c.Capsule.HalfHeight, Radius: c.Capsule.Radius + CharacterPadding} +} + +// Update moves the character for dt at its Velocity, between 2 steps of the world (during a step it panics, as a +// query). Call it after World.Step: the velocity of a platform is the one of the step just done, and the character +// follows it without lag. +// +// As CharacterVirtual::ExtendedUpdate of Jolt: the part of the velocity towards the slopes too steep is cancelled; +// the capsule slides along the contacts and sweeps; the ground is read from the contacts, and the weight of the +// character is put on a dynamic ground; if it left the ground without going up it sticks to the floor within +// StickToFloor; if the horizontal step it wanted was blocked by a steep contact, it walks a stair of StepHeight at +// most. Then the inner body is placed at the capsule, without velocity, awake when it moved so that +// the bodies which rest on it follow +func (c *CharacterVirtual) Update(dt float64) { + c.mustBeInWorld() + c.world.guard() + if dt <= 0 { + return + } + c.prepare() + c.lastDt = dt + + desired := c.Velocity + c.allowSliding = horizontal(desired).LenSqr() > 0 || !c.IsSupported() + velocity := c.cancelVelocityTowardsSteepSlopes(desired) + c.Velocity = velocity + old := c.Position + groundToAir := c.IsSupported() + + // the move, and the ground after it + c.moveShape(&c.Position, velocity, dt, true) + c.updateSupportingContact(false, dt) + c.weighOnTheGround(dt) + if c.IsSupported() { + groundToAir = false + } + + // stick to the floor when it left the ground without going up (ExtendedUpdate of Jolt) + if groundToAir && c.StickToFloor > 0 { + if vertical := c.Position.Sub(old).Dot(characterUp) / dt; vertical <= 1e-6 { + c.stickToFloor(characterUp.Mul(-c.StickToFloor), dt) + } + } + + // walk a stair when the step wanted was blocked + if c.StepHeight > 0 { + wanted := horizontal(desired.Mul(dt)) + if wantedLength := wanted.Len(); wantedLength > 0 { + forward := wanted.Mul(1 / wantedLength) + // only the motion towards the step counts: sliding downhill while climbing is not progress + achieved := math.Max(0, horizontal(c.Position.Sub(old)).Dot(forward)) + if achieved+characterMinTimeRemaining < wantedLength && c.canWalkStairs(desired) { + stepForward := forward.Mul(math.Max(characterMinStepForward, wantedLength-achieved)) + // the floor ahead is tested along the ground normal in the horizontal plane, or along the walk when + // they differ by more than 75° + test := horizontal(c.ground.normal.Mul(-1)) + if length := test.Len(); length > 0 && test.Dot(forward)/length >= characterCosAngleForwardContact { + test = test.Mul(characterStepForwardTest / length) + } else { + test = forward.Mul(characterStepForwardTest) + } + c.walkStairs(dt, characterUp.Mul(c.StepHeight), stepForward, test) + } + } + } + + c.placeBody(c.Position != old) +} + +// Teleport puts the character at the position, with its inner body, and reads its contacts there: nothing is pushed, +// the character placed in a body is pushed out at the next updates (as SetPosition then RefreshContacts of Jolt) +func (c *CharacterVirtual) Teleport(position mgl64.Vec3) { + c.mustBeInWorld() + c.world.guard() + c.prepare() + c.Position = position + c.placeBody(true) + c.refreshContacts() +} + +// refreshContacts: the contacts at the position, and the ground among them (RefreshContacts of Jolt) +func (c *CharacterVirtual) refreshContacts() { + direction := c.Velocity + if length := direction.Len(); length > 0 { + direction = direction.Mul(1 / length) + } + c.collectContacts(c.Position, direction) + c.contacts = append(c.contacts[:0], c.gathered...) + c.updateSupportingContact(true, 0) +} + +// placeBody: the inner body takes the capsule of the character, without velocity (the inner body of Jolt is placed +// with SetPositionAndRotation, DontActivate). Its leaf in the tree of the broad phase follows at once, for the sweeps +// of the other characters. When it moved it wakes up with its island: the bodies resting on it must follow it +func (c *CharacterVirtual) placeBody(moved bool) { + w, body := c.world, c.body + body.Transform.Position = c.Position + body.Velocity, body.AngularVelocity = mgl64.Vec3{}, mgl64.Vec3{} + body.ClearKinematicTarget() + body.UpdateAABB() + if c.index >= len(w.Bodies) || w.Bodies[c.index] != body { + c.index = -1 + for i, other := range w.Bodies { + if other == body { + c.index = i + break + } + } + } + if c.index >= 0 && c.index < len(w.tree.proxies) && w.tree.bodies[c.index] == body { + aabb := body.AABB() + margin := mgl64.Vec3{SpeculativeDistance, SpeculativeDistance, SpeculativeDistance} + w.tree.update(int32(c.index), body, actor.AABB{Min: aabb.Min.Sub(margin), Max: aabb.Max.Add(margin)}) + } + if moved { + w.islands.wake(body) + } +} + +// weighOnTheGround: the weight of the character during dt goes to the dynamic body it stands on, as an impulse at the +// ground point (CharacterVirtual::Update of Jolt) +func (c *CharacterVirtual) weighOnTheGround(dt float64) { + ground := c.ground.body + if ground == nil || ground.BodyType != actor.BodyTypeDynamic || !(c.Mass > 0) { + return + } + gravity := c.world.Gravity + length := gravity.Len() + if length == 0 { + return + } + along := c.ground.normal.Dot(gravity) + if along >= 0 { + return + } + ground.AddImpulseAtPoint(gravity.Mul(-c.Mass*along/length*dt), c.ground.position) +} + +// horizontal: the vector without its vertical component +func horizontal(v mgl64.Vec3) mgl64.Vec3 { + return v.Sub(characterUp.Mul(v.Dot(characterUp))) +} + +// cancelVelocityTowardsSteepSlopes: on a slope too steep, the horizontal velocity towards the steep contacts is +// removed, so that the character doesn't try to climb (CancelVelocityTowardsSteepSlopes of Jolt) +func (c *CharacterVirtual) cancelVelocityTowardsSteepSlopes(desired mgl64.Vec3) mgl64.Vec3 { + if c.ground.state == CharacterOnGround || c.ground.state == CharacterInAir { + return desired + } + velocity := desired + for i := range c.contacts { + contact := &c.contacts[i] + if !contact.hadCollision || contact.wasDiscarded || !c.tooSteep(contact.normal) { + continue + } + normal := horizontal(contact.normal) + if dot := normal.Dot(velocity); dot < 0 { + velocity = velocity.Sub(normal.Mul(dot / normal.LenSqr())) + } + } + return velocity +} + +// tooSteep: the normal is the one of a surface the character can't climb, with the cosine of the update +func (c *CharacterVirtual) tooSteep(normal mgl64.Vec3) bool { + return normal.Dot(characterUp) < c.cosMaxSlope +} + +// updateSupportingContact: the ground among the active contacts (UpdateSupportingContact of Jolt). A contact close +// enough and not left (or any contact, with skipVelocityCheck) is a collision; a contact holds the character if its +// point is in the lower sphere of the capsule (the supporting volume of the samples of Jolt: Plane(Y, -radius)); the +// character is on the ground if a holding contact is under the slope limit, on steep ground if it only has steep +// ones, unless they block it together (a crevice), not supported if it touches without being held, else in the air. +// The ground normal and velocity are the average of the contacts within 85° of up +func (c *CharacterVirtual) updateSupportingContact(skipVelocityCheck bool, dt float64) { + for i := range c.contacts { + contact := &c.contacts[i] + if !contact.wasDiscarded && !contact.hadCollision && contact.distance < characterCollisionTolerance && + (skipVelocityCheck || contact.normal.Dot(c.Velocity.Sub(contact.velocity)) <= 1e-4) { + contact.hadCollision = true + } + } + + supported, sliding, averaged := 0, 0, 0 + var normal, velocity mgl64.Vec3 + supporting, deepest := -1, -1 + maxCos, smallest := math.Inf(-1), math.Inf(1) + for i := range c.contacts { + contact := &c.contacts[i] + if !contact.hadCollision || contact.wasDiscarded { + continue + } + cos := contact.normal.Dot(characterUp) + if contact.distance < smallest { + deepest, smallest = i, contact.distance + } + // a contact above the center of the lower sphere can't hold the character + if contact.position.Sub(c.Position).Dot(characterUp) > -c.Capsule.HalfHeight { + continue + } + if cos > maxCos { + supporting, maxCos = i, cos + } + holds := cos >= c.cosMaxSlope + if holds { + supported++ + } else { + sliding++ + } + if cos >= characterGroundNormalCos { + normal = normal.Add(contact.normal) + averaged++ + if contact.body != nil && contact.body.BodyType == actor.BodyTypeKinematic && holds && contact.character == nil && dt > 0 { + // a kinematic platform: the velocity of the point of the platform under the character, on its arc + velocity = velocity.Add(c.groundVelocityOn(contact.body, dt)) + } else { + velocity = velocity.Add(contact.velocity) + } + } + } + + best := supporting + if best < 0 { + best = deepest + } + switch { + case averaged >= 1: + c.ground.normal, c.ground.velocity = normal.Normalize(), velocity.Mul(1/float64(averaged)) + case best >= 0: + c.ground.normal, c.ground.velocity = c.contacts[best].normal, c.contacts[best].velocity + default: + c.ground.normal, c.ground.velocity = mgl64.Vec3{}, mgl64.Vec3{} + } + if best >= 0 { + c.ground.body, c.ground.position = c.contacts[best].body, c.contacts[best].position + } else { + c.ground.body, c.ground.position = nil, mgl64.Vec3{} + } + + switch { + case supported > 0: + c.ground.state = CharacterOnGround + case sliding > 0: + if c.Velocity.Sub(c.contacts[deepest].velocity).Dot(characterUp) > 1e-4 { + // going up relative to the ground: not on it + c.ground.state = CharacterOnSteepGround + } else { + // sliding down between several steep contacts which block each other: held + c.gathered = append(c.gathered[:0], c.contacts...) + c.determineConstraints(1) + displacement, simulated := c.solveConstraints(characterUp.Mul(-1), 1, 1) + if simulated < 0.001 || displacement.LenSqr() < (0.6*dt)*(0.6*dt) { + c.ground.state = CharacterOnGround + } else { + c.ground.state = CharacterOnSteepGround + } + } + case best >= 0: + c.ground.state = CharacterNotSupported + default: + c.ground.state = CharacterInAir + } +} + +// groundVelocityOn: the velocity of the point of the body under the character, over dt: where the rotation of the +// body during dt brings this point, so that the character follows a turning platform without drifting +// (CalculateCharacterGroundVelocity of Jolt) +func (c *CharacterVirtual) groundVelocityOn(body *actor.RigidBody, dt float64) mgl64.Vec3 { + angular := body.AngularVelocity + rate := angular.Len() + if rate*rate < 1e-12 || dt <= 0 { + return body.Velocity + } + rotation := mgl64.QuatRotate(rate*dt, angular.Mul(1/rate)) + center := body.Transform.Position + moved := center.Add(rotation.Rotate(c.Position.Sub(center))) + return body.Velocity.Add(moved.Sub(c.Position).Mul(1 / dt)) +} + +// stickToFloor: the floor is looked for under the character, and the character moved onto it (StickToFloor of Jolt) +func (c *CharacterVirtual) stickToFloor(down mgl64.Vec3, dt float64) bool { + c.excluded = append(c.excluded[:0], c.body) + hit, ok := c.sweep(c.Position, down) + if !ok { + return false + } + c.moveToContact(c.Position.Add(down.Mul(hit.Fraction))) + return true +} + +// moveToContact: the character is placed, and its contacts and its ground read there (MoveToContact of Jolt) +func (c *CharacterVirtual) moveToContact(position mgl64.Vec3) { + c.Position = position + c.refreshContacts() +} + +// canWalkStairs: the character is supported, moves horizontally and pushes into a contact too steep (CanWalkStairs) +func (c *CharacterVirtual) canWalkStairs(velocity mgl64.Vec3) bool { + if !c.IsSupported() { + return false + } + flat := horizontal(velocity) + if flat.LenSqr() < 1e-12 { + return false + } + for i := range c.contacts { + contact := &c.contacts[i] + if contact.hadCollision && !contact.wasDiscarded && contact.normal.Dot(flat.Sub(contact.velocity)) < 0 && c.tooSteep(contact.normal) { + return true + } + } + return false +} + +// walkStairs: the character goes up by stepUp at most, forward by stepForward, and down onto the floor; the walk is +// cancelled if nothing was gained against the steep contacts, if there is no floor, or if the floor is too steep +// where it lands and at stepForwardTest further (WalkStairs of Jolt). Returns true if the character walked the stair +func (c *CharacterVirtual) walkStairs(dt float64, stepUp, stepForward, stepForwardTest mgl64.Vec3) bool { + c.excluded = append(c.excluded[:0], c.body) + up := stepUp + if hit, ok := c.sweep(c.Position, up); ok { + if hit.Fraction < 1e-6 { + return false + } + up = up.Mul(hit.Fraction) + } + upPosition := c.Position.Add(up) + + // the steep contacts the character pushes into, before the move changes the contacts + velocity := stepForward.Mul(1 / dt) + flat := horizontal(velocity) + c.steep = c.steep[:0] + for i := range c.contacts { + contact := &c.contacts[i] + if contact.hadCollision && !contact.wasDiscarded && contact.normal.Dot(flat.Sub(contact.velocity)) < 0 && c.tooSteep(contact.normal) { + c.steep = append(c.steep, contact.normal) + } + } + if len(c.steep) == 0 { + return false + } + + // forward, from the top of the step + newPosition := upPosition + c.moveShape(&newPosition, velocity, dt, false) + movement := newPosition.Sub(upPosition) + movementSqr := movement.LenSqr() + if movementSqr < 1e-8 { + return false + } + progress := false + for _, normal := range c.steep { + if normal.Dot(movement) < -characterProgressFraction*stepForward.Len() { + progress = true + break + } + } + if !progress { + return false + } + + // down onto the floor, as much as the character went up + down := up.Mul(-1) + c.excluded = append(c.excluded[:0], c.body) + hit, ok := c.sweep(newPosition, down) + if !ok { + return false + } + if c.tooSteep(hit.Normal) { + // the edge of the step may be hit with a normal too horizontal: the floor is tested further along the step + if stepForwardTest.LenSqr() < 1e-12 { + return false + } + testPosition := upPosition + c.moveShape(&testPosition, stepForwardTest.Mul(1/dt), dt, false) + if testPosition.Sub(upPosition).LenSqr() <= movementSqr+1e-8 { + return false + } + c.excluded = append(c.excluded[:0], c.body) + testHit, ok := c.sweep(testPosition, down) + if !ok || c.tooSteep(testHit.Normal) { + return false + } + } + c.moveToContact(newPosition.Add(down.Mul(hit.Fraction))) + // the floor further along is not too steep: the character is on the ground + c.ground.state = CharacterOnGround + return true +} diff --git a/charactervirtual_move.go b/charactervirtual_move.go new file mode 100644 index 0000000..2e887bc --- /dev/null +++ b/charactervirtual_move.go @@ -0,0 +1,423 @@ +package feather + +import ( + "math" + "slices" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== THE MOVE OF A CHARACTER ========== +// The move of a character is the one of CharacterVirtual::MoveShape of Jolt: the contacts of the padded capsule with +// everything within the predictive distance (collectContacts: the contact generator of the world, GJK/EPA and the +// patches of the surfaces, one plane per manifold), the planes made constraints with the velocity of their body +// (determineConstraints, a vertical plane added for a slope too steep), the velocity slid along the planes by time +// of impact (solveConstraints), a sweep to check that nothing was missed (sweep), and again while time remains, 5 +// times at most. A dynamic body met receives an impulse (handleContact), bounded by the strength of the character. + +// characterContact: a contact of the capsule of the character with a body (the Contact of CharacterVirtual) +type characterContact struct { + // body: the body touched; character: the other character when the body is its inner body + body *actor.RigidBody + character *CharacterVirtual + // position on the body; normal from the body towards the character; distance between them, the padding removed + // (negative when the padded capsule enters the body) + position mgl64.Vec3 + normal mgl64.Vec3 + distance float64 + // velocity of the body at the point (the velocity of the other character) + velocity mgl64.Vec3 + // hadCollision: the contact blocked the character; wasDiscarded: a conflicting contact, ignored by the sweeps; + // canPush: the body can push the character (always: the setting of Jolt is left to its listener) + hadCollision, wasDiscarded bool +} + +// characterConstraint: a plane the character can't cross, with its velocity (the Constraint of CharacterVirtual). +// The plane is normal · x + distance = 0 at the position of the character +type characterConstraint struct { + contact int32 + velocity mgl64.Vec3 + normal mgl64.Vec3 + distance float64 + // steep: the plane is a slope too steep, a vertical constraint follows it + steep bool + // toi: the time of impact of the velocity on the plane, projected: the speed of approach (solveConstraints) + toi, projected float64 +} + +// collectContacts: the contacts of the padded capsule at the position with the bodies within the predictive +// distance (GetContactsAtPosition of Jolt): the candidates of the trees for its AABB, filtered as the pairs of the +// world (World.ShouldCollide: the layers, the ignored pairs), the triggers skipped, then the contact generator of the +// world with the predictive distance as margin, one contact per manifold. The contacts are sorted by the index of +// their body: the same contacts whatever the trees +func (c *CharacterVirtual) collectContacts(position, direction mgl64.Vec3) { + w := c.world + c.gathered = c.gathered[:0] + c.mover.Transform.Position = position + c.mover.UpdateAABB() + bounds := c.mover.AABB() + margin := mgl64.Vec3{characterPredictiveDistance, characterPredictiveDistance, characterPredictiveDistance} + bounds = actor.AABB{Min: bounds.Min.Sub(margin), Max: bounds.Max.Add(margin)} + c.stack, c.candidates = w.tree.queryCandidates(bounds, c.stack, c.candidates[:0]) + slices.Sort(c.candidates) + for _, index := range c.candidates { + body := w.Bodies[index] + if body == c.body || body.IsTrigger || !w.ShouldCollide(c.body, body) || !bounds.Overlaps(body.AABB()) { + continue + } + count := collideAllMoving(c.contactBuffers, &c.mover, body, characterPredictiveDistance, direction, c.manifolds[:]) + for k := 0; k < count; k++ { + c.addContact(body, &c.manifolds[k]) + } + } +} + +// addContact: the deepest point of the manifold, its normal towards the character, the velocity of the body there +func (c *CharacterVirtual) addContact(body *actor.RigidBody, m *constraint.Manifold) { + deepest := 0 + for j := 1; j < m.Count; j++ { + if m.Points[j].Separation < m.Points[deepest].Separation { + deepest = j + } + } + point := &m.Points[deepest] + // the manifold goes from the mover to the body: the point on the body is half the separation further + position := point.Position.Add(m.Normal.Mul(point.Separation / 2)) + contact := characterContact{body: body, position: position, normal: m.Normal.Mul(-1), distance: point.Separation} + if other := c.world.characters[body]; other != nil { + // the other character moves by itself: its velocity is the one it wants (Jolt sFillCharacterContactProperties) + contact.character, contact.velocity = other, other.Velocity + } else if body.BodyType != actor.BodyTypeStatic { + contact.velocity = body.Velocity.Add(body.AngularVelocity.Cross(position.Sub(body.Transform.Position))) + } + c.gathered = append(c.gathered, contact) +} + +// removeConflictingContacts: 2 contacts of the same body which both enter it deeper than the conflict depth with +// opposed normals can't both be solved (the character is squeezed in the body): the shallower one is discarded, +// and ignored by the sweeps (RemoveConflictingContacts of Jolt) +func (c *CharacterVirtual) removeConflictingContacts() { + c.excluded = append(c.excluded[:0], c.body) + for i := range c.gathered { + first := &c.gathered[i] + if first.wasDiscarded || first.distance > -characterConflictDepth { + continue + } + for j := i + 1; j < len(c.gathered); j++ { + second := &c.gathered[j] + if second.wasDiscarded || second.body != first.body || second.distance > -characterConflictDepth || first.normal.Dot(second.normal) >= 0 { + continue + } + if first.distance < second.distance { + second.wasDiscarded = true + c.excluded = append(c.excluded, second.body) + } else { + first.wasDiscarded = true + c.excluded = append(c.excluded, first.body) + break + } + } + } +} + +// determineConstraints: the planes of the gathered contacts (DetermineConstraints of Jolt). A contact which enters +// the body gets a velocity out of it (the penetration recovery); a slope too steep gets a second, vertical plane +// which blocks the way up it +func (c *CharacterVirtual) determineConstraints(dt float64) { + c.constraints = c.constraints[:0] + for i := range c.gathered { + contact := &c.gathered[i] + if contact.wasDiscarded { + continue + } + velocity := contact.velocity + if contact.distance < 0 { + velocity = velocity.Sub(contact.normal.Mul(contact.distance * characterPenetrationRecovery / dt)) + } + c.constraints = append(c.constraints, characterConstraint{contact: int32(i), velocity: velocity, normal: contact.normal, distance: contact.distance}) + if !c.tooSteep(contact.normal) { + continue + } + // a plane which points up: a vertical plane blocks the way up the slope (a horizontal one is already vertical; + // a normal within the rounding of up has no horizontal part to make a vertical plane of) + dot := contact.normal.Dot(characterUp) + normal := contact.normal.Sub(characterUp.Mul(dot)) + if dot <= 1e-3 || normal.LenSqr() < characterVerticalEpsilon*characterVerticalEpsilon { + continue + } + c.constraints[len(c.constraints)-1].steep = true + normal = normal.Normalize() + // the velocity projected on the vertical normal: both planes push at the same rate; the distance is the one + // to travel horizontally to reach the contact plane + c.constraints = append(c.constraints, characterConstraint{ + contact: int32(i), velocity: normal.Mul(velocity.Dot(normal)), normal: normal, distance: contact.distance / normal.Dot(contact.normal), + }) + } +} + +// handleContact: the character hits the contact. A dynamic body receives an impulse which brings it to the velocity +// of the character along the normal (damped, plus a part of the penetration), bounded by the strength of the +// character, never downwards (HandleContact of Jolt; the gravity is the world's job). Returns false if the contact +// is not to be solved (never here: Jolt leaves it to its listener) +func (c *CharacterVirtual) handleContact(velocity mgl64.Vec3, constraint *characterConstraint, dt float64) bool { + contact := &c.gathered[constraint.contact] + contact.hadCollision = true + body := contact.body + if body.BodyType != actor.BodyTypeDynamic || !(c.MaxStrength > 0) { + return true + } + relative := velocity.Sub(contact.velocity) + projected := relative.Dot(contact.normal) + delta := -projected*characterPushDamping - math.Min(contact.distance, 0)*characterPushPenetration/dt + if delta < 0 { + // separating + return true + } + // the inverse mass of the body at the point along the normal + lever := contact.position.Sub(body.Transform.Position).Cross(contact.normal) + inverseMasses := body.InverseMassAxes() + inverseMass := 0.0 + for k := 0; k < 3; k++ { + inverseMass += inverseMasses[k] * contact.normal[k] * contact.normal[k] + } + inverseInertia := body.GetInverseInertiaWorld() + effective := inverseInertia.Mul3x1(lever).Dot(lever) + inverseMass + if effective <= 0 { + return true + } + impulse := math.Min(delta/effective, c.MaxStrength*dt) + world := contact.normal.Mul(-impulse) + if down := world.Dot(characterUp); down < 0 { + world = world.Sub(characterUp.Mul(down)) + } + body.AddImpulseAtPoint(world, contact.position) + return true +} + +// solveConstraints: the displacement of the character at the velocity during remaining, slid along the constraints +// (SolveConstraints of Jolt). At each of the iterations the constraints are ordered by their time of impact (the +// pushing ones first at t = 0, the static bodies first at a tie), the character goes to the first one, its velocity +// is projected on the plane (a steep slope first loses the velocity towards it), and if the new velocity would enter +// a plane hit earlier the character slides along the edge of both. It stops when no velocity is left, or when the +// velocity reversed. Returns the displacement and the time simulated +func (c *CharacterVirtual) solveConstraints(velocity mgl64.Vec3, dt, remaining float64) (mgl64.Vec3, float64) { + if len(c.constraints) == 0 { + return velocity.Mul(remaining), remaining + } + c.sorted = c.sorted[:0] + for i := range c.constraints { + c.sorted = append(c.sorted, int32(i)) + } + last := velocity + var displacement mgl64.Vec3 + simulated := 0.0 + c.previous = c.previous[:0] + + for iteration := 0; iteration < characterMaxConstraintIterations; iteration++ { + for i := range c.constraints { + k := &c.constraints[i] + k.projected = k.normal.Dot(k.velocity.Sub(velocity)) + if k.projected < 1e-6 { + k.toi = math.Inf(1) + continue + } + distance := k.normal.Dot(displacement) + k.distance + if distance-k.projected*remaining > -characterAcceptedPenetration { + // too little penetration: the movement is accepted + k.toi = math.Inf(1) + } else { + k.toi = math.Max(0, distance/k.projected) + } + } + slices.SortFunc(c.sorted, func(a, b int32) int { + ka, kb := &c.constraints[a], &c.constraints[b] + // both at t = 0: the one which pushes the most first (the deepest penetrations too) + if ka.toi <= 0 && kb.toi <= 0 { + return compare(kb.projected, ka.projected) + } + if ka.toi != kb.toi { + return compare(ka.toi, kb.toi) + } + // a static body first: it has the most influence + return compare(bodyTypeRank(c.gathered[kb.contact].body), bodyTypeRank(c.gathered[ka.contact].body)) + }) + + // the first constraint reached + var hit *characterConstraint + hitIndex := int32(-1) + for _, index := range c.sorted { + k := &c.constraints[index] + if k.toi >= remaining { + // the goal is reached + return displacement.Add(velocity.Mul(remaining)), simulated + remaining + } + contact := &c.gathered[k.contact] + if contact.wasDiscarded { + continue + } + if !contact.hadCollision && !c.handleContact(velocity, k, dt) { + contact.wasDiscarded = true + c.excluded = append(c.excluded, contact.body) + continue + } + hit, hitIndex = k, index + break + } + if hit == nil { + return displacement.Add(velocity.Mul(remaining)), simulated + remaining + } + + displacement = displacement.Add(velocity.Mul(hit.toi)) + remaining -= hit.toi + simulated += hit.toi + if remaining < characterMinTimeRemaining { + return displacement, simulated + } + if hit.toi > 1e-4 { + c.previous = c.previous[:0] + } + + normal := hit.normal + if hit.steep { + // a steep slope: the velocity towards it goes first, else the character would climb a little and jitter + vertical := normal.Sub(characterUp.Mul(normal.Dot(characterUp))) + relative := velocity.Sub(hit.velocity) + velocity = velocity.Sub(vertical.Mul(math.Min(0, relative.Dot(vertical)) / vertical.LenSqr())) + } + relative := velocity.Sub(hit.velocity) + next := velocity.Sub(normal.Mul(relative.Dot(normal))) + + // the earlier plane the new velocity would enter the most + var other *characterConstraint + highest := 0.0 + for _, index := range c.previous { + k := &c.constraints[index] + if k == hit { + continue + } + penetration := k.velocity.Sub(next).Dot(k.normal) + if penetration > highest { + if dot := k.normal.Dot(normal); dot < characterParallelCos && dot > -characterParallelCos { + highest, other = penetration, k + } + } + } + if other != nil { + // slide along the edge of both planes + slide := normal.Cross(other.normal).Normalize() + alongSlide := slide.Mul(next.Dot(slide)) + // neither plane pushes into the other anymore: no ping-pong + hit.velocity = hit.velocity.Sub(other.normal.Mul(math.Min(0, hit.velocity.Dot(other.normal)))) + other.velocity = other.velocity.Sub(normal.Mul(math.Min(0, other.velocity.Dot(normal)))) + perpendicular := hit.velocity.Sub(slide.Mul(hit.velocity.Dot(slide))) + otherPerpendicular := other.velocity.Sub(slide.Mul(other.velocity.Dot(slide))) + next = alongSlide.Add(perpendicular).Add(otherPerpendicular) + } + if !c.allowSliding && hit.velocity.LenSqr() < 1e-16 && !c.tooSteep(hit.normal) { + // standing without input on a walkable, still surface: the character doesn't creep down it + next = mgl64.Vec3{} + } + velocity = next + c.previous = append(c.previous, hitIndex) + + if hit.projected < 1e-8 && velocity.LenSqr() < 1e-8 { + // nothing pushes, no velocity left + return displacement, simulated + } + if hit.velocity.LenSqr() >= 1e-16 { + last = hit.velocity + } else if velocity.Dot(last) < 0 { + // the velocity reversed + return displacement, simulated + } + } + return displacement, simulated +} + +// compare for the sorts: -1, 0, 1 +func compare(a, b float64) int { + if a < b { + return -1 + } + if a > b { + return 1 + } + return 0 +} + +// bodyTypeRank: a static body first in a tie of the solver, then a kinematic one, then a dynamic one (the motion +// types of Jolt in order) +func bodyTypeRank(body *actor.RigidBody) float64 { + switch body.BodyType { + case actor.BodyTypeStatic: + return 2 + case actor.BodyTypeKinematic: + return 1 + } + return 0 +} + +// moveShape: the character at position moves at the velocity for dt: contacts, constraints, solve, sweep, and +// again while time remains and the character moves (MoveShape of Jolt). With store, the contacts of the move are +// kept as the active contacts of the character +func (c *CharacterVirtual) moveShape(position *mgl64.Vec3, velocity mgl64.Vec3, dt float64, store bool) { + direction := velocity + if length := direction.Len(); length > 0 { + direction = direction.Mul(1 / length) + } + remaining := dt + for iteration := 0; iteration < characterMaxCollisionIterations && remaining >= characterMinTimeRemaining; iteration++ { + c.collectContacts(*position, direction) + c.removeConflictingContacts() + c.determineConstraints(dt) + displacement, simulated := c.solveConstraints(velocity, dt, remaining) + if store { + c.contacts = append(c.contacts[:0], c.gathered...) + } + if hit, ok := c.sweep(*position, displacement); ok { + displacement = displacement.Mul(hit.Fraction) + simulated *= hit.Fraction + } + *position = position.Add(displacement) + remaining -= simulated + if displacement.LenSqr() < 1e-8 { + break + } + } +} + +// sweep: the first body the padded capsule meets from the position along the displacement, among the bodies the +// character collides with and which are not excluded (GetFirstContactForSweep of Jolt). The bodies the capsule +// starts in or in contact with are passed through: their planes are solved, the sweep only checks what the contacts +// missed (the hits at the fraction 0 are ignored, as in ContactCastCollector of Jolt and b2World_CastMover of +// Box2D); a hit which would enter the body by less than the collision tolerance is ignored too +func (c *CharacterVirtual) sweep(position, displacement mgl64.Vec3) (Hit, bool) { + if displacement.LenSqr() < 1e-8 { + return Hit{}, false + } + scratch := c.queryBuffers + filter := QueryFilter{Mask: c.body.Mask, Excluded: c.excluded} + query := newSweepQuery(c.world, &c.padded, actor.Transform{Position: position, Rotation: mgl64.QuatIdent()}, displacement, filter, scratch) + query.character = c + query.run() + best, found := query.best, query.found + scratch.clear() + return best, found +} + +// sweepAccepts: the body is one the character collides with (its layer in the mask of the body: the other way of the +// pair rule, the mask of the character is in the filter of the query; and the pair not ignored) +func (c *CharacterVirtual) sweepAccepts(body *actor.RigidBody) bool { + return !body.IsTrigger && body.Mask&c.body.Layer != 0 && c.world.filter.allows(c.body, body) +} + +// sweepKeeps: the hit of a sweep of the character is kept: not at the start, and entering the body deeper than the +// collision tolerance by the end of the displacement +func (c *CharacterVirtual) sweepKeeps(hit *Hit, displacement mgl64.Vec3) bool { + if hit.Fraction <= 0 { + return false + } + return -hit.Normal.Dot(displacement)*(1-hit.Fraction) >= characterCollisionTolerance +} diff --git a/charactervirtual_test.go b/charactervirtual_test.go new file mode 100644 index 0000000..d0c135f --- /dev/null +++ b/charactervirtual_test.go @@ -0,0 +1,750 @@ +package feather + +import ( + "math" + "runtime" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== CHARACTER ========== +// A character is a capsule moved by the queries of the world (CharacterVirtual): it walks on the ground, the terrains and +// the meshes, slides along the walls, climbs the slopes under its limit and the steps under its height, stays on the +// floor downhill, is carried by a moving platform, pushes the dynamic bodies with a bounded force, and never goes +// through another character. The game gives its velocity (walkingVelocity: the recipe of the samples of Jolt) and +// calls Update after each step of the world. + +const ( + // the capsule of the characters of these tests: 1.7 m tall, 0.3 m wide, the standing shape of the Jolt samples + characterRadius = 0.3 + characterHalfHeight = 0.55 + + // walkSpeed: the walking speed of the tests (m/s) + walkSpeed = 2.0 + + // characterDepthTolerance: the character never enters a body deeper than this (m): the collision tolerance of + // Jolt (CharacterVirtualSettings::mCollisionTolerance) + characterDepthTolerance = 1e-3 +) + +// addCharacter: a character standing at the position (the center of its capsule), with the defaults +func addCharacter(w *World, position mgl64.Vec3) *CharacterVirtual { + c := NewCharacterVirtual(actor.Capsule{HalfHeight: characterHalfHeight, Radius: characterRadius}, position) + w.AddCharacter(c) + return c +} + +// standing: the position of the center of a capsule whose feet are at the point +func standing(feet mgl64.Vec3) mgl64.Vec3 { + return feet.Add(mgl64.Vec3{0, characterHalfHeight + characterRadius, 0}) +} + +// walkingVelocity: the velocity of a character which wants to walk at the horizontal velocity: the recipe of +// CharacterVirtualTest::HandleInput of Jolt (the inertia disabled): on the ground, the velocity of the ground, else the +// vertical velocity kept; plus the gravity of the step, plus the input +func walkingVelocity(w *World, c *CharacterVirtual, horizontal mgl64.Vec3, dt float64) mgl64.Vec3 { + velocity := mgl64.Vec3{0, c.Velocity.Y(), 0} + if c.GroundState() == CharacterOnGround && !c.IsSlopeTooSteep(c.GroundNormal()) { + c.UpdateGroundVelocity() + velocity = c.GroundVelocity() + } + return velocity.Add(w.Gravity.Mul(dt)).Add(horizontal) +} + +// walk runs the world for seconds, the characters walking at their horizontal velocity (one per character), each +// updated after the step; each is called after every frame +func walk(w *World, characters []*CharacterVirtual, horizontal []mgl64.Vec3, seconds float64, each func()) { + for i := 0; i < int(math.Round(seconds/sceneDt)); i++ { + w.Step(sceneDt) + for k, c := range characters { + c.Velocity = walkingVelocity(w, c, horizontal[k], sceneDt) + c.Update(sceneDt) + } + if each != nil { + each() + } + } +} + +// settle: the character stands for seconds, without input +func settle(w *World, c *CharacterVirtual, seconds float64) { + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{}}, seconds, nil) +} + +// entersABody: the capsule of the character, shrunk by the tolerance, overlaps a body it collides with: the character +// is deeper in a body than the tolerance +func entersABody(w *World, c *CharacterVirtual) bool { + shrunk := actor.Capsule{HalfHeight: c.Capsule.HalfHeight, Radius: c.Capsule.Radius - characterDepthTolerance} + filter := DefaultQueryFilter() + filter.Excluded = []*actor.RigidBody{c.Body()} + var found []*actor.RigidBody + found = w.Overlap(&shrunk, actor.Transform{Position: c.Position, Rotation: mgl64.QuatIdent()}, filter, found) + return len(found) > 0 +} + +// hillyMesh: a mesh of 40 x 40 m of gentle hills (under 20° everywhere) +func hillyMesh() *actor.TriangleMesh { + return gridMesh(40, 1, func(x, z float64) float64 { + return 0.4*math.Sin(x*0.5) + 0.3*math.Cos(z*0.4+1) + }) +} + +// Criterion 1: the character walks on a plane, a terrain and a mesh: never deeper than the tolerance in them, and it +// stays supported. Stopped, it doesn't tremble +func TestCharacterWalksOnTheGround(t *testing.T) { + grounds := []struct { + name string + add func(w *World) + feet float64 + }{ + {"plane", func(w *World) { addGround(w, 0.6) }, 0}, + {"terrain", func(w *World) { bumpyTerrain(w, 7) }, 6}, + {"mesh", func(w *World) { + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), hillyMesh(), actor.BodyTypeStatic, 0.6, 0) + }, 1}, + } + for _, ground := range grounds { + t.Run(ground.name, func(t *testing.T) { + w := newScene(1) + ground.add(w) + c := addCharacter(w, standing(mgl64.Vec3{-8, ground.feet, -8})) + settle(w, c, 2) + if c.GroundState() != CharacterOnGround { + t.Fatalf("%s: after 2 s the character is %v, want on the ground", ground.name, c.GroundState()) + } + + // a walk of 16 m along the diagonal: the character stays on the ground, never in it + deepest, unsupported := false, 0 + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed / math.Sqrt2, 0, walkSpeed / math.Sqrt2}}, 8, func() { + if entersABody(w, c) { + deepest = true + } + if !c.IsSupported() { + unsupported++ + } + }) + if deepest { + t.Errorf("%s: the character entered the ground deeper than %.0f mm", ground.name, characterDepthTolerance*1000) + } + // over a crest the character can leave the ground for a frame (it was going up: it doesn't stick), and lands + // at the next one + if unsupported > 3 { + t.Errorf("%s: the character was not supported during %d frames of its walk", ground.name, unsupported) + } + if traveled := c.Position.Sub(mgl64.Vec3{-8, 0, -8}); math.Hypot(traveled.X(), traveled.Z()) < 14 { + t.Errorf("%s: the character walked %.2f m horizontally in 8 s at %g m/s", ground.name, math.Hypot(traveled.X(), traveled.Z()), walkSpeed) + } + + // stopped, it doesn't tremble + settle(w, c, 1) + rest := c.Position + worst := 0.0 + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{}}, 2, func() { + worst = math.Max(worst, c.Position.Sub(rest).Len()) + }) + if worst > 1e-5 { + t.Errorf("%s: the character stopped moves by %.3f mm", ground.name, worst*1000) + } + }) + } +} + +// Criterion 2: against a wall, the character slides along it: it keeps the component of its velocity along the wall, +// never enters it and doesn't stick to it +func TestCharacterSlidesAlongAWall(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + // a wall along z, 20 m long, at x = 1 + addBody(w, mgl64.Vec3{1.5, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 10}}, actor.BodyTypeStatic, 0.6, 0) + c := addCharacter(w, standing(mgl64.Vec3{0, 0, -8})) + settle(w, c, 0.5) + + // walking at 45° into the wall: the velocity along the wall is walkSpeed / √2 + start := c.Position + entered := false + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed / math.Sqrt2, 0, walkSpeed / math.Sqrt2}}, 4, func() { + if entersABody(w, c) { + entered = true + } + }) + if entered { + t.Error("the character entered the wall") + } + if x := c.Position.X(); x > 1-characterRadius+characterDepthTolerance { + t.Errorf("the character is at x = %.4f, the wall at x = 1: it entered the wall", x) + } + if along := c.Position.Z() - start.Z(); along < 0.95*4*walkSpeed/math.Sqrt2 { + t.Errorf("the character slid %.2f m along the wall in 4 s, want %.2f m", along, 4*walkSpeed/math.Sqrt2) + } + if c.GroundState() != CharacterOnGround { + t.Errorf("against the wall the character is %v, want on the ground", c.GroundState()) + } +} + +// ramp: a static box tilted by the angle around z, its top face through the origin, climbing towards +x: the surface +// is at the height x * tan(angle) +func ramp(w *World, angle float64) *actor.RigidBody { + rotation := mgl64.QuatRotate(angle, mgl64.Vec3{0, 0, 1}) + half := mgl64.Vec3{20, 1, 10} + center := rotation.Rotate(mgl64.Vec3{0, -half.Y(), 0}) + return addBody(w, center, rotation, &actor.Box{HalfExtents: half}, actor.BodyTypeStatic, 0.6, 0) +} + +// Criterion 3: a slope under the limit is climbed, over the limit it blocks at its foot; a step under the step height +// is climbed, over it blocks +func TestCharacterSlopesAndSteps(t *testing.T) { + t.Run("slopes", func(t *testing.T) { + for _, angle := range []float64{30, 60} { + w := newScene(1) + addGround(w, 0.6) + // the ramp rises from the ground at x = 0; the character walks to it from x = -1.5 + ramp(w, angle*math.Pi/180) + c := addCharacter(w, standing(mgl64.Vec3{-1.5, 0, 0})) + settle(w, c, 1) + start := c.Position + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 3, nil) + climbed := c.Position.Y() - start.Y() + radians := angle * math.Pi / 180 + if angle < DefaultCharacterMaxSlopeAngle*180/math.Pi { + // 0.75 s on the ground, then the horizontal velocity is projected on the slope (the slide of Jolt): the + // character climbs at walkSpeed * cos(angle) along it, so sin * cos up (Godot keeps the speed with + // floor_constant_speed only) + if climbed < 0.9*2.25*walkSpeed*math.Sin(radians)*math.Cos(radians) { + t.Errorf("slope of %g°: the character climbed %.2f m in 3 s, want %.2f", angle, climbed, 2.25*walkSpeed*math.Sin(radians)*math.Cos(radians)) + } + if c.GroundState() != CharacterOnGround { + t.Errorf("slope of %g°: the character is %v, want on the ground", angle, c.GroundState()) + } + } else if climbed > 0.05 || c.Position.X() > 0 { + // the lower sphere of the capsule stops against the slope before x = 0 + t.Errorf("slope of %g°: the character climbed %.3f m and is at x = %.3f, want blocked at the foot of the slope", angle, climbed, c.Position.X()) + } + } + }) + + t.Run("steps", func(t *testing.T) { + for _, rise := range []float64{0.2, 0.6} { + w := newScene(1) + addGround(w, 0.6) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), stairMesh(6, 0.6, rise, 3), actor.BodyTypeStatic, 0.6, 0) + c := addCharacter(w, standing(mgl64.Vec3{-2, 0, 0})) + settle(w, c, 0.5) + unsupported := 0 + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 3, func() { + if !c.IsSupported() { + unsupported++ + } + }) + if rise < DefaultCharacterStepHeight { + // 6 steps of 0.6 m: the top tread starts at x = 3, 6 rises up + if c.Position.X() < 3 || c.Position.Y() < standing(mgl64.Vec3{0, 6 * rise, 0}).Y()-characterDepthTolerance { + t.Errorf("steps of %g m: the character is at (%.2f, %.2f), want on the top tread (x ≥ 3, y ≥ %.2f)", rise, c.Position.X(), c.Position.Y(), standing(mgl64.Vec3{0, 6 * rise, 0}).Y()) + } + if unsupported > 0 { + t.Errorf("steps of %g m: the character left the ground during %d frames", rise, unsupported) + } + } else if c.Position.X() > -characterRadius-CharacterPadding+characterDepthTolerance || c.Position.Y() > standing(mgl64.Vec3{}).Y()+CharacterPadding+characterDepthTolerance { + t.Errorf("steps of %g m: the character is at (%.3f, %.3f), want blocked before the first riser", rise, c.Position.X(), c.Position.Y()) + } + } + }) +} + +// Criterion 4: downhill (a slope, then a step down) the character stays on the ground +func TestCharacterStaysOnTheFloorDownhill(t *testing.T) { + t.Run("slope", func(t *testing.T) { + w := newScene(1) + ramp(w, 30*math.Pi/180) + c := addCharacter(w, standing(mgl64.Vec3{6, 6 * math.Tan(30*math.Pi/180), 0}).Add(mgl64.Vec3{0, 0.3, 0})) + settle(w, c, 1) + unsupported, entered := 0, false + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{-1.5 * walkSpeed, 0, 0}}, 3, func() { + if c.GroundState() != CharacterOnGround { + unsupported++ + } + if entersABody(w, c) { + entered = true + } + }) + if unsupported > 0 { + t.Errorf("walking down a 30° slope at %g m/s, the character left the ground during %d frames", 1.5*walkSpeed, unsupported) + } + if entered { + t.Error("walking down the slope, the character entered it") + } + }) + + t.Run("step", func(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + // a platform 0.3 m high, the character walks off its edge at x = 2 + addBody(w, mgl64.Vec3{0, 0.15, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.15, 3}}, actor.BodyTypeStatic, 0.6, 0) + c := addCharacter(w, standing(mgl64.Vec3{0, 0.3, 0})) + settle(w, c, 0.5) + unsupported := 0 + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 2.5, func() { + if !c.IsSupported() { + unsupported++ + } + }) + if unsupported > 0 { + t.Errorf("walking off a step of 0.3 m, the character was in the air during %d frames", unsupported) + } + if feet := c.Position.Y() - characterHalfHeight - characterRadius; c.Position.X() < 3 || math.Abs(feet) > CharacterPadding+characterDepthTolerance { + t.Errorf("after the step the character is at (%.2f, feet %.4f), want on the ground past x = 3", c.Position.X(), feet) + } + }) +} + +// The stick to the floor is a setting: off a platform of 1 m, a character which sticks to nothing flies, one which +// looks 0.5 m down (the default of Jolt) doesn't find the floor and flies too, one which looks 1.5 m down is put on it +func TestCharacterStickToFloorIsASetting(t *testing.T) { + for _, stick := range []float64{0, DefaultCharacterStickToFloor, 1.5} { + w := newScene(1) + addGround(w, 0.6) + // a platform 1 m high: the default of 0.5 m doesn't reach the floor off its edge, 1.5 m does + addBody(w, mgl64.Vec3{0, 0.5, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 3}}, actor.BodyTypeStatic, 0.6, 0) + c := addCharacter(w, standing(mgl64.Vec3{0, 1, 0})) + if c.StickToFloor != DefaultCharacterStickToFloor { + t.Fatalf("a new character sticks to the floor within %g m, want the default %g", c.StickToFloor, DefaultCharacterStickToFloor) + } + c.StickToFloor = stick + settle(w, c, 0.5) + unsupported := 0 + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 2.5, func() { + if !c.IsSupported() { + unsupported++ + } + }) + if stick >= 1 && unsupported > 0 { + t.Errorf("sticking within %g m, the character was in the air during %d frames off the platform of 1 m", stick, unsupported) + } + // a fall of 1 m lasts about 27 frames, less what the edge of the platform takes + if stick < 1 && (unsupported < 10 || unsupported > 40) { + t.Errorf("sticking within %g m, the character was in the air during %d frames off the platform of 1 m, want a fall of 10 to 40 frames", stick, unsupported) + } + if feet := c.Position.Y() - characterHalfHeight - characterRadius; c.Position.X() < 3 || math.Abs(feet) > CharacterPadding+characterDepthTolerance { + t.Errorf("sticking within %g m, the character is at (%.2f, feet %.4f), want on the ground past x = 3", stick, c.Position.X(), feet) + } + } +} + +// Criterion 5: standing on a kinematic platform which moves, the character is carried, sideways and up +func TestCharacterIsCarriedByAPlatform(t *testing.T) { + for _, direction := range []mgl64.Vec3{{1, 0, 0}, {0, 0.5, 0}} { + w := newScene(1) + addGround(w, 0.6) + platform := kinematicBox(w, mgl64.Vec3{0, 0.5, 0}, mgl64.Vec3{2, 0.1, 2}) + c := addCharacter(w, standing(mgl64.Vec3{0, 0.6, 0})) + settle(w, c, 0.5) + start := c.Position + for i := 0; i < 180; i++ { + moveTo(platform, translated(platform.Transform, direction)) + w.Step(sceneDt) + c.Velocity = walkingVelocity(w, c, mgl64.Vec3{}, sceneDt) + c.Update(sceneDt) + } + carried := c.Position.Sub(start) + want := direction.Mul(3) + if carried.Sub(want).Len() > 0.05 { + t.Errorf("platform moving at %v: the character was carried by %v in 3 s, want %v", direction, carried, want) + } + if c.GroundState() != CharacterOnGround || c.GroundBody() != platform { + t.Errorf("platform moving at %v: the character is %v on %v, want on the ground of the platform", direction, c.GroundState(), c.GroundBody()) + } + } +} + +// Criterion 6: the character pushes a ball and a crate; a very heavy crate stops it. The pushed body never enters +// the inner body of the character further than the slop of the solver: after a step, the real capsule shrunk by the +// slop overlaps nothing (after an update the character can be in the padding of a body it pushes: its plane moves at +// the velocity of the body, and the step moves the body) +func TestCharacterPushesTheBodies(t *testing.T) { + cases := []struct { + name string + shape actor.ShapeInterface + density float64 + pushed bool + }{ + {"ball", &actor.Sphere{Radius: 0.3}, 100, true}, + {"crate", &actor.Box{HalfExtents: mgl64.Vec3{0.3, 0.3, 0.3}}, 100, true}, + {"heavy crate", &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}, 8000, false}, + } + for _, tc := range cases { + t.Run(tc.name, func(t *testing.T) { + w := newScene(1) + addGround(w, 0.3) + body := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{1, 0.5, 0}, Rotation: mgl64.QuatIdent()}, tc.shape, actor.BodyTypeDynamic, tc.density) + body.Material.StaticFriction, body.Material.DynamicFriction = 0.3, 0.3 + w.AddBody(body) + c := addCharacter(w, standing(mgl64.Vec3{0, 0, 0})) + settle(w, c, 1) + start := c.Position + entered := false + shrunk := actor.Capsule{HalfHeight: c.Capsule.HalfHeight, Radius: c.Capsule.Radius - LinearSlop} + filter := DefaultQueryFilter() + filter.Excluded = []*actor.RigidBody{c.Body()} + var found []*actor.RigidBody + for i := 0; i < 180; i++ { + w.Step(sceneDt) + if found = w.Overlap(&shrunk, actor.Transform{Position: c.Position, Rotation: mgl64.QuatIdent()}, filter, found[:0]); len(found) > 0 { + entered = true + } + c.Velocity = walkingVelocity(w, c, mgl64.Vec3{walkSpeed, 0, 0}, sceneDt) + c.Update(sceneDt) + } + advanced := c.Position.X() - start.X() + if entered { + t.Errorf("%s: the body entered the character further than the slop", tc.name) + } + if tc.pushed { + if body.Transform.Position.X() < 2.5 || advanced < 2 { + t.Errorf("%s: the body is at x = %.2f and the character advanced %.2f m in 3 s, want both pushed ahead", tc.name, body.Transform.Position.X(), advanced) + } + } else if advanced > 0.2 || body.Transform.Position.X() > 1.01 { + t.Errorf("%s: the character advanced %.2f m and the crate moved to x = %.3f, want the character stopped", tc.name, advanced, body.Transform.Position.X()) + } + }) + } +} + +// A heavy dynamic body never goes through a standing character: it is stopped by the inner body of the character +func TestAHeavyBodyDoesNotGoThroughACharacter(t *testing.T) { + w := newScene(1) + addGround(w, 0) + c := addCharacter(w, standing(mgl64.Vec3{0, 0, 0})) + settle(w, c, 0.5) + crate := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{-3, 0.5, 0}, Rotation: mgl64.QuatIdent()}, &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}, actor.BodyTypeDynamic, 8000) + crate.Velocity = mgl64.Vec3{4, 0, 0} + w.AddBody(crate) + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{}}, 2, nil) + if crate.Transform.Position.X() > c.Position.X() { + t.Errorf("the crate at x = %.2f went through the character at x = %.2f", crate.Transform.Position.X(), c.Position.X()) + } + if crate.Transform.Position.X() < -1 { + t.Errorf("the crate at x = %.2f never reached the character", crate.Transform.Position.X()) + } +} + +// Criterion 7: 2 characters walking into each other stop against each other, 2 characters crossing slide along each +// other: they never go through each other +func TestCharactersDoNotGoThroughEachOther(t *testing.T) { + for _, crossing := range []bool{false, true} { + w := newScene(1) + addGround(w, 0.6) + a := addCharacter(w, standing(mgl64.Vec3{-2, 0, 0})) + b := addCharacter(w, standing(mgl64.Vec3{2, 0, 0})) + if crossing { + b.Position = standing(mgl64.Vec3{0, 0, 2.2}) + } + settle(w, a, 0.5) + towardsB, towardsA := mgl64.Vec3{walkSpeed, 0, 0}, mgl64.Vec3{-walkSpeed, 0, 0} + if crossing { + towardsA = mgl64.Vec3{0, 0, -walkSpeed} + } + closest := math.Inf(1) + walk(w, []*CharacterVirtual{a, b}, []mgl64.Vec3{towardsB, towardsA}, 3, func() { + closest = math.Min(closest, a.Position.Sub(b.Position).Len()) + }) + if closest < 2*characterRadius-characterDepthTolerance { + t.Errorf("crossing=%v: the characters came within %.3f m of each other, their radii sum to %.1f", crossing, closest, 2*characterRadius) + } + if !crossing && (math.Abs(a.Position.X()-b.Position.X()) > 2*characterRadius+2*CharacterPadding+0.05) { + t.Errorf("head on, the characters are %.3f m apart along x, want stopped against each other", math.Abs(a.Position.X()-b.Position.X())) + } + if crossing && (a.Position.X() < 2 || b.Position.Z() > -1) { + t.Errorf("crossing, a is at x = %.2f and b at z = %.2f, want both past the crossing", a.Position.X(), b.Position.Z()) + } + } +} + +// Both filters: a character doesn't see the bodies whose layer it doesn't collide with, and the pairs ignored +func TestCharacterFiltering(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + ghost := addBody(w, mgl64.Vec3{2, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 2}}, actor.BodyTypeStatic, 0.6, 0) + ghost.Layer = 1 << 3 + ignored := addBody(w, mgl64.Vec3{5, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 2}}, actor.BodyTypeStatic, 0.6, 0) + c := addCharacter(w, standing(mgl64.Vec3{0, 0, 0})) + w.SetFilter(c.Body(), actor.LayerDefault, actor.AllLayers&^(1<<3)) + w.IgnoreCollision(c.Body(), ignored, true) + settle(w, c, 0.5) + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 4, nil) + if c.Position.X() < 7 { + t.Errorf("the character is at x = %.2f: a wall it doesn't collide with stopped it", c.Position.X()) + } +} + +// A character teleported takes its new place at once, and refreshes its contacts +func TestCharacterTeleport(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + c := addCharacter(w, standing(mgl64.Vec3{0, 0, 0})) + settle(w, c, 0.5) + c.Teleport(standing(mgl64.Vec3{10, 5, 0})) + if c.GroundState() != CharacterInAir { + t.Errorf("teleported in the air the character is %v", c.GroundState()) + } + if c.Body().Transform.Position != c.Position { + t.Errorf("the inner body is at %v, the character at %v", c.Body().Transform.Position, c.Position) + } + hit, ok := w.Raycast(mgl64.Vec3{10, 10, 0}, mgl64.Vec3{0, -10, 0}, DefaultQueryFilter()) + if !ok || hit.Body != c.Body() { + t.Errorf("a ray over the new place hits %v, want the character", hit.Body) + } + settle(w, c, 2) + if c.GroundState() != CharacterOnGround || math.Abs(c.Position.X()-10) > 1e-9 { + t.Errorf("after falling the character is %v at %v", c.GroundState(), c.Position) + } +} + +// A character removed leaves the world: its inner body is gone, a ray doesn't see it +func TestRemoveCharacter(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + c := addCharacter(w, standing(mgl64.Vec3{0, 0, 0})) + settle(w, c, 0.5) + bodies := len(w.Bodies) + w.RemoveCharacter(c) + if len(w.Bodies) != bodies-1 { + t.Fatalf("%d bodies after the removal, want %d", len(w.Bodies), bodies-1) + } + if _, ok := w.Raycast(mgl64.Vec3{0, 5, 0}, mgl64.Vec3{0, -4, 0}, DefaultQueryFilter()); ok { + t.Error("a ray still hits the removed character") + } +} + +// charactersScene: count characters walking on a bumpy terrain towards its center, among walls and crates (24 is the +// capacity of Harmony), the walls to slide along and to push against, the crates to push. The same scene whatever the +// workers. Without the decor, the characters walk the open terrain in circles (openTerrainScene) +func charactersScene(count, workers int) (*World, []*CharacterVirtual, []mgl64.Vec3) { + return charactersSceneOf(count, workers, nil) +} + +// charactersSceneOf: charactersScene with the characters given (new ones if nil), out of any world +func charactersSceneOf(count, workers int, given []*CharacterVirtual) (*World, []*CharacterVirtual, []mgl64.Vec3) { + w, characters, directions := openTerrainSceneOf(count, workers, given) + for i, c := range characters { + angle := float64(i) * 2 * math.Pi / float64(count) + c.Teleport(standing(mgl64.Vec3{(9 + float64(i%3)) * math.Cos(angle), 4, (9 + float64(i%3)) * math.Sin(angle)})) + directions[i] = mgl64.Vec3{-walkSpeed * math.Cos(angle), 0, -walkSpeed * math.Sin(angle)} + } + for i := 0; i < 6; i++ { + angle := float64(i) * math.Pi / 3 + addBody(w, mgl64.Vec3{6 * math.Cos(angle), 3, 6 * math.Sin(angle)}, mgl64.QuatRotate(-angle, mgl64.Vec3{0, 1, 0}), &actor.Box{HalfExtents: mgl64.Vec3{0.3, 2, 2}}, actor.BodyTypeStatic, 0.6, 0) + } + for i := 0; i < 12; i++ { + angle := float64(i) * math.Pi / 6 + addBody(w, mgl64.Vec3{3 * math.Cos(angle), 4, 3 * math.Sin(angle)}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + } + return w, characters, directions +} + +// openTerrainScene: count characters on the bumpy terrain, each walking its own circle of 3 m, away from the others +func openTerrainScene(count, workers int) (*World, []*CharacterVirtual, []mgl64.Vec3) { + return openTerrainSceneOf(count, workers, nil) +} + +// openTerrainSceneOf: openTerrainScene with the characters given (new ones if nil), out of any world: each is put at +// its place without velocity, as a new one +func openTerrainSceneOf(count, workers int, given []*CharacterVirtual) (*World, []*CharacterVirtual, []mgl64.Vec3) { + w := newScene(workers) + w.parallelFrom = 1 + bumpyTerrain(w, 11) + var characters []*CharacterVirtual + var directions []mgl64.Vec3 + for i := 0; i < count; i++ { + angle := float64(i) * 2 * math.Pi / float64(count) + radius := 9 + float64(i%3) + position := standing(mgl64.Vec3{radius * math.Cos(angle), 4, radius * math.Sin(angle)}) + if given == nil { + characters = append(characters, addCharacter(w, position)) + } else { + c := given[i] + c.Position, c.Velocity, c.lastDt = position, mgl64.Vec3{}, 0 + w.AddCharacter(c) + characters = append(characters, c) + } + directions = append(directions, mgl64.Vec3{-walkSpeed * math.Sin(angle), 0, walkSpeed * math.Cos(angle)}) + } + return w, characters, directions +} + +// circling: the horizontal velocities of the characters of openTerrainScene after the frame i: each turns on its own +// circle (a turn of 3 m of radius every 9.4 s) +func circling(directions []mgl64.Vec3, i int) []mgl64.Vec3 { + turned := make([]mgl64.Vec3, len(directions)) + rotation := mgl64.QuatRotate(float64(i)*sceneDt*walkSpeed/3, mgl64.Vec3{0, 1, 0}) + for k, direction := range directions { + turned[k] = rotation.Rotate(direction) + } + return turned +} + +// Criterion 8: the same bits with 1 and 8 workers, the characters crossing the scene and each other +func TestCharactersAreDeterministic(t *testing.T) { + var reference []mgl64.Vec3 + for _, workers := range []int{1, 8} { + w, characters, directions := charactersScene(24, workers) + walk(w, characters, directions, 6, nil) + if workers == 1 { + for _, c := range characters { + reference = append(reference, c.Position) + } + continue + } + for i, c := range characters { + for k := 0; k < 3; k++ { + if math.Float64bits(c.Position[k]) != math.Float64bits(reference[i][k]) { + t.Fatalf("workers=%d: the character %d is at %v, %v with 1 worker", workers, i, c.Position, reference[i]) + } + } + } + } +} + +// Criterion 8: the updates of 24 characters allocate nothing once their buffers have grown. The characters cross the +// crowded scene for 2 s, then the same scene built again and walked again from the same places: the same updates, bit +// for bit, in the buffers which grew for them the first time. The buffers of the contacts and of the sweeps of a +// character are its own: from a sync.Pool, which the garbage collector empties, they grew again at times, and an +// update allocated: 2 collections before each frame, which empty every sync.Pool, change nothing to the updates. +// The steps between the updates take theirs from the pools, they are not measured here (a step with +// kinematic bodies moving: TestKinematicStepDoesNotAllocate), nor the first frame of the second scene: its world grows +// its own buffers (the free nodes of its tree, when the inner bodies first move). +// The count is the one of testing.AllocsPerRun, per frame: the counter of the runtime counts its own allocations too, +// on its goroutines (the scavenger, the threads, the marking of a collection) and in its caches of the conversions +// between interfaces (filled at random, 1 call in 1024): in about 1 run in 25, one of them falls between 2 reads of the +// counter. An update which allocates allocates at every frame +func TestCharacterUpdateDoesNotAllocate(t *testing.T) { + const seconds = 2 + first, characters, directions := charactersScene(24, 1) + walk(first, characters, directions, seconds, nil) + var reached []mgl64.Vec3 + for _, c := range characters { + reached = append(reached, c.Position) + first.RemoveCharacter(c) + } + first.Close() + + second, _, _ := charactersSceneOf(24, 1, characters) + defer second.Close() + var mallocs uint64 + var before, after runtime.MemStats + frames := int(math.Round(seconds / sceneDt)) + for i := 0; i < frames; i++ { + second.Step(sceneDt) + runtime.GC() + runtime.GC() + runtime.ReadMemStats(&before) + for k, c := range characters { + c.Velocity = walkingVelocity(second, c, directions[k], sceneDt) + c.Update(sceneDt) + } + runtime.ReadMemStats(&after) + if i > 0 { + mallocs += after.Mallocs - before.Mallocs + } + } + for i, c := range characters { + if c.Position != reached[i] { + t.Fatalf("the character %d is at %v walking the scene again, %v the first time: not the same updates", i, c.Position, reached[i]) + } + } + if perFrame := mallocs / uint64(frames-1); perFrame > 0 { + t.Errorf("%d allocations per frame of 24 updates (%d in %d frames), want 0", perFrame, mallocs, frames-1) + } +} + +// BenchmarkCharacters: the cost of the updates of 24 and 200 characters, walking the open terrain in circles (the +// walk of a game) and pushing against the walls and the crates of the crowded scene (every frame against a wall: a +// stair walk tried and cancelled), and the cost of the frame of the crowded scene, step included +func BenchmarkCharacters(b *testing.B) { + for _, count := range []int{24, 200} { + w, characters, directions := openTerrainScene(count, 1) + walk(w, characters, directions, 1, nil) + b.Run("open_terrain="+itoa(count), func(b *testing.B) { + for i := 0; i < b.N; i++ { + turned := circling(directions, i) + w.Step(sceneDt) + b.StartTimer() + for k, c := range characters { + c.Velocity = walkingVelocity(w, c, turned[k], sceneDt) + c.Update(sceneDt) + } + b.StopTimer() + } + }) + w, characters, directions = charactersScene(count, 1) + walk(w, characters, directions, 1, nil) + b.Run("crowd="+itoa(count), func(b *testing.B) { + for i := 0; i < b.N; i++ { + w.Step(sceneDt) + b.StartTimer() + for k, c := range characters { + c.Velocity = walkingVelocity(w, c, directions[k], sceneDt) + c.Update(sceneDt) + } + b.StopTimer() + } + }) + b.Run("crowd_frame="+itoa(count), func(b *testing.B) { + for i := 0; i < b.N; i++ { + walk(w, characters, directions, sceneDt, nil) + } + }) + } +} + +func itoa(n int) string { + if n == 0 { + return "0" + } + s := "" + for n > 0 { + s = string(rune('0'+n%10)) + s + n /= 10 + } + return s +} + +// A character walks through a static trigger zone: its inner body enters the zone once and leaves it once, the zone +// never blocks it nor makes a contact with it; stopped in the zone, it stays in it (no exit while it stands) +func TestCharacterEntersAndLeavesATrigger(t *testing.T) { + w := newScene(1) + addGround(w, 0.5) + zone := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{zoneHalf, 1, zoneHalf}}, actor.BodyTypeStatic, 0, 0) + zone.IsTrigger = true + c := addCharacter(w, standing(mgl64.Vec3{-2 * zoneHalf, 0, 0})) + settle(w, c, 0.5) + log := listenTriggers(w).listenCollisions() + body := c.Body() + + // to the center of the zone, then standing in it + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 2*zoneHalf/walkSpeed, nil) + if x := c.Position.X(); math.Abs(x) > characterRadius { + t.Fatalf("the character is at x=%.3f after walking to the center of the zone, want 0: the zone blocked it", x) + } + if enter, exit := log.count(EventTriggerEnter, zone, body), log.count(EventTriggerExit, zone, body); enter != 1 || exit != 0 { + t.Fatalf("%d enters and %d exits walking into the zone, want 1 and 0", enter, exit) + } + settle(w, c, 2) + if exit := log.count(EventTriggerExit, zone, body); exit != 0 { + t.Fatalf("%d exits while the character stands in the zone, want 0", exit) + } + + // out of the zone + walk(w, []*CharacterVirtual{c}, []mgl64.Vec3{{walkSpeed, 0, 0}}, 2*zoneHalf/walkSpeed, nil) + if x := c.Position.X(); x < 2*zoneHalf-characterRadius { + t.Fatalf("the character is at x=%.3f after walking out of the zone, want %.1f", x, 2*zoneHalf) + } + if enter, exit := log.count(EventTriggerEnter, zone, body), log.count(EventTriggerExit, zone, body); enter != 1 || exit != 1 { + t.Fatalf("%d enters and %d exits after crossing the zone, want 1 and 1", enter, exit) + } + for _, record := range log.events { + if record.eventType == EventCollisionEnter || record.eventType == EventCollisionStay || record.eventType == EventCollisionExit { + t.Fatalf("collision event %v of the character with the zone or the ground", record.eventType) + } + } +} diff --git a/collision.go b/collision.go index 0f93e72..4738521 100644 --- a/collision.go +++ b/collision.go @@ -60,19 +60,14 @@ func narrowPhase(pairs []Pair, workersCount int, margin func(a, b *actor.RigidBo return compactManifolds(manifolds, offsets, counts) } -// manifoldsOf: the count of manifolds a pair can have, MaxManifoldsPerPair against a heightfield +// manifoldsOf: the count of manifolds a pair can have, MaxManifoldsPerPair against a surface (a heightfield, a mesh) func manifoldsOf(pair Pair) int { - if isHeightfield(pair.BodyA) || isHeightfield(pair.BodyB) { + if isSurface(pair.BodyA) || isSurface(pair.BodyB) { return MaxManifoldsPerPair } return 1 } -func isHeightfield(body *actor.RigidBody) bool { - _, ok := body.Shape.(*actor.Heightfield) - return ok -} - // collidePair: triggers keep only the real overlaps func collidePair(pair Pair, margin float64, out []constraint.Manifold) int { a, b := pair.BodyA, pair.BodyB @@ -167,8 +162,8 @@ func compactManifolds(manifolds []constraint.Manifold, offsets, counts []int) [] // - spheres & capsules: closest points of their segments (collision_capsule.go) // - other shapes: GJK/EPA, then the contact points are clipped (epa/manifold.go) // -// Against a heightfield, a body can touch the terrain with several normals: Collide keeps the deepest patch, -// CollideAll returns all of them +// Against a surface (a heightfield, a triangle mesh), a body can touch it with several normals: Collide keeps the +// deepest patch, CollideAll returns all of them func Collide(a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool { var manifolds [1]constraint.Manifold found := CollideAll(a, b, margin, manifolds[:]) > 0 @@ -178,41 +173,77 @@ func Collide(a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool // CollideAll writes in manifolds the contacts between a and b (MaxManifoldsPerPair at most), and returns their count func CollideAll(a, b *actor.RigidBody, margin float64, manifolds []constraint.Manifold) int { + return collideAllMoving(nil, a, b, margin, mgl64.Vec3{}, manifolds) +} + +// contactScratch: the buffers of the contacts of a pair (GJK, EPA and the clipping of the features, the points of a +// plane, the triangles of a surface), the own buffers of a character (CharacterVirtual). The pairs of a step pass nil: +// they take each buffer from its sync.Pool when they need it. A pool emptied by the garbage collector gives new +// buffers, which grow again: an update of a character, which allocated nothing, allocated then +type contactScratch struct { + simplex gjk.Simplex + epa epa.Scratch + plane planeBuffers + triangles triangleScratch +} + +// newContactScratch: the buffers, the triangle of the surfaces made +func newContactScratch() *contactScratch { + s := &contactScratch{plane: newPlaneBuffers()} + s.triangles.init() + s.triangles.epa = &s.epa + return s +} + +// collideAllMoving: CollideAll for a body a which moves in the direction movement (a unit vector, or zero): on a +// surface, the normal of a contact on an inactive edge follows the motion (contactNormal), as the contacts of a +// character. The buffers are those of s, those of the pools if s is nil +func collideAllMoving(s *contactScratch, a, b *actor.RigidBody, margin float64, movement mgl64.Vec3, manifolds []constraint.Manifold) int { if len(manifolds) == 0 { return 0 } m := &manifolds[0] m.Reset(a, b) - if field, ok := a.Shape.(*actor.Heightfield); ok { - return collideHeightfield(a, field, b, margin, false, manifolds) + // a surface against a surface never meets: both are static + if isSurface(a) { + return collideTriangles(s, a, b, margin, false, movement.Mul(-1), manifolds) } - if field, ok := b.Shape.(*actor.Heightfield); ok { - return collideHeightfield(b, field, a, margin, true, manifolds) + if isSurface(b) { + return collideTriangles(s, b, a, margin, true, movement, manifolds) } - if collide(a, b, margin, m) { + if collide(s, a, b, margin, m) { return 1 } return 0 } -// collide the convex shapes a & b -func collide(a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool { +// collide the convex shapes a & b, in the buffers of s (of the pools if nil) +func collide(s *contactScratch, a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool { if plane, ok := a.Shape.(*actor.Plane); ok { - return collidePlane(plane, b, margin, false, m) + return collidePlane(s, plane, b, margin, false, m) } if plane, ok := b.Shape.(*actor.Plane); ok { - return collidePlane(plane, a, margin, true, m) + return collidePlane(s, plane, a, margin, true, m) } if isAnalyticPair(a.Shape, b.Shape) { return collideAnalyticPair(a, b, margin, m) } + if s != nil { + result, ok := penetration(a, b, margin, &s.simplex, &s.epa) + if !ok { + return false + } + s.epa.Manifold(a, b, result, margin, m) + return m.Count > 0 + } + simplex := gjk.SimplexPool.Get().(*gjk.Simplex) defer gjk.SimplexPool.Put(simplex) - result, ok := penetration(a, b, margin, simplex) + result, ok := penetration(a, b, margin, simplex, nil) if !ok { return false } @@ -223,12 +254,19 @@ func collide(a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool // penetration of a + margin into b (the convex shapes), false if they are further than the margin. // A rounded shape (sphere, capsule) is its core with a radius: GJK gives the distance and the closest points of the // cores, exact against a polytope (Bullet, Jolt), and the radii are added along their direction. EPA runs on the full -// shapes only if the cores overlap, or are too close for their direction to be a normal -func penetration(a, b *actor.RigidBody, margin float64, simplex *gjk.Simplex) (epa.Result, bool) { +// shapes only if the cores overlap, or are too close for their direction to be a normal. EPA works in buffers, in those +// of its pool if nil +func penetration(a, b *actor.RigidBody, margin float64, simplex *gjk.Simplex, buffers *epa.Scratch) (epa.Result, bool) { coreA, radiusA := gjk.NewCoreProxy(a) coreB, radiusB := gjk.NewCoreProxy(b) + return penetrationOfCores(a, b, &coreA, radiusA, &coreB, radiusB, margin, simplex, buffers) +} + +// penetrationOfCores: penetration, the cores of the shapes and their radii already made (a triangle of a surface +// tests one body: its core is made once per pair, not per triangle) +func penetrationOfCores(a, b *actor.RigidBody, coreA *gjk.Proxy, radiusA float64, coreB *gjk.Proxy, radiusB, margin float64, simplex *gjk.Simplex, buffers *epa.Scratch) (epa.Result, bool) { if radiusA+radiusB > 0 { - if closest := gjk.Distance(&coreA, &coreB); !closest.Overlap && closest.Distance > normalEpsilon { + if closest := gjk.Distance(coreA, coreB); !closest.Overlap && closest.Distance > normalEpsilon { depth := radiusA + radiusB + margin - closest.Distance if depth < 0 { return epa.Result{}, false @@ -247,7 +285,13 @@ func penetration(a, b *actor.RigidBody, margin float64, simplex *gjk.Simplex) (e if !gjk.GJKProxies(&proxyA, &proxyB, margin, simplex) { return epa.Result{}, false } - result, err := epa.EPAProxies(&proxyA, &proxyB, simplex, margin) + var result epa.Result + var err error + if buffers != nil { + result, err = buffers.EPAProxies(&proxyA, &proxyB, simplex, margin) + } else { + result, err = epa.EPAProxies(&proxyA, &proxyB, simplex, margin) + } if err != nil { return epa.Result{}, false } @@ -260,15 +304,27 @@ type planeBuffers struct { points []constraint.ContactPoint } +// newPlaneBuffers: the buffers of collidePlane, for the 8 vertices of a box +func newPlaneBuffers() planeBuffers { + return planeBuffers{plane: make(actor.PlaneContact, 0, 8), points: make([]constraint.ContactPoint, 0, 8)} +} + var planeContactsPool = sync.Pool{New: func() any { - return &planeBuffers{plane: make(actor.PlaneContact, 0, 8), points: make([]constraint.ContactPoint, 0, 8)} + buffers := newPlaneBuffers() + return &buffers }} // collidePlane keeps the order of the pair: if the plane is body B, the normal is reversed. -// The points of the shape are reduced to 4 like the other contacts: the deepest first -func collidePlane(plane *actor.Plane, object *actor.RigidBody, margin float64, planeIsB bool, m *constraint.Manifold) bool { - buffers := planeContactsPool.Get().(*planeBuffers) - defer planeContactsPool.Put(buffers) +// The points of the shape are reduced to 4 like the other contacts: the deepest first. The buffers are those of s, +// those of the pool if nil +func collidePlane(s *contactScratch, plane *actor.Plane, object *actor.RigidBody, margin float64, planeIsB bool, m *constraint.Manifold) bool { + var buffers *planeBuffers + if s != nil { + buffers = &s.plane + } else { + buffers = planeContactsPool.Get().(*planeBuffers) + defer planeContactsPool.Put(buffers) + } buffers.plane = object.Shape.CollideWithPlane(plane.Normal, plane.Distance, object.Transform, margin, buffers.plane[:0]) if len(buffers.plane) == 0 { return false diff --git a/collision_heightfield.go b/collision_heightfield.go deleted file mode 100644 index 993d8e0..0000000 --- a/collision_heightfield.go +++ /dev/null @@ -1,335 +0,0 @@ -package feather - -import ( - "math" - "slices" - "sync" - - "github.com/akmonengine/feather/actor" - "github.com/akmonengine/feather/constraint" - "github.com/akmonengine/feather/epa" - "github.com/akmonengine/feather/gjk" - "github.com/go-gl/mathgl/mgl64" -) - -const ( - // MaxManifoldsPerPair: a body on a terrain touches it with 8 normals at most (8 patches): - // on a rough terrain, both ends of a capsule can touch 4 triangles each - MaxManifoldsPerPair = 8 - - // patchCos: the contacts of 2 triangles whose normals differ by less than 5° are in the same patch: cos(5°) - patchCos = 0.99619469809174553229501040247389 - - // triangleFaceCos: the contact of a triangle is a face contact if its normal is the normal of the triangle (0.5°) - triangleFaceCos = 0.99996 - - // edgeBarycentric: a point on a triangle with a barycentric coordinate under this value is on an edge - edgeBarycentric = 1e-3 - - // weldDistance: 2 points of a patch closer than this distance are the same point (m) - weldDistance = 1e-4 - - // insideTriangle: a point on a side of a triangle is inside (barycentric coordinate) - insideTriangle = -1e-9 -) - -// triangleShape is a triangle of a heightfield, in world space: its body has the identity transform -type triangleShape struct { - vertices [3]mgl64.Vec3 - aabb actor.AABB -} - -// ComputeAABB: the vertices are in world space -func (t *triangleShape) ComputeAABB(transform actor.Transform) actor.AABB { - return t.aabb -} - -func (t *triangleShape) ComputeMass(density float64) float64 { - return math.Inf(1) -} - -func (t *triangleShape) ComputeInertia(mass float64) mgl64.Mat3 { - return mgl64.Mat3{} -} - -func (t *triangleShape) Support(direction mgl64.Vec3) mgl64.Vec3 { - best := 0 - for i := 1; i < 3; i++ { - if t.vertices[i].Dot(direction) > t.vertices[best].Dot(direction) { - best = i - } - } - return t.vertices[best] -} - -// GetContactFeature: the face, the manifold keeps its deepest edge or vertex if it is not aligned with the normal -func (t *triangleShape) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { - copy(output[:3], t.vertices[:]) - *count = 3 -} - -func (t *triangleShape) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform actor.Transform, margin float64, contacts actor.PlaneContact) actor.PlaneContact { - return contacts -} - -// CastRay: a ray is cast on the heightfield, not on its triangles -func (t *triangleShape) CastRay(origin, translation mgl64.Vec3, maxFraction float64) (actor.RayHit, bool) { - return actor.RayHit{}, false -} - -// triangleContact: the points of a triangle, in heightfieldScratch.points, with their normal (from the terrain to the body). -// A witness contact is only used if its patch has no other contact -type triangleContact struct { - normal mgl64.Vec3 - first int - count int - separation float64 - witness bool -} - -// heightfieldScratch: the buffers of a collision with a heightfield, reused to avoid the allocations -type heightfieldScratch struct { - shape triangleShape - triangle actor.RigidBody - simplex gjk.Simplex - manifold constraint.Manifold - cells []int32 - plane actor.PlaneContact - contacts []triangleContact - points []constraint.ContactPoint - patches [MaxManifoldsPerPair][]constraint.ContactPoint -} - -var heightfieldPool = sync.Pool{New: func() any { - s := &heightfieldScratch{} - s.triangle = actor.RigidBody{Transform: actor.NewTransform(), BodyType: actor.BodyTypeStatic, Shape: &s.shape} - return s -}} - -// collideHeightfield writes in out the patches of contact between the terrain and the body, and returns their count. -// Each triangle under the body is tested with GJK/EPA. A contact on an inactive edge (between 2 triangles almost flat, -// or bent inwards) takes the normal of its triangle: a body sliding on the terrain doesn't hit the inner edges. -// A contact with the face of a triangle is the contact with its plane (CollideWithPlane of the shape), limited to -// the triangle: on a flat terrain, the bodies behave exactly as on a plane. -// The contacts are then grouped by normal: one patch (a manifold of 4 points) per normal. -// The order of the pair is kept: if the terrain is B, the normals point from the body to the terrain -func collideHeightfield(terrain *actor.RigidBody, field *actor.Heightfield, object *actor.RigidBody, margin float64, terrainIsB bool, out []constraint.Manifold) int { - s := heightfieldPool.Get().(*heightfieldScratch) - defer heightfieldPool.Put(s) - - bounds := object.AABB() - bounds = actor.AABB{Min: bounds.Min.Sub(mgl64.Vec3{margin, margin, margin}), Max: bounds.Max.Add(mgl64.Vec3{margin, margin, margin})} - s.cells = field.OverlapCells(localBounds(terrain.Transform, bounds), s.cells[:0]) - s.contacts, s.points = s.contacts[:0], s.points[:0] - - cellsZ := field.ZSamples - 1 - for _, cell := range s.cells { - x, z := int(cell)/cellsZ, int(cell)%cellsZ - for t := 0; t < 2; t++ { - local, edges := field.Triangle(x, z, t) - for i := range local { - s.shape.vertices[i] = terrain.Transform.ToWorld(local[i]) - } - s.shape.aabb = triangleAABB(s.shape.vertices) - if s.shape.aabb.Overlaps(bounds) { - s.collideTriangle(object, edges, margin) - } - } - } - if len(s.contacts) == 0 { - return 0 - } - - // ========== PATCHES ========== - // the deepest contacts first: they give the normal of their patch, the shallowest are dropped (as in Jolt) - slices.SortStableFunc(s.contacts, func(a, b triangleContact) int { - switch { - case a.separation < b.separation: - return -1 - case a.separation > b.separation: - return 1 - } - return 0 - }) - var normals [MaxManifoldsPerPair]mgl64.Vec3 - var witnesses [MaxManifoldsPerPair]int - patches := 0 - for c, contact := range s.contacts { - best, bestCos := -1, math.Inf(-1) - for k := 0; k < patches; k++ { - if cos := normals[k].Dot(contact.normal); cos > bestCos { - best, bestCos = k, cos - } - } - if bestCos < patchCos { - // a new normal: a new patch, unless the deepest patches are already found - if patches == min(len(out), MaxManifoldsPerPair) { - continue - } - best = patches - normals[best] = contact.normal - s.patches[best] = s.patches[best][:0] - witnesses[best] = -1 - patches++ - } - if contact.witness { - // the deepest witness contact of the patch, in case it has no other contact - if witnesses[best] < 0 { - witnesses[best] = c - } - continue - } - for _, point := range s.points[contact.first : contact.first+contact.count] { - s.patches[best] = weld(s.patches[best], point) - } - } - for k := 0; k < patches; k++ { - if len(s.patches[k]) == 0 { - witness := s.contacts[witnesses[k]] - s.patches[k] = append(s.patches[k], s.points[witness.first]) - } - } - - for k := 0; k < patches; k++ { - m := &out[k] - m.Reset(terrain, object) - m.Normal = normals[k] - if terrainIsB { - m.Reset(object, terrain) - m.Normal = normals[k].Mul(-1) - } - epa.Reduce(s.patches[k], normals[k], m) - } - return patches -} - -// collideTriangle adds the contact of the body with the triangle of s.shape -func (s *heightfieldScratch) collideTriangle(object *actor.RigidBody, edges uint8, margin float64) { - result, ok := penetration(&s.triangle, object, margin, &s.simplex) - if !ok { - return - } - - vertices := s.shape.vertices - faceNormal := vertices[1].Sub(vertices[0]).Cross(vertices[2].Sub(vertices[0])).Normalize() - - // ========== FACE ========== - // the points of the body above the triangle, closer to its plane than the margin - first := len(s.points) - s.plane = object.Shape.CollideWithPlane(faceNormal, -faceNormal.Dot(vertices[0]), object.Transform, margin, s.plane[:0]) - for _, point := range s.plane { - if u, v, w := barycentric(point.Position, vertices[0], vertices[1], vertices[2]); u >= insideTriangle && v >= insideTriangle && w >= insideTriangle { - s.points = append(s.points, constraint.ContactPoint{Position: point.Position, Separation: point.Separation}) - } - } - faceFound := s.addContact(faceNormal, first, false) - - // ========== EDGE ========== - // the body touches an active edge or vertex from above: the contact of EPA, with its normal - onTriangle := result.WitnessA.Sub(result.Normal.Mul(margin)) - cos := result.Normal.Dot(faceNormal) - if cos < triangleFaceCos && cos > 0 && touchesEdge(vertices, onTriangle, edges&0b111) { - first = len(s.points) - epa.Manifold(&s.triangle, object, result, margin, &s.manifold) - s.points = append(s.points, s.manifold.Points[:s.manifold.Count]...) - s.addContact(result.Normal, first, false) - return - } - - if !faceFound { - // the body is beside the triangle, above an inactive edge: the witness point, with the normal of the face, - // only used if no other triangle has a contact with this normal - first = len(s.points) - onB := result.WitnessB - s.points = append(s.points, constraint.ContactPoint{Position: onTriangle.Add(onB).Mul(0.5), Separation: margin - result.Depth}) - s.addContact(faceNormal, first, true) - } -} - -// addContact: the points from first to the end of s.points, with their normal. Returns false if there is no point -func (s *heightfieldScratch) addContact(normal mgl64.Vec3, first int, witness bool) bool { - count := len(s.points) - first - if count == 0 { - return false - } - deepest := math.Inf(1) - for _, point := range s.points[first:] { - deepest = math.Min(deepest, point.Separation) - } - s.contacts = append(s.contacts, triangleContact{normal: normal, first: first, count: count, separation: deepest, witness: witness}) - return true -} - -// touchesEdge: the point is on one of the edges of the triangle (bit e for the edge from the vertex e to the vertex e+1), -// or on a vertex of these edges -func touchesEdge(vertices [3]mgl64.Vec3, p mgl64.Vec3, edges uint8) bool { - u, v, w := barycentric(p, vertices[0], vertices[1], vertices[2]) - weights := [3]float64{u, v, w} - for e := 0; e < 3; e++ { - // the edge e goes from the vertex e to the vertex e+1: the weight of the opposite vertex is 0 - if weights[(e+2)%3] <= edgeBarycentric && edges&(1< t.vertices[best].Dot(direction) { + best = i + } + } + return t.vertices[best] +} + +// GetContactFeature: the face, the manifold keeps its deepest edge or vertex if it is not aligned with the normal +func (t *triangleShape) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { + copy(output[:3], t.vertices[:]) + *count = 3 +} + +func (t *triangleShape) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform actor.Transform, margin float64, contacts actor.PlaneContact) actor.PlaneContact { + return contacts +} + +// CastRay: a ray is cast on the surface, not on its triangles +func (t *triangleShape) CastRay(origin, translation mgl64.Vec3, maxFraction float64) (actor.RayHit, bool) { + return actor.RayHit{}, false +} + +// triangleContact: points of a triangle, in triangleScratch.points, with their normal (from the surface to the +// body) and the separation of the deepest one +type triangleContact struct { + normal mgl64.Vec3 + first int + count int + separation float64 + // edge: the points of the body on the edges of the triangle, else its vertices over the face of the triangle + edge bool + // closest: the closest point of the body alone + closest bool +} + +// triangleScratch: the buffers of a collision with a surface of triangles, reused to avoid the allocations +type triangleScratch struct { + shape triangleShape + triangle actor.RigidBody + simplex gjk.Simplex + // epa: the buffers of EPA of the character which owns the scratch (contactScratch), nil for those of the pool of EPA + epa *epa.Scratch + // the cells of a heightfield, the triangles of a mesh, under the body + cells []int32 + triangles []int32 + plane actor.PlaneContact + contacts []triangleContact + points []constraint.ContactPoint + patches [MaxManifoldsPerPair][]constraint.ContactPoint + clip [2][maxClipVertices]mgl64.Vec3 + // sides: the sides of the triangle each vertex of clip was cut on (bit e), 0 for a vertex of the body + sides [2][maxClipVertices]uint8 + // movement: the direction the body moves in, for the normals of the contacts on the inactive edges (contactNormal); + // zero for the pairs of a step + movement mgl64.Vec3 + // the cores of the triangle (made once: the shape is the one of the scratch, at the identity) and of the body + // (made once per pair), for the GJK of every triangle + triangleCore, objectCore gjk.Proxy + objectRadius float64 +} + +// init the triangle of the scratch and its core: they point to the scratch, which must not be copied +func (s *triangleScratch) init() { + s.triangle = actor.RigidBody{Transform: actor.NewTransform(), BodyType: actor.BodyTypeStatic, Shape: &s.shape} + s.triangleCore, _ = gjk.NewCoreProxy(&s.triangle) +} + +var trianglePool = sync.Pool{New: func() any { + s := &triangleScratch{} + s.init() + return s +}} + +// isSurface: the body is a heightfield or a triangle mesh: a static surface of triangles, which a body touches with +// several normals (MaxManifoldsPerPair manifolds) +func isSurface(body *actor.RigidBody) bool { + switch body.Shape.(type) { + case *actor.Heightfield, *actor.TriangleMesh: + return true + } + return false +} + +// collideTriangles writes in out the patches of contact between the surface (a heightfield or a triangle mesh, the +// shape of surface) and the body, and returns their count. The surfaces differ by the triangles they give under the +// AABB of the body (the cells of a heightfield, the leaves of the tree of a mesh) and by nothing else: one generator of +// contacts per triangle (collideTriangle) and one grouping in patches serve both, as b3ComputeMeshManifolds of Box3D +// takes the triangles of b3QueryMeshTriangles or of b3QueryHeightFieldTriangles (src/mesh_contact.c:51-73), as +// CollideConvexVsTriangles of Jolt is the visitor of its mesh and of its height field, and PCMConvexVsMeshContactGeneration +// of PhysX serves its mesh and its height field. +// Each triangle under the body, seen from the side of its normal, is tested with GJK/EPA (collideTriangle): it gives +// the vertices of the body over its face, and the points of the body on its edges (a ridge in the face of a box, an +// active edge). +// The contacts are then grouped by normal: one patch (a manifold of 4 points) per normal, for the vertices of the body, +// and one for the edges of the surface. On a flat surface a body has the manifold it has on a plane: its vertices. +// Jolt and Box3D group by normal alone, and keep 4 points out of all of them (PruneContactPoints, b3ReduceCluster). +// In one manifold of 4 points, the points on the edges of the surface take the place of a corner of the body: a plate +// resting flat lost 2 corners to 2 points of its face over an edge of the grid, and sank by 10 mm on this side while +// the pair cache kept its contact. +// The order of the pair is kept: if the surface is B, the normals point from the body to the surface. The buffers are +// those of owner, those of the pool if nil +func collideTriangles(owner *contactScratch, surface, object *actor.RigidBody, margin float64, surfaceIsB bool, movement mgl64.Vec3, out []constraint.Manifold) int { + var s *triangleScratch + if owner != nil { + s = &owner.triangles + } else { + s = trianglePool.Get().(*triangleScratch) + defer trianglePool.Put(s) + } + s.movement = movement + s.objectCore, s.objectRadius = gjk.NewCoreProxy(object) + + bounds := object.AABB() + bounds = actor.AABB{Min: bounds.Min.Sub(mgl64.Vec3{margin, margin, margin}), Max: bounds.Max.Add(mgl64.Vec3{margin, margin, margin})} + local := localBounds(surface.Transform, bounds) + s.contacts, s.points = s.contacts[:0], s.points[:0] + + switch shape := surface.Shape.(type) { + case *actor.Heightfield: + s.cells = shape.OverlapCells(local, s.cells[:0]) + cellsZ := shape.ZSamples - 1 + for _, cell := range s.cells { + x, z := int(cell)/cellsZ, int(cell)%cellsZ + for t := 0; t < 2; t++ { + vertices, edges := shape.Triangle(x, z, t) + s.collideLocalTriangle(surface, object, vertices, edges, bounds, margin) + } + } + case *actor.TriangleMesh: + s.triangles = shape.OverlapTriangles(local, s.triangles[:0]) + for _, t := range s.triangles { + vertices, edges := shape.Triangle(t) + s.collideLocalTriangle(surface, object, vertices, edges, bounds, margin) + } + } + if len(s.contacts) == 0 { + return 0 + } + + // ========== PATCHES ========== + // the deepest contacts first: they give the normal of their patch, the shallowest are dropped (Jolt replaces its + // shallowest manifold, and gives a manifold the mean of its normals). A closest point comes after the points as + // deep as it + slices.SortStableFunc(s.contacts, func(a, b triangleContact) int { + depthA, depthB := a.separation, b.separation + if a.closest { + depthA += epa.EPATieTolerance + } + if b.closest { + depthB += epa.EPATieTolerance + } + switch { + case depthA < depthB: + return -1 + case depthA > depthB: + return 1 + } + return 0 + }) + var normals [MaxManifoldsPerPair]mgl64.Vec3 + var edges [MaxManifoldsPerPair]bool + patches := 0 + for _, contact := range s.contacts { + best, bestCos, held := -1, math.Inf(-1), false + for k := 0; k < patches; k++ { + cos := normals[k].Dot(contact.normal) + held = held || cos >= patchCos + if edges[k] == contact.edge && cos > bestCos { + best, bestCos = k, cos + } + } + if contact.closest && contact.edge && held { + // a closest point on an edge is the point of the terrain several triangles see, on the edge they share: it + // is kept if it is the deepest point along its normal. A body resting on a triangle near an edge would have + // one more point per triangle around, moving its friction center (a sphere would not roll on a flat terrain + // as on a plane): Box3D and PhysX drop such a point when a triangle with a contact of its face owns the edge + // (b3ComputeMeshManifolds, generateLastContacts). The depth is compared instead: the end of a capsule right + // above a ridge is over neither of its triangles, along their normals, while the rest of the capsule gives + // them a contact of their face. Its deepest point was dropped this way, and bodies sank by centimetres. + // A closest point inside the triangle is a point of its face (a sphere): kept whatever its depth, as the + // vertices of a body are (Box3D and PhysX keep it too: a point of a face owns no edge). It was dropped when + // a patch within 5° had a deeper point: a sphere rolling to a triangle folded by less than 5° had no contact + // with it until it was the deepest, and sank by 3 mm + continue + } + if bestCos < patchCos { + // a new normal: a new patch, unless the deepest patches are already found + if patches == min(len(out), MaxManifoldsPerPair) { + continue + } + best = patches + normals[best], edges[best] = contact.normal, contact.edge + s.patches[best] = s.patches[best][:0] + patches++ + } + for _, point := range s.points[contact.first : contact.first+contact.count] { + s.patches[best] = weld(s.patches[best], point) + } + } + + for k := 0; k < patches; k++ { + m := &out[k] + m.Reset(surface, object) + m.Normal = normals[k] + if surfaceIsB { + m.Reset(object, surface) + m.Normal = normals[k].Mul(-1) + } + reducePatch(s.patches[k], normals[k], m) + } + return patches +} + +// collideLocalTriangle: the triangle of the surface, given in its local space with its edges (bit e active, bit 3+e +// convex), is taken to world space and tested if its AABB overlaps the bounds of the body +func (s *triangleScratch) collideLocalTriangle(surface, object *actor.RigidBody, vertices [3]mgl64.Vec3, edges uint8, bounds actor.AABB, margin float64) { + for i := range vertices { + s.shape.vertices[i] = surface.Transform.ToWorld(vertices[i]) + } + s.shape.aabb = triangleAABB(s.shape.vertices) + if s.shape.aabb.Overlaps(bounds) { + s.collideTriangle(object, edges&0b111, edges>>3, margin) + } +} + +// collideTriangle adds the contacts of the body with the triangle of s.shape: +// - a triangle seen from below is ignored: the center of the body is under its plane (CollideConvexVsTriangles of +// Jolt, b3ComputeMeshManifolds of Box3D, PCMConvexVsMeshContactGeneration of PhysX). The terrain is a surface: a +// body whose center went through it is not pushed back +// - the face: the vertices of the body over the triangle, along the normal of the triangle (clipFace), as against a +// plane. Whatever the closest point is: a corner which turns towards the face of a triangle during the step has +// its contact +// - the ridges too flat to be active edges (inactive, bent outwards): the points where the face of the body crosses +// them, along the normal of the triangle. The side of a capsule, the face of a box laid across such a ridge touch +// it there, with no vertex +// - GJK/EPA gives the closest points of the triangle and of the body, their normal and their depth. Their normal is +// the one of EPA, or the one of the triangle on an inactive edge (contactNormal) +// - with the normal of the triangle, the closest point is one more point of the triangle, if it is deeper than the +// others: a point of its face (a sphere), or of its edge with the body beside the triangle, above this edge +// - with another normal (an active edge), the points where the body crosses the active edges, along this normal, or +// the closest point alone (a sphere, an edge of a box across the edge) +// +// Every separation is measured along the normal of its contact. +// activeEdges, convexEdges: bit e for the edge from the vertex e to the vertex e+1 +func (s *triangleScratch) collideTriangle(object *actor.RigidBody, activeEdges, convexEdges uint8, margin float64) { + vertices := &s.shape.vertices + faceNormal := vertices[1].Sub(vertices[0]).Cross(vertices[2].Sub(vertices[0])).Normalize() + if faceNormal.Dot(object.Transform.Position.Sub(vertices[0])) < 0 { + return + } + result, ok := penetrationOfCores(&s.triangle, object, &s.triangleCore, 0, &s.objectCore, s.objectRadius, margin, &s.simplex, s.epa) + if !ok { + return + } + + // ========== FACE ========== + first := len(s.points) + s.clipFace(object, faceNormal, faceNormal, true, 0, margin) + faceDeepest := s.addContact(faceNormal, first, false, false) + + // ========== FLAT RIDGES ========== + // across a flat edge, or one bent inwards, the vertices of the body on both sides say everything. Across an active + // edge, the point belongs to the contact of this edge: the plane of the triangle stops at its edge, a face sliding + // across a ridge must not be held by the plane of the slope it leaves + if ridges := convexEdges &^ activeEdges; ridges != 0 { + first = len(s.points) + s.clipFace(object, faceNormal, faceNormal, false, ridges, margin) + faceDeepest = math.Min(faceDeepest, s.addContact(faceNormal, first, true, false)) + } + + // ========== CLOSEST POINT ========== + // WitnessA is on the triangle grown by the margin + onTriangle, onBody := result.WitnessA.Sub(result.Normal.Mul(margin)), result.WitnessB + touched := touchedEdges(vertices, onTriangle) + normal := contactNormal(faceNormal, activeEdges, touched, result.Normal, s.movement) + separation := onBody.Sub(onTriangle).Dot(normal) + closest := constraint.ContactPoint{Position: onBody.Sub(normal.Mul(separation / 2)), Separation: separation} + first = len(s.points) + if normal == faceNormal || normal.Dot(faceNormal) > nearFaceCos { + // a point of the face, inside the triangle (a sphere), or a point on an inactive edge, the body beside the + // triangle: with the points of the edges + if separation < faceDeepest-epa.EPATieTolerance { + s.points = append(s.points, closest) + s.addContact(normal, first, touched != 0, true) + } + return + } + + // ========== EDGE ========== + // the normal of EPA: on an active edge + s.clipFace(object, normal, faceNormal, false, activeEdges, margin) + if len(s.points) > first { + s.addContact(normal, first, true, false) + return + } + s.points = append(s.points, closest) + s.addContact(normal, first, true, true) +} + +// addContact: the points from first to the end of s.points are a contact. Returns the separation of its deepest point, +// +Inf without point +func (s *triangleScratch) addContact(normal mgl64.Vec3, first int, edge, closest bool) float64 { + deepest := math.Inf(1) + for _, point := range s.points[first:] { + deepest = math.Min(deepest, point.Separation) + } + if len(s.points) > first { + s.contacts = append(s.contacts, triangleContact{normal: normal, first: first, count: len(s.points) - first, separation: deepest, edge: edge, closest: closest}) + } + return deepest +} + +// touchedEdges: the edges of the triangle the point is on (bit e for the edge from the vertex e to the vertex e+1): one +// edge, the 2 edges of a vertex, 0 inside the triangle. The barycentric coordinates of ActiveEdges::FixNormal of Jolt: +// the edge e is opposite to the vertex e+2 +func touchedEdges(vertices *[3]mgl64.Vec3, point mgl64.Vec3) uint8 { + u, v, w := barycentric(point, vertices[0], vertices[1], vertices[2]) + switch { + case u > 1-edgeBarycentric: + return 0b101 + case v > 1-edgeBarycentric: + return 0b011 + case w > 1-edgeBarycentric: + return 0b110 + case u < edgeBarycentric: + return 0b010 + case v < edgeBarycentric: + return 0b100 + case w < edgeBarycentric: + return 0b001 + } + return 0 +} + +// contactNormal: the normal of the contact of a body with a triangle, given the normal of EPA and the edges of the +// triangle its point is on (touchedEdges). A body sliding on the terrain must not hit the edges between its triangles: +// on an inactive edge (between 2 triangles almost flat, or bent inwards) the normal is the one of the triangle. The rule +// of ActiveEdges::FixNormal of Jolt (ActiveEdges.h). The normal of EPA is kept: +// - if the body moves (movement, a unit direction, or zero), and the normal of EPA brakes its motion less than the +// normal of the triangle: the body grazes a triangle it can't reach by its face (a capsule level with the top edge +// of a riser, whose closest point is on the flat diagonal of the riser: with the normal of the riser it would be a +// wall at a distance of 0, where its real distance is along the normal of EPA), or leaves a triangle. The hint of +// Jolt, "to make a distinction between sliding over a horizontal triangulated grid and hitting an edge ... and +// grazing a vertical triangle with an inactive edge" +// - if the 3 edges of the triangle are active +// - if the triangle has an active edge, and the normal of EPA is closer than 1° to its normal +// - if the point is on an active edge, or on a vertex of an active edge +// +// The pairs of a step give no movement: Jolt keeps the normal of EPA when it brakes the motion of the body less than +// the normal of the triangle (a body leaving a triangle is not held by its plane): its margin is fixed (2 cm, +// mSpeculativeContactDistance), the speculative margin of Feather grows with the speed of the body (reach), and +// brings the vertices and the edges of the triangles around within it. A box falling flat on a flat terrain, straight +// on a vertex of the grid, had 5 contacts at the same corner: one straight up, 4 on the edges of the triangles around, +// tilted by 10 to 60°, each braking a vertical fall less than the plane. The tilted ones, nearly parallel, took impulse +// from the vertical one over the sub-steps, and the box landed 2 mm deep. The character (character_move.go) gives the +// direction of its velocity: its contacts are gathered within 10 cm whatever its speed, and the rule is the one of +// CharacterVirtual (CheckCollision passes the movement direction). PhysX and Box3D have no such rule +// (PCMContactConvexMesh, b3ComputeMeshManifolds). A box sliding across an active ridge is not held by the plane it +// leaves: the points cut on an active edge are never points of the face (clipFace) +// +// activeEdges: bit e for the edge from the vertex e to the vertex e+1. The normals point from the triangle to the body: +// the ones of Jolt point into the triangle, its test reads the other way +func contactNormal(faceNormal mgl64.Vec3, activeEdges, touched uint8, normal, movement mgl64.Vec3) mgl64.Vec3 { + if movement.Dot(normal) > movement.Dot(faceNormal) { + return normal + } + if activeEdges == 0b111 { + return normal + } + if activeEdges == 0 { + return faceNormal + } + if normal.Dot(faceNormal) > nearFaceCos || activeEdges&touched != 0 { + return normal + } + return faceNormal +} + +// clipFace adds to s.points the points of the face of the body over the triangle of s.shape, closer to it than the +// margin along the normal. The face is the one the body shows to a plane of this normal (CollideWithPlane, whatever +// the distance): the 4 corners of a face of a box, both ends of a capsule. It is taken without any threshold on its +// alignment with the normal, as the supporting face of Jolt (BoxShape::GetSupportingFace); a capsule gives both its +// ends whatever its tilt, where Jolt asks them within 2 cm along the normal (cCapsuleProjectionSlop): the points under +// its segment are on its surface at any tilt. Seen along the normal, the face is cut by the 3 sides of the triangle +// (Sutherland-Hodgman): what is left is the part of the face over the triangle, the same polygon as the triangle cut +// by the sides of the face (ManifoldBetweenTwoFaces of Jolt, ClipPolyVsPoly). A segment is cut the same way, as +// b3CollideTriangleAndCapsule of Box3D: ClipPolyVsEdge of Jolt can give a point out of the triangle when the capsule +// ends beside it. Each vertex left is a point of the body, at its distance to the plane of the triangle along the +// normal. +// +// bodyVertices: the vertices of the face over the triangle are kept: the points of the body against a plane. +// cutSides: the points cut on these sides of the triangle are kept (bit e for the edge from the vertex e to the vertex +// e+1): a ridge of the terrain in the face of a box, across the side of a capsule, is found where it crosses them. +// Jolt keeps all the points of the polygon. The points cut on a flat edge are between vertices of the body, on the +// same plane: they say nothing more, and take the place of a corner among the 4 points of a manifold +func (s *triangleScratch) clipFace(object *actor.RigidBody, normal, faceNormal mgl64.Vec3, bodyVertices bool, cutSides uint8, margin float64) { + vertices := &s.shape.vertices + alignment := normal.Dot(faceNormal) + if alignment <= 0 { + // the body touches the triangle from its side: no face over it + return + } + s.plane = object.Shape.CollideWithPlane(normal, -normal.Dot(vertices[0]), object.Transform, math.Inf(1), s.plane[:0]) + if len(s.plane) < 2 || len(s.plane) > maxClipVertices-3 { + return + } + face, sides, count := &s.clip[0], &s.sides[0], len(s.plane) + for i, point := range s.plane { + // Position is halfway between the body and the plane + face[i], sides[i] = point.Position.Add(normal.Mul(point.Separation/2)), 0 + } + clipped, clippedSides := &s.clip[1], &s.sides[1] + for e := 0; e < 3 && count > 0; e++ { + // the side of the triangle along the normal, its inner side + inward := normal.Cross(vertices[(e+1)%3].Sub(vertices[e])) + count = clipAgainstSide(face[:count], sides, vertices[e], inward, 1<= -epsilon { + out[0], outSides[0], count = in[0], inSides[0], 1 + } + return count + } + for i := 0; i < edges; i++ { + j := (i + 1) % len(in) + current, next := in[i], in[j] + distance, nextDistance := current.Sub(point).Dot(inward), next.Sub(point).Dot(inward) + if distance >= -epsilon { + out[count], outSides[count] = current, inSides[i] + count++ + } + if (distance >= -epsilon) != (nextDistance >= -epsilon) { + out[count] = current.Add(next.Sub(current).Mul(distance / (distance - nextDistance))) + outSides[count] = side | (inSides[i] & inSides[j]) + count++ + } + if len(in) == 2 && nextDistance >= -epsilon { + out[count], outSides[count] = next, inSides[j] + count++ + } + } + return count +} + +// reducePatch adds to m 4 points of the patch at most. The points of a patch come from several triangles. The steps of +// Reduce (the ones of b3ReduceManifoldPoints of Box3D): the deepest point, the furthest from it, then the points adding +// the most area. 2 differences, for the points of several triangles: +// - among the points as deep as the deepest (EPATieTolerance), the first one is the furthest along a tangent, then +// along the other (the first step of Box3D looks for an extreme point along a tangent): an end of the contact, +// never a point inside it +// - a point is added if it widens the contact by SpeculativeDistance at least (Box3D has the same tolerance, on the +// area), and the best one wins, without the bias Reduce gives to the order of its points: the order of the points +// of a patch is the order of its triangles +func reducePatch(points []constraint.ContactPoint, normal mgl64.Vec3, m *constraint.Manifold) { + if len(points) <= constraint.MaxContactPoints { + for _, p := range points { + m.Add(p.Position, p.Separation) + } + return + } + + // ========== THE DEEPEST, AT AN END ========== + tangent := normal.Cross(mgl64.Vec3{1, 0, 0}) + if math.Abs(normal.X()) > 0.5 { + tangent = normal.Cross(mgl64.Vec3{0, 0, 1}) + } + tangent = tangent.Normalize() + bitangent := normal.Cross(tangent) + lowest := math.Inf(1) + for _, p := range points { + lowest = math.Min(lowest, p.Separation) + } + first := -1 + for i, p := range points { + if p.Separation > lowest+epa.EPATieTolerance { + continue + } + if first < 0 { + first = i + continue + } + along := p.Position.Sub(points[first].Position).Dot(tangent) + if along > weldDistance || (along >= -weldDistance && p.Position.Sub(points[first].Position).Dot(bitangent) > 0) { + first = i + } + } + m.Add(points[first].Position, points[first].Separation) + + // ========== THE WIDEST CONTACT ========== + planar := func(a, b int) mgl64.Vec3 { + offset := points[b].Position.Sub(points[a].Position) + return offset.Sub(normal.Mul(offset.Dot(normal))) + } + // area: twice the area of the triangle, seen along the normal + area := func(a, b, c int) float64 { return planar(a, b).Cross(planar(a, c)).Dot(normal) } + kept := [constraint.MaxContactPoints]int{first} + orientation := 0.0 + for m.Count < constraint.MaxContactPoints { + chosen, highest := -1, 0.0 + for i := range points { + // how much the point widens the contact of the points kept, and if it does by SpeculativeDistance + value, widens := 0.0, false + switch m.Count { + case 1: + value = planar(first, i).LenSqr() + widens = value > SpeculativeDistance*SpeculativeDistance + case 2: + // its distance to the line of the 2 points + value = math.Abs(area(kept[0], kept[1], i)) + widens = value > SpeculativeDistance*planar(kept[0], kept[1]).Len() + default: + // the area added outside an edge of the triangle, by a point further than the tolerance from this edge + for e := 0; e < 3; e++ { + from, to := kept[e], kept[(e+1)%3] + outside := -orientation * area(from, to, i) + value = math.Max(value, outside) + widens = widens || outside > SpeculativeDistance*planar(from, to).Len() + } + } + if widens && value > highest { + chosen, highest = i, value + } + } + if chosen < 0 { + return + } + kept[m.Count] = chosen + m.Add(points[chosen].Position, points[chosen].Separation) + if m.Count == 3 { + orientation = math.Copysign(1, area(kept[0], kept[1], kept[2])) + } + } +} + +// barycentric coordinates of the projection of p on the triangle +func barycentric(p, a, b, c mgl64.Vec3) (float64, float64, float64) { + v0, v1, v2 := b.Sub(a), c.Sub(a), p.Sub(a) + d00, d01, d11 := v0.Dot(v0), v0.Dot(v1), v1.Dot(v1) + d20, d21 := v2.Dot(v0), v2.Dot(v1) + denominator := d00*d11 - d01*d01 + if denominator == 0 { + return 1, 0, 0 + } + v := (d11*d20 - d01*d21) / denominator + w := (d00*d21 - d01*d20) / denominator + return 1 - v - w, v, w +} + +// weld adds the point, unless the patch already has it +func weld(points []constraint.ContactPoint, point constraint.ContactPoint) []constraint.ContactPoint { + for i := range points { + if points[i].Position.Sub(point.Position).LenSqr() < weldDistance*weldDistance { + points[i].Separation = math.Min(points[i].Separation, point.Separation) + return points + } + } + return append(points, point) +} + +func triangleAABB(vertices [3]mgl64.Vec3) actor.AABB { + aabb := actor.AABB{Min: vertices[0], Max: vertices[0]} + for _, v := range vertices[1:] { + for k := 0; k < 3; k++ { + aabb.Min[k] = math.Min(aabb.Min[k], v[k]) + aabb.Max[k] = math.Max(aabb.Max[k], v[k]) + } + } + return aabb +} + +// localBounds: the world AABB seen in the local space of the transform (the AABB of its 8 corners) +func localBounds(transform actor.Transform, aabb actor.AABB) actor.AABB { + min := mgl64.Vec3{math.Inf(1), math.Inf(1), math.Inf(1)} + max := mgl64.Vec3{math.Inf(-1), math.Inf(-1), math.Inf(-1)} + for i := 0; i < 8; i++ { + corner := aabb.Min + if i&1 != 0 { + corner[0] = aabb.Max[0] + } + if i&2 != 0 { + corner[1] = aabb.Max[1] + } + if i&4 != 0 { + corner[2] = aabb.Max[2] + } + local := transform.ToLocal(corner) + for k := 0; k < 3; k++ { + min[k] = math.Min(min[k], local[k]) + max[k] = math.Max(max[k], local[k]) + } + } + return actor.AABB{Min: min, Max: max} +} diff --git a/contact_events_test.go b/contact_events_test.go new file mode 100644 index 0000000..8ea3e3e --- /dev/null +++ b/contact_events_test.go @@ -0,0 +1,153 @@ +package feather + +import ( + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// The collision events, the sleep and the removal: a contact ends when the bodies no longer touch, or when one of them +// leaves the world, never because a body falls asleep on another one, static or not (Box2D v3 keeps the contacts of a +// sleeping body touching and ends them in b2DestroyContact with a b2ContactEndTouchEvent, contact.c; PhysX sends +// eNOTIFY_TOUCH_LOST when an actor is removed; Unity sends no collision event while a body sleeps) + +// liftSpeed: a crate thrown up at this speed leaves the ground (m/s) +const liftSpeed = 3.0 + +// groundKinds: a crate lands on a plane, or on a static box +var groundKinds = []string{"plane", "box"} + +// groundScene: a ground of the kind, and a crate dropped on it +func groundScene(kind string) (*World, *actor.RigidBody, *actor.RigidBody) { + w := newScene(1) + var ground *actor.RigidBody + if kind == "plane" { + ground = addGround(w, 0.5) + } else { + ground = addBody(w, mgl64.Vec3{0, -0.5, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}, actor.BodyTypeStatic, 0.5, 0) + } + crate := addBody(w, mgl64.Vec3{0, crateDrop, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + return w, ground, crate +} + +// A crate asleep on a static ground keeps its contact: one enter, no exit and no stay during 10 s of sleep; woken up +// and thrown up it leaves once, and the waking sends no enter +func TestACrateAsleepOnTheGroundKeepsItsContact(t *testing.T) { + for _, kind := range groundKinds { + t.Run(kind, func(t *testing.T) { + w, ground, crate := groundScene(kind) + log := listenTriggers(w).listenCollisions() + simulate(w, settleTime, nil) + if !crate.IsSleeping { + t.Fatalf("the crate is awake after %v s: the scene doesn't test the sleep", settleTime) + } + stays := log.count(EventCollisionStay, ground, crate) + simulate(w, stayTime, nil) + if enter := log.count(EventCollisionEnter, ground, crate); enter != 1 { + t.Errorf("%d enters, want 1", enter) + } + if exit := log.count(EventCollisionExit, ground, crate); exit != 0 { + t.Errorf("%d exits while the crate sleeps on the ground, want 0", exit) + } + if more := log.count(EventCollisionStay, ground, crate) - stays; more != 0 { + t.Errorf("%d stays while the crate sleeps on the ground, want 0", more) + } + + crate.WakeUp() + crate.Velocity = mgl64.Vec3{0, liftSpeed, 0} + simulate(w, 0.1, nil) + if enter := log.count(EventCollisionEnter, ground, crate); enter != 1 { + t.Errorf("%d enters once the crate woke up, want 1: the contact was kept", enter) + } + if exit := log.count(EventCollisionExit, ground, crate); exit != 1 { + t.Errorf("%d exits once the crate was thrown up, want 1", exit) + } + }) + } +} + +// A crate removed from the world while it rests on the ground ends its contact: the exit is sent at the next step, +// once, asleep or awake +func TestARemovedCrateEndsItsContact(t *testing.T) { + for _, asleep := range []bool{true, false} { + name := "awake" + if asleep { + name = "asleep" + } + t.Run(name, func(t *testing.T) { + w, ground, crate := groundScene("box") + log := listenTriggers(w).listenCollisions() + seconds := settleTime + if !asleep { + seconds = 1 + } + simulate(w, seconds, nil) + if crate.IsSleeping != asleep || log.count(EventCollisionEnter, ground, crate) != 1 { + t.Fatalf("the crate is not on the ground (asleep %v, want %v)", crate.IsSleeping, asleep) + } + before := log.count(EventCollisionExit, ground, crate) + w.RemoveBody(crate) + w.Step(sceneDt) + if exit := log.count(EventCollisionExit, ground, crate) - before; exit != 1 { + t.Errorf("%d exits at the step after the removal of the crate, want 1", exit) + } + simulate(w, 1, nil) + if exit := log.count(EventCollisionExit, ground, crate) - before; exit != 1 { + t.Errorf("%d exits a second later, want 1", exit) + } + }) + } +} + +// The ground removed from the world ends the contact of each crate asleep on it, once; a crate in the air has none +func TestARemovedGroundEndsItsContacts(t *testing.T) { + w, ground, crate := groundScene("box") + other := addBody(w, mgl64.Vec3{1, crateDrop, 1}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + log := listenTriggers(w).listenCollisions() + simulate(w, settleTime, nil) + flying := addBody(w, mgl64.Vec3{0, 10, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + w.Step(sceneDt) + before := map[*actor.RigidBody]int{} + for _, body := range []*actor.RigidBody{crate, other} { + before[body] = log.count(EventCollisionExit, ground, body) + } + w.RemoveBody(ground) + w.Step(sceneDt) + for _, body := range []*actor.RigidBody{crate, other} { + if exit := log.count(EventCollisionExit, ground, body) - before[body]; exit != 1 { + t.Errorf("%d exits of a crate after the removal of the ground, want 1", exit) + } + } + if exit := log.count(EventCollisionExit, ground, flying); exit != 0 { + t.Errorf("%d exits of a crate which never touched the ground, want 0", exit) + } +} + +// A listener which removes the crate as it lands gets its exit in the same flush +func TestARemovalDuringTheCollisionEventsIsSent(t *testing.T) { + w, ground, crate := groundScene("box") + log := listenTriggers(w).listenCollisions() + w.Events.Subscribe(EventCollisionEnter, func(Event) { w.RemoveBody(crate) }) + enterStep := uint32(0) + simulate(w, settleTime, func() { + if enterStep == 0 && log.count(EventCollisionEnter, ground, crate) > 0 { + enterStep = w.step + } + }) + if enterStep == 0 { + t.Fatalf("the crate never landed") + } + exits := 0 + for _, record := range log.events { + if record.eventType == EventCollisionExit && pairOf(record, ground, crate) { + exits++ + if record.step != enterStep { + t.Errorf("the exit was sent at the step %d, the crate was removed at the step %d", record.step, enterStep) + } + } + } + if exits != 1 { + t.Errorf("%d exits, want 1", exits) + } +} diff --git a/determinism_test.go b/determinism_test.go new file mode 100644 index 0000000..7fc209d --- /dev/null +++ b/determinism_test.go @@ -0,0 +1,506 @@ +package feather + +import ( + "fmt" + "math" + "math/rand" + "slices" + "sync/atomic" + "testing" + "time" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// Determinism: the same inputs give the same state bit for bit, run after run and whatever the number of Workers. +// The states are compared by their bits (math.Float64bits), not by ==: -0 and 0 differ, a NaN equals itself. +// +// A step runs on several goroutines from minParallelBodies (256) bodies only, and each stage only when it has more +// items than a chunk. Both scenes below are checked to reach the parallel path of every stage (parallelStages): a scene +// which stops reaching one fails, it would prove nothing about it. +// - "mixed": 200 bodies, under the threshold, so World.parallelFrom forces the parallel paths; +// - "pile": 600 bodies, over the threshold, nothing forced: the paths of a game. + +const ( + // determinismSteps: the steps of the mixed scene (16.7 s: the bodies fall, bounce and pile up, some fall asleep) + determinismSteps = 1000 + + // raceSteps: the steps of the mixed scene under the race detector (10 to 20 times slower), which also plays each + // count of workers once: it looks for the data races, the bits are compared without it + raceSteps = 200 + + // determinismBodies: the bodies of the mixed scene, the ground included + determinismBodies = 200 + + // pileBodies & pileSteps: the pile over minParallelBodies, long enough to land and rest + pileBodies = 600 + pileSteps = 180 +) + +// determinismWorkers: the reference is the first run with 1 worker +var determinismWorkers = []int{1, 4, 8} + +// mixedScene: 200 bodies on a plane. 3 chains of 10 capsules (ball joints) hang from static anchors, 12 pairs of boxes +// are linked (hinge, distance & fixed joints), and boxes, spheres & capsules fall on them and bounce (restitution 0.5). +// The same scene whatever the workers +func mixedScene(workers int) *World { + w := newScene(workers) + w.parallelFrom = 1 + addGround(w, 0.6) + r := rand.New(rand.NewSource(804)) + + const links, halfHeight, radius = 10, 0.2, 0.05 + for chain := 0; chain < 3; chain++ { + top := mgl64.Vec3{float64(chain) - 1, 4.5, 0} + previous := addBody(w, top, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.02}, actor.BodyTypeStatic, 0, 0) + for i := 0; i < links; i++ { + anchor := top.Sub(mgl64.Vec3{0, float64(i) * 2 * halfHeight, 0}) + capsule := addBody(w, anchor.Sub(mgl64.Vec3{0, halfHeight, 0}), mgl64.QuatIdent(), &actor.Capsule{HalfHeight: halfHeight, Radius: radius}, actor.BodyTypeDynamic, 0.5, 0) + w.AddJoint(NewBallJoint(previous, capsule, anchor, mgl64.Vec3{0, -1, 0})) + previous = capsule + } + } + + for pair := 0; pair < 12; pair++ { + center := mgl64.Vec3{r.Float64()*3 - 1.5, 5 + float64(pair)*0.6, r.Float64()*3 - 1.5} + a := addBody(w, center.Sub(mgl64.Vec3{0.3, 0, 0}), mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0.5) + b := addBody(w, center.Add(mgl64.Vec3{0.3, 0, 0}), mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0.5) + switch pair % 3 { + case 0: + w.AddJoint(NewHingeJoint(a, b, center, mgl64.Vec3{0, 0, 1})) + case 1: + w.AddJoint(NewDistanceJoint(a, b, a.Transform.Position, b.Transform.Position)) + case 2: + w.AddJoint(NewFixedJoint(a, b, center)) + } + } + + for i := 0; len(w.Bodies) < determinismBodies; i++ { + rotation := mgl64.QuatRotate(r.Float64()*math.Pi, mgl64.Vec3{r.Float64(), r.Float64(), r.Float64()}.Normalize()) + var shape actor.ShapeInterface = cube() + switch i % 3 { + case 1: + shape = &actor.Sphere{Radius: cubeHalf} + case 2: + shape = &actor.Capsule{HalfHeight: 0.2, Radius: 0.15} + } + position := mgl64.Vec3{r.Float64()*3 - 1.5, 1 + float64(i)*0.15, r.Float64()*3 - 1.5} + addBody(w, position, rotation, shape, actor.BodyTypeDynamic, 0.6, 0.5) + } + return w +} + +// stateBits appends the bits of the whole state the next step starts from. The bodies in the order of World.Bodies +// (position, rotation, velocities, sleep), then the contacts in their order (indices of their bodies, normal, points, +// impulses of the warm start) +func stateBits(w *World, bits []uint64) []uint64 { + vector := func(v mgl64.Vec3) { + bits = append(bits, math.Float64bits(v[0]), math.Float64bits(v[1]), math.Float64bits(v[2])) + } + for _, body := range w.Bodies { + vector(body.Transform.Position) + bits = append(bits, math.Float64bits(body.Transform.Rotation.W)) + vector(body.Transform.Rotation.V) + vector(body.Velocity) + vector(body.AngularVelocity) + sleeping := uint64(0) + if body.IsSleeping { + sleeping = 1 + } + bits = append(bits, sleeping) + } + for i := range w.Contacts() { + contact := &w.Contacts()[i] + bits = append(bits, uint64(contact.IndexA), uint64(contact.IndexB), uint64(contact.Count)) + vector(contact.Normal) + for _, point := range contact.Points[:contact.Count] { + vector(point.Position) + bits = append(bits, math.Float64bits(point.Separation), math.Float64bits(point.NormalImpulse)) + } + vector(contact.FrictionImpulse) + bits = append(bits, math.Float64bits(contact.TwistImpulse)) + } + return bits +} + +// hashBits: FNV-1a over the values, to keep the state of every step +func hashBits(bits []uint64) uint64 { + const offset, prime = 14695981039346656037, 1099511628211 + hash := uint64(offset) + for _, value := range bits { + hash = (hash ^ value) * prime + } + return hash +} + +// parallelStages follows which stages of the steps of a World ran on several goroutines (workerPool.run splits a stage +// with more items than its chunk) +type parallelStages struct { + world *World + // the chunks of the pair search are only known during the step: its jobs are wrapped + queries, scans atomic.Bool + // the largest count of items of each other stage, read after each step + bodies, pairs, contacts, color int +} + +func watchParallelStages(w *World) *parallelStages { + stages := ¶llelStages{world: w} + w.tree.queryJob = func(chunk int) { + if chunk > 0 { + stages.queries.Store(true) + } + w.tree.query(chunk) + } + w.tree.scanJob = func(chunk int) { + if chunk > 0 { + stages.scans.Store(true) + } + w.tree.scan(chunk) + } + return stages +} + +// afterStep reads the sizes of the stages of the last step +func (stages *parallelStages) afterStep() { + w := stages.world + stages.bodies = max(stages.bodies, len(w.Bodies)) + stages.pairs = max(stages.pairs, len(w.pairs)) + stages.contacts = max(stages.contacts, len(w.Contacts())) + for k := range w.solver.graph.colors { + stages.color = max(stages.color, len(w.solver.graph.colors[k].items)) + } +} + +// missing: the stages which never had enough items to be split between the workers +func (stages *parallelStages) missing() []string { + var missing []string + check := func(reached bool, stage string) { + if !reached { + missing = append(missing, stage) + } + } + pool := stages.world.workerPool() + check(pool.helpers == stages.world.Workers-1 && pool.generation.Load() > 0, "the workers (none was given a stage)") + check(stages.queries.Load(), "the pairs of the moved bodies (Tree.query)") + check(stages.scans.Load(), "the pairs of the step (Tree.scan)") + check(stages.bodies > bodiesChunk, "the AABBs & the integration of the bodies") + check(stages.pairs > pairsPerChunk, "the narrow phase") + check(stages.contacts > constraintsChunk, "the preparation of the contacts") + check(stages.color > constraintsChunk, "the solver (no color larger than a chunk)") + return missing +} + +// runDeterministic plays the steps of a scene and returns the hash of its state after each step, and its final state +func runDeterministic(t *testing.T, w *World, steps int, each func(step int)) ([]uint64, []uint64) { + t.Helper() + defer w.Close() + stages := watchParallelStages(w) + hashes := make([]uint64, 0, steps) + var bits []uint64 + for step := 0; step < steps; step++ { + if each != nil { + each(step) + } + w.Step(sceneDt) + stages.afterStep() + bits = stateBits(w, bits[:0]) + hashes = append(hashes, hashBits(bits)) + } + if w.Workers > 1 { + if missing := stages.missing(); len(missing) > 0 { + t.Fatalf("workers=%d: stages never run in parallel, the scene proves nothing about them: %v", w.Workers, missing) + } + } + for _, body := range w.Bodies { + if !finite(body.Transform.Position) || !finite(body.Velocity) { + t.Fatalf("workers=%d: the scene exploded: %v", w.Workers, body.Transform.Position) + } + } + return hashes, bits +} + +// sameRun fails at the first step whose state differs from the reference, then names the first value of the final +// state which differs +func sameRun(t *testing.T, what string, hashes, final, referenceHashes, referenceFinal []uint64) { + t.Helper() + for step := range referenceHashes { + if hashes[step] != referenceHashes[step] { + t.Fatalf("%s: the state differs from the reference from the step %d of %d", what, step, len(referenceHashes)) + } + } + if len(final) != len(referenceFinal) { + t.Fatalf("%s: %d values in the final state, the reference has %d", what, len(final), len(referenceFinal)) + } + for i := range referenceFinal { + if final[i] != referenceFinal[i] { + t.Fatalf("%s: value %d of the final state is %#x, the reference has %#x", what, i, final[i], referenceFinal[i]) + } + } +} + +// compareRuns plays the scene twice with each count of workers (once under the race detector): every run gives the +// states of the first one +func compareRuns(t *testing.T, steps int, scene func(workers int) (*World, func(step int))) { + t.Helper() + runs := 2 + if raceEnabled { + runs = 1 + } + var referenceHashes, referenceFinal []uint64 + for _, workers := range determinismWorkers { + for run := 1; run <= runs; run++ { + w, each := scene(workers) + hashes, final := runDeterministic(t, w, steps, each) + if referenceHashes == nil { + referenceHashes, referenceFinal = hashes, final + continue + } + sameRun(t, fmt.Sprintf("workers=%d, run %d", workers, run), hashes, final, referenceHashes, referenceFinal) + } + } +} + +// 200 bodies (a pile bouncing on chains and on linked boxes), 1000 steps: the state after every step is the same bit for +// bit between two runs, and with 1, 4 and 8 workers. The scene covers the sleep: some bodies fall asleep during the run +// (and some wake up again: the fast bodies of the scene hit them) +func TestMixedSceneIsDeterministic(t *testing.T) { + steps := determinismSteps + if raceEnabled { + steps = raceSteps + } + asleep, moving := 0, 0 + compareRuns(t, steps, func(workers int) (*World, func(step int)) { + w := mixedScene(workers) + if len(w.Bodies) != determinismBodies || len(w.Joints) == 0 { + t.Fatalf("%d bodies & %d joints, want %d bodies and joints", len(w.Bodies), len(w.Joints), determinismBodies) + } + if workers == determinismWorkers[0] { + asleep, moving = 0, 0 + } + return w, func(step int) { + if workers != determinismWorkers[0] { + return + } + // the most bodies asleep at the start of a step, over the run + sleeping, awake := 0, 0 + for _, body := range w.Bodies { + if body.IsSleeping { + sleeping++ + } else if body.BodyType == actor.BodyTypeDynamic { + awake++ + } + } + if sleeping > asleep { + asleep, moving = sleeping, awake + } + } + }) + t.Logf("at most %d bodies asleep at a step, %d dynamic bodies awake then", asleep, moving) + if asleep == 0 { + t.Error("no body fell asleep: the scene doesn't cover the sleep") + } +} + +// 600 bodies, over minParallelBodies: nothing forces the parallel paths, they run as in a game +func TestLargePileIsDeterministic(t *testing.T) { + compareRuns(t, pileSteps, func(workers int) (*World, func(step int)) { + w := benchScene(pileBodies, workers) + if w.parallelFrom != 0 || len(w.Bodies) < minParallelBodies { + t.Fatalf("%d bodies, parallelFrom %d: the scene must be parallel by itself", len(w.Bodies), w.parallelFrom) + } + return w, nil + }) +} + +// consistentOrder: the order of World.Bodies is the one of the whole step. The proxies of the broad phase are those of +// the bodies at the same index, the pairs are sorted by the indices of their bodies, and the contacts follow the pairs +// (that the pairs are those of a search from scratch is TestTreePairsAsBruteForce). Returns the first gap +func consistentOrder(w *World) error { + tree := &w.tree + if !slices.Equal(tree.bodies, w.Bodies) || len(tree.proxies) != len(w.Bodies) { + return fmt.Errorf("the tree has %d bodies & %d proxies in another order than the %d bodies of the World", len(tree.bodies), len(tree.proxies), len(w.Bodies)) + } + planes := 0 + for i, body := range w.Bodies { + p := tree.proxies[i] + if p.kind != kindOf(body) { + return fmt.Errorf("body %d: proxy of kind %d, want %d", i, p.kind, kindOf(body)) + } + switch p.kind { + case proxyLarge: + if planes >= len(tree.planes) || tree.planes[planes] != int32(i) { + return fmt.Errorf("body %d: not the large body %d of the tree (%v)", i, planes, tree.planes) + } + planes++ + case proxyDynamic: + if owner := tree.dynamics.nodes[p.node].body; owner != int32(i) { + return fmt.Errorf("body %d: its leaf belongs to the body %d", i, owner) + } + case proxyStatic: + if owner := tree.statics.nodes[p.node].body; owner != int32(i) { + return fmt.Errorf("body %d: its leaf belongs to the body %d", i, owner) + } + } + } + if planes != len(tree.planes) { + return fmt.Errorf("%d large bodies in the tree, %d in the World", len(tree.planes), planes) + } + + // the pairs: by the index of their first body (the one which is not a plane, of the lowest index), its planes first + // in their order, then its other bodies in their order. Tree.sortPairs is not asked: the indices are read in + // World.Bodies + previous := [3]int{-1, 0, 0} + for i, pair := range w.pairs { + a, b := slices.Index(w.Bodies, pair.BodyA), slices.Index(w.Bodies, pair.BodyB) + if a < 0 || b < 0 || int(pair.IndexA) != a || int(pair.IndexB) != b { + return fmt.Errorf("pair %d: its indices %d & %d are not those of its bodies (%d & %d)", i, pair.IndexA, pair.IndexB, a, b) + } + key := [3]int{min(a, b), 1, max(a, b)} + if isLarge(pair.BodyA) { + key = [3]int{b, 0, a} + } else if isLarge(pair.BodyB) { + key = [3]int{a, 0, b} + } + if slices.Compare(key[:], previous[:]) <= 0 { + return fmt.Errorf("pair %d (bodies %d & %d): not after the pair %d in the order of the bodies", i, a, b, i-1) + } + previous = key + } + next := 0 + for i := range w.Contacts() { + contact := &w.Contacts()[i] + for next < len(w.pairs) && (w.pairs[next].BodyA != contact.BodyA || w.pairs[next].BodyB != contact.BodyB) { + next++ + } + if next == len(w.pairs) { + return fmt.Errorf("contact %d: out of the order of the pairs", i) + } + } + return nil +} + +// Bodies removed then added again keep a defined order: the bodies left keep theirs (they move up), a body added goes +// last, whatever its former index. The broad phase, the pairs & the contacts follow this order at every step, and the +// whole run is the same bit for bit between two runs and with 1, 4 and 8 workers. +// +// The bodies leave from the middle of the pile, from a chain (its joints leave with it) and from the end, then while +// they sleep; they come back 1 s later with their joints, in another order than they had +func TestRemovedAndAddedBodiesKeepADefinedOrder(t *testing.T) { + const removeAt, addAt, removeAsleepAt, addAsleepAt, steps = 90, 150, 300, 360, 450 + // the indices of the bodies removed first: a link in the middle of the first chain, the first box of a hinge, bodies + // of the pile, the last body + leaving := []int{5, 34, 80, 81, 150, determinismBodies - 1} + // sleepersLeaving: the count of sleeping bodies removed the second time, with the body in the middle of the World + const sleepersLeaving = 4 + + compareRuns(t, steps, func(workers int) (*World, func(step int)) { + w := mixedScene(workers) + var removed []*actor.RigidBody + var joints []Joint + + remove := func(step int, indices []int) { + before := slices.Clone(w.Bodies) + removed, joints = removed[:0], joints[:0] + // from the last one: the indices before it don't move + for i := len(indices) - 1; i >= 0; i-- { + body := w.Bodies[indices[i]] + for _, joint := range w.Joints { + if base := joint.base(); base.BodyA == body || base.BodyB == body { + joints = append(joints, joint) + } + } + removed = append(removed, body) + w.RemoveBody(body) + } + want := slices.DeleteFunc(before, func(body *actor.RigidBody) bool { return slices.Contains(removed, body) }) + if !slices.Equal(w.Bodies, want) { + t.Fatalf("workers=%d, step %d: the bodies left changed of order", workers, step) + } + for _, joint := range joints { + if slices.Contains(w.Joints, joint) { + t.Fatalf("workers=%d, step %d: a joint of a removed body is still in the World", workers, step) + } + } + } + add := func(step int) { + before := slices.Clone(w.Bodies) + // in the order they left: the last body of the World first + for _, body := range removed { + w.AddBody(body) + } + for _, joint := range joints { + w.AddJoint(joint) + } + if !slices.Equal(w.Bodies, append(before, removed...)) { + t.Fatalf("workers=%d, step %d: the bodies added are not the last ones, in the order of AddBody", workers, step) + } + } + + return w, func(step int) { + if step > 0 { + // the step before: its pairs & contacts follow the order of the bodies + if err := consistentOrder(w); err != nil { + t.Fatalf("workers=%d, step %d: %v", workers, step-1, err) + } + } + switch step { + case removeAt: + remove(step, leaving) + case removeAsleepAt: + indices := []int{len(w.Bodies) / 2} + for i, body := range w.Bodies { + if body.IsSleeping && i != indices[0] && len(indices) <= sleepersLeaving { + indices = append(indices, i) + } + } + if len(indices) <= sleepersLeaving { + t.Fatalf("workers=%d: %d bodies asleep at the step %d, want %d to remove", workers, len(indices)-1, step, sleepersLeaving) + } + slices.Sort(indices) + remove(step, indices) + case addAt, addAsleepAt: + add(step) + } + } + }) +} + +// The cost of Tree.sortPairs on the pairs of a step, as the pair search hands them over (in the order of its chunks), +// while a pile lands (0.5 s after its start: asleep, it has no pair left). "%step" is its share of the steps of the +// scene around this one, with these workers (the sort runs on one goroutine). The figures are in ARCHITECTURE.md +func BenchmarkSortPairs(b *testing.B) { + for _, count := range []int{500, 2000} { + for _, workers := range []int{1, 8} { + b.Run(fmt.Sprintf("%d_bodies_%d_workers", count, workers), func(b *testing.B) { + w := benchScene(count, workers) + defer w.Close() + simulate(w, 0.25, nil) + const measuredSteps = 15 + var step time.Duration + for range measuredSteps { + w.Step(sceneDt) + step += w.Profile().Step + } + var unsorted []Pair + for c := range w.tree.chunks { + unsorted = append(unsorted, w.tree.chunks[c].pairs...) + } + if len(unsorted) == 0 || len(unsorted) != len(w.pairs) { + b.Fatalf("%d pairs in the chunks of the search, %d in the step: nothing to measure", len(unsorted), len(w.pairs)) + } + // sortPairs reads its input and writes Tree.sorted, then swaps them: given back at each run + output := make([]Pair, len(unsorted)) + b.ResetTimer() + for range b.N { + w.tree.sorted = output + w.tree.sortPairs(unsorted, len(w.Bodies)) + } + b.StopTimer() + sort := float64(b.Elapsed().Nanoseconds()) / float64(b.N) + b.ReportMetric(float64(len(unsorted)), "pairs") + b.ReportMetric(100*sort*measuredSteps/float64(step.Nanoseconds()), "%step") + }) + } + } +} diff --git a/epa/epa.go b/epa/epa.go index 73d16a1..6f32785 100644 --- a/epa/epa.go +++ b/epa/epa.go @@ -74,6 +74,14 @@ type polytope struct { var polytopePool = sync.Pool{New: func() any { return &polytope{} }} +// Scratch: the buffers of EPAProxies and of Manifold, for a caller which keeps its own. The functions of the package +// take theirs from a sync.Pool, which the garbage collector empties: a new buffer grows again, an allocation where +// the same work allocated nothing before. A Scratch is used by one goroutine at a time +type Scratch struct { + polytope polytope + features features +} + // EPA computes the penetration of A (+ margin) into B, from the tetrahedron of GJK func EPA(a, b *actor.RigidBody, simplex *gjk.Simplex, margin float64) (Result, error) { proxyA, proxyB := gjk.NewProxy(a), gjk.NewProxy(b) @@ -85,9 +93,21 @@ func EPAProxies(a, b *gjk.Proxy, simplex *gjk.Simplex, margin float64) (Result, if simplex.Count != 4 { return Result{}, ErrNoConvergence } - p := polytopePool.Get().(*polytope) defer polytopePool.Put(p) + return p.penetration(a, b, simplex, margin) +} + +// EPAProxies is EPAProxies with the buffers of the scratch +func (s *Scratch) EPAProxies(a, b *gjk.Proxy, simplex *gjk.Simplex, margin float64) (Result, error) { + if simplex.Count != 4 { + return Result{}, ErrNoConvergence + } + return s.polytope.penetration(a, b, simplex, margin) +} + +// penetration: EPA in the buffers of the polytope +func (p *polytope) penetration(a, b *gjk.Proxy, simplex *gjk.Simplex, margin float64) (Result, error) { p.vertices = p.vertices[:0] p.faces = p.faces[:0] diff --git a/epa/manifold.go b/epa/manifold.go index ed450b0..2ce214e 100644 --- a/epa/manifold.go +++ b/epa/manifold.go @@ -59,13 +59,23 @@ func (p *polygon) add(v mgl64.Vec3) { // The deepest point has the separation of EPA, the other points are higher along the normal. // Points further than the margin are removed, and 4 points are kept at most func Manifold(a, b *actor.RigidBody, result Result, margin float64, m *constraint.Manifold) { + buffers := featuresPool.Get().(*features) + defer featuresPool.Put(buffers) + manifoldIn(a, b, result, margin, m, buffers) +} + +// Manifold is Manifold with the buffers of the scratch +func (s *Scratch) Manifold(a, b *actor.RigidBody, result Result, margin float64, m *constraint.Manifold) { + manifoldIn(a, b, result, margin, m, &s.features) +} + +// manifoldIn: Manifold in the buffers +func manifoldIn(a, b *actor.RigidBody, result Result, margin float64, m *constraint.Manifold, buffers *features) { m.Reset(a, b) normal := result.Normal m.Normal = normal separation := margin - result.Depth - buffers := featuresPool.Get().(*features) - defer featuresPool.Put(buffers) featureA, featureB := &buffers.a, &buffers.b feature(a, normal, featureA) feature(b, normal.Mul(-1), featureB) diff --git a/event.go b/event.go index 16c8191..c38c1c3 100644 --- a/event.go +++ b/event.go @@ -31,6 +31,17 @@ func makePairKey(bodyA, bodyB *actor.RigidBody) pairKey { return pairKey{bodyA: bodyA, bodyB: bodyB} } +// eventPair: a pair of the events, its bodies and its kind (a trigger pair or a contact) when it was recorded: a +// contact whose body is made a trigger by the game ends, and the trigger pair starts +type eventPair struct { + pairKey + trigger bool +} + +func makeEventPair(bodyA, bodyB *actor.RigidBody) eventPair { + return eventPair{pairKey: makePairKey(bodyA, bodyB), trigger: bodyA.IsTrigger || bodyB.IsTrigger} +} + type EventType uint8 // Event interface - all events implement this @@ -108,10 +119,10 @@ type Events struct { // Collision tracking for Enter/Stay/Exit detection // The slices keep the order of the pairs, so the events are always sent in the same order - previousPairs []pairKey - currentPairs []pairKey - previousActivePairs map[pairKey]bool - currentActivePairs map[pairKey]bool + previousPairs []eventPair + currentPairs []eventPair + previousActivePairs map[eventPair]bool + currentActivePairs map[eventPair]bool sleepStates map[*actor.RigidBody]bool } @@ -120,8 +131,8 @@ func NewEvents() Events { return Events{ listeners: make(map[EventType][]EventListener), buffer: make([]Event, 0, 256), - previousActivePairs: make(map[pairKey]bool), - currentActivePairs: make(map[pairKey]bool), + previousActivePairs: make(map[eventPair]bool), + currentActivePairs: make(map[eventPair]bool), sleepStates: make(map[*actor.RigidBody]bool), } } @@ -143,7 +154,7 @@ func (e *Events) recordCollisions(manifolds []constraint.Manifold) []constraint. m := &manifolds[i] isTrigger := m.BodyA.IsTrigger || m.BodyB.IsTrigger if tracked && (isTrigger || m.MinSeparation() <= touchingDistance) { - e.record(makePairKey(m.BodyA, m.BodyB)) + e.record(makeEventPair(m.BodyA, m.BodyB)) } if !isTrigger { manifolds[n] = *m @@ -153,7 +164,7 @@ func (e *Events) recordCollisions(manifolds []constraint.Manifold) []constraint. return manifolds[:n] } -func (e *Events) record(pair pairKey) { +func (e *Events) record(pair eventPair) { if e.currentActivePairs == nil { *e = NewEvents() } @@ -163,13 +174,23 @@ func (e *Events) record(pair pairKey) { } } -// forget a removed body +// forget a removed body. Its pairs end: their exit is sent with the next events (at the end of the next step, or in +// the flush running if a listener removed the body), as Box2D v3 sends a b2SensorEndTouchEvent when the sensor or its +// visitor is destroyed and a b2ContactEndTouchEvent when a touching contact is destroyed (b2DestroyContact), PhysX an +// eNOTIFY_TOUCH_LOST (PxTriggerPair, PxContactPair) and Jolt an OnContactRemoved func (e *Events) forget(body *actor.RigidBody) { delete(e.sleepStates, body) n := 0 for _, pair := range e.previousPairs { if pair.bodyA == body || pair.bodyB == body { delete(e.previousActivePairs, pair) + if pair.trigger { + if e.hasListeners(EventTriggerExit) { + e.buffer = append(e.buffer, TriggerExitEvent{BodyA: pair.bodyA, BodyB: pair.bodyB}) + } + } else if e.hasListeners(EventCollisionExit) { + e.buffer = append(e.buffer, CollisionExitEvent{BodyA: pair.bodyA, BodyB: pair.bodyB}) + } continue } e.previousPairs[n] = pair @@ -183,15 +204,14 @@ func (e *Events) forget(body *actor.RigidBody) { func (e *Events) processCollisionEvents() { // Detect Enter and Stay events for _, pair := range e.currentPairs { - // Skip if both bodies are sleeping, to avoid spamming events - if pair.bodyA.IsSleeping && pair.bodyB.IsSleeping { - continue - } - - isTrigger := pair.bodyA.IsTrigger || pair.bodyB.IsTrigger - + isTrigger := pair.trigger if e.previousActivePairs[pair] { - // Pair was active before and still is, Stay + // Pair was active before and still is, Stay. No stay while both bodies rest (static or asleep): nothing + // changes, as Unity sends no collision stay for a sleeping rigidbody and PhysX tests no trigger pair whose + // actors sleep + if isAtRest(pair) { + continue + } if isTrigger { if e.hasListeners(EventTriggerStay) { e.buffer = append(e.buffer, TriggerStayEvent{ @@ -229,14 +249,17 @@ func (e *Events) processCollisionEvents() { // Detect Exit events for _, pair := range e.previousPairs { - // Sleeping pairs are not detected anymore, but they are still touching - if !e.currentActivePairs[pair] && pair.bodyA.IsSleeping && pair.bodyB.IsSleeping { + isTrigger := pair.trigger + // A contact whose bodies both rest (a body asleep on a static body, or on another sleeping body) is not detected + // anymore, but its bodies still touch: it is kept, as Box2D v3 keeps the contacts of a sleeping body with their + // touching flag (b2ContactEndTouchEvent only when they stop touching or are destroyed), unless one of its bodies + // was made a trigger. A trigger pair is tested whatever the sleep: gone, its shapes no longer overlap + if !isTrigger && !e.currentActivePairs[pair] && isAtRest(pair) && !isTriggerPair(pair.pairKey) { e.record(pair) continue } if !e.currentActivePairs[pair] { // Pair was active but is no longer, Exit - isTrigger := pair.bodyA.IsTrigger || pair.bodyB.IsTrigger if isTrigger { if e.hasListeners(EventTriggerExit) { @@ -290,6 +313,16 @@ func (e *Events) processSleepEvents(bodies []*actor.RigidBody) { } } +// isAtRest: both bodies of the pair rest, static or asleep +func isAtRest(pair eventPair) bool { + return !isAwakeMover(pair.bodyA) && !isAwakeMover(pair.bodyB) +} + +// isTriggerPair: one of the bodies is a trigger +func isTriggerPair(pair pairKey) bool { + return pair.bodyA.IsTrigger || pair.bodyB.IsTrigger +} + // hasListeners: an event is only created if somebody listens to it (creating an event allocates) func (e *Events) hasListeners(eventType EventType) bool { return len(e.listeners[eventType]) > 0 @@ -305,11 +338,13 @@ func (e *Events) tracksCollisions() bool { return false } -// flush sends all buffered events and clears the buffer +// flush sends all buffered events and clears the buffer. A listener can remove a body: the exits of its triggers are +// added to the buffer, and sent in this flush func (e *Events) flush() { e.processCollisionEvents() - for _, event := range e.buffer { + for i := 0; i < len(e.buffer); i++ { + event := e.buffer[i] if listeners, ok := e.listeners[event.Type()]; ok { for _, listener := range listeners { listener(event) diff --git a/event_test.go b/event_test.go index 8fee0e9..c16c82c 100644 --- a/event_test.go +++ b/event_test.go @@ -201,7 +201,7 @@ func TestEvents_RecordCollisions_NormalCollision(t *testing.T) { } // Pair should be recorded - pair := makePairKey(bodyA, bodyB) + pair := makeEventPair(bodyA, bodyB) if !events.currentActivePairs[pair] { t.Error("Normal collision pair should be recorded in currentActivePairs") } @@ -226,7 +226,7 @@ func TestEvents_RecordCollisions_TriggerCollision(t *testing.T) { } // Pair should still be recorded for event generation - pair := makePairKey(bodyA, bodyB) + pair := makeEventPair(bodyA, bodyB) if !events.currentActivePairs[pair] { t.Error("Trigger pair should be recorded in currentActivePairs") } diff --git a/filter_test.go b/filter_test.go index 9240861..1bb1d06 100644 --- a/filter_test.go +++ b/filter_test.go @@ -627,12 +627,7 @@ func TestContinuousRespectsFilter(t *testing.T) { w.Step(sceneDt) // a motion through the wall, as if the solver had accelerated the ball - motion := sweep{start: actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, end: actor.Transform{Position: mgl64.Vec3{10, 0, 0}, Rotation: mgl64.QuatIdent()}} - ball.Transform = motion.end - scratch := ccdPool.Get().(*ccdScratch) - scratch.core.Radius = coreFraction * 0.02 - w.stopAtImpact(ball, &motion, 0.02, scratch) - ccdPool.Put(scratch) + moveThrough(w, ball, sweep{start: actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, end: actor.Transform{Position: mgl64.Vec3{10, 0, 0}, Rotation: mgl64.QuatIdent()}}) if stopped := ball.Transform.Position.X() < 5; stopped != c.stopped { t.Errorf("%s: the ball is at x=%.3f, stopped %v, want %v", c.name, ball.Transform.Position.X(), stopped, c.stopped) } diff --git a/gjk/gjk.go b/gjk/gjk.go index c164b67..c94ada5 100644 --- a/gjk/gjk.go +++ b/gjk/gjk.go @@ -82,6 +82,8 @@ type Proxy struct { Rotation mgl64.Mat3 // local to world Inverse mgl64.Mat3 // world to local Shape Supporter + // upright: the rotation is the identity, the supports skip it + upright bool } func NewProxy(body *actor.RigidBody) Proxy { @@ -133,11 +135,16 @@ func NewProxyAt(transform actor.Transform, shape Supporter) Proxy { 2 * (x*y - w*z), 1 - 2*(x*x+z*z), 2 * (y*z + w*x), 2 * (x*z + w*y), 2 * (y*z - w*x), 1 - 2*(x*x+y*y), } - return Proxy{Position: transform.Position, Rotation: rotation, Inverse: rotation.Transpose(), Shape: shape} + return Proxy{Position: transform.Position, Rotation: rotation, Inverse: rotation.Transpose(), Shape: shape, upright: q.V == mgl64.Vec3{} && q.W == 1} } -// SupportWorld returns the farthest point of the shape in the direction, in world space +// SupportWorld returns the farthest point of the shape in the direction, in world space. An upright shape (the +// identity rotation: a standing capsule, a triangle of a surface) skips the two rotations, which would give the +// same bits func (p *Proxy) SupportWorld(direction mgl64.Vec3) mgl64.Vec3 { + if p.upright { + return p.Position.Add(p.Shape.Support(direction)) + } return p.Position.Add(p.Rotation.Mul3x1(p.Shape.Support(p.Inverse.Mul3x1(direction)))) } diff --git a/heightfield_contact_test.go b/heightfield_contact_test.go new file mode 100644 index 0000000..3b070f1 --- /dev/null +++ b/heightfield_contact_test.go @@ -0,0 +1,535 @@ +package feather + +import ( + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// foldedRidge: a ridge along Z at x = 0 and y = 0, folded by the angle: both slopes go down by half of it. 17x17 +// samples every 0.5 m: the ridge is made of the edges of the triangles from a sample to the next +func foldedRidge(w *World, fold float64) *actor.RigidBody { + const samples = 17 + heights := make([]float32, samples*samples) + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + heights[x*samples+z] = float32(-math.Abs(float64(x)-8) * 0.5 * math.Tan(fold/2)) + } + } + field := actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{0.5, 1, 0.5}) + return addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), field, actor.BodyTypeStatic, 0.6, 0) +} + +// insideBody: how deep the point is in the body (m): its distance to the closest face of a box, to the surface of a +// sphere or of a capsule; 0 out of the body +func insideBody(b *actor.RigidBody, p mgl64.Vec3) float64 { + local := b.Transform.ToLocal(p) + switch shape := b.Shape.(type) { + case *actor.Box: + depth := math.Inf(1) + for k := 0; k < 3; k++ { + depth = math.Min(depth, shape.HalfExtents[k]-math.Abs(local[k])) + } + return math.Max(0, depth) + case *actor.Sphere: + return math.Max(0, shape.Radius-local.Len()) + case *actor.Capsule: + onSegment := mgl64.Vec3{0, mgl64.Clamp(local.Y(), -shape.HalfHeight, shape.HalfHeight), 0} + return math.Max(0, shape.Radius-local.Sub(onSegment).Len()) + } + return 0 +} + +// terrainInBody: how deep the terrain (identity transform) is in the body (m), from the points of its surface every +// spacing under the body: never more than the real depth +func terrainInBody(field *actor.Heightfield, b *actor.RigidBody, spacing float64) float64 { + bounds, depth := b.AABB(), 0.0 + for x := bounds.Min.X(); x <= bounds.Max.X(); x += spacing { + for z := bounds.Min.Z(); z <= bounds.Max.Z(); z += spacing { + if height, ok := field.HeightAt(x, z); ok { + depth = math.Max(depth, insideBody(b, mgl64.Vec3{x, height, z})) + } + } + } + return depth +} + +// terrainContacts: the contacts of the terrain (A) with the body, their deepest separation (+Inf without contact) +func terrainContacts(terrain, b *actor.RigidBody, margin float64) ([]constraint.Manifold, float64) { + var manifolds [MaxManifoldsPerPair]constraint.Manifold + count := CollideAll(terrain, b, margin, manifolds[:]) + deepest := math.Inf(1) + for k := 0; k < count; k++ { + deepest = math.Min(deepest, manifolds[k].MinSeparation()) + } + return manifolds[:count], deepest +} + +// A capsule and a box laid across a ridge rest on it: whatever its fold, the ridge is not in the body by more than +// LinearSlop, and the body stays where it is: balanced on the ridge, or tipped on a slope (its center then moves by +// its height above the ridge times the sine of the slope). Under 5° the ridge is an inactive edge, the body touches +// it by its side or by its face: none of its corners or of its ends +func TestHeightfieldRidge(t *testing.T) { + for _, kind := range []string{"capsule", "box"} { + for _, fold := range []float64{0, 1, 2, 3, 4, 4.9, 5.1, 6, 10} { + w := newScene(1) + field := foldedRidge(w, fold*math.Pi/180).Shape.(*actor.Heightfield) + // across the ridge, 1 mm above it, over the middle of an edge of the ridge + var b *actor.RigidBody + height := 0.15 + if kind == "capsule" { + height = 0.12 + b = addBody(w, mgl64.Vec3{0, 0.121, 0.1}, mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}), &actor.Capsule{HalfHeight: 0.25, Radius: 0.12}, actor.BodyTypeDynamic, 0.6, 0) + } else { + b = addBody(w, mgl64.Vec3{0, 0.151, 0.1}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.15, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + } + start, worst := b.Transform.Position, 0.0 + simulate(w, 4, func() { worst = math.Max(worst, terrainInBody(field, b, 0.01)) }) + drift := math.Hypot(b.Transform.Position.X()-start.X(), b.Transform.Position.Z()-start.Z()) + t.Logf("%s, ridge of %.1f°: %.3f mm in the body at worst, %.3f mm at rest, drift %.2f mm, asleep %v", kind, fold, worst*1000, + terrainInBody(field, b, 0.01)*1000, drift*1000, b.IsSleeping) + if worst > LinearSlop { + t.Errorf("%s, ridge of %.1f°: the ridge is %.2f mm in the body, want %.0f mm at most", kind, fold, worst*1000, LinearSlop*1000) + } + if drift > height*math.Sin(fold/2*math.Pi/180)+LinearSlop || !b.IsSleeping { + t.Errorf("%s, ridge of %.1f°: the body moved by %.2f mm, asleep %v", kind, fold, drift*1000, b.IsSleeping) + } + w.Close() + } + } +} + +// The points of a body across an inactive ridge: the ridge itself, where it crosses the side of the capsule or the +// face of the box, at its depth in the body +func TestHeightfieldRidgeContactPoints(t *testing.T) { + fold := 3 * math.Pi / 180 + // along the normal of a slope, the ridge 0.1 mm in the body as at rest (the side of a capsule is round: its point + // along this normal is 0.04 mm higher than its closest point) + const inBody = 0.0001 + wantSeparation := -inBody * math.Cos(fold/2) + for _, c := range []struct { + name string + shape actor.ShapeInterface + turn mgl64.Quat + half float64 + // span: the length of the ridge under the body + span float64 + }{ + {"capsule", &actor.Capsule{HalfHeight: 0.25, Radius: 0.12}, mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}), 0.12, 0}, + {"box", &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.15, 0.25}}, mgl64.QuatIdent(), 0.15, 0.5}, + } { + w := newScene(1) + terrain := foldedRidge(w, fold) + b := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0, c.half - inBody, 0.1}, Rotation: c.turn}, c.shape, actor.BodyTypeDynamic, 500) + manifolds, deepest := terrainContacts(terrain, b, SpeculativeDistance) + minZ, maxZ, onRidge := math.Inf(1), math.Inf(-1), 0 + for _, m := range manifolds { + for _, point := range m.Points[:m.Count] { + if math.Abs(point.Position.X()) < LinearSlop && math.Abs(point.Separation-wantSeparation) < weldDistance { + onRidge++ + minZ, maxZ = math.Min(minZ, point.Position.Z()), math.Max(maxZ, point.Position.Z()) + } + } + } + if onRidge == 0 || math.Abs(deepest-wantSeparation) > weldDistance { + t.Errorf("%s: %d points on the ridge, the deepest point at %.4f mm, want the ridge at %.4f mm: %+v", c.name, onRidge, deepest*1000, wantSeparation*1000, manifolds) + continue + } + if maxZ-minZ < c.span-1e-6 { + t.Errorf("%s: the points on the ridge span %.3f m, want both ends of the ridge under the body, %.1f m apart", c.name, maxZ-minZ, c.span) + } + // the vertices of the body keep their own manifold, as on a plane: the 4 corners of the box, both ends of the + // capsule, above the slopes + vertices := 4 + if c.name == "capsule" { + vertices = 2 + } + above := (0.25*math.Tan(fold/2) - inBody) * math.Cos(fold/2) + found := false + for _, m := range manifolds { + if m.Count == vertices && math.Abs(m.MinSeparation()-above) < weldDistance { + found = true + } + } + if len(manifolds) != 2 || !found { + t.Errorf("%s: %d manifolds, want the %d vertices of the body %.2f mm above the slopes in one, the ridge in another: %+v", c.name, len(manifolds), vertices, above*1000, manifolds) + } + } +} + +// The deepest point of a body is a contact, on an inactive edge too: a capsule with its lower end 10 mm in a ridge +// folded by 3°, its other end above a slope within the margin. The end above the slope was the only contact +func TestHeightfieldKeepsDeepestPoint(t *testing.T) { + fold := 3 * math.Pi / 180 + w := newScene(1) + terrain := foldedRidge(w, fold) + field := terrain.Shape.(*actor.Heightfield) + capsule := &actor.Capsule{HalfHeight: 0.25, Radius: 0.12} + // the lower end over the ridge, the other end 0.3 m beside it + low := mgl64.Vec3{0, capsule.Radius - 0.010, 0.1} + high := mgl64.Vec3{0.3, capsule.Radius - 0.3*math.Tan(fold/2) + 0.015, 0.5} + axis := high.Sub(low).Normalize() + b := actor.NewRigidBody(actor.Transform{Position: low.Add(axis.Mul(capsule.HalfHeight)), Rotation: mgl64.QuatBetweenVectors(mgl64.Vec3{0, 1, 0}, axis)}, + capsule, actor.BodyTypeDynamic, 500) + depth := terrainInBody(field, b, 0.002) + _, deepest := terrainContacts(terrain, b, SpeculativeDistance+0.01) + t.Logf("the terrain is %.2f mm in the capsule, its deepest contact at %.2f mm", depth*1000, deepest*1000) + if depth < 0.009 { + t.Fatalf("the ridge is %.2f mm in the capsule, want 10 mm", depth*1000) + } + if deepest > -depth*patchCos+LinearSlop/10 { + t.Errorf("the deepest contact is at %.2f mm, the terrain %.2f mm in the capsule", deepest*1000, depth*1000) + } +} + +// The normal of a contact on an edge of a triangle (the rule of ActiveEdges::FixNormal of Jolt, without its part on +// the motion of the body): a sphere 10 mm in a ridge, right above it +func TestHeightfieldContactNormal(t *testing.T) { + up := mgl64.Vec3{0, 1, 0} + z := 0.25 + contact := func(fold float64, velocity mgl64.Vec3) (mgl64.Vec3, float64) { + w := newScene(1) + terrain := foldedRidge(w, fold) + sphere := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0, 0.29, z}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 500) + sphere.Velocity = velocity + manifolds, deepest := terrainContacts(terrain, sphere, SpeculativeDistance) + // the normal of its deepest point + for _, m := range manifolds { + if m.MinSeparation() == deepest { + return m.Normal, deepest + } + } + t.Fatalf("ridge of %.0f°: no contact", degrees(fold)) + return mgl64.Vec3{}, 0 + } + slopeNormal := func(fold float64, normal mgl64.Vec3) bool { + left, right := mgl64.Vec3{-math.Sin(fold / 2), math.Cos(fold / 2), 0}, mgl64.Vec3{math.Sin(fold / 2), math.Cos(fold / 2), 0} + return normal.Sub(left).Len() < 1e-6 || normal.Sub(right).Len() < 1e-6 + } + + // an active edge (10°): the normal of the contact, straight up + if normal, deepest := contact(10*math.Pi/180, mgl64.Vec3{}); normal.Sub(up).Len() > 1e-9 || math.Abs(deepest+0.01) > 1e-9 { + t.Errorf("active edge: normal %v at %.6f mm, want straight up at -10 mm", normal, deepest*1000) + } + // an inactive edge (3°): the normal of a triangle, the point and its depth are kept: the depth along this normal + fold := 3 * math.Pi / 180 + if normal, deepest := contact(fold, mgl64.Vec3{}); !slopeNormal(fold, normal) || math.Abs(deepest+0.01*math.Cos(fold/2)) > 1e-7 { + t.Errorf("inactive edge: normal %v at %.6f mm, want the normal of a slope at %.6f mm", normal, deepest*1000, -10*math.Cos(fold/2)) + } + // a fold of 1°: no edge of the triangles is active, the normal of a slope + fold = 1 * math.Pi / 180 + if normal, deepest := contact(fold, mgl64.Vec3{}); !slopeNormal(fold, normal) || math.Abs(deepest+0.01*math.Cos(fold/2)) > 1e-7 { + t.Errorf("inactive edge of 1°: normal %v at %.6f mm, want the normal of a slope at %.6f mm", normal, deepest*1000, -10*math.Cos(fold/2)) + } + // the same ridge in the last cell of the terrain: a triangle has an active edge, on the border. The normal of the + // contact is closer than 1° to its normal: kept. Folded by 3°, it is not + z = 3.75 + if normal, deepest := contact(fold, mgl64.Vec3{}); normal.Sub(up).Len() > 1e-9 || math.Abs(deepest+0.01) > 1e-9 { + t.Errorf("inactive edge of 1°, a triangle with an active edge: normal %v at %.6f mm, want straight up at -10 mm", normal, deepest*1000) + } + fold = 3 * math.Pi / 180 + if normal, _ := contact(fold, mgl64.Vec3{}); !slopeNormal(fold, normal) { + t.Errorf("inactive edge of 3°, a triangle with an active edge: normal %v, want the normal of a slope", normal) + } + z = 0.25 + // an inactive edge, whatever the motion of the body: the normal of a slope. Jolt keeps the normal of the contact + // when it brakes the body less than the normal of the slope it leaves; the speculative margin of Feather grows with + // the speed, and brings the edges of the triangles around under this rule: a box falling flat got 4 contacts + // tilted by 10 to 60° on a flat terrain, and landed 2 mm deep + for _, velocity := range []mgl64.Vec3{{1, 0, 0}, {-1, 0, 0}, {0, -1, 0}, {0.3, -5, 0}} { + if normal, deepest := contact(fold, velocity); !slopeNormal(fold, normal) || math.Abs(deepest+0.01*math.Cos(fold/2)) > 1e-7 { + t.Errorf("inactive edge, moving at %v: normal %v at %.6f mm, want the normal of a slope at %.6f mm", velocity, normal, deepest*1000, -10*math.Cos(fold/2)) + } + } +} + +// A sphere in a valley folded by 3°: the closest point of each slope is inside its triangle, 10 mm deep on one, 9.8 mm +// on the other, their normals in the same patch. Both are contacts of a face: both are kept (the shallower one was +// dropped as a closest point in a patch which had a deeper one, and a sphere rolling from a triangle to the next, +// folded by less than 5°, lost the contact of the triangle it reached: it sank by 3 mm). Beside the valley, the +// closest point of the other slope is on the edge: dropped, the sphere has its one point, as on a plane +func TestHeightfieldSphereInAShallowValleyTouchesBothSlopes(t *testing.T) { + fold := 3 * math.Pi / 180 + w := newScene(1) + terrain := foldedRidge(w, -fold) + for _, x := range []float64{0.004, -0.004, 0.02} { + // the deepest point 10 mm in the nearest slope + nearest := mgl64.Vec3{-math.Copysign(math.Sin(fold/2), x), math.Cos(fold / 2), 0} + y := (0.29 - x*nearest.X()) / nearest.Y() + sphere := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{x, y, 0.25}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 500) + manifolds, deepest := terrainContacts(terrain, sphere, SpeculativeDistance) + if len(manifolds) != 1 || manifolds[0].Normal.Sub(nearest).Len() > 1e-9 || math.Abs(deepest+0.01) > 1e-9 { + t.Errorf("sphere at x = %.3f: %d manifolds, deepest %.4f mm, want 1 of normal %v at -10 mm: %+v", x, len(manifolds), deepest*1000, nearest, manifolds) + continue + } + m := manifolds[0] + // the other slope: its closest point is inside its triangle if the foot of the center is beyond the valley + other := 0.3 * math.Sin(fold/2) + if math.Abs(x) >= other { + if m.Count != 1 { + t.Errorf("sphere at x = %.3f, beside the valley: %d points, want its one point: %+v", x, m.Count, m.Points[:m.Count]) + } + continue + } + if m.Count != 2 { + t.Errorf("sphere at x = %.3f, in the valley: %d points, want one on each slope: %+v", x, m.Count, m.Points[:m.Count]) + continue + } + for _, point := range m.Points[:m.Count] { + side := math.Copysign(1, point.Position.X()) + want := -0.01 + if side != math.Copysign(1, x) { + want += 2 * math.Abs(x) * math.Sin(fold/2) + } + if math.Abs(point.Separation-want) > 1e-7 { + t.Errorf("sphere at x = %.3f: the point at x = %.4f is at %.4f mm, want %.4f mm", x, point.Position.X(), point.Separation*1000, want*1000) + } + } + } +} + +// 2 bodies of the bench dropped alone which landed deep for a contact missed at their landing: the sphere 28 of the +// pile 51, which sank by 3.4 mm in the hills as it rolled from a triangle to the next, folded by 4.8° (the contact +// of the triangle it reached was dropped), and the box 36 of the pile 22 on the same grid made flat, which landed +// 2 mm deep, straight on a vertex of the grid, held by 4 contacts tilted by the motion rule of Jolt. Each lands +// within a tenth of their old depth +func TestHeightfieldDroppedBodiesLand(t *testing.T) { + for _, c := range []struct { + name string + seed int64 + body int + flat bool + limit, before float64 + }{ + {"the sphere on the hills", 51, 28, false, 0.0003, 0.0034}, + {"the box on the flat terrain", 22, 36, true, 0.0002, 0.002}, + } { + w := newScene(1) + terrain := bumpyTerrain(w, c.seed) + field := terrain.Shape.(*actor.Heightfield) + if c.flat { + for k := range field.Heights { + field.Heights[k] = 0 + } + w.UpdateHeightfield(terrain, 0, 0, field.XSamples-1, field.ZSamples-1) + } + r := rand.New(rand.NewSource(c.seed + 100)) + var b *actor.RigidBody + for i := 0; i < 60; i++ { + x, z := r.Float64()*16-8, r.Float64()*16-8 + ground, _ := field.HeightAt(x, z) + position, rotation, shape := mgl64.Vec3{x, ground + 1 + r.Float64()*3, z}, droppedTurn(r), droppedShape(r, i) + if i == c.body { + b = addBody(w, position, rotation, shape, actor.BodyTypeDynamic, 0.6, 0) + b.Material.RollingResistance = 0.1 + } + } + worst, worstStep := 0.0, 0 + for step := 0; step < 240; step++ { + w.Step(sceneDt) + if depth := terrainInBody(field, b, 0.002); depth > worst { + worst, worstStep = depth, step + } + } + if worst > c.limit { + t.Errorf("%s landed %.2f mm deep at the step %d, want less than %.1f mm (%.1f mm before)", c.name, worst*1000, worstStep, c.limit*1000, c.before*1000) + } + w.Close() + } +} + +// A triangle seen from below is ignored: its plane is above the center of the body (as Jolt, Box3D and PhysX). A body +// whose center went through the terrain falls under it: the terrain is a surface, without thickness +func TestHeightfieldIgnoresTrianglesSeenFromBelow(t *testing.T) { + w := newScene(1) + terrain := slopeTerrain(w, 0, 0.5) + for _, c := range []struct { + name string + shape actor.ShapeInterface + }{ + {"sphere", &actor.Sphere{Radius: 0.3}}, + {"box", &actor.Box{HalfExtents: mgl64.Vec3{0.3, 0.3, 0.3}}}, + {"capsule", &actor.Capsule{HalfHeight: 0.2, Radius: 0.3}}, + } { + // the body still crosses the surface, 0.25 m above it or 0.25 m under it + for _, height := range []float64{0.05, -0.05} { + b := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0.1, height, 0.2}, Rotation: mgl64.QuatIdent()}, c.shape, actor.BodyTypeDynamic, 500) + manifolds, deepest := terrainContacts(terrain, b, SpeculativeDistance) + lowest := b.SupportWorld(mgl64.Vec3{0, -1, 0}).Y() + if height > 0 && (len(manifolds) != 1 || math.Abs(deepest-lowest) > 1e-9) { + t.Errorf("%s, center %.2f m above the terrain: %d manifolds, deepest %.4f m, want 1 at %.4f", c.name, height, len(manifolds), deepest, lowest) + } + if height < 0 && len(manifolds) != 0 { + t.Errorf("%s, center %.2f m under the terrain: %d manifolds, want none", c.name, -height, len(manifolds)) + } + } + } + + sphere := addBody(w, mgl64.Vec3{0.1, -0.05, 0.2}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + simulate(w, 1, nil) + if sphere.Transform.Position.Y() > -2 { + t.Errorf("the sphere whose center is under the terrain is at %.3f m, want it falling under the terrain", sphere.Transform.Position.Y()) + } +} + +// On a flat terrain the contact of a body is its contact with a plane. A box lying over several cells keeps its 4 +// corners, a lying capsule both its ends, at the same separation; a body on a corner, on an edge or on an end keeps +// its deepest point. The body moves, and the margin follows its speed as in the world: the edges of the triangles +// around are within it, and give no contact of their own (the motion rule of Jolt gave them one, tilted) +func TestHeightfieldFlatTerrainLikePlane(t *testing.T) { + w := newScene(1) + terrain := slopeTerrain(w, 0, 0.5) + plane := actor.NewRigidBody(actor.Transform{Rotation: mgl64.QuatIdent()}, &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}, actor.BodyTypeStatic, 0) + r := rand.New(rand.NewSource(1)) + lying := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) + for i := 0; i < 300; i++ { + yaw := mgl64.QuatRotate(r.Float64()*6, mgl64.Vec3{0, 1, 0}) + if i%10 == 0 { + // along the grid: the sides of the body on the edges of the triangles + yaw = mgl64.QuatRotate(float64(i/10%4)*math.Pi/4, mgl64.Vec3{0, 1, 0}) + } + tilt := mgl64.QuatRotate(r.Float64()*6, mgl64.Vec3{r.Float64(), r.Float64(), r.Float64()}.Normalize()) + var shape actor.ShapeInterface + var rotation mgl64.Quat + flat, name := false, "" + switch i % 5 { + case 0: + shape, rotation, flat, name = &actor.Box{HalfExtents: mgl64.Vec3{0.2 + 0.4*r.Float64(), 0.15, 0.25 + 0.3*r.Float64()}}, yaw, true, "lying box" + case 1: + shape, rotation, flat, name = &actor.Capsule{HalfHeight: 0.25 + 0.4*r.Float64(), Radius: 0.12}, yaw.Mul(lying), true, "lying capsule" + case 2: + shape, rotation, name = &actor.Box{HalfExtents: mgl64.Vec3{0.2 + 0.4*r.Float64(), 0.15, 0.25}}, tilt, "tilted box" + case 3: + shape, rotation, name = &actor.Capsule{HalfHeight: 0.25, Radius: 0.12}, tilt, "tilted capsule" + case 4: + shape, rotation, flat, name = &actor.Sphere{Radius: 0.2}, tilt, false, "sphere" + } + b := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{r.Float64()*8 - 4, 0, r.Float64()*8 - 4}, Rotation: rotation}, shape, actor.BodyTypeDynamic, 500) + // its lowest point 0.1 mm in the terrain + b.Transform.Position[1] = -b.SupportWorld(mgl64.Vec3{0, -1, 0}).Y() - 0.0001 + b.Velocity = mgl64.Vec3{r.Float64()*6 - 3, -r.Float64() * 6, r.Float64()*6 - 3} + margin := SpeculativeDistance + b.Velocity.Len()*sceneDt + + var onPlane constraint.Manifold + if !Collide(plane, b, margin, &onPlane) { + t.Fatalf("%s %d: no contact with the plane", name, i) + } + manifolds, deepest := terrainContacts(terrain, b, margin) + if len(manifolds) != 1 || manifolds[0].Normal.Sub(mgl64.Vec3{0, 1, 0}).Len() > 1e-9 { + normals := make([]mgl64.Vec3, len(manifolds)) + for k, m := range manifolds { + normals[k] = m.Normal + } + t.Errorf("%s %d: %d manifolds on the terrain, want 1 straight up: %.3f", name, i, len(manifolds), normals) + continue + } + onTerrain := manifolds[0] + has := func(m constraint.Manifold, point constraint.ContactPoint) bool { + for _, other := range m.Points[:m.Count] { + if other.Position.Sub(point.Position).Len() < 1e-9 && math.Abs(other.Separation-point.Separation) < 1e-9 { + return true + } + } + return false + } + if math.Abs(deepest-onPlane.MinSeparation()) > 1e-9 { + t.Errorf("%s %d: the deepest point at %.6f mm on the terrain, %.6f mm on the plane", name, i, deepest*1000, onPlane.MinSeparation()*1000) + } + if !flat { + continue + } + for _, point := range onPlane.Points[:onPlane.Count] { + if !has(onTerrain, point) { + t.Errorf("%s %d: the point %v of the plane is not a point of the terrain: %v", name, i, point.Position, onTerrain.Points[:onTerrain.Count]) + } + } + if _, box := shape.(*actor.Box); box && onTerrain.Count != onPlane.Count { + t.Errorf("%s %d: %d points on the terrain, %d on the plane", name, i, onTerrain.Count, onPlane.Count) + } + } +} + +// droppedShape & droppedTurn: the bodies of the piles of the bench (bench/regression.go: mixedShape, randomTurn) +func droppedShape(r *rand.Rand, i int) actor.ShapeInterface { + switch i % 3 { + case 1: + return &actor.Sphere{Radius: 0.2} + case 2: + return &actor.Capsule{HalfHeight: 0.25, Radius: 0.12} + } + return &actor.Box{HalfExtents: mgl64.Vec3{0.2 + 0.2*r.Float64(), 0.15, 0.25}} +} + +func droppedTurn(r *rand.Rand) mgl64.Quat { + return mgl64.QuatRotate(r.Float64()*6, mgl64.Vec3{r.Float64(), r.Float64(), r.Float64()}.Normalize()) +} + +// keepsItsContact: before each step, the deepest contact of the body with the terrain is as deep as the terrain is in +// the body, to the angle of a patch and a tenth of LinearSlop. A body deeper than its smallest half size is out of the +// rule: the triangles above its center are ignored +func keepsItsContact(t *testing.T, name string, terrain, b *actor.RigidBody, step int, worst *float64) { + t.Helper() + const thinnest = 0.12 + depth := terrainInBody(terrain.Shape.(*actor.Heightfield), b, 0.004) + if depth < LinearSlop/10 || depth > thinnest { + return + } + _, deepest := terrainContacts(terrain, b, SpeculativeDistance) + if missing := depth*patchCos - LinearSlop/10 + deepest; missing > *worst { + *worst = missing + t.Errorf("%s, step %d: the terrain is %.2f mm in the body, its deepest contact at %.2f mm", name, step, depth*1000, deepest*1000) + } +} + +// 2 bodies of the bench which sank in the hills, their deepest point without a contact: a capsule dropped alone (the +// body 44 of the pile 7) which fell 76 mm in the terrain through an inactive edge, and a box in the rain (the body 130 +// of the rain 33) which rested 19 cm in it +func TestHeightfieldDroppedBodiesKeepTheirContact(t *testing.T) { + // ========== THE CAPSULE ========== + w := newScene(1) + terrain := bumpyTerrain(w, 7) + field := terrain.Shape.(*actor.Heightfield) + r := rand.New(rand.NewSource(107)) + var capsule *actor.RigidBody + for i := 0; i < 60; i++ { + x, z := r.Float64()*16-8, r.Float64()*16-8 + ground, _ := field.HeightAt(x, z) + position, rotation, shape := mgl64.Vec3{x, ground + 1 + r.Float64()*3, z}, droppedTurn(r), droppedShape(r, i) + if i == 44 { + capsule = addBody(w, position, rotation, shape, actor.BodyTypeDynamic, 0.6, 0) + capsule.Material.RollingResistance = 0.1 + } + } + worst := 0.0 + for step := 0; step < 240; step++ { + keepsItsContact(t, "the capsule", terrain, capsule, step, &worst) + w.Step(sceneDt) + } + w.Close() + + // ========== THE BOX ========== + w = newScene(1) + terrain = bumpyTerrain(w, 33) + field = terrain.Shape.(*actor.Heightfield) + r = rand.New(rand.NewSource(34)) + worst = 0 + for step := 0; step < 200; step++ { + if step%5 == 0 && len(w.Bodies) < 200 { + for i := 0; i < 10; i++ { + x, z := r.Float64()*16-8, r.Float64()*16-8 + ground, _ := field.HeightAt(x, z) + b := addBody(w, mgl64.Vec3{x, ground + 4, z}, droppedTurn(r), droppedShape(r, i), actor.BodyTypeDynamic, 0.6, 0) + b.Velocity = mgl64.Vec3{0, -8, 0} + } + } + if len(w.Bodies) > 130 { + keepsItsContact(t, "the box", terrain, w.Bodies[130], step, &worst) + } + w.Step(sceneDt) + } + w.Close() +} diff --git a/island.go b/island.go index 42ef33c..4d7a0e5 100644 --- a/island.go +++ b/island.go @@ -9,6 +9,8 @@ import ( // sleepIslands: the bodies touching each other form an island (as in Box2D). An island falls asleep when all // its bodies are resting, and wakes up entirely when one of its bodies wakes up. // A body can't sleep under a moving body anymore, and the bodies above a removed body wake up. +// A kinematic body is in the island of the bodies it touches (as in Box2D and Jolt): on its way to a target it keeps +// them awake, stopped it sleeps with them, and a target wakes them all up. type sleepIslands struct { // union-find over the dynamic bodies of the solver parent []int @@ -52,7 +54,7 @@ func (si *sleepIslands) update(s *solver, dt float64) { // ========== 1. Timers ========== for i := range s.states { body := s.states[i].body - if body.Velocity.Len() < actor.DefaultSleepSpeed && body.AngularVelocity.Len() < actor.DefaultSleepSpeed { + if isResting(body) { body.SleepTimer += dt } else { body.SleepTimer = 0 diff --git a/joint.go b/joint.go index 1954ac2..9bfd409 100644 --- a/joint.go +++ b/joint.go @@ -120,15 +120,15 @@ func (j *JointBase) solvePoint(s *solver, stateA, stateB *bodyState, useBias boo applyLinear(stateA, stateB, rA, rB, impulse) } -// applyLinear: -impulse at rA on A, +impulse at rB on B +// applyLinear: -impulse at rA on A, +impulse at rB on B (only on the dynamic bodies, see jacobian.apply) func applyLinear(stateA, stateB *bodyState, rA, rB, impulse mgl64.Vec3) { - if stateA.body != nil { + if stateA.dynamic { v, w, m := &stateA.velocity, &stateA.angularVelocity, &stateA.invMassAxes v[0], v[1], v[2] = v[0]-impulse[0]*m[0], v[1]-impulse[1]*m[1], v[2]-impulse[2]*m[2] torque := actor.MulMat3(&stateA.inverseInertia, rA.Cross(impulse)) w[0], w[1], w[2] = w[0]-torque[0], w[1]-torque[1], w[2]-torque[2] } - if stateB.body != nil { + if stateB.dynamic { v, w, m := &stateB.velocity, &stateB.angularVelocity, &stateB.invMassAxes v[0], v[1], v[2] = v[0]+impulse[0]*m[0], v[1]+impulse[1]*m[1], v[2]+impulse[2]*m[2] torque := actor.MulMat3(&stateB.inverseInertia, rB.Cross(impulse)) @@ -136,13 +136,13 @@ func applyLinear(stateA, stateB *bodyState, rA, rB, impulse mgl64.Vec3) { } } -// applyAngular: -impulse on A, +impulse on B +// applyAngular: -impulse on A, +impulse on B (only on the dynamic bodies) func applyAngular(stateA, stateB *bodyState, impulse mgl64.Vec3) { - if stateA.body != nil { + if stateA.dynamic { w, t := &stateA.angularVelocity, actor.MulMat3(&stateA.inverseInertia, impulse) w[0], w[1], w[2] = w[0]-t[0], w[1]-t[1], w[2]-t[2] } - if stateB.body != nil { + if stateB.dynamic { w, t := &stateB.angularVelocity, actor.MulMat3(&stateB.inverseInertia, impulse) w[0], w[1], w[2] = w[0]+t[0], w[1]+t[1], w[2]+t[2] } diff --git a/kinematic.go b/kinematic.go new file mode 100644 index 0000000..16532ea --- /dev/null +++ b/kinematic.go @@ -0,0 +1,202 @@ +package feather + +import ( + "errors" + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== KINEMATIC BODIES ========== +// A kinematic body goes where the game puts it: a target pose per step (RigidBody.SetKinematicTarget), reached at the +// end of the step. Its velocity is the one of this motion, (target - pose) / dt and the axis of the rotation to the +// target times its angle / dt, deduced at the start of the step: the model of PhysX (PxRigidDynamic::setKinematicTarget, +// Sc::BodySim::calculateKinematicVelocity in ScKinematics.cpp, 5.6.1; the velocity is 0 for a step without target, +// and the pose is set to the target at the end of the step, updateKinematicPose), the same velocities as +// Body::MoveKinematic of Jolt (Body.cpp, MotionProperties.inl, v5.3.0) and b2Body_SetTargetTransform of Box2D +// (body.c, v3.1.0), which keep them until the next call. +// During the sub-steps, the body goes to its target by interpolation: its position linearly, its rotation along the +// shortest arc at a constant angular velocity (a slerp from the identity: the velocity the contacts read is the velocity +// of the motion at every sub-step), and the last sub-step puts it on its target bit for bit. +// +// For the solver, a kinematic body has no inverse mass nor inverse inertia ("Static and kinematic sims have zero +// mass", Box2D body.c:535; MotionProperties.h:94-95 of Jolt): the contacts and the joints read its velocity and its +// motion, and never write it. A dynamic body it meets takes its velocity (a platform carries, a leg pushes); nothing +// pushes it back. It doesn't collide with the static and the kinematic bodies (Jolt Body.inl:36-44, Box2D +// simulation.md "A shape on a kinematic body can only collide with a dynamic body", PhysX PxRigidBody.h:56). +// Teleport (World.Teleport) puts a body somewhere without any velocity: PxRigidActor::setGlobalPose on a kinematic +// actor ("it would go right through them", the PhysX guide, Kinematic Actors). + +// kinematicMotion: the velocities which bring a body from a pose to a target in dt: the translation over dt, and the +// axis of the rotation from the pose to the target times its angle over dt (the shortest way) +func kinematicMotion(from, to actor.Transform, dt float64) (mgl64.Vec3, mgl64.Vec3) { + velocity := to.Position.Sub(from.Position).Mul(1 / dt) + axis, angle := axisAngle(rotationDelta(from.Rotation, to.Rotation)) + return velocity, axis.Mul(angle / dt) +} + +// rotationDelta: the rotation from a to b in world space, b ⊗ a⁻¹, on the shortest way (W ≥ 0) +func rotationDelta(a, b mgl64.Quat) mgl64.Quat { + conjugate := mgl64.Quat{W: a.W, V: a.V.Mul(-1)} + delta := actor.MulQuat(&b, &conjugate) + if delta.W < 0 { + delta = delta.Scale(-1) + } + return delta +} + +// axisAngle of a unit quaternion with W ≥ 0: a unit axis and an angle in [0, π], the axis is null without rotation +func axisAngle(q mgl64.Quat) (mgl64.Vec3, float64) { + sine := q.V.Len() + if sine == 0 { + return mgl64.Vec3{}, 0 + } + return q.V.Mul(1 / sine), 2 * math.Atan2(sine, q.W) +} + +// rotationFraction: the fraction t of the rotation delta (W ≥ 0), around its axis: the slerp from the identity +func rotationFraction(delta mgl64.Quat, t float64) mgl64.Quat { + axis, angle := axisAngle(delta) + if angle == 0 { + return mgl64.QuatIdent() + } + half := 0.5 * t * angle + return mgl64.Quat{W: math.Cos(half), V: axis.Mul(math.Sin(half))} +} + +// moveKinematics, at the start of a step: the kinematic bodies take the velocity of the motion to their target. A body +// without target (or whose target is its pose) has no velocity. The velocity is read by the broad phase (the AABB +// enlarged by the distance it travels), the speculative margin of its contacts, the bodies it wakes up and the solver +func (w *World) moveKinematics(dt float64) { + for _, body := range w.Bodies { + if body.BodyType != actor.BodyTypeKinematic { + continue + } + target, ok := body.KinematicTarget() + if !ok { + body.Velocity, body.AngularVelocity = mgl64.Vec3{}, mgl64.Vec3{} + continue + } + body.Velocity, body.AngularVelocity = kinematicMotion(body.Transform, target, dt) + } +} + +// moveKinematic: the state i (a kinematic body) at the sub-step s.substep of s.substeps, on the way to its target: +// its position linearly, its rotation along the shortest arc. The last sub-step is the target itself +func (s *solver) moveKinematic(i int) { + state := &s.states[i] + target, ok := state.body.KinematicTarget() + if !ok { + return + } + start := state.body.Transform + translation, delta := target.Position.Sub(start.Position), rotationDelta(start.Rotation, target.Rotation) + if s.substep == s.substeps { + state.deltaPosition, state.deltaRotation = translation, delta + } else { + t := float64(s.substep) / float64(s.substeps) + state.deltaPosition, state.deltaRotation = translation.Mul(t), rotationFraction(delta, t) + } + state.deltaMatrix = rotationMatrix(&state.deltaRotation) +} + +// isAwakeMover: an awake body which moves by itself, dynamic or kinematic. The pairs of the step, the states of the +// solver and the bodies woken by a touch follow it: a dynamic body is needed in a pair (needsSolving) +func isAwakeMover(body *actor.RigidBody) bool { + return body.BodyType != actor.BodyTypeStatic && !body.IsSleeping +} + +// isMoving: an awake body moving fast enough to keep what it touches awake. A dynamic body under the sleep speed is +// resting; a kinematic body goes exactly where it is told, there is no jitter to filter: any motion of its target moves +// it. Box2D and Jolt apply their sleep threshold to the kinematic bodies too (b2FinalizeBodiesTask, solver.c:617-651; +// MotionProperties::AccumulateSleepTime), with a velocity kept from a step to the next, where PhysX keeps a kinematic +// actor awake as long as a target is set (PxRigidDynamic.h:150-152): so does Feather +func isMoving(body *actor.RigidBody) bool { + if body.IsSleeping { + return false + } + switch body.BodyType { + case actor.BodyTypeDynamic: + return body.Velocity.Len() >= actor.DefaultSleepSpeed || body.AngularVelocity.Len() >= actor.DefaultSleepSpeed + case actor.BodyTypeKinematic: + return body.Velocity != (mgl64.Vec3{}) || body.AngularVelocity != (mgl64.Vec3{}) + } + return false +} + +// isResting: the body is still enough for its sleep timer to run (the opposite of isMoving, for an awake body) +func isResting(body *actor.RigidBody) bool { + if body.BodyType == actor.BodyTypeKinematic { + return body.Velocity == (mgl64.Vec3{}) && body.AngularVelocity == (mgl64.Vec3{}) + } + return body.Velocity.Len() < actor.DefaultSleepSpeed && body.AngularVelocity.Len() < actor.DefaultSleepSpeed +} + +// dynamicIndex: the state index i if it is the one of a dynamic body, else -1: for what a kinematic body must not +// enter (the graph coloring, the trees of joints), as a static body +func (s *solver) dynamicIndex(i int) int { + if i >= 0 && s.states[i].dynamic { + return i + } + return -1 +} + +// ErrStaticBody: SetBodyType was asked to change a body from or to static. The body is left as it is +var ErrStaticBody = errors.New("feather: SetBodyType changes a body between kinematic and dynamic, a static body stays static") + +// ErrMasslessBody: SetBodyType was asked to make dynamic a body without mass. The body is left as it is +var ErrMasslessBody = errors.New("feather: a dynamic body needs a mass: create the kinematic body with a density") + +// SetBodyType changes a body between kinematic and dynamic, in place: the body keeps its index in World.Bodies and +// its place in the broad phase (and so the pairs and the contacts it had, with their warm start), its joints, its +// layers and the pairs it ignores, and its island: the bodies resting on it keep resting on it. As Body::SetMotionType +// of Jolt (Body.cpp:37-77, v5.3.0), where Box2D destroys the contacts of the body (b2Body_SetType, body.c:1044). +// The body and its island wake up (Box2D wakes the body and the bodies of its joints). +// - To dynamic: the body keeps the velocity of its last motion (a ragdoll thrown by its animation), and needs the +// mass of its shape: a kinematic body created with a density (as PhysX: "you do need to provide a mass for the +// kinematic actor"). Without mass, the body is refused: ErrMasslessBody. +// - To kinematic: the body stops (no velocity until a target), its forces are dropped (Jolt), its locks are kept for +// the day it becomes dynamic again. +// +// A static body is another kind of body (PxRigidStatic in PhysX; Jolt needs mAllowDynamicOrKinematic at the creation): +// a body is not changed from or to static, it is refused: ErrStaticBody. A refused body is left as it is. A body +// already of the type does nothing. The call allocates nothing +func (w *World) SetBodyType(body *actor.RigidBody, bodyType actor.BodyType) error { + if body.BodyType == bodyType { + return nil + } + if body.BodyType == actor.BodyTypeStatic || bodyType == actor.BodyTypeStatic { + return ErrStaticBody + } + if mass := body.Material.GetMass(); bodyType == actor.BodyTypeDynamic && !(mass > 0 && !math.IsInf(mass, 1)) { + return ErrMasslessBody + } + body.BodyType = bodyType + body.ClearKinematicTarget() + if bodyType == actor.BodyTypeKinematic { + body.Velocity, body.AngularVelocity = mgl64.Vec3{}, mgl64.Vec3{} + body.ClearForces() + } + w.islands.wake(body) + return nil +} + +// Teleport puts a body at a transform without any velocity: what it lands against is not pushed (a kinematic body +// brought there by SetKinematicTarget pushes with the velocity of its motion). The target of a kinematic body is +// dropped and it stops; a dynamic body keeps its velocity. The body wakes up with its island, and the sleeping bodies +// at the new place wake up: a body teleported into another one overlaps it, the spring of their contact pushes them +// apart at the next steps (at ContactSpeed at most). The trees see the body at its new place at the next step, or at +// SyncQueries. As PxRigidActor::setGlobalPose on a kinematic actor (PhysX: "teleport a kinematic actor to a new +// position"), Body::SetPositionAndRotation of Jolt, b2Body_SetTransform of Box2D +func (w *World) Teleport(body *actor.RigidBody, transform actor.Transform) { + transform.Rotation = transform.Rotation.Normalize() + body.Transform = transform + body.ClearKinematicTarget() + if body.BodyType == actor.BodyTypeKinematic { + body.Velocity, body.AngularVelocity = mgl64.Vec3{}, mgl64.Vec3{} + } + body.UpdateAABB() + w.islands.wake(body) + w.wakeNeighbors(body) +} diff --git a/kinematic_test.go b/kinematic_test.go new file mode 100644 index 0000000..9e5624a --- /dev/null +++ b/kinematic_test.go @@ -0,0 +1,667 @@ +package feather + +import ( + "errors" + "fmt" + "math" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== KINEMATIC BODIES ========== +// A kinematic body goes where the game tells it (SetKinematicTarget: the pose it reaches at the end of the next step), +// over the sub-steps of the step, and pushes the dynamic bodies with the velocity of this motion. Nothing moves it. +// The bounds come from the engine: landingDepth (LinearSlop) for a depth, SpeculativeDistance for a contact kept, and +// the bits for what must not change at all. + +const ( + // kinematicSpeed: the speed of the platforms and pushers of these scenes (m/s) + kinematicSpeed = 1.0 + + // legSpeed: the speed of the leg of a creature at a run, the criterion of #798 (m/s) + legSpeed = 5.0 +) + +// kinematicBox: a kinematic box of half extents half, friction 0.6 +func kinematicBox(w *World, position mgl64.Vec3, half mgl64.Vec3) *actor.RigidBody { + return addBody(w, position, mgl64.QuatIdent(), &actor.Box{HalfExtents: half}, actor.BodyTypeKinematic, 0.6, 0) +} + +// translated: the transform moved by the velocity during a step +func translated(transform actor.Transform, velocity mgl64.Vec3) actor.Transform { + transform.Position = transform.Position.Add(velocity.Mul(sceneDt)) + return transform +} + +// moveTo gives a kinematic body its target for the next step; the body is kinematic, the call can't fail +func moveTo(body *actor.RigidBody, target actor.Transform) { + if err := body.SetKinematicTarget(target); err != nil { + panic(err) + } +} + +// drive runs the scene for seconds, with the kinematic body moving at the velocity (a target before each step), and +// calls each after each step +func drive(w *World, body *actor.RigidBody, velocity mgl64.Vec3, seconds float64, each func()) { + for i := 0; i < int(math.Round(seconds/sceneDt)); i++ { + moveTo(body, translated(body.Transform, velocity)) + w.Step(sceneDt) + if each != nil { + each() + } + } +} + +// faceGap: the gap along X between the front face of the pusher and the back face of the box (both cubes, not turned) +func faceGap(pusher, box *actor.RigidBody) float64 { + return (box.Transform.Position.X() - cubeHalf) - (pusher.Transform.Position.X() + cubeHalf) +} + +// ========== ACCEPTANCE ========== + +// 1. A kinematic cube sliding on the ground pushes a dynamic cube resting in front of it: the cube takes its speed, +// rubbing on the ground, and stays against the pusher (within the speculative distance, never in it), flat on the ground +func TestKinematicPushesABox(t *testing.T) { + w := newScene(1) + ground := addGround(w, 0.6) + pusher := kinematicBox(w, mgl64.Vec3{0, cubeHalf, 0}, cube().HalfExtents) + box := addBody(w, mgl64.Vec3{2*cubeHalf + 0.01, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + + worstGap, worstDepth, elapsed := 0.0, 0.0, 0.0 + drive(w, pusher, mgl64.Vec3{kinematicSpeed, 0, 0}, 2, func() { + elapsed += sceneDt + if gap := faceGap(pusher, box); elapsed > 0.1 { + // in contact after the first centimeter: pushed, neither left behind nor entered + worstGap = math.Max(worstGap, gap) + worstDepth = math.Max(worstDepth, -gap) + } + worstDepth = math.Max(worstDepth, surfaceDepth(ground, box)) + }) + t.Logf("gap up to %.2f mm, depth up to %.2f mm, box at %.3f m/s", worstGap*1000, worstDepth*1000, box.Velocity.X()) + if worstGap > SpeculativeDistance { + t.Errorf("the box got %.1f mm ahead of the pusher: not pushed", worstGap*1000) + } + if worstDepth > landingDepth { + t.Errorf("the box went %.2f mm into the pusher or the ground", worstDepth*1000) + } + // the box has the speed of the pusher: it loses at most the friction of the ground during one step between two pushes + if slip := kinematicSpeed - box.Velocity.X(); slip > 0.6*sceneGravity*sceneDt || slip < -0.6*sceneGravity*sceneDt { + t.Errorf("the box moves at %.3f m/s, want the %.0f m/s of the pusher", box.Velocity.X(), kinematicSpeed) + } + if angle := rotationAngle(box.Transform.Rotation); angle > 1e-3 { + t.Errorf("the box turned by %.4f rad while pushed flat on the ground", angle) + } +} + +// 2. A kinematic body is not moved by what falls on it: a cube 100 times heavier than the platform would be, and a +// stack of 10 cubes, land on a kinematic plate in the air. Its transform and its velocity are the same bit for bit at +// every step, with or without a target at its own pose +func TestDynamicBodiesDoNotMoveAKinematic(t *testing.T) { + for _, targeted := range []bool{false, true} { + w := newScene(1) + plate := kinematicBox(w, mgl64.Vec3{0, 2, 0}, mgl64.Vec3{1, 0.1, 1}) + start := plate.Transform + heavy := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0.5, 3, 0.5}, Rotation: mgl64.QuatIdent()}, cube(), actor.BodyTypeDynamic, 50000) + heavy.Material.StaticFriction, heavy.Material.DynamicFriction = 0.6, 0.6 + w.AddBody(heavy) + for i := 0; i < 10; i++ { + addBody(w, mgl64.Vec3{-0.5, 2.1 + cubeHalf + float64(i)*2*cubeHalf, -0.5}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + } + for step := 0; step < 120; step++ { + if targeted { + moveTo(plate, start) + } + w.Step(sceneDt) + if plate.Transform != start { + t.Fatalf("targeted=%v, step %d: the plate moved to %v, want %v bit for bit", targeted, step, plate.Transform, start) + } + if plate.Velocity != (mgl64.Vec3{}) || plate.AngularVelocity != (mgl64.Vec3{}) { + t.Fatalf("targeted=%v, step %d: the plate has a velocity %v %v", targeted, step, plate.Velocity, plate.AngularVelocity) + } + } + if heavy.Transform.Position.Y() < 2.1+cubeHalf-landingDepth { + t.Errorf("targeted=%v: the heavy cube sank to y = %.4f through the plate", targeted, heavy.Transform.Position.Y()) + } + } +} + +// 3. The velocity of a kinematic body is the one of its motion: (target - pose) / dt, and the axis of the rotation +// to the target times its angle / dt. The body is at its target at the end of the step, bit for bit. Without a target, +// it doesn't move and its velocity is 0 +func TestKinematicVelocityIsItsMotion(t *testing.T) { + w := newScene(1) + body := kinematicBox(w, mgl64.Vec3{0, 1, 0}, cube().HalfExtents) + velocity := mgl64.Vec3{1, 0.5, -0.25} + axis := mgl64.Vec3{0.3, 1, 0.2}.Normalize() + const spin = 2.0 // rad/s + for step := 0; step < 60; step++ { + before := body.Transform + target := actor.Transform{ + Position: before.Position.Add(velocity.Mul(sceneDt)), + Rotation: mgl64.QuatRotate(spin*sceneDt, axis).Mul(before.Rotation).Normalize(), + } + moveTo(body, target) + w.Step(sceneDt) + if body.Transform != target { + t.Fatalf("step %d: the body is at %v, want its target %v bit for bit", step, body.Transform, target) + } + wantVelocity := target.Position.Sub(before.Position).Mul(1 / sceneDt) + if body.Velocity.Sub(wantVelocity).Len() > 1e-9 { + t.Fatalf("step %d: velocity %v, want (target - pose) / dt = %v", step, body.Velocity, wantVelocity) + } + if body.AngularVelocity.Sub(axis.Mul(spin)).Len() > 1e-9 { + t.Fatalf("step %d: angular velocity %v, want %v", step, body.AngularVelocity, axis.Mul(spin)) + } + } + // no target: nothing moves, no velocity + last := body.Transform + w.Step(sceneDt) + if body.Transform != last || body.Velocity != (mgl64.Vec3{}) || body.AngularVelocity != (mgl64.Vec3{}) { + t.Errorf("without a target: moved from %v to %v, velocity %v %v", last, body.Transform, body.Velocity, body.AngularVelocity) + } +} + +// 4. Teleport places a body without any velocity: a kinematic cube teleported against a resting cube (within the +// speculative distance) doesn't push it: the cube keeps the velocity of its own contact with the ground, the rounding +// (1e-19 m/s along X). The same cube brought there by a target pushes it +func TestTeleportGivesNoVelocity(t *testing.T) { + for _, teleport := range []bool{true, false} { + w := newScene(1) + addGround(w, 0.6) + box := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + pusher := kinematicBox(w, mgl64.Vec3{-3, cubeHalf, 0}, cube().HalfExtents) + simulate(w, 1, nil) + against := actor.Transform{Position: mgl64.Vec3{-2*cubeHalf - SpeculativeDistance/2, cubeHalf, 0}, Rotation: mgl64.QuatIdent()} + if teleport { + w.Teleport(pusher, against) + } else { + moveTo(pusher, against) + } + const rounding = 1e-9 // a nanometer (per second): the rounding of the contact with the ground, not a push + pushed := false + for step := 0; step < 30; step++ { + w.Step(sceneDt) + pushed = pushed || box.Velocity.X() > rounding || box.Transform.Position.X() > rounding + } + if pusher.Transform != against { + t.Errorf("teleport=%v: the pusher is at %v, want %v", teleport, pusher.Transform, against) + } + if teleport && pushed { + t.Errorf("a teleported kinematic pushed the box: velocity %v, position %v", box.Velocity, box.Transform.Position) + } + if teleport && pusher.Velocity != (mgl64.Vec3{}) { + t.Errorf("a teleported kinematic has a velocity %v", pusher.Velocity) + } + if !teleport && !pushed { + t.Errorf("a kinematic brought by a target didn't push the box") + } + } +} + +// 5. No contact between a kinematic body and a static or a kinematic body: a kinematic cube overlapping the ground, a +// static box and another kinematic cube has no contact and sends no event with them, and only pushes the dynamic cube +func TestNoContactBetweenKinematicAndStaticOrKinematic(t *testing.T) { + w := newScene(1) + ground := addGround(w, 0.6) + wall := addBody(w, mgl64.Vec3{0.3, 0.5, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.5, 1}}, actor.BodyTypeStatic, 0.6, 0) + mover := kinematicBox(w, mgl64.Vec3{0, 0.1, 0}, cube().HalfExtents) // in the ground and the wall + other := kinematicBox(w, mgl64.Vec3{0.1, 0.3, 0}, cube().HalfExtents) + dynamic := addBody(w, mgl64.Vec3{0, 0.35 + 2*cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + events := 0 + w.Events.Subscribe(EventCollisionEnter, func(e Event) { + pair := e.(CollisionEnterEvent) + if pair.BodyA != dynamic && pair.BodyB != dynamic { + t.Errorf("collision event between %v and %v, both without mass", pair.BodyA.BodyType, pair.BodyB.BodyType) + } + events++ + }) + w.Events.Subscribe(EventCollisionStay, func(Event) { events++ }) + drive(w, mover, mgl64.Vec3{0, 0, 0.1}, 1, func() { + moveTo(other, translated(other.Transform, mgl64.Vec3{0, 0, 0.1})) + for _, contact := range w.Contacts() { + if contact.BodyA.BodyType != actor.BodyTypeDynamic && contact.BodyB.BodyType != actor.BodyTypeDynamic { + t.Fatalf("contact between a %v and a %v", contact.BodyA.BodyType, contact.BodyB.BodyType) + } + } + }) + _, _ = ground, wall + if events == 0 { + t.Errorf("no collision event with the dynamic cube carried by the kinematic one") + } + if dynamic.Transform.Position.Z() < 0.05 { + t.Errorf("the dynamic cube was not carried along Z: z = %.3f", dynamic.Transform.Position.Z()) + } +} + +// segmentsDistance: the distance between 2 segments (Ericson 5.1.9, written here to measure the engine from outside) +func segmentsDistance(a0, a1, b0, b1 mgl64.Vec3) float64 { + d1, d2, r := a1.Sub(a0), b1.Sub(b0), a0.Sub(b0) + a, e, f := d1.Dot(d1), d2.Dot(d2), d2.Dot(r) + c, b := d1.Dot(r), d1.Dot(d2) + denominator := a*e - b*b + s := 0.0 + if denominator > 1e-12 { + s = math.Max(0, math.Min(1, (b*f-c*e)/denominator)) + } + t := (b*s + f) / e + if t < 0 { + t, s = 0, math.Max(0, math.Min(1, -c/a)) + } else if t > 1 { + t, s = 1, math.Max(0, math.Min(1, (b-c)/a)) + } + return a0.Add(d1.Mul(s)).Sub(b0.Add(d2.Mul(t))).Len() +} + +// capsuleDepth: how deep 2 capsules overlap, 0 apart +func capsuleDepth(a, b *actor.RigidBody) float64 { + segment := func(body *actor.RigidBody) (mgl64.Vec3, mgl64.Vec3, float64) { + capsule := body.Shape.(*actor.Capsule) + axis := body.Transform.Rotation.Rotate(mgl64.Vec3{0, capsule.HalfHeight, 0}) + return body.Transform.Position.Sub(axis), body.Transform.Position.Add(axis), capsule.Radius + } + a0, a1, radiusA := segment(a) + b0, b1, radiusB := segment(b) + return math.Max(0, radiusA+radiusB-segmentsDistance(a0, a1, b0, b1)) +} + +// 7. A kinematic leg at 5 m/s (8 cm per step at 60 Hz, 8 sub-steps) meets a dynamic body at rest on its path: the +// contact exists before they touch, from the speed of the kinematic body (speculative), and the dynamic body is pushed +// without being entered (landingDepth) nor gone through. A capsule against a capsule lying across (the leg of a Slyf +// against its tail), and a box against a box +func TestFastKinematicDoesNotGoThroughARestingBody(t *testing.T) { + cases := []struct { + name string + leg actor.ShapeInterface + tail actor.ShapeInterface + depth func(a, b *actor.RigidBody) float64 + }{ + {"capsules", &actor.Capsule{HalfHeight: 0.2, Radius: 0.05}, &actor.Capsule{HalfHeight: 0.3, Radius: 0.05}, capsuleDepth}, + {"boxes", &actor.Box{HalfExtents: mgl64.Vec3{0.05, 0.25, 0.05}}, &actor.Box{HalfExtents: mgl64.Vec3{0.05, 0.05, 0.35}}, boxOverlap}, + } + for _, c := range cases { + w := newScene(1) + addGround(w, 0.6) + legAABB := c.leg.ComputeAABB(actor.NewTransform()) + tailAABB := c.tail.ComputeAABB(actor.NewTransform()) + leg := addBody(w, mgl64.Vec3{0, -legAABB.Min.Y() + 0.001, 0}, mgl64.QuatIdent(), c.leg, actor.BodyTypeKinematic, 0.6, 0) + tailRotation := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{1, 0, 0}) // the capsule lies along Z + if _, isBox := c.tail.(*actor.Box); isBox { + tailRotation = mgl64.QuatIdent() + } + tail := addBody(w, mgl64.Vec3{0.5, -tailAABB.Min.Y(), 0}, tailRotation, c.tail, actor.BodyTypeDynamic, 0.6, 0) + tail.Transform.Position[1] = tail.Shape.ComputeAABB(actor.Transform{Rotation: tailRotation}).Max.Y() + tail.UpdateAABB() + simulate(w, 0.5, nil) // the tail settles + + worst, through := 0.0, false + drive(w, leg, mgl64.Vec3{legSpeed, 0, 0}, 0.4, func() { + worst = math.Max(worst, c.depth(leg, tail)) + through = through || tail.Transform.Position.X() < leg.Transform.Position.X() + }) + t.Logf("%s: %.2f mm deep at worst, tail at %.2f m, leg at %.2f m", c.name, worst*1000, tail.Transform.Position.X(), leg.Transform.Position.X()) + if through { + t.Errorf("%s: the leg went through the tail", c.name) + } + if worst > landingDepth { + t.Errorf("%s: the leg entered the tail by %.2f mm", c.name, worst*1000) + } + } +} + +// 8. SetBodyType changes a body between kinematic and dynamic in place: a kinematic plate carrying a cube, with a ball +// hanging from it by a distance joint, becomes dynamic and falls with them (the contact and the joint are kept, the +// cube doesn't jump), then becomes kinematic again and is lifted to its targets, the cube and the ball following +func TestSetBodyTypeKeepsTheContactsAndTheJoints(t *testing.T) { + w := newScene(1) + plate := kinematicBox(w, mgl64.Vec3{0, 3, 0}, mgl64.Vec3{0.5, 0.1, 0.5}) + cube := addBody(w, mgl64.Vec3{0.2, 3.1 + cubeHalf, 0.1}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + ball := addBody(w, mgl64.Vec3{0, 1.9, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0.6, 0) + const rope = 1.0 + w.AddJoint(NewDistanceJoint(plate, ball, mgl64.Vec3{0, 2.9, 0}, mgl64.Vec3{0, 1.9, 0})) + simulate(w, 1, nil) + if !plate.IsSleeping || !cube.IsSleeping || !ball.IsSleeping { + t.Fatalf("the plate, the cube and the ball should rest asleep together: %v %v %v", plate.IsSleeping, cube.IsSleeping, ball.IsSleeping) + } + contacts := func() int { + n := 0 + for _, contact := range w.Contacts() { + if (contact.BodyA == plate && contact.BodyB == cube) || (contact.BodyA == cube && contact.BodyB == plate) { + n++ + } + } + return n + } + ropeLength := func() float64 { + return ball.Transform.Position.Sub(plate.Transform.Position.Sub(mgl64.Vec3{0, 0.1, 0})).Len() + } + rides := func(what string) { + t.Helper() + if gap := cube.Transform.Position.Y() - cubeHalf - (plate.Transform.Position.Y() + 0.1); gap > SpeculativeDistance || gap < -landingDepth { + t.Fatalf("%s: the cube is %.2f mm off the plate", what, gap*1000) + } + if stretch := math.Abs(ropeLength() - rope); stretch > landingDepth { + t.Fatalf("%s: the rope is %.2f mm off its length", what, stretch*1000) + } + } + + // ========== kinematic -> dynamic: it falls, with the cube and the ball ========== + if err := w.SetBodyType(plate, actor.BodyTypeDynamic); err != nil { + t.Fatalf("the plate made dynamic: %v", err) + } + if plate.IsSleeping { + t.Fatalf("the plate should wake up when it becomes dynamic") + } + top := plate.Transform.Position.Y() + w.Step(sceneDt) + if contacts() == 0 { + t.Errorf("the contact between the plate and the cube was lost when the plate became dynamic") + } + if fall := top - plate.Transform.Position.Y(); fall > sceneGravity*sceneDt*sceneDt || fall <= 0 { + t.Errorf("the plate moved by %.4f m in the step it became dynamic, want a fall under g dt²", fall) + } + rides("after the switch to dynamic") + simulate(w, 0.5, func() { rides("falling") }) + // a free fall of 31 steps of 8 sub-steps, integrated as the solver does (v then x): g h² n (n + 1) / 2 + n, h := 31.0*sceneSubsteps, sceneDt/sceneSubsteps + if fall, want := top-plate.Transform.Position.Y(), sceneGravity*h*h*n*(n+1)/2; math.Abs(fall-want) > landingDepth { + t.Errorf("the plate fell by %.4f m, want the free fall of %.4f m: something held it", fall, want) + } + + // ========== dynamic -> kinematic: it stops, then is lifted to its targets ========== + if err := w.SetBodyType(plate, actor.BodyTypeKinematic); err != nil { + t.Fatalf("the plate made kinematic again: %v", err) + } + if plate.Velocity != (mgl64.Vec3{}) || plate.AngularVelocity != (mgl64.Vec3{}) { + t.Errorf("a body made kinematic keeps a velocity %v %v", plate.Velocity, plate.AngularVelocity) + } + held := plate.Transform + w.Step(sceneDt) + if plate.Transform != held { + t.Errorf("a body made kinematic moved without a target") + } + simulate(w, 0.5, nil) // the cube and the ball settle on the stopped plate + drive(w, plate, mgl64.Vec3{0, kinematicSpeed, 0}, 1, func() { + rides("lifted") + }) + if contacts() == 0 { + t.Errorf("no contact between the plate and the cube it lifts") + } + if plate.Transform.Position.Y() < top-0.5 { + t.Errorf("the plate is at y = %.3f after being lifted by 1 m", plate.Transform.Position.Y()) + } +} + +// SetBodyType only changes a body between kinematic and dynamic: a static body is another kind of body, and a +// kinematic body created without a density has no mass to become dynamic. Both are refused with an error (never in +// silence) and left as they are; a body already of the type is nothing to do. The refusal allocates nothing +func TestSetBodyTypeRefusesTheStaticBodies(t *testing.T) { + expectError := func(what string, want error, fn func() error) { + t.Helper() + if err := fn(); !errors.Is(err, want) { + t.Errorf("%s: error %v, want %v", what, err, want) + } + } + w := newScene(1) + static := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), cube(), actor.BodyTypeStatic, 0.6, 0) + dynamic := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + massless := actor.NewRigidBody(actor.NewTransform(), cube(), actor.BodyTypeKinematic, 0) + w.AddBody(massless) + expectError("a static body made kinematic", ErrStaticBody, func() error { return w.SetBodyType(static, actor.BodyTypeKinematic) }) + expectError("a static body made dynamic", ErrStaticBody, func() error { return w.SetBodyType(static, actor.BodyTypeDynamic) }) + expectError("a dynamic body made static", ErrStaticBody, func() error { return w.SetBodyType(dynamic, actor.BodyTypeStatic) }) + expectError("a kinematic body without mass made dynamic", ErrMasslessBody, func() error { return w.SetBodyType(massless, actor.BodyTypeDynamic) }) + if static.BodyType != actor.BodyTypeStatic || dynamic.BodyType != actor.BodyTypeDynamic || massless.BodyType != actor.BodyTypeKinematic { + t.Errorf("a refused body changed its type: %v %v %v", static.BodyType, dynamic.BodyType, massless.BodyType) + } + if err := w.SetBodyType(dynamic, actor.BodyTypeDynamic); err != nil { + t.Errorf("a body already of the type: %v, want nothing to do", err) + } + if allocs := testing.AllocsPerRun(10, func() { _ = w.SetBodyType(static, actor.BodyTypeKinematic) }); allocs > 0 { + t.Errorf("a refusal allocates %.1f, want 0", allocs) + } +} + +// ========== SLEEP ========== + +// A kinematic body which stops falls asleep with the bodies resting on it, in the same island; a target wakes them all +// up, and a target which moves it by a millimeter per second (under the sleep speed) keeps them awake: a kinematic body +// goes where it is told, there is no jitter to filter +func TestKinematicSleepsWithItsIsland(t *testing.T) { + w := newScene(1) + plate := kinematicBox(w, mgl64.Vec3{0, 1, 0}, mgl64.Vec3{1, 0.1, 1}) + rider := addBody(w, mgl64.Vec3{0, 1.1 + cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + sleeps, wakes := 0, 0 + w.Events.Subscribe(EventSleep, func(Event) { sleeps++ }) + w.Events.Subscribe(EventWake, func(Event) { wakes++ }) + drive(w, plate, mgl64.Vec3{kinematicSpeed, 0, 0}, 1, func() { + if plate.IsSleeping || rider.IsSleeping { + t.Fatalf("asleep while the plate moves: plate %v, rider %v", plate.IsSleeping, rider.IsSleeping) + } + }) + // no target: the plate stops at once, its rider slides on it (µ = 0.6: 1 m/s in 0.17 s) and rests, both sleep + // DefaultTimeToSleep later + simulate(w, kinematicSpeed/(0.6*sceneGravity)+actor.DefaultTimeToSleep+3*sceneDt, nil) + if !plate.IsSleeping || !rider.IsSleeping { + t.Fatalf("the plate stopped: plate asleep %v, rider asleep %v, want both asleep", plate.IsSleeping, rider.IsSleeping) + } + if sleeps != 2 { + t.Errorf("%d sleep events, want 2 (the plate and its rider)", sleeps) + } + + // a slow target wakes the island, and keeps it awake + start := plate.Transform.Position.X() + drive(w, plate, mgl64.Vec3{0.001, 0, 0}, 1, func() { + if plate.IsSleeping || rider.IsSleeping { + t.Fatalf("asleep while the plate crawls: plate %v, rider %v", plate.IsSleeping, rider.IsSleeping) + } + }) + if wakes != 2 { + t.Errorf("%d wake events, want 2", wakes) + } + if moved, carried := plate.Transform.Position.X()-start, rider.Transform.Position.X()-start; math.Abs(carried-moved) > landingDepth { + t.Errorf("the plate crawled by %.2f mm, its rider by %.2f mm: not carried", moved*1000, carried*1000) + } +} + +// A kinematic body arriving on a sleeping body wakes it up, and the sleeping bodies near a teleported body wake up +func TestKinematicWakesWhatItTouches(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + box := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + pusher := kinematicBox(w, mgl64.Vec3{-1, cubeHalf, 0}, cube().HalfExtents) + simulate(w, 1, nil) + if !box.IsSleeping { + t.Fatalf("the box should be asleep") + } + drive(w, pusher, mgl64.Vec3{kinematicSpeed, 0, 0}, 1, nil) + if box.IsSleeping || box.Transform.Position.X() < 0.3 { + t.Errorf("the box was not woken up and pushed: asleep %v, x = %.3f", box.IsSleeping, box.Transform.Position.X()) + } + + other := addBody(w, mgl64.Vec3{5, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 1, nil) + if !other.IsSleeping { + t.Fatalf("the other box should be asleep") + } + w.Teleport(pusher, actor.Transform{Position: mgl64.Vec3{5 - 2*cubeHalf - SpeculativeDistance/2, cubeHalf, 0}, Rotation: mgl64.QuatIdent()}) + if other.IsSleeping { + t.Errorf("the box next to the teleported body was not woken up") + } +} + +// ========== JOINTS & FORCES ========== + +// A ball hanging from a moving kinematic body by a distance joint follows it: the joint pulls the ball with the +// velocity of the kinematic body +func TestKinematicJointPullsADynamicBody(t *testing.T) { + w := newScene(1) + hook := kinematicBox(w, mgl64.Vec3{0, 3, 0}, mgl64.Vec3{0.1, 0.1, 0.1}) + ball := addBody(w, mgl64.Vec3{0, 2, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0.6, 0) + w.AddJoint(NewDistanceJoint(hook, ball, mgl64.Vec3{0, 2.9, 0}, mgl64.Vec3{0, 2, 0})) + simulate(w, 1, nil) + drive(w, hook, mgl64.Vec3{0, kinematicSpeed, 0}, 1, nil) + if stretch := math.Abs(ball.Transform.Position.Sub(hook.Transform.Position).Len() - 1); stretch > landingDepth { + t.Errorf("the rope is %.2f mm off its length while lifted", stretch*1000) + } + if math.Abs(ball.Velocity.Y()-kinematicSpeed) > 0.1*kinematicSpeed { + t.Errorf("the ball rises at %.3f m/s, want about %.0f m/s", ball.Velocity.Y(), kinematicSpeed) + } +} + +// Forces, torques and impulses don't move a kinematic body, and neither does the gravity +func TestKinematicIgnoresTheForces(t *testing.T) { + w := newScene(1) + body := kinematicBox(w, mgl64.Vec3{0, 1, 0}, cube().HalfExtents) + start := body.Transform + for step := 0; step < 60; step++ { + body.AddForce(mgl64.Vec3{100, 100, 100}) + body.AddTorque(mgl64.Vec3{10, 10, 10}) + body.AddImpulse(mgl64.Vec3{1, 1, 1}) + body.AddAngularImpulse(mgl64.Vec3{1, 1, 1}) + w.Step(sceneDt) + } + if body.Transform != start || body.Velocity != (mgl64.Vec3{}) || body.AngularVelocity != (mgl64.Vec3{}) { + t.Errorf("the kinematic body moved: %v, velocity %v %v", body.Transform, body.Velocity, body.AngularVelocity) + } +} + +// ========== DETERMINISM & ALLOCATIONS ========== + +// kinematicPile: a pile of boxes and spheres on the ground, 2 kinematic plates sweeping through it and a kinematic +// plate carrying part of it, on parallel paths whatever the count of bodies +func kinematicPile(count, workers int) (*World, []*actor.RigidBody) { + w := benchScene(count, workers) + w.parallelFrom = 1 + movers := []*actor.RigidBody{ + kinematicBox(w, mgl64.Vec3{-3, 0.3, 0}, mgl64.Vec3{0.2, 0.3, 2}), + kinematicBox(w, mgl64.Vec3{0, 0.3, -3}, mgl64.Vec3{2, 0.3, 0.2}), + kinematicBox(w, mgl64.Vec3{0, 0.05, 0}, mgl64.Vec3{1, 0.05, 1}), + } + return w, movers +} + +// driveMovers runs the pile for seconds: the plates sweep along X and Z, the carrier rises and turns +func driveMovers(w *World, movers []*actor.RigidBody, seconds float64) { + for i := 0; i < int(math.Round(seconds/sceneDt)); i++ { + moveTo(movers[0], translated(movers[0].Transform, mgl64.Vec3{kinematicSpeed, 0, 0})) + moveTo(movers[1], translated(movers[1].Transform, mgl64.Vec3{0, 0, kinematicSpeed})) + carrier := translated(movers[2].Transform, mgl64.Vec3{0, 0.2, 0}) + carrier.Rotation = mgl64.QuatRotate(0.5*sceneDt, mgl64.Vec3{0, 1, 0}).Mul(carrier.Rotation).Normalize() + moveTo(movers[2], carrier) + w.Step(sceneDt) + } +} + +// The bits of a pile swept by kinematic bodies are the same with 1, 4 and 8 workers +func TestKinematicDeterminism(t *testing.T) { + run := func(workers int) []uint64 { + w, movers := kinematicPile(300, workers) + driveMovers(w, movers, 2) + var out []uint64 + for _, b := range w.Bodies { + for _, v := range [3]mgl64.Vec3{b.Transform.Position, b.Transform.Rotation.V, b.Velocity} { + for _, x := range v { + out = append(out, math.Float64bits(x)) + } + } + } + return out + } + reference := run(1) + for _, workers := range []int{4, 8} { + got := run(workers) + for i := range reference { + if got[i] != reference[i] { + t.Fatalf("workers=%d: value %d differs: %x, want %x", workers, i, got[i], reference[i]) + } + } + } +} + +// carriedPile: the pile of the bench scene dropped on a kinematic plate, which carries it round (a circle of 1 m at +// kinematicSpeed), on parallel paths whatever the count of bodies. Once the pile has landed, its pairs don't change: +// the step allocates nothing (as a pile on the ground; a body sweeping through a pile makes new pairs at every step, +// and the buffers of the contacts grow with them) +func carriedPile(count, workers int) (*World, func()) { + w := newScene(workers) + w.parallelFrom = 1 + addGround(w, 0.6) + plate := kinematicBox(w, mgl64.Vec3{0, 0.1, 0}, mgl64.Vec3{4, 0.1, 4}) + pile := benchScene(count, 1) + for _, body := range pile.Bodies[1:] { + addBody(w, body.Transform.Position.Add(mgl64.Vec3{0, 0.2, 0}), mgl64.QuatIdent(), body.Shape, actor.BodyTypeDynamic, 0.6, 0) + } + angle := 0.0 + return w, func() { + angle += kinematicSpeed * sceneDt // the plate goes round a circle of radius 1 m + target := plate.Transform + target.Position = mgl64.Vec3{math.Cos(angle) - 1, 0.1, math.Sin(angle)} + moveTo(plate, target) + w.Step(sceneDt) + } +} + +// A step with kinematic bodies allocates nothing, once the buffers have grown +func TestKinematicStepDoesNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + for _, workers := range []int{1, 8} { + w, step := carriedPile(300, workers) + for i := 0; i < 120; i++ { + step() + } + carried := 0 + for _, contact := range w.Contacts() { + if contact.BodyA.BodyType == actor.BodyTypeKinematic || contact.BodyB.BodyType == actor.BodyTypeKinematic { + carried++ + } + } + if carried < 50 { + t.Fatalf("%d contacts with the plate: the scene doesn't cover the solver", carried) + } + allocs := testing.AllocsPerRun(10, step) + if allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", workers, allocs) + } + } +} + +// BenchmarkCarriedPile: the 300 bodies of the bench scene resting on a plate of 8 x 8 m, the plate kinematic and going +// round at 1 m/s (carriedPile) against the same plate static: the same contacts, the kinematic plate keeps them awake +// and rubbing. 2 s once the pile has landed +func BenchmarkCarriedPile(b *testing.B) { + for _, kinematic := range []bool{false, true} { + for _, workers := range []int{1, 8} { + name := "static_plate" + if kinematic { + name = "kinematic_plate" + } + b.Run(fmt.Sprintf("%s_%d_workers", name, workers), func(b *testing.B) { + b.ReportAllocs() + for i := 0; i < b.N; i++ { + b.StopTimer() + w, step := carriedPile(300, workers) + w.parallelFrom = 0 // the paths of a game: parallel from minParallelBodies + if !kinematic { + for _, body := range w.Bodies { + if body.BodyType == actor.BodyTypeKinematic { + body.BodyType = actor.BodyTypeStatic + } + } + step = func() { w.Step(sceneDt) } + } + for k := 0; k < 120; k++ { + step() + } + b.StartTimer() + for k := 0; k < 120; k++ { + step() + } + } + }) + } + } +} diff --git a/lock_test.go b/lock_test.go index 213e4db..92a59e7 100644 --- a/lock_test.go +++ b/lock_test.go @@ -529,14 +529,10 @@ func TestContinuousKeepsTheLockedAxes(t *testing.T) { // motions of a step through the ground, as if the solver had accelerated the box w.Step(sceneDt) - scratch := ccdPool.Get().(*ccdScratch) - scratch.core.Radius = coreFraction * cubeHalf for i := 0; i < 20; i++ { turned := mgl64.QuatRotate(0.3*float64(i), mgl64.Vec3{1, float64(i), 2}.Normalize()).Normalize() height := 3 + 0.1*float64(i) - motion := sweep{start: actor.Transform{Position: mgl64.Vec3{0.3, height, 0.4}, Rotation: turned}, end: actor.Transform{Position: mgl64.Vec3{0.3, -3, 1}, Rotation: turned}} - box.Transform = motion.end - w.stopAtImpact(box, &motion, cube().HalfExtents.Len(), scratch) + moveThrough(w, box, sweep{start: actor.Transform{Position: mgl64.Vec3{0.3, height, 0.4}, Rotation: turned}, end: actor.Transform{Position: mgl64.Vec3{0.3, -3, 1}, Rotation: turned}}) if y := box.Transform.Position.Y(); y < 0 || y > height { t.Fatalf("the box is at y=%.3f, want it stopped above the ground", y) } @@ -544,7 +540,6 @@ func TestContinuousKeepsTheLockedAxes(t *testing.T) { t.Errorf("the stopped box is at %v, want x=%v and the rotation %v bit for bit", box.Transform, start.Position.X(), turned) } } - ccdPool.Put(scratch) box.Transform = start simulate(w, 2, func() { @@ -1002,7 +997,7 @@ func TestLockedInverse2(t *testing.T) { // tangents by the locked axis. frictionMass is the inverse of K = Tᵀ (MA⁻¹ + MB⁻¹) T (the bodies don't turn here) func TestLockedFrictionMass(t *testing.T) { const inverseMass = 0.25 - locked := bodyState{body: &actor.RigidBody{}, invMass: inverseMass, invMassAxes: mgl64.Vec3{inverseMass, inverseMass, 0}, linearLock: actor.AxisZ} + locked := bodyState{body: &actor.RigidBody{}, dynamic: true, invMass: inverseMass, invMassAxes: mgl64.Vec3{inverseMass, inverseMass, 0}, linearLock: actor.AxisZ} static := bodyState{} for _, normal := range []mgl64.Vec3{{0.3, 0.8, 0.52}, {0.7, 0.1, -0.7}, {0, 1, 0}} { for _, order := range [][2]*bodyState{{&locked, &static}, {&static, &locked}} { @@ -1217,18 +1212,13 @@ func TestContinuousTurnsAPartlyLockedBody(t *testing.T) { addGround(w, 0.5) box := addBody(w, mgl64.Vec3{0, 3, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) w.Step(sceneDt) - scratch := ccdPool.Get().(*ccdScratch) - defer ccdPool.Put(scratch) - scratch.core.Radius = coreFraction * cubeHalf const turn = 1.2 for _, lock := range []actor.Axes{actor.AxisX, actor.AxisX | actor.AxisZ} { box.AngularLock = lock - motion := sweep{ + moveThrough(w, box, sweep{ start: actor.Transform{Position: mgl64.Vec3{0, 3, 0}, Rotation: mgl64.QuatIdent()}, end: actor.Transform{Position: mgl64.Vec3{0, -3, 0}, Rotation: mgl64.QuatRotate(turn, mgl64.Vec3{0, 1, 0})}, - } - box.Transform = motion.end - w.stopAtImpact(box, &motion, cube().HalfExtents.Len(), scratch) + }) y := box.Transform.Position.Y() if y < 0 || y > 3 { t.Fatalf("locks %d: the box is at y=%.3f, want it stopped above the ground", lock, y) diff --git a/query.go b/query.go index 3461cd5..aa5bf6b 100644 --- a/query.go +++ b/query.go @@ -49,8 +49,8 @@ type Hit struct { Point mgl64.Vec3 // world space, on the surface of Body Normal mgl64.Vec3 // unit, out of Body, at Point Fraction float64 // of the translation, in [0, 1] - // heightfield: 2*cell + t (cell = x*(ZSamples-1)+z), the lowest index if several triangles are hit at the same - // fraction (a ray on an edge); else actor.NoTriangle + // heightfield: 2*cell + t (cell = x*(ZSamples-1)+z); mesh: the index of the triangle; the lowest index if several + // triangles are hit at the same fraction (a ray on an edge); else actor.NoTriangle Triangle int32 // index of Body in World.Bodies when it was hit: the order of the hits at the same fraction index int32 @@ -75,6 +75,9 @@ type treeQuery struct { hits []Hit // sweep: the query moves a shape (query_sweep.go), else it is a ray sweep *sweepQuery + // character: the sweep is the one of a character (character_move.go): the bodies it collides with, the hits it + // keeps + character *CharacterVirtual } // run the query on the large bodies, then on both trees @@ -90,7 +93,7 @@ func (q *treeQuery) run() { // visit the body of a leaf, or a large body: its hit is kept if it comes first func (q *treeQuery) visit(index int32) { body := q.world.Bodies[index] - if !q.filter.Accepts(body) { + if !q.filter.Accepts(body) || (q.character != nil && !q.character.sweepAccepts(body)) { return } var found Hit @@ -106,6 +109,9 @@ func (q *treeQuery) visit(index int32) { } found = Hit{Body: body, Point: q.ray.origin.Add(q.ray.translation.Mul(hit.Fraction)), Normal: hit.Normal, Fraction: hit.Fraction, Triangle: hit.Triangle} } + if q.character != nil && !q.character.sweepKeeps(&found, q.sweep.translation) { + return + } found.index = index if q.all { q.hits = append(q.hits, found) @@ -149,7 +155,8 @@ func finiteSegment(origin, translation mgl64.Vec3) bool { // Raycast: the first body on the segment from origin to origin + translation, among the bodies the filter accepts. // A ray which starts in a body hits it at the fraction 0, the normal against its direction; a ray without length is a -// point, its hit has no normal. The top side of a heightfield only is hit. A ray which is not finite hits nothing +// point, its hit has no normal. The top side of a heightfield only is hit, the side of the normal of the triangles of a +// mesh. A ray which is not finite hits nothing func (w *World) Raycast(origin, translation mgl64.Vec3, filter QueryFilter) (Hit, bool) { w.guard() if !finiteSegment(origin, translation) { diff --git a/query_heightfield.go b/query_heightfield.go deleted file mode 100644 index ec1de9e..0000000 --- a/query_heightfield.go +++ /dev/null @@ -1,71 +0,0 @@ -package feather - -import ( - "github.com/akmonengine/feather/actor" - "github.com/akmonengine/feather/gjk" - "github.com/go-gl/mathgl/mgl64" -) - -// ========== QUERIES ON A HEIGHTFIELD ========== - -// sweepHeightfield: the moving shape against the triangles of the cells under its path, in the order of its motion -// (Heightfield.WalkCells, as b3ShapeCastHeightField of Box3D): a band of the width of the shape, not the whole AABB -// of the motion. The walk ends when the shape enters the next cells after its best hit -func (q *sweepQuery) sweepHeightfield(terrain *actor.RigidBody, field *actor.Heightfield) (Hit, bool) { - // the motion in the local space of the terrain: the AABB of the shape there, moved along the translation - rotation := &terrain.Transform.Rotation - conjugate := mgl64.Quat{W: rotation.W, V: rotation.V.Mul(-1)} - local := actor.Transform{ - Position: actor.RotateInverse(rotation, q.start.Position.Sub(terrain.Transform.Position)), - Rotation: actor.MulQuat(&conjugate, &q.start.Rotation), - } - bounds := q.shape.ComputeAABB(local) - reach := mgl64.Vec3{2 * sweepFlatGap, 2 * sweepFlatGap, 2 * sweepFlatGap} - walk := field.WalkCells(bounds.Min.Add(bounds.Max).Mul(0.5), actor.RotateInverse(rotation, q.translation), bounds.Max.Sub(bounds.Min).Mul(0.5).Add(reach), q.limit) - - best, found := Hit{Fraction: q.limit}, false - cellsZ := field.ZSamples - 1 - for { - x, z, ok := walk.Next(best.Fraction) - if !ok { - break - } - for t := 0; t < 2; t++ { - vertices, _ := field.Triangle(x, z, t) - for i := range vertices { - vertices[i] = terrain.Transform.Position.Add(actor.Rotate(rotation, vertices[i])) - } - hit, ok := q.sweepTriangle(vertices, best.Fraction) - hit.Triangle = int32(2*(x*cellsZ+z) + t) - // at the same fraction, the triangle of the lowest index - if ok && (!found || hit.Fraction < best.Fraction || (hit.Fraction == best.Fraction && hit.Triangle < best.Triangle)) { - best, found = hit, true - } - } - } - return best, found -} - -// overlapsHeightfield: a triangle of the cells under the bounds of the shape overlaps it, from any side. A hole has -// no triangle -func (s *queryScratch) overlapsHeightfield(core *gjk.Proxy, radius float64, bounds actor.AABB, terrain *actor.RigidBody, field *actor.Heightfield) bool { - s.cells = field.OverlapCells(localBounds(terrain.Transform, bounds), s.cells[:0]) - cellsZ := field.ZSamples - 1 - for _, cell := range s.cells { - for t := 0; t < 2; t++ { - vertices, _ := field.Triangle(int(cell)/cellsZ, int(cell)%cellsZ, t) - for i := range vertices { - vertices[i] = terrain.Transform.ToWorld(vertices[i]) - } - s.shape.vertices, s.shape.aabb = vertices, triangleAABB(vertices) - if !s.shape.aabb.Overlaps(bounds) { - continue - } - proxy := gjk.NewProxyAt(s.triangle.Transform, &s.shape) - if coresOverlap(core, radius, &proxy, 0) { - return true - } - } - } - return false -} diff --git a/query_overlap.go b/query_overlap.go index 08a1a2e..d95ee8b 100644 --- a/query_overlap.go +++ b/query_overlap.go @@ -15,8 +15,8 @@ import ( // Overlap appends to bodies the bodies which overlap the convex shape (a sphere, a capsule, a box) at the transform, // among the bodies the filter accepts, in the order of World.Bodies, and returns the slice: no allocation if its -// capacity is enough. A body in contact with the shape overlaps it. A heightfield overlaps the shape when one of its -// triangles does, from any side. A plane or a heightfield as the shape panics +// capacity is enough. A body in contact with the shape overlaps it. A heightfield or a mesh overlaps the shape when one +// of its triangles does, from any side. A plane, a heightfield or a mesh as the shape panics func (w *World) Overlap(shape actor.ShapeInterface, at actor.Transform, filter QueryFilter, bodies []*actor.RigidBody) []*actor.RigidBody { w.guard() mustBeConvex(shape) @@ -43,6 +43,8 @@ func (w *World) Overlap(shape actor.ShapeInterface, at actor.Transform, filter Q overlaps = lowest.Dot(other.Normal)+other.Distance <= 0 case *actor.Heightfield: overlaps = scratch.overlapsHeightfield(&core, radius, bounds, body, other) + case *actor.TriangleMesh: + overlaps = scratch.overlapsMesh(&core, radius, bounds, body, other) default: otherCore, otherRadius := gjk.NewCoreProxy(body) overlaps = coresOverlap(&core, radius, &otherCore, otherRadius) diff --git a/query_sweep.go b/query_sweep.go index 4b5b333..7858471 100644 --- a/query_sweep.go +++ b/query_sweep.go @@ -4,6 +4,7 @@ import ( "sync" "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/epa" "github.com/akmonengine/feather/gjk" "github.com/go-gl/mathgl/mgl64" ) @@ -33,31 +34,41 @@ const ( sweepFlatGap = 0.5e-4 // notConvex: the panic of a sweep or an overlap of a shape which is not convex - notConvex = "feather: a query needs a convex shape, not a plane nor a heightfield" + notConvex = "feather: a query needs a convex shape, not a plane, a heightfield nor a mesh" ) // queryScratch: the buffers of a query, reused to avoid the allocations type queryScratch struct { // mover: the moving shape as a body, for the overlaps (penetration needs bodies) mover actor.RigidBody - // a triangle of a terrain, in world space, and its body - shape triangleShape - triangle actor.RigidBody - simplex gjk.Simplex + // a triangle of a surface, in world space, its body and its proxy for GJK (made once: the shape is this one, at + // the identity) + shape triangleShape + triangle actor.RigidBody + triangleProxy gjk.Proxy + simplex gjk.Simplex + // epa: the buffers of EPA of the character which owns the scratch, nil for those of the pool of EPA + epa *epa.Scratch // sweep: the query of a moving shape. It points to itself (treeQuery.sweep): it cannot live on a stack sweep sweepQuery - // buffers of Overlap: the stack and the candidates of the trees, the cells of a terrain + // buffers of Overlap: the stack and the candidates of the trees, the cells of a terrain, the triangles of a mesh stack []int32 candidates []int32 cells []int32 + triangles []int32 } -var queryPool = sync.Pool{New: func() any { +// newQueryScratch: the buffers, the triangle of the surfaces made. The queries take theirs from queryPool; a character +// keeps its own (CharacterVirtual), as its contactScratch +func newQueryScratch() *queryScratch { s := &queryScratch{} s.mover = actor.RigidBody{BodyType: actor.BodyTypeStatic} s.triangle = actor.RigidBody{Transform: actor.NewTransform(), BodyType: actor.BodyTypeStatic, Shape: &s.shape} + s.triangleProxy = gjk.NewProxyAt(s.triangle.Transform, &s.shape) return s -}} +} + +var queryPool = sync.Pool{New: func() any { return newQueryScratch() }} // sweepQuery: a shape moving through a world type sweepQuery struct { @@ -71,14 +82,23 @@ type sweepQuery struct { scratch *queryScratch } -// mustBeConvex: a plane and a heightfield have no support point, they cannot be moved nor overlapped +// mustBeConvex: a plane, a heightfield and a mesh have no support point, they cannot be moved nor overlapped func mustBeConvex(shape actor.ShapeInterface) { - switch shape.(type) { - case *actor.Plane, *actor.Heightfield: + if !isConvex(shape) { panic(notConvex) } } +// isConvex: a plane, a heightfield and a mesh have no support point: they are surfaces, the shapes of the static +// bodies; every other shape is a convex volume GJK/EPA can move +func isConvex(shape actor.ShapeInterface) bool { + switch shape.(type) { + case *actor.Plane, *actor.Heightfield, *actor.TriangleMesh: + return false + } + return true +} + // newSweepQuery of the shape moved from start by translation. Through the trees, the shape is the center of its AABB, // and the AABBs of the nodes are enlarged by its half sizes func newSweepQuery(w *World, shape actor.ShapeInterface, start actor.Transform, translation mgl64.Vec3, filter QueryFilter, scratch *queryScratch) *sweepQuery { @@ -99,14 +119,15 @@ func newSweepQuery(w *World, shape actor.ShapeInterface, start actor.Transform, // without rotation, among the bodies the filter accepts. The shape stops between 0 and 1 µm from the body (0.1 mm // between 2 shapes without radius), never in it: Fraction is where it stops, Point the point of the body it touches, // Normal the direction from this point to the shape. A shape which starts in a body hits it at the fraction 0, the -// normal against its motion, at a point in both. The top side of a heightfield only is hit. +// normal against its motion, at a point in both. The top side of a heightfield only is hit, the side of the normal of +// the triangles of a mesh. // A shape which starts in exact contact with a body, or within 1 µm of it, hits it at the fraction 0 with the normal of // the contact if it moves towards the body, and doesn't hit it if it moves away or along it (the back facing test of // the shape casts of Jolt, ConvexShape.cpp; Box3D reports an initial overlap whatever the direction). The exact // contact is decided by the rounding: a shape put in contact by computed coordinates can be seen in the body, and // stopped whatever its direction. Start from the place given by a Sweep, or keep a skin. // After sweepIterations the hit is given where the shape is, before the contact (Box3D gives no hit). -// A plane or a heightfield as the moving shape panics +// A plane, a heightfield or a mesh as the moving shape panics func (w *World) Sweep(shape actor.ShapeInterface, start actor.Transform, translation mgl64.Vec3, filter QueryFilter) (Hit, bool) { w.guard() mustBeConvex(shape) @@ -121,11 +142,16 @@ func (w *World) Sweep(shape actor.ShapeInterface, start actor.Transform, transla return best, found } -// release the scratch: it keeps no body nor shape of the world alive +// release the scratch to the pool, cleared func (s *queryScratch) release() { + s.clear() + queryPool.Put(s) +} + +// clear the scratch: it keeps no body nor shape of the world alive +func (s *queryScratch) clear() { s.sweep = sweepQuery{} s.mover.Shape = nil - queryPool.Put(s) } // sweepBody: the hit of the moving shape on the body, before the limit of the query @@ -137,6 +163,8 @@ func (q *sweepQuery) sweepBody(body *actor.RigidBody) (Hit, bool) { hit, ok = q.sweepPlane(shape) case *actor.Heightfield: hit, ok = q.sweepHeightfield(body, shape) + case *actor.TriangleMesh: + hit, ok = q.sweepMesh(body, shape) default: // the exact AABB of the body, after the AABB of its leaf box := body.AABB() @@ -202,7 +230,7 @@ func (q *sweepQuery) sweepConvex(other *gjk.Proxy, otherRadius, limit float64) ( // overlapPoint: a point in both the moving shape at its start and the body: the middle of their deepest points func (q *sweepQuery) overlapPoint(body *actor.RigidBody) mgl64.Vec3 { - result, ok := penetration(&q.scratch.mover, body, 0, &q.scratch.simplex) + result, ok := penetration(&q.scratch.mover, body, 0, &q.scratch.simplex, q.scratch.epa) if !ok { return q.start.Position } @@ -239,7 +267,7 @@ func (q *sweepQuery) sweepPlane(plane *actor.Plane) (Hit, bool) { // sweepTriangle: the hit of the moving shape on a triangle in world space, its vertices turning around its normal, // before the limit. Only the side of the normal is hit: a triangle is skipped if the shape moves along its normal (it -// comes from behind, or leaves). The triangles of a mesh will be swept the same way +// comes from behind, or leaves). The triangles of a heightfield and of a mesh are swept this way func (q *sweepQuery) sweepTriangle(vertices [3]mgl64.Vec3, limit float64) (Hit, bool) { normal := vertices[1].Sub(vertices[0]).Cross(vertices[2].Sub(vertices[0])) if q.translation.Dot(normal) > 0 { @@ -251,8 +279,7 @@ func (q *sweepQuery) sweepTriangle(vertices [3]mgl64.Vec3, limit float64) (Hit, } s := q.scratch s.shape.vertices, s.shape.aabb = vertices, box - proxy := gjk.NewProxyAt(s.triangle.Transform, &s.shape) - hit, overlap, ok := q.sweepConvex(&proxy, 0, limit) + hit, overlap, ok := q.sweepConvex(&s.triangleProxy, 0, limit) if overlap { hit.Point = q.overlapPoint(&s.triangle) } diff --git a/query_triangles.go b/query_triangles.go new file mode 100644 index 0000000..9f9d698 --- /dev/null +++ b/query_triangles.go @@ -0,0 +1,122 @@ +package feather + +import ( + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/gjk" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== QUERIES ON A SURFACE ========== +// A heightfield and a triangle mesh are swept and overlapped triangle by triangle: the triangles come from the cells +// under the path of the shape (Heightfield.WalkCells), or from the tree of the mesh (TriangleMesh.Cast, +// OverlapTriangles); the test of a triangle is the same (sweepTriangle, overlapsTriangle) + +// localMotion: the transform of the moving shape and its translation in the local space of the surface +func (q *sweepQuery) localMotion(surface *actor.RigidBody) (actor.Transform, mgl64.Vec3) { + rotation := &surface.Transform.Rotation + conjugate := mgl64.Quat{W: rotation.W, V: rotation.V.Mul(-1)} + local := actor.Transform{ + Position: actor.RotateInverse(rotation, q.start.Position.Sub(surface.Transform.Position)), + Rotation: actor.MulQuat(&conjugate, &q.start.Rotation), + } + return local, actor.RotateInverse(rotation, q.translation) +} + +// sweepHeightfield: the moving shape against the triangles of the cells under its path, in the order of its motion +// (Heightfield.WalkCells, as b3ShapeCastHeightField of Box3D): a band of the width of the shape, not the whole AABB +// of the motion. The walk ends when the shape enters the next cells after its best hit +func (q *sweepQuery) sweepHeightfield(terrain *actor.RigidBody, field *actor.Heightfield) (Hit, bool) { + // the motion in the local space of the terrain: the AABB of the shape there, moved along the translation + local, translation := q.localMotion(terrain) + bounds := q.shape.ComputeAABB(local) + reach := mgl64.Vec3{2 * sweepFlatGap, 2 * sweepFlatGap, 2 * sweepFlatGap} + walk := field.WalkCells(bounds.Min.Add(bounds.Max).Mul(0.5), translation, bounds.Max.Sub(bounds.Min).Mul(0.5).Add(reach), q.limit) + + best, found := Hit{Fraction: q.limit}, false + cellsZ := field.ZSamples - 1 + for { + x, z, ok := walk.Next(best.Fraction) + if !ok { + break + } + for t := 0; t < 2; t++ { + vertices, _ := field.Triangle(x, z, t) + q.sweepLocalTriangle(terrain, vertices, int32(2*(x*cellsZ+z)+t), &best, &found) + } + } + return best, found +} + +// sweepMesh: the moving shape against the triangles of the leaves of the tree along its path, in the order of its +// motion (TriangleMesh.Cast, as b3ShapeCastMesh of Box3D); the nodes entered after the best hit are dropped +func (q *sweepQuery) sweepMesh(body *actor.RigidBody, mesh *actor.TriangleMesh) (Hit, bool) { + local, translation := q.localMotion(body) + bounds := q.shape.ComputeAABB(local) + reach := mgl64.Vec3{2 * sweepFlatGap, 2 * sweepFlatGap, 2 * sweepFlatGap} + cast := mesh.Cast(bounds.Min.Add(bounds.Max).Mul(0.5), translation, bounds.Max.Sub(bounds.Min).Mul(0.5).Add(reach), q.limit) + + best, found := Hit{Fraction: q.limit}, false + for { + t, ok := cast.Next(best.Fraction) + if !ok { + break + } + vertices, _ := mesh.Triangle(t) + q.sweepLocalTriangle(body, vertices, t, &best, &found) + } + return best, found +} + +// sweepLocalTriangle: the triangle of the surface, in its local space, taken to world space and swept; the hit is +// kept if it comes first (at the same fraction, the triangle of the lowest index) +func (q *sweepQuery) sweepLocalTriangle(surface *actor.RigidBody, vertices [3]mgl64.Vec3, triangle int32, best *Hit, found *bool) { + rotation := &surface.Transform.Rotation + for i := range vertices { + vertices[i] = surface.Transform.Position.Add(actor.Rotate(rotation, vertices[i])) + } + hit, ok := q.sweepTriangle(vertices, best.Fraction) + hit.Triangle = triangle + if ok && (!*found || hit.Fraction < best.Fraction || (hit.Fraction == best.Fraction && hit.Triangle < best.Triangle)) { + *best, *found = hit, true + } +} + +// overlapsHeightfield: a triangle of the cells under the bounds of the shape overlaps it, from any side. A hole has +// no triangle +func (s *queryScratch) overlapsHeightfield(core *gjk.Proxy, radius float64, bounds actor.AABB, terrain *actor.RigidBody, field *actor.Heightfield) bool { + s.cells = field.OverlapCells(localBounds(terrain.Transform, bounds), s.cells[:0]) + cellsZ := field.ZSamples - 1 + for _, cell := range s.cells { + for t := 0; t < 2; t++ { + vertices, _ := field.Triangle(int(cell)/cellsZ, int(cell)%cellsZ, t) + if s.overlapsTriangle(core, radius, bounds, terrain, vertices) { + return true + } + } + } + return false +} + +// overlapsMesh: a triangle of the leaves under the bounds of the shape overlaps it, from any side +func (s *queryScratch) overlapsMesh(core *gjk.Proxy, radius float64, bounds actor.AABB, body *actor.RigidBody, mesh *actor.TriangleMesh) bool { + s.triangles = mesh.OverlapTriangles(localBounds(body.Transform, bounds), s.triangles[:0]) + for _, t := range s.triangles { + vertices, _ := mesh.Triangle(t) + if s.overlapsTriangle(core, radius, bounds, body, vertices) { + return true + } + } + return false +} + +// overlapsTriangle: the triangle of the surface, in its local space, overlaps the core of the shape within its radius +func (s *queryScratch) overlapsTriangle(core *gjk.Proxy, radius float64, bounds actor.AABB, surface *actor.RigidBody, vertices [3]mgl64.Vec3) bool { + for i := range vertices { + vertices[i] = surface.Transform.ToWorld(vertices[i]) + } + s.shape.vertices, s.shape.aabb = vertices, triangleAABB(vertices) + if !s.shape.aabb.Overlaps(bounds) { + return false + } + return coresOverlap(core, radius, &s.triangleProxy, 0) +} diff --git a/shape.go b/shape.go new file mode 100644 index 0000000..34ba4e0 --- /dev/null +++ b/shape.go @@ -0,0 +1,87 @@ +package feather + +import ( + "errors" + + "github.com/akmonengine/feather/actor" +) + +// The shape and the density of a body changed while it is in the world (World.SetShape, World.SetDensity): the body +// keeps its index in World.Bodies, its proxy in the broad phase, its joints, its layers, the pairs it ignores and its +// island; what the change makes false is computed again at the next step. + +// ErrNoShape: SetShape was given no shape. The body is left as it is +var ErrNoShape = errors.New("feather: SetShape needs a shape") + +// ErrShapeNotConvex: SetShape was asked to give a plane, a heightfield or a triangle mesh to a dynamic or a kinematic +// body. These surfaces have no support point for GJK and no volume for a mass: they are the shapes of the static +// bodies (the complex collision of Unreal cannot be simulated, the concave shape of Godot is for the static bodies, a +// non-convex mesh collider of Unity is not simulated). The body is left as it is +var ErrShapeNotConvex = errors.New("feather: a plane, a heightfield and a triangle mesh are the shapes of the static bodies: a dynamic or a kinematic body needs a convex shape") + +// SetShape gives a body of the world another shape, in place, as BodyInterface::SetShape of Jolt (BodyInterface.cpp: +// 298-326, v5.3.0, with the mass properties updated and the body activated): +// - the body keeps its index, its proxy (the broad phase moves it to the tree of its new kind at the next step: a +// plane made a box leaves the planes for the static tree), its joints, its layers, the pairs it ignores, its island, +// its velocities, its target and its locks; +// - a dynamic or a kinematic body takes the mass and the inertia of the shape at its density (Body::SetShapeInternal, +// MotionProperties::SetMassProperties); a static body keeps its infinite mass; +// - the contacts of its pairs are computed again at the next step instead of being taken from the pair cache +// (BodyManager::InvalidateContactCacheForBody: the cache of the body is ignored for one step; the pair cache of +// Feather would move the corners of a cube made a sphere with the bodies); +// - the body wakes up with its island, and the sleeping bodies around its old and its new shape wake up: a static +// pedestal shrunk under a pile lets it fall, a static slab grown under a crate lifts it, a trigger made its +// inscribed sphere tests again the overlap of the crate asleep in its corner (the rule of Teleport and +// UpdateHeightfield; Jolt leaves the neighbours asleep). +// +// The shapes of Feather are centered on their origin: the body stays where it is (Jolt moves it by the difference of +// the centers of mass). The caller places the body where the new shape belongs, by its Transform or Teleport. A body +// which overlaps something with its new shape is pushed out by the spring of the contact over the next steps, at +// ContactSpeed at most. PhysX (PxShape::setGeometry, PxShape.h:122-133) keeps the type of the geometry and "does not +// guarantee correct/continuous behavior when objects are resting on top of old or new geometry": Feather changes the +// kind and keeps the resting bodies stable (TestSetShapeOfARestingBodyKeepsItStable). +// +// No shape is refused (ErrNoShape), a plane, a heightfield or a mesh on a dynamic or a kinematic body too +// (ErrShapeNotConvex): a refused body is left as it is. The same shape again does nothing. The call allocates nothing +// once the world has stepped +func (w *World) SetShape(body *actor.RigidBody, shape actor.ShapeInterface) error { + if shape == nil { + return ErrNoShape + } + if body.BodyType != actor.BodyTypeStatic && !isConvex(shape) { + return ErrShapeNotConvex + } + if body.Shape == shape { + return nil + } + before := body.AABB() + body.SetShape(shape) + w.bodyChanged(body, union(before, body.AABB())) + return nil +} + +// SetDensity gives a body of the world another density, in place: a dynamic or a kinematic body takes the mass and the +// inertia of its shape at this density (PxRigidBodyExt::updateMassAndInertia of PhysX, ExtRigidBodyExt.cpp:282-290, +// v5.6.0), and wakes up with its island; a static body keeps the density written, for the day it becomes dynamic, and +// its infinite mass. A density which is not positive on a dynamic body is refused (ErrMasslessBody), the body left as +// it is. The call allocates nothing +func (w *World) SetDensity(body *actor.RigidBody, density float64) error { + if body.BodyType == actor.BodyTypeDynamic && !(density > 0) { + return ErrMasslessBody + } + body.SetDensity(density) + if body.BodyType != actor.BodyTypeStatic { + w.islands.wake(body) + } + return nil +} + +// bodyChanged: the shape of the body changed, reach is the union of its old and its new AABB. Its pairs are computed +// again at the next step (isChanged, as a changed heightfield), the body wakes up with its island and the sleeping +// bodies within reach wake up (a trigger pair at rest keeps its overlap, overlapKept: its body awake, it is tested +// again) +func (w *World) bodyChanged(body *actor.RigidBody, reach actor.AABB) { + w.changed = append(w.changed, body) + w.islands.wake(body) + w.wakeNeighborsIn(body, reach) +} diff --git a/shape_test.go b/shape_test.go new file mode 100644 index 0000000..9d9da24 --- /dev/null +++ b/shape_test.go @@ -0,0 +1,461 @@ +package feather + +import ( + "errors" + "math" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// The shape and the density of a body changed in place (World.SetShape, World.SetDensity): the body keeps its index, +// its proxy, its joints, its filters and its island; its mass and its inertia follow; it wakes up with the bodies +// around it, and the pairs it had are tested again. + +// restTolerance: how far (m) a resting body may sit from its resting height, the contact being soft; landingTolerance: +// how far it may sink under it when it lands from a fall (the spring of the contact absorbs the impact) +const ( + restTolerance = 2 * LinearSlop + landingTolerance = 4 * LinearSlop +) + +// settleAt runs the scene for 2 s and fails if the body ever goes under the height it should rest at (or under its +// start, when it starts overlapping the ground) by more than a landing, then checks it rests at floor +func settleAt(t *testing.T, w *World, body *actor.RigidBody, floor float64) { + t.Helper() + lowest := min(body.Transform.Position.Y(), floor) + simulate(w, 2, func() { + if y := body.Transform.Position.Y(); y < lowest-landingTolerance { + t.Fatalf("the body went down to %.4f, under %.4f: it goes through the ground", y, lowest) + } + }) + if y := body.Transform.Position.Y(); math.Abs(y-floor) > restTolerance { + t.Fatalf("the body rests at %.4f, want %.4f", y, floor) + } +} + +// requireMassOfShape: the mass and the inertia of the body are those of its shape at its density, to the bit +func requireMassOfShape(t *testing.T, body *actor.RigidBody) { + t.Helper() + wantMass := body.Shape.ComputeMass(body.Material.Density) + if body.Material.GetMass() != wantMass { + t.Fatalf("mass %v, want the mass of the shape %v", body.Material.GetMass(), wantMass) + } + if body.InertiaLocal != body.Shape.ComputeInertia(wantMass) { + t.Fatalf("inertia %v, want the inertia of the shape %v", body.InertiaLocal, body.Shape.ComputeInertia(wantMass)) + } + if body.InverseInertiaLocal != body.InertiaLocal.Inv() { + t.Fatalf("inverse inertia %v, want the inverse of %v", body.InverseInertiaLocal, body.InertiaLocal) + } +} + +// 1. A resting cube becomes a box twice as big, then a sphere, then a taller capsule: it stays stable, never goes +// through the ground, rests at the height of its new shape; its mass and its inertia are those of the new shape +func TestSetShapeOfARestingBodyKeepsItStable(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + body := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 1, nil) + if !body.IsSleeping { + t.Fatal("the cube should rest asleep") + } + index := func() int { + for i, b := range w.Bodies { + if b == body { + return i + } + } + return -1 + } + wasAt := index() + for _, next := range []struct { + name string + shape actor.ShapeInterface + floor float64 + }{ + {"a box twice as big", &actor.Box{HalfExtents: mgl64.Vec3{2 * cubeHalf, 2 * cubeHalf, 2 * cubeHalf}}, 2 * cubeHalf}, + {"a sphere", &actor.Sphere{Radius: cubeHalf}, cubeHalf}, + {"a taller capsule", &actor.Capsule{Radius: cubeHalf, HalfHeight: 2 * cubeHalf}, 3 * cubeHalf}, + } { + if err := w.SetShape(body, next.shape); err != nil { + t.Fatalf("%s: %v", next.name, err) + } + if body.Shape != next.shape { + t.Fatalf("%s: the body has the shape %T", next.name, body.Shape) + } + if body.IsSleeping { + t.Fatalf("%s: the body is still asleep", next.name) + } + if body.AABB() != next.shape.ComputeAABB(body.Transform) { + t.Fatalf("%s: the AABB %v is not the one of the shape", next.name, body.AABB()) + } + requireMassOfShape(t, body) + if index() != wasAt { + t.Fatalf("%s: the body moved from the index %d to %d", next.name, wasAt, index()) + } + settleAt(t, w, body, next.floor) + if !body.IsSleeping { + t.Fatalf("%s: the body should fall asleep again", next.name) + } + } +} + +// 1b. The contacts of the body are computed again at the step after the change, not taken from the pair cache: a +// cube resting awake made a sphere of the same height has one contact point with the ground at the next step, not the +// four corners of the cube moved with the bodies +func TestSetShapeComputesTheContactsAgain(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + body := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, actor.DefaultTimeToSleep/2, nil) + if body.IsSleeping { + t.Fatal("the cube is asleep already: the pair cache is empty, the test proves nothing") + } + points := func() int { + n := 0 + for _, contact := range w.Contacts() { + if contact.BodyA == body || contact.BodyB == body { + n += contact.Count + } + } + return n + } + if points() != 4 { + t.Fatalf("the resting cube has %d contact points, want its 4 corners", points()) + } + if err := w.SetShape(body, &actor.Sphere{Radius: cubeHalf}); err != nil { + t.Fatal(err) + } + w.Step(sceneDt) + if points() != 1 { + t.Fatalf("the sphere has %d contact points at the step after the change, want 1", points()) + } +} + +// 2. A ball hanging from a static anchor by a distance joint becomes a box: the joint holds, the rope keeps its length +func TestSetShapeKeepsTheJoints(t *testing.T) { + w := newScene(1) + anchor := addBody(w, mgl64.Vec3{0, 3, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.02}, actor.BodyTypeStatic, 0, 0) + const rope = 1.0 + ball := addBody(w, anchor.Transform.Position.Sub(mgl64.Vec3{0, rope, 0}), mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0.5, 0) + w.AddJoint(NewDistanceJoint(anchor, ball, anchor.Transform.Position, ball.Transform.Position)) + ball.Velocity = mgl64.Vec3{1.5, 0, 0} // it swings + simulate(w, 1, nil) + if err := w.SetShape(ball, &actor.Box{HalfExtents: mgl64.Vec3{0.15, 0.15, 0.15}}); err != nil { + t.Fatal(err) + } + if len(w.Joints) != 1 { + t.Fatalf("%d joints, want the rope", len(w.Joints)) + } + const tolerance = 0.02 + simulate(w, 2, func() { + if length := ball.Transform.Position.Sub(anchor.Transform.Position).Len(); math.Abs(length-rope) > tolerance { + t.Fatalf("the rope is %.3f m long, want %.1f", length, rope) + } + }) +} + +// 3. A sleeping plate carrying a crate becomes thinner: the plate and the crate wake up (the crate is in the air), the +// crate lands on the thinner plate +func TestSetShapeWakesTheBodyAndItsNeighbors(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + const thick, thin = 0.1, 0.02 + plate := addBody(w, mgl64.Vec3{0, thick, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, thick, 1}}, actor.BodyTypeDynamic, 0.6, 0) + crate := addBody(w, mgl64.Vec3{0.3, 2*thick + cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 1.5, nil) + if !plate.IsSleeping || !crate.IsSleeping { + t.Fatalf("the plate and the crate should sleep: %v %v", plate.IsSleeping, crate.IsSleeping) + } + wakes := 0 + w.Events.Subscribe(EventWake, func(Event) { wakes++ }) + if err := w.SetShape(plate, &actor.Box{HalfExtents: mgl64.Vec3{1, thin, 1}}); err != nil { + t.Fatal(err) + } + if plate.IsSleeping || crate.IsSleeping { + t.Fatalf("after the change: plate asleep %v, crate asleep %v, want both awake", plate.IsSleeping, crate.IsSleeping) + } + // the plate falls on the ground by thick - thin, the crate lands on it + settleAt(t, w, crate, 2*thin+cubeHalf) + if math.Abs(plate.Transform.Position.Y()-thin) > restTolerance { + t.Errorf("the plate rests at %.4f, want %.4f", plate.Transform.Position.Y(), thin) + } +} + +// 4. The shape of a static body changes under a sleeping crate: a wide box made a small one lets the crate fall to the +// ground; the ground plane made a box lifts the crate (the proxy leaves the planes for the static tree), made a plane +// again it lets it down (the proxy goes back) +func TestSetShapeOfAStaticBodyUpdatesItsPairs(t *testing.T) { + w := newScene(1) + ground := addGround(w, 0.6) + const top = 0.5 + pedestal := addBody(w, mgl64.Vec3{0, top / 2, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, top / 2, 1}}, actor.BodyTypeStatic, 0.6, 0) + crate := addBody(w, mgl64.Vec3{0.5, top + cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 1, nil) + if !crate.IsSleeping { + t.Fatal("the crate should sleep on the pedestal") + } + // the pedestal shrinks to a post under its own center: the crate at x = 0.5 is in the air + if err := w.SetShape(pedestal, &actor.Box{HalfExtents: mgl64.Vec3{0.1, top / 2, 0.1}}); err != nil { + t.Fatal(err) + } + if crate.IsSleeping { + t.Fatal("the crate is still asleep over the post") + } + settleAt(t, w, crate, cubeHalf) + // the ground becomes a slab of 2 x top: its top face is at top, the crate is lifted + if err := w.SetShape(ground, &actor.Box{HalfExtents: mgl64.Vec3{5, top, 5}}); err != nil { + t.Fatal(err) + } + if crate.IsSleeping { + t.Fatal("the crate is still asleep in the slab") + } + settleAt(t, w, crate, top+cubeHalf) + if err := consistentOrder(w); err != nil { + t.Fatal(err) + } + // and a plane again: the crate falls back + if err := w.SetShape(ground, &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}); err != nil { + t.Fatal(err) + } + settleAt(t, w, crate, cubeHalf) + if err := consistentOrder(w); err != nil { + t.Fatal(err) + } +} + +// 5. A trigger box holding a sleeping crate in its corner becomes its inscribed sphere, with the same AABB to the bit: +// the overlap is tested again (a pair at rest keeps it otherwise), the crate leaves the zone once +func TestSetShapeOfATriggerRetestsItsOverlap(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + const half, small = 0.5, 0.05 + crate := addBody(w, mgl64.Vec3{half - small, small, half - small}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{small, small, small}}, actor.BodyTypeDynamic, 0.6, 0) + zone := addBody(w, mgl64.Vec3{0, small, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{half, half, half}}, actor.BodyTypeStatic, 0, 0) + zone.IsTrigger = true + log := listenTriggers(w) + simulate(w, 1, nil) + if !crate.IsSleeping { + t.Fatal("the crate should sleep in the zone") + } + if enters, exits := log.count(EventTriggerEnter, zone, crate), log.count(EventTriggerExit, zone, crate); enters != 1 || exits != 0 { + t.Fatalf("%d enters & %d exits before the change, want 1 & 0", enters, exits) + } + sphere := &actor.Sphere{Radius: half} + if sphere.ComputeAABB(zone.Transform) != zone.AABB() { + t.Fatalf("the sphere has the AABB %v, the box %v: the test needs the same", sphere.ComputeAABB(zone.Transform), zone.AABB()) + } + if err := w.SetShape(zone, sphere); err != nil { + t.Fatal(err) + } + simulate(w, 0.5, nil) + if exits := log.count(EventTriggerExit, zone, crate); exits != 1 { + t.Fatalf("%d exits after the zone became a sphere, want 1: the corner of the box is out of the sphere", exits) + } + // and the box again: the crate is in it again + if err := w.SetShape(zone, &actor.Box{HalfExtents: mgl64.Vec3{half, half, half}}); err != nil { + t.Fatal(err) + } + simulate(w, 0.5, nil) + if enters := log.count(EventTriggerEnter, zone, crate); enters != 2 { + t.Fatalf("%d enters after the zone became a box again, want 2", enters) + } +} + +// 6. A shape which cannot be simulated is refused with an error, the body left as it is: no shape, a plane, a +// heightfield or a mesh on a dynamic or a kinematic body; the same shape again does nothing; a refusal allocates nothing +func TestSetShapeRefusesWhatCannotBeSimulated(t *testing.T) { + w := newScene(1) + dynamic := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + kinematic := kinematicBox(w, mgl64.Vec3{2, 1, 0}, cube().HalfExtents) + static := addBody(w, mgl64.Vec3{4, 1, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeStatic, 0.6, 0) + mesh, err := actor.NewTriangleMesh([]mgl64.Vec3{{-1, 0, -1}, {1, 0, -1}, {1, 0, 1}, {-1, 0, 1}}, []int32{0, 2, 1, 0, 3, 2}) + if err != nil { + t.Fatal(err) + } + expectError := func(what string, want error, fn func() error) { + t.Helper() + if err := fn(); !errors.Is(err, want) { + t.Errorf("%s: error %v, want %v", what, err, want) + } + } + plane := &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}} + expectError("no shape on a dynamic body", ErrNoShape, func() error { return w.SetShape(dynamic, nil) }) + expectError("no shape on a static body", ErrNoShape, func() error { return w.SetShape(static, nil) }) + expectError("a plane on a dynamic body", ErrShapeNotConvex, func() error { return w.SetShape(dynamic, plane) }) + expectError("a mesh on a dynamic body", ErrShapeNotConvex, func() error { return w.SetShape(dynamic, mesh) }) + expectError("a mesh on a kinematic body", ErrShapeNotConvex, func() error { return w.SetShape(kinematic, mesh) }) + for _, body := range []*actor.RigidBody{dynamic, kinematic, static} { + if _, isBox := body.Shape.(*actor.Box); !isBox { + t.Errorf("a refused body changed its shape to %T", body.Shape) + } + } + // a static body takes a mesh and a plane + if err := w.SetShape(static, mesh); err != nil || static.Shape != mesh { + t.Errorf("a mesh on a static body: %v, shape %T", err, static.Shape) + } + if err := w.SetShape(static, plane); err != nil || static.Shape != plane { + t.Errorf("a plane on a static body: %v, shape %T", err, static.Shape) + } + // the same shape again: nothing happens, the sleeping body stays asleep + simulate(w, 1, nil) + shape := dynamic.Shape + if err := w.SetShape(dynamic, shape); err != nil { + t.Errorf("the same shape again: %v", err) + } + if allocs := testing.AllocsPerRun(10, func() { _ = w.SetShape(dynamic, mesh) }); allocs > 0 { + t.Errorf("a refusal allocates %.1f, want 0", allocs) + } +} + +// 7. The density of a body changed in place: the mass and the inertia of its shape at the new density, the same +// impulse gives half the velocity at twice the density; a sleeping body wakes up; a static body keeps the density +// written, without mass; a density which is not positive is refused on a dynamic body +func TestSetDensityRecomputesTheMassAndTheInertia(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + body := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + static := addBody(w, mgl64.Vec3{3, 1, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeStatic, 0.6, 0) + kinematic := kinematicBox(w, mgl64.Vec3{6, 1, 0}, cube().HalfExtents) + before := body.Material.GetMass() + if err := w.SetDensity(body, 2*body.Material.Density); err != nil { + t.Fatal(err) + } + requireMassOfShape(t, body) + if body.Material.GetMass() != 2*before { + t.Fatalf("mass %v at twice the density, want %v", body.Material.GetMass(), 2*before) + } + body.AddImpulse(mgl64.Vec3{1, 0, 0}) + w.Step(sceneDt) + if want := 1 / (2 * before); math.Abs(body.Velocity.X()-want) > 1e-12 { + t.Errorf("velocity %v after an impulse of 1 N·s, want 1 / mass = %v", body.Velocity.X(), want) + } + body.Velocity = mgl64.Vec3{} + simulate(w, actor.DefaultTimeToSleep+3*sceneDt, nil) + if !body.IsSleeping { + t.Fatal("the body at rest should sleep") + } + if err := w.SetDensity(body, 500); err != nil || body.IsSleeping { + t.Fatalf("a sleeping body given a density: %v, asleep %v", err, body.IsSleeping) + } + // a static body keeps the density, for the day it becomes dynamic, and its infinite mass + if err := w.SetDensity(static, 700); err != nil { + t.Fatal(err) + } + if static.Material.Density != 700 || !math.IsInf(static.Material.GetMass(), 1) || static.InverseMass() != 0 { + t.Errorf("a static body: density %v, mass %v, inverse mass %v, want 700, +Inf, 0", static.Material.Density, static.Material.GetMass(), static.InverseMass()) + } + // a kinematic body gets the mass of its shape, for the day it becomes dynamic; the solver never reads it + if err := w.SetDensity(kinematic, 700); err != nil { + t.Fatal(err) + } + requireMassOfShape(t, kinematic) + if kinematic.InverseMass() != 0 { + t.Errorf("a kinematic body has an inverse mass %v, want 0", kinematic.InverseMass()) + } + for _, density := range []float64{0, -1, math.NaN()} { + if err := w.SetDensity(body, density); !errors.Is(err, ErrMasslessBody) { + t.Errorf("density %v on a dynamic body: %v, want ErrMasslessBody", density, err) + } + } + if body.Material.Density != 500 { + t.Errorf("a refused density changed the body: %v", body.Material.Density) + } + if allocs := testing.AllocsPerRun(10, func() { _ = w.SetDensity(body, 600) }); allocs > 0 { + t.Errorf("SetDensity allocates %.1f, want 0", allocs) + } +} + +// 8. The mixed scene with shapes and densities changed during the run gives the same bits with 1, 4 and 8 workers: a +// change every 8 steps, a sphere, a box, a capsule, a density in turn, on 30 bodies over 4 s (the static anchors of +// the chains, the chains, the linked boxes and the bodies falling on them). The changes keep the scene awake: a run of +// 1000 steps with a change every 25 steps had no body asleep either +func TestSetShapeIsDeterministic(t *testing.T) { + const setShapeSteps, setShapeEvery = 240, 8 + steps := setShapeSteps + if raceEnabled { + steps = min(steps, raceSteps) + } + changed := 0 + compareRuns(t, steps, func(workers int) (*World, func(step int)) { + w := mixedScene(workers) + if workers == determinismWorkers[0] { + changed = 0 + } + return w, func(step int) { + if step%setShapeEvery != setShapeEvery/2 { + return + } + // a body of the scene, by turns; the chains are the first bodies, the pile the last ones + body := w.Bodies[(step/setShapeEvery*7)%len(w.Bodies)] + var err error + switch (step / setShapeEvery) % 4 { + case 0: + err = w.SetShape(body, &actor.Sphere{Radius: 0.08}) + case 1: + err = w.SetShape(body, &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.06, 0.08}}) + case 2: + err = w.SetShape(body, &actor.Capsule{Radius: 0.05, HalfHeight: 0.1}) + default: + if body.BodyType == actor.BodyTypeDynamic { + err = w.SetDensity(body, body.Material.Density*1.5) + } + } + if err != nil { + t.Fatalf("step %d: %v", step, err) + } + if workers == determinismWorkers[0] { + changed++ + } + } + }) + if want := steps/setShapeEvery - 1; changed < want { + t.Fatalf("%d changes over the run, want %d at least: the scene doesn't cover them", changed, want) + } +} + +// 9. A step after a change of shape allocates nothing, with 1 and 8 workers: three crates standing on three cubes, the +// cubes made lower and back (a box, its top 10 % lower) at every step and given another density at every third, in a +// steady state — the buffers of the islands, of the pairs and of the contacts have grown for the bodies the changes keep +// awake (changedWarmup steps). Changes walking a pile would wake a new part of it at each step, and the buffers, sized to +// the contacts of a step, would grow with it: an allocation of the step, not of the change +func TestStepAfterSetShapeAllocatesNothing(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + const changedWarmup, stacks = 240, 3 + for _, workers := range []int{1, 8} { + w := newScene(workers) + addGround(w, 0.6) + cube := &actor.Box{HalfExtents: mgl64.Vec3{cubeHalf, cubeHalf, cubeHalf}} + var changed []*actor.RigidBody + for i := range stacks { + x := float64(i-1) * 2 + changed = append(changed, addBody(w, mgl64.Vec3{x, cubeHalf, 0}, mgl64.QuatIdent(), cube, actor.BodyTypeDynamic, 0.6, 0)) + addBody(w, mgl64.Vec3{x, 3 * cubeHalf, 0}, mgl64.QuatIdent(), cube, actor.BodyTypeDynamic, 0.6, 0) + } + simulate(w, 1, nil) + shapes := []actor.ShapeInterface{&actor.Box{HalfExtents: mgl64.Vec3{cubeHalf, 0.9 * cubeHalf, cubeHalf}}, cube} + turn := 0 + step := func() { + body := changed[turn%stacks] + if err := w.SetShape(body, shapes[(turn/stacks)%2]); err != nil { + t.Fatal(err) + } + if turn%3 == 0 { + if err := w.SetDensity(body, 400+float64(turn%7)); err != nil { + t.Fatal(err) + } + } + turn++ + w.Step(sceneDt) + } + for range changedWarmup { + step() + } + if allocs := testing.AllocsPerRun(10, step); allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", workers, allocs) + } + } +} diff --git a/solver.go b/solver.go index 5c652d9..f080ab8 100644 --- a/solver.go +++ b/solver.go @@ -108,13 +108,17 @@ func skewTerm(inertia *mgl64.Mat3, rX, rY mgl64.Vec3) mgl64.Mat3 { return actor.Mul3(&t, &sy) } -// bodyState is the copy of a dynamic body used by the solver during a step +// bodyState is the copy of a body of the solver during a step: an awake dynamic body, or an awake kinematic body (it +// moves, the contacts read its velocity and its motion, but it has no mass: no row writes it) type bodyState struct { // the impulses read and write these 64 bytes: a cache line body *actor.RigidBody velocity mgl64.Vec3 angularVelocity mgl64.Vec3 invMass float64 // without its locks: see linearMass + // dynamic: the rows write the body (an impulse changes its velocities). False for the shared static state and for a + // kinematic body: the rows of a kinematic body read it only, so they can share a color (graph.go) + dynamic bool // the locked world axes of the body: it has no inverse mass along linearLock (invMassAxes), no inverse inertia around // angularLock (inverseInertia) @@ -231,6 +235,8 @@ type solver struct { indices map[*actor.RigidBody]int h float64 invH float64 + // substep: the current sub-step, from 1 to substeps (the kinematic bodies interpolate their motion on it) + substep, substeps int // locked: an awake body has a locked axis (the mass matrix of the joints can be singular: lockedInverse) locked bool @@ -293,10 +299,12 @@ func (s *solver) prepare(bodies []*actor.RigidBody, manifolds []constraint.Manif s.initJobs() s.h = dt / float64(substeps) s.invH = 1 / s.h + s.substeps = substeps s.static = bodyState{deltaRotation: mgl64.QuatIdent(), deltaMatrix: mgl64.Ident3()} // ========== 1. Body states ========== - // the awake dynamic bodies get a state, numbered in the order of the World; the states are filled in parallel + // the awake dynamic and kinematic bodies get a state, numbered in the order of the World; the states are filled in + // parallel if s.indices == nil { s.indices = make(map[*actor.RigidBody]int) } @@ -313,11 +321,11 @@ func (s *solver) prepare(bodies []*actor.RigidBody, manifolds []constraint.Manif s.locked = false for i, body := range bodies { s.stateIndex[i] = -1 - if !isAwakeDynamic(body) { + if !isAwakeMover(body) { continue } s.stateIndex[i] = int32(len(s.stateBody)) - s.locked = s.locked || body.LinearLock|body.AngularLock != actor.NoAxes + s.locked = s.locked || (body.BodyType == actor.BodyTypeDynamic && body.LinearLock|body.AngularLock != actor.NoAxes) if len(s.indices) > 0 { if _, ok := s.indices[body]; ok { s.indices[body] = len(s.stateBody) @@ -354,13 +362,16 @@ func (s *solver) prepare(bodies []*actor.RigidBody, manifolds []constraint.Manif s.buildArticulations() // ========== 4. Graph coloring: the contacts, then the joints ========== + // a kinematic body counts as a static one: its rows read it and never write it, they can share a color. Box2D v3 + // colors it as a dynamic body, because its SIMD solver scatters every body of a constraint, written or not + // (constraint_graph.c:20-23, v3.1.0); the rows of Feather only write the bodies which answer (bodyState.dynamic) s.items = s.items[:0] for i := range s.constraints { - s.items = append(s.items, graphItem{s.constraints[i].indexA, s.constraints[i].indexB}) + s.items = append(s.items, graphItem{s.dynamicIndex(s.constraints[i].indexA), s.dynamicIndex(s.constraints[i].indexB)}) } for _, joint := range s.joints { base := joint.base() - s.items = append(s.items, graphItem{base.indexA, base.indexB}) + s.items = append(s.items, graphItem{s.dynamicIndex(base.indexA), s.dynamicIndex(base.indexB)}) } s.graph.color(s.items, len(s.states)) } @@ -382,12 +393,15 @@ func (s *solver) prepareConstraint(i int) { normal: manifold.Normal, pointsCount: manifold.Count, } - if c.indexA < 0 && c.indexB < 0 { - // nothing to solve + stateA, stateB := s.state(c.indexA), s.state(c.indexB) + if !stateA.dynamic && !stateB.dynamic { + // nothing to solve (no pair gives 2 bodies without mass, but a step must not rely on it) c.pointsCount = 0 return } + // the contacts with a static body are stiffer, not the ones with a kinematic body: as Box2D v3.1 (contact_solver.c:1514, + // the softness of a static body is for a body without solver state) and Box3D (b3_contactStaticFlag, contact.c:241) c.spring = s.contactSpring if c.indexA < 0 || c.indexB < 0 { c.spring = s.staticSpring @@ -397,7 +411,6 @@ func (s *solver) prepareConstraint(i int) { staticFriction := constraint.ComputeStaticFriction(manifold.BodyA.Material, manifold.BodyB.Material) dynamicFriction := constraint.ComputeDynamicFriction(manifold.BodyA.Material, manifold.BodyB.Material) - stateA, stateB := s.state(c.indexA), s.state(c.indexB) radiusA, radiusB := shapeRadius(manifold.BodyA.Shape), shapeRadius(manifold.BodyB.Shape) c.radiusA, c.radiusB = radiusA, radiusB c.rollingResistance = constraint.ComputeRollingResistance(manifold.BodyA.Material, manifold.BodyB.Material, radiusA, radiusB) @@ -529,10 +542,10 @@ func shapeRadius(shape actor.ShapeInterface) float64 { // applyRolling applies the rolling impulses λ around both tangents: -λ on A, +λ on B func (c *contactConstraint) applyRolling(stateA, stateB *bodyState, lambda [2]float64) { - if stateA.body != nil { + if stateA.dynamic { stateA.angularVelocity = stateA.angularVelocity.Sub(c.rollingA[0].Mul(lambda[0])).Sub(c.rollingA[1].Mul(lambda[1])) } - if stateB.body != nil { + if stateB.dynamic { stateB.angularVelocity = stateB.angularVelocity.Add(c.rollingB[0].Mul(lambda[0])).Add(c.rollingB[1].Mul(lambda[1])) } } @@ -545,16 +558,29 @@ func (s *solver) indexOf(body *actor.RigidBody) int { return -1 } -// stateOf fills the state of the body i +// stateOf fills the state of the body i. A kinematic body has its velocities and its motion (moveKinematic), no mass, +// no inertia and no lock func (s *solver) stateOf(i int) { body := s.bodies[s.stateBody[i]] + state := &s.states[i] + if body.BodyType == actor.BodyTypeKinematic { + s.starts[i] = bodyStart{rotation: body.Transform.Rotation} + *state = bodyState{ + body: body, + velocity: body.Velocity, + angularVelocity: body.AngularVelocity, + deltaRotation: mgl64.QuatIdent(), + deltaMatrix: mgl64.Ident3(), + } + return + } inverseInertia := body.GetFreeInverseInertiaWorld() s.starts[i] = bodyStart{inertia: inverseInertia, rotation: body.Transform.Rotation} - state := &s.states[i] *state = bodyState{ body: body, velocity: body.Velocity, angularVelocity: body.AngularVelocity, + dynamic: true, deltaRotation: mgl64.QuatIdent(), deltaMatrix: mgl64.Ident3(), invMass: body.InverseMass(), @@ -587,16 +613,17 @@ func (j *jacobian) velocity(stateA, stateB *bodyState, direction mgl64.Vec3) flo } // apply the impulse λ along the direction: -λ on A, +λ on B. -// The static state is shared by all the static bodies: it is never written (it has no mass anyway). +// Only a dynamic body is written: the static state is shared by all the static bodies, and a kinematic body has no +// mass (writing it would be a data race between the colors, Box2D colors it as a dynamic body for this reason). // The inverse mass is the one of each axis: a locked axis keeps its velocity. // The components are written out: the vector methods go through the stack func (j *jacobian) apply(stateA, stateB *bodyState, direction mgl64.Vec3, lambda float64) { - if stateA.body != nil { + if stateA.dynamic { v, w, m := &stateA.velocity, &stateA.angularVelocity, &stateA.invMassAxes v[0], v[1], v[2] = v[0]-direction[0]*(lambda*m[0]), v[1]-direction[1]*(lambda*m[1]), v[2]-direction[2]*(lambda*m[2]) w[0], w[1], w[2] = w[0]-j.impulseA[0]*lambda, w[1]-j.impulseA[1]*lambda, w[2]-j.impulseA[2]*lambda } - if stateB.body != nil { + if stateB.dynamic { v, w, m := &stateB.velocity, &stateB.angularVelocity, &stateB.invMassAxes v[0], v[1], v[2] = v[0]+direction[0]*(lambda*m[0]), v[1]+direction[1]*(lambda*m[1]), v[2]+direction[2]*(lambda*m[2]) w[0], w[1], w[2] = w[0]+j.impulseB[0]*lambda, w[1]+j.impulseB[1]*lambda, w[2]+j.impulseB[2]*lambda @@ -614,7 +641,7 @@ func relativeVelocity(stateA, stateB *bodyState, rA, rB mgl64.Vec3) mgl64.Vec3 { } // currentSeparation: the contact points are not computed again during the sub-steps, -// the separation is updated from the motion of both bodies +// the separation is updated from the motion of both bodies (a kinematic body moves: its delta is its interpolation) func currentSeparation(stateA, stateB *bodyState, cp *contactPoint, normal mgl64.Vec3) float64 { // a static body doesn't move: its core stays (its delta is the identity) coreA := cp.coreA @@ -704,6 +731,11 @@ func (s *solver) integrateVelocities(gravity mgl64.Vec3) { func (s *solver) integrateVelocity(i int) { h, gravity := s.h, s.gravity state := &s.states[i] + if !state.dynamic { + // a kinematic body has the velocity of its motion: no gravity, no force, no damping ("Gravity scale will be zero + // for kinematic bodies", Box2D solver.c:94), and no clamping (Jolt, PhysicsSystem.cpp:1531) + return + } body := state.body linearDamping := 1 / (1 + h*body.Material.LinearDamping) @@ -763,6 +795,10 @@ func (s *solver) integratePositions(dt float64) { func (s *solver) integratePosition(i int) { h, maxAngularSpeed := s.h, s.maxAngularSpeed state := &s.states[i] + if !state.dynamic { + s.moveKinematic(i) + return + } if state.velocity.LenSqr() > MaxLinearSpeed*MaxLinearSpeed { state.velocity = state.velocity.Mul(MaxLinearSpeed / state.velocity.Len()) } @@ -1157,13 +1193,13 @@ func (c *contactConstraint) solveFriction(stateA, stateB *bodyState) { // applyTwist: the angular impulse around the normal, -λ on A, +λ on B func (c *contactConstraint) applyTwist(stateA, stateB *bodyState, lambda float64) { t := [3]float64{c.normal[0] * lambda, c.normal[1] * lambda, c.normal[2] * lambda} - if stateA.body != nil { + if stateA.dynamic { w, m := &stateA.angularVelocity, &stateA.inverseInertia w[0] -= m[0]*t[0] + m[3]*t[1] + m[6]*t[2] w[1] -= m[1]*t[0] + m[4]*t[1] + m[7]*t[2] w[2] -= m[2]*t[0] + m[5]*t[1] + m[8]*t[2] } - if stateB.body != nil { + if stateB.dynamic { w, m := &stateB.angularVelocity, &stateB.inverseInertia w[0] += m[0]*t[0] + m[3]*t[1] + m[6]*t[2] w[1] += m[1]*t[0] + m[4]*t[1] + m[7]*t[2] @@ -1244,7 +1280,9 @@ func (s *solver) storeImpulsesConstraint(i int) { c.manifold.TwistImpulse = c.twistImpulse } -// finalize writes the new transform and velocities into the bodies +// finalize writes the new transform and velocities into the bodies. A kinematic body is put on its target, bit for +// bit, and the target is dropped (PhysX, updateKinematicPose): the game gives the next one. It keeps the velocity of +// its motion func (s *solver) finalize() { s.forEachBody(s.jobs.finalize) } @@ -1252,6 +1290,14 @@ func (s *solver) finalize() { func (s *solver) finalizeBody(i int) { state := &s.states[i] body := state.body + if !state.dynamic { + if target, ok := body.KinematicTarget(); ok { + body.Transform = target + body.ClearKinematicTarget() + body.UpdateAABB() + } + return + } body.Transform.Position = body.Transform.Position.Add(state.deltaPosition) body.Transform.Rotation = state.deltaRotation.Mul(s.starts[i].rotation).Normalize() body.Velocity = state.velocity diff --git a/spinning_test.go b/spinning_test.go index b14505f..ec75dec 100644 --- a/spinning_test.go +++ b/spinning_test.go @@ -495,8 +495,8 @@ func TestSolveSpinning(t *testing.T) { normal := mgl64.Vec3{0, 1, 0} bodyA, bodyB := &actor.RigidBody{}, &actor.RigidBody{} newStates := func(spinA, spinB float64) (*bodyState, *bodyState) { - return &bodyState{body: bodyA, angularVelocity: mgl64.Vec3{0.3, spinA, 0}, inverseInertia: mgl64.Diag3(mgl64.Vec3{2, 2, 2})}, - &bodyState{body: bodyB, angularVelocity: mgl64.Vec3{0, spinB, -0.7}, inverseInertia: mgl64.Diag3(mgl64.Vec3{4, 4, 4})} + return &bodyState{body: bodyA, dynamic: true, angularVelocity: mgl64.Vec3{0.3, spinA, 0}, inverseInertia: mgl64.Diag3(mgl64.Vec3{2, 2, 2})}, + &bodyState{body: bodyB, dynamic: true, angularVelocity: mgl64.Vec3{0, spinB, -0.7}, inverseInertia: mgl64.Diag3(mgl64.Vec3{4, 4, 4})} } newConstraint := func(resistance float64) *contactConstraint { c := &contactConstraint{normal: normal, pointsCount: 2, spinningResistance: resistance, twistMass: 1.0 / (2 + 4)} @@ -569,8 +569,8 @@ func TestSolveSpinning(t *testing.T) { // end with the same spin around the normal, and their angular velocity along the tangents is kept tilted := mgl64.Vec3{0.36, 0.48, 0.8} side := mgl64.Vec3{0.8, 0, -0.36} - stateA = &bodyState{body: bodyA, angularVelocity: tilted.Mul(1).Add(side), inverseInertia: mgl64.Diag3(mgl64.Vec3{2, 2, 2})} - stateB = &bodyState{body: bodyB, angularVelocity: tilted.Mul(4), inverseInertia: mgl64.Diag3(mgl64.Vec3{4, 4, 4})} + stateA = &bodyState{body: bodyA, dynamic: true, angularVelocity: tilted.Mul(1).Add(side), inverseInertia: mgl64.Diag3(mgl64.Vec3{2, 2, 2})} + stateB = &bodyState{body: bodyB, dynamic: true, angularVelocity: tilted.Mul(4), inverseInertia: mgl64.Diag3(mgl64.Vec3{4, 4, 4})} c = newConstraint(1) c.normal = tilted c.solveSpinning(stateA, stateB) diff --git a/tree.go b/tree.go index ef75902..7ac0043 100644 --- a/tree.go +++ b/tree.go @@ -1,6 +1,7 @@ package feather import ( + "cmp" "slices" "github.com/akmonengine/feather/actor" @@ -10,10 +11,12 @@ import ( // ========== BROAD PHASE ========== // The broad phase is a pair of dynamic AABB trees (Catto, "Dynamic Bounding Volume Hierarchies", GDC 2019; the // b2DynamicTree of Box2D, the btDbvt of Bullet), one per kind of body as the trees of Box2D v3: one for the static -// bodies, updated when a body is added, removed or moved by the game, one for the dynamic bodies, awake or asleep. -// A dynamic body is stored with its AABB enlarged by AABBMargin: a body which moves inside its enlarged AABB doesn't -// touch the tree, a sleeping body never does. The planes and the heightfields are not in the trees: they are tested -// against every awake body. +// bodies, updated when a body is added, removed or moved by the game, one for the dynamic and the kinematic bodies, +// awake or asleep (Box2D v3 gives the kinematic bodies a third tree, so that a kinematic proxy never queries the static +// tree for its contacts: here a kinematic proxy queries it for the triggers, and its pairs with the static bodies are +// emitted only with a trigger, Tree.scan). A dynamic or kinematic body is stored with its AABB enlarged by +// AABBMargin: a body which moves inside its enlarged AABB doesn't touch the tree, a sleeping body never does. The +// planes and the heightfields are not in the trees: they are tested against every awake dynamic body. // // The pairs of overlapping stored AABBs are kept from a step to the next (see findPairs): only a body put back in a // tree queries it. The pairs of the step are those whose exact AABBs overlap, sorted by the index of the first body, @@ -322,13 +325,19 @@ type proxyKind uint8 const ( proxyDynamic proxyKind = iota proxyStatic - proxyLarge // planes & heightfields: tested against every awake body + proxyLarge // planes & heightfields: tested against every awake dynamic body + proxyKinematic // in the tree of the dynamic bodies: its pairs with the static bodies and the planes are only for the triggers ) type proxy struct { node int32 kind proxyKind aabb actor.AABB // as stored in the tree (enlarged for a dynamic body) + // box: the AABB of the body at its last sync, and changed: the step it last changed at (the body moved by a step, or + // by the game). A trigger pair at rest keeps its overlap while the boxes of both bodies stay the same + // (World.overlapKept) + box actor.AABB + changed uint32 } // Tree is the broad phase of a World: both AABB trees and the proxy of each body, in the order of World.Bodies @@ -338,6 +347,8 @@ type Tree struct { planes []int32 // the large bodies proxies []proxy bodies []*actor.RigidBody // the bodies the proxies were made for: a mismatch rebuilds everything + // step: the step of the World, the stamp of a change of the box of a proxy + step uint32 // filter: the pairs of bodies which never collide (nil without a World) filter *pairFilter @@ -380,14 +391,19 @@ type pairRecord struct { key fatPair first, count int32 stamp uint32 + // trigger: the pair had a trigger at the step stamp, and overlap: its shapes overlapped (it has no contact) + trigger, overlap bool } func kindOf(body *actor.RigidBody) proxyKind { if isLarge(body) { return proxyLarge } - if body.BodyType == actor.BodyTypeDynamic { + switch body.BodyType { + case actor.BodyTypeDynamic: return proxyDynamic + case actor.BodyTypeKinematic: + return proxyKinematic } return proxyStatic } @@ -457,9 +473,10 @@ func (t *Tree) rebuild(bodies []*actor.RigidBody, boxes []actor.AABB) { // place the body in its tree (or in the planes) func (t *Tree) place(i int32, body *actor.RigidBody, aabb actor.AABB) { p := &t.proxies[i] + p.box, p.changed = aabb, t.step p.kind = kindOf(body) switch p.kind { - case proxyDynamic: + case proxyDynamic, proxyKinematic: p.aabb = enlarged(aabb) p.node = t.dynamics.insert(p.aabb, i) t.moved = append(t.moved, i) @@ -478,7 +495,7 @@ func (t *Tree) place(i int32, body *actor.RigidBody, aabb actor.AABB) { func (t *Tree) unplace(i int32) { p := &t.proxies[i] switch p.kind { - case proxyDynamic: + case proxyDynamic, proxyKinematic: t.dynamics.remove(p.node) case proxyStatic: t.statics.remove(p.node) @@ -492,6 +509,9 @@ func (t *Tree) unplace(i int32) { func (t *Tree) update(i int32, body *actor.RigidBody, aabb actor.AABB) { p := &t.proxies[i] + if p.box != aabb { + p.box, p.changed = aabb, t.step + } kind := kindOf(body) if kind != p.kind { t.unplace(i) @@ -499,7 +519,7 @@ func (t *Tree) update(i int32, body *actor.RigidBody, aabb actor.AABB) { return } switch kind { - case proxyDynamic: + case proxyDynamic, proxyKinematic: if !contains(p.aabb, aabb) { t.dynamics.remove(p.node) p.aabb = enlarged(aabb) @@ -533,10 +553,10 @@ func (t *Tree) removed(k int) { for i := k; i < len(t.proxies); i++ { p := &t.proxies[i] if p.node != nullNode { - if p.kind == proxyDynamic { - t.dynamics.nodes[p.node].body = int32(i) - } else { + if p.kind == proxyStatic { t.statics.nodes[p.node].body = int32(i) + } else { + t.dynamics.nodes[p.node].body = int32(i) } } } @@ -609,14 +629,14 @@ func (t *Tree) queryCandidates(aabb actor.AABB, stack []int32, out []int32) ([]i // proxy put in a tree since the last search (a dynamic body out of its enlarged AABB, a static body moved by the // game, a body added) queries the trees, and a resting body costs nothing. The planes and the heightfields pair with // every body which moved. The pairs are dropped when their stored AABBs no longer overlap. The pairs of the step are -// the ones whose exact AABBs overlap and which pass the collision filters (filter.go), with an awake dynamic body: the -// same pairs as a search from scratch, in the same order. A filtered pair stays in the records (the filters can change -// at any time) but is never emitted. Each pair keeps the contacts of its last step (pairRecord): the World finds them -// without any lookup +// the ones whose exact AABBs overlap and which pass the collision filters (filter.go), with an awake dynamic body, or a +// trigger and a dynamic or kinematic body awake or asleep (detectsTrigger): the same pairs as a search from scratch, in the same +// order. A filtered pair stays in the records (the filters can change at any time) but is never emitted. Each pair +// keeps the contacts of its last step (pairRecord): the World finds them without any lookup // findPairs: the pairs of bodies whose AABBs overlap and which pass the collision filters, with at least an awake -// dynamic body, sorted by the index of the first body (the planes of a body before its other pairs, in the order of the -// planes). The slice is reused +// dynamic body, or a trigger (detectsTrigger), sorted by the index of the first body (the planes of a body before its +// other pairs, in the order of the planes). The slice is reused func (t *Tree) findPairs(bodies []*actor.RigidBody, boxes []actor.AABB, pool *workerPool) []Pair { t.pairs = t.pairs[:0] t.bodyList, t.boxes = bodies, boxes @@ -683,7 +703,10 @@ const ( ) // query: the chunk c of the moved proxies finds its pairs (a pair of static bodies never needs solving, nor a static -// body against a plane) +// body against a plane). A kinematic body has no contact with the static bodies, the planes and the heightfields, as +// the kinematic proxies of Box2D v3 only query its dynamic tree for their contacts, but it enters and leaves their +// triggers, as the sensors of Box2D v3 query its 3 trees (b2SensorTask): it queries them too, its pairs without a +// trigger are kept but never emitted (Tree.scan), as a pair filtered func (t *Tree) query(c int) { chunk := &t.chunks[c] chunk.found = chunk.found[:0] @@ -692,7 +715,7 @@ func (t *Tree) query(c int) { p := &t.proxies[i] chunk.candidates = chunk.candidates[:0] chunk.stack, chunk.candidates = t.dynamics.query(p.aabb, chunk.stack, chunk.candidates) - if p.kind == proxyDynamic { + if p.kind == proxyDynamic || p.kind == proxyKinematic { chunk.stack, chunk.candidates = t.statics.query(p.aabb, chunk.stack, chunk.candidates) chunk.candidates = append(chunk.candidates, t.planes...) } @@ -732,21 +755,25 @@ func (t *Tree) scan(c int) { if !plane { i, j = j, i } - if !isAwakeDynamic(bodies[j]) || !t.filter.collides(bodies[i], bodies[j]) { + tested := isAwakeDynamic(bodies[j]) || detectsTrigger(bodies[i], bodies[j]) + if !tested || !t.filter.collides(bodies[i], bodies[j]) { continue } chunk.pairs = append(chunk.pairs, Pair{BodyA: bodies[i], BodyB: bodies[j], IndexA: i, IndexB: j, first: j, second: int32(slices.Index(t.planes, i)), plane: true, slot: int32(k)}) continue } // the filters last: the pairs of resting bodies, the most of them, are skipped before - if needsSolving(bodies[i], bodies[j]) && t.filter.collides(bodies[i], bodies[j]) { + if (needsSolving(bodies[i], bodies[j]) || detectsTrigger(bodies[i], bodies[j])) && t.filter.collides(bodies[i], bodies[j]) { chunk.pairs = append(chunk.pairs, Pair{BodyA: bodies[i], BodyB: bodies[j], IndexA: i, IndexB: j, first: i, second: j, slot: int32(k)}) } } } // sortPairs by the index of the first body: a counting sort (O(pairs + bodies), the pairs are many and the keys are -// small), then the few pairs of a body by their second key. Returns the sorted slice (the buffers are swapped) +// small), then the pairs of each body by their second key with the sort of the standard library (pdqsort, an insertion +// sort under 12 elements: a body with a few pairs costs as before, a zone or a ground with hundreds of pairs no longer +// costs their square). The keys of a body are unique, the order doesn't depend on the sort. Returns the sorted slice +// (the buffers are swapped) func (t *Tree) sortPairs(pairs []Pair, bodiesCount int) []Pair { if cap(t.counts) < bodiesCount+1 { t.counts = make([]int32, bodiesCount+1) @@ -770,16 +797,14 @@ func (t *Tree) sortPairs(pairs []Pair, bodiesCount int) []Pair { sorted[counts[k]] = pairs[i] counts[k]++ } - // within a body: its planes first (in their order), then its other pairs by index: an insertion sort of a few pairs + // within a body: its planes first (in their order), then its other pairs by index start := 0 for end := 1; end <= len(sorted); end++ { if end < len(sorted) && sorted[end].first == sorted[start].first { continue } - for i := start + 1; i < end; i++ { - for j := i; j > start && pairBefore(sorted[j], sorted[j-1]); j-- { - sorted[j], sorted[j-1] = sorted[j-1], sorted[j] - } + if end-start > 1 { + slices.SortFunc(sorted[start:end], comparePairs) } start = end } @@ -787,17 +812,36 @@ func (t *Tree) sortPairs(pairs []Pair, bodiesCount int) []Pair { return sorted } -// pairBefore: the order of the pairs of a body -func pairBefore(a, b Pair) bool { +// comparePairs: the order of the pairs of a body, its planes first +func comparePairs(a, b Pair) int { if a.plane != b.plane { - return a.plane + if a.plane { + return -1 + } + return 1 } - return a.second < b.second + return cmp.Compare(a.second, b.second) } -// needsSolving - At least one body must be dynamic and awake +// needsSolving: a dynamic body, and an awake body which moves (dynamic or kinematic): a pair of a kinematic body with a +// static or a kinematic body has no contact, a sleeping dynamic body is woken up by the kinematic body which reaches it func needsSolving(a, b *actor.RigidBody) bool { - return isAwakeDynamic(a) || isAwakeDynamic(b) + return (isAwakeMover(a) || isAwakeMover(b)) && (a.BodyType == actor.BodyTypeDynamic || b.BodyType == actor.BodyTypeDynamic) +} + +// detectsTrigger: a pair of a trigger and a body which moves (dynamic or kinematic), the trigger or the other one, is +// tested whatever the sleep, as the sensors of Box2D v3 ("Sensors do not consider sleep", docs/simulation.md; +// b2SensorTask queries every sensor against its 3 trees at each step): the pair ends when the shapes no longer +// overlap, never because a body falls asleep, and a static trigger the game moves away from a sleeping body, or over +// it, ends or starts the pair. While both bodies rest, the pair keeps its overlap without any test (World.overlapKept): +// it costs its place in the pairs of the step. A kinematic body enters a static trigger, and a kinematic trigger +// detects the static and the kinematic bodies, as in Box2D v3 and in Unity ("A dynamic or kinematic trigger collider +// collides with any collider type. A static trigger collider collides with any dynamic or Kinematic collider"); Jolt +// does it for a static sensor ("These sensors will only detect collisions with active Dynamic or Kinematic bodies", +// Body.h), for a kinematic sensor against the static bodies only on demand (SetCollideKinematicVsNonDynamic). 2 static +// bodies never pair +func detectsTrigger(a, b *actor.RigidBody) bool { + return (a.IsTrigger || b.IsTrigger) && (a.BodyType != actor.BodyTypeStatic || b.BodyType != actor.BodyTypeStatic) } func isAwakeDynamic(body *actor.RigidBody) bool { diff --git a/tree_test.go b/tree_test.go index 10d25e9..404e696 100644 --- a/tree_test.go +++ b/tree_test.go @@ -99,13 +99,13 @@ func bruteForcePairs(bodies []*actor.RigidBody, boxes []actor.AABB) []Pair { continue } for j, b := range bodies { - if isLarge(b) && needsSolving(a, b) && boxes[i].Overlaps(boxes[j]) { + if isLarge(b) && emitted(a, b) && boxes[i].Overlaps(boxes[j]) { pairs = append(pairs, Pair{BodyA: b, BodyB: a}) } } for j := i + 1; j < len(bodies); j++ { b := bodies[j] - if !isLarge(b) && needsSolving(a, b) && boxes[i].Overlaps(boxes[j]) { + if !isLarge(b) && emitted(a, b) && boxes[i].Overlaps(boxes[j]) { pairs = append(pairs, Pair{BodyA: a, BodyB: b}) } } @@ -113,6 +113,11 @@ func bruteForcePairs(bodies []*actor.RigidBody, boxes []actor.AABB) []Pair { return pairs } +// emitted: the pair of the bodies is one of the step, to solve or a trigger +func emitted(a, b *actor.RigidBody) bool { + return needsSolving(a, b) || detectsTrigger(a, b) +} + func samePairs(t *testing.T, got, want []Pair, what string) { t.Helper() if len(got) != len(want) { @@ -171,6 +176,50 @@ func TestTreePairsAsBruteForce(t *testing.T) { } } +// The pairs of a body with hundreds of pairs (a static zone with 400 crates in it, asleep or awake, kinematic cubes +// moving in it, and a kinematic trigger) are those of the brute force, in the same order, with 1 and 8 workers +func TestTreePairsOfAZoneAsBruteForce(t *testing.T) { + for _, workers := range []int{1, 8} { + w := newScene(workers) + addGround(w, 0.6) + zone := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{10, 1, 10}}, actor.BodyTypeStatic, 0, 0) + zone.IsTrigger = true + for k := 0; k < 400; k++ { + addBody(w, mgl64.Vec3{float64(k%20)*0.8 - 8, 0.3 + float64(k%3), float64(k/20)*0.8 - 8}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + } + var movers []*actor.RigidBody + for k := 0; k < 8; k++ { + movers = append(movers, kinematicBox(w, mgl64.Vec3{float64(k)*2 - 8, 2.5, 0}, cube().HalfExtents)) + } + movers[0].IsTrigger = true + for step := 0; step < 240; step++ { + for _, mover := range movers { + moveTo(mover, translated(mover.Transform, mgl64.Vec3{0, 0, kinematicSpeed})) + } + w.Step(sceneDt) + if step%40 == 39 { + pool := w.workerPool() + pool.begin(workers) + got := w.tree.findPairs(w.Bodies, w.aabbs, pool) + pool.end() + samePairs(t, got, bruteForcePairs(w.Bodies, w.aabbs), "zone") + zonePairs := 0 + for _, pair := range got { + if pair.BodyA == zone || pair.BodyB == zone { + zonePairs++ + } + } + if zonePairs < 400 { + t.Fatalf("workers %d: %d pairs of the zone, the test doesn't cover a body with hundreds of pairs", workers, zonePairs) + } + } + } + if !slices.ContainsFunc(w.Bodies, func(b *actor.RigidBody) bool { return b.IsSleeping && b.BodyType == actor.BodyTypeDynamic }) { + t.Errorf("workers %d: no crate asleep, the test doesn't cover the pairs at rest", workers) + } + } +} + // A body removed right after a step it left its enlarged AABB in (the tree keeps it to find its pairs at the next step) // is forgotten: the next step finds the pairs of the bodies left, the removed body being the last one of the World or // one in the middle diff --git a/trianglemesh_test.go b/trianglemesh_test.go new file mode 100644 index 0000000..c4bdeb1 --- /dev/null +++ b/trianglemesh_test.go @@ -0,0 +1,587 @@ +package feather + +import ( + "math" + "math/rand" + "testing" + "time" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== MESHES ========== + +// gridMesh: n x n cells of size cell, at the height of the function, the vertices shared, the triangles facing up +func gridMesh(n int, cell float64, height func(x, z float64) float64) *actor.TriangleMesh { + vertices := make([]mgl64.Vec3, 0, (n+1)*(n+1)) + for x := 0; x <= n; x++ { + for z := 0; z <= n; z++ { + px, pz := (float64(x)-float64(n)/2)*cell, (float64(z)-float64(n)/2)*cell + vertices = append(vertices, mgl64.Vec3{px, height(px, pz), pz}) + } + } + indices := make([]int32, 0, 6*n*n) + for x := 0; x < n; x++ { + for z := 0; z < n; z++ { + a, b, c, d := int32(x*(n+1)+z), int32(x*(n+1)+z+1), int32((x+1)*(n+1)+z+1), int32((x+1)*(n+1)+z) + indices = append(indices, a, b, c, a, c, d) + } + } + m, err := actor.NewTriangleMesh(vertices, indices) + if err != nil { + panic(err) + } + return m +} + +// stairMesh: steps going up along x, each of width and rise, 2*depth wide: the risers face -x, the treads face up. The +// treads start at x = 0: the tread s (from 0) is at the height (s+1)*rise, from x = s*width to (s+1)*width +func stairMesh(steps int, width, rise, depth float64) *actor.TriangleMesh { + var vertices []mgl64.Vec3 + var indices []int32 + quad := func(a, b, c, d mgl64.Vec3) { + i := int32(len(vertices)) + vertices = append(vertices, a, b, c, d) + indices = append(indices, i, i+1, i+2, i, i+2, i+3) + } + for s := 0; s < steps; s++ { + x0, x1 := float64(s)*width, float64(s+1)*width + y := float64(s+1) * rise + quad(mgl64.Vec3{x0, y - rise, -depth}, mgl64.Vec3{x0, y - rise, depth}, mgl64.Vec3{x0, y, depth}, mgl64.Vec3{x0, y, -depth}) + quad(mgl64.Vec3{x0, y, -depth}, mgl64.Vec3{x0, y, depth}, mgl64.Vec3{x1, y, depth}, mgl64.Vec3{x1, y, -depth}) + } + m, err := actor.NewTriangleMesh(vertices, indices) + if err != nil { + panic(err) + } + return m +} + +// meshDepth: the deepest point of the body under a flat grid mesh at the height 0 (its bottom along the normal) +func meshDepth(body *actor.RigidBody) float64 { + return math.Max(0, -body.SupportWorld(mgl64.Vec3{0, -1, 0}).Y()) +} + +// Criterion 2: on a mesh plane (a tilted grid), a sphere rolls exactly as on a plane: the inner edges are invisible +func TestSphereRollsOnAMeshLikeOnAPlane(t *testing.T) { + angle := 15 * math.Pi / 180 + normal := mgl64.Vec3{-math.Sin(angle), math.Cos(angle), 0} + start := mgl64.Vec3{3.1, 3.1*math.Tan(angle) + 0.3/math.Cos(angle), 0.37} + + onPlane := newScene(1) + addBody(onPlane, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Plane{Normal: normal}, actor.BodyTypeStatic, 0.5, 0) + planeSphere := addBody(onPlane, start, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + + onMesh := newScene(1) + mesh := gridMesh(64, 0.5, func(x, z float64) float64 { return x * math.Tan(angle) }) + addBody(onMesh, mgl64.Vec3{}, mgl64.QuatIdent(), mesh, actor.BodyTypeStatic, 0.5, 0) + meshSphere := addBody(onMesh, start, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + + worst := 0.0 + for step := 0; step < int(math.Round(2/sceneDt)); step++ { + onPlane.Step(sceneDt) + onMesh.Step(sceneDt) + worst = math.Max(worst, planeSphere.Transform.Position.Sub(meshSphere.Transform.Position).Len()) + } + travelled := meshSphere.Transform.Position.Sub(start).Len() + t.Logf("travelled %.2f m, worst gap with the plane %.4f mm", travelled, worst*1000) + if travelled < 2 { + t.Errorf("the sphere didn't roll: %.2f m", travelled) + } + if worst > 0.001 { + t.Errorf("the sphere is %.3f mm from the sphere on the plane", worst*1000) + } +} + +// A box sliding on a flat mesh crosses the inner edges without being kicked, across the diagonals and the sides +func TestBoxSlidesOverTheInnerEdgesOfAMesh(t *testing.T) { + w := newScene(1) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), gridMesh(40, 0.5, func(x, z float64) float64 { return 0 }), actor.BodyTypeStatic, 0.1, 0) + position := mgl64.Vec3{-6, cubeHalf, -3.2} + box := addBody(w, position, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.1, 0) + box.Velocity = mgl64.Vec3{5, 0, 2} + worstNormalSpeed, worstSpin, worstDepth := 0.0, 0.0, 0.0 + simulate(w, 1, func() { + worstNormalSpeed = math.Max(worstNormalSpeed, math.Abs(box.Velocity.Y())) + worstSpin = math.Max(worstSpin, box.AngularVelocity.Len()) + worstDepth = math.Max(worstDepth, meshDepth(box)) + }) + travelled := box.Transform.Position.Sub(position).Len() + t.Logf("travelled %.2f m, worst normal speed %.4f m/s, worst spin %.4f rad/s, worst depth %.3f mm", travelled, worstNormalSpeed, worstSpin, worstDepth*1000) + if travelled < 2 { + t.Errorf("the box stopped after %.2f m", travelled) + } + if worstNormalSpeed > 0.01 || worstSpin > 0.05 { + t.Errorf("the box was kicked by an inner edge") + } + if worstDepth > landingDepth { + t.Errorf("the box went %.2f mm in the mesh", worstDepth*1000) + } +} + +// bruteForceMeshRay: the first triangle of the mesh (at the identity transform) on the ray, from the side of its normal, +// the lowest index at the same fraction +func bruteForceMeshRay(m *actor.TriangleMesh, origin, translation mgl64.Vec3) (float64, int32, bool) { + best, triangle, found := 1.0, actor.NoTriangle, false + for i := 0; i < m.TriangleCount(); i++ { + vertices, _ := m.Triangle(int32(i)) + edge1, edge2 := vertices[1].Sub(vertices[0]), vertices[2].Sub(vertices[0]) + p := translation.Cross(edge2) + determinant := edge1.Dot(p) + if determinant <= 0 { + continue + } + s := origin.Sub(vertices[0]) + u := s.Dot(p) / determinant + q := s.Cross(edge1) + v := translation.Dot(q) / determinant + fraction := edge2.Dot(q) / determinant + if u < -1e-9 || v < -1e-9 || u+v > 1+1e-9 || fraction < 0 || fraction > best || (found && fraction == best && int32(i) > triangle) { + continue + } + best, triangle, found = fraction, int32(i), true + } + return best, triangle, found +} + +// Criterion 3: a ray on a mesh of 100 000 triangles gives the triangle of the brute force (250 rays, each tested +// against every triangle), and takes less than 5 µs on average (measured on 100 000 rays, without the race detector) +func TestRayOnAMeshOf100000Triangles(t *testing.T) { + mesh := gridMesh(224, 0.25, func(x, z float64) float64 { return 2*math.Sin(x*0.3)*math.Cos(z*0.2) + 0.3*math.Sin(x*2+z) }) + if mesh.TriangleCount() < 100000 { + t.Fatalf("%d triangles", mesh.TriangleCount()) + } + w := newScene(1) + body := addBody(w, mgl64.Vec3{1, 2, 3}, mgl64.QuatRotate(0.3, mgl64.Vec3{0, 1, 0}), mesh, actor.BodyTypeStatic, 0.5, 0) + addBody(w, mgl64.Vec3{0, 50, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 1}, actor.BodyTypeDynamic, 0.5, 0) + w.SyncQueries() + r := rand.New(rand.NewSource(7)) + filter := DefaultQueryFilter() + checked := 0 + for i := 0; i < 250; i++ { + origin := mgl64.Vec3{(r.Float64() - 0.5) * 50, 10 + r.Float64()*5, (r.Float64() - 0.5) * 50} + translation := mgl64.Vec3{r.NormFloat64() * 5, -20, r.NormFloat64() * 5} + wantFraction, wantTriangle, wantOk := bruteForceMeshRay(mesh, body.Transform.ToLocal(origin), body.Transform.Rotation.Conjugate().Rotate(translation)) + hit, ok := w.Raycast(origin, translation, filter) + if ok != wantOk || (ok && hit.Body != body) { + t.Fatalf("ray %d: hit %v, the brute force says %v", i, ok, wantOk) + } + if !ok { + continue + } + checked++ + if hit.Triangle != wantTriangle || math.Abs(hit.Fraction-wantFraction) > 1e-12 { + t.Fatalf("ray %d: triangle %d at %.12f, the brute force says %d at %.12f", i, hit.Triangle, hit.Fraction, wantTriangle, wantFraction) + } + if gap := hit.Point.Sub(origin.Add(translation.Mul(hit.Fraction))).Len(); gap > 1e-9 { + t.Fatalf("ray %d: the point is %.3g from the ray", i, gap) + } + vertices, _ := mesh.Triangle(hit.Triangle) + if normal := body.Transform.Rotation.Rotate(vertices[1].Sub(vertices[0]).Cross(vertices[2].Sub(vertices[0])).Normalize()); hit.Normal.Sub(normal).Len() > 1e-9 { + t.Fatalf("ray %d: normal %v, want %v", i, hit.Normal, normal) + } + } + if checked < 125 { + t.Fatalf("only %d rays checked", checked) + } + + const rays = 100000 + origins := make([]mgl64.Vec3, rays) + for i := range origins { + origins[i] = mgl64.Vec3{(r.Float64() - 0.5) * 50, 12, (r.Float64() - 0.5) * 50} + } + hits := 0 + start := time.Now() + for _, origin := range origins { + if _, ok := w.Raycast(origin, mgl64.Vec3{1, -20, 0.5}, filter); ok { + hits++ + } + } + perRay := time.Since(start) / rays + t.Logf("%d rays on %d triangles: %d hits, %v per ray", rays, mesh.TriangleCount(), hits, perRay) + if hits < rays/2 { + t.Errorf("only %d hits", hits) + } + if !raceEnabled && perRay > 5*time.Microsecond { + t.Errorf("%v per ray, want less than 5 µs", perRay) + } +} + +// Criterion 4: dynamic boxes rest on a staircase mesh for 10 s: a cube on a tread, a cube overhanging the edge of a +// tread, a plank lying along the slope on the edges of 3 treads (active edges only). None drifts, none sinks +func TestBoxesRestOnMeshStairs(t *testing.T) { + const width, rise = 0.3, 0.17 + w := newScene(1) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), stairMesh(8, width, rise, 1), actor.BodyTypeStatic, 0.6, 0) + small := &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.1, 0.1}} + onTread := addBody(w, mgl64.Vec3{2.5 * width, 3*rise + 0.1 + 0.002, 0}, mgl64.QuatIdent(), small, actor.BodyTypeDynamic, 0.6, 0) + overhanging := addBody(w, mgl64.Vec3{4*width + 0.05, 5*rise + 0.1 + 0.002, 0.5}, mgl64.QuatIdent(), small, actor.BodyTypeDynamic, 0.6, 0) + slope := math.Atan2(rise, width) + plank := &actor.Box{HalfExtents: mgl64.Vec3{0.6, 0.03, 0.1}} + onEdges := addBody(w, mgl64.Vec3{4 * width, 4*rise + 0.25, -0.6}, mgl64.QuatRotate(slope, mgl64.Vec3{0, 0, 1}), plank, actor.BodyTypeDynamic, 0.8, 0) + + bodies := map[string]*actor.RigidBody{"cube on a tread": onTread, "cube overhanging an edge": overhanging, "plank on 3 edges": onEdges} + simulate(w, 2, nil) + settled := map[string]actor.Transform{} + for name, body := range bodies { + settled[name] = body.Transform + } + simulate(w, 10, nil) + for name, body := range bodies { + drift := body.Transform.Position.Sub(settled[name].Position).Len() + turned := 2 * math.Acos(math.Min(1, math.Abs(body.Transform.Rotation.Dot(settled[name].Rotation)))) + t.Logf("%s: drifted %.3f mm and turned %.4f° in 10 s, sleeping %v", name, drift*1000, turned*180/math.Pi, body.IsSleeping) + if drift > 0.0005 || turned > 0.001 || !body.IsSleeping { + t.Errorf("%s is not stable", name) + } + } + // the cube on its tread, the plank on the edges: at their height + if depth := 3*rise + 0.1 - onTread.Transform.Position.Y(); math.Abs(depth) > landingDepth { + t.Errorf("the cube on the tread is %.2f mm deep", depth*1000) + } + // the plank's bottom line passes 3 cm (its half thickness) over the edges, in the direction of its normal + normal := onEdges.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}) + for _, edge := range []mgl64.Vec3{{3 * width, 4 * rise, -0.6}, {4 * width, 5 * rise, -0.6}, {5 * width, 6 * rise, -0.6}} { + if gap := onEdges.Transform.Position.Sub(edge).Dot(normal) - 0.03; math.Abs(gap) > landingDepth { + t.Errorf("the plank is %.2f mm from the edge %v", gap*1000, edge) + } + } +} + +// A mesh is a surface with a side: a sphere coming from behind a wall goes through it, a ray from behind misses it, a +// sweep from behind misses it; the continuous collision stops a fast ball on its front +func TestMeshIsOneSided(t *testing.T) { + // a wall at x = 0 facing -x + wall, err := actor.NewTriangleMesh([]mgl64.Vec3{{0, -2, -2}, {0, 2, -2}, {0, 2, 2}, {0, -2, 2}}, []int32{0, 2, 1, 0, 3, 2}) + if err != nil { + t.Fatal(err) + } + w := newScene(1) + w.Gravity = mgl64.Vec3{} + wallBody := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), wall, actor.BodyTypeStatic, 0, 0) + fast := addBody(w, mgl64.Vec3{-6, 0.3, 0.2}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.05}, actor.BodyTypeDynamic, 0, 0) + fast.Velocity = mgl64.Vec3{200, 0, 0} + slow := addBody(w, mgl64.Vec3{-1, -0.5, -0.5}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0, 0) + slow.Velocity = mgl64.Vec3{3, 0, 0} + behind := addBody(w, mgl64.Vec3{1, 0.5, 0.5}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0, 0) + behind.Velocity = mgl64.Vec3{-3, 0, 0} + simulate(w, 0.5, nil) + t.Logf("fast ball at x = %.4f, slow ball at x = %.4f, ball from behind at x = %.3f", fast.Transform.Position.X(), slow.Transform.Position.X(), behind.Transform.Position.X()) + if fast.Transform.Position.X() > 0 || fast.Transform.Position.X() < -0.05-landingDepth { + t.Errorf("the fast ball is at x = %.4f, want on the wall", fast.Transform.Position.X()) + } + if slow.Transform.Position.X() > 0 || slow.Transform.Position.X() < -0.1-landingDepth { + t.Errorf("the slow ball is at x = %.4f, want on the wall", slow.Transform.Position.X()) + } + if behind.Transform.Position.X() > -0.3 { + t.Errorf("the ball from behind was stopped at x = %.3f", behind.Transform.Position.X()) + } + + w.SyncQueries() + filter := DefaultQueryFilter() + filter.Excluded = []*actor.RigidBody{fast, slow, behind} + if hit, ok := w.Raycast(mgl64.Vec3{-1, 0.5, 0.5}, mgl64.Vec3{3, 0, 0}, filter); !ok || hit.Body != wallBody || math.Abs(hit.Fraction-1.0/3) > 1e-12 || hit.Normal != (mgl64.Vec3{-1, 0, 0}) || hit.Triangle != 0 { + t.Errorf("ray on the front: %v %v", hit, ok) + } + if _, ok := w.Raycast(mgl64.Vec3{1, 0.5, 0.5}, mgl64.Vec3{-3, 0, 0}, filter); ok { + t.Errorf("a ray from behind hits the wall") + } + ball := &actor.Sphere{Radius: 0.1} + // (-0.5, 0.5) is in the triangle 1 of the wall, off its diagonal + if hit, ok := w.Sweep(ball, actor.Transform{Position: mgl64.Vec3{-1, -0.5, 0.5}, Rotation: mgl64.QuatIdent()}, mgl64.Vec3{3, 0, 0}, filter); !ok || hit.Body != wallBody || math.Abs(hit.Fraction-0.3) > 1e-5 || hit.Triangle != 1 || hit.Point.Sub(mgl64.Vec3{0, -0.5, 0.5}).Len() > 1e-5 { + t.Errorf("sweep on the front: %v %v", hit, ok) + } + if _, ok := w.Sweep(ball, actor.Transform{Position: mgl64.Vec3{1, 0.5, 0.5}, Rotation: mgl64.QuatIdent()}, mgl64.Vec3{-3, 0, 0}, filter); ok { + t.Errorf("a sweep from behind hits the wall") + } + // an overlap sees both sides + for _, x := range []float64{-0.05, 0.05} { + if bodies := w.Overlap(ball, actor.Transform{Position: mgl64.Vec3{x, 0.5, 0.5}, Rotation: mgl64.QuatIdent()}, filter, nil); len(bodies) != 1 || bodies[0] != wallBody { + t.Errorf("overlap at x = %v: %d bodies", x, len(bodies)) + } + } + if bodies := w.Overlap(ball, actor.Transform{Position: mgl64.Vec3{-0.2, 0.5, 0.5}, Rotation: mgl64.QuatIdent()}, filter, nil); len(bodies) != 0 { + t.Errorf("overlap away from the wall: %d bodies", len(bodies)) + } +} + +// A sweep on the stairs stops on the right tread, with its triangle; RaycastAll on a mesh gives one hit per body; a +// mesh as the moving shape of a sweep or of an overlap panics +func TestMeshQueries(t *testing.T) { + const width, rise = 0.3, 0.17 + stairs := stairMesh(8, width, rise, 1) + w := newScene(1) + body := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), stairs, actor.BodyTypeStatic, 0.6, 0) + w.SyncQueries() + filter := DefaultQueryFilter() + sole := &actor.Sphere{Radius: 0.08} + for s := 0; s < 8; s++ { + x := (float64(s) + 0.5) * width + hit, ok := w.Sweep(sole, actor.Transform{Position: mgl64.Vec3{x, 3, 0.2}, Rotation: mgl64.QuatIdent()}, mgl64.Vec3{0, -4, 0}, filter) + if !ok || hit.Body != body { + t.Fatalf("step %d: no hit", s) + } + wantY := float64(s+1) * rise + if math.Abs(3-4*hit.Fraction-0.08-wantY) > 1e-5 || math.Abs(hit.Point.Y()-wantY) > 1e-5 || hit.Normal.Sub(mgl64.Vec3{0, 1, 0}).Len() > 1e-6 { + t.Errorf("step %d: stopped at %.5f, point %v, normal %v, want the tread at %.2f", s, 3-4*hit.Fraction, hit.Point, hit.Normal, wantY) + } + // the tread s is the quads 2s+1: triangles 4s+2 and 4s+3 + if hit.Triangle/4 != int32(s) || hit.Triangle%4 < 2 { + t.Errorf("step %d: triangle %d, want one of the tread", s, hit.Triangle) + } + ray, ok := w.Raycast(mgl64.Vec3{x, 3, 0.2}, mgl64.Vec3{0, -4, 0}, filter) + if !ok || math.Abs(ray.Point.Y()-wantY) > 1e-9 || ray.Triangle/4 != int32(s) { + t.Errorf("step %d: ray %v %v", s, ray, ok) + } + } + // a ray along the stairs, through 3 risers: the first one + hit, ok := w.Raycast(mgl64.Vec3{-1, 2.5 * rise, 0}, mgl64.Vec3{4, 0, 0}, filter) + if !ok || hit.Triangle/4 != 2 || hit.Triangle%4 > 1 || math.Abs(hit.Point.X()-2*width) > 1e-9 { + t.Errorf("ray along the stairs: %v %v, want the riser of the step 2", hit, ok) + } + hits := w.RaycastAll(mgl64.Vec3{-1, 2.5 * rise, 0}, mgl64.Vec3{4, 0, 0}, filter, nil) + if len(hits) != 1 { + t.Errorf("RaycastAll gives %d hits on the mesh, want 1 (its first triangle)", len(hits)) + } + + for _, shape := range []actor.ShapeInterface{stairs, &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}} { + func() { + defer func() { + if recover() == nil { + t.Errorf("a sweep of a %T didn't panic", shape) + } + }() + w.Sweep(shape, actor.NewTransform(), mgl64.Vec3{1, 0, 0}, filter) + }() + func() { + defer func() { + if recover() == nil { + t.Errorf("an overlap of a %T didn't panic", shape) + } + }() + w.Overlap(shape, actor.NewTransform(), filter, nil) + }() + } +} + +// meshPile: bodies of every shape (boxes, spheres, capsules, hulls) dropped on a hilly mesh, one every 0.4 m of height +func meshPile(workers, count int) (*World, []*actor.RigidBody) { + w := newScene(workers) + w.parallelFrom = 1 + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), gridMesh(40, 0.5, func(x, z float64) float64 { return 0.3 * math.Sin(x) * math.Cos(z*0.7) }), actor.BodyTypeStatic, 0.6, 0) + r := rand.New(rand.NewSource(5)) + var bodies []*actor.RigidBody + var points []mgl64.Vec3 + for i := 0; i < count; i++ { + var shape actor.ShapeInterface = &actor.Box{HalfExtents: mgl64.Vec3{0.2, 0.15, 0.25}} + switch i % 4 { + case 1: + shape = &actor.Sphere{Radius: 0.2} + case 2: + shape = &actor.Capsule{HalfHeight: 0.25, Radius: 0.12} + case 3: + points = points[:0] + for k := 0; k < 20; k++ { + points = append(points, mgl64.Vec3{r.NormFloat64() * 0.2, r.NormFloat64() * 0.2, r.NormFloat64() * 0.2}) + } + hull, err := actor.NewConvexHull(points, 32) + if err != nil { + panic(err) + } + shape = hull + } + position := mgl64.Vec3{(r.Float64() - 0.5) * 6, 1 + float64(i)*0.4, (r.Float64() - 0.5) * 6} + rotation := mgl64.QuatRotate(r.Float64()*3, mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize()) + bodies = append(bodies, addBody(w, position, rotation, shape, actor.BodyTypeDynamic, 0.6, 0)) + } + return w, bodies +} + +// A pile of every shape on a mesh gives the same bits with 1 and 8 workers, and nothing falls through the mesh: 64 +// bodies for 3 s (the last one, dropped from 26 m, lands within 2.3 s) +func TestMeshPileIsDeterministic(t *testing.T) { + single, bodies := meshPile(1, 64) + parallel, others := meshPile(8, 64) + defer parallel.Close() + for i := 0; i < 180; i++ { + single.Step(sceneDt) + parallel.Step(sceneDt) + } + for i := range bodies { + if bodies[i].Transform != others[i].Transform { + t.Fatalf("body %d: %v with 1 worker, %v with 8", i, bodies[i].Transform, others[i].Transform) + } + if !finite(bodies[i].Transform.Position) || bodies[i].Transform.Position.Y() < -1 { + t.Errorf("body %d fell through the mesh: %v", i, bodies[i].Transform.Position) + } + } +} + +// Criterion 5: a step with meshes and hulls allocates nothing once the buffers have grown, nor the queries on a mesh: a +// pile of 150 bodies after 4 s (a pile of 64 is still settling then, and its steps grow their buffers at times) +func TestMeshDoesNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + for _, workers := range []int{1, 4} { + w, _ := meshPile(workers, 150) + for i := 0; i < 240; i++ { + w.Step(sceneDt) + } + if allocs := testing.AllocsPerRun(10, func() { w.Step(sceneDt) }); allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", workers, allocs) + } + w.Close() + } + + w := newScene(1) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), stairMesh(8, 0.3, 0.17, 1), actor.BodyTypeStatic, 0.6, 0) + w.SyncQueries() + filter := DefaultQueryFilter() + sole := &actor.Sphere{Radius: 0.08} + at := actor.Transform{Position: mgl64.Vec3{1.35, 3, 0.2}, Rotation: mgl64.QuatIdent()} + var bodies []*actor.RigidBody + var hits []Hit + found := 0 + allocs := testing.AllocsPerRun(10, func() { + if _, ok := w.Raycast(at.Position, mgl64.Vec3{0, -4, 0}, filter); ok { + found++ + } + if _, ok := w.Sweep(sole, at, mgl64.Vec3{0, -4, 0}, filter); ok { + found++ + } + bodies = w.Overlap(sole, actor.Transform{Position: mgl64.Vec3{1.35, 0.9, 0.2}, Rotation: mgl64.QuatIdent()}, filter, bodies[:0]) + hits = w.RaycastAll(at.Position, mgl64.Vec3{0, -4, 0}, filter, hits[:0]) + found += len(bodies) + len(hits) + }) + if allocs > 0 || found == 0 { + t.Errorf("%.1f allocations for the queries on a mesh (%d hits), want 0", allocs, found) + } +} + +// ========== HULLS ========== + +// cubeHull: the hull of the 8 corners of a cube +func cubeHull(half float64) *actor.ConvexHull { + corners := make([]mgl64.Vec3, 0, 8) + for i := 0; i < 8; i++ { + corners = append(corners, mgl64.Vec3{float64(i&1)*2 - 1, float64(i>>1&1)*2 - 1, float64(i>>2&1)*2 - 1}.Mul(half)) + } + hull, err := actor.NewConvexHull(corners, 8) + if err != nil { + panic(err) + } + return hull +} + +// The hull of a cube rests on the ground and on a terrain as the Box does: the same depth within 0.1 mm, the same +// count of contact points, asleep +func TestHullCubeRestsLikeABox(t *testing.T) { + for _, terrain := range []bool{false, true} { + w := newScene(1) + var surface *actor.RigidBody + if terrain { + surface = slopeTerrain(w, 0.1, 0.5) + } else { + surface = addGround(w, 0.5) + } + rotation := mgl64.QuatRotate(0.4, mgl64.Vec3{0, 1, 0}) + asHull := addBody(w, mgl64.Vec3{1, 1, 0}, rotation, cubeHull(cubeHalf), actor.BodyTypeDynamic, 0.5, 0) + asBox := addBody(w, mgl64.Vec3{-1, 1, 0}, rotation, cube(), actor.BodyTypeDynamic, 0.5, 0) + simulate(w, 3, nil) + hullDepth, boxDepth := surfaceDepth(surface, asHull), surfaceDepth(surface, asBox) + t.Logf("terrain %v: the hull rests %.4f mm deep, the box %.4f mm", terrain, hullDepth*1000, boxDepth*1000) + if math.Abs(hullDepth-boxDepth) > 0.0001 || !asHull.IsSleeping { + t.Errorf("terrain %v: the hull doesn't rest as the box", terrain) + } + if asHull.Material.GetMass() != asBox.Material.GetMass() { + t.Errorf("mass %v, the box has %v", asHull.Material.GetMass(), asBox.Material.GetMass()) + } + asHull.WakeUp() + asBox.WakeUp() + w.Step(sceneDt) + hullPoints, boxPoints := 0, 0 + for _, m := range w.Contacts() { + if m.BodyA == asHull || m.BodyB == asHull { + hullPoints += m.Count + } + if m.BodyA == asBox || m.BodyB == asBox { + boxPoints += m.Count + } + } + if hullPoints != boxPoints || hullPoints < 4 { + t.Errorf("terrain %v: the hull rests on %d points, the box on %d", terrain, hullPoints, boxPoints) + } + } +} + +// Hulls dropped on hills and on stairs come to rest, none is lost; a hull of 32 vertices and one of 256 behave +func TestHullsRestOnSurfaces(t *testing.T) { + r := rand.New(rand.NewSource(9)) + var points []mgl64.Vec3 + for _, stairs := range []bool{false, true} { + w := newScene(1) + if stairs { + addGround(w, 0.6) + addBody(w, mgl64.Vec3{-1.5, 0, 0}, mgl64.QuatIdent(), stairMesh(10, 0.3, 0.17, 3), actor.BodyTypeStatic, 0.6, 0) + } else { + bumpyTerrain(w, 4) + } + var hulls []*actor.RigidBody + for i := 0; i < 20; i++ { + points = points[:0] + count, limit := 30, 32 + if i%5 == 0 { + count, limit = 400, actor.MaxHullVertices + } + for k := 0; k < count; k++ { + points = append(points, mgl64.Vec3{r.NormFloat64() * 0.2, r.NormFloat64() * 0.15, r.NormFloat64() * 0.2}) + } + hull, err := actor.NewConvexHull(points, limit) + if err != nil { + t.Fatal(err) + } + position := mgl64.Vec3{(r.Float64() - 0.5) * 4, 3 + float64(i)*0.3, (r.Float64() - 0.5) * 4} + rotation := mgl64.QuatRotate(r.Float64()*3, mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize()) + hulls = append(hulls, addBody(w, position, rotation, hull, actor.BodyTypeDynamic, 0.6, 0)) + } + simulate(w, 8, nil) + asleep := 0 + for i, h := range hulls { + if h.IsSleeping { + asleep++ + } + if !finite(h.Transform.Position) || h.Transform.Position.Y() < -3 { + t.Errorf("stairs %v: the hull %d is lost at %v", stairs, i, h.Transform.Position) + } + } + t.Logf("stairs %v: %d/20 hulls asleep", stairs, asleep) + if asleep < 18 { + t.Errorf("stairs %v: only %d hulls asleep", stairs, asleep) + } + } +} + +// A hull is swept and overlapped as any convex shape, and a ray hits it; a hull and a mesh collide through CollideAll +func TestHullQueries(t *testing.T) { + w := newScene(1) + hull := cubeHull(0.5) + body := addBody(w, mgl64.Vec3{3, 0, 0}, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}), hull, actor.BodyTypeStatic, 0.5, 0) + w.SyncQueries() + filter := DefaultQueryFilter() + // the corner of the turned cube is at x = 3 - sqrt(0.5) + corner := 3 - math.Sqrt(0.5) + if hit, ok := w.Raycast(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{4, 0, 0}, filter); !ok || hit.Body != body || math.Abs(hit.Point.X()-corner) > 1e-9 { + t.Errorf("ray on the hull: %v %v", hit, ok) + } + if hit, ok := w.Sweep(&actor.Sphere{Radius: 0.1}, actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, mgl64.Vec3{4, 0, 0}, filter); !ok || math.Abs(hit.Point.X()-corner) > 1e-5 || math.Abs(4*hit.Fraction+0.1-corner) > 1e-5 { + t.Errorf("sweep on the hull: %v %v", hit, ok) + } + if bodies := w.Overlap(hull, actor.Transform{Position: mgl64.Vec3{3.9, 0, 0}, Rotation: mgl64.QuatIdent()}, filter, nil); len(bodies) != 1 { + t.Errorf("overlap of a hull: %d bodies", len(bodies)) + } + if bodies := w.Overlap(hull, actor.Transform{Position: mgl64.Vec3{4.3, 0, 0}, Rotation: mgl64.QuatIdent()}, filter, nil); len(bodies) != 0 { + t.Errorf("overlap of a hull away: %d bodies", len(bodies)) + } +} diff --git a/trigger_test.go b/trigger_test.go new file mode 100644 index 0000000..416bb85 --- /dev/null +++ b/trigger_test.go @@ -0,0 +1,623 @@ +package feather + +import ( + "slices" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// The pairs of a trigger and the sleep: a trigger reports the end of an overlap when the shapes no longer overlap, or +// when one of the bodies leaves the world, never because one of them falls asleep (b2SensorEndTouchEvent of Box2D v3, +// whose sensors "do not consider sleep"; the trigger pairs of PhysX, kept while both actors sleep) + +// triggerLog records the trigger events of a world, by step (and its collision events, listenCollisions) +type triggerLog struct { + w *World + events []triggerRecord + // counting: the events are only counted, without allocation + counting bool + counted int +} + +type triggerRecord struct { + step uint32 + eventType EventType + a, b uint64 // the serials of the bodies, in the order of the event +} + +func listenTriggers(w *World) *triggerLog { + return listenEvents(w, EventTriggerEnter, EventTriggerStay, EventTriggerExit) +} + +// listenEvents: a log of the events of the types +func listenEvents(w *World, eventTypes ...EventType) *triggerLog { + log := &triggerLog{w: w} + for _, eventType := range eventTypes { + w.Events.Subscribe(eventType, log.record) + } + return log +} + +func (log *triggerLog) record(event Event) { + if log.counting { + log.counted++ + return + } + var a, b *actor.RigidBody + switch e := event.(type) { + case TriggerEnterEvent: + a, b = e.BodyA, e.BodyB + case TriggerStayEvent: + a, b = e.BodyA, e.BodyB + case TriggerExitEvent: + a, b = e.BodyA, e.BodyB + case CollisionEnterEvent: + a, b = e.BodyA, e.BodyB + case CollisionStayEvent: + a, b = e.BodyA, e.BodyB + case CollisionExitEvent: + a, b = e.BodyA, e.BodyB + } + log.events = append(log.events, triggerRecord{step: log.w.step, eventType: event.Type(), a: a.Serial(), b: b.Serial()}) +} + +// listenCollisions adds the collision events to the log +func (log *triggerLog) listenCollisions() *triggerLog { + for _, eventType := range []EventType{EventCollisionEnter, EventCollisionStay, EventCollisionExit} { + log.w.Events.Subscribe(eventType, log.record) + } + return log +} + +// count of the events of the type between the trigger and the body +func (log *triggerLog) count(eventType EventType, trigger, body *actor.RigidBody) int { + n := 0 + for _, record := range log.events { + if record.eventType == eventType && pairOf(record, trigger, body) { + n++ + } + } + return n +} + +func pairOf(record triggerRecord, a, b *actor.RigidBody) bool { + return (record.a == a.Serial() && record.b == b.Serial()) || (record.a == b.Serial() && record.b == a.Serial()) +} + +const ( + zoneHalf = 2.0 // the zone is a box of 4 x 2 x 4 m, on the ground + crateDrop = 1.5 // a crate falls from this height, into the zone + settleTime = 3.0 // s: a crate dropped into the zone has landed and fallen asleep + stayTime = 10.0 + pushSpeed = 6.0 // m/s: with a friction of 0.5 a crate slides 3.7 m, out of the zone +) + +// zoneScene: a trigger zone on the ground, of the body type, and a crate dropped in it +func zoneScene(workers int, zoneType actor.BodyType) (*World, *actor.RigidBody, *actor.RigidBody) { + w := newScene(workers) + addGround(w, 0.5) + zone := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{zoneHalf, 1, zoneHalf}}, zoneType, 0, 0) + zone.IsTrigger = true + crate := addBody(w, mgl64.Vec3{0, crateDrop, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + return w, zone, crate +} + +// A crate dropped in a zone lands, falls asleep, and stays in the zone: one enter, and no exit during 10 s +func TestASleepingCrateStaysInTheTrigger(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + log := listenTriggers(w) + simulate(w, settleTime, nil) + if !crate.IsSleeping { + t.Fatalf("the crate is awake after %v s: the scene doesn't test the sleep", settleTime) + } + simulate(w, stayTime, nil) + if !crate.IsSleeping { + t.Errorf("the crate woke up") + } + if enter := log.count(EventTriggerEnter, zone, crate); enter != 1 { + t.Errorf("%d enters, want 1", enter) + } + if exit := log.count(EventTriggerExit, zone, crate); exit != 0 { + t.Errorf("%d exits while the crate sleeps in the zone, want 0", exit) + } +} + +// While both bodies rest the overlap can't change: no stay event is sent (as for 2 sleeping bodies), the pair is kept +func TestASleepingCrateSendsNoStay(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + log := listenTriggers(w) + simulate(w, settleTime, nil) + stays := log.count(EventTriggerStay, zone, crate) + if stays == 0 { + t.Fatalf("no stay while the crate fell in the zone") + } + simulate(w, stayTime, nil) + if more := log.count(EventTriggerStay, zone, crate) - stays; more != 0 { + t.Errorf("%d stays while the crate sleeps in the zone, want 0", more) + } +} + +// A sleeping crate pushed out of the zone leaves it once, when it is out +func TestACratePushedOutOfTheTriggerLeavesOnce(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + log := listenTriggers(w) + simulate(w, settleTime, nil) + if !crate.IsSleeping { + t.Fatalf("the crate is awake after %v s", settleTime) + } + crate.WakeUp() + crate.Velocity = mgl64.Vec3{pushSpeed, 0, 0} + var exitX float64 + simulate(w, settleTime, func() { + if exitX == 0 && log.count(EventTriggerExit, zone, crate) > 0 { + exitX = crate.Transform.Position.X() + } + }) + if crate.Transform.Position.X() < zoneHalf+cubeHalf { + t.Fatalf("the crate stopped at x=%.2f, in the zone", crate.Transform.Position.X()) + } + if exit := log.count(EventTriggerExit, zone, crate); exit != 1 { + t.Errorf("%d exits, want 1", exit) + } + if enter := log.count(EventTriggerEnter, zone, crate); enter != 1 { + t.Errorf("%d enters, want 1", enter) + } + // the exit is sent at the step the crate leaves the zone: its near face is past the far face of the zone + if exitX-cubeHalf < zoneHalf-LinearSlop || exitX-cubeHalf > zoneHalf+pushSpeed*sceneDt { + t.Errorf("the exit was sent with the crate at x=%.3f, want its face just past %.2f", exitX, zoneHalf) + } +} + +// A crate removed from the world while it sleeps in the zone leaves it: the exit is sent at the next step, once +func TestACrateRemovedFromTheTriggerLeaves(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + log := listenTriggers(w) + simulate(w, settleTime, nil) + before := log.count(EventTriggerExit, zone, crate) + w.RemoveBody(crate) + w.Step(sceneDt) + if exit := log.count(EventTriggerExit, zone, crate) - before; exit != 1 { + t.Errorf("%d exits at the step after the removal of the crate, want 1", exit) + } + simulate(w, 1, nil) + if exit := log.count(EventTriggerExit, zone, crate) - before; exit != 1 { + t.Errorf("%d exits a second later, want 1", exit) + } +} + +// A trigger removed from the world ends its pairs: each body in it leaves, once +func TestARemovedTriggerEndsItsPairs(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + other := addBody(w, mgl64.Vec3{1, crateDrop, 1}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + log := listenTriggers(w) + simulate(w, settleTime, nil) + before := map[*actor.RigidBody]int{} + for _, body := range []*actor.RigidBody{crate, other} { + before[body] = log.count(EventTriggerExit, zone, body) + } + w.RemoveBody(zone) + simulate(w, 1, nil) + for _, body := range []*actor.RigidBody{crate, other} { + if exit := log.count(EventTriggerExit, zone, body) - before[body]; exit != 1 { + t.Errorf("%d exits of a crate after the removal of the zone, want 1", exit) + } + } +} + +// A static zone moved by the game away from a sleeping crate: the crate is out, it leaves (Box2D tests its sensors +// whatever the sleep, PhysX tests a trigger pair again when the pose of an actor is set) +func TestAZoneMovedAwayFromASleepingCrate(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + log := listenTriggers(w) + simulate(w, settleTime, nil) + before := log.count(EventTriggerExit, zone, crate) + zone.Transform.Position = mgl64.Vec3{3 * zoneHalf, 1, 0} + zone.UpdateAABB() + w.Step(sceneDt) + if exit := log.count(EventTriggerExit, zone, crate) - before; exit != 1 { + t.Errorf("%d exits once the zone moved away, want 1", exit) + } + if !crate.IsSleeping { + t.Errorf("the zone woke the crate up") + } + // and back over the crate: it enters again + zone.Transform.Position = mgl64.Vec3{0, 1, 0} + zone.UpdateAABB() + w.Step(sceneDt) + if enter := log.count(EventTriggerEnter, zone, crate); enter != 2 { + t.Errorf("%d enters once the zone is back, want 2", enter) + } +} + +// A sleeping crate moved by hand out of the zone (its Transform, then UpdateAABB, without waking it up) leaves it: +// the overlap of a pair at rest is kept only while the AABBs of its bodies stay the same +func TestASleepingCrateMovedByHandLeaves(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + // the crate lands across the side of the zone, 5 cm in it + crate.Transform.Position = mgl64.Vec3{zoneHalf + cubeHalf - 0.05, crateDrop, 0} + crate.UpdateAABB() + log := listenTriggers(w) + simulate(w, settleTime, nil) + if !crate.IsSleeping || log.count(EventTriggerEnter, zone, crate) != 1 { + t.Fatalf("the crate is not asleep in the zone (asleep %v)", crate.IsSleeping) + } + before := log.count(EventTriggerExit, zone, crate) + // 7 cm further, out of the zone: the crate stays in its enlarged AABB of the broad phase (AABBMargin) + crate.Transform.Position = crate.Transform.Position.Add(mgl64.Vec3{0.07, 0, 0}) + crate.UpdateAABB() + w.Step(sceneDt) + if exit := log.count(EventTriggerExit, zone, crate) - before; exit != 1 { + t.Errorf("%d exits once the crate was moved out of the zone, want 1", exit) + } + if !crate.IsSleeping { + t.Errorf("the move woke the crate up: the test doesn't test a body at rest") + } +} + +// A static box made a trigger by the game under a crate asleep on it: the crate enters it (its contact with the box +// is an overlap), its contact with the box ends, and nothing wakes up +func TestABoxMadeATriggerUnderASleepingCrate(t *testing.T) { + w := newScene(1) + addGround(w, 0.5) + pedestal := addBody(w, mgl64.Vec3{0, 0.5, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 0.5, 1}}, actor.BodyTypeStatic, 0.5, 0) + crate := addBody(w, mgl64.Vec3{0, 1 + crateDrop, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + log := listenTriggers(w).listenCollisions() + simulate(w, settleTime, nil) + if !crate.IsSleeping { + t.Fatalf("the crate is awake after %v s", settleTime) + } + exits := log.count(EventCollisionExit, pedestal, crate) + pedestal.IsTrigger = true + w.Step(sceneDt) + if enter := log.count(EventTriggerEnter, pedestal, crate); enter != 1 { + t.Errorf("%d enters once the box is a trigger, want 1", enter) + } + if exit := log.count(EventCollisionExit, pedestal, crate) - exits; exit != 1 { + t.Errorf("%d collision exits once the box is a trigger, want 1: the contact ended", exit) + } + if !crate.IsSleeping { + t.Errorf("the crate woke up") + } +} + +// A listener which removes the crate as it enters gets its exit in the same flush: an event of a removal during the +// flush is not lost +func TestARemovalDuringTheEventsIsSent(t *testing.T) { + w, zone, crate := zoneScene(1, actor.BodyTypeStatic) + log := listenTriggers(w) + w.Events.Subscribe(EventTriggerEnter, func(Event) { w.RemoveBody(crate) }) + enterStep := uint32(0) + simulate(w, settleTime, func() { + if enterStep == 0 && log.count(EventTriggerEnter, zone, crate) > 0 { + enterStep = w.step + } + }) + if enterStep == 0 { + t.Fatalf("the crate never entered the zone") + } + exits := 0 + for _, record := range log.events { + if record.eventType == EventTriggerExit && pairOf(record, zone, crate) { + exits++ + if record.step != enterStep { + t.Errorf("the exit was sent at the step %d, the crate was removed at the step %d", record.step, enterStep) + } + } + } + if exits != 1 { + t.Errorf("%d exits, want 1", exits) + } +} + +// zoneFull: crates dropped in a zone, which fall asleep in it, then half of them pushed out, the other half staying +func zoneFull(workers int) (*World, *triggerLog) { + w, _, _ := zoneScene(workers, actor.BodyTypeStatic) + w.parallelFrom = 1 + for x := -2; x <= 2; x++ { + for z := -2; z <= 2; z++ { + addBody(w, mgl64.Vec3{float64(x) * 0.7, crateDrop + 1, float64(z) * 0.7}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + } + } + log := listenTriggers(w) + simulate(w, settleTime, nil) + for _, body := range w.Bodies[2:] { + if body.Transform.Position.Z() > 0 { + body.WakeUp() + body.Velocity = mgl64.Vec3{0, 0, pushSpeed} + } + } + simulate(w, settleTime, nil) + return w, log +} + +// The events are the same, in the same order, with 1 and 8 workers +func TestTriggerEventsAreDeterministic(t *testing.T) { + w, one := zoneFull(1) + _, eight := zoneFull(8) + sleeping, inside := 0, 0 + for _, body := range w.Bodies[2:] { + if body.IsSleeping { + sleeping++ + } + if body.Transform.Position.Z() < zoneHalf { + inside++ + } + } + if sleeping == 0 || inside == 0 || inside == len(w.Bodies)-2 { + t.Fatalf("%d crates asleep, %d in the zone: the scene doesn't test the sleep and the exits", sleeping, inside) + } + exits := 0 + for _, record := range one.events { + if record.eventType == EventTriggerExit { + exits++ + } + } + if exits != len(w.Bodies)-2-inside { + t.Errorf("%d exits, %d crates out of the zone", exits, len(w.Bodies)-2-inside) + } + // the serials differ between both worlds: the events are compared by the index of their bodies + if len(one.events) != len(eight.events) { + t.Fatalf("%d events with 1 worker, %d with 8", len(one.events), len(eight.events)) + } + first, second := serialIndices(one), serialIndices(eight) + for i := range one.events { + a, b := one.events[i], eight.events[i] + if a.step != b.step || a.eventType != b.eventType || first[a.a] != second[b.a] || first[a.b] != second[b.b] { + t.Fatalf("event %d: %+v with 1 worker, %+v with 8", i, a, b) + } + } +} + +// serialIndices: the index of each body of the log's world, by serial +func serialIndices(log *triggerLog) map[uint64]int { + indices := map[uint64]int{} + for i, body := range log.w.Bodies { + indices[body.Serial()] = i + } + return indices +} + +// A step with crates asleep in a zone, listened to, allocates nothing +func TestSleepingTriggerPairsDoNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + for _, workers := range []int{1, 8} { + w, log := zoneFull(workers) + asleep := slices.ContainsFunc(w.Bodies[2:], func(body *actor.RigidBody) bool { return body.IsSleeping }) + if !asleep { + t.Fatalf("workers=%d: no crate asleep", workers) + } + log.counting = true + if allocs := testing.AllocsPerRun(10, func() { w.Step(sceneDt) }); allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", workers, allocs) + } + } +} + +// ========== KINEMATIC BODIES ========== +// A kinematic body enters a trigger and leaves it as a dynamic body does, the trigger static or kinematic, and a +// kinematic trigger detects the static and the kinematic bodies: Box2D v3 tests every sensor against its 3 trees +// (b2SensorTask: static, kinematic and dynamic), Jolt "These sensors will only detect collisions with active Dynamic or +// Kinematic bodies" for a static sensor (Body.h), and Unity "A dynamic or kinematic trigger collider collides with any +// collider type. A static trigger collider collides with any dynamic or Kinematic collider". A kinematic body still has +// no contact with them + +// crossSpeed: a kinematic cube crosses the zone at this speed (m/s) +const crossSpeed = 2.0 + +// kinematicZoneScene: a static trigger zone on the ground plane, and a kinematic cube 4 m left of its center +func kinematicZoneScene(workers int) (*World, *actor.RigidBody, *actor.RigidBody) { + w := newScene(workers) + addGround(w, 0.5) + zone := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{zoneHalf, 1, zoneHalf}}, actor.BodyTypeStatic, 0, 0) + zone.IsTrigger = true + mover := kinematicBox(w, mgl64.Vec3{-2 * zoneHalf, 0.5, 0}, cube().HalfExtents) + return w, zone, mover +} + +// A kinematic cube driven through a static zone enters it once when its face reaches the zone, stays in it while it +// moves, and leaves it once when its back face is past the zone; it has no contact with the zone nor the ground +func TestAKinematicBodyCrossesAStaticTrigger(t *testing.T) { + w, zone, mover := kinematicZoneScene(1) + log := listenTriggers(w).listenCollisions() + var enterX, exitX float64 + entered, left := false, false + drive(w, mover, mgl64.Vec3{crossSpeed, 0, 0}, 4*zoneHalf/crossSpeed, func() { + if !entered && log.count(EventTriggerEnter, zone, mover) > 0 { + entered, enterX = true, mover.Transform.Position.X() + } + if !left && log.count(EventTriggerExit, zone, mover) > 0 { + left, exitX = true, mover.Transform.Position.X() + } + }) + if enter := log.count(EventTriggerEnter, zone, mover); enter != 1 { + t.Fatalf("%d enters of the kinematic cube, want 1", enter) + } + if exit := log.count(EventTriggerExit, zone, mover); exit != 1 { + t.Fatalf("%d exits of the kinematic cube, want 1", exit) + } + if stays := log.count(EventTriggerStay, zone, mover); stays == 0 { + t.Errorf("no stay while the kinematic cube moved in the zone") + } + // the pair is tested at the start of a step, before the step moves the cube: within 2 steps of travel + travel := 2 * crossSpeed * sceneDt + if face := enterX + cubeHalf; face < -zoneHalf || face > -zoneHalf+travel { + t.Errorf("the enter was sent with the front face at x=%.3f, want just past %.2f", face, -zoneHalf) + } + if face := exitX - cubeHalf; face < zoneHalf || face > zoneHalf+travel { + t.Errorf("the exit was sent with the back face at x=%.3f, want just past %.2f", face, zoneHalf) + } + for _, record := range log.events { + if record.eventType == EventCollisionEnter || record.eventType == EventCollisionStay || record.eventType == EventCollisionExit { + t.Fatalf("collision event %v between bodies without mass", record.eventType) + } + } +} + +// A kinematic cube stopped in a static zone falls asleep in it and stays in it: no exit and no stay during 10 s; +// removed from the world, it leaves once +func TestAKinematicBodyAsleepInATriggerStaysInIt(t *testing.T) { + w, zone, mover := kinematicZoneScene(1) + log := listenTriggers(w) + drive(w, mover, mgl64.Vec3{crossSpeed, 0, 0}, 2*zoneHalf/crossSpeed, nil) + simulate(w, settleTime, nil) + if !mover.IsSleeping { + t.Fatalf("the kinematic cube is awake %v s after its last target", settleTime) + } + stays := log.count(EventTriggerStay, zone, mover) + simulate(w, stayTime, nil) + if enter := log.count(EventTriggerEnter, zone, mover); enter != 1 { + t.Errorf("%d enters, want 1", enter) + } + if exit := log.count(EventTriggerExit, zone, mover); exit != 0 { + t.Errorf("%d exits while the kinematic cube sleeps in the zone, want 0", exit) + } + if more := log.count(EventTriggerStay, zone, mover) - stays; more != 0 { + t.Errorf("%d stays while the kinematic cube sleeps in the zone, want 0", more) + } + w.RemoveBody(mover) + w.Step(sceneDt) + if exit := log.count(EventTriggerExit, zone, mover); exit != 1 { + t.Errorf("%d exits at the step after the removal of the kinematic cube, want 1", exit) + } +} + +// A kinematic trigger driven over a static box and a kinematic cube enters each of them once and leaves each once, +// without any contact +func TestAKinematicTriggerDetectsStaticAndKinematicBodies(t *testing.T) { + w := newScene(1) + addGround(w, 0.5) + block := addBody(w, mgl64.Vec3{-1, 0.5, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeStatic, 0.5, 0) + still := kinematicBox(w, mgl64.Vec3{1, 0.5, 0}, cube().HalfExtents) + zone := kinematicBox(w, mgl64.Vec3{-3, 0.5, 0}, mgl64.Vec3{0.5, 0.5, 0.5}) + zone.IsTrigger = true + log := listenTriggers(w).listenCollisions() + drive(w, zone, mgl64.Vec3{crossSpeed, 0, 0}, 6/crossSpeed, nil) + for _, body := range []*actor.RigidBody{block, still} { + if enter := log.count(EventTriggerEnter, zone, body); enter != 1 { + t.Errorf("%d enters of the kinematic zone with a %v body, want 1", enter, body.BodyType) + } + if exit := log.count(EventTriggerExit, zone, body); exit != 1 { + t.Errorf("%d exits of the kinematic zone with a %v body, want 1", exit, body.BodyType) + } + } + if contacts := len(w.Contacts()); contacts != 0 { + t.Errorf("%d contacts, want 0", contacts) + } + for _, record := range log.events { + if record.eventType == EventCollisionEnter || record.eventType == EventCollisionStay || record.eventType == EventCollisionExit { + t.Fatalf("collision event %v between bodies without mass", record.eventType) + } + } +} + +// kinematicCrossing: kinematic cubes driven across a static zone at several depths, half of them stopping in it; the +// events of the types logged +func kinematicCrossing(workers int, eventTypes ...EventType) (*World, *triggerLog, func()) { + w, _, _ := kinematicZoneScene(workers) + w.parallelFrom = 1 + var movers []*actor.RigidBody + for k := 0; k < 9; k++ { + movers = append(movers, kinematicBox(w, mgl64.Vec3{-2*zoneHalf - 0.3*float64(k), 0.5, float64(k)*0.4 - 1.6}, cube().HalfExtents)) + } + log := listenEvents(w, eventTypes...) + step := func() { + for k, mover := range movers { + if k%2 == 1 && mover.Transform.Position.X() > 0 { + continue + } + moveTo(mover, translated(mover.Transform, mgl64.Vec3{crossSpeed, 0, 0})) + } + w.Step(sceneDt) + } + return w, log, step +} + +// The events of kinematic bodies are the same, in the same order, with 1 and 8 workers +func TestKinematicTriggerEventsAreDeterministic(t *testing.T) { + _, one, step1 := kinematicCrossing(1, EventTriggerEnter, EventTriggerStay, EventTriggerExit) + _, eight, step8 := kinematicCrossing(8, EventTriggerEnter, EventTriggerStay, EventTriggerExit) + for i := 0; i < int(4*zoneHalf/crossSpeed/sceneDt)+60; i++ { + step1() + step8() + } + enters, exits := 0, 0 + for _, record := range one.events { + switch record.eventType { + case EventTriggerEnter: + enters++ + case EventTriggerExit: + exits++ + } + } + if enters != 9 || exits == 0 || exits == 9 { + t.Fatalf("%d enters, %d exits: the scene doesn't test the kinematic bodies in and out of the zone", enters, exits) + } + if len(one.events) != len(eight.events) { + t.Fatalf("%d events with 1 worker, %d with 8", len(one.events), len(eight.events)) + } + first, second := serialIndices(one), serialIndices(eight) + for i := range one.events { + a, b := one.events[i], eight.events[i] + if a.step != b.step || a.eventType != b.eventType || first[a.a] != second[b.a] || first[a.b] != second[b.b] { + t.Fatalf("event %d: %+v with 1 worker, %+v with 8", i, a, b) + } + } +} + +// A step with kinematic bodies moving in a zone, their enters and exits listened to, allocates nothing (an event +// allocates, as any value in an interface: the steps measured send none) +func TestKinematicTriggerPairsDoNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + for _, workers := range []int{1, 8} { + _, log, step := kinematicCrossing(workers, EventTriggerEnter, EventTriggerExit) + // 2.5 s: the 9 cubes are in the zone, the first leaves it at 3.25 s + for i := 0; i < int(2.5/sceneDt); i++ { + step() + } + if enters := len(log.events); enters != 9 { + t.Fatalf("workers=%d: %d enters, want the 9 cubes in the zone", workers, enters) + } + log.counting = true + if allocs := testing.AllocsPerRun(10, step); allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", workers, allocs) + } + if log.counted != 0 { + t.Fatalf("workers=%d: %d events during the steps measured", workers, log.counted) + } + } +} + +// A trigger mesh: a mesh has no core for GJK (its support point is null), so its overlap with a body is the one of its +// real contacts, as a plane or a heightfield. A sheet of 4 x 4 m at the height 1, its origin off the sheet: a crate +// falling through it and a kinematic cube driven down through it each enter it once and leave it once +func TestBodiesCrossATriggerMesh(t *testing.T) { + w := newScene(1) + addGround(w, 0.5) + sheet := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), gridMesh(4, 1, func(x, z float64) float64 { return 1 }), actor.BodyTypeStatic, 0, 0) + sheet.IsTrigger = true + crate := addBody(w, mgl64.Vec3{1.2, 2, 0.7}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + mover := kinematicBox(w, mgl64.Vec3{-1.2, 2, -0.7}, cube().HalfExtents) + log := listenTriggers(w).listenCollisions() + drive(w, mover, mgl64.Vec3{0, -crossSpeed, 0}, 1.5, nil) + simulate(w, settleTime, nil) + for _, body := range []*actor.RigidBody{crate, mover} { + if enter := log.count(EventTriggerEnter, sheet, body); enter != 1 { + t.Errorf("%d enters of body %d in the trigger mesh, want 1", enter, body.Serial()) + } + if exit := log.count(EventTriggerExit, sheet, body); exit != 1 { + t.Errorf("%d exits of body %d from the trigger mesh, want 1", exit, body.Serial()) + } + if collisions := log.count(EventCollisionEnter, sheet, body); collisions != 0 { + t.Errorf("%d collisions of body %d with the trigger mesh", collisions, body.Serial()) + } + } + if !crate.IsSleeping || crate.Transform.Position.Y() > 2*cubeHalf { + t.Errorf("the crate is at y=%.3f, asleep %v: want asleep on the ground, under the sheet", crate.Transform.Position.Y(), crate.IsSleeping) + } +} diff --git a/twist_test.go b/twist_test.go index 63e0301..2895a46 100644 --- a/twist_test.go +++ b/twist_test.go @@ -173,8 +173,8 @@ func TestSolveFrictionBoundsTheTwistByTheLeverArms(t *testing.T) { normal := mgl64.Vec3{0, 1, 0} bodyA, bodyB := &actor.RigidBody{}, &actor.RigidBody{} newStates := func(spinA, spinB float64) (*bodyState, *bodyState) { - return &bodyState{body: bodyA, angularVelocity: mgl64.Vec3{0.3, spinA, 0}, inverseInertia: mgl64.Diag3(mgl64.Vec3{2, 2, 2})}, - &bodyState{body: bodyB, angularVelocity: mgl64.Vec3{0, spinB, -0.7}, inverseInertia: mgl64.Diag3(mgl64.Vec3{4, 4, 4})} + return &bodyState{body: bodyA, dynamic: true, angularVelocity: mgl64.Vec3{0.3, spinA, 0}, inverseInertia: mgl64.Diag3(mgl64.Vec3{2, 2, 2})}, + &bodyState{body: bodyB, dynamic: true, angularVelocity: mgl64.Vec3{0, spinB, -0.7}, inverseInertia: mgl64.Diag3(mgl64.Vec3{4, 4, 4})} } // the tangent rows have no mass here: the twist alone is solved newConstraint := func(friction float64) *contactConstraint { diff --git a/world.go b/world.go index 5b81b2f..b2dbbad 100644 --- a/world.go +++ b/world.go @@ -8,6 +8,7 @@ import ( "github.com/akmonengine/feather/actor" "github.com/akmonengine/feather/constraint" + "github.com/akmonengine/feather/gjk" "github.com/go-gl/mathgl/mgl64" ) @@ -40,6 +41,9 @@ type World struct { contacts []constraint.Manifold previous []constraint.Manifold aabbs []actor.AABB + // fast: the body i can move more than half of its smallest extent during the step (isFast): its pairs get the + // speculative margin of its speed + fast []bool // step: the count of steps, the stamp of the contacts kept by the pairs of the broad phase step uint32 shift []int32 // buffer of RemoveBody @@ -56,6 +60,8 @@ type World struct { counts []int // heightfields changed during this step: their contacts are computed again changed []*actor.RigidBody + // characters of the world, by their inner body (character.go) + characters map[*actor.RigidBody]*CharacterVirtual // profile of the last step profile Profile @@ -133,6 +139,7 @@ func (w *World) RemoveBody(body *actor.RigidBody) { } w.Events.forget(body) + delete(w.characters, body) // the bodies touching the removed body wake up (with their islands): they may have to fall w.islands.remove(body) w.wakeNeighbors(body) @@ -155,7 +162,12 @@ func (w *World) RemoveBody(body *actor.RigidBody) { // wakeNeighbors: the sleeping bodies which collide with the body wake up, with their islands. The sleeping bodies have // no contact anymore: their AABB is used func (w *World) wakeNeighbors(body *actor.RigidBody) { - aabb := body.AABB() + w.wakeNeighborsIn(body, body.AABB()) +} + +// wakeNeighborsIn: the sleeping bodies which collide with the body and whose AABB touches aabb, within the speculative +// distance, wake up with their islands +func (w *World) wakeNeighborsIn(body *actor.RigidBody, aabb actor.AABB) { margin := mgl64.Vec3{SpeculativeDistance, SpeculativeDistance, SpeculativeDistance} aabb = actor.AABB{Min: aabb.Min.Sub(margin), Max: aabb.Max.Add(margin)} for _, other := range w.Bodies { @@ -231,6 +243,7 @@ func (w *World) Step(dt float64) { start := time.Now() w.profile = Profile{} + w.moveKinematics(dt) w.wakeTouchedBodies() pool := w.workerPool() parallelFrom := w.parallelFrom @@ -246,6 +259,7 @@ func (w *World) Step(dt float64) { w.previous = w.contacts w.buffer = 1 - w.buffer w.step++ + w.tree.step = w.step manifolds := w.detectCollision(dt, pool) if w.wakeTouched(manifolds) { // the woken bodies get their contacts in this step (as in Jolt) @@ -260,7 +274,8 @@ func (w *World) Step(dt float64) { s.joints = w.activeJoints() s.prepare(w.Bodies, manifolds, dt, substeps, contactHertz, pool) mark = w.lap(&w.profile.Prepare, mark) - for range substeps { + for k := range substeps { + s.substep = k + 1 s.integrateVelocities(w.Gravity) s.warmStart() s.push() @@ -273,7 +288,7 @@ func (w *World) Step(dt float64) { s.finalize() pool.end() mark = w.lap(&w.profile.Restitution, mark) - w.continuous(s, dt) + w.continuous(s, manifolds, dt) w.contacts = manifolds w.recordContacts() @@ -345,8 +360,9 @@ func (w *World) detectCollision(dt float64, pool *workerPool) []constraint.Manif func (w *World) syncTrees(pool *workerPool) { if cap(w.aabbs) < len(w.Bodies) { w.aabbs = make([]actor.AABB, len(w.Bodies)) + w.fast = make([]bool, len(w.Bodies)) } - w.aabbs = w.aabbs[:len(w.Bodies)] + w.aabbs, w.fast = w.aabbs[:len(w.Bodies)], w.fast[:len(w.Bodies)] if w.aabbJob == nil { w.aabbJob = w.computeAABB } @@ -368,16 +384,16 @@ func (w *World) syncMoved() { } } -// wakeTouched: a sleeping body touched by an awake dynamic body wakes up with its island (as in Box2D & Jolt). -// Returns true if a body woke up: its contacts must be found in this step +// wakeTouched: a sleeping body touched by an awake dynamic or kinematic body wakes up with its island (as in Box2D & +// Jolt). Returns true if a body woke up: its contacts must be found in this step func (w *World) wakeTouched(manifolds []constraint.Manifold) bool { woke := false for i := range manifolds { a, b := manifolds[i].BodyA, manifolds[i].BodyB - if a.IsSleeping && isAwakeDynamic(b) { + if a.IsSleeping && isAwakeMover(b) { w.islands.wake(a) woke = true - } else if b.IsSleeping && isAwakeDynamic(a) { + } else if b.IsSleeping && isAwakeMover(a) { w.islands.wake(b) woke = true } @@ -397,9 +413,10 @@ func (w *World) activeJoints() []Joint { return w.solverJoints } -// computeAABB of the body i, enlarged by the distance it can travel during the step +// computeAABB of the body i, enlarged by the distance it can travel during the step; and whether the body is fast func (w *World) computeAABB(i int) { body := w.Bodies[i] + w.fast[i] = isFast(body, w.dt) aabb := body.AABB() if _, isPlane := body.Shape.(*actor.Plane); !isPlane { margin := reach(body, aabb, w.dt) @@ -408,44 +425,88 @@ func (w *World) computeAABB(i int) { w.aabbs[i] = aabb } -// collide the pair i. The triggers only need the real overlaps, the other pairs get speculative contacts +// collide the pair i. A pair with a trigger only needs to know if the shapes overlap, the other pairs get speculative +// contacts func (w *World) collide(i int) { pair := w.pairs[i] out := w.manifolds[w.offsets[i]:w.offsets[i+1]] - margin := 0.0 - if !pair.BodyA.IsTrigger && !pair.BodyB.IsTrigger { - // against a static body, the contact exists before the body touches it, from its speed (the speculative CCD of - // PhysX): it doesn't sink into the ground. Between 2 dynamic bodies, only within SpeculativeDistance (as in - // Box2D v3): a fast impact is then absorbed by the spring of the contact over a few substeps, a rigid stop in one - // substep would throw the lighter body and turn both - margin = SpeculativeDistance - if pair.BodyA.BodyType == actor.BodyTypeStatic || pair.BodyB.BodyType == actor.BodyTypeStatic { - margin += relativeSpeed(pair.BodyA, pair.BodyB) * w.dt - } - - // pair cache: the contacts of the previous step, if the bodies barely moved relative to each other - previous := w.previousContacts(pair) - if len(previous) > 0 && !w.isChanged(pair) { - count := 0 - for k := range previous { - if count < len(out) && reuseManifold(&previous[k], margin, &out[count]) { - count++ - } - } - if count > 0 { - w.counts[i] = count - indexManifolds(out[:count], pair) - warmStartPair(out[:count], previous) - return + if pair.BodyA.IsTrigger || pair.BodyB.IsTrigger { + w.counts[i] = w.overlapTrigger(pair, out) + return + } + // against a static or a kinematic body, the contact exists before the body touches it, from their relative speed + // (the speculative CCD of PhysX, legal on its kinematic actors): it doesn't sink into the ground, a leg at 5 m/s + // doesn't enter the tail it meets. The pairs of a fast dynamic body (isFast) get the same margin: 2 fast spheres + // don't go through each other. Between 2 dynamic bodies which are not fast, only within SpeculativeDistance (as in + // Box2D v3): an impact is then absorbed by the spring of the contact over a few substeps, a rigid stop in one + // substep would throw the lighter body and turn both + margin := SpeculativeDistance + if pair.BodyA.BodyType != actor.BodyTypeDynamic || pair.BodyB.BodyType != actor.BodyTypeDynamic || + w.fast[pair.IndexA] || w.fast[pair.IndexB] { + margin += relativeSpeed(pair.BodyA, pair.BodyB) * w.dt + } + + // pair cache: the contacts of the previous step, if the bodies barely moved relative to each other + previous := w.previousContacts(pair) + if len(previous) > 0 && !w.isChanged(pair) { + count := 0 + for k := range previous { + if count < len(out) && reuseManifold(&previous[k], margin, &out[count]) { + count++ } } - w.counts[i] = collidePair(pair, margin, out) - indexManifolds(out[:w.counts[i]], pair) - warmStartPair(out[:w.counts[i]], previous) - return + if count > 0 { + w.counts[i] = count + indexManifolds(out[:count], pair) + warmStartPair(out[:count], previous) + return + } } w.counts[i] = collidePair(pair, margin, out) indexManifolds(out[:w.counts[i]], pair) + warmStartPair(out[:w.counts[i]], previous) +} + +// overlapTrigger: a pair with a trigger has no contact, only an overlap: one manifold without any point if the shapes +// overlap, none otherwise. The overlap is the one of World.Overlap, the distance between the cores of both shapes by +// GJK, not over the sum of their radii: a body in contact with a trigger is in it. So are the sensors of Box2D v3 +// (b2SensorQueryCallback: b2ShapeDistance with the radii, an overlap under 10 * FLT_EPSILON), without contact points. +// A plane, a heightfield or a triangle mesh keeps its real contacts: it has no core for GJK (the support point of a +// mesh is null). A pair at rest keeps its overlap (overlapKept) +func (w *World) overlapTrigger(pair Pair, out []constraint.Manifold) int { + var overlaps bool + switch { + case w.overlapKept(pair): + overlaps = w.tree.fat[pair.slot].overlap + case isLarge(pair.BodyA) || isLarge(pair.BodyB) || isSurface(pair.BodyA) || isSurface(pair.BodyB): + return collidePair(pair, 0, out) + default: + coreA, radiusA := gjk.NewCoreProxy(pair.BodyA) + coreB, radiusB := gjk.NewCoreProxy(pair.BodyB) + overlaps = coresOverlap(&coreA, radiusA, &coreB, radiusB) + } + if !overlaps { + return 0 + } + out[0].Reset(pair.BodyA, pair.BodyB) + indexManifolds(out[:1], pair) + return 1 +} + +// overlapKept: both bodies of a trigger pair rest (static or asleep), the pair was tested or kept at the previous step +// with a trigger (a body made a trigger by the game is tested), and the AABBs of both bodies are the same since the step it was tested at: the overlap can't have changed, it is +// kept without any test. As the trigger pairs of PhysX, not tested while both actors sleep ("the overlap state can not +// change if both objects are sleeping") and tested again when the pose of an actor is set (ScTriggerInteraction.cpp, +// onActivate, PROCESS_THIS_FRAME): here a body moved by the game, whose AABB changed (UpdateAABB), as the broad phase +// sees a static body moved by the game +func (w *World) overlapKept(pair Pair) bool { + if isAwakeMover(pair.BodyA) || isAwakeMover(pair.BodyB) { + return false + } + record := &w.tree.fat[pair.slot] + proxies := w.tree.proxies + return record.trigger && record.stamp == w.step-1 && + proxies[pair.IndexA].changed < record.stamp && proxies[pair.IndexB].changed < record.stamp } // previousContacts of the pair: the manifolds it had in the previous step (none if it had none, or if the bodies @@ -458,17 +519,19 @@ func (w *World) previousContacts(pair Pair) []constraint.Manifold { return w.previous[record.first : record.first+record.count] } -// recordContacts: each pair keeps where its contacts of this step are, for the next step. The contacts with a trigger -// are not kept (they are not in the contacts) +// recordContacts: each pair keeps where its contacts of this step are, for the next step. A pair with a trigger has no +// contact (it is not in the contacts): it keeps whether its shapes overlap (overlapKept) func (w *World) recordContacts() { first := int32(0) for i := range w.pairs { pair := &w.pairs[i] count := int32(w.counts[i]) - if pair.BodyA.IsTrigger || pair.BodyB.IsTrigger { + record := &w.tree.fat[pair.slot] + record.trigger = pair.BodyA.IsTrigger || pair.BodyB.IsTrigger + record.overlap = record.trigger && count > 0 + if record.trigger { count = 0 } - record := &w.tree.fat[pair.slot] record.first, record.count, record.stamp = first, count, w.step first += count } @@ -491,7 +554,8 @@ func (w *World) isChanged(pair Pair) bool { return false } -// reach is the distance a body can travel during dt, plus the speculative distance +// reach is the distance a body can travel during dt, plus the speculative distance. A kinematic body travels at the +// velocity of the motion to its target func reach(body *actor.RigidBody, aabb actor.AABB, dt float64) float64 { if body.BodyType == actor.BodyTypeStatic || body.IsSleeping { return SpeculativeDistance @@ -558,15 +622,15 @@ func warmStartPair(manifolds, previous []constraint.Manifold) { } } -// wakeTouchedBodies: a sleeping body touched by a moving body wakes up with its island, -// otherwise it would be pushed without moving +// wakeTouchedBodies: a sleeping body touched by a moving body (dynamic, or a kinematic body on its way to a target) +// wakes up with its island, otherwise it would be pushed without moving func (w *World) wakeTouchedBodies() { w.islands.wakeWoken() for _, joint := range w.Joints { base := joint.base() - if base.BodyA.IsSleeping && isAwakeDynamic(base.BodyB) { + if base.BodyA.IsSleeping && (isAwakeDynamic(base.BodyB) || isMoving(base.BodyB)) { w.islands.wake(base.BodyA) - } else if base.BodyB.IsSleeping && isAwakeDynamic(base.BodyA) { + } else if base.BodyB.IsSleeping && (isAwakeDynamic(base.BodyA) || isMoving(base.BodyA)) { w.islands.wake(base.BodyB) } } @@ -580,11 +644,6 @@ func (w *World) wakeTouchedBodies() { } } -func isMoving(body *actor.RigidBody) bool { - return isAwakeDynamic(body) && - (body.Velocity.Len() >= actor.DefaultSleepSpeed || body.AngularVelocity.Len() >= actor.DefaultSleepSpeed) -} - // parallelFor calls fn(i) for each i in [0, n), split between the workers. // Each i writes only its own result, so the order of execution does not matter. func parallelFor(n, workers int, fn func(i int)) {