diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 54d0baf..d9f5fdf 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -15,10 +15,10 @@ * @defgroup world World * These functions allow you to create a simulation world. * - * You can add rigid bodies and joint constraints to the world and run the simulation. You can get contact - * information to get contact points and normals as well as events. You can query the world, checking for overlaps and casting - * rays or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find - * documentation here: https://box2d.org/ + * You can add rigid bodies and joint constraints to the world and run the simulation. You can get + * contact information to get contact points and normals as well as events. You can query the world, + * checking for overlaps and casting rays or shapes. There is also debugging information such as + * debug draw, timing information, and counters. You can find documentation here: https://box2d.org/ * @{ */ @@ -29,8 +29,9 @@ #define b3CreateWorld b3CreateWorldDoublePrecision #endif -/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You may create -/// up to 128 worlds. Each world is completely independent and may be simulated in parallel. +/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You +/// may create up to 128 worlds. Each world is completely independent and may be simulated in +/// parallel. /// @return the world id. B3_API b3WorldId b3CreateWorld( const b3WorldDef* def ); @@ -46,41 +47,47 @@ B3_API int b3GetMaxWorldCount( void ); /// World id validation. Provides validation for up to 64K allocations. B3_API bool b3World_IsValid( b3WorldId id ); -/// Simulate a world for one time step. This performs collision detection, integration, and constraint solution. -/// A time step of zero runs the broad-phase and collision, but skips the solver. So you can still get contact -/// and sensor events. +/// Simulate a world for one time step. This performs collision detection, integration, and +/// constraint solution. A time step of zero runs the broad-phase and collision, but skips the +/// solver. So you can still get contact and sensor events. /// @param worldId The world to simulate /// @param timeStep The amount of time to simulate, this should be a fixed number. Usually 1/60. -/// @param subStepCount The number of sub-steps, increasing the sub-step count can increase accuracy. Usually 4. +/// @param subStepCount The number of sub-steps, increasing the sub-step count can increase +/// accuracy. Usually 4. B3_API void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ); /// Call this to draw shapes and other debug draw data B3_API void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ); -/// Get the world's bounds. This is the bounding box that covers the current simulation. May have a small -/// amount of padding. +/// Get the world's bounds. This is the bounding box that covers the current simulation. May have a +/// small amount of padding. B3_API b3AABB b3World_GetBounds( b3WorldId worldId ); -/// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. +/// Get the body events for the current time step. The event data is transient. Do not store a +/// reference to this data. B3_API b3BodyEvents b3World_GetBodyEvents( b3WorldId worldId ); -/// Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. +/// Get sensor events for the current time step. The event data is transient. Do not store a +/// reference to this data. B3_API b3SensorEvents b3World_GetSensorEvents( b3WorldId worldId ); -/// Get contact events for this current time step. The event data is transient. Do not store a reference to this data. +/// Get contact events for this current time step. The event data is transient. Do not store a +/// reference to this data. B3_API b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ); -/// Get the joint events for the current time step. The event data is transient. Do not store a reference to this data. +/// Get the joint events for the current time step. The event data is transient. Do not store a +/// reference to this data. B3_API b3JointEvents b3World_GetJointEvents( b3WorldId worldId ); /// Overlap test for all shapes that *potentially* overlap the provided AABB -B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, - void* context ); +B3_API b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, + b3OverlapResultFcn* fcn, void* context ); /// Overlap test for all shapes that overlap the provided shape proxy. The proxy points are relative /// to the world origin, which lets the query stay precise far from the world origin. -B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, - b3OverlapResultFcn* fcn, void* context ); +B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3QueryFilter filter, b3OverlapResultFcn* fcn, + void* context ); /// Cast a ray into the world to collect shapes in the path of the ray. /// Your callback function controls whether you get the closest point, any point, or n-points. @@ -93,24 +100,28 @@ B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const /// @param fcn A user implemented callback function /// @param context A user context that is passed along to the callback function /// @return traversal performance counters -B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, - b3CastResultFcn* fcn, void* context ); +B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); -/// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap. -/// This is less general than b3World_CastRay() and does not allow for custom filtering. Ignores back-side collision -/// on meshes and height-fields. -B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter ); +/// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores +/// initial overlap. This is less general than b3World_CastRay() and does not allow for custom +/// filtering. Ignores back-side collision on meshes and height-fields. +B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, + b3QueryFilter filter ); -/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. -/// The proxy points are relative to the origin and the hit points come back as world positions, so the -/// cast stays precise far from the world origin. Ignores back-side collision on meshes and height-fields. +/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a +/// point. The proxy points are relative to the origin and the hit points come back as world +/// positions, so the cast stays precise far from the world origin. Ignores back-side collision on +/// meshes and height-fields. /// @see b3World_CastRay -B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, - b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); +B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, + b3Vec3 translation, b3QueryFilter filter, + b3CastResultFcn* fcn, void* context ); -/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing -/// clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by -/// b3World_CollideMover. Ignores back-side collision on meshes and height-fields. +/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along +/// other shapes while reducing clipping. This is not a good source of information about what the +/// mover is touching. Instead use the planes returned by b3World_CollideMover. Ignores back-side +/// collision on meshes and height-fields. /// @param worldId World to cast the mover against /// @param origin World position the mover capsule is relative to /// @param mover Capsule mover, relative to the origin @@ -119,14 +130,15 @@ B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3S /// @param fcn Optional callback for custom shape filtering /// @param context A user context that is passed along to the callback function /// @return the translation fraction -B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter, - b3MoverFilterFcn* fcn, void* context ); +B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, + b3Vec3 translation, b3QueryFilter filter, b3MoverFilterFcn* fcn, + void* context ); -/// Collide a capsule mover with the world, gathering collision planes that can be fed to b3SolvePlanes. Useful for -/// kinematic character movement. The mover and the returned planes are relative to the origin. -/// Ignores back-side collision on meshes and height-fields. -B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, - b3PlaneResultFcn* fcn, void* context ); +/// Collide a capsule mover with the world, gathering collision planes that can be fed to +/// b3SolvePlanes. Useful for kinematic character movement. The mover and the returned planes are +/// relative to the origin. Ignores back-side collision on meshes and height-fields. +B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, + b3QueryFilter filter, b3PlaneResultFcn* fcn, void* context ); /// Enable/disable sleep. If your application does not need sleeping, you can gain some performance /// by disabling sleep completely at the world level. @@ -136,9 +148,9 @@ B3_API void b3World_EnableSleeping( b3WorldId worldId, bool flag ); /// Is body sleeping enabled? B3_API bool b3World_IsSleepingEnabled( b3WorldId worldId ); -/// Enable/disable continuous collision between dynamic and static bodies. Generally you should keep continuous -/// collision enabled to prevent fast moving objects from going through static objects. The performance gain from -/// disabling continuous collision is minor. +/// Enable/disable continuous collision between dynamic and static bodies. Generally you should keep +/// continuous collision enabled to prevent fast moving objects from going through static objects. +/// The performance gain from disabling continuous collision is minor. /// @see b3WorldDef B3_API void b3World_EnableContinuous( b3WorldId worldId, bool flag ); @@ -166,8 +178,8 @@ B3_API void b3World_EnableRestitutionPropagation( b3WorldId worldId, bool flag ) /// Is restitution propagation enabled? B3_API bool b3World_IsRestitutionPropagationEnabled( b3WorldId worldId ); -/// Adjust the hit event threshold. This controls the collision speed needed to generate a b3ContactHitEvent. -/// Usually in meters per second. +/// Adjust the hit event threshold. This controls the collision speed needed to generate a +/// b3ContactHitEvent. Usually in meters per second. /// @see b3WorldDef::hitEventThreshold B3_API void b3World_SetHitEventThreshold( b3WorldId worldId, float value ); @@ -175,7 +187,8 @@ B3_API void b3World_SetHitEventThreshold( b3WorldId worldId, float value ); B3_API float b3World_GetHitEventThreshold( b3WorldId worldId ); /// Register the custom filter callback. This is optional. -B3_API void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ); +B3_API void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, + void* context ); /// Register the pre-solve callback. This is optional. B3_API void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, void* context ); @@ -199,7 +212,8 @@ B3_API void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionD /// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) /// @param contactSpeed The maximum contact constraint push out speed (meters per second) /// @note Advanced feature -B3_API void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ); +B3_API void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, + float contactSpeed ); /// Set the contact point recycling distance. Setting this to zero disables contact point recycling. /// Usually in meters. @@ -396,8 +410,8 @@ B3_API int b3RecPlayer_GetDivergeFrame( const b3RecPlayer* player ); /// Set the worker count of the replay world. Clamped to [1, B3_MAX_WORKERS]. Applied to the live /// world at once and reused whenever the player rebuilds its world on Restart or a backward seek. -/// Replaying at a different count than recorded re-partitions the constraint graph, so the StateHash -/// check becomes a cross-thread determinism test. +/// Replaying at a different count than recorded re-partitions the constraint graph, so the +/// StateHash check becomes a cross-thread determinism test. B3_API void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count ); /// Tune the keyframe ring used to speed up backward seeking. A keyframe is a periodic snapshot the @@ -407,7 +421,8 @@ B3_API void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count ); /// @param minIntervalFrames finest spacing between keyframes, in frames /// A zero budget or a non-positive interval keeps that value. Clears the existing ring, so call /// b3RecPlayer_Restart afterward to repopulate it under the new policy. -B3_API void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames ); +B3_API void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, + int minIntervalFrames ); /// @return the keyframe memory budget in bytes B3_API size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player ); @@ -438,16 +453,20 @@ B3_API b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index ); /// @param createDebugShape called when a replayed shape is added; returns a user draw handle /// @param destroyDebugShape called when a replayed shape is removed; may be NULL /// @param context user context passed to both callbacks -B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape, - b3DestroyDebugShapeCallback* destroyDebugShape, void* context ); +B3_API void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, + b3CreateDebugShapeCallback* createDebugShape, + b3DestroyDebugShapeCallback* destroyDebugShape, + void* context ); /// Draw the spatial queries recorded during the most recently replayed frame, layered on top of the /// world. Call after b3World_Draw. NULL draw function pointers are skipped. /// @param player a valid player handle /// @param draw debug draw callbacks /// @param queryIndex index of the frame query to draw, or -1 to draw all of them -/// @param selectedIndex index of the query to emphasize (reserved color plus a label), or -1 for none -B3_API void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex ); +/// @param selectedIndex index of the query to emphasize (reserved color plus a label), or -1 for +/// none +B3_API void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, + int selectedIndex ); /// The kind of a recorded spatial query, matching the public query and cast functions. typedef enum b3RecQueryType @@ -491,7 +510,8 @@ B3_API int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player ); B3_API b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index ); /// Get one result of a recorded query from the most recently replayed frame. -B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex ); +B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, + int hitIndex ); /**@}*/ // recording @@ -503,8 +523,8 @@ B3_API b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, in * @{ */ -/// Create a rigid body given a definition. No reference to the definition is retained. So you can create the definition -/// on the stack and pass it as a pointer. +/// Create a rigid body given a definition. No reference to the definition is retained. So you can +/// create the definition on the stack and pass it as a pointer. /// @code{.c} /// b3BodyDef bodyDef = b3DefaultBodyDef(); /// b3BodyId myBodyId = b3CreateBody(myWorldId, &bodyDef); @@ -581,7 +601,8 @@ B3_API void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ) /// The result will be close but maybe not exact. This is meant for kinematic bodies. /// The target is not applied if the velocity would be below the sleep threshold. /// This will optionally wake the body if asleep, but only if the movement is significant. -B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake ); +B3_API void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, + bool wake ); /// Get the linear velocity of a local point attached to a body. Usually in meters per second. B3_API b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ); @@ -698,8 +719,8 @@ B3_API float b3Body_GetGravityScale( b3BodyId bodyId ); B3_API bool b3Body_IsAwake( b3BodyId bodyId ); /// Wake a body from sleep. This wakes the entire island the body is touching. -/// @warning Putting a body to sleep will put the entire island of bodies touching this body to sleep, -/// which can be expensive and possibly unintuitive. +/// @warning Putting a body to sleep will put the entire island of bodies touching this body to +/// sleep, which can be expensive and possibly unintuitive. B3_API void b3Body_SetAwake( b3BodyId bodyId, bool awake ); /// Enable or disable sleeping for this body. If sleeping is disabled the body will wake. @@ -714,7 +735,8 @@ B3_API void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ); /// Get the sleep threshold, usually in meters per second. B3_API float b3Body_GetSleepThreshold( b3BodyId bodyId ); -/// Set the continuous collision safety factor. Smaller is safer but can lead to hitching. Recommended range [0.01, 0.5]. Non-dimensional. +/// Set the continuous collision safety factor. Smaller is safer but can lead to hitching. +/// Recommended range [0.01, 0.5]. Non-dimensional. B3_API void b3Body_SetSafetyFactor( b3BodyId bodyId, float safetyFactor ); /// Get the continuous collision safety factor. Non-dimensional. @@ -750,10 +772,10 @@ B3_API void b3Body_AllowFastRotation( b3BodyId bodyId, bool flag ); /// Is this body allowed to rotate fast? B3_API bool b3Body_IsFastRotationAllowed( b3BodyId bodyId ); -/// Enable or disable contact recycling for this body. Contact recycling is a performance optimization -/// that reuses contact manifolds when bodies move slightly. Disabling it can avoid ghost collisions -/// on characters at the cost of higher per-step work. Existing contacts retain their prior setting; -/// only contacts created after this call see the new value. +/// Enable or disable contact recycling for this body. Contact recycling is a performance +/// optimization that reuses contact manifolds when bodies move slightly. Disabling it can avoid +/// ghost collisions on characters at the cost of higher per-step work. Existing contacts retain +/// their prior setting; only contacts created after this call see the new value. /// @see b3BodyDef::enableContactRecycling B3_API void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag ); @@ -787,8 +809,9 @@ B3_API int b3Body_GetContactCapacity( b3BodyId bodyId ); /// Get the touching contact data for a body B3_API int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capacity ); -/// Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. -/// If there are no shapes attached then the returned AABB is empty and centered on the body origin. +/// Get the current world AABB that contains all the attached shapes. Note that this may not +/// encompass the body origin. If there are no shapes attached then the returned AABB is empty and +/// centered on the body origin. B3_API b3AABB b3Body_ComputeAABB( b3BodyId bodyId ); /// The minimum distance from any shape to the shape centroid. @@ -805,29 +828,34 @@ B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 tar /// Cast a ray at a specific body using a specified body transform. /// Ignores back-side collision on meshes and height-fields. -B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, - float maxFraction, b3WorldTransform bodyTransform ); +B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, + b3QueryFilter filter, float maxFraction, + b3WorldTransform bodyTransform ); /// Cast a shape at a specific body using a specified body transform. /// Ignores back-side collision on meshes and height-fields. -B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, - b3QueryFilter filter, float maxFraction, bool canEncroach, +B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, + b3Vec3 translation, b3QueryFilter filter, + float maxFraction, bool canEncroach, b3WorldTransform bodyTransform ); /// Overlap a shape with a specific body using a specified body transform. -B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, - b3WorldTransform bodyTransform ); +B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, + b3QueryFilter filter, b3WorldTransform bodyTransform ); /// Collide a character mover with a specific body using a specified body transform. /// Only considers convex shapes on the body. -B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, - const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform ); +B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, + b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, + b3WorldTransform bodyTransform ); -/// Perform a time of impact between a character mover and a body using specified sweep transforms. +/// Perform a time of impact between a character mover and a body using specified sweep transforms. /// Initial overlap of any shape on the body is ignored. A non-overlapped shape can still be hit. /// Only considers convex shapes on the body. -B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation, - b3QueryFilter filter, b3WorldTransform bodyTransform1, +B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, + const b3Capsule* mover, b3Vec3 moverTranslation, + b3QueryFilter filter, + b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 ); /** @} */ // body @@ -835,56 +863,68 @@ B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, /** * @defgroup shape Shape * Functions to create, destroy, and access. - * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. - * You may add multiple shapes to a single body. There are no hard limits on shape count per body. + * Shapes bind raw geometry to bodies and hold material properties including friction and + * restitution. You may add multiple shapes to a single body. There are no hard limits on shape + * count per body. * - * When you create a shape on a body the center of mass moves. This can lead to the body linear velocity - * changing if the angular velocity is non-zero. + * When you create a shape on a body the center of mass moves. This can lead to the body linear + * velocity changing if the angular velocity is non-zero. * @{ */ -/// Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Create a circle shape and attach it to a body. The shape definition and geometry are fully +/// cloned. Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, + const b3Sphere* sphere ); + +/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully +/// cloned. Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B3_API b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, + const b3Capsule* capsule ); + +/// Create a convex hull shape and attach it to a body. The shape definition is fully cloned. /// Contacts are not created until the next time step. /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ); +B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, + const b3HullData* hull ); -/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. -/// Contacts are not created until the next time step. +/// Create a convex hull shape and attach it to a body. The hull is cloned then transformed with +/// scale applied first. Use this for non-uniform or mirrored scale or a baked local transform. The +/// baked result is shared through the world hull database. The shape definition and geometry are +/// fully cloned. Contacts are not created until the next time step. /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ); +B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, + const b3HullData* hull, b3Transform transform, + b3Vec3 scale ); -/// Create a convex hull shape and attach it to a body. The shape definition is fully cloned. Contacts are not created -/// until the next time step. +/// Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the +/// mesh is not. Contacts are not created until the next time step. Mesh collision only creates +/// contacts on static bodies. +/// @warning this holds reference to the input mesh data which must remain valid for the lifetime of +/// this shape /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull ); +B3_API b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, + b3Vec3 scale ); -/// Create a convex hull shape and attach it to a body. The hull is cloned then transformed with scale applied first. -/// Use this for non-uniform or mirrored scale or a baked local transform. The baked result is shared through the -/// world hull database. The shape definition and geometry are fully cloned. Contacts are not created until the next time step. +/// Create a height-field shape and attach it to a body. The shape definition is fully cloned but +/// the height field is not. Contacts are not created until the next time step. Height field is only +/// allowed on static bodies. +/// @warning this holds reference to the input height field which must remain valid for the lifetime +/// of this shape /// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, - b3Transform transform, b3Vec3 scale ); - -/// Create a mesh hull shape and attach it to a body. The shape definition is fully cloned but the mesh is not. -/// Contacts are not created until the next time step. -/// Mesh collision only creates contacts on static bodies. -/// @warning this holds reference to the input mesh data which must remain valid for the lifetime of this shape -/// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ); - -/// Create a height-field shape and attach it to a body. The shape definition is fully cloned but the height field is not. -/// Contacts are not created until the next time step. -/// Height field is only allowed on static bodies. -/// @warning this holds reference to the input height field which must remain valid for the lifetime of this shape -/// @return the shape id for accessing the shape -B3_API b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightFieldData* heightField ); +B3_API b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, + const b3HeightFieldData* heightField ); /// Baked compound shapes are only allowed on static bodies. /// Note: runtime compounds are achieved by adding multiple shapes to a body. /// Runtime compounds can be dynamic and/or kinematic. -B3_API b3ShapeId b3CreateBakedCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* compound ); +B3_API b3ShapeId b3CreateBakedCompoundShape( b3BodyId bodyId, b3ShapeDef* def, + const b3CompoundData* compound ); -/// Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a +/// Destroy a shape. You may defer the body mass update which can improve performance if several +/// shapes on a /// body are destroyed at once. /// @see b3Body_ApplyMassFromShapes B3_API void b3DestroyShape( b3ShapeId shapeId, bool updateBodyMass ); @@ -947,7 +987,8 @@ B3_API b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ); B3_API int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ); /// Set a surface material for a mesh shape. -B3_API void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ); +B3_API void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, + int index ); /// Get a surface material for a mesh shape B3_API b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ); @@ -959,25 +1000,28 @@ B3_API b3Filter b3Shape_GetFilter( b3ShapeId shapeId ); /// @see b3ShapeDef::filter /// @param shapeId the shape /// @param filter the new filter -/// @param invokeContacts if true then the shape will have all contacts recomputed the next time step (expensive) +/// @param invokeContacts if true then the shape will have all contacts recomputed the next time +/// step (expensive) B3_API void b3Shape_SetFilter( b3ShapeId shapeId, b3Filter filter, bool invokeContacts ); -/// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for +/// sensors. /// @see b3ShapeDef::isSensor B3_API void b3Shape_EnableSensorEvents( b3ShapeId shapeId, bool flag ); /// Returns true if sensor events are enabled B3_API bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId ); -/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for +/// sensors. /// @see b3ShapeDef::enableContactEvents B3_API void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag ); /// Returns true if contact events are enabled B3_API bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId ); -/// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive -/// and must be carefully handled due to multithreading. Ignored for sensors. +/// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are +/// expensive and must be carefully handled due to multithreading. Ignored for sensors. /// @see b3PreSolveFcn B3_API void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag ); @@ -1036,7 +1080,8 @@ B3_API void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Ve /// Get the maximum capacity required for retrieving all the touching contacts on a shape B3_API int b3Shape_GetContactCapacity( b3ShapeId shapeId ); -/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. +/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or +/// shapeIdB on the contact data. /// @note Box3D uses speculative collision so some contact points may be separated. /// @returns the number of elements filled in the provided array /// @warning do not ignore the return value, it specifies the valid number of elements @@ -1073,15 +1118,18 @@ B3_API b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ); /// @param wind the wind velocity in world space /// @param drag the drag coefficient, the force that opposes the relative velocity /// @param lift the lift coefficient, the force that is perpendicular to the relative velocity -/// @param maxSpeed the maximum relative speed. Speed cap is necessary for stability. Typically 10m/s or less. +/// @param maxSpeed the maximum relative speed. Speed cap is necessary for stability. Typically +/// 10m/s or less. /// @param wake should this wake the body -B3_API void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ); +B3_API void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, + float maxSpeed, bool wake ); /** @} */ // shape /** * @defgroup joint Joint - * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative motions. + * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative + * motions. * @{ */ @@ -1139,10 +1187,12 @@ B3_API b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ); /// Get the current constraint torque for this joint B3_API b3Vec3 b3Joint_GetConstraintTorque( b3JointId jointId ); -/// Get the current linear separation error for this joint. Does not consider admissible movement. Usually in meters. +/// Get the current linear separation error for this joint. Does not consider admissible movement. +/// Usually in meters. B3_API float b3Joint_GetLinearSeparation( b3JointId jointId ); -/// Get the current angular separation error for this joint. Does not consider admissible movement. Usually in radians. +/// Get the current angular separation error for this joint. Does not consider admissible movement. +/// Usually in radians. B3_API float b3Joint_GetAngularSeparation( b3JointId jointId ); /// Set the joint constraint tuning. Advanced feature. @@ -1221,10 +1271,12 @@ B3_API void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring ) B3_API bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId ); /// Set the force range for the spring. -B3_API void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce ); +B3_API void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, + float upperForce ); /// Get the force range for the spring. -B3_API void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce ); +B3_API void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, + float* upperForce ); /// Set the spring stiffness in Hertz B3_API void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz ); @@ -1238,8 +1290,8 @@ B3_API float b3DistanceJoint_GetSpringHertz( b3JointId jointId ); /// Get the spring damping ratio B3_API float b3DistanceJoint_GetSpringDampingRatio( b3JointId jointId ); -/// Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid -/// and the limit has no effect. +/// Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is +/// rigid and the limit has no effect. B3_API void b3DistanceJoint_EnableLimit( b3JointId jointId, bool enableLimit ); /// Is the distance joint limit enabled? @@ -1358,12 +1410,101 @@ B3_API float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId ); /**@}*/ // motor_joint +/** + * @defgroup mover_joint Mover Joint + * @brief Functions for the mover joint. + * + * The mover joint is designed for a dynamic character mover. + * + * @warning Experimental. The mover joint may change in future versions. + * @{ + */ + +/// Create a mover joint. +/// @see b3MoverJointDef for details +B3_API b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def ); + +/// Set the desired relative linear velocity, usually in meters per second. +B3_API void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ); + +/// Get the desired relative linear velocity, usually in meters per second. +B3_API b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId ); + +/// Set the joint maximum force, usually in newtons. +B3_API void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ); + +/// Get the joint maximum force, usually in newtons. +B3_API float b3MoverJoint_GetMaxVelocityForce( b3JointId jointId ); + +/**@}*/ // mover_joint + +/** + * @defgroup pogo_joint Pogo Joint + * @brief Functions for the pogo joint. Used for dynamic character movers. + * + * @warning Experimental. The pogo joint may change in future versions. + * @{ + */ + +/// Create a pogo joint +/// @see b3PogoJointDef for details +B3_API b3JointId b3CreatePogoJoint( b3WorldId worldId, const b3PogoJointDef* def ); + +/// Set the rest length of a pogo joint +/// @param jointId The id for a pogo joint +/// @param length The new pogo joint length +B3_API void b3PogoJoint_SetRestLength( b3JointId jointId, float length ); + +/// Get the rest length of a pogo joint +B3_API float b3PogoJoint_GetRestLength( b3JointId jointId ); + +/// Set the spring stiffness in Hertz +B3_API void b3PogoJoint_SetSpringHertz( b3JointId jointId, float hertz ); + +/// Get the spring Hertz +B3_API float b3PogoJoint_GetSpringHertz( b3JointId jointId ); + +/// Set the spring damping ratio, non-dimensional +B3_API void b3PogoJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ); + +/// Get the spring damping ratio +B3_API float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId ); + +/// Set the max tension force, usually in Newtons. +B3_API void b3PogoJoint_SetMaxTensionForce( b3JointId jointId, float force ); + +/// Get the max tension force, usually in Newtons. +B3_API float b3PogoJoint_GetMaxTensionForce( b3JointId jointId ); + +/// Set the max compression force, usually in Newtons. +B3_API void b3PogoJoint_SetMaxCompressionForce( b3JointId jointId, float force ); + +/// Get the max compression force, usually in Newtons. +B3_API float b3PogoJoint_GetMaxCompressionForce( b3JointId jointId ); + +// Set the unit ground normal. +B3_API void b3PogoJoint_SetNormal( b3JointId jointId, b3Vec3 normal ); + +// Get the unit ground normal. +B3_API b3Vec3 b3PogoJoint_GetNormal( b3JointId jointId ); + +/// Get the current length of the pogo. +B3_API float b3PogoJoint_GetCurrentLength( b3JointId jointId ); + +/// Get the internal pogo velocity. +B3_API float b3PogoJoint_GetCurrentSpeed( b3JointId jointId ); + +/// Get the internal pogo impulse. +B3_API float b3PogoJoint_GetImpulse( b3JointId jointId ); + +/**@}*/ // pogo_joint + /** * @defgroup filter_joint Filter Joint * @brief Functions for the filter joint. * - * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also - * keeps the two bodies in the same simulation island. + * The filter joint is used to disable collision between two bodies. As a side effect of being a + * joint, it also keeps the two bodies in the same simulation island. * @{ */ @@ -1377,8 +1518,8 @@ B3_API b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* * @defgroup prismatic_joint Prismatic Joint * @brief A prismatic joint allows for translation along a single axis with no rotation. * - * The prismatic joint is useful for things like pistons and moving platforms, where you want a body to translate - * along an axis and have no rotation. Also called a *slider* joint. + * The prismatic joint is useful for things like pistons and moving platforms, where you want a body + * to translate along an axis and have no rotation. Also called a *slider* joint. * @{ */ @@ -1393,8 +1534,8 @@ B3_API void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring B3_API bool b3PrismaticJoint_IsSpringEnabled( b3JointId jointId ); /// Set the prismatic joint stiffness in Hertz. -/// This should usually be less than a quarter of the simulation rate. For example, if the simulation -/// runs at 60Hz then the joint stiffness should be 15Hz or less. +/// This should usually be less than a quarter of the simulation rate. For example, if the +/// simulation runs at 60Hz then the joint stiffness should be 15Hz or less. B3_API void b3PrismaticJoint_SetSpringHertz( b3JointId jointId, float hertz ); /// Get the prismatic joint stiffness in Hertz @@ -1458,7 +1599,8 @@ B3_API float b3PrismaticJoint_GetSpeed( b3JointId jointId ); /** * @defgroup revolute_joint Revolute Joint - * @brief A revolute joint allows for relative rotation about a single axis with no relative translation. + * @brief A revolute joint allows for relative rotation about a single axis with no relative + * translation. * * Also called a *hinge* or *pin* joint. * @{ @@ -1509,7 +1651,8 @@ B3_API float b3RevoluteJoint_GetLowerLimit( b3JointId jointId ); B3_API float b3RevoluteJoint_GetUpperLimit( b3JointId jointId ); /// Set the revolute joint limits in radians -B3_API void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); +B3_API void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, + float upperLimitRadians ); /// Enable/disable a revolute joint motor B3_API void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor ); @@ -1536,7 +1679,8 @@ B3_API float b3RevoluteJoint_GetMaxMotorTorque( b3JointId jointId ); /** * @defgroup spherical_joint Spherical Joint - * @brief A spherical joint allows for relative rotation in the 3D space with no relative translation. + * @brief A spherical joint allows for relative rotation in the 3D space with no relative + * translation. * * Also called a *ball-in-socket* or *point-to-point* joint. * @{ @@ -1574,7 +1718,8 @@ B3_API float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId ); B3_API float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId ); /// Set the spherical joint limits in radians -B3_API void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians ); +B3_API void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, + float upperLimitRadians ); /// Get the spherical joint current twist angle in radians. B3_API float b3SphericalJoint_GetTwistAngle( b3JointId jointId ); @@ -1630,12 +1775,14 @@ B3_API float b3SphericalJoint_GetMaxMotorTorque( b3JointId jointId ); /** * @defgroup weld_joint Weld Joint - * @brief A weld joint fully constrains the relative transform between two bodies while allowing for springiness + * @brief A weld joint fully constrains the relative transform between two bodies while allowing for + * springiness * - * A weld joint constrains the relative rotation and translation between two bodies. Both rotation and translation - * can have damped springs. + * A weld joint constrains the relative rotation and translation between two bodies. Both rotation + * and translation can have damped springs. * - * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld joints may flex. + * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld + * joints may flex. * @{ */ @@ -1777,7 +1924,8 @@ B3_API float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ); B3_API float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ); /// Set the wheel joint steering limits in radians. -B3_API void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ); +B3_API void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, + float upperRadians ); /// Set the wheel joint target steering angle in radians. B3_API void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ); diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h index a12f250..8f81d93 100644 --- a/include/box3d/math_functions.h +++ b/include/box3d/math_functions.h @@ -1053,6 +1053,20 @@ B3_INLINE b3Vec3 b3ClosestPointToAABB( b3Vec3 point, b3AABB a ) return b3Clamp( point, a.lowerBound, a.upperBound ); } +/// One-dimensional mass-spring-damper simulation. Returns the new velocity given the position and +/// time step. You can then compute the new position using: +/// position += timeStep * newVelocity +/// This drives towards a zero position. By using implicit integration we get a stable solution that +/// doesn't require transcendental functions. +B3_INLINE float b3SpringDamper( float hertz, float dampingRatio, float position, float velocity, + float timeStep ) +{ + float omega = 2.0f * B3_PI * hertz; + float omegaH = omega * timeStep; + return ( velocity - omega * omegaH * position ) / + ( 1.0f + 2.0f * dampingRatio * omegaH + omegaH * omegaH ); +} + /// The closest points between to segments or infinite lines. typedef struct b3SegmentDistanceResult { diff --git a/include/box3d/types.h b/include/box3d/types.h index 2cbbbeb..20fd0ce 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -15,50 +15,57 @@ #define B3_DEFAULT_MASK_BITS UINT64_MAX /// Task interface -/// This is the prototype for a Box3D task. Your task system is expected to run this callback on a worker thread, -/// exactly once per enqueue, passing back the same taskContext pointer supplied to b3EnqueueTaskCallback. +/// This is the prototype for a Box3D task. Your task system is expected to run this callback on a +/// worker thread, exactly once per enqueue, passing back the same taskContext pointer supplied to +/// b3EnqueueTaskCallback. /// @ingroup world typedef void b3TaskCallback( void* taskContext ); /// These functions can be provided to Box3D to invoke a task system. -/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box3D that the work was executed -/// serially within the callback and there is no need to call b3FinishTaskCallback. Otherwise the returned -/// value must be non-null will be passed to b3FinishTaskCallback as the userTask. +/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box3D that +/// the work was executed serially within the callback and there is no need to call +/// b3FinishTaskCallback. Otherwise the returned value must be non-null will be passed to +/// b3FinishTaskCallback as the userTask. /// @param task the Box3D task to be called by the scheduler /// @param taskContext the Box3D context object that the scheduler must pass to the task /// @param userContext the scheduler context object that is opaque to Box3D /// @param taskName the Box3D task name that the scheduler can use for diagnostics /// @ingroup world -typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, const char* taskName ); +typedef void* b3EnqueueTaskCallback( b3TaskCallback* task, void* taskContext, void* userContext, + const char* taskName ); -/// Finishes a user task object that wraps a Box3D task. This must block until the task has completed. -/// The step blocks here on the tasks it spawned, so b3World_Step holds its stack across every -/// fork/join. Drive it from a thread you can dedicate to the step, or from a fiber this callback can -/// park to free the underlying thread. In a job system that cannot park a job's stack, do not call -/// b3World_Step from inside a job: a job that blocks on its own sub-jobs without yielding its thread -/// can deadlock. The in-tree scheduler instead runs other pending tasks on the waiting thread. +/// Finishes a user task object that wraps a Box3D task. This must block until the task has +/// completed. The step blocks here on the tasks it spawned, so b3World_Step holds its stack across +/// every fork/join. Drive it from a thread you can dedicate to the step, or from a fiber this +/// callback can park to free the underlying thread. In a job system that cannot park a job's stack, +/// do not call b3World_Step from inside a job: a job that blocks on its own sub-jobs without +/// yielding its thread can deadlock. The in-tree scheduler instead runs other pending tasks on the +/// waiting thread. /// @ingroup world typedef void b3FinishTaskCallback( void* userTask, void* userContext ); typedef struct b3DebugShape b3DebugShape; -/// The user needs to be able to create debug draw shapes for multi-pass rendering to work efficiently. -/// These user shapes are created and destroyed via callback so they can be bound to shape lifetime and scaling updates. +/// The user needs to be able to create debug draw shapes for multi-pass rendering to work +/// efficiently. These user shapes are created and destroyed via callback so they can be bound to +/// shape lifetime and scaling updates. /// @ingroup debug_draw typedef void* b3CreateDebugShapeCallback( const b3DebugShape* debugShape, void* userContext ); typedef void b3DestroyDebugShapeCallback( void* userShape, void* userContext ); -/// Optional friction mixing callback. This intentionally provides no context objects because this is called -/// from a worker thread. +/// Optional friction mixing callback. This intentionally provides no context objects because this +/// is called from a worker thread. /// @warning This function should not attempt to modify Box3D state or user application state. /// @ingroup world -typedef float b3FrictionCallback( float frictionA, uint64_t userMaterialIdA, float frictionB, uint64_t userMaterialIdB ); +typedef float b3FrictionCallback( float frictionA, uint64_t userMaterialIdA, float frictionB, + uint64_t userMaterialIdB ); -/// Optional restitution mixing callback. This intentionally provides no context objects because this is called -/// from a worker thread. +/// Optional restitution mixing callback. This intentionally provides no context objects because +/// this is called from a worker thread. /// @warning This function should not attempt to modify Box3D state or user application state. /// @ingroup world -typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, float restitutionB, uint64_t userMaterialIdB ); +typedef float b3RestitutionCallback( float restitutionA, uint64_t userMaterialIdA, + float restitutionB, uint64_t userMaterialIdB ); /// Prototype for a contact filter callback. /// This is called when a contact pair is considered for collision. This allows you to @@ -86,7 +93,8 @@ typedef bool b3CustomFilterFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, void* co /// full contact manifold. /// @warning Do not attempt to modify the world inside this callback /// @ingroup world -typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, void* context ); +typedef bool b3PreSolveFcn( b3ShapeId shapeIdA, b3ShapeId shapeIdB, b3Pos point, b3Vec3 normal, + void* context ); /// Prototype callback for overlap queries. /// Called for each shape found in the query. @@ -107,14 +115,16 @@ typedef bool b3OverlapResultFcn( b3ShapeId shapeId, void* context ); /// @param normal the normal vector at the point of intersection /// @param fraction the fraction along the ray at the point of intersection /// @param userMaterialId the shape or triangle surface type -/// @param triangleIndex the triangle index for mesh or height field shapes or -1 for other shape types +/// @param triangleIndex the triangle index for mesh or height field shapes or -1 for other shape +/// types /// @param childIndex the child shape index for compound shapes /// @param context the user context /// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue /// @see b3World_CastRay /// @ingroup world -typedef float b3CastResultFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, - int triangleIndex, int childIndex, void* context ); +typedef float b3CastResultFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, + uint64_t userMaterialId, int triangleIndex, int childIndex, + void* context ); /// Optional world capacities that can be use to avoid run-time allocations /// @ingroup world @@ -147,7 +157,8 @@ typedef struct b3WorldDef /// speed have restitution applied (will bounce). float restitutionThreshold; - /// Number of iterations of the restitution solver. More iterations can lead to less box spinning. + /// Number of iterations of the restitution solver. More iterations can lead to less box + /// spinning. /// @see B3_MAX_RESTITUTION_ITERATIONS int restitutionIterations; @@ -158,16 +169,17 @@ typedef struct b3WorldDef /// speed can generate hit events if the shape also enables hit events. float hitEventThreshold; - /// Contact stiffness. Cycles per second. Increasing this increases the speed of overlap recovery, but can introduce jitter. + /// Contact stiffness. Cycles per second. Increasing this increases the speed of overlap + /// recovery, but can introduce jitter. float contactHertz; - /// Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this with - /// the trade-off that overlap resolution becomes more energetic. + /// Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this + /// with the trade-off that overlap resolution becomes more energetic. float contactDampingRatio; - /// This parameter controls how fast overlap is resolved and usually has units of meters per second. This only - /// puts a cap on the resolution speed. The resolution speed is increased by increasing the hertz and/or - /// decreasing the damping ratio. + /// This parameter controls how fast overlap is resolved and usually has units of meters per + /// second. This only puts a cap on the resolution speed. The resolution speed is increased by + /// increasing the hertz and/or decreasing the damping ratio. float contactSpeed; /// Maximum linear speed. Usually meters per second. @@ -190,11 +202,11 @@ typedef struct b3WorldDef bool enableContinuous; /// Number of workers to use with the provided task system. Box3D performs best when using only - /// performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading provide - /// little benefit and may even harm performance. - /// This is clamped to the range [1, B3_MAX_WORKERS]. Using a value above 1 will turn on multithreading. - /// If task callbacks are provided then Box3D will use the user provided task system. Otherwise Box3D - /// will create threads and use an internal scheduler. + /// performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading + /// provide little benefit and may even harm performance. This is clamped to the range [1, + /// B3_MAX_WORKERS]. Using a value above 1 will turn on multithreading. If task callbacks are + /// provided then Box3D will use the user provided task system. Otherwise Box3D will create + /// threads and use an internal scheduler. uint32_t workerCount; /// function to spawn task @@ -231,7 +243,8 @@ typedef struct b3WorldDef B3_API b3WorldDef b3DefaultWorldDef( void ); /// The body simulation type. -/// Each body is one of these three types. The type determines how the body behaves in the simulation. +/// Each body is one of these three types. The type determines how the body behaves in the +/// simulation. /// @ingroup body typedef enum b3BodyType { @@ -282,8 +295,8 @@ typedef struct b3BodyDef b3BodyType type; /// The initial world position of the body. Bodies should be created with the desired position. - /// @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially - /// if the body is moved after shapes have been added. + /// @note Creating bodies at the origin and then moving them nearly doubles the cost of body + /// creation, especially if the body is moved after shapes have been added. b3Pos position; /// The initial world rotation of the body. @@ -315,10 +328,11 @@ typedef struct b3BodyDef float sleepThreshold; /// Continuous collision safety factor. The solver only uses continuous collision if there is a - /// risk of tunneling. If the body is moving fast enough to risk tunneling then it is considered a "fast body". - /// This improves performance and prevents movement hitches. If a body moving N meter risks tunneling, then the - /// body will be considered fast if it moves more than a safetyFactor times N meters over one full time step. - /// Non-dimensional. Recommended range [0.01, 0.5]. Default is 0.5 for high performance with low tunneling risk. + /// risk of tunneling. If the body is moving fast enough to risk tunneling then it is considered + /// a "fast body". This improves performance and prevents movement hitches. If a body moving N + /// meter risks tunneling, then the body will be considered fast if it moves more than a + /// safetyFactor times N meters over one full time step. Non-dimensional. Recommended range + /// [0.01, 0.5]. Default is 0.5 for high performance with low tunneling risk. float safetyFactor; /// Optional body name for debugging. @@ -338,18 +352,19 @@ typedef struct b3BodyDef /// Treat this body as a high speed object that performs continuous collision detection /// against dynamic and kinematic bodies, but not other bullet bodies. - /// @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic - /// continuous collision. They do not guarantee accurate collision if both bodies are fast moving because - /// the bullet does a continuous check after all non-bullet bodies have moved. You could get unlucky and have - /// the bullet body end a time step very close to a non-bullet body and the non-bullet body then moves over - /// the bullet body. In continuous collision, initial overlap is ignored to avoid freezing bodies in place. - /// I do not recommend using them for game projectiles if precise collision timing is needed. Instead consider - /// using a ray or shape cast. You can use a marching ray or shape cast for projectile that moves over time. - /// If you want a fast moving projectile to collide with a fast moving target, you need to consider the relative - /// movement in your ray or shape cast. This is out of the scope of Box3D. - /// So what are good use cases for bullets? Pinball games or games with dynamic containers that hold other objects. - /// It should be a use case where it doesn't break the game if there is a collision missed, but having them - /// captured improves the quality of the game. + /// @warning Bullets should be used sparingly. They are not a solution for general + /// dynamic-versus-dynamic continuous collision. They do not guarantee accurate collision if + /// both bodies are fast moving because the bullet does a continuous check after all non-bullet + /// bodies have moved. You could get unlucky and have the bullet body end a time step very close + /// to a non-bullet body and the non-bullet body then moves over the bullet body. In continuous + /// collision, initial overlap is ignored to avoid freezing bodies in place. I do not recommend + /// using them for game projectiles if precise collision timing is needed. Instead consider + /// using a ray or shape cast. You can use a marching ray or shape cast for projectile that + /// moves over time. If you want a fast moving projectile to collide with a fast moving target, + /// you need to consider the relative movement in your ray or shape cast. This is out of the + /// scope of Box3D. So what are good use cases for bullets? Pinball games or games with dynamic + /// containers that hold other objects. It should be a use case where it doesn't break the game + /// if there is a collision missed, but having them captured improves the quality of the game. bool isBullet; /// Used to disable a body. A disabled body does not move or collide. @@ -403,8 +418,8 @@ typedef struct b3Filter /// or always collide (positive). A group index of zero has no effect. Non-zero group filtering /// always wins against the mask bits. /// For example, you may want ragdolls to collide with other ragdolls but you don't want - /// ragdoll self-collision. In this case you would give each ragdoll a unique negative group index - /// and apply that group index to all shapes on the ragdoll. + /// ragdoll self-collision. In this case you would give each ragdoll a unique negative group + /// index and apply that group index to all shapes on the ragdoll. int groupIndex; } b3Filter; @@ -423,7 +438,8 @@ typedef struct b3SurfaceMaterial /// https://en.wikipedia.org/wiki/Coefficient_of_restitution float restitution; - /// The rolling resistance usually in the range [0,1]. This is only used for spheres and capsules. + /// The rolling resistance usually in the range [0,1]. This is only used for spheres and + /// capsules. float rollingResistance; /// The tangent velocity for conveyor belts. This is local to the shape and will be projected @@ -483,7 +499,8 @@ typedef struct b3ShapeDef /// Use this to store application specific shape data. void* userData; - /// Surface material used on mesh shapes per triangle. Ignored for convex shapes. Ignored for compound shapes. + /// Surface material used on mesh shapes per triangle. Ignored for convex shapes. Ignored for + /// compound shapes. b3SurfaceMaterial* materials; /// Surface material count. @@ -501,42 +518,49 @@ typedef struct b3ShapeDef /// Contact filtering data. b3Filter filter; - /// Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See b3WorldDef. + /// Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See + /// b3WorldDef. bool enableCustomFiltering; /// A sensor shape generates overlap events but never generates a collision response. - /// Sensors do not have continuous collision. Instead, use a ray or shape cast for those scenarios. - /// Sensors still contribute to the body mass if they have non-zero density. + /// Sensors do not have continuous collision. Instead, use a ray or shape cast for those + /// scenarios. Sensors still contribute to the body mass if they have non-zero density. /// @note Sensor events are disabled by default. /// @see enableSensorEvents bool isSensor; - /// Enable sensor events for this shape. This applies to sensors and non-sensors. False by default, even for sensors. - /// Only convex shapes may act as sensor visitors. + /// Enable sensor events for this shape. This applies to sensors and non-sensors. False by + /// default, even for sensors. Only convex shapes may act as sensor visitors. bool enableSensorEvents; - /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored + /// for sensors. False by default. bool enableContactEvents; - /// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + /// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for + /// sensors. False by default. bool enableHitEvents; - /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are + /// expensive /// and must be carefully handled due to multithreading. Ignored for sensors. bool enablePreSolveEvents; - /// When shapes are created they will scan the environment for collision the next time step. This can significantly slow down - /// static body creation when there are many static shapes. - /// This is flag is ignored for dynamic and kinematic shapes which always invoke contact creation. + /// When shapes are created they will scan the environment for collision the next time step. + /// This can significantly slow down static body creation when there are many static shapes. + /// This is flag is ignored for dynamic and kinematic shapes which always invoke contact + /// creation. bool invokeContactCreation; /// Should the body update the mass properties when this shape is created. Default is true. - /// Warning: if this is false, you MUST call b3Body_ApplyMassFromShapes or b3Body_SetMassData before simulating the world. + /// Warning: if this is false, you MUST call b3Body_ApplyMassFromShapes or b3Body_SetMassData + /// before simulating the world. bool updateBodyMass; /// Enable speculative collision. Leave this true unless you care about reducing ghost collision /// more than continuous collision under rotation. - /// Experimental: this can only disable speculative contact between hulls and triangles (meshes and height fields). + /// Experimental: this can only disable speculative contact between hulls and triangles (meshes + /// and height fields). bool enableSpeculativeContact; /// Used internally to detect a valid definition. DO NOT SET. @@ -628,6 +652,8 @@ typedef enum b3JointType b3_sphericalJoint, b3_weldJoint, b3_wheelJoint, + b3_moverJoint, + b3_pogoJoint, } b3JointType; /// Base joint definition used by all joint types. The local frames are measured from the @@ -767,6 +793,64 @@ typedef struct b3MotorJointDef /// @ingroup motor_joint B3_API b3MotorJointDef b3DefaultMotorJointDef( void ); +/// A mover joint is used to move a dynamic character mover through velocity commands. Force is only +/// applied in the xz plane of joint frame B. Does not affect rotation. +/// @ingroup mover_joint +typedef struct b3MoverJointDef +{ + /// Base joint definition + b3JointDef base; + + /// The desired linear velocity + b3Vec3 linearVelocity; + + /// The maximum motor force in newtons. + float maxForce; +} b3MoverJointDef; + +/// Use this to initialize your joint definition +/// @ingroup mover_joint +B3_API b3MoverJointDef b3DefaultMoverJointDef( void ); + +/// The pogo joint is used for the dynamic character mover. The pogo axis is along the frame B +/// y-axis. +/// @ingroup pogo_joint +typedef struct b3PogoJointDef +{ + /// Base joint definition + b3JointDef base; + + /// Normal vector at the contact + b3Vec3 normal; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// Spring length. + float restLength; + + /// Maximum tension force. Controls how sticky the mover is to ground. + float maxTensionForce; + + /// Maximum compression force. How hard the pogo pushes the mover up from the ground. + float maxCompressionForce; + + /// The initial pogo internal impulse. The pogo is typically recreated every frame. This gives a + /// way to move the internal state forward for smoother spring behavior. + float impulse; + + /// The initial pogo internal velocity. The pogo is typically recreated every frame. This gives + /// a way to move the internal state forward for smoother spring behavior. + float velocity; +} b3PogoJointDef; + +/// Use this to initialize your joint definition +/// @ingroup pogo_joint +B3_API b3PogoJointDef b3DefaultPogoJointDef( void ); + /// A filter joint is used to disable collision between two specific bodies. /// @ingroup filter_joint typedef struct b3FilterJointDef @@ -1075,8 +1159,9 @@ B3_API b3ExplosionDef b3DefaultExplosionDef( void ); * Also when events occur in the simulation step it may be problematic to modify the world, which is * often what applications want to do when events occur. * - * With event arrays, you can scan the events in a loop and modify the world. However, you need to be careful - * that some event data may become invalid. There are several samples that show how to do this safely. + * With event arrays, you can scan the events in a loop and modify the world. However, you need to + * be careful that some event data may become invalid. There are several samples that show how to do + * this safely. * * @{ */ @@ -1135,8 +1220,8 @@ typedef struct b3ContactBeginTouchEvent /// Id of the second shape b3ShapeId shapeIdB; - /// The transient contact id. This contact may be destroyed automatically when the world is modified or simulated. - /// Use b3Contact_IsValid before using this id. + /// The transient contact id. This contact may be destroyed automatically when the world is + /// modified or simulated. Use b3Contact_IsValid before using this id. b3ContactId contactId; } b3ContactBeginTouchEvent; @@ -1162,8 +1247,8 @@ typedef struct b3ContactEndTouchEvent b3ContactId contactId; } b3ContactEndTouchEvent; -/// A hit touch event is generated when two shapes collide with a speed faster than the hit speed threshold. -/// This may be reported for speculative contacts that have a confirmed impulse. +/// A hit touch event is generated when two shapes collide with a speed faster than the hit speed +/// threshold. This may be reported for speculative contacts that have a confirmed impulse. typedef struct b3ContactHitEvent { /// Id of the first shape @@ -1227,8 +1312,8 @@ typedef struct b3ContactEvents /// On the other hand if the flag does not indicate the body went to sleep then the application /// can treat the actor/entity/object associated with the body as awake. /// This is an efficient way for an application to update game object transforms rather than -/// calling functions such as b3Body_GetTransform() because this data is delivered as a contiguous array -/// and it is only populated with bodies that have moved. +/// calling functions such as b3Body_GetTransform() because this data is delivered as a contiguous +/// array and it is only populated with bodies that have moved. /// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. typedef struct b3BodyMoveEvent { @@ -1299,7 +1384,8 @@ typedef struct b3ContactData /// this pointer. const struct b3Manifold* manifolds; - /// The number of contact manifolds. For mesh and height-field collision there can be multiple manifolds. + /// The number of contact manifolds. For mesh and height-field collision there can be multiple + /// manifolds. int manifoldCount; } b3ContactData; @@ -1325,14 +1411,15 @@ typedef struct b3QueryFilter /// query would accept for collision. uint64_t maskBits; - /// Optional id combined with @ref name to identify this query in a recording, e.g. an entity id. - /// Need not be unique on its own. 0 with a null name means untagged. Ignored when not recording. + /// Optional id combined with @ref name to identify this query in a recording, e.g. an entity + /// id. Need not be unique on its own. 0 with a null name means untagged. Ignored when not + /// recording. uint64_t id; - /// Optional label combined with @ref id to identify this query, e.g. "bullet". Need not be unique - /// on its own. The recorder hashes (id, name) into one stable key the viewer tracks the query by, - /// so the same id and name pair identifies the same query across frames. NULL means none. Ignored - /// when not recording. + /// Optional label combined with @ref id to identify this query, e.g. "bullet". Need not be + /// unique on its own. The recorder hashes (id, name) into one stable key the viewer tracks the + /// query by, so the same id and name pair identifies the same query across frames. NULL means + /// none. Ignored when not recording. const char* name; } b3QueryFilter; @@ -1416,7 +1503,8 @@ typedef struct b3ShapeCastInput /// The maximum fraction of the translation to consider, typically 1. float maxFraction; - /// Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero. + /// Allow shape cast to encroach when initially touching. This only works if the radius is + /// greater than zero. bool canEncroach; } b3ShapeCastInput; @@ -1562,7 +1650,7 @@ typedef struct b3ShapeCastPairInput b3Transform transform; ///< Transform of shape B in shape A's frame, the relative pose B in A b3Vec3 translationB; ///< The translation of shape B, in A's frame float maxFraction; ///< The fraction of the translation to consider, typically 1 - bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching + bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching } b3ShapeCastPairInput; /// Input for b3ShapeDistance @@ -1575,7 +1663,8 @@ typedef struct b3DistanceInput b3ShapeProxy proxyB; /// Transform of shape B in shape A's frame, the relative pose B in A - /// (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in frame A. + /// (b3InvMulWorldTransforms( worldA, worldB )). The query is origin independent and runs in + /// frame A. b3Transform transform; /// Should the proxy radius be considered? @@ -1611,9 +1700,9 @@ typedef struct b3Simplex int count; ///< number of valid vertices } b3Simplex; -/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin, -/// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass -/// position. +/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect +/// to the body origin, which may not coincide with the center of mass. However, to support dynamics +/// we must interpolate the center of mass position. typedef struct b3Sweep { b3Vec3 localCenter; ///< Local center of mass position @@ -1682,9 +1771,9 @@ typedef struct b3TOIOutput * @defgroup tree Dynamic Tree * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects * - * Box3D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy. - * This data structure may have uses in games for organizing other geometry data and may be used independently - * of Box3D rigid body simulation. + * Box3D uses the dynamic tree internally to sort collision shapes into a binary bounding volume + * hierarchy. This data structure may have uses in games for organizing other geometry data and may + * be used independently of Box3D rigid body simulation. * * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. * A dynamic tree arranges data in a binary tree to accelerate @@ -1813,23 +1902,27 @@ typedef struct b3TreeStats /// @return true if the query should continue typedef bool b3TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* context ); -/// This function receives the minimum distance squared so far and proxy to check in the closest query. +/// This function receives the minimum distance squared so far and proxy to check in the closest +/// query. /// @return minimum distance squared to user objects in the proxy -typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, void* context ); +typedef float b3TreeQueryClosestCallbackFcn( float distanceSqrMin, int proxyId, uint64_t userData, + void* context ); /// This function receives clipped AABB cast input for a proxy. The function returns the new cast /// fraction. /// - return a value of 0 to terminate the cast /// - return a value less than input->maxFraction to clip the cast /// - return a value of input->maxFraction to continue the cast without clipping -typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ); +typedef float b3TreeBoxCastCallbackFcn( const b3BoxCastInput* input, int proxyId, uint64_t userData, + void* context ); /// This function receives clipped ray cast input for a proxy. The function /// returns the new ray fraction. /// - return a value of 0 to terminate the ray cast /// - return a value less than input->maxFraction to clip the ray /// - return a value of input->maxFraction to continue the ray cast without clipping -typedef float b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ); +typedef float b3TreeRayCastCallbackFcn( const b3RayCastInput* input, int proxyId, uint64_t userData, + void* context ); /**@}*/ // tree @@ -1873,7 +1966,8 @@ typedef struct b3CollisionPlane /// The push on the mover determined by b3SolvePlanes. Usually in meters. float push; - /// Indicates if b3ClipVector should clip against this plane. Should be false for soft collision. + /// Indicates if b3ClipVector should clip against this plane. Should be false for soft + /// collision. bool clipVelocity; } b3CollisionPlane; @@ -1915,7 +2009,8 @@ typedef struct b3BodyTOIResult /// Used to collect collision planes for character movers. /// Return true to continue gathering planes. -typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, void* context ); +typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, + void* context ); /// Used to filter shapes for shape casting character movers. /// Return true to accept the collision @@ -3032,8 +3127,8 @@ B3_INLINE uint32_t b3MakeDebugColor( b3HexColor rgb, b3DebugMaterial material ) /// (B3_GRAPH_COLOR_COUNT - 1) is the overflow color. B3_API b3HexColor b3GetGraphColor( int index ); -/// This is sent to the user for debug shape creation. The user should know the type in case they have -/// custom sphere or capsule rendering. +/// This is sent to the user for debug shape creation. The user should know the type in case they +/// have custom sphere or capsule rendering. typedef struct b3DebugShape { /// Shape id. @@ -3059,10 +3154,11 @@ typedef struct b3DebugShape /// it stays accurate far from the origin. Shift into your own camera frame inside the callbacks. typedef struct b3DebugDraw { - /// Draws a user shape. The userShape pointer is owned by the application and is known to Box3D as - /// an opaque pointer returned from b3CreateDebugShapeCallback. When this is called the drawn shape has - /// passed a culling test against drawingBounds below. - void ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, void* context ); + /// Draws a user shape. The userShape pointer is owned by the application and is known to Box3D + /// as an opaque pointer returned from b3CreateDebugShapeCallback. When this is called the drawn + /// shape has passed a culling test against drawingBounds below. + void ( *DrawShapeFcn )( void* userShape, b3WorldTransform transform, b3HexColor color, + void* context ); /// Draw a line segment. void ( *DrawSegmentFcn )( b3Pos p1, b3Pos p2, b3HexColor color, void* context ); @@ -3077,13 +3173,15 @@ typedef struct b3DebugDraw void ( *DrawSphereFcn )( b3Pos p, float radius, b3HexColor color, float alpha, void* context ); /// Draw a capsule. - void ( *DrawCapsuleFcn )( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context ); + void ( *DrawCapsuleFcn )( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, + void* context ); /// Draw a bounding box. void ( *DrawBoundsFcn )( b3AABB aabb, b3HexColor color, void* context ); /// Draw an oriented box. - void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context ); + void ( *DrawBoxFcn )( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, + void* context ); /// Draw a string in world space void ( *DrawStringFcn )( b3Pos p, const char* s, b3HexColor color, void* context ); diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 257de2e..d9d29de 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -169,8 +169,10 @@ set(SAMPLE_FILES sample.h mesh_loader.cpp mesh_loader.h - mover.cpp - mover.h + dynamic_mover.cpp + dynamic_mover.h + geometric_mover.cpp + geometric_mover.h jsmn.h tiny_obj_loader.h earcut.h diff --git a/samples/dynamic_mover.cpp b/samples/dynamic_mover.cpp new file mode 100644 index 0000000..7f85b41 --- /dev/null +++ b/samples/dynamic_mover.cpp @@ -0,0 +1,584 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "dynamic_mover.h" + +#include "gfx/draw.h" + +#include "box3d/box3d.h" +#include "box3d/math_functions.h" + +#include + +struct CastContext +{ + DynamicMoverCastResult result; + b3BodyId moverId; +}; + +static float CastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, + uint64_t userMaterialId, int triangleIndex, int childIndex, + void* context ) +{ + (void)triangleIndex; + (void)childIndex; + + CastContext* castContext = (CastContext*)context; + b3BodyId bodyId = b3Shape_GetBody( shapeId ); + + // The probes start inside or on the capsule. Sensors can't hold up the mover. + if ( B3_ID_EQUALS( bodyId, castContext->moverId ) || b3Shape_IsSensor( shapeId ) ) + { + return -1.0f; + } + + DynamicMoverCastResult* result = &castContext->result; + result->point = point; + result->normal = normal; + result->bodyId = bodyId; + result->userMaterialId = userMaterialId; + result->fraction = fraction; + result->hit = true; + return fraction; +} + +DynamicMover::DynamicMover() +{ + DynamicMoverDef def; + m_jumpSpeed = def.jumpSpeed; + m_maxSpeed = def.maxSpeed; + m_minSpeed = def.minSpeed; + m_stopSpeed = def.stopSpeed; + m_accelerate = def.accelerate; + m_airSteer = def.airSteer; + m_friction = def.friction; + m_gravityScale = def.gravityScale; + m_maxGroundForce = def.maxGroundForce; + m_maxAirForce = def.maxAirForce; + m_pogoRestLength = def.pogoRestLength; + m_pogoHertz = def.pogoHertz; + m_pogoDampingRatio = def.pogoDampingRatio; + m_pogoCompressionScale = def.pogoCompressionScale; + m_pogoTensionScale = def.pogoTensionScale; + m_enableSkirtPogo = def.enableSkirtPogo; + m_platformMaterialId = def.platformMaterialId; + m_minGroundNormalY = 0.7f; + + m_capsule = def.capsule; + m_filter = def.filter; + + m_worldId = b3_nullWorldId; + m_groundId = b3_nullBodyId; + m_moverId = b3_nullBodyId; + m_surfaceBodyId = b3_nullBodyId; + m_moverJointId = b3_nullJointId; + m_pogoJointId = b3_nullJointId; + + m_castResult = {}; + + m_velocity = b3Vec3_zero; + m_timeSliding = 0.0f; + m_slideDuration = 0.2f; + m_onGround = false; + m_walkable = false; + m_jumping = false; + m_jumpTicks = 0; + + m_pogoImpulse = 0.0f; + m_pogoVelocity = 0.0f; + m_pogoLength = 0.0f; + + for ( DynamicMoverProbe& probe : m_probes ) + { + probe = { b3Pos_zero, b3Vec3_zero, 1.0f, false }; + } + + m_probeCount = 0; + m_selectedProbe = 0; +} + +void DynamicMover::Create( b3WorldId worldId, const DynamicMoverDef* def ) +{ + m_capsule = def->capsule; + m_filter = def->filter; + m_jumpSpeed = def->jumpSpeed; + m_maxSpeed = def->maxSpeed; + m_minSpeed = def->minSpeed; + m_stopSpeed = def->stopSpeed; + m_accelerate = def->accelerate; + m_airSteer = def->airSteer; + m_friction = def->friction; + m_gravityScale = def->gravityScale; + m_maxGroundForce = def->maxGroundForce; + m_maxAirForce = def->maxAirForce; + m_pogoRestLength = def->pogoRestLength; + m_pogoHertz = def->pogoHertz; + m_pogoDampingRatio = def->pogoDampingRatio; + m_pogoCompressionScale = def->pogoCompressionScale; + m_pogoTensionScale = def->pogoTensionScale; + m_enableSkirtPogo = def->enableSkirtPogo; + m_platformMaterialId = def->platformMaterialId; + + m_worldId = worldId; + m_velocity = b3Vec3_zero; + m_onGround = false; + m_walkable = false; + m_jumping = false; + m_jumpTicks = 0; + m_pogoImpulse = 0.0f; + m_pogoVelocity = 0.0f; + m_pogoLength = 0.0f; + m_pogoJointId = b3_nullJointId; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_groundId = b3CreateBody( worldId, &bodyDef ); + + // Position is the center of the capsule. + bodyDef.type = b3_dynamicBody; + bodyDef.position = def->position; + bodyDef.gravityScale = m_gravityScale; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + bodyDef.enableSleep = false; + bodyDef.name = "mover"; + + m_moverId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = def->density; + + // The mover joint and the pogo joint handle all the movement, surface friction would fight + // them. + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + shapeDef.baseMaterial.customColor = b3_colorDodgerBlue; + shapeDef.filter = def->filter; + shapeDef.enablePreSolveEvents = def->enablePreSolveEvents; + shapeDef.enableSensorEvents = def->enableSensorEvents; + + b3CreateCapsuleShape( m_moverId, &shapeDef, &m_capsule ); + + CreateMoverJoint( m_groundId ); +} + +void DynamicMover::CreateMoverJoint( b3BodyId baseId ) +{ + b3MoverJointDef moverDef = b3DefaultMoverJointDef(); + moverDef.linearVelocity = m_velocity; + moverDef.maxForce = m_maxGroundForce; + moverDef.base.bodyIdA = baseId; + moverDef.base.bodyIdB = m_moverId; + moverDef.base.collideConnected = true; + + m_moverJointId = b3CreateMoverJoint( m_worldId, &moverDef ); +} + +void DynamicMover::Destroy() +{ + if ( B3_IS_NON_NULL( m_moverId ) ) + { + b3DestroyBody( m_moverId ); + } + + if ( B3_IS_NON_NULL( m_groundId ) ) + { + b3DestroyBody( m_groundId ); + } + + m_groundId = b3_nullBodyId; + m_moverId = b3_nullBodyId; + + // Joints are implicitly destroyed. + m_moverJointId = b3_nullJointId; + m_pogoJointId = b3_nullJointId; +} + +void DynamicMover::SetGravityScale( float gravityScale ) +{ + m_gravityScale = gravityScale; + + if ( B3_IS_NON_NULL( m_moverId ) ) + { + b3Body_SetGravityScale( m_moverId, gravityScale ); + } +} + +bool DynamicMover::Jump() +{ + if ( m_onGround == false || m_walkable == false ) + { + return false; + } + + float surfaceVelocity = 0.0f; + if ( b3Body_IsValid( m_castResult.bodyId ) ) + { + b3Vec3 v = b3Body_GetWorldPointVelocity( m_castResult.bodyId, m_castResult.point ); + surfaceVelocity = v.y; + } + + float mass = b3Body_GetMass( m_moverId ); + float vy = b3Body_GetLinearVelocity( m_moverId ).y; + + // Remove the pogo constraint velocity but add the surface velocity. + float dv = b3MaxFloat( 0.0f, m_jumpSpeed - vy ) + surfaceVelocity; + b3Body_ApplyLinearImpulseToCenter( m_moverId, { 0.0f, mass * dv, 0.0f }, true ); + + Launch(); + return true; +} + +void DynamicMover::Launch() +{ + // This removes the pogo step down on the next update. + m_onGround = false; + m_walkable = false; + m_jumping = true; + m_jumpTicks = 0; +} + +// Movement follows the Quake ground and air model: +// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390 + +// onGround - controls friction, mover max force, and pogo step-down extension +// walkable - controls air steer +// jumping - blocks the pogo from pulling down +void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, + float speedScale ) +{ + // Reach further while grounded so the spring can find the ground over a step + float stepDownLength = m_pogoRestLength; + float rayLength = m_onGround ? m_pogoRestLength + stepDownLength : m_pogoRestLength; + b3Vec3 translation = { 0.0f, -rayLength, 0.0f }; + + b3Pos position = b3Body_GetPosition( m_moverId ); + b3Pos origin = position + m_capsule.center1; + + b3QueryFilter queryFilter = { + .categoryBits = m_filter.categoryBits, + .maskBits = m_filter.maskBits, + .id = 0, + .name = "dynamic_mover_pogo", + }; + + CastContext castContext = { {}, m_moverId }; + b3World_CastRay( m_worldId, origin, translation, queryFilter, CastCallback, &castContext ); + m_castResult = castContext.result; + + m_probes[0] = { origin, translation, m_castResult.hit ? m_castResult.fraction : 1.0f, + m_castResult.hit }; + m_probeCount = 1; + m_selectedProbe = 0; + + b3Vec3 pogoOffset = b3Vec3_zero; + + bool centerWalkable = m_castResult.hit && m_castResult.normal.y >= m_minGroundNormalY; + if ( centerWalkable == false && m_enableSkirtPogo ) + { + // The skirt turns with the direction of travel so ties go to the leading probe + b3Vec3 moverVelocity = b3Body_GetLinearVelocity( m_moverId ); + b3Vec3 heading = { moverVelocity.x, 0.0f, moverVelocity.z }; + if ( b3LengthSquared( heading ) < FLT_EPSILON ) + { + heading = b3Vec3_axisX; + } + + heading = b3Normalize( heading ); + b3Vec3 side = { -heading.z, 0.0f, heading.x }; + + float bestDisplacement = FLT_MAX; + for ( int i = 0; i < m_skirtCount; ++i ) + { + b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI * i / m_skirtCount ); + b3Vec3 offset = m_capsule.radius * ( cs.cosine * heading + cs.sine * side ); + + b3Pos skirtOrigin = origin + offset; + CastContext skirtContext = { {}, m_moverId }; + b3World_CastRay( m_worldId, skirtOrigin, translation, queryFilter, CastCallback, + &skirtContext ); + const DynamicMoverCastResult& result = skirtContext.result; + + m_probes[m_probeCount] = { skirtOrigin, translation, + result.hit ? result.fraction : 1.0f, result.hit }; + + // Use the walkable result that causes the least movement. With none the mover keeps + // the central hit and slides off the steep surface. + if ( result.hit && result.normal.y >= m_minGroundNormalY ) + { + float displacement = b3AbsFloat( result.fraction * rayLength - m_pogoRestLength ); + if ( displacement < bestDisplacement ) + { + bestDisplacement = displacement; + m_castResult = result; + m_selectedProbe = m_probeCount; + pogoOffset = offset; + } + } + + m_probeCount += 1; + } + } + + // Should jumping end? + if ( m_jumping ) + { + m_jumpTicks += 1; + + // Jump ticks allow time for the jump to leave the ground. Otherwise jumping ends + // when the character approaches the current ground. + if ( m_jumpTicks > 2 && m_castResult.hit && + b3Dot( m_velocity, m_castResult.normal ) <= 0.0f ) + { + m_jumping = false; + } + } + + if ( m_castResult.hit ) + { + // Is the ground too steep to walk? + m_walkable = m_castResult.normal.y >= m_minGroundNormalY; + + // The cast can still hit while jumping. + m_onGround = m_jumping == false; + + if ( m_onGround ) + { + if ( m_walkable ) + { + m_timeSliding = b3MaxFloat( m_timeSliding - timeStep, 0.0f ); + } + else + { + m_timeSliding = b3MinFloat( m_timeSliding + timeStep, m_slideDuration ); + } + } + } + else + { + m_onGround = false; + m_walkable = false; + m_timeSliding = b3MaxFloat( m_timeSliding - timeStep, 0.0f ); + } + + // Connect the mover joint to platform. Acts like friction. + b3BodyId baseId = m_groundId; + if ( m_castResult.hit && m_platformMaterialId != 0 && + m_castResult.userMaterialId == m_platformMaterialId && + b3Body_GetType( m_castResult.bodyId ) == b3_kinematicBody ) + { + baseId = m_castResult.bodyId; + } + + // The mover joint can be destroyed if attached to a platform that gets destroyed. + bool moverJointValid = b3Joint_IsValid( m_moverJointId ); + + if ( moverJointValid == false || + B3_ID_EQUALS( b3Joint_GetBodyA( m_moverJointId ), baseId ) == false ) + { + if ( moverJointValid ) + { + b3DestroyJoint( m_moverJointId, false ); + } + + CreateMoverJoint( baseId ); + } + + m_velocity = b3Body_GetLinearVelocity( m_moverId ) - b3Body_GetLinearVelocity( baseId ); + + // Friction acts in the horizontal plane. The pogo owns the vertical motion. + if ( m_onGround ) + { + float speed = sqrtf( m_velocity.x * m_velocity.x + m_velocity.z * m_velocity.z ); + if ( speed < m_minSpeed ) + { + m_velocity.x = 0.0f; + m_velocity.z = 0.0f; + } + else + { + // Linear damping above stopSpeed and fixed reduction below stopSpeed + float control = speed < m_stopSpeed ? m_stopSpeed : speed; + + // friction has units of 1/time + float drop = control * m_friction * timeStep; + float newSpeed = b3MaxFloat( 0.0f, speed - drop ); + float ratio = newSpeed / speed; + m_velocity.x *= ratio; + m_velocity.z *= ratio; + } + } + + // Mover force + float maxForce; + if ( m_onGround ) + { + if ( m_slideDuration > 0.0f ) + { + float scale = b3ClampFloat( 1.0f - m_timeSliding / m_slideDuration, 0.0f, 1.0f ); + maxForce = scale * m_maxGroundForce; + } + else + { + maxForce = m_walkable ? m_maxGroundForce : 0.0f; + } + } + else + { + maxForce = m_maxAirForce; + } + + b3MoverJoint_SetMaxVelocityForce( m_moverJointId, maxForce ); + + float maxSpeed = speedScale * m_maxSpeed; + b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right; + desiredVelocity.y = 0.0f; + + float desiredSpeed; + b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); + + if ( desiredSpeed > maxSpeed ) + { + desiredSpeed = maxSpeed; + } + + // Accelerate + float currentSpeed = b3Dot( m_velocity, desiredDirection ); + float addSpeed = desiredSpeed - currentSpeed; + if ( addSpeed > 0.0f ) + { + float steer = m_walkable ? 1.0f : m_airSteer; + float accelSpeed = steer * m_accelerate * maxSpeed * timeStep; + if ( accelSpeed > addSpeed ) + { + accelSpeed = addSpeed; + } + + m_velocity.x += accelSpeed * desiredDirection.x; + m_velocity.z += accelSpeed * desiredDirection.z; + } + + if ( m_castResult.hit == true && b3IsNormalized( m_castResult.normal ) ) + { + m_pogoLength = m_castResult.fraction * rayLength; + + float moverMass = b3Body_GetMass( m_moverId ); + float gravity = b3Length( b3World_GetGravity( m_worldId ) ); + float moverWeight = m_gravityScale * gravity * moverMass; + + bool pogoJointValid = b3Joint_IsValid( m_pogoJointId ); + + // Create or recreate the pogo joint as needed. + if ( B3_ID_EQUALS( m_castResult.bodyId, m_surfaceBodyId ) == false || + pogoJointValid == false ) + { + if ( pogoJointValid ) + { + m_pogoImpulse = b3PogoJoint_GetImpulse( m_pogoJointId ); + m_pogoVelocity = b3PogoJoint_GetCurrentSpeed( m_pogoJointId ); + + b3DestroyJoint( m_pogoJointId, false ); + m_pogoJointId = b3_nullJointId; + } + + m_surfaceBodyId = m_castResult.bodyId; + + b3PogoJointDef pogoDef = b3DefaultPogoJointDef(); + pogoDef.base.localFrameA.p = + b3Body_GetLocalPoint( m_surfaceBodyId, m_castResult.point ); + pogoDef.base.localFrameB.p = m_capsule.center1 + pogoOffset; + pogoDef.normal = m_castResult.normal; + pogoDef.base.bodyIdA = m_surfaceBodyId; + pogoDef.base.bodyIdB = m_moverId; + pogoDef.base.collideConnected = true; + pogoDef.restLength = m_pogoRestLength; + pogoDef.hertz = m_pogoHertz; + pogoDef.dampingRatio = m_pogoDampingRatio; + pogoDef.maxCompressionForce = m_pogoCompressionScale * moverWeight; + + // Warm start from the joint that was just destroyed + pogoDef.impulse = m_pogoImpulse; + pogoDef.velocity = m_pogoVelocity; + + if ( m_jumping ) + { + // Don't allow the pogo to pull down + pogoDef.maxTensionForce = 0.0f; + } + else + { + // The pogo can pull down at a multiple of the gravity force. + pogoDef.maxTensionForce = m_pogoTensionScale * moverWeight; + } + + m_pogoJointId = b3CreatePogoJoint( m_worldId, &pogoDef ); + } + else + { + // Preserve the joint, improving performance. + + b3Transform localFrameA; + localFrameA.p = b3Body_GetLocalPoint( m_surfaceBodyId, m_castResult.point ); + localFrameA.q = b3Quat_identity; + b3Joint_SetLocalFrameA( m_pogoJointId, localFrameA ); + + b3Transform localFrameB; + localFrameB.p = m_capsule.center1 + pogoOffset; + localFrameB.q = b3Quat_identity; + b3Joint_SetLocalFrameB( m_pogoJointId, localFrameB ); + + b3PogoJoint_SetNormal( m_pogoJointId, m_castResult.normal ); + + if ( m_jumping ) + { + // Don't allow the pogo to pull down + b3PogoJoint_SetMaxTensionForce( m_pogoJointId, 0.0f ); + } + else + { + // The pogo can pull down at a multiple of the gravity force. + b3PogoJoint_SetMaxTensionForce( m_pogoJointId, m_pogoTensionScale * moverWeight ); + } + + b3PogoJoint_SetMaxCompressionForce( m_pogoJointId, + m_pogoCompressionScale * moverWeight ); + + b3PogoJoint_SetSpringHertz( m_pogoJointId, m_pogoHertz ); + b3PogoJoint_SetSpringDampingRatio( m_pogoJointId, m_pogoDampingRatio ); + } + } + else + { + if ( b3Joint_IsValid( m_pogoJointId ) ) + { + b3DestroyJoint( m_pogoJointId, false ); + m_pogoJointId = b3_nullJointId; + } + + m_surfaceBodyId = b3_nullBodyId; + m_pogoImpulse = 0.0f; + m_pogoVelocity = 0.0f; + m_pogoLength = 0.0f; + } + + b3MoverJoint_SetLinearVelocity( m_moverJointId, m_velocity ); +} + +void DynamicMover::Draw() const +{ + for ( int i = 0; i < m_probeCount; ++i ) + { + const DynamicMoverProbe& probe = m_probes[i]; + b3Pos end = b3OffsetPos( probe.origin, probe.fraction * probe.translation ); + + if ( probe.hit == false ) + { + DrawLine( probe.origin, end, MakeColor( b3_colorGray ) ); + continue; + } + + b3HexColor color = i == m_selectedProbe ? b3_colorGreen : b3_colorYellow; + DrawLine( probe.origin, end, MakeColor( color ) ); + DrawPoint( end, 6.0f, MakeColor( color ) ); + } +} diff --git a/samples/dynamic_mover.h b/samples/dynamic_mover.h new file mode 100644 index 0000000..fcb1ed1 --- /dev/null +++ b/samples/dynamic_mover.h @@ -0,0 +1,167 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/types.h" + +struct DynamicMoverDef +{ + b3Pos position = {}; + b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f }; + + float density = 200.0f; + + b3Filter filter = b3DefaultFilter(); + float jumpSpeed = 7.0f; + float maxSpeed = 6.0f; + float minSpeed = 0.1f; + float stopSpeed = 3.0f; + float accelerate = 20.0f; + float airSteer = 0.25f; + float friction = 8.0f; + float gravityScale = 1.5f; + float maxGroundForce = 6000.0f; + float maxAirForce = 1500.0f; + float pogoRestLength = 0.9f; + float pogoHertz = 3.0f; + float pogoDampingRatio = 0.8f; + float pogoCompressionScale = 100.0f; + float pogoTensionScale = 100.0f; + uint64_t platformMaterialId = 0; + bool enableSkirtPogo = true; + bool enablePreSolveEvents = false; + bool enableSensorEvents = false; +}; + +struct DynamicMoverCastResult +{ + b3Pos point; + b3Vec3 normal; + b3BodyId bodyId; + uint64_t userMaterialId; + float fraction; + bool hit; +}; + +struct DynamicMoverProbe +{ + b3Pos origin; + b3Vec3 translation; + float fraction; + bool hit; +}; + +// Dynamic character mover. The capsule is a rigid body with locked rotation, so it +// collides and is pushed like everything else in the world. A mover joint drives it to +// the target velocity and a pogo joint holds it above the ground so it can climb +// steps without touching the capsule. +// +// The dynamic mover should be vendored into your project so you can alter it to meet +// your game's specific needs. +// +// This system is inspired by Tom Waterson's presentation: +// https://www.youtube.com/watch?v=_jRLlTDqoGI +class DynamicMover +{ +public: + DynamicMover(); + + void Create( b3WorldId worldId, const DynamicMoverDef* def ); + void Destroy(); + + // Advance the mover. Throttle is the input along the forward and right directions on [-1, 1]. + void Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, float speedScale ); + + // Jump if grounded. Returns false when airborne. + bool Jump(); + + // Release the ground so an external impulse can carry the mover into the air. Otherwise the + // pogo pulls it back down. + void Launch(); + + // Gravity scale is used to size the pogo forces, so change it here. + void SetGravityScale( float gravityScale ); + + b3BodyId GetBodyId() const + { + return m_moverId; + } + + void Draw() const; + + static constexpr int m_skirtCount = 8; + static constexpr int m_probeCapacity = m_skirtCount + 1; + + float m_jumpSpeed; + float m_maxSpeed; + float m_minSpeed; + float m_stopSpeed; + float m_accelerate; + + // Fraction of the ground acceleration available in the air. + float m_airSteer; + + // Ground friction in units of 1/time. + float m_friction; + + // A character usually wants to fall faster than the rest of the world. + float m_gravityScale; + + // Force budget the mover joint has for reaching the target velocity. The air value + // is small so the character keeps its momentum while jumping. + float m_maxGroundForce; + float m_maxAirForce; + + float m_pogoRestLength; + float m_pogoHertz; + float m_pogoDampingRatio; + + // Pogo force limits as a multiple of the mover weight. + float m_pogoCompressionScale; + float m_pogoTensionScale; + + // Casts an octagon skirt of rays around the capsule when the central ray misses or + // lands on a surface too steep to walk, so the mover can stand on a ledge or a thin + // rail, or step off the foot of a steep slope. + bool m_enableSkirtPogo; + + // Used for the pogo to detect moving platforms. + uint64_t m_platformMaterialId; + + // Surfaces steeper than this are not ground. + float m_minGroundNormalY; + + b3Capsule m_capsule; + b3Filter m_filter; + + b3WorldId m_worldId; + b3BodyId m_groundId; + b3BodyId m_moverId; + b3BodyId m_surfaceBodyId; + b3JointId m_moverJointId; + b3JointId m_pogoJointId; + + DynamicMoverCastResult m_castResult; + + b3Vec3 m_velocity; + float m_timeSliding; + float m_slideDuration; + bool m_onGround; + bool m_walkable; + bool m_jumping; + int m_jumpTicks; + + // Pogo joint state, carried across the joint being recreated each update. + float m_pogoImpulse; + float m_pogoVelocity; + float m_pogoLength; + + // Pogo ray cast results for drawing. + DynamicMoverProbe m_probes[m_probeCapacity]; + int m_probeCount; + int m_selectedProbe; + +private: + void CreateMoverJoint( b3BodyId baseId ); +}; diff --git a/samples/geometric_mover.cpp b/samples/geometric_mover.cpp new file mode 100644 index 0000000..69f159a --- /dev/null +++ b/samples/geometric_mover.cpp @@ -0,0 +1,351 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "geometric_mover.h" + +#include "gfx/draw.h" + +#include "box3d/box3d.h" +#include "box3d/math_functions.h" + +#include +#include + +static bool MoverFilterCallback( b3ShapeId shapeId, void* context ) +{ + GeometricMover* self = (GeometricMover*)context; + for ( int i = 0; i < self->m_ignoreCount; ++i ) + { + if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) ) + { + return false; + } + } + + return true; +} + +static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount, + void* context ) +{ + if ( MoverFilterCallback( shapeId, context ) == false ) + { + // ignore these planes but continue looking for more + return true; + } + + GeometricMover* self = static_cast( context ); + float maxPush = FLT_MAX; + bool clipVelocity = true; + MoverShapeUserData* userData = + static_cast( b3Shape_GetUserData( shapeId ) ); + if ( userData != nullptr ) + { + maxPush = userData->maxPush; + clipVelocity = userData->clipVelocity; + } + + for ( int i = 0; i < planeCount && self->m_planeCount < GeometricMover::m_planeCapacity; ++i ) + { + assert( b3IsValidPlane( planeResults[i].plane ) ); + self->m_planes[self->m_planeCount] = { + .plane = planeResults[i].plane, + .pushLimit = maxPush, + .push = 0.0f, + .clipVelocity = clipVelocity, + }; + + // The plane point is relative to the origin passed to the collide call + self->m_planeExtras[self->m_planeCount] = { + .point = b3OffsetPos( self->m_position, planeResults[i].point ), + .shapeId = shapeId, + }; + self->m_planeCount += 1; + } + + return true; +} + +GeometricMover::GeometricMover() +{ + GeometricMoverDef def; + m_jumpSpeed = def.jumpSpeed; + m_maxSpeed = def.maxSpeed; + m_minSpeed = def.minSpeed; + m_stopSpeed = def.stopSpeed; + m_accelerate = def.accelerate; + m_friction = def.friction; + m_gravity = def.gravity; + m_pogoRestLength = def.pogoRestLength; + m_pogoHertz = def.pogoHertz; + m_pogoDampingRatio = def.pogoDampingRatio; + m_clipVelocity = true; + m_ignoreShapeIds = nullptr; + m_ignoreCount = 0; + + m_capsule = def.capsule; + m_worldId = b3_nullWorldId; + m_position = def.position; + m_velocity = b3Vec3_zero; + m_pogoVelocity = 0.0f; + m_onGround = false; + + m_planeCount = 0; + m_totalIterations = 0; + + m_pogoOrigin = b3Pos_zero; + m_pogoTranslation = b3Vec3_zero; + m_pogoPoint = b3Pos_zero; + m_pogoHit = false; +} + +void GeometricMover::Create( b3WorldId worldId, const GeometricMoverDef* def ) +{ + m_capsule = def->capsule; + m_jumpSpeed = def->jumpSpeed; + m_maxSpeed = def->maxSpeed; + m_minSpeed = def->minSpeed; + m_stopSpeed = def->stopSpeed; + m_accelerate = def->accelerate; + m_friction = def->friction; + m_gravity = def->gravity; + m_pogoRestLength = def->pogoRestLength; + m_pogoHertz = def->pogoHertz; + m_pogoDampingRatio = def->pogoDampingRatio; + + m_worldId = worldId; + m_position = def->position; + m_velocity = b3Vec3_zero; + m_pogoVelocity = 0.0f; + m_onGround = false; + m_planeCount = 0; + m_totalIterations = 0; + m_pogoHit = false; +} + +bool GeometricMover::Jump() +{ + if ( m_onGround == false ) + { + return false; + } + + m_velocity.y = m_jumpSpeed; + m_onGround = false; + return true; +} + +// Solve a normal constraint between the mover and each touched rigid body. This is the +// same velocity constraint the solver uses for a contact, except the mover is treated as +// having infinite mass, so all of the impulse lands on the body. +void GeometricMover::PushBodies() +{ + for ( int i = 0; i < m_planeCount; ++i ) + { + b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId ); + if ( b3Body_GetType( bodyId ) != b3_dynamicBody ) + { + continue; + } + + MoverShapeUserData* userData = + static_cast( b3Shape_GetUserData( m_planeExtras[i].shapeId ) ); + if ( userData != nullptr && userData->canMoverPush == false ) + { + continue; + } + + // Plane normals point at the mover + b3Pos point = m_planeExtras[i].point; + b3Vec3 normal = b3Neg( m_planes[i].plane.normal ); + + float invMassA = 0.0f; + float invMassB = b3Body_GetInverseMass( bodyId ); + b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId ); + + b3Pos centerB = b3Body_GetWorldCenter( bodyId ); + b3Vec3 rB = b3SubPos( point, centerB ); + + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) ); + float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + b3Vec3 vB = b3Body_GetLinearVelocity( bodyId ); + b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId ); + b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) ); + float vn = b3Dot( b3Sub( vrB, m_velocity ), normal ); + + // Push only, a separating body is left alone + float impulse = b3MaxFloat( -normalMass * vn, 0.0f ); + + b3Vec3 P = b3MulSV( impulse, normal ); + m_velocity = b3MulSub( m_velocity, invMassA, P ); + + b3Body_ApplyLinearImpulse( bodyId, P, point, true ); + } +} + +// Movement follows the Quake ground and air model: +// https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390 +void GeometricMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, + float speedScale ) +{ + // Friction + float speed = b3Length( m_velocity ); + if ( speed < m_minSpeed ) + { + m_velocity.x = 0.0f; + m_velocity.z = 0.0f; + } + else + { + // Linear damping above stopSpeed and fixed reduction below stopSpeed + float control = speed < m_stopSpeed ? m_stopSpeed : speed; + + // friction has units of 1/time + float drop = control * m_friction * timeStep; + float newSpeed = b3MaxFloat( 0.0f, speed - drop ); + float ratio = newSpeed / speed; + m_velocity.x *= ratio; + m_velocity.z *= ratio; + } + + float maxSpeed = speedScale * m_maxSpeed; + + b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right; + float desiredSpeed; + b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); + + if ( desiredSpeed > maxSpeed ) + { + desiredSpeed = maxSpeed; + } + + if ( m_onGround ) + { + m_velocity.y = 0.0f; + } + + // Accelerate + float currentSpeed = b3Dot( m_velocity, desiredDirection ); + float addSpeed = desiredSpeed - currentSpeed; + if ( addSpeed > 0.0f ) + { + float accelSpeed = m_accelerate * maxSpeed * timeStep; + if ( accelSpeed > addSpeed ) + { + accelSpeed = addSpeed; + } + + m_velocity += accelSpeed * desiredDirection; + } + + m_velocity.y -= m_gravity * timeStep; + + // The probe reaches past the rest length so the spring can be stretched + float rayLength = m_pogoRestLength + m_capsule.radius; + b3Pos rayOrigin = m_position + m_capsule.center1; + b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY; + b3QueryFilter pogoFilter = { 1, ~2ull, 0, "pogo" }; + b3RayResult rayResult = + b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, pogoFilter ); + + m_pogoOrigin = rayOrigin; + m_pogoTranslation = rayTranslation; + m_pogoPoint = rayResult.point; + + // After gravity was applied, disable pogo when still moving up. + // Avoids getting pulled back to the ground when jumping. + bool suppressPogo = m_velocity.y > 0.0f; + + if ( rayResult.hit == false || suppressPogo ) + { + m_onGround = false; + m_pogoVelocity = 0.0f; + } + else + { + m_onGround = true; + float pogoCurrentLength = rayResult.fraction * rayLength; + float offset = pogoCurrentLength - m_pogoRestLength; + m_pogoVelocity = + b3SpringDamper( m_pogoHertz, m_pogoDampingRatio, offset, m_pogoVelocity, timeStep ); + } + + m_pogoHit = m_onGround; + + b3Pos startPosition = m_position; + b3Pos target = m_position + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY; + + // Want the mover to collide with allies + b3QueryFilter moverFilter = { + .categoryBits = 1, .maskBits = ~0ull, .id = 1, .name = "mover_collide" }; + + // The cast should ignore allies + b3QueryFilter castFilter = { + .categoryBits = 1, .maskBits = ~2ull, .id = 1, .name = "mover_cast" }; + + m_totalIterations = 0; + float tolerance = 0.01f; + + for ( int iteration = 0; iteration < 5; ++iteration ) + { + m_planeCount = 0; + + b3World_CollideMover( m_worldId, m_position, &m_capsule, moverFilter, PlaneResultFcn, + this ); + + b3Vec3 targetDelta = target - m_position; + b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount ); + + m_totalIterations += result.iterationCount; + + float fraction = b3World_CastMover( m_worldId, m_position, &m_capsule, result.delta, + castFilter, MoverFilterCallback, this ); + + b3Vec3 delta = fraction * result.delta; + m_position = m_position + delta; + + if ( b3LengthSquared( delta ) < tolerance * tolerance ) + { + break; + } + } + + PushBodies(); + + if ( m_clipVelocity ) + { + // Stop pushing into surfaces so speed can't accumulate against a wall + m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount ); + } + else if ( timeStep > 0.0f ) + { + m_velocity = ( 1.0f / timeStep ) * ( m_position - startPosition ); + } +} + +void GeometricMover::Draw() const +{ + if ( m_pogoHit ) + { + DrawLine( m_pogoOrigin, m_pogoPoint, MakeColor( b3_colorGreen ) ); + } + else + { + DrawLine( m_pogoOrigin, b3OffsetPos( m_pogoOrigin, m_pogoTranslation ), + MakeColor( b3_colorGray ) ); + } + + for ( int i = 0; i < m_planeCount; ++i ) + { + b3Plane plane = m_planes[i].plane; + b3Pos p1 = m_position + ( plane.offset - m_capsule.radius ) * plane.normal; + b3Pos p2 = p1 + 0.1f * plane.normal; + DrawPoint( p1, 5.0f, MakeColor( b3_colorYellow ) ); + DrawLine( p1, p2, MakeColor( b3_colorYellow ) ); + } + + DrawSolidCapsule( { m_position, b3Quat_identity }, m_capsule, MakeColor( b3_colorBlue ) ); + DrawLine( m_position, m_position + m_velocity, MakeColor( b3_colorPurple ) ); +} diff --git a/samples/geometric_mover.h b/samples/geometric_mover.h new file mode 100644 index 0000000..e8adac1 --- /dev/null +++ b/samples/geometric_mover.h @@ -0,0 +1,114 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/collision.h" +#include "box3d/types.h" + +struct GeometricMoverDef +{ + b3Pos position = {}; + b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f }; + float jumpSpeed = 5.0f; + float maxSpeed = 6.0f; + float minSpeed = 0.01f; + float stopSpeed = 1.0f; + float accelerate = 30.0f; + float friction = 4.0f; + float gravity = 15.0f; + float pogoRestLength = 0.9f; + float pogoHertz = 4.0f; + float pogoDampingRatio = 0.7f; +}; + +// Optional shape user data read by GeometricMover when it gathers collision planes. +// Shapes without user data are treated as rigid, fully clipping, and pushable. +struct MoverShapeUserData +{ + // Distance limit on the push this surface may apply to the mover. FLT_MAX is rigid. + float maxPush; + + // False keeps the velocity along this surface, which is what soft collision needs. + bool clipVelocity; + + // False stops the mover from shoving this body. + bool canMoverPush; +}; + +// Extra data kept alongside each collision plane so the mover can push rigid bodies. +struct PlaneExtra +{ + b3Pos point; + b3ShapeId shapeId; +}; + +// Geometric character mover. The capsule is moved by casting and by solving collision +// planes, so the game keeps full control of the motion. +// +// A pogo spring holds the capsule above the ground so it can move up steps. +class GeometricMover +{ +public: + GeometricMover(); + + void Create( b3WorldId worldId, const GeometricMoverDef* def ); + + // Advance the mover. Throttle is the input along the forward and right directions on [-1, 1]. + void Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, float speedScale ); + + // Jump if grounded. Returns false when airborne. + bool Jump(); + + void Draw() const; + + static constexpr int m_planeCapacity = 8; + + float m_jumpSpeed; + float m_maxSpeed; + float m_minSpeed; + float m_stopSpeed; + float m_accelerate; + + // Friction in units of 1/time. + float m_friction; + + // The mover is not simulated so it needs its own gravity. + float m_gravity; + + float m_pogoRestLength; + float m_pogoHertz; + float m_pogoDampingRatio; + + // The clipper avoids picking up velocity from soft collision depenetration. Otherwise the + // velocity comes from the position delta, which is more intuitive in some cases. + bool m_clipVelocity; + + // Shapes the mover passes through. Set before calling Update. + const b3ShapeId* m_ignoreShapeIds; + int m_ignoreCount; + + // The capsule is relative to the mover position. + b3Capsule m_capsule; + + b3WorldId m_worldId; + + b3Pos m_position; + b3Vec3 m_velocity; + float m_pogoVelocity; + bool m_onGround; + + // Planes from the last solve, kept for debug drawing. + b3CollisionPlane m_planes[m_planeCapacity] = {}; + PlaneExtra m_planeExtras[m_planeCapacity] = {}; + int m_planeCount; + int m_totalIterations; + + b3Pos m_pogoOrigin; + b3Vec3 m_pogoTranslation; + b3Pos m_pogoPoint; + bool m_pogoHit; + +private: + void PushBodies(); +}; diff --git a/samples/gfx/debug_adapter.c b/samples/gfx/debug_adapter.c index b2080e4..66ffd38 100644 --- a/samples/gfx/debug_adapter.c +++ b/samples/gfx/debug_adapter.c @@ -122,6 +122,10 @@ typedef struct bool transparentDynamic; bool transparentKinematic; + // One body drawn with reduced alpha, such as a character the camera is close to + b3BodyId alphaBodyId; + float bodyAlpha; + // View box for compound child culling, world space, refreshed once per frame. b3AABB viewBounds; bool hasViewBounds; @@ -158,6 +162,8 @@ void InitAdapter( void ) s_adapter.selectedShapeId = b3_nullShapeId; s_adapter.transparentDynamic = false; s_adapter.transparentKinematic = false; + s_adapter.alphaBodyId = b3_nullBodyId; + s_adapter.bodyAlpha = 1.0f; s_adapter.hasViewBounds = false; s_adapter.lastCompoundAppended = 0; @@ -197,6 +203,8 @@ void ResetAdapterPool( void ) s_adapter.selectedShapeId = b3_nullShapeId; s_adapter.transparentDynamic = false; s_adapter.transparentKinematic = false; + s_adapter.alphaBodyId = b3_nullBodyId; + s_adapter.bodyAlpha = 1.0f; s_adapter.hasViewBounds = false; s_adapter.lastCompoundAppended = 0; @@ -239,6 +247,12 @@ void SetTransparentKinematic( bool enabled ) s_adapter.transparentKinematic = enabled; } +void SetBodyAlpha( b3BodyId bodyId, float alpha ) +{ + s_adapter.alphaBodyId = bodyId; + s_adapter.bodyAlpha = alpha; +} + void SetViewBounds( b3AABB bounds ) { s_adapter.viewBounds = bounds; @@ -862,6 +876,11 @@ static void DrawShape( void* userShape, b3WorldTransform shapeTransform, b3HexCo c.w = BOX3D_TRANSPARENT_ALPHA; } + if ( s_adapter.bodyAlpha < 1.0f && B3_ID_EQUALS( us->bodyId, s_adapter.alphaBodyId ) ) + { + c.w = b3MinFloat( c.w, s_adapter.bodyAlpha ); + } + if ( s_adapter.transparentKinematic && us->bodyType == b3_kinematicBody ) { c.w = BOX3D_TRANSPARENT_ALPHA; diff --git a/samples/gfx/debug_adapter.h b/samples/gfx/debug_adapter.h index 44eb0f6..5d56af4 100644 --- a/samples/gfx/debug_adapter.h +++ b/samples/gfx/debug_adapter.h @@ -42,6 +42,7 @@ void SetGroundShape( b3ShapeId shapeId ); void SetShapeMaterial( b3ShapeId shapeId, Vec4 color, float metallic, float roughness ); void SetTransparentDynamic( bool enabled ); void SetTransparentKinematic( bool enabled ); +void SetBodyAlpha( b3BodyId bodyId, float alpha ); // View box for compound child culling, world space. Set once per frame before // b3World_Draw. A compound then submits only the children overlapping this box diff --git a/samples/gfx/keycodes.h b/samples/gfx/keycodes.h index a2a62c8..48c81d0 100644 --- a/samples/gfx/keycodes.h +++ b/samples/gfx/keycodes.h @@ -16,6 +16,7 @@ enum KEY_D = SAPP_KEYCODE_D, KEY_F = SAPP_KEYCODE_F, KEY_G = SAPP_KEYCODE_G, + KEY_K = SAPP_KEYCODE_K, KEY_L = SAPP_KEYCODE_L, KEY_M = SAPP_KEYCODE_M, KEY_O = SAPP_KEYCODE_O, @@ -32,6 +33,9 @@ enum KEY_ESCAPE = SAPP_KEYCODE_ESCAPE, KEY_TAB = SAPP_KEYCODE_TAB, KEY_LEFT_SHIFT = SAPP_KEYCODE_LEFT_SHIFT, + KEY_RIGHT_SHIFT = SAPP_KEYCODE_RIGHT_SHIFT, + KEY_LEFT_CONTROL = SAPP_KEYCODE_LEFT_CONTROL, + KEY_RIGHT_CONTROL = SAPP_KEYCODE_RIGHT_CONTROL, KEY_LEFT_BRACKET = SAPP_KEYCODE_LEFT_BRACKET, KEY_RIGHT_BRACKET = SAPP_KEYCODE_RIGHT_BRACKET, KEY_PAUSE = SAPP_KEYCODE_PAUSE, diff --git a/samples/host/camera.cpp b/samples/host/camera.cpp index c59a256..acafcb6 100644 --- a/samples/host/camera.cpp +++ b/samples/host/camera.cpp @@ -12,7 +12,6 @@ static constexpr float ORBIT_SENS = 0.005f; // radians per pixel static constexpr float FLY_LOOK_SENS = 0.005f; // radians per pixel static constexpr float PAN_SENS = 0.005f; // meters per pixel per meter of radius static constexpr float RADIAL_ZOOM_SENS = 0.02f; // meters per pixel (alt+right-drag) -static constexpr float FLY_SPEED_STEP = 1.0f; // m/s per scroll tick static constexpr float ZOOM_STEP = 0.9f; // multiplier per scroll tick static constexpr float MIN_DIST = 0.1f; static constexpr float MIN_SPEED = 0.06f; // matches box3d's 0.001*60 @@ -502,7 +501,7 @@ void Camera::Update( float dt, int width, int height ) if ( m_speedScrollAccum != 0.0f ) { - m_speed += m_speedScrollAccum * FLY_SPEED_STEP; + m_speed *= powf( 1.1f, m_speedScrollAccum ); m_speed = b3ClampFloat( m_speed, MIN_SPEED, MAX_SPEED ); } } diff --git a/samples/main.cpp b/samples/main.cpp index a92df26..9cc23f1 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -14,6 +14,7 @@ #include "gfx/debug_adapter.h" #include "gfx/keycodes.h" #include "gfx/renderer.h" +#include "gfx/text.h" #include "host/gui.h" #include "sample.h" #include "sokol_app.h" @@ -160,6 +161,17 @@ static void OnEvent( const sapp_event* e ) s_context.camera.OnEvent( e ); } + // Releases go to the key table even when the UI captures them. Otherwise Ctrl let go + // while the Ctrl+O picker is open stays down in the next sample. + if ( e->type == SAPP_EVENTTYPE_KEY_UP ) + { + SetKeyDown( e->key_code, false ); + } + else if ( e->type == SAPP_EVENTTYPE_UNFOCUSED ) + { + ClearKeysDown(); + } + if ( uiCaptured ) { return; @@ -181,7 +193,18 @@ static void OnEvent( const sapp_event* e ) switch ( e->key_code ) { case SAPP_KEYCODE_TAB: - s_context.showUI = !s_context.showUI; + if ( mods & SAPP_MODIFIER_SHIFT ) + { + bool hide = s_context.showUI || s_context.showText; + s_context.showUI = !hide; + s_context.showText = !hide; + } + else + { + // Plain Tab also restores text so it always leads back to the full view + s_context.showUI = !s_context.showUI; + s_context.showText = true; + } break; case SAPP_KEYCODE_ESCAPE: @@ -267,10 +290,6 @@ static void OnEvent( const sapp_event* e ) } break; - case SAPP_EVENTTYPE_KEY_UP: - SetKeyDown( e->key_code, false ); - break; - case SAPP_EVENTTYPE_CHAR: // ? toggles the controls window. Read the typed character so it works // on any layout and a text field can still type a literal ?, since the @@ -406,6 +425,12 @@ static void OnFrame( void ) s_context.sample->Step(); s_context.sample->Render(); + // Before RenderFrame so world labels drop too + if ( s_context.showText == false ) + { + ResetTextArena(); + } + FrameInput fi{}; fi.view = camera.View(); fi.viewInv = camera.ViewInverse(); diff --git a/samples/mover.cpp b/samples/mover.cpp deleted file mode 100644 index 4db9a13..0000000 --- a/samples/mover.cpp +++ /dev/null @@ -1,353 +0,0 @@ -// SPDX-FileCopyrightText: 2026 Erin Catto -// SPDX-License-Identifier: MIT - -#include "mover.h" - -#include "gfx/draw.h" -#include "sample.h" - -#include "box3d/box3d.h" - -#include -#include - -static bool MoverFilterCallback( b3ShapeId shapeId, void* context ) -{ - CharacterMover* self = (CharacterMover*)context; - for ( int i = 0; i < self->m_ignoreCount; ++i ) - { - if ( B3_ID_EQUALS( shapeId, self->m_ignoreShapeIds[i] ) ) - { - return false; - } - } - - return true; -} - -void CharacterMover::Initialize( Sample* sample, b3Pos position ) -{ - m_sample = sample; - m_transform.p = position; - m_transform.q = b3Quat_identity; - m_velocity = { 0.0f, 0.0f, 0.0f }; - m_capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f }; - - m_planeCount = 0; - m_totalIterations = 0; - m_pogoVelocity = 0.0f; - m_onGround = false; - m_sprint = false; - - m_ignoreShapeIds = nullptr; - m_ignoreCount = 0; -} - -static bool PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* planeResults, int planeCount, void* context ) -{ - if ( MoverFilterCallback( shapeId, context ) == false ) - { - // ignore these planes but continue looking for more - return true; - } - - CharacterMover* self = static_cast( context ); - float maxPush = FLT_MAX; - bool clipVelocity = true; - MoverShapeUserData* userData = static_cast( (void*)b3Shape_GetUserData( shapeId ) ); - if ( userData != nullptr ) - { - maxPush = userData->maxPush; - clipVelocity = userData->clipVelocity; - } - - for ( int i = 0; i < planeCount && self->m_planeCount < CharacterMover::m_planeCapacity; ++i ) - { - assert( b3IsValidPlane( planeResults[i].plane ) ); - self->m_planes[self->m_planeCount] = { - .plane = planeResults[i].plane, - .pushLimit = maxPush, - .push = 0.0f, - .clipVelocity = clipVelocity, - }; - self->m_planeExtras[self->m_planeCount] = { - .point = b3OffsetPos( self->m_transform.p, planeResults[i].point ), - .shapeId = shapeId, - }; - self->m_planeCount += 1; - } - - return true; -} - -void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity ) -{ - // Friction - float speed = b3Length( m_velocity ); - if ( speed < m_minSpeed ) - { - m_velocity.x = 0.0f; - m_velocity.z = 0.0f; - } - else - { - // Linear damping above stopSpeed and fixed reduction below stopSpeed - float control = speed < m_stopSpeed ? m_stopSpeed : speed; - - // friction has units of 1/time - float drop = control * m_friction * timeStep; - float newSpeed = b3MaxFloat( 0.0f, speed - drop ); - float ratio = newSpeed / speed; - m_velocity.x *= ratio; - m_velocity.z *= ratio; - } - - float maxSpeed = m_sprint ? 1.5f * m_maxSpeed : m_maxSpeed; - - b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right; - float desiredSpeed; - b3Vec3 desiredDirection = b3GetLengthAndNormalize( &desiredSpeed, desiredVelocity ); - - if ( desiredSpeed > maxSpeed ) - { - desiredVelocity *= maxSpeed / desiredSpeed; - desiredSpeed = maxSpeed; - } - - if ( m_onGround ) - { - m_velocity.y = 0.0f; - } - - // Accelerate - float currentSpeed = b3Dot( m_velocity, desiredDirection ); - float addSpeed = desiredSpeed - currentSpeed; - if ( addSpeed > 0.0f ) - { - float accelSpeed = m_accelerate * maxSpeed * timeStep; - if ( accelSpeed > addSpeed ) - { - accelSpeed = addSpeed; - } - - m_velocity += accelSpeed * desiredDirection; - } - - m_velocity.y -= m_gravity * timeStep; - - b3WorldId worldId = m_sample->m_worldId; - - float pogoRestLength = 3.0f * m_capsule.radius; - float rayLength = pogoRestLength + m_capsule.radius; - b3Pos rayOrigin = b3TransformWorldPoint( m_transform, m_capsule.center1 ); - b3Vec3 rayTranslation = -rayLength * b3Vec3_axisY; - b3QueryFilter skipTeamFilter = { 1, ~2u }; - skipTeamFilter.name = "pogo"; - b3RayResult rayResult = b3World_CastRayClosest( worldId, rayOrigin, rayTranslation, skipTeamFilter ); - - // After gravity was applied, disable pogo when still moving up. - // Avoids getting pulled back to the ground when jumping. - bool suppressPogo = m_velocity.y > 0.0f; - - if ( rayResult.hit == false || suppressPogo ) - { - m_onGround = false; - m_pogoVelocity = 0.0f; - - DrawLine( rayOrigin, b3OffsetPos( rayOrigin, rayTranslation ), MakeColor( b3_colorGray ) ); - } - else - { - m_onGround = true; - float pogoCurrentLength = rayResult.fraction * rayLength; - - float zeta = 0.7f; - float hertz = 4.0f; - float omega = 2.0f * B3_PI * hertz; - float omegaH = omega * timeStep; - - m_pogoVelocity = ( m_pogoVelocity - omega * omegaH * ( pogoCurrentLength - pogoRestLength ) ) / - ( 1.0f + 2.0f * zeta * omegaH + omegaH * omegaH ); - DrawLine( rayOrigin, rayResult.point, MakeColor( b3_colorGreen ) ); - } - - b3Pos startPosition = m_transform.p; - b3Pos target = m_transform.p + timeStep * m_velocity + timeStep * m_pogoVelocity * b3Vec3_axisY; - - // Want the mover to collide with allies - b3QueryFilter moverFilter = { .categoryBits = 1, .maskBits = ~0u, .id = 1, .name = "mover_collide" }; - - // The cast should ignore allies - b3QueryFilter castFilter = { .categoryBits = 1, .maskBits = ~2u, .id = 1, .name = "mover_cast" }; - - m_totalIterations = 0; - float tolerance = 0.01f; - - for ( int iteration = 0; iteration < 5; ++iteration ) - { - m_planeCount = 0; - - b3Capsule mover; - mover.center1 = m_capsule.center1; - mover.center2 = m_capsule.center2; - mover.radius = m_capsule.radius; - - b3World_CollideMover( worldId, m_transform.p, &mover, moverFilter, PlaneResultFcn, this ); - - b3Vec3 targetDelta = target - m_transform.p; - b3PlaneSolverResult result = b3SolvePlanes( targetDelta, m_planes, m_planeCount ); - - m_totalIterations += result.iterationCount; - - b3Vec3 delta = result.delta; - - float fraction = b3World_CastMover( worldId, m_transform.p, &mover, delta, castFilter, MoverFilterCallback, this ); - - delta *= fraction; - m_transform.p = m_transform.p + delta; - - if ( b3LengthSquared( delta ) < tolerance * tolerance ) - { - break; - } - } - - for ( int i = 0; i < m_planeCount; ++i ) - { - b3BodyId bodyId = b3Shape_GetBody( m_planeExtras[i].shapeId ); - b3BodyType bodyType = b3Body_GetType( bodyId ); - if ( bodyType != b3_dynamicBody ) - { - continue; - } - - MoverShapeUserData* userData = static_cast( b3Shape_GetUserData( m_planeExtras[i].shapeId ) ); - if ( userData != nullptr && userData->canMoverPush == false ) - { - continue; - } - - b3Pos point = m_planeExtras[i].point; - b3Vec3 normal = b3Neg( m_planes[i].plane.normal ); - - float invMassA = 0.0f; - float invMassB = b3Body_GetInverseMass( bodyId ); - b3Matrix3 invIB = b3Body_GetWorldInverseRotationalInertia( bodyId ); - - b3Pos pB = b3Body_GetWorldCenter( bodyId ); - b3Vec3 rB = b3SubPos( point, pB ); - - b3Vec3 rnB = b3Cross( rB, normal ); - float kNormal = invMassA + invMassB + b3Dot( rnB, b3MulMV( invIB, rnB ) ); - float normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - - b3Vec3 vB = b3Body_GetLinearVelocity( bodyId ); - b3Vec3 omegaB = b3Body_GetAngularVelocity( bodyId ); - b3Vec3 vrB = b3Add( vB, b3Cross( omegaB, rB ) ); - float vn = b3Dot( b3Sub( vrB, m_velocity ), normal ); - float impulse = b3MaxFloat( -normalMass * vn, 0.0f ); - - b3Vec3 P = b3MulSV( impulse, normal ); - m_velocity = b3MulSub( m_velocity, invMassA, P ); - - b3Body_ApplyLinearImpulse( bodyId, P, point, true ); - } - - if ( clipVelocity ) - { - // Using the velocity clipper can avoid picking up velocity from depenetration. - // This allows the mover to avoid velocity from soft collision depenetration. - m_velocity = b3ClipVector( m_velocity, m_planes, m_planeCount ); - } - else if ( timeStep > 0.0f ) - { - // Using the position delta is more holistic and intuitive in some cases. - m_velocity = ( 1.0f / timeStep ) * ( m_transform.p - startPosition ); - } -} - -void CharacterMover::Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity ) -{ - m_ignoreShapeIds = ignoreShapes; - m_ignoreCount = ignoreCount; - - b3Vec2 throttle = { 0.0f, 0.0f }; - b3Vec3 forward = -m_sample->m_camera->GetForward(); - b3Vec3 right = m_sample->m_camera->GetRight(); - - forward.y = 0.0f; - forward = b3Normalize( forward ); - - if ( m_sample->m_camera->m_thirdPerson ) - { - if ( IsKeyDown( KEY_W ) ) - { - throttle.x += 1.0f; - } - - if ( IsKeyDown( KEY_S ) ) - { - throttle.x -= 1.0f; - } - - if ( IsKeyDown( KEY_A ) ) - { - throttle.y -= 1.0f; - } - - if ( IsKeyDown( KEY_D ) ) - { - throttle.y += 1.0f; - } - - if ( IsKeyDown( KEY_SPACE ) && m_onGround == true ) - { - m_velocity.y = m_jumpSpeed; - m_onGround = false; - } - - if ( m_onGround == true ) - { - m_sprint = IsKeyDown( KEY_LEFT_SHIFT ); - } - else - { - m_sprint = false; - } - } - - float hertz = m_sample->m_context->hertz; - float timeStep = hertz > 0.0f ? 1.0f / hertz : 0.0f; - - // throttle = { 0.0f, 0.0f, -1.0f }; - - SolveMove( timeStep, forward, right, throttle, clipVelocity ); - - b3Pos position = m_transform.p; - - // Follow the mover and latch the draw origin before drawing, so the overlays below demote - // against the same eye the view renders from. - if ( m_sample->m_camera->m_thirdPerson ) - { - m_sample->m_camera->m_pivot = position; - m_sample->m_camera->UpdateTransform(); - } - - SetDrawOrigin( m_sample->m_camera->DrawOrigin() ); - - int count = m_planeCount; - for ( int i = 0; i < count; ++i ) - { - b3Plane plane = m_planes[i].plane; - b3Pos p1 = position + ( plane.offset - m_capsule.radius ) * plane.normal; - b3Pos p2 = p1 + 0.1f * plane.normal; - DrawPoint( p1, 5.0f, MakeColor( b3_colorYellow ) ); - DrawLine( p1, p2, MakeColor( b3_colorYellow ) ); - } - - DrawSolidCapsule( m_transform, m_capsule, MakeColor( b3_colorBlue ) ); - DrawLine( position, position + m_velocity, MakeColor( b3_colorPurple ) ); - - m_ignoreShapeIds = nullptr; - m_ignoreCount = 0; -} diff --git a/samples/mover.h b/samples/mover.h deleted file mode 100644 index 724b59b..0000000 --- a/samples/mover.h +++ /dev/null @@ -1,53 +0,0 @@ -// SPDX-FileCopyrightText: 2026 Erin Catto -// SPDX-License-Identifier: MIT - -#pragma once - -#include "box3d/types.h" - -class Sample; - -struct MoverShapeUserData -{ - float maxPush; - bool clipVelocity; - bool canMoverPush; -}; - -struct PlaneExtra -{ - b3Pos point; - b3ShapeId shapeId; -}; - -struct CharacterMover -{ - static constexpr int m_planeCapacity = 8; - static constexpr float m_jumpSpeed = 5.0f; - static constexpr float m_maxSpeed = 6.0f; - static constexpr float m_minSpeed = 0.01f; - static constexpr float m_stopSpeed = 1.0f; - static constexpr float m_accelerate = 30.0f; - static constexpr float m_friction = 4.0f; - static constexpr float m_gravity = 15.0f; - - void Initialize( Sample* sample, b3Pos position ); - void SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle, bool clipVelocity ); - void Step( b3ShapeId* ignoreShapes, int ignoreCount, bool clipVelocity ); - - Sample* m_sample; - b3WorldTransform m_transform; - b3Vec3 m_velocity; - b3Capsule m_capsule; - b3CollisionPlane m_planes[m_planeCapacity] = {}; - PlaneExtra m_planeExtras[m_planeCapacity] = {}; - int m_planeCount; - int m_totalIterations; - float m_pogoVelocity; - bool m_onGround; - bool m_sprint; - - // Transient - b3ShapeId* m_ignoreShapeIds; - int m_ignoreCount; -}; diff --git a/samples/sample.cpp b/samples/sample.cpp index 5abea6b..22c0b35 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -97,6 +97,11 @@ void SetKeyDown( int key, bool down ) } } +void ClearKeysDown() +{ + memset( s_keyDown, 0, sizeof( s_keyDown ) ); +} + void SampleContext::Save() { const b3DebugDraw* gd = GetGuiDraw(); @@ -1789,6 +1794,11 @@ static void DrawMenuBar( SampleContext* context ) { context->showUI = false; } + if ( ImGui::MenuItem( "Hide UI and Text", "Shift+Tab" ) ) + { + context->showUI = false; + context->showText = false; + } if ( ImGui::MenuItem( "Frame Selection", "F" ) ) { FrameSelection( context ); @@ -1921,6 +1931,7 @@ static void DrawMenuBar( SampleContext* context ) if ( ImGui::BeginTable( "keys", 2, ImGuiTableFlags_SizingFixedFit ) ) { DrawRow( "Tab", "Show / hide UI" ); + DrawRow( "Shift+Tab", "Show / hide UI and text" ); DrawRow( "M", "Show / hide metrics" ); DrawRow( "I", "Show / hide profile" ); DrawRow( "P", "Pause / resume" ); @@ -2261,7 +2272,7 @@ static void DrawHud( SampleContext* context ) const SampleEntry& entry = g_sampleEntries[context->sampleIndex]; float fontSize = ImGui::GetFontSize(); - DrawScreenStringFormat( 5, (int)( 1.5f * fontSize ), MakeColor( b3_colorYellow ), "%s : %s", entry.Category, entry.Name ); + DrawScreenStringFormat( 5, (int)( 0.5f * fontSize ), MakeColor( b3_colorYellow ), "%s : %s", entry.Category, entry.Name ); if ( context->pause ) { @@ -2277,7 +2288,10 @@ void DrawUI( SampleContext* context ) // Hidden shows only the minimal in-world HUD. if ( context->showUI == false ) { - DrawHud( context ); + if ( context->showText ) + { + DrawHud( context ); + } return; } @@ -2308,6 +2322,46 @@ void Sample::ToggleThirdPerson() } } +MoverInput Sample::GetMoverInput() const +{ + MoverInput input = {}; + input.forward = -m_camera->GetForward(); + input.right = m_camera->GetRight(); + + input.forward.y = 0.0f; + input.forward = b3Normalize( input.forward ); + + if ( m_camera->m_thirdPerson == false ) + { + return input; + } + + if ( IsKeyDown( KEY_W ) ) + { + input.throttle.x += 1.0f; + } + + if ( IsKeyDown( KEY_S ) ) + { + input.throttle.x -= 1.0f; + } + + if ( IsKeyDown( KEY_A ) ) + { + input.throttle.y -= 1.0f; + } + + if ( IsKeyDown( KEY_D ) ) + { + input.throttle.y += 1.0f; + } + + input.jump = IsKeyDown( KEY_SPACE ); + input.sprint = IsKeyDown( KEY_LEFT_SHIFT ) || IsKeyDown( KEY_RIGHT_SHIFT ); + input.walk = IsKeyDown( KEY_LEFT_CONTROL ) || IsKeyDown( KEY_RIGHT_CONTROL ); + return input; +} + float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex, int childIndex, void* context ) { diff --git a/samples/sample.h b/samples/sample.h index 6d09f41..d6027ff 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -11,6 +11,7 @@ // Fed from the host event loop; read with the KEY_* aliases in gfx/keycodes.h. bool IsKeyDown( int key ); void SetKeyDown( int key, bool down ); +void ClearKeysDown(); // Key action passed to Keyboard, matching the old GLFW press/release values so // sample code reads naturally without leaking sokol_app event types. @@ -20,6 +21,17 @@ enum ACTION_PRESS = 1, }; +// Character input relative to the camera. Throttle x is forward and y is right. +struct MoverInput +{ + b3Vec3 forward; + b3Vec3 right; + b3Vec2 throttle; + bool jump; + bool sprint; + bool walk; +}; + struct SampleContext { void Save(); @@ -85,6 +97,9 @@ struct SampleContext // UI visibility (Tab / View > Hide UI). When hidden only the minimal HUD shows. bool showUI = true; + // Viewport text: HUD, sample text lines and world labels. + bool showText = true; + // Bottom metrics drawer (M), set by Ctrl+O for the fuzzy picker. bool showMetrics = false; bool openSamplePicker = false; @@ -211,6 +226,9 @@ public: void ToggleThirdPerson(); + // WASD, space, shift and ctrl, active only in third person so the free camera keeps its keys. + MoverInput GetMoverInput() const; + B3_PRINTF_FORMAT( 2, 3 ) void DrawTextLine( const char* text, ... ); void ResetProfile(); diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 77b1c86..3790835 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -1,16 +1,21 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "dynamic_mover.h" +#include "geometric_mover.h" +#include "gfx/debug_adapter.h" +#include "gfx/debug_shapes.h" #include "gfx/draw.h" #include "gfx/keycodes.h" #include "mesh_loader.h" -#include "mover.h" #include "sample.h" #include "box3d/box3d.h" #include +#include + class CapsulePlane : public Sample { public: @@ -57,9 +62,12 @@ public: Sample::Render(); DrawGroundGrid( 10 ); - DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisX ), MakeColor( b3_colorRed ) ); - DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisY ), MakeColor( b3_colorGreen ) ); - DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisZ ), MakeColor( b3_colorBlue ) ); + DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisX ), + MakeColor( b3_colorRed ) ); + DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisY ), + MakeColor( b3_colorGreen ) ); + DrawLine( b3Pos_zero, b3OffsetPos( b3Pos_zero, 2.0f * b3Vec3_axisZ ), + MakeColor( b3_colorBlue ) ); } bool DrawControls() override @@ -72,7 +80,8 @@ public: return true; } - static bool PlaneResultFcn( b3ShapeId shape, const b3PlaneResult* results, int planeCount, void* context ) + static bool PlaneResultFcn( b3ShapeId shape, const b3PlaneResult* results, int planeCount, + void* context ) { CapsulePlane* self = static_cast( context ); for ( int i = 0; i < planeCount && self->m_planeCount < 3; ++i ) @@ -212,7 +221,8 @@ public: DrawGroundGrid( 12 ); } - static bool PlaneResultFcn( b3ShapeId /*shape*/, const b3PlaneResult* results, int planeCount, void* context ) + static bool PlaneResultFcn( b3ShapeId /*shape*/, const b3PlaneResult* results, int planeCount, + void* context ) { MoverOverlap* self = static_cast( context ); for ( int i = 0; i < planeCount && self->m_planeCount < m_planeCapacity; ++i ) @@ -321,10 +331,10 @@ struct MoverImpact bool valid; }; -class GeometricMover : public Sample +class GeometricMoverSample : public Sample { public: - explicit GeometricMover( SampleContext* context ) + explicit GeometricMoverSample( SampleContext* context ) : Sample( context ) { b3Pos moverPosition = { 7.5f, 0.75f, 9.0f }; @@ -334,10 +344,15 @@ public: m_camera->SetView( 120.0f, 30.0f, 5.0f, moverPosition ); } - m_mover.Initialize( this, moverPosition ); + GeometricMoverDef moverDef; + moverDef.position = moverPosition; + m_mover.Create( m_worldId, &moverDef ); + m_mover.m_ignoreShapeIds = &m_ignoreShapeId; + m_mover.m_ignoreCount = 1; { - m_levelMesh = CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true ); + m_levelMesh = + CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true ); b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); @@ -348,9 +363,10 @@ public: b3CreateMeshShape( body, &shapeDef, m_levelMesh, b3Vec3_one ); } - // b3Transform transform = { { 0.0f, 1.0f, 14.0f }, b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.75f * B3_PI ) }; - // b3BoxHull box = b3MakeTransformedBoxHull( 5.0f, 1.0f, 0.5f, transform ); - // b3CreateHullShape( body, &shapeDef, &box.base ); + // b3Transform transform = { { 0.0f, 1.0f, 14.0f }, b3MakeQuatFromAxisAngle( + // b3Vec3_axisY, 0.75f * B3_PI ) }; b3BoxHull box = + // b3MakeTransformedBoxHull( 5.0f, 1.0f, 0.5f, transform ); b3CreateHullShape( body, + // &shapeDef, &box.base ); { b3Transform transform = { { 4.0f, 1.0f, 14.0f }, b3Quat_identity }; @@ -363,7 +379,8 @@ public: b3CreateHullShape( body, &shapeDef, &box.base ); } { - b3Transform transform = { { 5.8f, 1.0f, 13.7f }, b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.1f * B3_PI ) }; + b3Transform transform = { { 5.8f, 1.0f, 13.7f }, + b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.1f * B3_PI ) }; b3BoxHull box = b3MakeTransformedBoxHull( 1.0f, 1.0f, 1.0f, transform ); b3CreateHullShape( body, &shapeDef, &box.base ); } @@ -487,7 +504,8 @@ public: b3ShapeDef shapeDef = b3DefaultShapeDef(); shapeDef.baseMaterial.customColor = b3_colorGold; - b3BoxHull box = b3MakeBoxHull( m_fallingExtent.x, m_fallingExtent.y, m_fallingExtent.z ); + b3BoxHull box = + b3MakeBoxHull( m_fallingExtent.x, m_fallingExtent.y, m_fallingExtent.z ); b3CreateHullShape( m_fallingBoxId, &shapeDef, &box.base ); m_respawnTimer = 0.0f; @@ -557,7 +575,6 @@ public: //} m_camera->m_thirdPerson = false; - m_clipVelocity = true; // sapp_lock_mouse( true ); // m_haveMouseLast = false; @@ -565,7 +582,7 @@ public: // m_mouseDelta = { 0.0f, 0.0f }; } - ~GeometricMover() override + ~GeometricMoverSample() override { m_camera->m_thirdPerson = false; sapp_lock_mouse( false ); @@ -589,7 +606,7 @@ public: ToggleThirdPerson(); } - ImGui::Checkbox( "Clip Velocity", &m_clipVelocity ); + ImGui::Checkbox( "Clip Velocity", &m_mover.m_clipVelocity ); return true; } @@ -602,7 +619,6 @@ public: } } - // One tree from the falling trees benchmark: a stack of shrinking cylinders sharing a body. void CreateFallingTree() { b3BodyDef bodyDef = b3DefaultBodyDef(); @@ -634,7 +650,8 @@ public: b3Pos center = b3Body_GetWorldCenter( m_treeId ); b3Body_SetAngularVelocity( m_treeId, m_treeSpin ); - b3Body_SetLinearVelocity( m_treeId, b3Cross( m_treeSpin, b3SubPos( center, m_treeSpawn ) ) ); + b3Body_SetLinearVelocity( m_treeId, + b3Cross( m_treeSpin, b3SubPos( center, m_treeSpawn ) ) ); } // Spin about the base so the trunk sweeps down through anything standing in the fall line. @@ -691,10 +708,11 @@ public: void UpdateFallingTree( b3Pos moverStart, b3WorldTransform treeStart ) { b3WorldTransform treeEnd = b3Body_GetTransform( m_treeId ); - b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart ); + b3Vec3 moverTranslation = b3SubPos( m_mover.m_position, moverStart ); - b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_treeId, moverStart, &m_mover.m_capsule, moverTranslation, - b3DefaultQueryFilter(), treeStart, treeEnd ); + b3BodyTOIResult toi = + b3Body_TimeOfImpactMover( m_treeId, moverStart, &m_mover.m_capsule, moverTranslation, + b3DefaultQueryFilter(), treeStart, treeEnd ); if ( toi.fraction > 0.0f ) { @@ -708,17 +726,19 @@ public: } // Sweep the mover capsule against the box motion from this step and keep the box pose at first - // touch. Both sweeps cover the same interval, so the mover translation comes from this step too. + // touch. Both sweeps cover the same interval, so the mover translation comes from this step + // too. void UpdateFallingBox( b3Pos moverStart, b3WorldTransform boxStart ) { b3WorldTransform boxEnd = b3Body_GetTransform( m_fallingBoxId ); - b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart ); + b3Vec3 moverTranslation = b3SubPos( m_mover.m_position, moverStart ); - b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_fallingBoxId, moverStart, &m_mover.m_capsule, moverTranslation, - b3DefaultQueryFilter(), boxStart, boxEnd ); + b3BodyTOIResult toi = + b3Body_TimeOfImpactMover( m_fallingBoxId, moverStart, &m_mover.m_capsule, + moverTranslation, b3DefaultQueryFilter(), boxStart, boxEnd ); - // A zero fraction means the box was already resting against the mover, so keep the pose from - // the step that first reached it. + // A zero fraction means the box was already resting against the mover, so keep the pose + // from the step that first reached it. if ( toi.fraction > 0.0f ) { m_impactTransform.p = boxStart.p + toi.fraction * b3SubPos( boxEnd.p, boxStart.p ); @@ -742,11 +762,37 @@ public: void Step() override { - b3Pos moverStart = m_mover.m_transform.p; + b3Pos moverStart = m_mover.m_position; b3WorldTransform boxStart = b3Body_GetTransform( m_fallingBoxId ); b3WorldTransform treeStart = b3Body_GetTransform( m_treeId ); - m_mover.Step( &m_ignoreShapeId, 1, m_clipVelocity ); + MoverInput input = GetMoverInput(); + if ( input.jump ) + { + m_mover.Jump(); + } + + float speedScale = 1.0f; + if ( input.walk ) + { + speedScale = 0.25f; + } + else if ( m_mover.m_onGround && input.sprint ) + { + speedScale = 1.5f; + } + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + m_mover.Update( timeStep, input.forward, input.right, input.throttle, speedScale ); + + if ( m_camera->m_thirdPerson ) + { + m_camera->m_pivot = m_mover.m_position; + m_camera->UpdateTransform(); + } + + SetDrawOrigin( m_camera->DrawOrigin() ); + m_mover.Draw(); + DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson ); DrawTextLine( "deltaX = %g, deltaY = %g", m_mouseDelta.x, m_mouseDelta.y ); @@ -760,7 +806,8 @@ public: if ( m_boxImpact.valid ) { - DrawCube( m_impactTransform, 2.0f * m_fallingExtent, MakeColorAlpha( b3_colorYellow, 0.4f ) ); + DrawCube( m_impactTransform, 2.0f * m_fallingExtent, + MakeColorAlpha( b3_colorYellow, 0.4f ) ); } DrawImpact( &m_boxImpact, b3_colorYellow ); @@ -769,10 +816,10 @@ public: static Sample* Create( SampleContext* context ) { - return new GeometricMover( context ); + return new GeometricMoverSample( context ); } - CharacterMover m_mover; + GeometricMover m_mover; MoverShapeUserData m_enemyShape; MoverShapeUserData m_friendlyShape; MoverShapeUserData m_treeShapeUserData; @@ -793,10 +840,1040 @@ public: b3Vec3 m_fallingExtent; b3Vec3 m_fallingSpin; float m_respawnTimer; - bool m_clipVelocity; }; -static int sampleMover = RegisterSample( "Character", "Geometric Mover", GeometricMover::Create ); +static int sampleMover = + RegisterSample( "Character", "Geometric Mover", GeometricMoverSample::Create ); + +class DynamicMoverSample : public Sample +{ +public: + explicit DynamicMoverSample( SampleContext* context ) + : Sample( context ) + { + b3Pos startPosition = { 0.0f, 1.5f, 0.0f }; + + if ( m_context->restart == false ) + { + m_camera->SetView( 180.0f, 20.0f, 6.0f, startPosition ); + } + + b3BodyId groundId = AddGroundBox( 80.0f ); + + { + DynamicMoverDef def; + def.position = startPosition; + def.filter = { MoverBit, AllBits & ~DebrisBit, 0 }; + def.platformMaterialId = m_platformMaterial; + def.enableSensorEvents = true; + m_mover.Create( m_worldId, &def ); + } + + // Stairs up to the bridge + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId stairsId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorOliveDrab; + + for ( int i = 0; i < 10; ++i ) + { + float halfHeight = 0.1f * ( i + 1 ); + b3Transform transform = { { -6.0f, halfHeight, 3.3f + 0.6f * i }, b3Quat_identity }; + b3BoxHull box = b3MakeTransformedBoxHull( 1.5f, halfHeight, 0.3f, transform ); + b3CreateHullShape( stairsId, &shapeDef, &box.base ); + } + } + + // Platforms next to the bridge + b3BodyId towerIds[2]; + for ( int i = 0; i < 2; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { i == 0 ? -6.0f : 6.0f, 1.0f, 11.0f }; + towerIds[i] = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorSlateGray; + b3BoxHull box = b3MakeBoxHull( 1.5f, 1.0f, 2.0f ); + b3CreateHullShape( towerIds[i], &shapeDef, &box.base ); + } + + // Suspension bridge + { + int plankCount = 18; + float plankLength = 9.0f / plankCount; + float xBase = -4.5f; + float y = 1.95f; + float z = 11.0f; + + b3ShapeDef plankShapeDef = b3DefaultShapeDef(); + plankShapeDef.density = 300.0f; + plankShapeDef.baseMaterial.customColor = b3_colorBurlywood; + b3BoxHull plank = b3MakeBoxHull( 0.5f * plankLength - 0.01f, 0.05f, 0.75f ); + + // The hinge is the local z-axis, across the bridge + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 50.0f; + + b3BodyId prevId = towerIds[0]; + for ( int i = 0; i < plankCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { xBase + plankLength * ( i + 0.5f ), y, z }; + bodyDef.angularDamping = 0.5f; + b3BodyId plankId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( plankId, &plankShapeDef, &plank.base ); + + b3Pos pivot = { xBase + plankLength * i, y, z }; + jointDef.base.bodyIdA = prevId; + jointDef.base.bodyIdB = plankId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevId, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( plankId, pivot ); + b3CreateRevoluteJoint( m_worldId, &jointDef ); + + prevId = plankId; + } + + b3Pos pivot = { xBase + plankLength * plankCount, y, z }; + jointDef.base.bodyIdA = prevId; + jointDef.base.bodyIdB = towerIds[1]; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( prevId, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( towerIds[1], pivot ); + b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Ramp to bridge. + AddRamp( { 6.0f, 2.0f, 9.0f }, { 6.0f, 0.0f, 3.5f }, 1.5f, b3_colorOliveDrab ); + + // Wall + AddRamp( { 20.0f, 3.0f, -40.0f }, { 20.0f, -0.5f, -40.0f }, 5.0f, b3_colorLightSteelBlue ); + + // Ramp + AddRamp( { 35.0f, 3.0f, -46.0f }, { 35.0f, -0.5f, -39.5f }, 5.0f, b3_colorLightSteelBlue ); + + // Steep + AddRamp( { 50.0f, 3.0f, -42.5f }, { 50.0f, -0.5f, -39.5f }, 5.0f, b3_colorLightSteelBlue ); + + // Curb + AddRamp( { 65.0f, 0.25f, -40.0f }, { 65.0f, -0.5f, -40.0f }, 5.0f, b3_colorLightSteelBlue ); + + // Ledges + for ( int i = 0; i < 2; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { i == 0 ? 18.0f : 32.0f, 2.0f, 0.0f }; + b3BodyId ledgeId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorDarkSlateGray; + b3BoxHull box = b3MakeBoxHull( 2.0f, 2.0f, 2.0f ); + b3CreateHullShape( ledgeId, &shapeDef, &box.base ); + } + + // Moving platforms + for ( int i = 0; i < m_platformCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position = GetPlatformPosition( i, 0.0f ); + m_platformIds[i] = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef platformShapeDef = b3DefaultShapeDef(); + platformShapeDef.filter.categoryBits = DynamicBit; + platformShapeDef.baseMaterial.userMaterialId = m_platformMaterial; + platformShapeDef.baseMaterial.customColor = b3_colorGoldenRod; + b3BoxHull box = b3MakeBoxHull( 1.5f, 0.1f, 1.5f ); + b3CreateHullShape( m_platformIds[i], &platformShapeDef, &box.base ); + } + + // Seesaw + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -14.0f, 0.4f, 0.0f }; + b3BodyId fulcrumId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorSlateGray; + b3BoxHull fulcrum = b3MakeBoxHull( 0.25f, 0.4f, 0.75f ); + b3CreateHullShape( fulcrumId, &shapeDef, &fulcrum.base ); + + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -14.0f, 0.9f, 0.0f }; + b3BodyId plankId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef plankShapeDef = b3DefaultShapeDef(); + plankShapeDef.filter.categoryBits = DynamicBit; + plankShapeDef.density = 200.0f; + plankShapeDef.baseMaterial.customColor = b3_colorBurlywood; + b3BoxHull plank = b3MakeBoxHull( 3.5f, 0.1f, 0.8f ); + b3CreateHullShape( plankId, &plankShapeDef, &plank.base ); + + b3Pos pivot = { -14.0f, 0.8f, 0.0f }; + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = fulcrumId; + jointDef.base.bodyIdB = plankId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( fulcrumId, pivot ); + jointDef.base.localFrameB.p = b3Body_GetLocalPoint( plankId, pivot ); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.3f; + jointDef.upperAngle = 0.3f; + b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Large pyramid past the seesaw, standing in the yz plane + { + int baseCount = m_isDebug ? 20 : 100; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef boxShapeDef = b3DefaultShapeDef(); + boxShapeDef.filter.categoryBits = DynamicBit; + boxShapeDef.density = 50.0f; + + float h = 0.25f; + b3BoxHull box = b3MakeCubeHull( h ); + + for ( int i = 0; i < baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * h; + + for ( int j = i; j < baseCount; ++j ) + { + float z = ( i + 1.0f ) * h + 2.0f * ( j - i ) * h - h * baseCount; + + bodyDef.position = { -50.0f, y, z }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &boxShapeDef, &box.base ); + } + } + } + + // Door + { + b3Pos doorPosition = { -10.0f, 1.6f, -8.0f }; + float doorHalfWidth = 0.75f; + float doorHalfHeight = 1.5f; + + // The door fits the frame tightly. The joint keeps the two from colliding. + b3BodyDef frameDef = b3DefaultBodyDef(); + frameDef.position = doorPosition; + b3BodyId frameId = b3CreateBody( m_worldId, &frameDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorSlateGray; + + float frameHalfWidth = 0.3f; + float postBottom = -doorPosition.y; + float postHalfHeight = 0.5f * ( doorHalfHeight - postBottom ); + float postOffset = doorHalfWidth + frameHalfWidth; + + for ( float side : { -1.0f, 1.0f } ) + { + b3Transform transform = { { side * postOffset, postBottom + postHalfHeight, 0.0f }, + b3Quat_identity }; + b3BoxHull post = + b3MakeTransformedBoxHull( frameHalfWidth, postHalfHeight, 0.2f, transform ); + b3CreateHullShape( frameId, &shapeDef, &post.base ); + } + + b3Transform transform = { { 0.0f, doorHalfHeight + frameHalfWidth, 0.0f }, + b3Quat_identity }; + b3BoxHull lintel = b3MakeTransformedBoxHull( postOffset + frameHalfWidth, + frameHalfWidth, 0.2f, transform ); + b3CreateHullShape( frameId, &shapeDef, &lintel.base ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = doorPosition; + bodyDef.gravityScale = 2.0f; + b3BodyId doorId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef doorShapeDef = b3DefaultShapeDef(); + doorShapeDef.filter.categoryBits = DynamicBit; + doorShapeDef.density = 100.0f; + doorShapeDef.baseMaterial.customColor = b3_colorPeru; + b3BoxHull box = b3MakeBoxHull( doorHalfWidth, doorHalfHeight, 0.1f ); + b3CreateHullShape( doorId, &doorShapeDef, &box.base ); + + b3Quat axisQuat = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + b3Vec3 offset = { -doorHalfWidth, 0.0f, 0.0f }; + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = frameId; + jointDef.base.bodyIdB = doorId; + jointDef.base.localFrameA = { offset, axisQuat }; + jointDef.base.localFrameB = { offset, axisQuat }; + jointDef.enableLimit = true; + jointDef.lowerAngle = B3_DEG_TO_RAD * -90.0f; + jointDef.upperAngle = B3_DEG_TO_RAD * 90.0f; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.5f; + jointDef.base.drawScale = 2.0f; + b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Ball pit + { + b3ShapeDef sphereShapeDef = b3DefaultShapeDef(); + sphereShapeDef.density = 100.0f; + sphereShapeDef.baseMaterial.friction = 0.2f; + sphereShapeDef.baseMaterial.customColor = b3_colorLightSeaGreen; + sphereShapeDef.filter.categoryBits = DynamicBit; + b3Sphere sphere = { b3Vec3_zero, 0.35f }; + + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.linearHertz = 0.5f; + jointDef.linearDampingRatio = 0.2f; + jointDef.base.bodyIdA = groundId; + jointDef.base.collideConnected = true; + + int gridCount = 20; + for ( int i = 0; i < gridCount; ++i ) + { + for ( int j = 0; j < gridCount; ++j ) + { + b3Pos position = { -9.5f + 1.0f * i, 1.3f, 17.0f + 1.0f * j }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = position; + bodyDef.gravityScale = 0.0f; + b3BodyId sphereId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( sphereId, &sphereShapeDef, &sphere ); + + jointDef.base.bodyIdB = sphereId; + jointDef.base.localFrameA.p = b3Body_GetLocalPoint( groundId, position ); + jointDef.base.localFrameB.p = b3Vec3_zero; + b3CreateWeldJoint( m_worldId, &jointDef ); + } + } + } + + // Pressure plate + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = m_plateCenter; + b3BodyId plateId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef plateShapeDef = b3DefaultShapeDef(); + plateShapeDef.density = 20.0f; + plateShapeDef.baseMaterial.customColor = b3_colorFireBrick; + // Only allow the mover to press this. + plateShapeDef.filter = { DynamicBit, MoverBit, 0 }; + plateShapeDef.explosionScale = 0.0f; + + b3BoxHull plate = b3MakeBoxHull( 1.5f, 0.1f, 1.5f ); + b3CreateHullShape( plateId, &plateShapeDef, &plate.base ); + + // The motor force works like a preloaded spring. The plate holds its own weight but + // sinks under the mover. + float gravity = b3Length( b3World_GetGravity( m_worldId ) ); + float plateWeight = gravity * b3Body_GetMass( plateId ); + float moverWeight = + m_mover.m_gravityScale * gravity * b3Body_GetMass( m_mover.GetBodyId() ); + + // The prismatic axis is the local x-axis, so turn it upright + b3Quat upright = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ); + + b3PrismaticJointDef jointDef = b3DefaultPrismaticJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = plateId; + jointDef.base.localFrameA = { b3Body_GetLocalPoint( groundId, m_plateCenter ), + upright }; + jointDef.base.localFrameB = { b3Vec3_zero, upright }; + jointDef.enableLimit = true; + jointDef.lowerTranslation = -m_plateTravel; + jointDef.upperTranslation = 0.0f; + jointDef.enableMotor = true; + jointDef.motorSpeed = 0.5f; + jointDef.maxMotorForce = plateWeight + 0.5f * moverWeight; + m_plateJointId = b3CreatePrismaticJoint( m_worldId, &jointDef ); + m_plateArmed = true; + } + + // Skittles + if constexpr ( m_isDebug == false ) + { + b3ShapeDef boxShapeDef = b3DefaultShapeDef(); + + // Debris doesn't collide with the mover or other debris. + boxShapeDef.filter.categoryBits = DebrisBit; + boxShapeDef.filter.maskBits = DebrisBit | ProjectileBit | DynamicBit | StaticBit; + boxShapeDef.density = 100.0f; + boxShapeDef.explosionScale = 0.05f; + + b3HexColor colors[5] = { b3_colorCrimson, b3_colorDarkOrange, b3_colorGold, + b3_colorLimeGreen, b3_colorRebeccaPurple }; + + constexpr int sides = 6; + float radius = 0.1f; + float halfHeight = 0.05f; + float bulge = 1.3f; + b3Vec3 points[3 * sides]; + for ( int i = 0; i < sides; ++i ) + { + b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI * i / sides ); + b3Vec3 rim = { radius * cs.cosine, 0.0f, radius * cs.sine }; + points[3 * i + 0] = { rim.x, -halfHeight, rim.z }; + points[3 * i + 1] = { bulge * rim.x, 0.0f, bulge * rim.z }; + points[3 * i + 2] = { rim.x, halfHeight, rim.z }; + } + + b3HullData* skittle = b3CreateHull( points, 3 * sides, 3 * sides ); + + int gridCount = 32; + int layerCount = 3; + float spacing = 0.5f; + float offset = -0.5f * spacing * ( gridCount - 1 ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.isAwake = false; + + for ( int layer = 0; layer < layerCount; ++layer ) + { + for ( int i = 0; i < gridCount; ++i ) + { + for ( int j = 0; j < gridCount; ++j ) + { + bodyDef.position = { m_plateCenter.x + offset + spacing * i, + 8.0f + spacing * layer, + m_plateCenter.z + offset + spacing * j }; + b3BodyId boxId = b3CreateBody( m_worldId, &bodyDef ); + boxShapeDef.baseMaterial.customColor = colors[( i + 3 * j ) % 5]; + b3CreateHullShape( boxId, &boxShapeDef, skittle ); + } + } + } + + b3DestroyHull( skittle ); + } + + // Big sphere + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 6.0f, 1.0f, -6.0f }; + b3BodyId ballId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef ballShapeDef = b3DefaultShapeDef(); + ballShapeDef.filter.categoryBits = DynamicBit; + ballShapeDef.density = 30.0f; + ballShapeDef.baseMaterial.rollingResistance = 0.05f; + ballShapeDef.baseMaterial.customColor = b3_colorTomato; + b3Sphere sphere = { b3Vec3_zero, 1.0f }; + b3CreateSphereShape( ballId, &ballShapeDef, &sphere ); + } + + // Sweeper + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = m_sweeperCenter; + b3BodyId sweeperId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.filter.categoryBits = DynamicBit; + shapeDef.baseMaterial.customColor = b3_colorCrimson; + + b3BoxHull post = b3MakeTransformedBoxHull( 0.3f, 0.6f, 0.3f, + { { 0.0f, 0.6f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( sweeperId, &shapeDef, &post.base ); + + b3BoxHull armX = b3MakeTransformedBoxHull( 5.0f, 0.25f, 0.1f, + { { 0.0f, 1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( sweeperId, &shapeDef, &armX.base ); + + b3BoxHull armZ = b3MakeTransformedBoxHull( 0.1f, 0.25f, 5.0f, + { { 0.0f, 1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( sweeperId, &shapeDef, &armZ.base ); + + b3Quat upright = b3MakeQuatFromAxisAngle( b3Vec3_axisX, -0.5f * B3_PI ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = sweeperId; + jointDef.base.localFrameA = { b3Body_GetLocalPoint( groundId, m_sweeperCenter ), + upright }; + jointDef.base.localFrameB = { b3Vec3_zero, upright }; + jointDef.enableMotor = true; + jointDef.motorSpeed = 1.0f; + jointDef.maxMotorTorque = 3000.0f; + + b3CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Jump puzzle + for ( int i = 0; i < m_pillarCount; ++i ) + { + b3Pos top = GetPillarTop( i ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { top.x, 0.0f, top.z }; + b3BodyId pillarId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorMediumPurple; + b3HullData* pillar = b3CreateCylinder( (float)top.y, 0.75f, 0.0f, 12 ); + b3CreateHullShape( pillarId, &shapeDef, pillar ); + b3DestroyHull( pillar ); + } + + SpawnGems(); + + m_camera->m_thirdPerson = true; + sapp_lock_mouse( true ); + + m_jumpReleased = true; + m_drawProbes = true; + m_useBox = false; + + for ( b3BodyId& id : m_ballIds ) + { + id = b3_nullBodyId; + } + + m_ballIndex = 0; + m_ballSpeed = 50.0f; + m_aiming = false; + m_aimBlend = 0.0f; + m_shootRequested = false; + + // Flat ended so the muzzle sits at the front face + float launcherLength = m_launcherBack + m_launcherFront + m_launcherRadius; + m_launcherHull = b3CreateCylinder( launcherLength, m_launcherRadius, -m_launcherBack, 5 ); + m_launcherMesh = FindOrAddHull( m_launcherHull ); + m_time = 0.0f; + } + + ~DynamicMoverSample() override + { + ReleaseMeshReference( m_launcherMesh ); + b3DestroyHull( m_launcherHull ); + + m_camera->m_thirdPerson = false; + sapp_lock_mouse( false ); + } + + void AddRamp( b3Vec3 high, b3Vec3 low, float halfWidth, b3HexColor color ) + { + b3Vec3 axis = b3Sub( high, low ); + float length = b3Length( axis ); + axis = b3MulSV( 1.0f / length, axis ); + + b3Quat rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, axis ); + b3Vec3 normal = b3RotateVector( rotation, b3Vec3_axisY ); + + float halfThickness = 0.1f; + b3Vec3 center = b3MulAdd( b3Lerp( low, high, 0.5f ), -halfThickness, normal ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId rampId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = color; + b3BoxHull box = b3MakeTransformedBoxHull( halfWidth, halfThickness, 0.5f * length, + { center, rotation } ); + b3CreateHullShape( rampId, &shapeDef, &box.base ); + } + + b3Pos GetPlatformPosition( int index, float time ) const + { + if ( index == 0 ) + { + // Vertical + float y = 1.95f - 1.95f * cosf( 0.6f * time ); + return { 14.0f, y, 0.0f }; + } + + // Horizontal + float x = 25.0f - 3.5f * cosf( 0.5f * time ); + return { x, 3.9f, 0.0f }; + } + + b3Pos GetPillarTop( int index ) const + { + return { -20.0f, 0.5f + 0.6f * index, -3.0f - 5.0f * index }; + } + + void SpawnGems() + { + b3Pos positions[m_gemCount] = { + { 0.0f, 1.2f, 5.0f }, { -6.0f, 2.2f, 6.0f }, { -6.0f, 3.2f, 11.0f }, + { 0.0f, 3.2f, 11.0f }, { 6.0f, 3.2f, 11.0f }, { 6.0f, 2.2f, 6.0f }, + { 18.0f, 5.2f, 0.0f }, { 25.0f, 5.2f, 0.0f }, { 32.0f, 5.2f, 0.0f }, + { -17.0f, 2.2f, 0.0f }, { -10.0f, 4.4f, -8.0f }, { 6.0f, 1.2f, -9.5f }, + { 3.0f, 1.2f, -16.0f }, { -3.0f, 1.2f, -16.0f }, { 0.0f, 1.2f, -19.0f }, + { 0.0f, 1.2f, -13.0f }, { -20.0f, 0.0f, 0.0f }, + }; + + positions[m_gemCount - 1] = GetPillarTop( m_pillarCount - 1 ); + positions[m_gemCount - 1].y += 1.2f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.isSensor = true; + shapeDef.enableSensorEvents = true; + shapeDef.filter = { GemBit, MoverBit, 0 }; + shapeDef.baseMaterial.customColor = b3_colorGold; + + b3Quat tilt = b3MakeQuatFromAxisAngle( b3Normalize( { 1.0f, 0.0f, -1.0f } ), 0.9553f ); + b3BoxHull gem = b3MakeTransformedBoxHull( 0.2f, 0.2f, 0.2f, { b3Vec3_zero, tilt } ); + + for ( int i = 0; i < m_gemCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + bodyDef.position = positions[i]; + bodyDef.angularVelocity = { 0.0f, 2.0f, 0.0f }; + m_gemIds[i] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_gemIds[i], &shapeDef, &gem.base ); + } + } + + void UpdatePressurePlate() + { + float translation = b3PrismaticJoint_GetTranslation( m_plateJointId ); + + if ( m_plateArmed && translation < -m_plateTravel + 0.01f ) + { + b3ExplosionDef def = b3DefaultExplosionDef(); + def.position = b3OffsetPos( m_plateCenter, b3Vec3{ 0.0f, -0.1f, 0.0f } ); + def.radius = 10.0f; + def.falloff = 10.0f; + def.impulsePerArea = 3000.0f; + b3World_Explode( m_worldId, &def ); + + m_mover.Launch(); + m_plateArmed = false; + } + else if ( m_plateArmed == false && translation > -0.01f ) + { + m_plateArmed = true; + } + } + + void CollectGems() + { + b3SensorEvents events = b3World_GetSensorEvents( m_worldId ); + for ( int i = 0; i < events.beginCount; ++i ) + { + b3SensorBeginTouchEvent* event = events.beginEvents + i; + if ( b3Shape_IsValid( event->visitorShapeId ) == false || + b3Shape_IsValid( event->sensorShapeId ) == false ) + { + continue; + } + + b3BodyId visitorId = b3Shape_GetBody( event->visitorShapeId ); + if ( B3_ID_EQUALS( visitorId, m_mover.GetBodyId() ) == false ) + { + continue; + } + + b3BodyId gemId = b3Shape_GetBody( event->sensorShapeId ); + for ( b3BodyId& id : m_gemIds ) + { + if ( B3_ID_EQUALS( id, gemId ) ) + { + b3DestroyBody( id ); + id = b3_nullBodyId; + break; + } + } + } + } + + void SwapMoverShape() + { + b3BodyId bodyId = m_mover.GetBodyId(); + + b3ShapeId shapeId; + b3Body_GetShapes( bodyId, &shapeId, 1 ); + float density = b3Shape_GetDensity( shapeId ); + b3DestroyShape( shapeId, false ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + shapeDef.filter = { MoverBit, AllBits, 0 }; + shapeDef.enableSensorEvents = true; + + b3Capsule capsule = m_mover.m_capsule; + if ( m_useBox ) + { + float r = capsule.radius; + float hy = 0.5f * b3Length( capsule.center2 - capsule.center1 ) + r; + b3Vec3 c = b3Lerp( capsule.center1, capsule.center2, 0.5f ); + b3BoxHull box = b3MakeOffsetBoxHull( r, hy, r, c ); + shapeDef.density = density; + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + else + { + shapeDef.density = density; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + } + + struct Aim + { + b3Pos shoulder; + b3Pos muzzle; + b3Vec3 direction; + b3Vec3 right; + b3Vec3 velocity; + }; + + Aim ComputeAim() const + { + const b3Capsule& capsule = m_mover.m_capsule; + + b3Vec3 right = m_camera->GetRight(); + right.y = 0.0f; + right = b3Normalize( right ); + + Aim aim = {}; + aim.right = right; + + b3Vec3 shoulderOffset = capsule.center2 + ( capsule.radius + m_launcherRadius ) * right; + aim.shoulder = b3Body_GetPosition( m_mover.GetBodyId() ) + shoulderOffset; + aim.direction = -m_camera->GetForward(); + aim.muzzle = aim.shoulder + ( m_launcherFront + m_launcherRadius ) * aim.direction; + aim.velocity = m_ballSpeed * aim.direction; + + return aim; + } + + void ShootBall( const Aim& aim, float leadTime ) + { + b3BodyId& ballId = m_ballIds[m_ballIndex]; + if ( b3Body_IsValid( ballId ) ) + { + b3DestroyBody( ballId ); + } + + // Back up the spawn point so we see it drawn at the muzzle. + float backDistance = + b3MinFloat( m_ballSpeed * leadTime, m_launcherFront + m_launcherRadius ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = aim.muzzle - backDistance * aim.direction; + bodyDef.linearVelocity = aim.velocity; + + // This is good use case for the bullet setting because this is a fast projectile. + bodyDef.isBullet = true; + bodyDef.name = "ball"; + ballId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 4000.0f; + shapeDef.baseMaterial.friction = 0.2f; + shapeDef.baseMaterial.restitution = 0.5f; + shapeDef.baseMaterial.rollingResistance = 0.1f; + shapeDef.baseMaterial.customColor = b3_colorOrangeRed; + shapeDef.filter = { ProjectileBit, AllBits & ~MoverBit, 0 }; + b3Sphere sphere = { b3Vec3_zero, m_ballRadius }; + b3CreateSphereShape( ballId, &shapeDef, &sphere ); + + m_ballIndex = ( m_ballIndex + 1 ) % m_ballCapacity; + } + + void DrawWeapon( const Aim& aim ) const + { + // The cylinder axis is local y. Roll with the camera to keep it steady. + b3Matrix3 basis = { aim.right, aim.direction, b3Cross( aim.right, aim.direction ) }; + b3WorldTransform launcherTransform = { aim.shoulder, b3MakeQuatFromMatrix( &basis ) }; + b3Transform relative = b3ToRelativeTransform( launcherTransform, GetDrawOrigin() ); + AppendMesh( m_launcherMesh, relative, b3Vec3_one, MakeColor( b3_colorDimGray ), 0.6f, 0.35f, + MESH_MATERIAL_MODE_SOLID, 0.0f, TRANSPARENT_SHADOW_FULL ); + } + + void MouseDown( b3Vec2 p, int button, int modifiers ) override + { + if ( m_camera->m_thirdPerson ) + { + if ( button == 1 ) + { + m_aiming = true; + } + else if ( button == 0 && m_aiming ) + { + m_shootRequested = true; + } + + return; + } + + Sample::MouseDown( p, button, modifiers ); + } + + void MouseUp( b3Vec2 p, int button ) override + { + if ( button == 1 ) + { + m_aiming = false; + } + + Sample::MouseUp( p, button ); + } + + // Pull the boom in when the view to the mover is blocked + void FollowMover( b3Pos position ) + { + if ( m_camera->m_thirdPerson == false ) + { + m_aiming = false; + m_aimBlend = 0.0f; + SetBodyAlpha( b3_nullBodyId, 1.0f ); + return; + } + + // Frame time so the blend still runs while paused + float blendTime = 0.2f; + float blendStep = ImGui::GetIO().DeltaTime / blendTime; + m_aimBlend = b3ClampFloat( m_aimBlend + ( m_aiming ? blendStep : -blendStep ), 0.0f, 1.0f ); + float blend = m_aimBlend * m_aimBlend * ( 3.0f - 2.0f * m_aimBlend ); + + // The zoomed camera sits right behind the mover, so fade it to see past + SetBodyAlpha( m_mover.GetBodyId(), 1.0f - 0.7f * blend ); + + // The wheel zoom stays in the camera radius, aiming only overrides it for this frame + float savedRadius = m_camera->m_radius; + float aimRadius = b3MinFloat( savedRadius, 0.8f ); + m_camera->m_radius = savedRadius + blend * ( aimRadius - savedRadius ); + + m_camera->m_pivot = position; + m_camera->UpdateTransform(); + + // Over the shoulder so the mover and launcher sit low and left of the crosshair + if ( blend > 0.0f ) + { + b3Vec3 shoulder = 0.75f * b3Vec3_axisY + 0.6f * m_camera->GetRight(); + m_camera->m_pivot = position + blend * shoulder; + m_camera->UpdateTransform(); + } + + b3Vec3 translation = b3SubPos( m_camera->m_worldEye, position ); + float distance = b3Length( translation ); + if ( distance > 0.01f ) + { + b3QueryFilter filter = { StaticBit, StaticBit, 0, "camera" }; + b3RayResult result = b3World_CastRayClosest( m_worldId, position, translation, filter ); + if ( result.hit ) + { + m_camera->m_radius = b3MaxFloat( 0.1f, result.fraction * distance - 0.15f ); + m_camera->UpdateTransform(); + } + } + + m_camera->m_radius = savedRadius; + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == KEY_T && action == ACTION_PRESS ) + { + ToggleThirdPerson(); + } + + // Align movement for video + if ( key == KEY_K && action == ACTION_PRESS ) + { + float quarterTurn = 0.5f * B3_PI; + m_camera->m_yaw = quarterTurn * roundf( m_camera->m_yaw / quarterTurn ); + m_camera->m_pitch = 15.0f * B3_DEG_TO_RAD; + + b3BodyId bodyId = m_mover.GetBodyId(); + b3Pos position = b3Body_GetPosition( bodyId ); + position.x = roundf( position.x ); + position.z = roundf( position.z ); + b3Body_SetTransform( bodyId, position, b3Body_GetRotation( bodyId ) ); + b3Body_SetLinearVelocity( bodyId, b3Vec3_zero ); + } + } + + bool DrawControls() override + { + bool thirdPerson = m_camera->m_thirdPerson; + if ( ImGui::Checkbox( "Third Person (T)", &thirdPerson ) ) + { + ToggleThirdPerson(); + } + + ImGui::Checkbox( "Skirt Pogo", &m_mover.m_enableSkirtPogo ); + ImGui::Checkbox( "Draw Probes", &m_drawProbes ); + + if ( ImGui::Checkbox( "Box Mover", &m_useBox ) ) + { + SwapMoverShape(); + } + + ImGui::Separator(); + + ImGui::SliderFloat( "Jump Speed", &m_mover.m_jumpSpeed, 0.0f, 20.0f, "%.1f" ); + ImGui::SliderFloat( "Max Speed", &m_mover.m_maxSpeed, 0.0f, 20.0f, "%.0f" ); + ImGui::SliderFloat( "Accelerate", &m_mover.m_accelerate, 0.0f, 100.0f, "%.0f" ); + ImGui::SliderFloat( "Friction", &m_mover.m_friction, 0.0f, 20.0f, "%.1f" ); + ImGui::SliderFloat( "Air Steer", &m_mover.m_airSteer, 0.0f, 1.0f, "%.2f" ); + + float gravityScale = m_mover.m_gravityScale; + if ( ImGui::SliderFloat( "Gravity Scale", &gravityScale, 0.0f, 4.0f, "%.1f" ) ) + { + m_mover.SetGravityScale( gravityScale ); + } + + ImGui::SliderFloat( "Pogo Hertz", &m_mover.m_pogoHertz, 0.0f, 30.0f, "%.0f" ); + ImGui::SliderFloat( "Pogo Damping", &m_mover.m_pogoDampingRatio, 0.0f, 4.0f, "%.1f" ); + ImGui::SliderFloat( "Ground Force", &m_mover.m_maxGroundForce, 0.0f, 20000.0f, "%.0f" ); + + ImGui::Separator(); + + ImGui::SliderFloat( "Ball Speed", &m_ballSpeed, 10.0f, 200.0f, "%.0f" ); + + return true; + } + + void Step() override + { + bool pause = m_context->pause && m_context->singleStep == 0; + float timeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + + if ( pause == false && timeStep > 0.0f ) + { + m_time += timeStep; + + for ( int i = 0; i < m_platformCount; ++i ) + { + b3WorldTransform target = { GetPlatformPosition( i, m_time ), b3Quat_identity }; + b3Body_SetTargetTransform( m_platformIds[i], target, timeStep, true ); + } + + MoverInput input = GetMoverInput(); + + // Jumping again needs a fresh press + if ( input.jump ) + { + if ( m_jumpReleased && m_mover.Jump() ) + { + m_jumpReleased = false; + } + } + else + { + m_jumpReleased = true; + } + + float speedScale = 1.0f; + if ( input.walk || m_aiming ) + { + speedScale = 0.25f; + } + else if ( input.sprint ) + { + speedScale = 1.5f; + } + m_mover.Update( timeStep, input.forward, input.right, input.throttle, speedScale ); + } + + // The base step draws the world, so the camera has to be placed before it. Otherwise the + // world renders from the previous eye and the overlay from the new one. + FollowMover( b3Body_GetPosition( m_mover.GetBodyId() ) ); + + // Shoot before the step so the ball is in the world when it is drawn + if ( m_aiming && m_shootRequested ) + { + float leadTime = pause == false ? timeStep : 0.0f; + ShootBall( ComputeAim(), leadTime ); + } + + m_shootRequested = false; + + Sample::Step(); + + if ( m_didStep ) + { + CollectGems(); + UpdatePressurePlate(); + } + + if ( m_drawProbes ) + { + m_mover.Draw(); + } + + if ( m_aiming ) + { + Aim aim = ComputeAim(); + DrawWeapon( aim ); + } + + DrawTextLine( "WASD = move, Space = jump, Shift = sprint, Ctrl = walk" ); + DrawTextLine( "T = camera, RMB = aim, LMB = shoot" ); + + float normalY = m_mover.m_castResult.hit ? m_mover.m_castResult.normal.y : 0.0f; + DrawTextLine( "on ground %d, walkable %d, normal y %.2f", (int)m_mover.m_onGround, + (int)m_mover.m_walkable, normalY ); + DrawTextLine( "pogo length %.2f / %.2f", m_mover.m_pogoLength, m_mover.m_pogoRestLength ); + + DrawTextLine( "time sliding %.2f", m_mover.m_timeSliding ); + } + + static Sample* Create( SampleContext* context ) + { + return new DynamicMoverSample( context ); + } + + static constexpr uint64_t StaticBit = 0x1; + static constexpr uint64_t MoverBit = 0x2; + static constexpr uint64_t GemBit = 0x4; + static constexpr uint64_t DynamicBit = 0x10; + static constexpr uint64_t ProjectileBit = 0x20; + static constexpr uint64_t DebrisBit = 0x40; + static constexpr uint64_t AllBits = ~0ull; + + static constexpr int m_ballCapacity = 50; + + static constexpr float m_launcherRadius = 0.03f; + static constexpr float m_launcherBack = 0.05f; + static constexpr float m_launcherFront = 0.55f; + static constexpr float m_ballRadius = 0.1f; + + static constexpr uint64_t m_platformMaterial = 7; + static constexpr int m_platformCount = 2; + static constexpr int m_pillarCount = 6; + static constexpr int m_gemCount = 17; + static constexpr b3Pos m_sweeperCenter = { 0.0f, 0.0f, -16.0f }; + static constexpr b3Pos m_plateCenter = { 0.0f, 0.05f, 26.5f }; + static constexpr float m_plateTravel = 0.1f; + + DynamicMover m_mover; + b3JointId m_plateJointId; + bool m_plateArmed; + b3BodyId m_platformIds[m_platformCount]; + b3BodyId m_gemIds[m_gemCount]; + float m_time; + bool m_jumpReleased; + bool m_drawProbes; + bool m_useBox; + + b3BodyId m_ballIds[m_ballCapacity]; + int m_ballIndex; + float m_ballSpeed; + bool m_aiming; + float m_aimBlend; + bool m_shootRequested; + b3HullData* m_launcherHull; + MeshHandle m_launcherMesh; +}; + +static int sampleDynamicMover = + RegisterSample( "Character", "Dynamic Mover", DynamicMoverSample::Create ); struct ClosestShapeCastContext { @@ -810,8 +1887,9 @@ struct ClosestShapeCastContext bool startedSolid; }; -static float ClosestShapeCastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, - int triangleIndex, int childIndex, void* context ) +static float ClosestShapeCastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, + float fraction, uint64_t userMaterialId, int triangleIndex, + int childIndex, void* context ) { auto* ctx = static_cast( context ); @@ -951,8 +2029,10 @@ struct RigidbodyCharacter float feetVolume = 8.0f * halfExtX * halfExtY * halfExtZ; shapeDef.density = ( m_characterMass * 0.4f ) / feetVolume; - b3Transform feetTransform = { { 0.0f, -m_totalHeight * 0.5f + halfExtY, 0.0f }, b3Quat_identity }; - b3BoxHull feetBox = b3MakeTransformedBoxHull( halfExtX, halfExtY, halfExtZ, feetTransform ); + b3Transform feetTransform = { { 0.0f, -m_totalHeight * 0.5f + halfExtY, 0.0f }, + b3Quat_identity }; + b3BoxHull feetBox = + b3MakeTransformedBoxHull( halfExtX, halfExtY, halfExtZ, feetTransform ); m_feetBoxId = b3CreateHullShape( m_bodyId, &shapeDef, &feetBox.base ); } @@ -992,7 +2072,8 @@ struct RigidbodyCharacter // --- TraceBody: box shape cast matching s&box's TraceBody --- // Casts a box from `from` to `to` with given radius and height scale. - TraceResult TraceBody( b3Pos from, b3Pos to, float radiusScale = 1.0f, float heightScale = 1.0f ) const + TraceResult TraceBody( b3Pos from, b3Pos to, float radiusScale = 1.0f, + float heightScale = 1.0f ) const { TraceResult result = {}; result.endPosition = to; @@ -1047,7 +2128,8 @@ struct RigidbodyCharacter ctx.closestShape = b3_nullShapeId; b3QueryFilter filter = b3DefaultQueryFilter(); - b3World_CastShape( m_sample->m_worldId, from, &proxy, translation, filter, ClosestShapeCastCallback, &ctx ); + b3World_CastShape( m_sample->m_worldId, from, &proxy, translation, filter, + ClosestShapeCastCallback, &ctx ); result.startedSolid = ctx.startedSolid; if ( ctx.hit ) @@ -1100,7 +2182,8 @@ struct RigidbodyCharacter tr = TraceBody( from, to, radiusScale, 0.5f ); } - if ( !tr.startedSolid && tr.hit && IsStandableSurface( tr.normal ) && m_jumpCooldown <= 0.0f ) + if ( !tr.startedSolid && tr.hit && IsStandableSurface( tr.normal ) && + m_jumpCooldown <= 0.0f ) { UpdateGround( true, tr.normal ); DrawLine( from, tr.hitPoint, MakeColor( b3_colorGreen ) ); @@ -1193,7 +2276,8 @@ struct RigidbodyCharacter // Phase 1 — FORWARD: trace body forward in velocity direction // Start slightly behind (offset by skin) - float forwardDist = hSpeed * ( 1.0f / 60.0f ) + m_bodyRadius; // one frame of movement + radius + float forwardDist = + hSpeed * ( 1.0f / 60.0f ) + m_bodyRadius; // one frame of movement + radius b3Pos forwardFrom = pos - m_skin * moveDir; b3Pos forwardTo = pos + forwardDist * moveDir; @@ -1287,7 +2371,8 @@ struct RigidbodyCharacter DrawPoint( trDown.endPosition, 8.0f, MakeColor( b3_colorYellow ) ); // Teleport body to step position - b3Pos stepPos = { trDown.endPosition.x, trDown.endPosition.y + 0.01f, trDown.endPosition.z }; + b3Pos stepPos = { trDown.endPosition.x, trDown.endPosition.y + 0.01f, + trDown.endPosition.z }; b3Quat rot = b3Body_GetRotation( m_bodyId ); b3Body_SetTransform( m_bodyId, stepPos, rot ); @@ -1381,7 +2466,8 @@ struct RigidbodyCharacter b3Body_SetGravityScale( m_bodyId, wantsGravity ? ( m_characterGravity / 10.0f ) : 0.0f ); // Linear damping — s&box: brakes when wish < 1 unit/s && groundVel < 1 unit/s - bool wantsDamping = m_onGround && wishLen < ( 1.0f * SRC ) && b3Length( m_groundVelocity ) < ( 1.0f * SRC ); + bool wantsDamping = + m_onGround && wishLen < ( 1.0f * SRC ) && b3Length( m_groundVelocity ) < ( 1.0f * SRC ); b3Body_SetLinearDamping( m_bodyId, wantsDamping ? 10.0f * m_brakePower : m_airFriction ); } @@ -1514,7 +2600,8 @@ struct RigidbodyCharacter if ( m_onGround ) { b3Pos bottom = { pos.x, pos.y - m_totalHeight * 0.5f, pos.z }; - DrawLine( bottom, b3OffsetPos( bottom, 0.3f * m_groundNormal ), MakeColor( b3_colorGreen ) ); + DrawLine( bottom, b3OffsetPos( bottom, 0.3f * m_groundNormal ), + MakeColor( b3_colorGreen ) ); } } }; @@ -1536,7 +2623,8 @@ public: // --- Main level mesh (test_map01) --- { - m_levelMesh = CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true ); + m_levelMesh = + CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true ); b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); @@ -1564,7 +2652,8 @@ public: // --- High-poly building mesh --- { - m_building = CreateMeshData( "data/meshes/building.obj", 1.0f, false, false, true, true ); + m_building = + CreateMeshData( "data/meshes/building.obj", 1.0f, false, false, true, true ); b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.position = { -5.0f, 0.0f, -10.0f }; b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); @@ -1578,7 +2667,8 @@ public: // --- Voxel meshes (dense tri terrain) --- { - m_voxel01 = CreateMeshData( "data/meshes/voxel_mesh_01.obj", 1.0f, false, false, true, true ); + m_voxel01 = + CreateMeshData( "data/meshes/voxel_mesh_01.obj", 1.0f, false, false, true, true ); b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.position = { 10.0f, 0.0f, -10.0f }; b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); @@ -1590,7 +2680,8 @@ public: } } { - m_voxel02 = CreateMeshData( "data/meshes/voxel_mesh_02.obj", 1.0f, false, false, true, true ); + m_voxel02 = + CreateMeshData( "data/meshes/voxel_mesh_02.obj", 1.0f, false, false, true, true ); b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.position = { 10.0f, 0.0f, 10.0f }; b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); @@ -1797,7 +2888,8 @@ public: if ( desiredDist > 0.01f ) { b3QueryFilter filter = b3DefaultQueryFilter(); - b3RayResult rayResult = b3World_CastRayClosest( m_worldId, charPos, translation, filter ); + b3RayResult rayResult = + b3World_CastRayClosest( m_worldId, charPos, translation, filter ); if ( rayResult.hit ) { @@ -1829,7 +2921,8 @@ public: DrawTextLine( "Rigid Body Character (s&box-style)" ); DrawTextLine( "position: %.2f %.2f %.2f", pos.x, pos.y, pos.z ); DrawTextLine( "velocity: %.2f %.2f %.2f (horizontal: %.2f)", vel.x, vel.y, vel.z, speed ); - DrawTextLine( "on ground: %s | sprint: %s", m_character.m_onGround ? "yes" : "no", m_character.m_sprint ? "yes" : "no" ); + DrawTextLine( "on ground: %s | sprint: %s", m_character.m_onGround ? "yes" : "no", + m_character.m_sprint ? "yes" : "no" ); DrawTextLine( "WASD=move Space=jump Shift=sprint T=camera V=debug" ); } @@ -1879,4 +2972,5 @@ public: bool m_showDebug; }; -static int sampleRBCharacter = RegisterSample( "Character", "Rigid Body", RigidBodyCharacter::Create ); +static int sampleRBCharacter = + RegisterSample( "Character", "Rigid Body", RigidBodyCharacter::Create ); diff --git a/samples/sample_compound.cpp b/samples/sample_compound.cpp index 363dd37..0faeee0 100644 --- a/samples/sample_compound.cpp +++ b/samples/sample_compound.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "geometric_mover.h" #include "gfx/debug_adapter.h" #include "gfx/draw.h" #include "human.h" #include "mesh_loader.h" -#include "mover.h" #include "sample.h" #include "utils.h" @@ -13,6 +13,8 @@ #include +#include + class SimpleCompound : public Sample { public: @@ -484,6 +486,39 @@ static bool OverlapResultFcn( b3ShapeId shapeId, void* context ) return false; } +struct VillageBoxQuery +{ + b3AABB aabb; + int shapeCount; + int childCount; +}; + +static bool CountCompoundChildFcn( const b3CompoundData* compound, int childIndex, void* context ) +{ + (void)compound; + (void)childIndex; + int* count = (int*)context; + *count += 1; + return true; +} + +// The world query only reaches the compound as a whole, so descend into its children like a game would +static bool VillageBoxQueryFcn( b3ShapeId shapeId, void* context ) +{ + VillageBoxQuery* query = (VillageBoxQuery*)context; + query->shapeCount += 1; + + if ( b3Shape_GetType( shapeId ) == b3_compoundShape ) + { + b3WorldTransform bodyTransform = b3Body_GetTransform( b3Shape_GetBody( shapeId ) ); + b3Transform worldToLocal = b3InvMulWorldTransforms( bodyTransform, b3WorldTransform_identity ); + b3AABB localBox = b3AABB_Transform( worldToLocal, query->aabb ); + b3QueryCompound( b3Shape_GetCompound( shapeId ), localBox, CountCompoundChildFcn, &query->childCount ); + } + + return true; +} + class Village : public Sample { public: @@ -501,7 +536,9 @@ public: } m_launchSpeedScale = 2.0f; - m_mover.Initialize( this, position ); + GeometricMoverDef moverDef; + moverDef.position = position; + m_mover.Create( m_worldId, &moverDef ); { constexpr int capsuleCapacity = gridCount * gridCount / 8 + 1; @@ -689,6 +726,80 @@ public: } } + // Fixed seed so timings compare across runs and builds + void CreateStressQueries() + { + uint32_t savedSeed = g_randomSeed; + g_randomSeed = 271828; + + float h = 0.45f * m_worldWidth; + + m_stressRays.resize( m_stressRayCount ); + for ( StressRay& ray : m_stressRays ) + { + ray.origin = { RandomFloatRange( -h, h ), 20.0f, RandomFloatRange( -h, h ) }; + ray.translation = { RandomFloatRange( -10.0f, 10.0f ), -40.0f, RandomFloatRange( -10.0f, 10.0f ) }; + } + + m_stressBoxes.resize( m_stressBoxCount ); + for ( b3AABB& box : m_stressBoxes ) + { + b3Vec3 center = { RandomFloatRange( -h, h ), RandomFloatRange( 0.0f, 8.0f ), RandomFloatRange( -h, h ) }; + b3Vec3 extents = RandomVec3( { 0.5f, 0.5f, 0.5f }, { 4.0f, 4.0f, 4.0f } ); + box = { center - extents, center + extents }; + } + + g_randomSeed = savedSeed; + } + + void RunStressQueries() + { + b3QueryFilter filter = b3DefaultQueryFilter(); + + uint64_t ticks = b3GetTicks(); + + int rayHitCount = 0; + for ( const StressRay& ray : m_stressRays ) + { + CastClosestContext context = {}; + (void)b3World_CastRay( m_worldId, ray.origin, ray.translation, filter, CastClosestCallback, &context ); + rayHitCount += context.hit ? 1 : 0; + } + + float rayTime = b3GetMillisecondsAndReset( &ticks ); + + VillageBoxQuery query = {}; + for ( b3AABB box : m_stressBoxes ) + { + query.aabb = box; + (void)b3World_OverlapAABB( m_worldId, box, filter, VillageBoxQueryFcn, &query ); + } + + float boxTime = b3GetMilliseconds( ticks ); + + int rayCount = (int)m_stressRays.size(); + int boxCount = (int)m_stressBoxes.size(); + + DrawTextLine( "rays = %d, hits = %d", rayCount, rayHitCount ); + DrawTextLine( "ray time = %.3f ms, ave = %.3f us", rayTime, 1000.0f * rayTime / rayCount ); + DrawTextLine( "aabb queries = %d, shapes = %d, children = %d", boxCount, query.shapeCount, query.childCount ); + DrawTextLine( "aabb time = %.3f ms, ave = %.3f us", boxTime, 1000.0f * boxTime / boxCount ); + } + + bool DrawControls() override + { + bool changed = ImGui::Checkbox( "Query Stress", &m_stressQueries ); + changed = ImGui::SliderInt( "Rays", &m_stressRayCount, 1, 100000 ) || changed; + changed = ImGui::SliderInt( "AABB Queries", &m_stressBoxCount, 1, 100000 ) || changed; + + if ( changed && m_stressQueries ) + { + CreateStressQueries(); + } + + return true; + } + void Render() override { Sample::Render(); @@ -711,9 +822,40 @@ public: void Step() override { - m_mover.Step( nullptr, 0, true ); + MoverInput input = GetMoverInput(); + if ( input.jump ) + { + m_mover.Jump(); + } + + float speedScale = 1.0f; + if ( input.walk ) + { + speedScale = 0.25f; + } + else if ( m_mover.m_onGround && input.sprint ) + { + speedScale = 1.5f; + } + float moverTimeStep = m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + m_mover.Update( moverTimeStep, input.forward, input.right, input.throttle, speedScale ); + + if ( m_camera->m_thirdPerson ) + { + m_camera->m_pivot = m_mover.m_position; + m_camera->UpdateTransform(); + } + + SetDrawOrigin( m_camera->DrawOrigin() ); + m_mover.Draw(); + DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson ); + if ( m_stressQueries ) + { + RunStressQueries(); + } + b3Vec3 translation = { 10.0f, -40.0f, -5.0f }; b3QueryFilter filter = b3DefaultQueryFilter(); @@ -800,10 +942,22 @@ public: return new Village( context ); } + struct StressRay + { + b3Pos origin; + b3Vec3 translation; + }; + b3CompoundData* m_compound; - CharacterMover m_mover; + GeometricMover m_mover; float m_worldWidth; b3Pos m_rayOrigin; + + std::vector m_stressRays; + std::vector m_stressBoxes; + int m_stressRayCount = 1000; + int m_stressBoxCount = 1000; + bool m_stressQueries = false; }; static int sampleVillage = RegisterSample( "Compound", "Village", Village::Create ); diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 08e8847..d908f5c 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -1722,13 +1722,17 @@ public: b3Vec3 axis = b3Normalize( b3Cross( u, v ) ); b3Vec3 w = b3Cross( axis, u ); constexpr int segmentCount = 96; + b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI / segmentCount ); + float x1 = 1.0f, y1 = 0.0f; b3Pos previous = GaussPoint( u ); - for ( int i = 1; i <= segmentCount; ++i ) + for ( int i = 0; i < segmentCount; ++i ) { - float angle = 2.0f * B3_PI * i / segmentCount; - b3CosSin cs = b3ComputeCosSin( angle ); - b3Pos point = GaussPoint( b3Add( b3MulSV( cs.cosine, u ), b3MulSV( cs.sine, w ) ) ); + float x2 = cs.cosine * x1 - cs.sine * y1; + float y2 = cs.sine * x1 + cs.cosine * y1; + b3Pos point = GaussPoint( b3Add( b3MulSV( x2, u ), b3MulSV( y2, w ) ) ); DrawLineEx( previous, point, color, 1.0f, OVERLAY_THICKNESS_PIXELS, OVERLAY_OCCLUSION_DASHED ); + x1 = x2; + y1 = y2; previous = point; } } diff --git a/samples/sample_replay.cpp b/samples/sample_replay.cpp index 0a363fb..4879c1d 100644 --- a/samples/sample_replay.cpp +++ b/samples/sample_replay.cpp @@ -94,6 +94,10 @@ static const char* ReplayJointTypeName( b3JointType type ) return "weld"; case b3_wheelJoint: return "wheel"; + case b3_moverJoint: + return "mover"; + case b3_pogoJoint: + return "pogo"; default: return "?"; } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index aa65d79..eaa22b7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -51,6 +51,7 @@ set(BOX3D_SOURCE_FILES mesh.c motor_joint.c mover.c + mover_joint.c name_cache.c name_cache.h parallel_for.c @@ -59,6 +60,7 @@ set(BOX3D_SOURCE_FILES physics_world.c physics_world.h platform.h + pogo_joint.c prismatic_joint.c qsort.h rapidhash.h diff --git a/src/joint.c b/src/joint.c index 068a29d..fa01bb5 100644 --- a/src/joint.c +++ b/src/joint.c @@ -62,6 +62,21 @@ b3MotorJointDef b3DefaultMotorJointDef( void ) return def; } +b3MoverJointDef b3DefaultMoverJointDef( void ) +{ + b3MoverJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3PogoJointDef b3DefaultPogoJointDef( void ) +{ + b3PogoJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.normal = b3Vec3_axisY; + return def; +} + b3FilterJointDef b3DefaultFilterJointDef( void ) { b3FilterJointDef def = { 0 }; @@ -315,7 +330,8 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint else { // joint connected between sleeping and/or static bodies - B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || bodyB->setIndex >= b3_firstSleepingSet ); + B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || + bodyB->setIndex >= b3_firstSleepingSet ); B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); // joint should go into the sleeping set (not static set) @@ -511,6 +527,66 @@ b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def ) return jointId; } +b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( b3IsValidVec3( def->linearVelocity ) ); + B3_ASSERT( b3IsValidFloat( def->maxForce ) && def->maxForce >= 0.0f ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_moverJoint ); + b3JointSim* joint = pair.jointSim; + + joint->moverJoint = (b3MoverJoint){ 0 }; + joint->moverJoint.linearVelocity = def->linearVelocity; + joint->moverJoint.maxForce = def->maxForce; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateMoverJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreatePogoJoint( b3WorldId worldId, const b3PogoJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( b3IsValidVec3( def->normal ) && b3IsNormalized( def->normal ) ); + B3_ASSERT( b3IsValidFloat( def->restLength ) && def->restLength >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->hertz ) && def->hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->maxTensionForce ) && def->maxTensionForce >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->maxCompressionForce ) && def->maxCompressionForce >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->impulse ) ); + B3_ASSERT( b3IsValidFloat( def->velocity ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_pogoJoint ); + b3JointSim* joint = pair.jointSim; + + joint->pogoJoint = (b3PogoJoint){ 0 }; + joint->pogoJoint.normal = def->normal; + joint->pogoJoint.restLength = def->restLength; + joint->pogoJoint.hertz = def->hertz; + joint->pogoJoint.dampingRatio = def->dampingRatio; + joint->pogoJoint.maxTensionForce = def->maxTensionForce; + joint->pogoJoint.maxCompressionForce = def->maxCompressionForce; + joint->pogoJoint.impulse = def->impulse; + joint->pogoJoint.velocity = def->velocity; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreatePogoJoint, jointId, worldId, *def ); + return jointId; +} + b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ) { B3_CHECK_JOINT_DEF( def ); @@ -647,8 +723,10 @@ b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* float lowerAngle = b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle ); float upperAngle = b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle ); - joint->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); - joint->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->sphericalJoint.lowerTwistAngle = + b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->sphericalJoint.upperTwistAngle = + b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); joint->sphericalJoint.maxMotorTorque = def->maxMotorTorque; joint->sphericalJoint.motorVelocity = def->motorVelocity; @@ -805,7 +883,8 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) if ( setIndex == b3_awakeSet ) { - b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, + joint->colorIndex, localIndex ); } else { @@ -1017,7 +1096,8 @@ bool b3Joint_IsAwake( b3JointId jointId ) return joint->setIndex == b3_awakeSet; } -void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ) +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, + float* torque ) { float linearImpulse = 0.0f; float angularImpulse = 0.0f; @@ -1039,15 +1119,18 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo case b3_distanceJoint: { b3DistanceJoint* joint = &sim->distanceJoint; - linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ); + linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + + joint->motorImpulse ); } break; case b3_motorJoint: { b3MotorJoint* joint = &sim->motorJoint; - linearImpulse = b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); - angularImpulse = b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); + linearImpulse = + b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); + angularImpulse = + b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); } break; @@ -1094,7 +1177,8 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); - impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = + b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); angularImpulse = b3Length( impulse ); @@ -1114,12 +1198,28 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo // todo probably wrong b3WheelJoint* joint = &sim->wheelJoint; b3Vec2 perpImpulse = joint->linearImpulse; - float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; - linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + axialImpulse * axialImpulse ); + float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - + joint->upperSuspensionImpulse; + linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + + axialImpulse * axialImpulse ); angularImpulse = b3AbsFloat( joint->spinImpulse ); } break; + case b3_moverJoint: + { + b3MoverJoint* joint = &sim->moverJoint; + linearImpulse = b3Length2( joint->impulse ); + } + break; + + case b3_pogoJoint: + { + b3PogoJoint* joint = &sim->pogoJoint; + linearImpulse = b3AbsFloat( joint->impulse ); + } + break; + default: break; } @@ -1161,6 +1261,12 @@ static b3Vec3 b3GetJointConstraintForce( b3World* world, b3Joint* joint ) case b3_wheelJoint: return b3GetWheelJointForce( world, base ); + case b3_moverJoint: + return b3GetMoverJointForce( world, base ); + + case b3_pogoJoint: + return b3GetPogoJointForce( world, base ); + default: B3_ASSERT( false ); return b3Vec3_zero; @@ -1200,6 +1306,10 @@ static b3Vec3 b3GetJointConstraintTorque( b3World* world, b3Joint* joint ) case b3_wheelJoint: return b3GetWheelJointTorque( world, base ); + case b3_moverJoint: + case b3_pogoJoint: + return b3Vec3_zero; + default: B3_ASSERT( false ); return b3Vec3_zero; @@ -1268,6 +1378,12 @@ float b3Joint_GetLinearSeparation( b3JointId jointId ) case b3_motorJoint: return 0.0f; + case b3_moverJoint: + return 0.0f; + + case b3_pogoJoint: + return 0.0f; + case b3_filterJoint: return 0.0f; @@ -1293,7 +1409,8 @@ float b3Joint_GetLinearSeparation( b3JointId jointId ) } } - return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + return sqrtf( perpendicularSeparation * perpendicularSeparation + + limitSeparation * limitSeparation ); } case b3_revoluteJoint: @@ -1335,7 +1452,8 @@ float b3Joint_GetLinearSeparation( b3JointId jointId ) } } - return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + return sqrtf( perpendicularSeparation * perpendicularSeparation + + limitSeparation * limitSeparation ); } default: @@ -1370,6 +1488,12 @@ float b3Joint_GetAngularSeparation( b3JointId jointId ) case b3_motorJoint: return 0.0f; + case b3_moverJoint: + return 0.0f; + + case b3_pogoJoint: + return 0.0f; + case b3_filterJoint: return 0.0f; @@ -1522,6 +1646,14 @@ void b3PrepareJoint( b3JointSim* joint, b3StepContext* context ) b3PrepareWheelJoint( joint, context ); break; + case b3_moverJoint: + b3PrepareMoverJoint( joint, context ); + break; + + case b3_pogoJoint: + b3PreparePogoJoint( joint, context ); + break; + default: B3_ASSERT( false ); } @@ -1566,6 +1698,14 @@ void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context ) b3WarmStartWheelJoint( joint, context ); break; + case b3_moverJoint: + b3WarmStartMoverJoint( joint, context ); + break; + + case b3_pogoJoint: + b3WarmStartPogoJoint( joint, context ); + break; + default: B3_ASSERT( false ); } @@ -1612,6 +1752,14 @@ void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias ) b3SolveWheelJoint( joint, context, useBias ); break; + case b3_moverJoint: + b3SolveMoverJoint( joint, context ); + break; + + case b3_pogoJoint: + b3SolvePogoJoint( joint, context, useBias ); + break; + default: B3_ASSERT( false ); } @@ -1728,6 +1876,14 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) b3DrawWheelJoint( draw, jointSim, transformA, transformB, scale ); break; + case b3_moverJoint: + b3DrawMoverJoint( draw, world, jointSim, transformA, transformB ); + break; + + case b3_pogoJoint: + b3DrawPogoJoint( draw, world, jointSim, transformA, transformB, scale ); + break; + default: draw->DrawSegmentFcn( transformA.p, pA, color, draw->context ); draw->DrawSegmentFcn( pA, pB, color, draw->context ); @@ -1738,10 +1894,12 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) if ( draw->drawGraphColors ) { b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { - b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, - b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, - b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, - b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, + b3_colorCyan, b3_colorBlue, b3_colorViolet, b3_colorPink, + b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, + b3_colorPlum, b3_colorSnow, b3_colorTeal, b3_colorKhaki, + b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, }; int colorIndex = joint->colorIndex; @@ -1752,13 +1910,14 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) } } - if ( draw->drawJointExtras ) + if ( draw->drawJointExtras && joint->type != b3_moverJoint && joint->type != b3_pogoJoint ) { b3Vec3 force = b3GetJointConstraintForce( world, joint ); b3Vec3 torque = b3GetJointConstraintTorque( world, joint ); b3Pos p = b3LerpPosition( pA, pB, 0.5f ); - draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( 0.001f, force ) ), b3_colorAzure, draw->context ); + draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( draw->forceScale, force ) ), + b3_colorAzure, draw->context ); char buffer[64]; snprintf( buffer, 64, "f = %g, t = %g", b3Length( force ), b3Length( torque ) ); diff --git a/src/joint.h b/src/joint.h index 9a8c347..45da807 100644 --- a/src/joint.h +++ b/src/joint.h @@ -121,6 +121,39 @@ typedef struct b3MotorJoint b3Matrix3 angularMass; } b3MotorJoint; +typedef struct b3MoverJoint +{ + b3Vec3 linearVelocity; + float maxForce; + + b3Vec3 axisX; + b3Vec3 axisZ; + b3Vec2 impulse; + + int indexA; + int indexB; + float linearMass; +} b3MoverJoint; + +typedef struct b3PogoJoint +{ + b3Vec3 normal; + float restLength; + float hertz; + float dampingRatio; + float maxTensionForce; + float maxCompressionForce; + + float impulse; + int indexA; + int indexB; + b3Transform frameA; + b3Transform frameB; + b3Vec3 deltaCenter; + float linearMass; + float velocity; +} b3PogoJoint; + typedef struct b3ParallelJoint { float hertz; @@ -336,7 +369,9 @@ typedef struct b3JointSim { b3DistanceJoint distanceJoint; b3MotorJoint motorJoint; + b3MoverJoint moverJoint; b3ParallelJoint parallelJoint; + b3PogoJoint pogoJoint; b3RevoluteJoint revoluteJoint; b3SphericalJoint sphericalJoint; b3PrismaticJoint prismaticJoint; @@ -365,6 +400,8 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ); b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base ); b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetMoverJointForce( b3World* world, b3JointSim* base ); +b3Vec3 b3GetPogoJointForce( b3World* world, b3JointSim* base ); b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base ); b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base ); b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base ); @@ -381,6 +418,8 @@ b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base ); void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context ); void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context ); +void b3PreparePogoJoint( b3JointSim* base, b3StepContext* context ); void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context ); void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ); void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context ); @@ -390,6 +429,8 @@ void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ); void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context ); void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartMoverJoint( b3JointSim* base, b3StepContext* context ); +void b3WarmStartPogoJoint( b3JointSim* base, b3StepContext* context ); void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context ); void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context ); void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context ); @@ -399,6 +440,8 @@ void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ); void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ); +void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context ); +void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context ); void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias ); @@ -407,6 +450,10 @@ void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB ); +void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, + b3WorldTransform transformB ); +void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, + b3WorldTransform transformB, float scale ); void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); diff --git a/src/mover_joint.c b/src/mover_joint.c new file mode 100644 index 0000000..51b0808 --- /dev/null +++ b/src/mover_joint.c @@ -0,0 +1,286 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +#include + +void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ) +{ + B3_ASSERT( b3IsValidVec3( velocity ) ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MoverJointSetLinearVelocity, jointId, velocity ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint ); + joint->moverJoint.linearVelocity = velocity; +} + +b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint ); + return joint->moverJoint.linearVelocity; +} + +void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ) +{ + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, MoverJointSetMaxVelocityForce, jointId, maxForce ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint ); + joint->moverJoint.maxForce = maxForce; +} + +float b3MoverJoint_GetMaxVelocityForce( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint ); + return joint->moverJoint.maxForce; +} + +b3Vec3 b3GetMoverJointForce( b3World* world, b3JointSim* base ) +{ + b3MoverJoint* joint = &base->moverJoint; + b3Vec3 P = b3Blend2( joint->impulse.x, joint->axisX, joint->impulse.y, joint->axisZ ); + return b3MulSV( world->inv_h, P ); +} + +void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_moverJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3World* world = context->world; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float mB = bodySimB->invMass; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = b3Mat3_zero; + base->invIB = b3Mat3_zero; + + b3MoverJoint* joint = &base->moverJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + joint->axisX = matrixB.cx; + joint->axisZ = matrixB.cz; + + float k = mA + mB; + joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->impulse = (b3Vec2){ 0 }; + } +} + +void b3WarmStartMoverJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_moverJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + + b3MoverJoint* joint = &base->moverJoint; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3BodyState* stateA = + joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = + joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 P = b3Blend2( joint->impulse.x, joint->axisX, joint->impulse.y, joint->axisZ ); + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, P ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, P ); + } +} + +void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_moverJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3MoverJoint* joint = &base->moverJoint; + b3BodyState* stateA = + joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = + joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 vB = stateB->linearVelocity; + + float maxForce = joint->maxForce; + b3Vec3 vr = b3Sub( b3Sub( vB, vA ), joint->linearVelocity ); + b3Vec2 cdot = { b3Dot( vr, joint->axisX ), b3Dot( vr, joint->axisZ ) }; + b3Vec2 deltaImpulse = b3MulSV2( -joint->linearMass, cdot ); + b3Vec2 newImpulse = { + joint->impulse.x + deltaImpulse.x, + joint->impulse.y + deltaImpulse.y, + }; + + float maxImpulse = context->h * maxForce; + + float lengthSquared = b3Dot2( newImpulse, newImpulse ); + if ( lengthSquared > maxImpulse * maxImpulse ) + { + float scale = maxImpulse / sqrtf( lengthSquared ); + newImpulse.x *= scale; + newImpulse.y *= scale; + } + + deltaImpulse = b3Sub2( newImpulse, joint->impulse ); + joint->impulse = newImpulse; + + b3Vec3 P = b3Blend2( deltaImpulse.x, joint->axisX, deltaImpulse.y, joint->axisZ ); + vA = b3MulSub( vA, mA, P ); + vB = b3MulAdd( vB, mB, P ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + } +} + +static b3HexColor b3LerpColor( b3HexColor a, b3HexColor b, float t ) +{ + uint32_t result = 0; + for ( int shift = 0; shift < 24; shift += 8 ) + { + float ca = (float)( ( (uint32_t)a >> shift ) & 0xFF ); + float cb = (float)( ( (uint32_t)b >> shift ) & 0xFF ); + uint32_t c = (uint32_t)( b3LerpFloat( ca, cb, t ) + 0.5f ); + result |= c << shift; + } + return (b3HexColor)result; +} + +static void b3DrawArrow( b3DebugDraw* draw, b3Pos a, b3Pos b, b3Vec3 planeNormal, float size, + b3HexColor color ) +{ + draw->DrawSegmentFcn( a, b, color, draw->context ); + + b3Vec3 shaft = b3SubPos( b, a ); + float shaftLen = b3Length( shaft ); + if ( shaftLen < 1e-6f ) + { + return; + } + + b3Vec3 dir = { shaft.x / shaftLen, shaft.y / shaftLen, shaft.z / shaftLen }; + float headLen = size; + + b3Vec3 perp = b3Normalize( b3Cross( planeNormal, dir ) ); + b3Vec3 backFromTip = b3MulSV( -headLen, dir ); + b3Vec3 sideStep = b3MulSV( headLen * 0.5f, perp ); + b3Pos tip1 = b3OffsetPos( b, b3Add( backFromTip, sideStep ) ); + b3Pos tip2 = b3OffsetPos( b, b3Sub( backFromTip, sideStep ) ); + draw->DrawSegmentFcn( b, tip1, color, draw->context ); + draw->DrawSegmentFcn( b, tip2, color, draw->context ); +} + +void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, + b3WorldTransform transformA, b3WorldTransform transformB ) +{ + B3_ASSERT( base->type == b3_moverJoint ); + B3_UNUSED( transformA ); + + b3MoverJoint* joint = &base->moverJoint; + + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( frameB.q ); + b3Vec3 axisX = matrixB.cx; + b3Vec3 axisZ = matrixB.cz; + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + b3BodyState* stateA = b3GetBodyState( world, bodyA ); + b3BodyState* stateB = b3GetBodyState( world, bodyB ); + b3Vec3 vA = stateA != NULL ? stateA->linearVelocity : b3Vec3_zero; + b3Vec3 vB = stateB != NULL ? stateB->linearVelocity : b3Vec3_zero; + + b3Vec3 velocityError = b3Sub( b3Sub( vB, vA ), joint->linearVelocity ); + b3Vec2 planarVelocityError = { b3Dot( velocityError, axisX ), b3Dot( velocityError, axisZ ) }; + b3Vec2 targetVelocity = { b3Dot( joint->linearVelocity, axisX ), + b3Dot( joint->linearVelocity, axisZ ) }; + + // Colorize ring by ratio of velocity error magnitude to target speed. + b3HexColor color = b3_colorWhite; + float targetSpeed = b3Length2( targetVelocity ); + if ( targetSpeed > 0.0f ) + { + float t = b3ClampFloat( b3Length2( planarVelocityError ) / targetSpeed, 0.0f, 1.0f ); + color = b3LerpColor( b3_colorWhite, b3_colorBlue, t ); + } + + float radius = draw->jointScale; + b3Vec3 rx = b3MulSV( radius, axisX ); + b3Vec3 rz = b3MulSV( radius, axisZ ); + const int segmentCount = 32; + b3CosSin cs = b3ComputeCosSin( 2.0f * B3_PI / segmentCount ); + float x1 = 1.0f, y1 = 0.0f; + b3Pos p1 = b3OffsetPos( frameB.p, rx ); + for ( int i = 0; i < segmentCount; ++i ) + { + float x2 = cs.cosine * x1 - cs.sine * y1; + float y2 = cs.sine * x1 + cs.cosine * y1; + b3Pos p2 = b3OffsetPos( frameB.p, b3Blend2( x2, rx, y2, rz ) ); + draw->DrawSegmentFcn( p1, p2, color, draw->context ); + x1 = x2; + y1 = y2; + p1 = p2; + } + + if ( joint->maxForce > 0.0f ) + { + b3Vec3 force = b3Blend2( joint->impulse.x, rx, joint->impulse.y, rz ); + b3Vec3 dir = b3Normalize( force ); + b3Pos origin = b3OffsetPos( frameB.p, b3MulSV( -radius, dir ) ); + float scale = world->inv_h / joint->maxForce; + b3Pos tip = b3OffsetPos( origin, b3MulSV( scale, force ) ); + b3DrawArrow( draw, origin, tip, matrixB.cy, 0.1f * draw->jointScale, b3_colorWhite ); + } +} diff --git a/src/physics_world.c b/src/physics_world.c index 192cfdb..ce86343 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -646,6 +646,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3WorldTransform transformA = bodySimA->transform; b3WorldTransform transformB = bodySimB->transform; + // This is really just needed for mesh collision to prevent tunneling. bool isFast = ( ( bodySimA->flags | bodySimB->flags ) & b3_isFast ) != 0; float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching; @@ -743,7 +744,8 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* contact->flags |= b3_relativeTransformValid; // This updates solid contacts - bool touching = b3UpdateContact( world, workerIndex, contact, shapeA, bodySimA->localCenter, transformA, shapeB, + bool touching = b3UpdateContact( world, workerIndex, contact, shapeA, bodySimA->localCenter, + transformA, shapeB, bodySimB->localCenter, transformB, isFast, taskContext->arena ); int bucketIndex = b3MinInt( contact->manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); diff --git a/src/pogo_joint.c b/src/pogo_joint.c new file mode 100644 index 0000000..db7bffc --- /dev/null +++ b/src/pogo_joint.c @@ -0,0 +1,449 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "math_internal.h" +#include "physics_world.h" +#include "recording.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +#include + +void b3PogoJoint_SetRestLength( b3JointId jointId, float length ) +{ + B3_ASSERT( b3IsValidFloat( length ) && length >= 0.0f ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + if ( joint->pogoJoint.restLength == length ) + { + return; + } + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PogoJointSetRestLength, jointId, length ); + joint->pogoJoint.restLength = length; +} + +float b3PogoJoint_GetRestLength( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.restLength; +} + +void b3PogoJoint_SetSpringHertz( b3JointId jointId, float hertz ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + if ( joint->pogoJoint.hertz == hertz ) + { + return; + } + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PogoJointSetSpringHertz, jointId, hertz ); + joint->pogoJoint.hertz = hertz; +} + +float b3PogoJoint_GetSpringHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.hertz; +} + +void b3PogoJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + if ( joint->pogoJoint.dampingRatio == dampingRatio ) + { + return; + } + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PogoJointSetSpringDampingRatio, jointId, dampingRatio ); + joint->pogoJoint.dampingRatio = dampingRatio; +} + +float b3PogoJoint_GetSpringDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.dampingRatio; +} + +void b3PogoJoint_SetMaxTensionForce( b3JointId jointId, float force ) +{ + B3_ASSERT( b3IsValidFloat( force ) && force >= 0.0f ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + if ( joint->pogoJoint.maxTensionForce == force ) + { + return; + } + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PogoJointSetMaxTensionForce, jointId, force ); + joint->pogoJoint.maxTensionForce = force; +} + +float b3PogoJoint_GetMaxTensionForce(b3JointId jointId) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.maxTensionForce; +} + +void b3PogoJoint_SetMaxCompressionForce(b3JointId jointId, float force) +{ + B3_ASSERT( b3IsValidFloat( force ) && force >= 0.0f ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + if ( joint->pogoJoint.maxCompressionForce == force ) + { + return; + } + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PogoJointSetMaxCompressionForce, jointId, force ); + joint->pogoJoint.maxCompressionForce = force; +} + +float b3PogoJoint_GetMaxCompressionForce(b3JointId jointId) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.maxCompressionForce; +} + +void b3PogoJoint_SetNormal( b3JointId jointId, b3Vec3 normal ) +{ + B3_ASSERT( b3IsValidVec3( normal ) && b3IsNormalized( normal ) ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + b3Vec3 current = joint->pogoJoint.normal; + if ( current.x == normal.x && current.y == normal.y && current.z == normal.z ) + { + return; + } + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, PogoJointSetNormal, jointId, normal ); + joint->pogoJoint.normal = normal; +} + +b3Vec3 b3PogoJoint_GetNormal( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.normal; +} + +float b3PogoJoint_GetCurrentLength( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + + b3WorldTransform worldTransformA = b3GetBodyTransform( world, base->bodyIdA ); + b3Transform transformA = b3ToRelativeTransform( worldTransformA, worldTransformA.p ); + b3Transform transformB = + b3ToRelativeTransform( b3GetBodyTransform( world, base->bodyIdB ), worldTransformA.p ); + + b3Vec3 axis = b3RotateVector( b3MulQuat( transformB.q, base->localFrameB.q ), b3Vec3_axisY ); + b3Vec3 pA = b3TransformPoint( transformA, base->localFrameA.p ); + b3Vec3 pB = b3TransformPoint( transformB, base->localFrameB.p ); + return b3Dot( b3Sub( pB, pA ), axis ); +} + +float b3PogoJoint_GetImpulse( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.impulse; +} + +float b3PogoJoint_GetCurrentSpeed( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_pogoJoint ); + return joint->pogoJoint.velocity; +} + +b3Vec3 b3GetPogoJointForce( b3World* world, b3JointSim* base ) +{ + return b3MulSV( world->inv_h * base->pogoJoint.impulse, base->pogoJoint.normal ); +} + +void b3PreparePogoJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_pogoJoint ); + + b3World* world = context->world; + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA ); + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float mB = bodySimB->invMass; + b3Matrix3 iA = bodySimA->invInertiaWorld; + b3Matrix3 iB = bodySimB->invInertiaWorld; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b3PogoJoint* joint = &base->pogoJoint; + joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; + joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + + // Compute joint anchor frames with world space rotation, relative to center of mass + joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + joint->frameA.p = b3RotateVector( bodySimA->transform.q, + b3Sub( base->localFrameA.p, bodySimA->localCenter ) ); + joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + joint->frameB.p = b3RotateVector( bodySimB->transform.q, + b3Sub( base->localFrameB.p, bodySimB->localCenter ) ); + + // Compute the initial center delta. Incremental position updates are relative to this. + joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center ); + + b3Vec3 rnA = b3Cross( joint->frameA.p, joint->normal ); + b3Vec3 rnB = b3Cross( joint->frameB.p, joint->normal ); + float k = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->velocity = 0.0f; + joint->impulse = 0.0f; + } +} + +void b3WarmStartPogoJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_pogoJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + b3PogoJoint* joint = &base->pogoJoint; + + if ( joint->hertz == 0.0f ) + { + joint->impulse = 0.0f; + return; + } + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3BodyState* stateA = + joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = + joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + b3Vec3 linearImpulse = b3MulSV( joint->impulse, joint->normal ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = + b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Cross( rA, linearImpulse ) ) ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity = + b3Add( stateB->angularVelocity, b3MulMV( iB, b3Cross( rB, linearImpulse ) ) ); + } +} + +void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + B3_ASSERT( base->type == b3_pogoJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + b3Matrix3 iA = base->invIA; + b3Matrix3 iB = base->invIB; + + // dummy state for static bodies + b3BodyState dummyState = b3_identityBodyState; + + b3PogoJoint* joint = &base->pogoJoint; + if ( joint->hertz == 0.0f ) + { + joint->impulse = 0.0f; + joint->velocity = 0.0f; + return; + } + + b3BodyState* stateA = + joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b3BodyState* stateB = + joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + float bias = 0.0f; + if ( useBias ) + { + // This is a custom pogo position correction designed to avoid feeding velocity into + // the mover body. Otherwise the mover can make huge jumps going up stairs. This still + // works with continuous collision. + + b3Vec3 dcA = stateA->deltaPosition; + b3Vec3 dcB = stateB->deltaPosition; + b3Vec3 d = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter ); + + // The pogo axis is usually fixed as the world y-axis. Different than the ground normal. + b3Vec3 pogoAxis = b3RotateVector( joint->frameB.q, b3Vec3_axisY ); + float c = b3Dot( pogoAxis, d ) - joint->restLength; + + // This could be divided by dot(joint->normal, pogoAxis) to account for the constraint + // direction, but I'd rather diminish the pogo recovery rate than risk making it huge. + joint->velocity = + b3SpringDamper( joint->hertz, joint->dampingRatio, c, joint->velocity, context->h ); + bias = -joint->velocity; + } + + b3Vec3 vr = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) ); + float cdot = b3Dot( joint->normal, vr ); + + float maxTensionImpulse = context->h * joint->maxTensionForce; + float maxCompressionImpulse = context->h * joint->maxCompressionForce; + float oldImpulse = joint->impulse; + float impulse = -joint->linearMass * ( cdot + bias ); + joint->impulse = + b3ClampFloat( joint->impulse + impulse, -maxTensionImpulse, maxCompressionImpulse ); + impulse = joint->impulse - oldImpulse; + + b3Vec3 P = b3MulSV( impulse, joint->normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, + b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + B3_ASSERT( base->type == b3_pogoJoint ); + + b3PogoJoint* joint = &base->pogoJoint; + + b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p ); + b3Vec3 axis = b3RotateVector( b3MulQuat( transformB.q, base->localFrameB.q ), b3Vec3_axisY ); + b3Vec3 perp1 = b3Perp( axis ); + b3Vec3 perp2 = b3Cross( axis, perp1 ); + + float length = b3Dot( b3SubPos( pB, pA ), axis ); + b3Pos foot = b3OffsetPos( pB, b3MulSV( -length, axis ) ); + + float force = world->inv_h * joint->impulse; + + // Tolerance for reaching max force. + float saturation = 0.99f; + + b3HexColor color; + if ( joint->hertz == 0.0f ) + { + color = b3_colorGray; + } + else if ( force > 0.0f && force >= saturation * joint->maxCompressionForce ) + { + color = b3_colorRed; + } + else if ( force < 0.0f && -force >= saturation * joint->maxTensionForce ) + { + color = b3_colorRed; + } + else if ( force > 0.0f ) + { + color = b3_colorLimeGreen; + } + else if ( force < 0.0f ) + { + color = b3_colorDeepSkyBlue; + } + else + { + color = b3_colorLightGray; + } + + // Spring + int segmentCount = 12; + float width = 0.1f * scale; + b3Pos p1 = pB; + for ( int i = 1; i <= segmentCount; ++i ) + { + float side = i == segmentCount ? 0.0f : ( i & 1 ) ? width : -width; + float t = (float)i / (float)segmentCount; + b3Pos p2 = b3OffsetPos( pB, b3MulAdd( b3MulSV( -t * length, axis ), side, perp2 ) ); + draw->DrawSegmentFcn( p1, p2, color, draw->context ); + p1 = p2; + } + + // Ground plane + b3Pos rest = b3OffsetPos( pB, b3MulSV( -joint->restLength, axis ) ); + float tick = 1.5f * width; + draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp1 ) ), + b3OffsetPos( rest, b3MulSV( tick, perp1 ) ), b3_colorWhite, + draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp2 ) ), + b3OffsetPos( rest, b3MulSV( tick, perp2 ) ), b3_colorWhite, + draw->context ); + + if ( b3Length( b3SubPos( pA, foot ) ) > B3_LINEAR_SLOP ) + { + draw->DrawSegmentFcn( foot, pA, b3_colorDimGray, draw->context ); + } + + draw->DrawPointFcn( pA, 6.0f, color, draw->context ); + draw->DrawSegmentFcn( pA, b3OffsetPos( pA, b3MulSV( 0.5f * scale, joint->normal ) ), + b3_colorOrange, draw->context ); + + if ( draw->drawJointExtras ) + { + char buffer[96]; + snprintf( buffer, 96, "length = %.3f, rest = %.3f, f = %.1f, v = %.3f", length, + joint->restLength, force, joint->velocity ); + draw->DrawStringFcn( pB, buffer, color, draw->context ); + } +} diff --git a/src/recording.c b/src/recording.c index 7e9bc03..e3cdced 100644 --- a/src/recording.c +++ b/src/recording.c @@ -342,6 +342,10 @@ _Static_assert( sizeof( void* ) != 8 || sizeof( b3WeldJointDef ) == 128, "b3WeldJointDef changed: update b3RecW_WELDJOINTDEF and its reader together" ); _Static_assert( sizeof( void* ) != 8 || sizeof( b3WheelJointDef ) == 184, "b3WheelJointDef changed: update b3RecW_WHEELJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3MoverJointDef ) == 128, + "b3MoverJointDef changed: update b3RecW_MOVERJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3PogoJointDef ) == 152, + "b3PogoJointDef changed: update b3RecW_POGOJOINTDEF and its reader together" ); void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v ) { @@ -548,6 +552,26 @@ void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v ) b3RecW_F32( buf, v.upperSteeringLimit ); } +void b3RecW_MOVERJOINTDEF( b3RecBuffer* buf, b3MoverJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_VEC3( buf, v.linearVelocity ); + b3RecW_F32( buf, v.maxForce ); +} + +void b3RecW_POGOJOINTDEF( b3RecBuffer* buf, b3PogoJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_VEC3( buf, v.normal ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_F32( buf, v.restLength ); + b3RecW_F32( buf, v.maxTensionForce ); + b3RecW_F32( buf, v.maxCompressionForce ); + b3RecW_F32( buf, v.impulse ); + b3RecW_F32( buf, v.velocity ); +} + // Query recording. A query collects a variable number of hits through a user callback, so the count // is not known until the callback stops firing. The record is built in a local buffer with a // reserved hit-count slot, then committed whole under the lock so concurrent query threads never diff --git a/src/recording.h b/src/recording.h index 6f9df4d..1afd800 100644 --- a/src/recording.h +++ b/src/recording.h @@ -52,7 +52,7 @@ typedef struct b3World b3World; // Minor tracks op-stream additions that keep the 48 byte header shape. // Minor version 7 made the contact body sim locators persistent -#define B3_REC_VERSION_MINOR 9 +#define B3_REC_VERSION_MINOR 10 // File header, fixed 48 bytes. Contains the registry locator so the player // can load geometry before replaying any ops. @@ -196,6 +196,8 @@ typedef b3RevoluteJointDef b3RecCType_REVOLUTEJOINTDEF; typedef b3SphericalJointDef b3RecCType_SPHERICALJOINTDEF; typedef b3WeldJointDef b3RecCType_WELDJOINTDEF; typedef b3WheelJointDef b3RecCType_WHEELJOINTDEF; +typedef b3MoverJointDef b3RecCType_MOVERJOINTDEF; +typedef b3PogoJointDef b3RecCType_POGOJOINTDEF; // Codegen pass 1a: arg structs. Generated here so call sites (body.c, shape.c, etc.) can see them. #define ARG( TAG, field ) b3RecCType_##TAG field; @@ -266,6 +268,8 @@ void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v ); void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v ); void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v ); void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v ); +void b3RecW_MOVERJOINTDEF( b3RecBuffer* buf, b3MoverJointDef v ); +void b3RecW_POGOJOINTDEF( b3RecBuffer* buf, b3PogoJointDef v ); // Record framing void b3RecBeginRecord( b3Recording* rec, uint8_t opcode ); diff --git a/src/recording_ops.inl b/src/recording_ops.inl index ed4b7d6..df13ebe 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -15,6 +15,7 @@ // 0x40-0x4F shape create/destroy // 0x50-0x6F shape mutators // 0x80 step +// 0x81-0x8F mover and pogo joints // 0x90-0xE7 joints (create, generic, per-type) // 0xE8-0xEF spatial queries // 0xF0-0xFF markers @@ -211,6 +212,20 @@ B3_REC_OP( 0xE5, WheelJointEnableSteeringLimit, RET_NONE, ARG( JOINTID, joint ) B3_REC_OP( 0xE6, WheelJointSetSteeringLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) ) B3_REC_OP( 0xE7, WheelJointSetTargetSteeringAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, radians ) ) +// Mover joint +B3_REC_OP( 0x81, CreateMoverJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOVERJOINTDEF, def ) ) +B3_REC_OP( 0x82, MoverJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) ) +B3_REC_OP( 0x83, MoverJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) + +// Pogo joint +B3_REC_OP( 0x84, CreatePogoJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( POGOJOINTDEF, def ) ) +B3_REC_OP( 0x85, PogoJointSetRestLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) ) +B3_REC_OP( 0x86, PogoJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ) +B3_REC_OP( 0x87, PogoJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) ) +B3_REC_OP( 0x88, PogoJointSetMaxTensionForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) +B3_REC_OP( 0x89, PogoJointSetMaxCompressionForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) ) +B3_REC_OP( 0x8A, PogoJointSetNormal, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, normal ) ) + // Spatial queries. Inputs flow through the manifest (reader side). The hit tail and result are // hand-written in recording.c / recording_replay.c since they are variable length. B3_REC_OP( 0xE8, QueryOverlapAABB, RET_NONE, ARG( WORLDID, world ) ARG( AABB, aabb ) ARG( QUERYFILTER, filter ) ) diff --git a/src/replay.c b/src/replay.c index 38bda1a..78b73df 100644 --- a/src/replay.c +++ b/src/replay.c @@ -649,6 +649,30 @@ b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr ) return def; } +b3MoverJointDef b3RecR_MOVERJOINTDEF( b3RecReader* rdr ) +{ + b3MoverJointDef def = b3DefaultMoverJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.linearVelocity = b3RecR_VEC3( rdr ); + def.maxForce = b3RecR_F32( rdr ); + return def; +} + +b3PogoJointDef b3RecR_POGOJOINTDEF( b3RecReader* rdr ) +{ + b3PogoJointDef def = b3DefaultPogoJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.normal = b3RecR_VEC3( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.restLength = b3RecR_F32( rdr ); + def.maxTensionForce = b3RecR_F32( rdr ); + def.maxCompressionForce = b3RecR_F32( rdr ); + def.impulse = b3RecR_F32( rdr ); + def.velocity = b3RecR_F32( rdr ); + return def; +} + // Outliner body tracking. Defined after the player struct; forward declared here because the // create/destroy dispatch sits above that struct. static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id ); @@ -1262,6 +1286,24 @@ static void b3RecDispatch_CreateWheelJoint( const b3RecArgs_CreateWheelJoint* a, b3RecCheckJointId( rdr, b3CreateWheelJoint( rdr->replayWorldId, &def ), recId ); } +static void b3RecDispatch_CreateMoverJoint( const b3RecArgs_CreateMoverJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3MoverJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateMoverJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreatePogoJoint( const b3RecArgs_CreatePogoJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3PogoJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreatePogoJoint( rdr->replayWorldId, &def ), recId ); +} + static void b3RecDispatch_DestroyJoint( const b3RecArgs_DestroyJoint* a, b3RecReader* rdr ) { b3DestroyJoint( b3RecMakeJointId( rdr, a->joint ), a->wakeAttached ); @@ -1668,6 +1710,53 @@ static void b3RecDispatch_WheelJointSetTargetSteeringAngle( const b3RecArgs_Whee b3WheelJoint_SetTargetSteeringAngle( b3RecMakeJointId( rdr, a->joint ), a->radians ); } +static void b3RecDispatch_MoverJointSetLinearVelocity( + const b3RecArgs_MoverJointSetLinearVelocity* a, b3RecReader* rdr ) +{ + b3MoverJoint_SetLinearVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity ); +} + +static void b3RecDispatch_MoverJointSetMaxVelocityForce( + const b3RecArgs_MoverJointSetMaxVelocityForce* a, b3RecReader* rdr ) +{ + b3MoverJoint_SetMaxVelocityForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce ); +} + +static void b3RecDispatch_PogoJointSetRestLength( const b3RecArgs_PogoJointSetRestLength* a, + b3RecReader* rdr ) +{ + b3PogoJoint_SetRestLength( b3RecMakeJointId( rdr, a->joint ), a->length ); +} + +static void b3RecDispatch_PogoJointSetSpringHertz( const b3RecArgs_PogoJointSetSpringHertz* a, + b3RecReader* rdr ) +{ + b3PogoJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_PogoJointSetSpringDampingRatio( + const b3RecArgs_PogoJointSetSpringDampingRatio* a, b3RecReader* rdr ) +{ + b3PogoJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_PogoJointSetMaxTensionForce( + const b3RecArgs_PogoJointSetMaxTensionForce* a, b3RecReader* rdr ) +{ + b3PogoJoint_SetMaxTensionForce( b3RecMakeJointId( rdr, a->joint ), a->force ); +} + +static void b3RecDispatch_PogoJointSetMaxCompressionForce( + const b3RecArgs_PogoJointSetMaxCompressionForce* a, b3RecReader* rdr ) +{ + b3PogoJoint_SetMaxCompressionForce( b3RecMakeJointId( rdr, a->joint ), a->force ); +} + +static void b3RecDispatch_PogoJointSetNormal( const b3RecArgs_PogoJointSetNormal* a, b3RecReader* rdr ) +{ + b3PogoJoint_SetNormal( b3RecMakeJointId( rdr, a->joint ), a->normal ); +} + static void b3RecDispatch_StateHash( const b3RecArgs_StateHash* a, b3RecReader* rdr ) { b3World* world = b3GetWorldFromId( rdr->replayWorldId ); diff --git a/src/replay.h b/src/replay.h index ea2bae8..8aeddd9 100644 --- a/src/replay.h +++ b/src/replay.h @@ -246,6 +246,8 @@ b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr ); b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr ); b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr ); b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr ); +b3MoverJointDef b3RecR_MOVERJOINTDEF( b3RecReader* rdr ); +b3PogoJointDef b3RecR_POGOJOINTDEF( b3RecReader* rdr ); b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr ); b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr ); b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr ); diff --git a/src/simd_avx2.h b/src/simd_avx2.h index a751249..36c79d6 100644 --- a/src/simd_avx2.h +++ b/src/simd_avx2.h @@ -92,14 +92,6 @@ static inline b3FloatW8 b3MaxW8( b3FloatW8 a, b3FloatW8 b ) return _mm256_max_ps( a, b ); } -// Clamp a to [-b, b] -static inline b3FloatW8 b3SymClampW8( b3FloatW8 a, b3FloatW8 b ) -{ - b3FloatW8 nb = b3NegW8( b ); - b3FloatW8 c = b3MaxW8( nb, a ); - return b3MinW8( c, b ); -} - static inline b3FloatW8 b3AndW8( b3FloatW8 a, b3FloatW8 b ) { return _mm256_and_ps( a, b ); diff --git a/src/simd_neon.h b/src/simd_neon.h index c35f39b..ce9b560 100644 --- a/src/simd_neon.h +++ b/src/simd_neon.h @@ -89,14 +89,6 @@ static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) return vmaxq_f32( a, b ); } -// clamp a to [-b, b] -static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) -{ - b3FloatW4 nb = b3NegW4( b ); - b3FloatW4 c = b3MaxW4( nb, a ); - return b3MinW4( c, b ); -} - static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) { return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); diff --git a/src/simd_scalar.h b/src/simd_scalar.h index 56cb045..8ae1b85 100644 --- a/src/simd_scalar.h +++ b/src/simd_scalar.h @@ -104,21 +104,6 @@ static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) return r; } -// clamp a to [-b, b] -static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) -{ - b3FloatW4 r; - r.x = a.x <= b.x ? a.x : b.x; - r.y = a.y <= b.y ? a.y : b.y; - r.z = a.z <= b.z ? a.z : b.z; - r.w = a.w <= b.w ? a.w : b.w; - r.x = r.x <= -b.x ? -b.x : r.x; - r.y = r.y <= -b.y ? -b.y : r.y; - r.z = r.z <= -b.z ? -b.z : r.z; - r.w = r.w <= -b.w ? -b.w : r.w; - return r; -} - // Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD. static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) diff --git a/src/simd_sse2.h b/src/simd_sse2.h index b09a17c..8041c47 100644 --- a/src/simd_sse2.h +++ b/src/simd_sse2.h @@ -92,14 +92,6 @@ static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) return _mm_max_ps( a, b ); } -// clamp a to [-b, b] -static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) -{ - b3FloatW4 nb = b3NegW4( b ); - b3FloatW4 c = b3MaxW4( nb, a ); - return b3MinW4( c, b ); -} - static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) { return _mm_and_ps( a, b ); diff --git a/src/simd_wide.h b/src/simd_wide.h index fe0bb9c..f7c8879 100644 --- a/src/simd_wide.h +++ b/src/simd_wide.h @@ -41,7 +41,6 @@ typedef B3_WIDE( b3Vec3 ) b3Vec3W; #define b3MulAddW B3_WIDE( b3MulAdd ) #define b3MinW B3_WIDE( b3Min ) #define b3MaxW B3_WIDE( b3Max ) -#define b3SymClampW B3_WIDE( b3SymClamp ) #define b3AndW B3_WIDE( b3And ) #define b3OrW B3_WIDE( b3Or ) #define b3AndNotW B3_WIDE( b3AndNot ) diff --git a/src/types.c b/src/types.c index 9caab0a..f5e1ad3 100644 --- a/src/types.c +++ b/src/types.c @@ -153,7 +153,7 @@ b3DebugDraw b3DefaultDebugDraw( void ) }; draw.jointScale = 1.0f; - draw.forceScale = 1.0f; + draw.forceScale = 0.001f; return draw; } diff --git a/src/wheel_joint.c b/src/wheel_joint.c index 05665e9..fb229c3 100644 --- a/src/wheel_joint.c +++ b/src/wheel_joint.c @@ -1072,14 +1072,16 @@ void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform tra float upper = joint->upperSteeringLimit; b3CosSin cs = b3ComputeCosSin( lower ); + b3CosSin delta = b3ComputeCosSin( ( upper - lower ) / sliceCount ); b3Pos vertex1 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); for ( int index = 0; index < sliceCount; ++index ) { - float t2 = ( index + 1.0f ) / sliceCount; - float phi = b3LerpFloat( lower, upper, t2 ); + float cosine = delta.cosine * cs.cosine - delta.sine * cs.sine; + float sine = delta.sine * cs.cosine + delta.cosine * cs.sine; + cs.cosine = cosine; + cs.sine = sine; - cs = b3ComputeCosSin( phi ); b3Pos vertex2 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); if ( index == 0 ) diff --git a/test/test_joint.c b/test/test_joint.c index 4e38d75..103e6d7 100644 --- a/test/test_joint.c +++ b/test/test_joint.c @@ -296,6 +296,116 @@ static int TestMotorJoint( void ) return FinishJoint( jointId, f.worldId ); } +static int TestMoverJoint( void ) +{ + JointFixture f = CreateJointFixture(); + + b3MoverJointDef def = b3DefaultMoverJointDef(); + SetCommonFrames( &def.base, &f ); + b3JointId jointId = b3CreateMoverJoint( f.worldId, &def ); + + if ( ExerciseJointBase( jointId, f.worldId, f.groundId, f.bodyId, b3_moverJoint ) != 0 ) + { + return 1; + } + + b3Vec3 velocity = { 1.0f, 2.0f, -3.0f }; + b3MoverJoint_SetLinearVelocity( jointId, velocity ); + b3Vec3 gotVelocity = b3MoverJoint_GetLinearVelocity( jointId ); + ENSURE( gotVelocity.x == 1.0f && gotVelocity.y == 2.0f && gotVelocity.z == -3.0f ); + + // The y axis has no force budget, so it must stay at rest while x and z reach the target + float maxForce = 1000.0f; + b3MoverJoint_SetMaxVelocityForce( jointId, maxForce ); + float gotForce = b3MoverJoint_GetMaxVelocityForce( jointId ); + ENSURE( gotForce == 1000.0f ); + + for ( int i = 0; i < 30; ++i ) + { + b3World_Step( f.worldId, 1.0f / 60.0f, 4 ); + } + + b3Vec3 v = b3Body_GetLinearVelocity( f.bodyId ); + ENSURE_SMALL( v.x - 1.0f, 1.0e-4f ); + ENSURE_SMALL( v.y, 1.0e-4f ); + ENSURE_SMALL( v.z + 3.0f, 1.0e-4f ); + + b3Vec3 w = b3Body_GetAngularVelocity( f.bodyId ); + ENSURE( w.x == 0.0f && w.y == 0.0f && w.z == 0.0f ); + + return FinishJoint( jointId, f.worldId ); +} + +// A box held up by a pogo spring under gravity should settle at the rest length +static int TestPogoJoint( void ) +{ + JointFixture f = CreateJointFixture(); + b3World_SetGravity( f.worldId, (b3Vec3){ 0.0f, -10.0f, 0.0f } ); + + float restLength = 2.0f; + float mass = b3Body_GetMass( f.bodyId ); + + b3PogoJointDef def = b3DefaultPogoJointDef(); + def.base.bodyIdA = f.groundId; + def.base.bodyIdB = f.bodyId; + def.base.localFrameA.p = (b3Vec3){ 0.0f, 1.0f, 0.0f }; + def.base.localFrameB.p = (b3Vec3){ 0.0f, 0.0f, 0.0f }; + def.normal = b3Vec3_axisY; + def.restLength = restLength; + def.hertz = 2.0f; + def.dampingRatio = 1.0f; + def.maxCompressionForce = 100.0f * 10.0f * mass; + def.maxTensionForce = 100.0f * 10.0f * mass; + b3JointId jointId = b3CreatePogoJoint( f.worldId, &def ); + + if ( ExerciseJointBase( jointId, f.worldId, f.groundId, f.bodyId, b3_pogoJoint ) != 0 ) + { + return 1; + } + + b3PogoJoint_SetRestLength( jointId, restLength ); + ENSURE( b3PogoJoint_GetRestLength( jointId ) == restLength ); + + b3PogoJoint_SetSpringHertz( jointId, 3.0f ); + ENSURE( b3PogoJoint_GetSpringHertz( jointId ) == 3.0f ); + + b3PogoJoint_SetSpringDampingRatio( jointId, 0.9f ); + ENSURE( b3PogoJoint_GetSpringDampingRatio( jointId ) == 0.9f ); + + float maxTension = 50.0f * 10.0f * mass; + b3PogoJoint_SetMaxTensionForce( jointId, maxTension ); + ENSURE( b3PogoJoint_GetMaxTensionForce( jointId ) == maxTension ); + + float maxCompression = 60.0f * 10.0f * mass; + b3PogoJoint_SetMaxCompressionForce( jointId, maxCompression ); + ENSURE( b3PogoJoint_GetMaxCompressionForce( jointId ) == maxCompression ); + + b3Vec3 normal = b3Normalize( (b3Vec3){ 0.2f, 1.0f, 0.0f } ); + b3PogoJoint_SetNormal( jointId, normal ); + b3Vec3 gotNormal = b3PogoJoint_GetNormal( jointId ); + ENSURE( gotNormal.x == normal.x && gotNormal.y == normal.y && gotNormal.z == normal.z ); + b3PogoJoint_SetNormal( jointId, b3Vec3_axisY ); + + // Starts stretched by one meter + ENSURE_SMALL( b3PogoJoint_GetCurrentLength( jointId ) - 3.0f, 1.0e-5f ); + + for ( int i = 0; i < 300; ++i ) + { + b3World_Step( f.worldId, 1.0f / 60.0f, 4 ); + } + + ENSURE_SMALL( b3PogoJoint_GetCurrentLength( jointId ) - restLength, 0.01f ); + ENSURE_SMALL( b3Body_GetLinearVelocity( f.bodyId ).y, 0.01f ); + + // Holding the weight takes an impulse of m * g * h each sub-step + ENSURE_SMALL( b3PogoJoint_GetImpulse( jointId ) - mass * 10.0f / 240.0f, 0.001f * mass ); + + b3Vec3 force = b3Joint_GetConstraintForce( jointId ); + ENSURE_SMALL( force.y - mass * 10.0f, 0.1f * mass ); + + return FinishJoint( jointId, f.worldId ); +} + static int TestPrismaticJoint( void ) { JointFixture f = CreateJointFixture(); @@ -673,6 +783,8 @@ int JointTest( void ) RUN_SUBTEST( TestDistanceJoint ); RUN_SUBTEST( TestFilterJoint ); RUN_SUBTEST( TestMotorJoint ); + RUN_SUBTEST( TestMoverJoint ); + RUN_SUBTEST( TestPogoJoint ); RUN_SUBTEST( TestPrismaticJoint ); RUN_SUBTEST( TestRevoluteJoint ); RUN_SUBTEST( TestSphericalJoint ); diff --git a/test/test_recording.c b/test/test_recording.c index 12f7316..46f1e30 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -76,8 +76,9 @@ static int HullDedup( void ) { // Build a small convex hull b3Vec3 pts[8] = { - { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, - { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, }; b3HullData* hull = b3CreateHull( pts, 8, 8 ); ENSURE( hull != NULL ); @@ -127,7 +128,8 @@ static int HullDedup( void ) // entryCount is a little-endian u32 at the start of the registry block const uint8_t* rp = bytes + (int)regOff; - uint32_t entryCount = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | ( (uint32_t)rp[3] << 24 ); + uint32_t entryCount = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | + ( (uint32_t)rp[3] << 24 ); ENSURE( entryCount == 1 ); b3DestroyRecording( rec ); @@ -174,7 +176,8 @@ static int CrossWidthReplay( void ) for ( int k = 0; k < 3; ++k ) { seed = seed * 1664525u + 1013904223u; - v[k] = ( (float)( seed >> 8 ) / (float)( 1 << 24 ) - 0.5f ) * ( k == 1 ? 0.6f : 1.0f ); + v[k] = + ( (float)( seed >> 8 ) / (float)( 1 << 24 ) - 0.5f ) * ( k == 1 ? 0.6f : 1.0f ); } points[i] = (b3Vec3){ v[0], v[1], v[2] }; } @@ -204,8 +207,10 @@ static int CrossWidthReplay( void ) { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; - bodyDef.position = (b3Pos){ 0.4f * (float)( i % 3 ) - 0.4f, 1.5f + 0.9f * (float)i, 0.3f * (float)( i % 2 ) }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.37f * (float)i ); + bodyDef.position = (b3Pos){ 0.4f * (float)( i % 3 ) - 0.4f, 1.5f + 0.9f * (float)i, + 0.3f * (float)( i % 2 ) }; + bodyDef.rotation = + b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.37f * (float)i ); b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); b3CreateHullShape( bodyId, &dynamicShape, hulls[i % hullCount] ); } @@ -452,8 +457,9 @@ static int ScrubBackward( void ) static int SeekWithHull( void ) { b3Vec3 pts[8] = { - { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, - { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, }; b3HullData* hull = b3CreateHull( pts, 8, 8 ); ENSURE( hull != NULL ); @@ -549,7 +555,8 @@ static void RecTestDestroyDebugShape( void* userShape, void* userContext ) counters->destroyed += 1; } -static void RecTestDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, void* context ) +static void RecTestDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, + void* context ) { (void)userShape; (void)transform; @@ -618,12 +625,14 @@ static int DebugShapeCallbacks( void ) b3Recording* loaded = b3LoadRecordingFromFile( path ); ENSURE( loaded != NULL ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); ENSURE( player != NULL ); // Wiring the callbacks rebuilds the world and rewinds to frame 0. DebugShapeCounters counters = { 0, 0 }; - b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, &counters ); + b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, + &counters ); ENSURE( b3RecPlayer_GetFrame( player ) == 0 ); // Replay to the end, then draw: createDebugShape fires once per shape (ground + 4 boxes = 5). @@ -635,8 +644,8 @@ static int DebugShapeCallbacks( void ) ENSURE( counters.created >= 5 ); ENSURE( !b3RecPlayer_HasDiverged( player ) ); - // A backward seek restores the empty seed in place, releasing the live debug shapes, then forward - // stepping recreates them through the callbacks, so more creates fire. + // A backward seek restores the empty seed in place, releasing the live debug shapes, then + // forward stepping recreates them through the callbacks, so more creates fire. int createdBefore = counters.created; b3RecPlayer_SeekFrame( player, 0 ); b3RecPlayer_SeekFrame( player, totalFrames ); @@ -705,7 +714,8 @@ static int PlayerAccessors( void ) b3World_StopRecording( worldId ); b3DestroyWorld( worldId ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); ENSURE( player != NULL ); // Info reflects the recorded tuning and a non-degenerate bounds. @@ -754,10 +764,11 @@ static int PlayerAccessors( void ) return 0; } -// A keyframe restore is a deterministic replay state, so shapes that persist must keep their renderer -// handle rather than being torn down and rebuilt every seek. Record a snapshot-seeded session (shapes -// exist at frame 0), replay with handle callbacks, scrub backward across keyframes repeatedly, and -// verify no new handles are built per restore and the create/destroy counts still balance at teardown. +// A keyframe restore is a deterministic replay state, so shapes that persist must keep their +// renderer handle rather than being torn down and rebuilt every seek. Record a snapshot-seeded +// session (shapes exist at frame 0), replay with handle callbacks, scrub backward across keyframes +// repeatedly, and verify no new handles are built per restore and the create/destroy counts still +// balance at teardown. static int KeyframeHandleReuse( void ) { b3WorldDef worldDef = b3DefaultWorldDef(); @@ -803,11 +814,13 @@ static int KeyframeHandleReuse( void ) b3World_StopRecording( worldId ); b3DestroyWorld( worldId ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); ENSURE( player != NULL ); DebugShapeCounters counters = { 0, 0 }; - b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, &counters ); + b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, + &counters ); // Replay to the end and draw: one handle per shape. b3RecPlayer_SeekFrame( player, totalFrames ); @@ -815,8 +828,8 @@ static int KeyframeHandleReuse( void ) ENSURE( !b3RecPlayer_HasDiverged( player ) ); ENSURE( counters.created == shapeCount ); - // Scrub backward and forward across keyframes. Each restore keeps the persistent shapes' handles, - // so drawing builds no new ones. + // Scrub backward and forward across keyframes. Each restore keeps the persistent shapes' + // handles, so drawing builds no new ones. int createdAfterFirstDraw = counters.created; int seekTargets[] = { 40, 70, 20, 60, 8, 75 }; for ( int k = 0; k < (int)( sizeof( seekTargets ) / sizeof( seekTargets[0] ) ); ++k ) @@ -841,8 +854,9 @@ static bool QueryReplayOverlapFcn( b3ShapeId shapeId, void* context ) return true; } -static float QueryReplayCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, - int triangleIndex, int childIndex, void* context ) +static float QueryReplayCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, + uint64_t userMaterialId, int triangleIndex, int childIndex, + void* context ) { (void)shapeId; (void)point; @@ -855,8 +869,9 @@ static float QueryReplayCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, return fraction; } -static float CompoundHitCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, - int triangleIndex, int childIndex, void* context ) +static float CompoundHitCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, + uint64_t userMaterialId, int triangleIndex, int childIndex, + void* context ) { (void)point; (void)normal; @@ -871,7 +886,8 @@ static float CompoundHitCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, return 1.0f; } -static bool QueryReplayPlaneFcn( b3ShapeId shapeId, const b3PlaneResult* planes, int planeCount, void* context ) +static bool QueryReplayPlaneFcn( b3ShapeId shapeId, const b3PlaneResult* planes, int planeCount, + void* context ) { (void)shapeId; (void)planes; @@ -887,9 +903,9 @@ static bool QueryReplayMoverFilterFcn( b3ShapeId shapeId, void* context ) return true; } -// Issue all seven world queries each frame, then replay. Every query is re-issued against the replay -// world and compared to what was recorded, so a clean (non-diverged) replay proves the queries -// reproduce. Also opens a player and confirms the per-frame query store surfaces all seven. +// Issue all seven world queries each frame, then replay. Every query is re-issued against the +// replay world and compared to what was recorded, so a clean (non-diverged) replay proves the +// queries reproduce. Also opens a player and confirms the per-frame query store surfaces all seven. static int QueryReplay( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -939,7 +955,8 @@ static int QueryReplay( void ) b3World_CastRay( worldId, origin, translation, filter, QueryReplayCastFcn, NULL ); b3World_CastRayClosest( worldId, origin, translation, filter ); b3World_CastShape( worldId, origin, &proxy, translation, filter, QueryReplayCastFcn, NULL ); - b3World_CastMover( worldId, origin, &mover, translation, filter, QueryReplayMoverFilterFcn, NULL ); + b3World_CastMover( worldId, origin, &mover, translation, filter, QueryReplayMoverFilterFcn, + NULL ); b3World_CollideMover( worldId, origin, &mover, filter, QueryReplayPlaneFcn, NULL ); b3World_Step( worldId, 1.0f / 60.0f, 4 ); @@ -952,7 +969,8 @@ static int QueryReplay( void ) ENSURE( b3ValidateReplay( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ) ); // Player path: seek to a mid frame and confirm the per-frame store holds all seven queries. - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); ENSURE( player != NULL ); b3RecPlayer_SeekFrame( player, 15 ); @@ -980,10 +998,10 @@ static int QueryReplay( void ) return 0; } -// Tagged queries: the caller (id, label) is hashed into a key that rides the QueryTag op, with the id -// and label interned in the trailing tag table. The same label under two entity ids is two distinct -// keys. All survive a file round-trip and resolve back through b3RecQueryInfo; untagged queries report -// key 0 / id 0 / name NULL. +// Tagged queries: the caller (id, label) is hashed into a key that rides the QueryTag op, with the +// id and label interned in the trailing tag table. The same label under two entity ids is two +// distinct keys. All survive a file round-trip and resolve back through b3RecQueryInfo; untagged +// queries report key 0 / id 0 / name NULL. static int TaggedQuery( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -1042,7 +1060,8 @@ static int TaggedQuery( void ) b3Recording* loaded = b3LoadRecordingFromFile( path ); ENSURE( loaded != NULL ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); ENSURE( player != NULL ); b3RecPlayer_SeekFrame( player, 5 ); @@ -1178,7 +1197,8 @@ static int AllOps( void ) b3ShapeDef sphereShapeDef = b3DefaultShapeDef(); sphereShapeDef.density = 1.0f; - // Over-length shape name so replay exercises the clamp in the shape def reader, like the body above. + // Over-length shape name so replay exercises the clamp in the shape def reader, like the body + // above. sphereShapeDef.name = "sphereNameThatExceedsTheLimit"; b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; b3ShapeId sphereShapeId = b3CreateSphereShape( bodyId, &sphereShapeDef, &sphere ); @@ -1199,8 +1219,9 @@ static int AllOps( void ) // Custom hull shape on a third dynamic body b3Vec3 hullPts[8] = { - { -0.5f, -0.5f, -0.5f }, { 0.5f, -0.5f, -0.5f }, { 0.5f, 0.5f, -0.5f }, { -0.5f, 0.5f, -0.5f }, - { -0.5f, -0.5f, 0.5f }, { 0.5f, -0.5f, 0.5f }, { 0.5f, 0.5f, 0.5f }, { -0.5f, 0.5f, 0.5f }, + { -0.5f, -0.5f, -0.5f }, { 0.5f, -0.5f, -0.5f }, { 0.5f, 0.5f, -0.5f }, + { -0.5f, 0.5f, -0.5f }, { -0.5f, -0.5f, 0.5f }, { 0.5f, -0.5f, 0.5f }, + { 0.5f, 0.5f, 0.5f }, { -0.5f, 0.5f, 0.5f }, }; b3HullData* customHull = b3CreateHull( hullPts, 8, 8 ); ENSURE( customHull != NULL ); @@ -1237,9 +1258,10 @@ static int AllOps( void ) ENSURE( b3Body_IsValid( xformBodyId ) ); b3ShapeDef xformShapeDef = b3DefaultShapeDef(); xformShapeDef.density = 1.0f; - b3Transform xformXf = { (b3Vec3){ 0.1f, 0.2f, -0.1f }, b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.4f ) }; - b3ShapeId xformShapeId = - b3CreateTransformedHullShape( xformBodyId, &xformShapeDef, customHull, xformXf, (b3Vec3){ 1.25f, 0.75f, 1.5f } ); + b3Transform xformXf = { (b3Vec3){ 0.1f, 0.2f, -0.1f }, + b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.4f ) }; + b3ShapeId xformShapeId = b3CreateTransformedHullShape( + xformBodyId, &xformShapeDef, customHull, xformXf, (b3Vec3){ 1.25f, 0.75f, 1.5f } ); ENSURE( b3Shape_IsValid( xformShapeId ) ); // Mesh, height field, and compound static shapes (3D-only) @@ -1252,7 +1274,8 @@ static int AllOps( void ) b3MeshData* swapMeshData = b3CreateGridMesh( 4, 4, 1.5f, 0, false ); ENSURE( swapMeshData != NULL ); b3ShapeDef meshShapeDef = b3DefaultShapeDef(); - b3ShapeId meshShapeId = b3CreateMeshShape( meshBodyId, &meshShapeDef, meshData, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + b3ShapeId meshShapeId = + b3CreateMeshShape( meshBodyId, &meshShapeDef, meshData, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); ENSURE( b3Shape_IsValid( meshShapeId ) ); b3BodyDef hfBodyDef = b3DefaultBodyDef(); @@ -1339,8 +1362,8 @@ static int AllOps( void ) // Body mutators: SetTransform, SetLinearVelocity/AngularVelocity (Vec3), SetName, // damping, gravity scale, sleep threshold, SetAwake, EnableSleep, SetBullet, SetMotionLocks, - // SetMassData, ApplyMassFromShapes, SetType, SetTargetTransform, Disable/Enable, EnableContactRecycling, - // EnableHitEvents, all force/impulse/torque variants + // SetMassData, ApplyMassFromShapes, SetType, SetTargetTransform, Disable/Enable, + // EnableContactRecycling, EnableHitEvents, all force/impulse/torque variants b3Body_SetTransform( bodyId, (b3Pos){ 1.0f, 6.0f, 0.0f }, b3Quat_identity ); b3Body_SetLinearVelocity( bodyId, (b3Vec3){ 0.5f, 0.0f, 0.0f } ); b3Body_SetAngularVelocity( bodyId, (b3Vec3){ 0.0f, 0.25f, 0.0f } ); @@ -1407,7 +1430,8 @@ static int AllOps( void ) b3Body_ApplyForce( bodyId, (b3Vec3){ 0.0f, 50.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, true ); b3Body_ApplyForceToCenter( bodyId, (b3Vec3){ 5.0f, 0.0f, 0.0f }, true ); b3Body_ApplyTorque( bodyId, (b3Vec3){ 0.0f, 1.0f, 0.0f }, true ); - b3Body_ApplyLinearImpulse( bodyId, (b3Vec3){ 0.1f, 0.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, true ); + b3Body_ApplyLinearImpulse( bodyId, (b3Vec3){ 0.1f, 0.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, + true ); b3Body_ApplyLinearImpulseToCenter( bodyId, (b3Vec3){ 0.0f, 0.1f, 0.0f }, true ); b3Body_ApplyAngularImpulse( bodyId, (b3Vec3){ 0.0f, 0.05f, 0.0f }, true ); @@ -1443,7 +1467,8 @@ static int AllOps( void ) b3RevoluteJoint_SetSpringDampingRatio( revId, 0.5f ); b3RevoluteJoint_SetTargetAngle( revId, 0.25f ); b3Joint_SetLocalFrameA( revId, (b3Transform){ (b3Vec3){ 1.0f, 0.0f, 0.0f }, b3Quat_identity } ); - b3Joint_SetLocalFrameB( revId, (b3Transform){ (b3Vec3){ -1.0f, 0.0f, 0.0f }, b3Quat_identity } ); + b3Joint_SetLocalFrameB( revId, + (b3Transform){ (b3Vec3){ -1.0f, 0.0f, 0.0f }, b3Quat_identity } ); b3Joint_SetConstraintTuning( revId, 60.0f, 2.0f ); b3Joint_SetForceThreshold( revId, 100.0f ); b3Joint_SetTorqueThreshold( revId, 50.0f ); @@ -1569,6 +1594,46 @@ static int AllOps( void ) b3ParallelJoint_SetSpringDampingRatio( parallelId, 0.5f ); b3ParallelJoint_SetMaxTorque( parallelId, 20.0f ); + // Dynamic mover: a rotation locked capsule driven by a mover joint and held up by a pogo joint + b3BodyDef moverBodyDef = b3DefaultBodyDef(); + moverBodyDef.type = b3_dynamicBody; + moverBodyDef.position = (b3Pos){ -20.0f, 3.0f, 0.0f }; + moverBodyDef.motionLocks = + (b3MotionLocks){ .angularX = true, .angularY = true, .angularZ = true }; + b3BodyId moverBodyId = b3CreateBody( worldId, &moverBodyDef ); + b3ShapeDef moverShapeDef = b3DefaultShapeDef(); + moverShapeDef.density = 1.0f; + moverShapeDef.baseMaterial.friction = 0.0f; + b3Capsule moverCapsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f }; + b3CreateCapsuleShape( moverBodyId, &moverShapeDef, &moverCapsule ); + + b3MoverJointDef moverDef = b3DefaultMoverJointDef(); + moverDef.base.bodyIdA = groundId; + moverDef.base.bodyIdB = moverBodyId; + moverDef.base.collideConnected = true; + moverDef.linearVelocity = (b3Vec3){ 1.0f, 0.0f, 0.5f }; + moverDef.maxForce = 0.5f; + b3JointId moverJointId = b3CreateMoverJoint( worldId, &moverDef ); + + b3PogoJointDef pogoDef = b3DefaultPogoJointDef(); + pogoDef.base.bodyIdA = groundId; + pogoDef.base.bodyIdB = moverBodyId; + pogoDef.base.localFrameA.p = (b3Vec3){ -20.0f, 1.0f, 0.0f }; + pogoDef.base.localFrameB.p = moverCapsule.center1; + pogoDef.base.collideConnected = true; + pogoDef.restLength = 1.0f; + pogoDef.hertz = 5.0f; + pogoDef.dampingRatio = 0.8f; + pogoDef.maxCompressionForce = 1000.0f; + pogoDef.maxTensionForce = 1000.0f; + b3JointId pogoJointId = b3CreatePogoJoint( worldId, &pogoDef ); + b3PogoJoint_SetRestLength( pogoJointId, 0.9f ); + b3PogoJoint_SetSpringHertz( pogoJointId, 4.0f ); + b3PogoJoint_SetSpringDampingRatio( pogoJointId, 0.7f ); + b3PogoJoint_SetNormal( pogoJointId, b3Normalize( (b3Vec3){ 0.2f, 1.0f, 0.0f } ) ); + b3PogoJoint_SetMaxTensionForce( pogoJointId, 800.0f ); + b3PogoJoint_SetMaxCompressionForce( pogoJointId, 900.0f ); + // World config mutators b3World_SetGravity( worldId, (b3Vec3){ 0.0f, -9.8f, 0.0f } ); b3World_EnableSleeping( worldId, true ); @@ -1619,7 +1684,8 @@ static int AllOps( void ) b3World_CastRayClosest( worldId, qorigin, qTranslation, qfilter ); b3World_CastShape( worldId, qorigin, &proxy, qTranslation, qfilter, QueryReplayCastFcn, NULL ); b3Capsule mover = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, 0.3f }; - b3World_CastMover( worldId, qorigin, &mover, qTranslation, qfilter, QueryReplayMoverFilterFcn, NULL ); + b3World_CastMover( worldId, qorigin, &mover, qTranslation, qfilter, QueryReplayMoverFilterFcn, + NULL ); b3World_CollideMover( worldId, qorigin, &mover, qfilter, QueryReplayPlaneFcn, NULL ); float timeStep = 1.0f / 60.0f; @@ -1632,8 +1698,21 @@ static int AllOps( void ) b3Body_ApplyLinearImpulseToCenter( capsuleBodyId, (b3Vec3){ 2.0f, 0.0f, 0.0f }, true ); b3Body_SetGravityScale( bodyId, 1.0f ); b3Body_SetSafetyFactor( fastId, 0.4f ); + b3MoverJoint_SetMaxVelocityForce( moverJointId, 60.0f ); } + // The dynamic mover rebuilds its pogo every step, warm started from the old one + if ( i >= 6 ) + { + pogoDef.impulse = b3PogoJoint_GetImpulse( pogoJointId ); + pogoDef.velocity = b3PogoJoint_GetCurrentSpeed( pogoJointId ); + pogoDef.base.localFrameA.p = + b3Body_GetLocalPoint( groundId, b3Body_GetPosition( moverBodyId ) ); + pogoDef.base.localFrameA.p.y = 1.0f; + b3DestroyJoint( pogoJointId, false ); + pogoJointId = b3CreatePogoJoint( worldId, &pogoDef ); + b3MoverJoint_SetLinearVelocity( moverJointId, (b3Vec3){ -0.5f, 0.0f, 1.0f } ); + } // Issue queries mid-loop to exercise recording across steps if ( i == 3 ) { @@ -1715,8 +1794,9 @@ static int AllOps( void ) ENSURE( b3RecPlayer_GetFrame( player ) == seekTargets[k] ); bool hitCompound = false; - b3World_CastRay( b3RecPlayer_GetWorldId( player ), (b3Pos){ 30.0f, 10.0f, 0.0f }, (b3Vec3){ 0.0f, -20.0f, 0.0f }, - b3DefaultQueryFilter(), CompoundHitCastFcn, &hitCompound ); + b3World_CastRay( b3RecPlayer_GetWorldId( player ), (b3Pos){ 30.0f, 10.0f, 0.0f }, + (b3Vec3){ 0.0f, -20.0f, 0.0f }, b3DefaultQueryFilter(), + CompoundHitCastFcn, &hitCompound ); ENSURE( hitCompound ); } while ( b3RecPlayer_StepFrame( player ) ) @@ -1775,8 +1855,9 @@ static int AllOps( void ) static int TransformedHullRoundTrip( void ) { b3Vec3 pts[8] = { - { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, - { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, }; b3HullData* hull = b3CreateHull( pts, 8, 8 ); ENSURE( hull != NULL ); @@ -1792,8 +1873,10 @@ static int TransformedHullRoundTrip( void ) b3ShapeDef shapeDef = b3DefaultShapeDef(); shapeDef.density = 1.0f; - // Baked transform with a rotation and non-uniform scale, the path Unreal uses for instanced hulls. - b3Transform xf = { (b3Vec3){ 0.25f, 0.0f, -0.5f }, b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 0.0f, 1.0f }, 0.3f ) }; + // Baked transform with a rotation and non-uniform scale, the path Unreal uses for instanced + // hulls. + b3Transform xf = { (b3Vec3){ 0.25f, 0.0f, -0.5f }, + b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 0.0f, 1.0f }, 0.3f ) }; b3Vec3 scl = { 1.5f, 0.5f, 2.0f }; for ( int i = 0; i < 3; ++i ) { @@ -1805,8 +1888,9 @@ static int TransformedHullRoundTrip( void ) ENSURE( b3Shape_IsValid( sid ) ); } - // A plain hull after the transformed ones: if the transformed creates desynced the id pool, this - // shape's recorded id would not match what replay allocates and b3ValidateReplay would fail. + // A plain hull after the transformed ones: if the transformed creates desynced the id pool, + // this shape's recorded id would not match what replay allocates and b3ValidateReplay would + // fail. { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; @@ -1816,8 +1900,8 @@ static int TransformedHullRoundTrip( void ) ENSURE( b3Shape_IsValid( sid ) ); } - // Step past the keyframe interval so replay captures a keyframe. That path re-serializes the live - // world and interns the baked hull, which must already be in the pre-seeded registry. + // Step past the keyframe interval so replay captures a keyframe. That path re-serializes the + // live world and interns the baked hull, which must already be in the pre-seeded registry. for ( int i = 0; i < 20; ++i ) { b3World_Step( worldId, 1.0f / 60.0f, 4 ); @@ -1935,7 +2019,8 @@ static int CorruptCompound( void ) int recSize = b3Recording_GetSize( rec ); ENSURE( b3ValidateReplay( recData, recSize, 1 ) ); - // The registry entry is a kind byte and a 4 byte length, then the blob, which leads with its version + // The registry entry is a kind byte and a 4 byte length, then the blob, which leads with its + // version uint64_t version = B3_COMPOUND_VERSION; int blobOffset = -1; for ( int i = 5; i + (int)sizeof( version ) <= recSize; ++i ) @@ -1978,9 +2063,10 @@ static int CorruptCompound( void ) } // Geometry that shares a content hash but differs in bytes must dedup exactly. Mirrors the keyframe -// flow that crashed: the seed appends every slot 1:1 (even byte-identical duplicates a hash collision -// left in an already-recorded file), then capture must resolve a live blob back to an existing slot -// without growing the registry. Forces the collision by handing the same hash to distinct blobs. +// flow that crashed: the seed appends every slot 1:1 (even byte-identical duplicates a hash +// collision left in an already-recorded file), then capture must resolve a live blob back to an +// existing slot without growing the registry. Forces the collision by handing the same hash to +// distinct blobs. static int GeometryHashCollision( void ) { enum @@ -2017,8 +2103,8 @@ static int GeometryHashCollision( void ) b3FreeRegistry( ® ); - // Seed-then-capture: appending byte-identical duplicate slots keeps id == slot index, and a later - // exact intern still resolves to one of them without appending a new entry. + // Seed-then-capture: appending byte-identical duplicate slots keeps id == slot index, and a + // later exact intern still resolves to one of them without appending a new entry. b3GeometryRegistry seeded = { 0 }; uint8_t slot0[n]; uint8_t slot1[n]; @@ -2042,11 +2128,12 @@ static int GeometryHashCollision( void ) return 0; } -// Staged stepping must reveal a mid-stream body at its creation transform. A body created and given an -// impulse in one recorded step is first placed by CreateBody, then displaced by the following Step. Atomic -// replay fuses the two, so the body is only ever seen already moved. Staged replay parks between them so -// the pre-integration pose is drawable. Verify the parked pose is the creation transform, that atomic -// replay does not show it, and that the extra park does not perturb the end state. +// Staged stepping must reveal a mid-stream body at its creation transform. A body created and given +// an impulse in one recorded step is first placed by CreateBody, then displaced by the following +// Step. Atomic replay fuses the two, so the body is only ever seen already moved. Staged replay +// parks between them so the pre-integration pose is drawable. Verify the parked pose is the +// creation transform, that atomic replay does not show it, and that the extra park does not perturb +// the end state. static int StagedStepCreationPose( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -2068,8 +2155,8 @@ static int StagedStepCreationPose( void ) b3World_Step( worldId, timeStep, subStepCount ); } - // The creation transform under test. No ground, so the body is the only create and stays ballistic. - // Components are exact in float so the round-trip pose compares tight. + // The creation transform under test. No ground, so the body is the only create and stays + // ballistic. Components are exact in float so the round-trip pose compares tight. const b3Pos spawn = { 1.0, 20.0, -2.0 }; b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; @@ -2081,7 +2168,8 @@ static int StagedStepCreationPose( void ) shapeDef.density = 1.0f; b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // Impulse along +x so the first integrated step moves the body a visible distance off the spawn. + // Impulse along +x so the first integrated step moves the body a visible distance off the + // spawn. b3Body_ApplyLinearImpulseToCenter( bodyId, (b3Vec3){ 5.0f, 0.0f, 0.0f }, true ); const int creationFrame = leadFrames + 1; @@ -2120,8 +2208,8 @@ static int StagedStepCreationPose( void ) uint64_t atomicHash = b3HashWorldState( b3GetWorldFromId( b3RecPlayer_GetWorldId( atomic ) ) ); b3DestroyPlayer( atomic ); - // Staged replay: step forward until the first pre-step park. It must sit at the creation frame's - // pre-integration state with the new body at exactly its spawn transform. + // Staged replay: step forward until the first pre-step park. It must sit at the creation + // frame's pre-integration state with the new body at exactly its spawn transform. b3RecPlayer* staged = b3CreatePlayer( data, sz, 1 ); ENSURE( staged != NULL ); while ( !b3RecPlayer_IsAtEnd( staged ) ) @@ -2133,7 +2221,8 @@ static int StagedStepCreationPose( void ) } } ENSURE( b3RecPlayer_IsAtPreStep( staged ) ); - ENSURE( b3RecPlayer_GetFrame( staged ) == creationFrame - 1 ); // parked before the step, frame not advanced + ENSURE( b3RecPlayer_GetFrame( staged ) == + creationFrame - 1 ); // parked before the step, frame not advanced b3BodyId stagedId = b3RecPlayer_GetBodyId( staged, 0 ); ENSURE( b3Body_IsValid( stagedId ) ); @@ -2147,7 +2236,8 @@ static int StagedStepCreationPose( void ) ENSURE( b3RecPlayer_GetFrame( staged ) == creationFrame ); ENSURE( !b3RecPlayer_IsAtPreStep( staged ) ); - // Run staged to the end: the same op stream, so it must land on the atomic end state bit for bit. + // Run staged to the end: the same op stream, so it must land on the atomic end state bit for + // bit. while ( !b3RecPlayer_IsAtEnd( staged ) ) { b3RecPlayer_SubStepFrame( staged ); @@ -2162,9 +2252,9 @@ static int StagedStepCreationPose( void ) return 0; } -// Shape names are debug only and do not feed the determinism hash, so b3ValidateReplay cannot catch a -// broken name round-trip. Replay through the player and read the names back to prove the def field and -// the ShapeSetName op survive serialization. +// Shape names are debug only and do not feed the determinism hash, so b3ValidateReplay cannot catch +// a broken name round-trip. Replay through the player and read the names back to prove the def +// field and the ShapeSetName op survive serialization. static int ShapeNameReplay( void ) { const char* names[3] = { @@ -2248,8 +2338,9 @@ static int ShapeNameReplay( void ) // A box sliding across a mesh floor, with the option to swap both geometries and retune a // per-triangle material part way through. Returns the final state hash so the caller can prove the // mutations move the simulation. Recording is optional so the same scene serves as the control. -static uint64_t RunGeometryMutatorScene( b3Recording* rec, bool mutate, const b3MeshData* meshA, const b3MeshData* meshB, - const b3HullData* swapHull, float swapFriction ) +static uint64_t RunGeometryMutatorScene( b3Recording* rec, bool mutate, const b3MeshData* meshA, + const b3MeshData* meshB, const b3HullData* swapHull, + float swapFriction ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = 1; @@ -2271,7 +2362,8 @@ static uint64_t RunGeometryMutatorScene( b3Recording* rec, bool mutate, const b3 b3ShapeDef meshShapeDef = b3DefaultShapeDef(); meshShapeDef.materials = meshMaterials; meshShapeDef.materialCount = 2; - b3ShapeId meshShapeId = b3CreateMeshShape( meshBodyId, &meshShapeDef, meshA, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + b3ShapeId meshShapeId = + b3CreateMeshShape( meshBodyId, &meshShapeDef, meshA, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); b3BodyDef boxBodyDef = b3DefaultBodyDef(); boxBodyDef.type = b3_dynamicBody; @@ -2334,11 +2426,13 @@ static int GeometryMutatorReplay( void ) b3BoxHull swapHull = b3MakeBoxHull( 0.25f, 1.5f, 0.25f ); const float swapFriction = 0.95f; - uint64_t controlHash = RunGeometryMutatorScene( NULL, false, meshA, meshB, &swapHull.base, swapFriction ); + uint64_t controlHash = + RunGeometryMutatorScene( NULL, false, meshA, meshB, &swapHull.base, swapFriction ); b3Recording* rec = b3CreateRecording( 0 ); ENSURE( rec != NULL ); - uint64_t mutatedHash = RunGeometryMutatorScene( rec, true, meshA, meshB, &swapHull.base, swapFriction ); + uint64_t mutatedHash = + RunGeometryMutatorScene( rec, true, meshA, meshB, &swapHull.base, swapFriction ); // Without this the replay gate below could pass on a recording that never carried the ops. ENSURE( mutatedHash != controlHash );