YourWishes/dusk
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

Physics refactor

Browse Source
This commit is contained in:
Dominic Masters
2026-04-14 13:45:16 -05:00
parent 0e3871ac26
commit c91243f6e9
14 changed files with 719 additions and 707 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/dusk/engine/engine.c
+33 -56
View File
@@ -17,6 +17,7 @@
#include "script/scriptmanager.h"
#include "assert/assert.h"
#include "entity/entitymanager.h"
#include "entity/component/physics/entityphysics.h"
#include "game/game.h"
#include "physics/physicsmanager.h"
#include "display/mesh/cube.h"
@@ -24,10 +25,9 @@
engine_t ENGINE;
/* Physics demo entities */
static entityid_t phFloorEnt, phBoxEnt;
static componentid_t phFloorPos, phFloorMesh, phFloorMat;
static componentid_t phBoxPos, phBoxMesh, phBoxMat, phBoxPhys;
/* Kept module-level only because engineUpdate needs them for the reset. */
static entityid_t phBoxEnt;
static componentid_t phBoxPhys;
errorret_t engineInit(const int32_t argc, const char_t **argv) {
memoryZero(&ENGINE, sizeof(engine_t));
@@ -62,50 +62,38 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) {
entityCameraSetZFar(cam, camCam, distance * 6.0f);
/* ---- Static floor (visual + physics) ---- */
phFloorEnt = entityManagerAdd();
phFloorPos = entityAddComponent(phFloorEnt, COMPONENT_TYPE_POSITION);
phFloorMesh = entityAddComponent(phFloorEnt, COMPONENT_TYPE_MESH);
phFloorMat = entityAddComponent(phFloorEnt, COMPONENT_TYPE_MATERIAL);
entityid_t floorEnt = entityManagerAdd();
componentid_t floorPos = entityAddComponent(floorEnt, COMPONENT_TYPE_POSITION);
componentid_t floorMesh = entityAddComponent(floorEnt, COMPONENT_TYPE_MESH);
componentid_t floorMat = entityAddComponent(floorEnt, COMPONENT_TYPE_MATERIAL);
componentid_t floorPhys = entityAddComponent(floorEnt, COMPONENT_TYPE_PHYSICS);
/* Scale the unit XZ plane to 10×10, centred on origin */
entityPositionSetPosition(phFloorEnt, phFloorPos, (vec3){ -5.0f, 0.0f, -5.0f });
entityPositionSetScale(phFloorEnt, phFloorPos, (vec3){ 10.0f, 1.0f, 10.0f });
entityMeshSetMesh(phFloorEnt, phFloorMesh, &PLANE_MESH_SIMPLE);
entityMaterialGetShaderMaterial(phFloorEnt, phFloorMat)->unlit.color = COLOR_GREEN;
entityPositionSetPosition(floorEnt, floorPos, (vec3){ -5.0f, 0.0f, -5.0f });
entityPositionSetScale(floorEnt, floorPos, (vec3){ 10.0f, 1.0f, 10.0f });
entityMeshSetMesh(floorEnt, floorMesh, &PLANE_MESH_SIMPLE);
entityMaterialGetShaderMaterial(floorEnt, floorMat)->unlit.color = COLOR_GREEN;
/* No PHYSICS component for the floor — we add the body manually so it never
* gets disposed by the entity system before we're done with it. */
physicsbody_t *floorBody = physicsWorldAddBody(&PHYSICS_WORLD);
floorBody->type = PHYSICS_BODY_STATIC;
floorBody->shape.type = PHYSICS_SHAPE_PLANE;
floorBody->shape.data.plane.normal[0] = 0.0f;
floorBody->shape.data.plane.normal[1] = 1.0f;
floorBody->shape.data.plane.normal[2] = 0.0f;
floorBody->shape.data.plane.distance = 0.0f;
entityphysics_t *floorPhysData = entityPhysicsGet(floorEnt, floorPhys);
floorPhysData->type = PHYSICS_BODY_STATIC;
floorPhysData->shape.type = PHYSICS_SHAPE_PLANE;
floorPhysData->shape.data.plane.normal[0] = 0.0f;
floorPhysData->shape.data.plane.normal[1] = 1.0f;
floorPhysData->shape.data.plane.normal[2] = 0.0f;
floorPhysData->shape.data.plane.distance = 0.0f;
/* ---- Dynamic box ---- */
phBoxEnt = entityManagerAdd();
phBoxPos = entityAddComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
phBoxMesh = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MESH);
phBoxMat = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MATERIAL);
componentid_t boxPos = entityAddComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
componentid_t boxMesh = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MESH);
componentid_t boxMat = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MATERIAL);
phBoxPhys = entityAddComponent(phBoxEnt, COMPONENT_TYPE_PHYSICS);
entityMeshSetMesh(phBoxEnt, phBoxMesh, &CUBE_MESH_SIMPLE);
entityMaterialGetShaderMaterial(phBoxEnt, phBoxMat)->unlit.color = COLOR_RED;
entityMeshSetMesh(phBoxEnt, boxMesh, &CUBE_MESH_SIMPLE);
entityMaterialGetShaderMaterial(phBoxEnt, boxMat)->unlit.color = COLOR_RED;
/* Start the box 4 units above the floor; default shape is a unit AABB */
physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
boxBody->position[0] = 0.0f;
boxBody->position[1] = 4.0f;
boxBody->position[2] = 0.0f;
/* Sync visual position for first frame */
vec3 visualPos = {
boxBody->position[0] - boxBody->shape.data.cube.halfExtents[0],
boxBody->position[1] - boxBody->shape.data.cube.halfExtents[1],
boxBody->position[2] - boxBody->shape.data.cube.halfExtents[2]
};
entityPositionSetPosition(phBoxEnt, phBoxPos, visualPos);
/* Physics position lives in the POSITION component. CUBE_MESH_SIMPLE is
* centred at origin (-0.5..0.5), so entity position == physics centre. */
entityPositionSetPosition(phBoxEnt, boxPos, (vec3){ 0.0f, 4.0f, 0.0f });
/* Run the init script. */
scriptcontext_t ctx;
@@ -123,27 +111,16 @@ errorret_t engineUpdate(void) {
uiUpdate();
errorChain(sceneUpdate());
// Reset cube
/* Reset the box to its start position on demand. */
if(inputIsDown(INPUT_ACTION_ACCEPT)) {
entityPositionSetPosition(phBoxEnt, phBoxPos, (vec3){ 0.0f, 4.0f, 0.0f });
physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
physicsBodySetPosition(boxBody, (vec3){ 0.0f, 4.0f, 0.0f });
physicsBodySetVelocity(boxBody, (vec3){ 0.0f, 0.0f, 0.0f });
componentid_t posComp = entityGetComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
entityPositionSetPosition(phBoxEnt, posComp, (vec3){ 0.0f, 4.0f, 0.0f });
entityPhysicsSetVelocity(phBoxEnt, phBoxPhys, (vec3){ 0.0f, 0.0f, 0.0f });
}
/* Step physics simulation */
/* Step physics — positions are updated directly on POSITION components. */
physicsManagerUpdate();
/* Sync dynamic box visual to physics body (centre → corner offset) */
physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
vec3 visualPos = {
boxBody->position[0] - boxBody->shape.data.cube.halfExtents[0],
boxBody->position[1] - boxBody->shape.data.cube.halfExtents[1],
boxBody->position[2] - boxBody->shape.data.cube.halfExtents[2]
};
entityPositionSetPosition(phBoxEnt, phBoxPos, visualPos);
errorChain(gameUpdate());
errorChain(displayUpdate());
src/dusk/entity/component/physics/entityphysics.c
+72 -39
View File
@@ -10,65 +10,98 @@
#include "entity/component/display/entityposition.h"
#include "physics/physicsmanager.h"
#include "assert/assert.h"
#include "util/memory.h"
void entityPhysicsInit(
const entityid_t entityId,
const componentid_t componentId
) {
entityphysics_t *phys = componentGetData(
entityId, componentId, COMPONENT_TYPE_PHYSICS
);
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
phys->body = physicsWorldAddBody(&PHYSICS_WORLD);
assertNotNull(phys->body, "Physics world body limit reached");
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;
}
void entityPhysicsSyncPosition(
entityphysics_t *entityPhysicsGet(
const entityid_t entityId,
const componentid_t componentId
) {
entityphysics_t *phys = componentGetData(
entityId, componentId, COMPONENT_TYPE_PHYSICS
);
assertNotNull(phys->body, "Physics body is NULL");
/* Find the entity's POSITION component and update it. */
componentid_t posId = entityGetComponent(entityId, COMPONENT_TYPE_POSITION);
if (posId == 0xFF) return; /* entity has no POSITION component */
entityPositionSetPosition(entityId, posId, phys->body->position);
return componentGetData(entityId, componentId, COMPONENT_TYPE_PHYSICS);
}
physicsbody_t *entityPhysicsGetBody(
const entityid_t entityId,
const componentid_t componentId
) {
entityphysics_t *phys = componentGetData(
entityId, componentId, COMPONENT_TYPE_PHYSICS
);
return phys->body;
}
void entityPhysicsMove(
void entityPhysicsSetShape(
const entityid_t entityId,
const componentid_t componentId,
const vec3 motion
const physicsshape_t shape
) {
entityphysics_t *phys = componentGetData(
entityId, componentId, COMPONENT_TYPE_PHYSICS
);
assertNotNull(phys->body, "Physics body is NULL");
physicsWorldMoveBody(&PHYSICS_WORLD, phys->body, motion);
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
phys->shape = shape;
// TODO: Do I need to reset the state for ground/active?
}
physicsshape_t entityPhysicsGetShape(
const entityid_t entityId,
const componentid_t componentId
) {
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
return phys->shape;
}
void entityPhysicsGetVelocity(
const entityid_t entityId,
const componentid_t componentId,
vec3 dest
) {
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
glm_vec3_copy(phys->velocity, dest);
}
void entityPhysicsSetVelocity(
const entityid_t entityId,
const componentid_t componentId,
vec3 velocity
) {
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
glm_vec3_copy(velocity, phys->velocity);
}
void entityPhysicsApplyImpulse(
const entityid_t entityId,
const componentid_t componentId,
vec3 impulse
) {
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
if (phys->type == PHYSICS_BODY_STATIC) return;
glm_vec3_add(phys->velocity, impulse, phys->velocity);
}
bool_t entityPhysicsIsOnGround(
const entityid_t entityId,
const componentid_t componentId
) {
entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
assertNotNull(phys, "Failed to get physics component data");
return phys->onGround;
}
void entityPhysicsDispose(
const entityid_t entityId,
const componentid_t componentId
) {
entityphysics_t *phys = componentGetData(
entityId, componentId, COMPONENT_TYPE_PHYSICS
);
if (!phys->body) return;
physicsWorldRemoveBody(&PHYSICS_WORLD, phys->body);
phys->body = NULL;
}
src/dusk/entity/component/physics/entityphysics.h
+90 -18
View File
@@ -7,11 +7,15 @@
#pragma once
#include "entity/entitybase.h"
#include "physics/physicsbody.h"
#include "physics/physicsshape.h"
#include "physics/physicsbodytype.h"
typedef struct {
/** Pointer into PHYSICS_WORLD.bodies[]. Allocated on component init. */
physicsbody_t *body;
physicsbodytype_t type;
physicsshape_t shape;
vec3 velocity;
float_t gravityScale;
bool_t onGround;
} entityphysics_t;
/**
@@ -24,30 +28,98 @@ void entityPhysicsInit(
);
/**
* Copies the physics body's position back into the entity's POSITION
* component (if present). Call this after physicsManagerStep each frame.
* 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 entityId The entity ID.
* @param componentId The component ID.
* @return The physics component data for the given entity and component ID.
*/
void entityPhysicsSyncPosition(
entityphysics_t *entityPhysicsGet(
const entityid_t entityId,
const componentid_t componentId
);
/**
* Returns the raw physics body for direct manipulation.
* 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 entityId The entity ID.
* @param componentId The component ID.
* @param shape The new shape to set on the physics body.
*/
physicsbody_t *entityPhysicsGetBody(
const entityid_t entityId,
const componentid_t componentId
);
/**
* Moves a KINEMATIC body by the given world-space motion vector and resolves
* collisions. Convenience wrapper around physicsWorldMoveBody.
*/
void entityPhysicsMove(
void entityPhysicsSetShape(
const entityid_t entityId,
const componentid_t componentId,
const vec3 motion
const physicsshape_t shape
);
/**
* Gets the shape of the entity's physics body.
*
* @param entityId The entity ID.
* @param componentId The component ID.
* @return The shape of the physics body.
*/
physicsshape_t entityPhysicsGetShape(
const entityid_t entityId,
const componentid_t componentId
);
/**
* Gets the velocity of the entity's physics body.
*
* @param entityId The entity ID.
* @param componentId The component ID.
* @param dest The destination vec3 to write the velocity to.
*/
void entityPhysicsGetVelocity(
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 entityId The entity ID.
* @param componentId The component ID.
* @param velocity The new velocity to set on the physics body.
*/
void entityPhysicsSetVelocity(
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 entityId The entity ID.
* @param componentId The component ID.
* @param impulse The impulse to apply to the physics body.
*/
void entityPhysicsApplyImpulse(
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
* step or move.
*
* @param entityId The entity ID.
* @param componentId The component ID.
* @return True if the body is on the ground, false otherwise.
*/
bool_t entityPhysicsIsOnGround(
const entityid_t entityId,
const componentid_t componentId
);
/**
src/dusk/entity/entity.h
+2 -1
View File
@@ -34,7 +34,8 @@ componentid_t entityAddComponent(
);
/**
* Gets the ID of the component of the given type on the entity with the given ID.
* Gets the ID of the component of the given type on the entity with the given
* ID, or 0xFF if the entity lacks the component.
*
* @param entityId The ID of the entity to get the component from.
* @param type The type of the component to get.
src/dusk/physics/CMakeLists.txt
+1 -1
View File
@@ -7,6 +7,6 @@
target_sources(${DUSK_LIBRARY_TARGET_NAME}
PUBLIC
physicsmanager.c
physicsbody.c
physicsworld.c
physicstest.c
)
src/dusk/physics/physicsbody.c
-33
View File
@@ -1,33 +0,0 @@
/**
* Copyright (c) 2026 Dominic Masters
*
* This software is released under the MIT License.
* https://opensource.org/licenses/MIT
*/
#include "physicsbody.h"
void physicsBodySetPosition(physicsbody_t *body, const vec3 position) {
glm_vec3_copy((float_t *)position, body->position);
}
void physicsBodyGetPosition(const physicsbody_t *body, vec3 out) {
glm_vec3_copy((float_t *)body->position, out);
}
void physicsBodySetVelocity(physicsbody_t *body, const vec3 velocity) {
glm_vec3_copy((float_t *)velocity, body->velocity);
}
void physicsBodyGetVelocity(const physicsbody_t *body, vec3 out) {
glm_vec3_copy((float_t *)body->velocity, out);
}
void physicsBodyApplyImpulse(physicsbody_t *body, const vec3 impulse) {
if (body->type == PHYSICS_BODY_STATIC) return;
glm_vec3_add(body->velocity, (float_t *)impulse, body->velocity);
}
bool physicsBodyIsOnGround(const physicsbody_t *body) {
return body->onGround;
}
src/dusk/physics/physicsbody.h
-57
View File
@@ -1,57 +0,0 @@
/**
* Copyright (c) 2026 Dominic Masters
*
* This software is released under the MIT License.
* https://opensource.org/licenses/MIT
*/
#pragma once
#include "physicsshape.h"
#include "physicsbodytype.h"
typedef struct {
bool active;
physicsbodytype_t type;
physicsshape_t shape;
vec3 position;
/**
* Linear velocity (m/s). For KINEMATIC bodies this is not driven by the
* simulation — set it yourself and pass velocity*dt to physicsWorldMoveBody.
*/
vec3 velocity;
/** Multiplier applied to world gravity (1.0 = normal, 0.0 = no gravity). */
float_t gravityScale;
/** Set to true after a downward-facing collision is resolved. Reset each
* step / each physicsWorldMoveBody call. */
bool onGround;
} physicsbody_t;
/**
* Copies position into the body.
*/
void physicsBodySetPosition(physicsbody_t *body, const vec3 position);
/**
* Copies the body's current position into out.
*/
void physicsBodyGetPosition(const physicsbody_t *body, vec3 out);
/**
* Copies velocity into the body.
*/
void physicsBodySetVelocity(physicsbody_t *body, const vec3 velocity);
/**
* Copies the body's current velocity into out.
*/
void physicsBodyGetVelocity(const physicsbody_t *body, vec3 out);
/**
* Adds impulse (immediate velocity change) to a body. No-op on STATIC bodies.
*/
void physicsBodyApplyImpulse(physicsbody_t *body, const vec3 impulse);
/**
* Returns true when the body rested on a surface during the last step or move.
*/
bool physicsBodyIsOnGround(const physicsbody_t *body);
src/dusk/physics/physicsbodytype.h
+1
View File
@@ -6,6 +6,7 @@
*/
#pragma once
#include "dusk.h"
typedef enum {
/** Never moves. Acts as an immovable collision surface. */
src/dusk/physics/physicsmanager.c
+2 -4
View File
@@ -8,10 +8,8 @@
#include "physicsmanager.h"
#include "time/time.h"
physicsworld_t PHYSICS_WORLD;
void physicsManagerInit(void) {
physicsWorldInit(&PHYSICS_WORLD);
physicsWorldInit();
}
void physicsManagerUpdate() {
@@ -19,5 +17,5 @@ void physicsManagerUpdate() {
if(TIME.dynamicUpdate) return; // Don't update on dynamic updates.
#endif
physicsWorldStep(&PHYSICS_WORLD, TIME.delta);
physicsWorldStep(TIME.delta);
}
src/dusk/physics/physicsmanager.h
+1 -3
View File
@@ -8,10 +8,8 @@
#pragma once
#include "physicsworld.h"
extern physicsworld_t PHYSICS_WORLD;
/**
* Initializes the global physics world with default gravity (0, -9.81, 0).
* Initializes the physics manager.
*/
void physicsManagerInit(void);
src/dusk/physics/physicstest.c
+360
View File
@@ -0,0 +1,360 @@
/**
* Copyright (c) 2026 Dominic Masters
*
* This software is released under the MIT License.
* https://opensource.org/licenses/MIT
*/
#include "physicstest.h"
/* ===========================================================
* Low-level collision primitives.
* Convention for all helpers:
* outNormal — points from shape-B toward shape-A
* (add outNormal * outDepth to A to separate it from B)
* outDepth — positive penetration depth
* return — true if overlapping
* =========================================================== */
static bool aabbVsAabb(
const vec3 ac, const vec3 ah, /* A center, half-extents */
const vec3 bc, const vec3 bh, /* B center, half-extents */
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;
}
static bool sphereVsSphere(
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); /* A - B */
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;
}
/* outNormal: from AABB (b) toward sphere (a) */
static bool sphereVsAabb(
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 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 {
/* Sphere center is inside the AABB — find nearest face. */
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])
};
static const float_t normals[6][3] = {
{1,0,0},{-1,0,0},{0,1,0},{0,-1,0},{0,0,1},{0,0,-1}
};
int mi = 0;
for (int 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;
}
/* outNormal: plane normal (from plane toward A) */
static bool sphereVsPlane(
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;
}
static bool aabbVsPlane(
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;
}
static void closestPointOnSegment(
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);
}
static void closestPointsBetweenSegments(
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);
}
/* capsule axis: (cc.x, cc.y ± halfHeight, cc.z), oriented along Y. */
/* outNormal: from sphere toward capsule */
static bool capsuleVsSphere(
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;
closestPointOnSegment(capA, capB, sc, closest);
return sphereVsSphere(closest, cr, sc, sr, outNormal, outDepth);
}
/* outNormal: from AABB toward capsule */
static bool capsuleVsAabb(
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;
closestPointOnSegment(capA, capB, ac, closest);
return sphereVsAabb(closest, cr, ac, ah, outNormal, outDepth);
}
/* outNormal: from plane toward capsule */
static bool capsuleVsPlane(
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;
}
/* outNormal: from capsule-B toward capsule-A */
static bool capsuleVsCapsule(
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;
closestPointsBetweenSegments(a1, b1, a2, b2, p1, p2);
return sphereVsSphere(p1, r1, p2, r2, outNormal, outDepth);
}
/* ===========================================================
* Dispatch: tests two shapes and returns push-out for A.
* outNormal points from B toward A.
* =========================================================== */
static bool 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;
/* Plane is always the reference surface; treat as B. */
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 aabbVsPlane(aPos, aShape.data.cube.halfExtents, pn, pd, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE:
return sphereVsPlane(aPos, aShape.data.sphere.radius, pn, pd, outNormal, outDepth);
case PHYSICS_SHAPE_CAPSULE:
return capsuleVsPlane(aPos, aShape.data.capsule.radius, aShape.data.capsule.halfHeight, pn, pd, outNormal, outDepth);
default:
return false;
}
}
/* If A is a plane, swap roles and negate. */
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, *ah = aShape.data.cube.halfExtents;
switch (tb) {
case PHYSICS_SHAPE_CUBE:
return aabbVsAabb(ac, ah, bPos, bShape.data.cube.halfExtents, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE: {
vec3 tmp; float_t d;
if (!sphereVsAabb(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 (!capsuleVsAabb(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 sphereVsAabb(aPos, sr, bPos, bShape.data.cube.halfExtents, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE:
return sphereVsSphere(aPos, sr, bPos, bShape.data.sphere.radius, outNormal, outDepth);
case PHYSICS_SHAPE_CAPSULE: {
vec3 tmp; float_t d;
if (!capsuleVsSphere(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 capsuleVsAabb(aPos, cr, chh, bPos, bShape.data.cube.halfExtents, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE:
return capsuleVsSphere(aPos, cr, chh, bPos, bShape.data.sphere.radius, outNormal, outDepth);
case PHYSICS_SHAPE_CAPSULE:
return capsuleVsCapsule(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);
}
src/dusk/physics/physicstest.h
+32
View File
@@ -0,0 +1,32 @@
/**
* Copyright (c) 2026 Dominic Masters
*
* This software is released under the MIT License.
* https://opensource.org/licenses/MIT
*/
#pragma once
#include "physicsshape.h"
/**
* Tests for collision between two shapes. Returns true if they overlap, and if
* so, outputs the push-out normal and depth.
*
* @param aPos Position of shape A.
* @param aShape Shape of A.
* @param bPos Position of shape B.
* @param bShape Shape of B.
* @param outNormal Output push-out normal (points from B toward A).
* @param outDepth Output penetration depth (positive).
* @return true if 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
);
src/dusk/physics/physicsworld.c
+110 -469
View File
@@ -8,507 +8,148 @@
#include "physicsworld.h"
#include "assert/assert.h"
#include "util/memory.h"
#include "entity/entity.h"
#include "entity/component.h"
#include "physicstest.h"
/* ---- Threshold for "close enough to ground" (cos ~45°) ---- */
#define PHYSICS_GROUND_THRESHOLD 0.707f
physicsworld_t PHYSICS_WORLD;
/* ===========================================================
* Low-level collision primitives.
* Convention for all helpers:
* outNormal — points from shape-B toward shape-A
* (add outNormal * outDepth to A to separate it from B)
* outDepth — positive penetration depth
* return — true if overlapping
* =========================================================== */
void physicsWorldInit() {
memoryZero(&PHYSICS_WORLD, sizeof(physicsworld_t));
static bool aabbVsAabb(
const vec3 ac, const vec3 ah, /* A center, half-extents */
const vec3 bc, const vec3 bh, /* B center, half-extents */
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;
PHYSICS_WORLD.gravity[0] = 0.0f;
PHYSICS_WORLD.gravity[1] = -9.81f;
PHYSICS_WORLD.gravity[2] = 0.0f;
}
static bool sphereVsSphere(
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); /* A - B */
float_t dist2 = glm_vec3_norm2(diff);
float_t sumR = ar + br;
void physicsWorldStep(const float_t dt) {
assertTrue(dt > 0.0f, "Delta time must be positive");
if (dist2 >= sumR * sumR) return false;
entityid_t physEnts[ENTITY_COUNT_MAX];
componentid_t physComps[ENTITY_COUNT_MAX];
entityid_t physCount = componentGetEntitiesWithComponent(
COMPONENT_TYPE_PHYSICS, physEnts, physComps
);
float_t dist = sqrtf(dist2);
*outDepth = sumR - dist;
/* Phase 1: integrate dynamic bodies (gravity + velocity → position). */
for(entityid_t i = 0; i < physCount; i++) {
entityid_t id = physEnts[i];
componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
if(posComp == 0xFF) continue;
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;
entityphysics_t *phys = entityPhysicsGet(id, physComps[i]);
if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
phys->onGround = false;
phys->velocity[0] += PHYSICS_WORLD.gravity[0] * phys->gravityScale * dt;
phys->velocity[1] += PHYSICS_WORLD.gravity[1] * phys->gravityScale * dt;
phys->velocity[2] += PHYSICS_WORLD.gravity[2] * phys->gravityScale * dt;
vec3 pos;
entityPositionGetPosition(id, posComp, pos);
pos[0] += phys->velocity[0] * dt;
pos[1] += phys->velocity[1] * dt;
pos[2] += phys->velocity[2] * dt;
entityPositionSetPosition(id, posComp, pos);
}
/* outNormal: from AABB (b) toward sphere (a) */
static bool sphereVsAabb(
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])
};
/* Phase 2: dynamic vs static/kinematic — push dynamic fully. */
for(entityid_t i = 0; i < physCount; i++) {
entityid_t id = physEnts[i];
componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
if(posComp == 0xFF) continue;
vec3 diff;
glm_vec3_sub((float_t *)sc, closest, diff);
float_t dist2 = glm_vec3_norm2(diff);
entityphysics_t *phys = entityPhysicsGet(id, physComps[i]);
if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
bool inside = (dist2 < 1e-10f);
if (!inside && dist2 >= sr * sr) return false;
vec3 pos;
entityPositionGetPosition(id, posComp, pos);
if (!inside) {
float_t dist = sqrtf(dist2);
*outDepth = sr - dist;
glm_vec3_scale(diff, 1.0f / dist, outNormal);
} else {
/* Sphere center is inside the AABB — find nearest face. */
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])
};
static const float_t normals[6][3] = {
{1,0,0},{-1,0,0},{0,1,0},{0,-1,0},{0,0,1},{0,0,-1}
};
int mi = 0;
for (int 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;
}
/* outNormal: plane normal (from plane toward A) */
static bool sphereVsPlane(
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;
}
static bool aabbVsPlane(
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;
}
static void closestPointOnSegment(
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);
}
static void closestPointsBetweenSegments(
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);
}
/* capsule axis: (cc.x, cc.y ± halfHeight, cc.z), oriented along Y. */
/* outNormal: from sphere toward capsule */
static bool capsuleVsSphere(
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;
closestPointOnSegment(capA, capB, sc, closest);
return sphereVsSphere(closest, cr, sc, sr, outNormal, outDepth);
}
/* outNormal: from AABB toward capsule */
static bool capsuleVsAabb(
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;
closestPointOnSegment(capA, capB, ac, closest);
return sphereVsAabb(closest, cr, ac, ah, outNormal, outDepth);
}
/* outNormal: from plane toward capsule */
static bool capsuleVsPlane(
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;
}
/* outNormal: from capsule-B toward capsule-A */
static bool capsuleVsCapsule(
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;
closestPointsBetweenSegments(a1, b1, a2, b2, p1, p2);
return sphereVsSphere(p1, r1, p2, r2, outNormal, outDepth);
}
/* ===========================================================
* Dispatch: tests two bodies and returns the push-out vector
* for body A (outNormal points from B toward A).
* =========================================================== */
static bool physicsTestOverlapBodies(
const physicsbody_t *a,
const physicsbody_t *b,
vec3 outNormal,
float_t *outDepth
) {
physicshapetype_t ta = a->shape.type;
physicshapetype_t tb = b->shape.type;
/* Plane is always the reference surface; treat as B. */
if (tb == PHYSICS_SHAPE_PLANE) {
const float_t *pn = b->shape.data.plane.normal;
const float_t pd = b->shape.data.plane.distance;
switch (ta) {
case PHYSICS_SHAPE_CUBE:
return aabbVsPlane(a->position, a->shape.data.cube.halfExtents, pn, pd, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE:
return sphereVsPlane(a->position, a->shape.data.sphere.radius, pn, pd, outNormal, outDepth);
case PHYSICS_SHAPE_CAPSULE:
return capsuleVsPlane(a->position, a->shape.data.capsule.radius, a->shape.data.capsule.halfHeight, pn, pd, outNormal, outDepth);
default: return false;
}
}
/* If A is a plane, swap roles and negate. */
if (ta == PHYSICS_SHAPE_PLANE) {
vec3 tmp; float_t d;
if (!physicsTestOverlapBodies(b, a, 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 = a->position, *ah = a->shape.data.cube.halfExtents;
switch (tb) {
case PHYSICS_SHAPE_CUBE:
return aabbVsAabb(ac, ah, b->position, b->shape.data.cube.halfExtents, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE: {
/* A=cube B=sphere: want normal from B toward A; sphereVsAabb gives from A(AABB) toward B(sphere) → negate. */
vec3 tmp; float_t d;
if (!sphereVsAabb(b->position, b->shape.data.sphere.radius, ac, ah, tmp, &d)) return false;
glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
}
case PHYSICS_SHAPE_CAPSULE: {
/* A=cube B=capsule: capsuleVsAabb(capsule=B, aabb=A) → normal from A(AABB) toward B(cap) → negate. */
vec3 tmp; float_t d;
if (!capsuleVsAabb(b->position, b->shape.data.capsule.radius, b->shape.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 = a->shape.data.sphere.radius;
switch (tb) {
case PHYSICS_SHAPE_CUBE:
/* sphereVsAabb(sphere=A, aabb=B): normal from AABB(B) toward sphere(A) ✓ */
return sphereVsAabb(a->position, sr, b->position, b->shape.data.cube.halfExtents, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE:
return sphereVsSphere(a->position, sr, b->position, b->shape.data.sphere.radius, outNormal, outDepth);
case PHYSICS_SHAPE_CAPSULE: {
/* A=sphere B=capsule: capsuleVsSphere(cap=B, sphere=A) → normal from A(sphere) toward B(cap) → negate. */
vec3 tmp; float_t d;
if (!capsuleVsSphere(b->position, b->shape.data.capsule.radius, b->shape.data.capsule.halfHeight, a->position, 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 = a->shape.data.capsule.radius;
const float_t chh = a->shape.data.capsule.halfHeight;
switch (tb) {
case PHYSICS_SHAPE_CUBE:
/* capsuleVsAabb(cap=A, aabb=B): normal from AABB(B) toward cap(A) ✓ */
return capsuleVsAabb(a->position, cr, chh, b->position, b->shape.data.cube.halfExtents, outNormal, outDepth);
case PHYSICS_SHAPE_SPHERE:
/* capsuleVsSphere(cap=A, sphere=B): normal from sphere(B) toward cap(A) ✓ */
return capsuleVsSphere(a->position, cr, chh, b->position, b->shape.data.sphere.radius, outNormal, outDepth);
case PHYSICS_SHAPE_CAPSULE:
return capsuleVsCapsule(a->position, cr, chh, b->position, b->shape.data.capsule.radius, b->shape.data.capsule.halfHeight, outNormal, outDepth);
default: return false;
}
}
default: return false;
}
}
/* ===========================================================
* Public API
* =========================================================== */
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;
}
physicsbody_t *physicsWorldAddBody(physicsworld_t *world) {
assertNotNull(world, "World cannot be NULL");
for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
physicsbody_t *b = &world->bodies[i];
if (b->active) continue;
memoryZero(b, sizeof(physicsbody_t));
b->active = true;
b->type = PHYSICS_BODY_DYNAMIC;
b->gravityScale = 1.0f;
b->shape.type = PHYSICS_SHAPE_CUBE;
b->shape.data.cube.halfExtents[0] = 0.5f;
b->shape.data.cube.halfExtents[1] = 0.5f;
b->shape.data.cube.halfExtents[2] = 0.5f;
return b;
}
return NULL;
}
void physicsWorldRemoveBody(physicsworld_t *world, physicsbody_t *body) {
assertNotNull(world, "World cannot be NULL");
assertNotNull(body, "Body cannot be NULL");
body->active = false;
}
void physicsWorldStep(physicsworld_t *world, float_t dt) {
assertNotNull(world, "World cannot be NULL");
/* 1. Reset ground flags and integrate dynamic bodies. */
for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
physicsbody_t *b = &world->bodies[i];
if (!b->active || b->type != PHYSICS_BODY_DYNAMIC) continue;
b->onGround = false;
b->velocity[0] += world->gravity[0] * b->gravityScale * dt;
b->velocity[1] += world->gravity[1] * b->gravityScale * dt;
b->velocity[2] += world->gravity[2] * b->gravityScale * dt;
b->position[0] += b->velocity[0] * dt;
b->position[1] += b->velocity[1] * dt;
b->position[2] += b->velocity[2] * dt;
}
/* 2. Resolve dynamic vs static / kinematic (push dynamic fully). */
for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
physicsbody_t *a = &world->bodies[i];
if (!a->active || a->type != PHYSICS_BODY_DYNAMIC) continue;
for (int32_t j = 0; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
for(entityid_t j = 0; j < physCount; j++) {
if(i == j) continue;
physicsbody_t *b = &world->bodies[j];
if (!b->active || b->type == PHYSICS_BODY_DYNAMIC) continue;
entityid_t otherId = physEnts[j];
componentid_t otherPosComp = entityGetComponent(otherId, COMPONENT_TYPE_POSITION);
if(otherPosComp == 0xFF) continue;
entityphysics_t *otherPhys = entityPhysicsGet(otherId, physComps[j]);
if(otherPhys->type == PHYSICS_BODY_DYNAMIC) continue;
vec3 otherPos;
entityPositionGetPosition(otherId, otherPosComp, otherPos);
vec3 normal; float_t depth;
if (!physicsTestOverlapBodies(a, b, normal, &depth)) continue;
if(!physicsTestShapeVsShape(
pos, phys->shape, otherPos, otherPhys->shape, normal, &depth
)) continue;
a->position[0] += normal[0] * depth;
a->position[1] += normal[1] * depth;
a->position[2] += normal[2] * depth;
pos[0] += normal[0] * depth;
pos[1] += normal[1] * depth;
pos[2] += normal[2] * depth;
entityPositionSetPosition(id, posComp, pos);
/* Cancel velocity into the surface. */
float_t vn = glm_vec3_dot(a->velocity, normal);
float_t vn = glm_vec3_dot(phys->velocity, normal);
if(vn < 0.0f) {
a->velocity[0] -= vn * normal[0];
a->velocity[1] -= vn * normal[1];
a->velocity[2] -= vn * normal[2];
phys->velocity[0] -= vn * normal[0];
phys->velocity[1] -= vn * normal[1];
phys->velocity[2] -= vn * normal[2];
}
if (normal[1] > PHYSICS_GROUND_THRESHOLD) a->onGround = true;
if(normal[1] > PHYSICS_GROUND_THRESHOLD) phys->onGround = true;
}
}
/* 3. Resolve dynamic vs dynamic (each pair once, elastic equal-mass). */
for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
physicsbody_t *a = &world->bodies[i];
if (!a->active || a->type != PHYSICS_BODY_DYNAMIC) continue;
/* Phase 3: dynamic vs dynamic — push each half-way, exchange velocities. */
for(entityid_t i = 0; i < physCount; i++) {
entityid_t idA = physEnts[i];
componentid_t posCompA = entityGetComponent(idA, COMPONENT_TYPE_POSITION);
if(posCompA == 0xFF) continue;
for (int32_t j = i + 1; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
physicsbody_t *b = &world->bodies[j];
if (!b->active || b->type != PHYSICS_BODY_DYNAMIC) continue;
entityphysics_t *physA = entityPhysicsGet(idA, physComps[i]);
if(physA->type != PHYSICS_BODY_DYNAMIC) continue;
vec3 posA;
entityPositionGetPosition(idA, posCompA, posA);
for(entityid_t j = i + 1; j < physCount; j++) {
entityid_t idB = physEnts[j];
componentid_t posCompB = entityGetComponent(idB, COMPONENT_TYPE_POSITION);
if(posCompB == 0xFF) continue;
entityphysics_t *physB = entityPhysicsGet(idB, physComps[j]);
if(physB->type != PHYSICS_BODY_DYNAMIC) continue;
vec3 posB;
entityPositionGetPosition(idB, posCompB, posB);
vec3 normal; float_t depth;
if (!physicsTestOverlapBodies(a, b, normal, &depth)) continue;
if(!physicsTestShapeVsShape(
posA, physA->shape, posB, physB->shape, normal, &depth
)) continue;
/* Push both half-way. */
a->position[0] += normal[0] * depth * 0.5f;
a->position[1] += normal[1] * depth * 0.5f;
a->position[2] += normal[2] * depth * 0.5f;
b->position[0] -= normal[0] * depth * 0.5f;
b->position[1] -= normal[1] * depth * 0.5f;
b->position[2] -= normal[2] * depth * 0.5f;
posA[0] += normal[0] * depth * 0.5f;
posA[1] += normal[1] * depth * 0.5f;
posA[2] += normal[2] * depth * 0.5f;
entityPositionSetPosition(idA, posCompA, posA);
/* Exchange velocity components along normal (elastic equal-mass). */
float_t v_rel = glm_vec3_dot(a->velocity, normal)
- glm_vec3_dot(b->velocity, normal);
posB[0] -= normal[0] * depth * 0.5f;
posB[1] -= normal[1] * depth * 0.5f;
posB[2] -= normal[2] * depth * 0.5f;
entityPositionSetPosition(idB, posCompB, posB);
float_t v_rel = glm_vec3_dot(physA->velocity, normal)
- glm_vec3_dot(physB->velocity, normal);
if(v_rel < 0.0f) {
a->velocity[0] -= v_rel * normal[0];
a->velocity[1] -= v_rel * normal[1];
a->velocity[2] -= v_rel * normal[2];
b->velocity[0] += v_rel * normal[0];
b->velocity[1] += v_rel * normal[1];
b->velocity[2] += v_rel * normal[2];
physA->velocity[0] -= v_rel * normal[0];
physA->velocity[1] -= v_rel * normal[1];
physA->velocity[2] -= v_rel * normal[2];
physB->velocity[0] += v_rel * normal[0];
physB->velocity[1] += v_rel * normal[1];
physB->velocity[2] += v_rel * normal[2];
}
if ( normal[1] > PHYSICS_GROUND_THRESHOLD) a->onGround = true;
if (-normal[1] > PHYSICS_GROUND_THRESHOLD) b->onGround = true;
if( normal[1] > PHYSICS_GROUND_THRESHOLD) physA->onGround = true;
if(-normal[1] > PHYSICS_GROUND_THRESHOLD) physB->onGround = true;
}
}
}
void physicsWorldMoveBody(
physicsworld_t *world,
physicsbody_t *body,
const vec3 motion
) {
assertNotNull(world, "World cannot be NULL");
assertNotNull(body, "Body cannot be NULL");
body->onGround = false;
body->position[0] += motion[0];
body->position[1] += motion[1];
body->position[2] += motion[2];
for (int32_t j = 0; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
physicsbody_t *b = &world->bodies[j];
if (!b->active || b == body) continue;
if (b->type == PHYSICS_BODY_KINEMATIC) continue;
vec3 normal; float_t depth;
if (!physicsTestOverlapBodies(body, b, normal, &depth)) continue;
body->position[0] += normal[0] * depth;
body->position[1] += normal[1] * depth;
body->position[2] += normal[2] * depth;
if (normal[1] > PHYSICS_GROUND_THRESHOLD) body->onGround = true;
}
}
src/dusk/physics/physicsworld.h
+15 -26
View File
@@ -6,39 +6,28 @@
*/
#pragma once
#include "physicsbody.h"
#include "physics/physicsshape.h"
#include "physics/physicsbodytype.h"
#define PHYSICS_WORLD_BODY_COUNT_MAX 64
#define PHYSICS_GROUND_THRESHOLD 0.707f
typedef struct {
physicsbody_t bodies[PHYSICS_WORLD_BODY_COUNT_MAX];
vec3 gravity;
} physicsworld_t;
/**
* Initializes the physics world with default gravity (0, -9.81, 0).
*/
void physicsWorldInit(physicsworld_t *world);
extern physicsworld_t PHYSICS_WORLD;
/**
* Allocates a body slot from the world and returns a pointer to it.
* Defaults: DYNAMIC, unit CUBE half-extents, gravityScale=1.
* Returns NULL if the world is full.
* Initializes the physics world.
*/
physicsbody_t *physicsWorldAddBody(physicsworld_t *world);
void physicsWorldInit(void);
/**
* Releases a body slot back to the world.
* Steps the physics simulation forward.
*
* @param dt The time delta in seconds since the last step.
*/
void physicsWorldRemoveBody(physicsworld_t *world, physicsbody_t *body);
/**
* Steps the simulation by dt seconds:
* 1. Integrates DYNAMIC bodies (gravity + velocity).
* 2. Resolves DYNAMIC vs STATIC/KINEMATIC collisions.
* 3. Resolves DYNAMIC vs DYNAMIC collisions (each pair once).
*/
void physicsWorldStep(physicsworld_t *world, float_t dt);
void physicsWorldStep(const float_t dt);
/**
* Moves a KINEMATIC body by motion and immediately resolves overlaps against
@@ -48,8 +37,8 @@ void physicsWorldStep(physicsworld_t *world, float_t dt);
* @param body The kinematic body to move (must be PHYSICS_BODY_KINEMATIC).
* @param motion World-space displacement for this frame.
*/
void physicsWorldMoveBody(
physicsworld_t *world,
physicsbody_t *body,
const vec3 motion
);
// void physicsWorldMoveBody(
// physicsworld_t *world,
// physicsbody_t *body,
// const vec3 motion
// );