From a618ff46fe6dc9ec208b585fd41185faf276a058 Mon Sep 17 00:00:00 2001 From: Dominic Masters Date: Sat, 18 Jul 2026 20:01:44 -0500 Subject: [PATCH] Physics --- src/dusk/CMakeLists.txt | 1 + src/dusk/entity/component/CMakeLists.txt | 1 + .../entity/component/physics/CMakeLists.txt | 10 + .../entity/component/physics/entityphysics.c | 115 +++++ .../entity/component/physics/entityphysics.h | 172 +++++++ src/dusk/entity/componentlist.h | 2 + src/dusk/physics/CMakeLists.txt | 11 + src/dusk/physics/physicsbodytype.h | 28 ++ src/dusk/physics/physicsshape.h | 109 +++++ src/dusk/physics/physicstest.c | 428 ++++++++++++++++++ src/dusk/physics/physicstest.h | 250 ++++++++++ src/dusk/physics/physicsworld.c | 162 +++++++ src/dusk/physics/physicsworld.h | 47 ++ src/dusk/scene/scene.c | 11 +- src/dusk/scene/scene.h | 14 +- test/CMakeLists.txt | 1 + test/physics/CMakeLists.txt | 10 + test/physics/test_physicstest.c | 228 ++++++++++ test/physics/test_physicsworld.c | 217 +++++++++ 19 files changed, 1815 insertions(+), 2 deletions(-) create mode 100644 src/dusk/entity/component/physics/CMakeLists.txt create mode 100644 src/dusk/entity/component/physics/entityphysics.c create mode 100644 src/dusk/entity/component/physics/entityphysics.h create mode 100644 src/dusk/physics/CMakeLists.txt create mode 100644 src/dusk/physics/physicsbodytype.h create mode 100644 src/dusk/physics/physicsshape.h create mode 100644 src/dusk/physics/physicstest.c create mode 100644 src/dusk/physics/physicstest.h create mode 100644 src/dusk/physics/physicsworld.c create mode 100644 src/dusk/physics/physicsworld.h create mode 100644 test/physics/CMakeLists.txt create mode 100644 test/physics/test_physicstest.c create mode 100644 test/physics/test_physicsworld.c diff --git a/src/dusk/CMakeLists.txt b/src/dusk/CMakeLists.txt index 8c01ff34..ce93e862 100644 --- a/src/dusk/CMakeLists.txt +++ b/src/dusk/CMakeLists.txt @@ -69,6 +69,7 @@ add_subdirectory(system) add_subdirectory(time) add_subdirectory(ui) add_subdirectory(network) +add_subdirectory(physics) add_subdirectory(save) add_subdirectory(util) add_subdirectory(thread) \ No newline at end of file diff --git a/src/dusk/entity/component/CMakeLists.txt b/src/dusk/entity/component/CMakeLists.txt index f1938885..01fe2dca 100644 --- a/src/dusk/entity/component/CMakeLists.txt +++ b/src/dusk/entity/component/CMakeLists.txt @@ -5,3 +5,4 @@ # Subdirs add_subdirectory(display) +add_subdirectory(physics) diff --git a/src/dusk/entity/component/physics/CMakeLists.txt b/src/dusk/entity/component/physics/CMakeLists.txt new file mode 100644 index 00000000..88588512 --- /dev/null +++ b/src/dusk/entity/component/physics/CMakeLists.txt @@ -0,0 +1,10 @@ +# Copyright (c) 2026 Dominic Masters +# +# This software is released under the MIT License. +# https://opensource.org/licenses/MIT + +# Sources +target_sources(${DUSK_LIBRARY_TARGET_NAME} + PUBLIC + entityphysics.c +) diff --git a/src/dusk/entity/component/physics/entityphysics.c b/src/dusk/entity/component/physics/entityphysics.c new file mode 100644 index 00000000..9ea6d7fc --- /dev/null +++ b/src/dusk/entity/component/physics/entityphysics.c @@ -0,0 +1,115 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#include "entityphysics.h" +#include "entity/entitymanager.h" +#include "util/memory.h" + +void entityPhysicsInit( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + + memoryZero(phys, sizeof(entityphysics_t)); + + // Default to cube + phys->type = PHYSICS_BODY_DYNAMIC; + phys->shape.type = PHYSICS_SHAPE_CUBE; + phys->shape.data.cube.halfExtents[0] = 0.5f; + phys->shape.data.cube.halfExtents[1] = 0.5f; + phys->shape.data.cube.halfExtents[2] = 0.5f; + phys->gravityScale = 1.0f; + phys->onGround = false; +} + +entityphysics_t *entityPhysicsGet( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +) { + return componentGetData(mgr, entityId, componentId, COMPONENT_TYPE_PHYSICS); +} + +void entityPhysicsSetShape( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + const physicsshape_t shape +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + phys->shape = shape; +} + +physicsshape_t entityPhysicsGetShape( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + return phys->shape; +} + +void entityPhysicsGetVelocity( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + vec3 dest +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + glm_vec3_copy(phys->velocity, dest); +} + +void entityPhysicsSetVelocity( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + vec3 velocity +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + glm_vec3_copy(velocity, phys->velocity); +} + +void entityPhysicsApplyImpulse( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + vec3 impulse +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + if(phys->type == PHYSICS_BODY_STATIC) return; + glm_vec3_add(phys->velocity, impulse, phys->velocity); +} + +bool_t entityPhysicsIsOnGround( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + return phys->onGround; +} + +void entityPhysicsSetBodyType( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + const physicsbodytype_t type +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + phys->type = type; +} + +physicsbodytype_t entityPhysicsGetBodyType( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +) { + entityphysics_t *phys = entityPhysicsGet(mgr, entityId, componentId); + return phys->type; +} diff --git a/src/dusk/entity/component/physics/entityphysics.h b/src/dusk/entity/component/physics/entityphysics.h new file mode 100644 index 00000000..eb66f1b6 --- /dev/null +++ b/src/dusk/entity/component/physics/entityphysics.h @@ -0,0 +1,172 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#pragma once +#include "entity/entitybase.h" +#include "physics/physicsshape.h" +#include "physics/physicsbodytype.h" + +typedef struct { + physicsbodytype_t type; + physicsshape_t shape; + vec3 velocity; + float_t gravityScale; + bool_t onGround; +} entityphysics_t; + +/** + * Initializes the physics component: defaults to a dynamic 1x1x1 cube + * body, zero velocity, unit gravity scale, and onGround false. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + */ +void entityPhysicsInit( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +); + +/** + * Gets the underlying physics structure (temporarily) for the given entity. + * This is really just intended for doing operations faster than using the + * getters and setters, but it is preferred that you use those. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @return The physics component data for the given entity and component ID. + */ +entityphysics_t *entityPhysicsGet( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +); + +/** + * Sets the shape of the entity's physics body. This will not reset the body + * state, so if you change from a cube to a sphere, it will keep the same + * velocity and onGround state. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @param shape The new shape to set on the physics body. + */ +void entityPhysicsSetShape( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + const physicsshape_t shape +); + +/** + * Gets the shape of the entity's physics body. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @return The shape of the physics body. + */ +physicsshape_t entityPhysicsGetShape( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +); + +/** + * Gets the velocity of the entity's physics body. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @param dest The destination vec3 to write the velocity to. + */ +void entityPhysicsGetVelocity( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + vec3 dest +); + +/** + * Sets the velocity of the entity's physics body. This is not an impulse, so + * it will be affected by mass and drag. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @param velocity The new velocity to set on the physics body. + */ +void entityPhysicsSetVelocity( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + vec3 velocity +); + +/** + * Applies an impulse to the entity's physics body. This is an immediate + * velocity change that is not affected by mass or drag. No-op on STATIC + * bodies. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @param impulse The impulse to apply to the physics body. + */ +void entityPhysicsApplyImpulse( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + vec3 impulse +); + +/** + * Returns true if the entity's physics body rested on a surface during the + * last physicsWorldStep. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @return True if the body is on the ground, false otherwise. + */ +bool_t entityPhysicsIsOnGround( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +); + +/** + * Sets the body type of the entity's physics body. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @param type The body type to set. + */ +void entityPhysicsSetBodyType( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + const physicsbodytype_t type +); + +/** + * Gets the body type of the entity's physics body. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity ID. + * @param componentId The component ID. + * @return The body type of the physics body. + */ +physicsbodytype_t entityPhysicsGetBodyType( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId +); diff --git a/src/dusk/entity/componentlist.h b/src/dusk/entity/componentlist.h index 4a38433e..b9c9d228 100644 --- a/src/dusk/entity/componentlist.h +++ b/src/dusk/entity/componentlist.h @@ -8,6 +8,7 @@ #include "entity/component/display/entityposition.h" #include "entity/component/display/entitycamera.h" #include "entity/component/display/entityrenderable.h" +#include "entity/component/physics/entityphysics.h" // Name (Uppercase) // Structure @@ -20,3 +21,4 @@ X(POSITION, entityposition_t, position, entityPositionInit, NULL, NULL) X(CAMERA, entitycamera_t, camera, entityCameraInit, NULL, NULL) X(RENDERABLE, entityrenderable_t, renderable, entityRenderableInit, entityRenderableDispose, NULL) +X(PHYSICS, entityphysics_t, physics, entityPhysicsInit, NULL, NULL) diff --git a/src/dusk/physics/CMakeLists.txt b/src/dusk/physics/CMakeLists.txt new file mode 100644 index 00000000..3eeca058 --- /dev/null +++ b/src/dusk/physics/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (c) 2026 Dominic Masters +# +# This software is released under the MIT License. +# https://opensource.org/licenses/MIT + +# Sources +target_sources(${DUSK_LIBRARY_TARGET_NAME} + PUBLIC + physicsworld.c + physicstest.c +) diff --git a/src/dusk/physics/physicsbodytype.h b/src/dusk/physics/physicsbodytype.h new file mode 100644 index 00000000..b4aff902 --- /dev/null +++ b/src/dusk/physics/physicsbodytype.h @@ -0,0 +1,28 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#pragma once +#include "dusk.h" + +typedef enum { + /** + * Never moves. Acts as an immovable collision surface. + */ + PHYSICS_BODY_STATIC, + + /** + * Simulated by the world step: gravity, forces, and collision response. + */ + PHYSICS_BODY_DYNAMIC, + + /** + * Moved programmatically via the owning entity's position component; + * collides but is not driven by the simulation. Typical use: player + * character controller. + */ + PHYSICS_BODY_KINEMATIC +} physicsbodytype_t; diff --git a/src/dusk/physics/physicsshape.h b/src/dusk/physics/physicsshape.h new file mode 100644 index 00000000..712639e2 --- /dev/null +++ b/src/dusk/physics/physicsshape.h @@ -0,0 +1,109 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#pragma once +#include "dusk.h" + +typedef enum { + PHYSICS_SHAPE_CUBE, + PHYSICS_SHAPE_SPHERE, + PHYSICS_SHAPE_CAPSULE, + PHYSICS_SHAPE_PLANE, + + /** + * Arbitrary shape of any complexity (heightfield, mesh, etc), tested via + * a caller-supplied callback rather than a built-in primitive routine. + * See physicsshapecustom_t. + */ + PHYSICS_SHAPE_CUSTOM +} physicshapetype_t; + +typedef struct { + vec3 halfExtents; +} physicsshapecube_t; + +typedef struct { + float_t radius; +} physicsshapesphere_t; + +typedef struct { + float_t radius; + float_t halfHeight; +} physicsshapecapsule_t; + +typedef struct { + vec3 normal; + float_t distance; +} physicsshapeplane_t; + +typedef struct physicsshape_t physicsshape_t; + +/** + * Tests a custom shape (positioned at selfPos, describing itself via + * selfData) against another shape (positioned at otherPos). Return true + * and fill outNormal/outDepth if the shapes overlap. + * + * outNormal points from self toward other -- the direction to push other + * away from self, e.g. straight up for a body resting on flat ground. + * This is the opposite convention from physicsTestShapeVsShape's + * A-vs-B/outNormal (which points from B toward A); physicsWorldStep + * always queries a custom shape as the static/kinematic side, so this + * convention lets an implementation delegate directly to a primitive test + * like physicsTestSphereVsPlane (self=ground, other=body) and return its + * result unmodified. + * + * @param selfPos The custom shape's position. + * @param selfData The custom shape's own describing data (physicsshapecustom_t.userData). + * @param otherPos The other shape's position. + * @param otherShape The other shape's full descriptor. May itself be any + * type except PHYSICS_SHAPE_CUSTOM -- custom-vs-custom is not supported. + * @param outNormal Push-out normal, pointing from self toward other. + * @param outDepth Penetration depth (positive when overlapping). + * @return true if the shapes overlap, false otherwise. + */ +typedef bool_t (*physicsshapecustomtest_t)( + const vec3 selfPos, + const void *selfData, + const vec3 otherPos, + const physicsshape_t *otherShape, + vec3 outNormal, + float_t *outDepth +); + +/** + * A shape of arbitrary complexity, e.g. landscape/terrain geometry, that + * the built-in primitive tests (cube/sphere/capsule/plane) cannot describe. + * Rather than embedding the shape's data (which may be large -- a + * heightfield, a triangle mesh, a BVH) inline, this just holds an opaque + * pointer plus a callback that the physics system invokes to test overlap + * against it. + * + * Ownership: userData is not owned or freed by the physics system. The + * struct/asset that actually describes the shape (e.g. a chunk's + * heightfield) must outlive this physicsshape_t -- typically it's owned by + * whatever entity/asset also owns the entityphysics_t component that holds + * this shape. + */ +typedef struct { + /** Opaque pointer to the owner's shape-describing data. */ + void *userData; + /** Callback invoked to test this shape against another. */ + physicsshapecustomtest_t test; +} physicsshapecustom_t; + +typedef union { + physicsshapecube_t cube; + physicsshapesphere_t sphere; + physicsshapecapsule_t capsule; + physicsshapeplane_t plane; + physicsshapecustom_t custom; +} physicsshapedata_t; + +typedef struct physicsshape_t { + physicshapetype_t type; + physicsshapedata_t data; +} physicsshape_t; diff --git a/src/dusk/physics/physicstest.c b/src/dusk/physics/physicstest.c new file mode 100644 index 00000000..714f03e0 --- /dev/null +++ b/src/dusk/physics/physicstest.c @@ -0,0 +1,428 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#include "physicstest.h" +#include "assert/assert.h" + +bool_t physicsTestAabbVsAabb( + const vec3 ac, const vec3 ah, + const vec3 bc, const vec3 bh, + vec3 outNormal, float_t *outDepth +) { + float_t dx = ac[0] - bc[0]; + float_t dy = ac[1] - bc[1]; + float_t dz = ac[2] - bc[2]; + + float_t px = (ah[0] + bh[0]) - fabsf(dx); + float_t py = (ah[1] + bh[1]) - fabsf(dy); + float_t pz = (ah[2] + bh[2]) - fabsf(dz); + + if(px <= 0.0f || py <= 0.0f || pz <= 0.0f) return false; + + outNormal[0] = outNormal[1] = outNormal[2] = 0.0f; + if(px < py && px < pz) { + *outDepth = px; + outNormal[0] = dx >= 0.0f ? 1.0f : -1.0f; + } else if(py < pz) { + *outDepth = py; + outNormal[1] = dy >= 0.0f ? 1.0f : -1.0f; + } else { + *outDepth = pz; + outNormal[2] = dz >= 0.0f ? 1.0f : -1.0f; + } + return true; +} + +bool_t physicsTestSphereVsSphere( + const vec3 ac, const float_t ar, + const vec3 bc, const float_t br, + vec3 outNormal, float_t *outDepth +) { + vec3 diff; + glm_vec3_sub((float_t *)ac, (float_t *)bc, diff); + float_t dist2 = glm_vec3_norm2(diff); + float_t sumR = ar + br; + + if(dist2 >= sumR * sumR) return false; + + float_t dist = sqrtf(dist2); + *outDepth = sumR - dist; + + if(dist > 1e-6f) { + glm_vec3_scale(diff, 1.0f / dist, outNormal); + } else { + outNormal[0] = 0.0f; + outNormal[1] = 1.0f; + outNormal[2] = 0.0f; + } + return true; +} + +bool_t physicsTestSphereVsAabb( + const vec3 sc, const float_t sr, + const vec3 ac, const vec3 ah, + vec3 outNormal, float_t *outDepth +) { + vec3 closest = { + glm_clamp(sc[0], ac[0] - ah[0], ac[0] + ah[0]), + glm_clamp(sc[1], ac[1] - ah[1], ac[1] + ah[1]), + glm_clamp(sc[2], ac[2] - ah[2], ac[2] + ah[2]) + }; + + vec3 diff; + glm_vec3_sub((float_t *)sc, closest, diff); + float_t dist2 = glm_vec3_norm2(diff); + + bool_t inside = (dist2 < 1e-10f); + if(!inside && dist2 >= sr * sr) return false; + + if(!inside) { + float_t dist = sqrtf(dist2); + *outDepth = sr - dist; + glm_vec3_scale(diff, 1.0f / dist, outNormal); + } else { + float_t faces[6] = { + (ac[0] + ah[0]) - sc[0], + sc[0] - (ac[0] - ah[0]), + (ac[1] + ah[1]) - sc[1], + sc[1] - (ac[1] - ah[1]), + (ac[2] + ah[2]) - sc[2], + sc[2] - (ac[2] - ah[2]) + }; + const float_t normals[6][3] = { + {1,0,0},{-1,0,0},{0,1,0},{0,-1,0},{0,0,1},{0,0,-1} + }; + int32_t mi = 0; + for(int32_t k = 1; k < 6; k++) { + if(faces[k] < faces[mi]) mi = k; + } + *outDepth = sr + faces[mi]; + outNormal[0] = normals[mi][0]; + outNormal[1] = normals[mi][1]; + outNormal[2] = normals[mi][2]; + } + return true; +} + +bool_t physicsTestSphereVsPlane( + const vec3 sc, const float_t sr, + const vec3 pn, const float_t pd, + vec3 outNormal, float_t *outDepth +) { + float_t signedDist = glm_vec3_dot((float_t *)pn, (float_t *)sc) - pd; + *outDepth = sr - signedDist; + if(*outDepth <= 0.0f) return false; + glm_vec3_copy((float_t *)pn, outNormal); + return true; +} + +bool_t physicsTestAabbVsPlane( + const vec3 ac, const vec3 ah, + const vec3 pn, const float_t pd, + vec3 outNormal, float_t *outDepth +) { + float_t proj = fabsf(pn[0] * ah[0]) + + fabsf(pn[1] * ah[1]) + + fabsf(pn[2] * ah[2]); + float_t signedDist = glm_vec3_dot((float_t *)pn, (float_t *)ac) - pd; + *outDepth = proj - signedDist; + if(*outDepth <= 0.0f) return false; + glm_vec3_copy((float_t *)pn, outNormal); + return true; +} + +void physicsTestClosestPointOnSegment( + const vec3 a, const vec3 b, const vec3 p, vec3 out +) { + vec3 ab, ap; + glm_vec3_sub((float_t *)b, (float_t *)a, ab); + glm_vec3_sub((float_t *)p, (float_t *)a, ap); + float_t denom = glm_vec3_dot(ab, ab); + float_t t = (denom > 1e-10f) + ? glm_clamp(glm_vec3_dot(ap, ab) / denom, 0.0f, 1.0f) + : 0.0f; + glm_vec3_lerp((float_t *)a, (float_t *)b, t, out); +} + +void physicsTestClosestPointsBetweenSegments( + const vec3 a1, const vec3 b1, + const vec3 a2, const vec3 b2, + vec3 outP1, vec3 outP2 +) { + vec3 d1, d2, r; + glm_vec3_sub((float_t *)b1, (float_t *)a1, d1); + glm_vec3_sub((float_t *)b2, (float_t *)a2, d2); + glm_vec3_sub((float_t *)a1, (float_t *)a2, r); + + float_t a = glm_vec3_dot(d1, d1); + float_t e = glm_vec3_dot(d2, d2); + float_t f = glm_vec3_dot(d2, r); + float_t s, t; + + if(a <= 1e-10f && e <= 1e-10f) { + glm_vec3_copy((float_t *)a1, outP1); + glm_vec3_copy((float_t *)a2, outP2); + return; + } + if(a <= 1e-10f) { + t = 0.0f; + s = glm_clamp(f / e, 0.0f, 1.0f); + } else { + float_t c = glm_vec3_dot(d1, r); + if(e <= 1e-10f) { + s = 0.0f; + t = glm_clamp(-c / a, 0.0f, 1.0f); + } else { + float_t b = glm_vec3_dot(d1, d2); + float_t denom = a * e - b * b; + t = (fabsf(denom) > 1e-10f) + ? glm_clamp((b * f - c * e) / denom, 0.0f, 1.0f) + : 0.0f; + s = glm_clamp((b * t + f) / e, 0.0f, 1.0f); + t = glm_clamp((b * s - c) / a, 0.0f, 1.0f); + } + } + glm_vec3_lerp((float_t *)a1, (float_t *)b1, t, outP1); + glm_vec3_lerp((float_t *)a2, (float_t *)b2, s, outP2); +} + +bool_t physicsTestCapsuleVsSphere( + const vec3 cc, const float_t cr, const float_t chh, + const vec3 sc, const float_t sr, + vec3 outNormal, float_t *outDepth +) { + vec3 capA = { cc[0], cc[1] - chh, cc[2] }; + vec3 capB = { cc[0], cc[1] + chh, cc[2] }; + vec3 closest; + physicsTestClosestPointOnSegment(capA, capB, sc, closest); + return physicsTestSphereVsSphere( + closest, cr, sc, sr, outNormal, outDepth + ); +} + +bool_t physicsTestCapsuleVsAabb( + const vec3 cc, const float_t cr, const float_t chh, + const vec3 ac, const vec3 ah, + vec3 outNormal, float_t *outDepth +) { + vec3 capA = { cc[0], cc[1] - chh, cc[2] }; + vec3 capB = { cc[0], cc[1] + chh, cc[2] }; + vec3 closest; + physicsTestClosestPointOnSegment(capA, capB, ac, closest); + return physicsTestSphereVsAabb( + closest, cr, ac, ah, outNormal, outDepth + ); +} + +bool_t physicsTestCapsuleVsPlane( + const vec3 cc, const float_t cr, const float_t chh, + const vec3 pn, const float_t pd, + vec3 outNormal, float_t *outDepth +) { + vec3 capA = { cc[0], cc[1] - chh, cc[2] }; + vec3 capB = { cc[0], cc[1] + chh, cc[2] }; + float_t da = glm_vec3_dot((float_t *)pn, capA) - pd; + float_t db = glm_vec3_dot((float_t *)pn, capB) - pd; + float_t minDist = (da < db) ? da : db; + *outDepth = cr - minDist; + if(*outDepth <= 0.0f) return false; + glm_vec3_copy((float_t *)pn, outNormal); + return true; +} + +bool_t physicsTestCapsuleVsCapsule( + const vec3 c1, const float_t r1, const float_t hh1, + const vec3 c2, const float_t r2, const float_t hh2, + vec3 outNormal, float_t *outDepth +) { + vec3 a1 = { c1[0], c1[1] - hh1, c1[2] }; + vec3 b1 = { c1[0], c1[1] + hh1, c1[2] }; + vec3 a2 = { c2[0], c2[1] - hh2, c2[2] }; + vec3 b2 = { c2[0], c2[1] + hh2, c2[2] }; + vec3 p1, p2; + physicsTestClosestPointsBetweenSegments(a1, b1, a2, b2, p1, p2); + return physicsTestSphereVsSphere(p1, r1, p2, r2, outNormal, outDepth); +} + +bool_t physicsTestDispatch( + const vec3 aPos, const physicsshape_t aShape, + const vec3 bPos, const physicsshape_t bShape, + vec3 outNormal, float_t *outDepth +) { + physicshapetype_t ta = aShape.type; + physicshapetype_t tb = bShape.type; + + assertFalse( + ta == PHYSICS_SHAPE_CUSTOM && tb == PHYSICS_SHAPE_CUSTOM, + "Custom-vs-custom shape collision is not supported" + ); + + if(ta == PHYSICS_SHAPE_CUSTOM) { + // Callback returns self(A)->other(B); this function's contract needs + // B->A, so negate. + vec3 tmp; float_t d; + if(!aShape.data.custom.test( + aPos, aShape.data.custom.userData, bPos, &bShape, tmp, &d + )) return false; + glm_vec3_scale(tmp, -1.0f, outNormal); + *outDepth = d; + return true; + } + + if(tb == PHYSICS_SHAPE_CUSTOM) { + // Callback returns self(B)->other(A), which already matches this + // function's B->A contract -- no negation needed. + return bShape.data.custom.test( + bPos, bShape.data.custom.userData, aPos, &aShape, outNormal, outDepth + ); + } + + if(tb == PHYSICS_SHAPE_PLANE) { + const float_t *pn = bShape.data.plane.normal; + const float_t pd = bShape.data.plane.distance; + switch(ta) { + case PHYSICS_SHAPE_CUBE: + return physicsTestAabbVsPlane( + aPos, aShape.data.cube.halfExtents, + pn, pd, outNormal, outDepth + ); + case PHYSICS_SHAPE_SPHERE: + return physicsTestSphereVsPlane( + aPos, aShape.data.sphere.radius, + pn, pd, outNormal, outDepth + ); + case PHYSICS_SHAPE_CAPSULE: + return physicsTestCapsuleVsPlane( + aPos, + aShape.data.capsule.radius, + aShape.data.capsule.halfHeight, + pn, pd, outNormal, outDepth + ); + default: + return false; + } + } + + if(ta == PHYSICS_SHAPE_PLANE) { + vec3 tmp; float_t d; + if(!physicsTestDispatch( + bPos, bShape, aPos, aShape, tmp, &d + )) return false; + glm_vec3_scale(tmp, -1.0f, outNormal); + *outDepth = d; + return true; + } + + switch(ta) { + case PHYSICS_SHAPE_CUBE: { + const float_t *ac = aPos; + const float_t *ah = aShape.data.cube.halfExtents; + switch(tb) { + case PHYSICS_SHAPE_CUBE: + return physicsTestAabbVsAabb( + ac, ah, + bPos, bShape.data.cube.halfExtents, + outNormal, outDepth + ); + case PHYSICS_SHAPE_SPHERE: { + vec3 tmp; float_t d; + if(!physicsTestSphereVsAabb( + bPos, bShape.data.sphere.radius, + ac, ah, tmp, &d + )) return false; + glm_vec3_scale(tmp, -1.0f, outNormal); + *outDepth = d; + return true; + } + case PHYSICS_SHAPE_CAPSULE: { + vec3 tmp; float_t d; + if(!physicsTestCapsuleVsAabb( + bPos, + bShape.data.capsule.radius, + bShape.data.capsule.halfHeight, + ac, ah, tmp, &d + )) return false; + glm_vec3_scale(tmp, -1.0f, outNormal); + *outDepth = d; + return true; + } + default: return false; + } + } + + case PHYSICS_SHAPE_SPHERE: { + const float_t sr = aShape.data.sphere.radius; + switch(tb) { + case PHYSICS_SHAPE_CUBE: + return physicsTestSphereVsAabb( + aPos, sr, + bPos, bShape.data.cube.halfExtents, + outNormal, outDepth + ); + case PHYSICS_SHAPE_SPHERE: + return physicsTestSphereVsSphere( + aPos, sr, + bPos, bShape.data.sphere.radius, + outNormal, outDepth + ); + case PHYSICS_SHAPE_CAPSULE: { + vec3 tmp; float_t d; + if(!physicsTestCapsuleVsSphere( + bPos, + bShape.data.capsule.radius, + bShape.data.capsule.halfHeight, + aPos, sr, tmp, &d + )) return false; + glm_vec3_scale(tmp, -1.0f, outNormal); + *outDepth = d; + return true; + } + default: return false; + } + } + + case PHYSICS_SHAPE_CAPSULE: { + const float_t cr = aShape.data.capsule.radius; + const float_t chh = aShape.data.capsule.halfHeight; + switch(tb) { + case PHYSICS_SHAPE_CUBE: + return physicsTestCapsuleVsAabb( + aPos, cr, chh, + bPos, bShape.data.cube.halfExtents, + outNormal, outDepth + ); + case PHYSICS_SHAPE_SPHERE: + return physicsTestCapsuleVsSphere( + aPos, cr, chh, + bPos, bShape.data.sphere.radius, + outNormal, outDepth + ); + case PHYSICS_SHAPE_CAPSULE: + return physicsTestCapsuleVsCapsule( + aPos, cr, chh, + bPos, + bShape.data.capsule.radius, + bShape.data.capsule.halfHeight, + outNormal, outDepth + ); + default: return false; + } + } + + default: return false; + } +} + +bool_t physicsTestShapeVsShape( + const vec3 aPos, const physicsshape_t aShape, + const vec3 bPos, const physicsshape_t bShape, + vec3 outNormal, float_t *outDepth +) { + return physicsTestDispatch( + aPos, aShape, bPos, bShape, outNormal, outDepth + ); +} diff --git a/src/dusk/physics/physicstest.h b/src/dusk/physics/physicstest.h new file mode 100644 index 00000000..debb5860 --- /dev/null +++ b/src/dusk/physics/physicstest.h @@ -0,0 +1,250 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#pragma once +#include "physicsshape.h" + +/** + * Tests overlap between two axis-aligned bounding boxes. + * outNormal points from B toward A. + * + * @param ac Center of AABB A. + * @param ah Half-extents of AABB A. + * @param bc Center of AABB B. + * @param bh Half-extents of AABB B. + * @param outNormal Push-out normal (B toward A). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestAabbVsAabb( + const vec3 ac, const vec3 ah, + const vec3 bc, const vec3 bh, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between two spheres. + * outNormal points from B toward A. + * + * @param ac Center of sphere A. + * @param ar Radius of sphere A. + * @param bc Center of sphere B. + * @param br Radius of sphere B. + * @param outNormal Push-out normal (B toward A). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestSphereVsSphere( + const vec3 ac, const float_t ar, + const vec3 bc, const float_t br, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between a sphere and an axis-aligned bounding box. + * outNormal points from the AABB toward the sphere. + * + * @param sc Center of the sphere. + * @param sr Radius of the sphere. + * @param ac Center of the AABB. + * @param ah Half-extents of the AABB. + * @param outNormal Push-out normal (AABB toward sphere). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestSphereVsAabb( + const vec3 sc, const float_t sr, + const vec3 ac, const vec3 ah, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between a sphere and an infinite plane. + * outNormal equals the plane normal (pointing away from the surface). + * + * @param sc Center of the sphere. + * @param sr Radius of the sphere. + * @param pn Plane normal (unit vector, world-space). + * @param pd Plane offset: dot(pn, surfacePoint) == pd. + * @param outNormal Push-out normal (equals pn). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestSphereVsPlane( + const vec3 sc, const float_t sr, + const vec3 pn, const float_t pd, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between an AABB and an infinite plane. + * outNormal equals the plane normal. + * + * @param ac Center of the AABB. + * @param ah Half-extents of the AABB. + * @param pn Plane normal (unit vector, world-space). + * @param pd Plane offset (see physicsTestSphereVsPlane). + * @param outNormal Push-out normal (equals pn). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestAabbVsPlane( + const vec3 ac, const vec3 ah, + const vec3 pn, const float_t pd, + vec3 outNormal, float_t *outDepth +); + +/** + * Finds the closest point on segment [a, b] to query point p. + * + * @param a Start of the segment. + * @param b End of the segment. + * @param p Query point. + * @param out Receives the closest point on [a, b] to p. + */ +void physicsTestClosestPointOnSegment( + const vec3 a, const vec3 b, const vec3 p, vec3 out +); + +/** + * Finds the closest points between two line segments. + * + * @param a1 Start of segment 1. + * @param b1 End of segment 1. + * @param a2 Start of segment 2. + * @param b2 End of segment 2. + * @param outP1 Receives the closest point on segment 1. + * @param outP2 Receives the closest point on segment 2. + */ +void physicsTestClosestPointsBetweenSegments( + const vec3 a1, const vec3 b1, + const vec3 a2, const vec3 b2, + vec3 outP1, vec3 outP2 +); + +/** + * Tests overlap between a Y-axis-aligned capsule and a sphere. + * outNormal points from the sphere toward the capsule. + * + * @param cc Center of the capsule. + * @param cr Radius of the capsule. + * @param chh Half-height of the capsule's cylindrical segment. + * @param sc Center of the sphere. + * @param sr Radius of the sphere. + * @param outNormal Push-out normal (sphere toward capsule). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestCapsuleVsSphere( + const vec3 cc, const float_t cr, const float_t chh, + const vec3 sc, const float_t sr, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between a Y-axis-aligned capsule and an AABB. + * outNormal points from the AABB toward the capsule. + * + * @param cc Center of the capsule. + * @param cr Radius of the capsule. + * @param chh Half-height of the capsule's cylindrical segment. + * @param ac Center of the AABB. + * @param ah Half-extents of the AABB. + * @param outNormal Push-out normal (AABB toward capsule). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestCapsuleVsAabb( + const vec3 cc, const float_t cr, const float_t chh, + const vec3 ac, const vec3 ah, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between a Y-axis-aligned capsule and an infinite plane. + * outNormal equals the plane normal. + * + * @param cc Center of the capsule. + * @param cr Radius of the capsule. + * @param chh Half-height of the capsule's cylindrical segment. + * @param pn Plane normal (unit vector, world-space). + * @param pd Plane offset (see physicsTestSphereVsPlane). + * @param outNormal Push-out normal (equals pn). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestCapsuleVsPlane( + const vec3 cc, const float_t cr, const float_t chh, + const vec3 pn, const float_t pd, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests overlap between two Y-axis-aligned capsules. + * outNormal points from capsule B toward capsule A. + * + * @param c1 Center of capsule A. + * @param r1 Radius of capsule A. + * @param hh1 Half-height of capsule A's cylindrical segment. + * @param c2 Center of capsule B. + * @param r2 Radius of capsule B. + * @param hh2 Half-height of capsule B's cylindrical segment. + * @param outNormal Push-out normal (B toward A). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestCapsuleVsCapsule( + const vec3 c1, const float_t r1, const float_t hh1, + const vec3 c2, const float_t r2, const float_t hh2, + vec3 outNormal, float_t *outDepth +); + +/** + * Routes a shape-pair collision test to the correct primitive. + * When A is a plane, delegates with swapped arguments and negates + * the resulting normal. outNormal points from B toward A. + * If either shape is PHYSICS_SHAPE_CUSTOM, delegates to its + * physicsshapecustom_t.test callback instead (swapping/negating the same + * way if it's B that's custom). Asserts if both shapes are custom. + * + * @param aPos Position of shape A. + * @param aShape Shape descriptor of A. + * @param bPos Position of shape B. + * @param bShape Shape descriptor of B. + * @param outNormal Push-out normal (B toward A). + * @param outDepth Penetration depth. + * @return true if overlapping. + */ +bool_t physicsTestDispatch( + const vec3 aPos, const physicsshape_t aShape, + const vec3 bPos, const physicsshape_t bShape, + vec3 outNormal, float_t *outDepth +); + +/** + * Tests for collision between two shapes. Returns true if they + * overlap, and if so, outputs the push-out normal and depth. + * + * outNormal always points from shape B toward shape A, so adding + * (outNormal * outDepth) to A's position separates the two shapes. + * + * @param aPos Position of shape A. + * @param aShape Shape descriptor of A. + * @param bPos Position of shape B. + * @param bShape Shape descriptor of B. + * @param outNormal Push-out normal, pointing from B toward A. + * @param outDepth Penetration depth (positive when overlapping). + * @return true if the shapes overlap, false otherwise. + */ +bool_t physicsTestShapeVsShape( + const vec3 aPos, + const physicsshape_t aShape, + const vec3 bPos, + const physicsshape_t bShape, + vec3 outNormal, + float_t *outDepth +); diff --git a/src/dusk/physics/physicsworld.c b/src/dusk/physics/physicsworld.c new file mode 100644 index 00000000..985bb326 --- /dev/null +++ b/src/dusk/physics/physicsworld.c @@ -0,0 +1,162 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#include "physicsworld.h" +#include "assert/assert.h" +#include "util/memory.h" +#include "entity/entitymanager.h" +#include "entity/component/display/entityposition.h" +#include "entity/component/physics/entityphysics.h" +#include "physicstest.h" + +void physicsWorldInit(physicsworld_t *world) { + assertNotNull(world, "World cannot be null"); + memoryZero(world, sizeof(physicsworld_t)); + + world->gravity[0] = 0.0f; + world->gravity[1] = -9.81f; + world->gravity[2] = 0.0f; +} + +void physicsWorldStep( + const physicsworld_t *world, + entitymanager_t *mgr, + const float_t dt +) { + assertNotNull(world, "World cannot be null"); + assertNotNull(mgr, "Entity manager cannot be null"); + assertTrue(dt > 0.0f, "Delta time must be positive"); + + entityid_t physEnts[ENTITY_COUNT_MAX]; + componentid_t physComps[ENTITY_COUNT_MAX]; + entityid_t physCount = componentGetEntitiesWithComponent( + mgr, COMPONENT_TYPE_PHYSICS, physEnts, physComps + ); + if(physCount == 0) return; + + // Pre-fetch all position and physics pointers once. Ensure each dynamic + // body's PRS cache is up to date before this step reads/writes + // ->position directly, in case it was last touched via a raw transform + // write (e.g. entityPositionLookAt) rather than a PRS setter. + entityposition_t *positions[ENTITY_COUNT_MAX]; + entityphysics_t *physBodies[ENTITY_COUNT_MAX]; + for(entityid_t i = 0; i < physCount; i++) { + componentid_t posComp = entityGetComponent( + mgr, physEnts[i], COMPONENT_TYPE_POSITION + ); + positions[i] = (posComp != COMPONENT_ID_INVALID) + ? entityPositionGet(mgr, physEnts[i], posComp) + : NULL; + if(positions[i]) entityPositionEnsurePRS(positions[i]); + physBodies[i] = entityPhysicsGet(mgr, physEnts[i], physComps[i]); + } + + // Phase 1: integrate dynamic bodies (gravity + velocity -> position). + // Writes directly to pos->position, matrix rebuilt at the end. + for(entityid_t i = 0; i < physCount; i++) { + if(!positions[i]) continue; + entityphysics_t *phys = physBodies[i]; + if(phys->type != PHYSICS_BODY_DYNAMIC) continue; + + phys->onGround = false; + + phys->velocity[0] += world->gravity[0] * phys->gravityScale * dt; + phys->velocity[1] += world->gravity[1] * phys->gravityScale * dt; + phys->velocity[2] += world->gravity[2] * phys->gravityScale * dt; + + float_t *pos = positions[i]->position; + pos[0] += phys->velocity[0] * dt; + pos[1] += phys->velocity[1] * dt; + pos[2] += phys->velocity[2] * dt; + } + + // Phase 2: dynamic vs static/kinematic. + for(entityid_t i = 0; i < physCount; i++) { + if(!positions[i]) continue; + entityphysics_t *phys = physBodies[i]; + if(phys->type != PHYSICS_BODY_DYNAMIC) continue; + + float_t *pos = positions[i]->position; + + for(entityid_t j = 0; j < physCount; j++) { + if(i == j || !positions[j]) continue; + entityphysics_t *otherPhys = physBodies[j]; + if(otherPhys->type == PHYSICS_BODY_DYNAMIC) continue; + + vec3 normal; float_t depth; + if(!physicsTestShapeVsShape( + pos, phys->shape, + positions[j]->position, otherPhys->shape, + normal, &depth + )) continue; + + pos[0] += normal[0] * depth; + pos[1] += normal[1] * depth; + pos[2] += normal[2] * depth; + + float_t vn = glm_vec3_dot(phys->velocity, normal); + if(vn < 0.0f) { + phys->velocity[0] -= vn * normal[0]; + phys->velocity[1] -= vn * normal[1]; + phys->velocity[2] -= vn * normal[2]; + } + + if(normal[1] > PHYSICS_GROUND_THRESHOLD) phys->onGround = true; + } + } + + // Phase 3: dynamic vs dynamic. + for(entityid_t i = 0; i < physCount; i++) { + if(!positions[i]) continue; + entityphysics_t *physA = physBodies[i]; + if(physA->type != PHYSICS_BODY_DYNAMIC) continue; + + float_t *posA = positions[i]->position; + + for(entityid_t j = i + 1; j < physCount; j++) { + if(!positions[j]) continue; + entityphysics_t *physB = physBodies[j]; + if(physB->type != PHYSICS_BODY_DYNAMIC) continue; + + float_t *posB = positions[j]->position; + + vec3 normal; float_t depth; + if(!physicsTestShapeVsShape( + posA, physA->shape, posB, physB->shape, normal, &depth + )) continue; + + posA[0] += normal[0] * depth * 0.5f; + posA[1] += normal[1] * depth * 0.5f; + posA[2] += normal[2] * depth * 0.5f; + + posB[0] -= normal[0] * depth * 0.5f; + posB[1] -= normal[1] * depth * 0.5f; + posB[2] -= normal[2] * depth * 0.5f; + + float_t vRel = glm_vec3_dot(physA->velocity, normal) + - glm_vec3_dot(physB->velocity, normal); + if(vRel < 0.0f) { + physA->velocity[0] -= vRel * normal[0]; + physA->velocity[1] -= vRel * normal[1]; + physA->velocity[2] -= vRel * normal[2]; + physB->velocity[0] += vRel * normal[0]; + physB->velocity[1] += vRel * normal[1]; + physB->velocity[2] += vRel * normal[2]; + } + + if( normal[1] > PHYSICS_GROUND_THRESHOLD) physA->onGround = true; + if(-normal[1] > PHYSICS_GROUND_THRESHOLD) physB->onGround = true; + } + } + + // Rebuild transforms for all dynamic bodies once, after all phases. + for(entityid_t i = 0; i < physCount; i++) { + if(!positions[i]) continue; + if(physBodies[i]->type != PHYSICS_BODY_DYNAMIC) continue; + entityPositionRebuild(mgr, positions[i]); + } +} diff --git a/src/dusk/physics/physicsworld.h b/src/dusk/physics/physicsworld.h new file mode 100644 index 00000000..adb40136 --- /dev/null +++ b/src/dusk/physics/physicsworld.h @@ -0,0 +1,47 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#pragma once +#include "physics/physicsshape.h" +#include "physics/physicsbodytype.h" +#include "entity/entitybase.h" + +/** + * Surface normals with a Y component above this are considered "ground" + * (angle from vertical <= ~45 degrees) and set a resolved body's onGround + * flag. + */ +#define PHYSICS_GROUND_THRESHOLD 0.707f + +typedef struct { + /** Downward acceleration applied to every dynamic body each step. */ + vec3 gravity; +} physicsworld_t; + +/** + * Initializes a physics world with Earth-like default gravity + * (0, -9.81, 0). + * + * @param world The physics world to initialize. + */ +void physicsWorldInit(physicsworld_t *world); + +/** + * Steps every PHYSICS-component entity in mgr forward by dt: integrates + * gravity into dynamic bodies, resolves dynamic-vs-static/kinematic and + * dynamic-vs-dynamic overlap, then rebuilds each moved entity's transform. + * A no-op if mgr has no entities with a PHYSICS component. + * + * @param world The physics world configuration (gravity) to step with. + * @param mgr The entity manager whose PHYSICS-component entities to step. + * @param dt Timestep, in seconds (use DUSK_TIME_STEP for the fixed step). + */ +void physicsWorldStep( + const physicsworld_t *world, + entitymanager_t *mgr, + const float_t dt +); diff --git a/src/dusk/scene/scene.c b/src/dusk/scene/scene.c index 2f59e7f8..29703880 100644 --- a/src/dusk/scene/scene.c +++ b/src/dusk/scene/scene.c @@ -29,6 +29,7 @@ sceneid_t sceneCreate(void) { if(SCENE_MANAGER.scenes[i].used) continue; scene_t *scene = &SCENE_MANAGER.scenes[i]; entityManagerInit(&scene->entities); + physicsWorldInit(&scene->physics); scene->used = true; return i; } @@ -64,6 +65,12 @@ entitymanager_t *sceneGetEntities(const sceneid_t id) { return &SCENE_MANAGER.scenes[id].entities; } +physicsworld_t *sceneGetPhysics(const sceneid_t id) { + assertTrue(id < SCENE_COUNT_MAX, "Scene ID OOB"); + assertTrue(SCENE_MANAGER.scenes[id].used, "Scene is not in use"); + return &SCENE_MANAGER.scenes[id].physics; +} + errorret_t sceneUpdate(void) { if(SCENE_MANAGER.active == SCENE_ID_INVALID) errorOk(); @@ -71,7 +78,9 @@ errorret_t sceneUpdate(void) { if(!TIME.dynamicUpdate) errorOk(); #endif - entityManagerUpdate(sceneGetEntities(SCENE_MANAGER.active)); + scene_t *scene = &SCENE_MANAGER.scenes[SCENE_MANAGER.active]; + entityManagerUpdate(&scene->entities); + physicsWorldStep(&scene->physics, &scene->entities, TIME.delta); errorOk(); } diff --git a/src/dusk/scene/scene.h b/src/dusk/scene/scene.h index 9cfc833a..6762903b 100644 --- a/src/dusk/scene/scene.h +++ b/src/dusk/scene/scene.h @@ -8,10 +8,12 @@ #pragma once #include "scenebase.h" #include "entity/entitymanager.h" +#include "physics/physicsworld.h" typedef struct { bool_t used; entitymanager_t entities; + physicsworld_t physics; } scene_t; typedef struct { @@ -72,7 +74,17 @@ sceneid_t sceneGetActive(void); entitymanager_t *sceneGetEntities(const sceneid_t id); /** - * Ticks the active scene's entities. + * Gets the physics world owned by a given scene, e.g. to change its + * gravity. + * + * @param id The ID of the scene. + * @return Pointer to the scene's physics world. + */ +physicsworld_t *sceneGetPhysics(const sceneid_t id); + +/** + * Ticks the active scene's entities (update callbacks, then a physics + * step) on fixed timesteps only, per DUSK_TIME_DYNAMIC. * * @return An error if the update failed, or errorOk() if it succeeded. */ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c631f99a..4b5ce181 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -10,6 +10,7 @@ add_subdirectory(network) add_subdirectory(thread) add_subdirectory(display) add_subdirectory(entity) +add_subdirectory(physics) add_subdirectory(scene) # add_subdirectory(item) add_subdirectory(time) diff --git a/test/physics/CMakeLists.txt b/test/physics/CMakeLists.txt new file mode 100644 index 00000000..99e90c07 --- /dev/null +++ b/test/physics/CMakeLists.txt @@ -0,0 +1,10 @@ +# Copyright (c) 2026 Dominic Masters +# +# This software is released under the MIT License. +# https://opensource.org/licenses/MIT + +include(dusktest) + +# Tests +dusktest(test_physicstest.c) +dusktest(test_physicsworld.c) diff --git a/test/physics/test_physicstest.c b/test/physics/test_physicstest.c new file mode 100644 index 00000000..55e6b2e4 --- /dev/null +++ b/test/physics/test_physicstest.c @@ -0,0 +1,228 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#include "dusktest.h" +#include "util/memory.h" +#include "physics/physicstest.h" + +static void test_aabbVsAabbOverlapAndSeparation(void **state) { + vec3 normal; float_t depth; + + // Overlapping on X (smallest penetration axis). + vec3 ac = { 0.6f, 0.0f, 0.0f }, ah = { 0.5f, 0.5f, 0.5f }; + vec3 bc = { 0.0f, 0.0f, 0.0f }, bh = { 0.5f, 0.5f, 0.5f }; + assert_true(physicsTestAabbVsAabb(ac, ah, bc, bh, normal, &depth)); + assert_float_equal(depth, 0.4f, 0.0001f); + assert_float_equal(normal[0], 1.0f, 0.0001f); + assert_float_equal(normal[1], 0.0f, 0.0001f); + assert_float_equal(normal[2], 0.0f, 0.0001f); + + // Far apart: no overlap. + vec3 cc = { 10.0f, 0.0f, 0.0f }; + assert_false(physicsTestAabbVsAabb(cc, ah, bc, bh, normal, &depth)); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_sphereVsSphere(void **state) { + vec3 normal; float_t depth; + + vec3 ac = { 1.0f, 0.0f, 0.0f }; + vec3 bc = { 0.0f, 0.0f, 0.0f }; + assert_true(physicsTestSphereVsSphere(ac, 0.75f, bc, 0.75f, normal, &depth)); + assert_float_equal(depth, 0.5f, 0.0001f); + assert_float_equal(normal[0], 1.0f, 0.0001f); + + assert_false(physicsTestSphereVsSphere(ac, 0.25f, bc, 0.25f, normal, &depth)); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_sphereVsAabbOutsideAndInside(void **state) { + vec3 normal; float_t depth; + + // Sphere just outside the box corner, overlapping. + vec3 sc = { 1.2f, 0.0f, 0.0f }; + vec3 ac = { 0.0f, 0.0f, 0.0f }, ah = { 0.5f, 0.5f, 0.5f }; + assert_true(physicsTestSphereVsAabb(sc, 0.75f, ac, ah, normal, &depth)); + assert_float_equal(normal[0], 1.0f, 0.0001f); + assert_float_equal(depth, 0.75f - 0.7f, 0.0001f); + + // Sphere center inside the box: pushes out the nearest face. + vec3 scInside = { 0.4f, 0.0f, 0.0f }; + assert_true( + physicsTestSphereVsAabb(scInside, 0.1f, ac, ah, normal, &depth) + ); + assert_float_equal(normal[0], 1.0f, 0.0001f); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_aabbVsPlane(void **state) { + vec3 normal; float_t depth; + + vec3 ac = { 0.0f, 0.4f, 0.0f }, ah = { 0.5f, 0.5f, 0.5f }; + vec3 pn = { 0.0f, 1.0f, 0.0f }; + assert_true(physicsTestAabbVsPlane(ac, ah, pn, 0.0f, normal, &depth)); + assert_float_equal(depth, 0.1f, 0.0001f); + assert_float_equal(normal[1], 1.0f, 0.0001f); + + vec3 acAbove = { 0.0f, 10.0f, 0.0f }; + assert_false(physicsTestAabbVsPlane(acAbove, ah, pn, 0.0f, normal, &depth)); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_dispatchSymmetryAndPlaneRouting(void **state) { + physicsshape_t cubeA = { + .type = PHYSICS_SHAPE_CUBE, + .data.cube.halfExtents = { 0.5f, 0.5f, 0.5f } + }; + physicsshape_t sphereB = { + .type = PHYSICS_SHAPE_SPHERE, + .data.sphere.radius = 0.75f + }; + vec3 aPos = { 0.6f, 0.0f, 0.0f }; + vec3 bPos = { 0.0f, 0.0f, 0.0f }; + + vec3 normalAB; float_t depthAB; + assert_true(physicsTestShapeVsShape( + aPos, cubeA, bPos, sphereB, normalAB, &depthAB + )); + + vec3 normalBA; float_t depthBA; + assert_true(physicsTestShapeVsShape( + bPos, sphereB, aPos, cubeA, normalBA, &depthBA + )); + + // Swapping A/B should give the same depth and a negated normal. + assert_float_equal(depthAB, depthBA, 0.0001f); + assert_float_equal(normalAB[0], -normalBA[0], 0.0001f); + + // Cube vs plane routes to physicsTestAabbVsPlane. + physicsshape_t plane = { + .type = PHYSICS_SHAPE_PLANE, + .data.plane = { .normal = { 0.0f, 1.0f, 0.0f }, .distance = 0.0f } + }; + vec3 cubePos = { 0.0f, 0.4f, 0.0f }; + vec3 normal; float_t depth; + assert_true(physicsTestShapeVsShape( + cubePos, cubeA, bPos, plane, normal, &depth + )); + assert_float_equal(depth, 0.1f, 0.0001f); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_capsuleVsSphere(void **state) { + vec3 normal; float_t depth; + + // Capsule standing on the Y axis, sphere touching its side. + vec3 cc = { 0.0f, 0.0f, 0.0f }; + vec3 sc = { 0.9f, 0.0f, 0.0f }; + assert_true(physicsTestCapsuleVsSphere( + cc, 0.5f, 1.0f, sc, 0.5f, normal, &depth + )); + assert_float_equal(normal[1], 0.0f, 0.0001f); + assert_true(depth > 0.0f); + + vec3 scFar = { 5.0f, 0.0f, 0.0f }; + assert_false(physicsTestCapsuleVsSphere( + cc, 0.5f, 1.0f, scFar, 0.5f, normal, &depth + )); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +// Example custom shape callback: an infinite flat landscape at a fixed +// height, described by a single float_t (userData). Mirrors how a real +// terrain/heightfield callback would delegate to a primitive test. +static bool_t flatLandscapeTest( + const vec3 selfPos, const void *selfData, + const vec3 otherPos, const physicsshape_t *otherShape, + vec3 outNormal, float_t *outDepth +) { + const float_t *groundHeight = (const float_t *)selfData; + vec3 planeNormal = { 0.0f, 1.0f, 0.0f }; + if(otherShape->type != PHYSICS_SHAPE_SPHERE) return false; + return physicsTestSphereVsPlane( + otherPos, otherShape->data.sphere.radius, + planeNormal, selfPos[1] + *groundHeight, outNormal, outDepth + ); +} + +static void test_customShapeFlatLandscape(void **state) { + float_t groundHeight = 0.0f; + physicsshape_t landscape = { + .type = PHYSICS_SHAPE_CUSTOM, + .data.custom = { .userData = &groundHeight, .test = flatLandscapeTest } + }; + physicsshape_t sphere = { + .type = PHYSICS_SHAPE_SPHERE, + .data.sphere.radius = 0.5f + }; + vec3 landscapePos = { 0.0f, 0.0f, 0.0f }; + // Sphere center at y=0.3, radius 0.5: overlaps the ground by 0.2. + vec3 spherePos = { 0.0f, 0.3f, 0.0f }; + + // Landscape as B: the ordering physicsWorldStep actually uses (dynamic + // body as A, static/custom body as B). + vec3 normal; float_t depth; + assert_true(physicsTestShapeVsShape( + spherePos, sphere, landscapePos, landscape, normal, &depth + )); + assert_float_equal(depth, 0.2f, 0.0001f); + assert_float_equal(normal[0], 0.0f, 0.0001f); + assert_float_equal(normal[1], 1.0f, 0.0001f); // pushes the sphere UP + assert_float_equal(normal[2], 0.0f, 0.0001f); + + // Landscape as A: same depth, negated normal. + vec3 normalSwapped; float_t depthSwapped; + assert_true(physicsTestShapeVsShape( + landscapePos, landscape, spherePos, sphere, normalSwapped, &depthSwapped + )); + assert_float_equal(depthSwapped, depth, 0.0001f); + assert_float_equal(normalSwapped[1], -normal[1], 0.0001f); + + // Sphere far above the ground: no overlap. + vec3 sphereFar = { 0.0f, 10.0f, 0.0f }; + assert_false(physicsTestShapeVsShape( + sphereFar, sphere, landscapePos, landscape, normal, &depth + )); + + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_customVsCustomAsserts(void **state) { + float_t groundHeight = 0.0f; + physicsshape_t landscapeA = { + .type = PHYSICS_SHAPE_CUSTOM, + .data.custom = { .userData = &groundHeight, .test = flatLandscapeTest } + }; + physicsshape_t landscapeB = landscapeA; + vec3 pos = { 0.0f, 0.0f, 0.0f }; + vec3 normal; float_t depth; + + expect_assert_failure(physicsTestShapeVsShape( + pos, landscapeA, pos, landscapeB, normal, &depth + )); +} + +int main(int argc, char **argv) { + const struct CMUnitTest tests[] = { + cmocka_unit_test(test_aabbVsAabbOverlapAndSeparation), + cmocka_unit_test(test_sphereVsSphere), + cmocka_unit_test(test_sphereVsAabbOutsideAndInside), + cmocka_unit_test(test_aabbVsPlane), + cmocka_unit_test(test_dispatchSymmetryAndPlaneRouting), + cmocka_unit_test(test_capsuleVsSphere), + cmocka_unit_test(test_customShapeFlatLandscape), + cmocka_unit_test(test_customVsCustomAsserts), + }; + + return cmocka_run_group_tests(tests, NULL, NULL); +} diff --git a/test/physics/test_physicsworld.c b/test/physics/test_physicsworld.c new file mode 100644 index 00000000..14325b56 --- /dev/null +++ b/test/physics/test_physicsworld.c @@ -0,0 +1,217 @@ +/** + * Copyright (c) 2026 Dominic Masters + * + * This software is released under the MIT License. + * https://opensource.org/licenses/MIT + */ + +#include "dusktest.h" +#include "util/memory.h" +#include "entity/entitymanager.h" +#include "entity/component/display/entityposition.h" +#include "entity/component/physics/entityphysics.h" +#include "physics/physicsworld.h" + +static void test_entityPhysicsGettersAndSetters(void **state) { + entitymanager_t mgr; + entityManagerInit(&mgr); + + entityid_t entity = entityManagerAdd(&mgr); + componentid_t phys = entityAddComponent(&mgr, entity, COMPONENT_TYPE_PHYSICS); + + // Defaults: dynamic 1x1x1 cube, zero velocity, not on ground. + assert_int_equal( + entityPhysicsGetBodyType(&mgr, entity, phys), PHYSICS_BODY_DYNAMIC + ); + assert_false(entityPhysicsIsOnGround(&mgr, entity, phys)); + + vec3 velocity; + entityPhysicsGetVelocity(&mgr, entity, phys, velocity); + assert_float_equal(velocity[0], 0.0f, 0.0001f); + assert_float_equal(velocity[1], 0.0f, 0.0001f); + assert_float_equal(velocity[2], 0.0f, 0.0001f); + + vec3 setVel = { 1.0f, 2.0f, 3.0f }; + entityPhysicsSetVelocity(&mgr, entity, phys, setVel); + entityPhysicsGetVelocity(&mgr, entity, phys, velocity); + assert_float_equal(velocity[0], 1.0f, 0.0001f); + assert_float_equal(velocity[1], 2.0f, 0.0001f); + assert_float_equal(velocity[2], 3.0f, 0.0001f); + + vec3 impulse = { 1.0f, 0.0f, 0.0f }; + entityPhysicsApplyImpulse(&mgr, entity, phys, impulse); + entityPhysicsGetVelocity(&mgr, entity, phys, velocity); + assert_float_equal(velocity[0], 2.0f, 0.0001f); + + // Impulses are a no-op on static bodies. + entityPhysicsSetBodyType(&mgr, entity, phys, PHYSICS_BODY_STATIC); + entityPhysicsApplyImpulse(&mgr, entity, phys, impulse); + entityPhysicsGetVelocity(&mgr, entity, phys, velocity); + assert_float_equal(velocity[0], 2.0f, 0.0001f); + assert_int_equal( + entityPhysicsGetBodyType(&mgr, entity, phys), PHYSICS_BODY_STATIC + ); + + physicsshape_t sphere = { + .type = PHYSICS_SHAPE_SPHERE, + .data.sphere.radius = 2.0f + }; + entityPhysicsSetShape(&mgr, entity, phys, sphere); + physicsshape_t got = entityPhysicsGetShape(&mgr, entity, phys); + assert_int_equal(got.type, PHYSICS_SHAPE_SPHERE); + assert_float_equal(got.data.sphere.radius, 2.0f, 0.0001f); + + entityManagerDispose(&mgr); + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_physicsWorldGravityIntegratesDynamicBody(void **state) { + entitymanager_t mgr; + entityManagerInit(&mgr); + physicsworld_t world; + physicsWorldInit(&world); + + entityid_t entity = entityManagerAdd(&mgr); + componentid_t posComp = entityAddComponent( + &mgr, entity, COMPONENT_TYPE_POSITION + ); + entityAddComponent(&mgr, entity, COMPONENT_TYPE_PHYSICS); + + const float_t dt = 1.0f / 60.0f; + physicsWorldStep(&world, &mgr, dt); + + vec3 pos; + entityPositionGetWorldPosition(&mgr, entity, posComp, pos); + + // One step of gravity: velocity.y = gravity.y * dt, position integrates + // that same-step velocity (semi-implicit Euler). + float_t expectedVelY = world.gravity[1] * dt; + float_t expectedPosY = expectedVelY * dt; + assert_float_equal(pos[1], expectedPosY, 0.0001f); + assert_true(pos[1] < 0.0f); + + entityManagerDispose(&mgr); + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_physicsWorldRestsOnStaticFloor(void **state) { + entitymanager_t mgr; + entityManagerInit(&mgr); + physicsworld_t world; + physicsWorldInit(&world); + + // Static floor: a wide, thin platform centered at y=0 (top face at 0.5). + entityid_t floorEntity = entityManagerAdd(&mgr); + componentid_t floorPosComp = entityAddComponent( + &mgr, floorEntity, COMPONENT_TYPE_POSITION + ); + componentid_t floorPhysComp = entityAddComponent( + &mgr, floorEntity, COMPONENT_TYPE_PHYSICS + ); + entityPhysicsSetBodyType(&mgr, floorEntity, floorPhysComp, PHYSICS_BODY_STATIC); + physicsshape_t floorShape = { + .type = PHYSICS_SHAPE_CUBE, + .data.cube.halfExtents = { 5.0f, 0.5f, 5.0f } + }; + entityPhysicsSetShape(&mgr, floorEntity, floorPhysComp, floorShape); + vec3 floorPos = { 0.0f, 0.0f, 0.0f }; + entityPositionSetLocalPosition(&mgr, floorEntity, floorPosComp, floorPos); + + // Dynamic body (default 1x1x1 cube) starting 4 units above the floor. + entityid_t entity = entityManagerAdd(&mgr); + componentid_t posComp = entityAddComponent( + &mgr, entity, COMPONENT_TYPE_POSITION + ); + componentid_t physComp = entityAddComponent( + &mgr, entity, COMPONENT_TYPE_PHYSICS + ); + vec3 startPos = { 0.0f, 4.0f, 0.0f }; + entityPositionSetLocalPosition(&mgr, entity, posComp, startPos); + + const float_t dt = 1.0f / 60.0f; + for(int32_t i = 0; i < 300; i++) { + physicsWorldStep(&world, &mgr, dt); + } + + vec3 pos; + entityPositionGetWorldPosition(&mgr, entity, posComp, pos); + // Resting height: floor top (0.5) + body half-extent (0.5). + assert_float_equal(pos[1], 1.0f, 0.01f); + assert_true(entityPhysicsIsOnGround(&mgr, entity, physComp)); + + vec3 velocity; + entityPhysicsGetVelocity(&mgr, entity, physComp, velocity); + assert_float_equal(velocity[1], 0.0f, 0.01f); + + entityManagerDispose(&mgr); + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_physicsWorldDynamicVsDynamicSeparates(void **state) { + entitymanager_t mgr; + entityManagerInit(&mgr); + physicsworld_t world; + physicsWorldInit(&world); + + entityid_t entityA = entityManagerAdd(&mgr); + componentid_t posA = entityAddComponent( + &mgr, entityA, COMPONENT_TYPE_POSITION + ); + componentid_t physA = entityAddComponent( + &mgr, entityA, COMPONENT_TYPE_PHYSICS + ); + vec3 posAStart = { 0.0f, 0.0f, 0.3f }; + entityPositionSetLocalPosition(&mgr, entityA, posA, posAStart); + entityPhysicsGet(&mgr, entityA, physA)->gravityScale = 0.0f; + + entityid_t entityB = entityManagerAdd(&mgr); + componentid_t posB = entityAddComponent( + &mgr, entityB, COMPONENT_TYPE_POSITION + ); + componentid_t physB = entityAddComponent( + &mgr, entityB, COMPONENT_TYPE_PHYSICS + ); + vec3 posBStart = { 0.0f, 0.0f, -0.3f }; + entityPositionSetLocalPosition(&mgr, entityB, posB, posBStart); + entityPhysicsGet(&mgr, entityB, physB)->gravityScale = 0.0f; + + // Both default to 1x1x1 cubes (half-extent 0.5), overlapping by 0.4 on Z. + physicsWorldStep(&world, &mgr, 1.0f / 60.0f); + + vec3 finalA, finalB; + entityPositionGetWorldPosition(&mgr, entityA, posA, finalA); + entityPositionGetWorldPosition(&mgr, entityB, posB, finalB); + + assert_float_equal(finalA[2], 0.5f, 0.0001f); + assert_float_equal(finalB[2], -0.5f, 0.0001f); + // Fully separated: touching exactly, no remaining overlap. + assert_float_equal(finalA[2] - finalB[2], 1.0f, 0.0001f); + + entityManagerDispose(&mgr); + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +static void test_physicsWorldStepNoPhysicsEntitiesIsNoop(void **state) { + entitymanager_t mgr; + entityManagerInit(&mgr); + physicsworld_t world; + physicsWorldInit(&world); + + entityManagerAdd(&mgr); + physicsWorldStep(&world, &mgr, 1.0f / 60.0f); + + entityManagerDispose(&mgr); + assert_int_equal(memoryGetAllocatedCount(), 0); +} + +int main(int argc, char **argv) { + const struct CMUnitTest tests[] = { + cmocka_unit_test(test_entityPhysicsGettersAndSetters), + cmocka_unit_test(test_physicsWorldGravityIntegratesDynamicBody), + cmocka_unit_test(test_physicsWorldRestsOnStaticFloor), + cmocka_unit_test(test_physicsWorldDynamicVsDynamicSeparates), + cmocka_unit_test(test_physicsWorldStepNoPhysicsEntitiesIsNoop), + }; + + return cmocka_run_group_tests(tests, NULL, NULL); +}