Physics
This commit is contained in:
@@ -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)
|
||||
@@ -5,3 +5,4 @@
|
||||
|
||||
# Subdirs
|
||||
add_subdirectory(display)
|
||||
add_subdirectory(physics)
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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;
|
||||
@@ -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;
|
||||
@@ -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
|
||||
);
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
@@ -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]);
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
+10
-1
@@ -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();
|
||||
}
|
||||
|
||||
|
||||
+13
-1
@@ -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.
|
||||
*/
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
Reference in New Issue
Block a user