From 6c5738c863f80e0fc208802d9784c0f6797066ef Mon Sep 17 00:00:00 2001 From: Dominic Masters Date: Sat, 18 Jul 2026 20:53:22 -0500 Subject: [PATCH] Testing physics --- src/dusk/display/mesh/cube.c | 4 +- src/dusk/display/mesh/cube.h | 4 +- src/dusk/engine/engine.c | 79 +++++++++++++------ src/dusk/engine/engine.h | 24 +----- .../component/display/entityrenderable.c | 16 ++++ .../component/display/entityrenderable.h | 17 ++++ src/dusk/physics/physicsshapemesh.c | 58 +++++++++++++- src/dusk/physics/physicsshapemesh.h | 39 +++++++-- src/dusk/physics/physicstest.c | 64 +++++++-------- src/dusk/physics/physicstest.h | 8 +- src/dusk/physics/physicsworld.c | 51 +++++++----- src/dusk/system/system.c | 8 ++ src/dusk/system/system.h | 13 ++- src/duskdolphin/system/systemdolphin.c | 4 + src/duskdolphin/system/systemdolphin.h | 10 ++- src/duskdolphin/system/systemplatform.h | 3 +- src/dusklinux/system/systemlinux.c | 20 +++++ src/dusklinux/system/systemlinux.h | 13 ++- src/dusklinux/system/systemplatform.h | 3 +- src/duskpsp/system/systemplatform.h | 3 +- src/duskpsp/system/systempsp.c | 6 ++ src/duskpsp/system/systempsp.h | 10 ++- test/physics/test_physicsshapemesh.c | 2 +- test/physics/test_physicstest.c | 14 ++-- 24 files changed, 347 insertions(+), 126 deletions(-) diff --git a/src/dusk/display/mesh/cube.c b/src/dusk/display/mesh/cube.c index 4e70b81d..25a24545 100644 --- a/src/dusk/display/mesh/cube.c +++ b/src/dusk/display/mesh/cube.c @@ -12,8 +12,8 @@ mesh_t CUBE_MESH_SIMPLE; meshvertex_t CUBE_MESH_SIMPLE_VERTICES[CUBE_VERTEX_COUNT]; errorret_t cubeInit() { - vec3 min = { 0.0f, 0.0f, 0.0f }; - vec3 max = { 1.0f, 1.0f, 1.0f }; + vec3 min = { -0.5f, -0.5f, -0.5f }; + vec3 max = { 0.5f, 0.5f, 0.5f }; cubeBuffer(CUBE_MESH_SIMPLE_VERTICES, min, max); errorChain(meshInit( &CUBE_MESH_SIMPLE, diff --git a/src/dusk/display/mesh/cube.h b/src/dusk/display/mesh/cube.h index 8a21b72c..4b6a7d97 100644 --- a/src/dusk/display/mesh/cube.h +++ b/src/dusk/display/mesh/cube.h @@ -17,7 +17,9 @@ extern mesh_t CUBE_MESH_SIMPLE; extern meshvertex_t CUBE_MESH_SIMPLE_VERTICES[CUBE_VERTEX_COUNT]; /** - * Initializes the simple unit cube mesh (0,0,0) to (1,1,1). + * Initializes the simple unit cube mesh, centered at (0,0,0), spanning + * (-0.5,-0.5,-0.5) to (0.5,0.5,0.5) -- matching the centered convention + * physics shapes and the sphere mesh use (position = center). * * @return Error for initialization of the cube mesh. */ diff --git a/src/dusk/engine/engine.c b/src/dusk/engine/engine.c index 9052d5e5..4561e511 100644 --- a/src/dusk/engine/engine.c +++ b/src/dusk/engine/engine.c @@ -14,6 +14,9 @@ #include "scene/scene.h" #include "entity/entitymanager.h" #include "entity/component/display/entityposition.h" +#include "entity/component/display/entityrenderable.h" +#include "entity/component/physics/entityphysics.h" +#include "display/color.h" #include "asset/asset.h" #include "ui/ui.h" #include "assert/assert.h" @@ -48,7 +51,8 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) { errorChain(networkInit()); errorChain(sceneInit()); - // Test: a spinning cube, viewed by a static camera. + // Test: three colored cubes falling onto each other and a static floor, + // viewed by a static camera. ENGINE.testSceneId = sceneCreate(); sceneSetActive(ENGINE.testSceneId); entitymanager_t *testEntities = sceneGetEntities(ENGINE.testSceneId); @@ -60,22 +64,64 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) { entityAddComponent(testEntities, testCamera, COMPONENT_TYPE_CAMERA); entityPositionLookAt( testEntities, testCamera, testCameraPosition, - (vec3){ 3.0f, 3.0f, 3.0f }, + (vec3){ 4.0f, 3.0f, 4.0f }, (vec3){ 0.0f, 0.0f, 0.0f }, (vec3){ 0.0f, 1.0f, 0.0f } ); - ENGINE.testCubeEntity = entityManagerAdd(testEntities); - ENGINE.testCubePositionComponent = entityAddComponent( - testEntities, ENGINE.testCubeEntity, COMPONENT_TYPE_POSITION + // Static floor: a wide, thin box. Its render scale matches its physics + // half-extents exactly, since CUBE_MESH_SIMPLE and PHYSICS_SHAPE_CUBE are + // both centered on the entity's position. + entityid_t floorEntity = entityManagerAdd(testEntities); + componentid_t floorPosition = entityAddComponent( + testEntities, floorEntity, COMPONENT_TYPE_POSITION ); - entityAddComponent( - testEntities, ENGINE.testCubeEntity, COMPONENT_TYPE_RENDERABLE + entityPositionSetLocalPosition( + testEntities, floorEntity, floorPosition, (vec3){ 0.0f, -1.0f, 0.0f } ); - entityUpdateAdd( - testEntities, ENGINE.testCubeEntity, engineTestCubeRotate, - ENGINE.testCubePositionComponent, NULL + entityPositionSetLocalScale( + testEntities, floorEntity, floorPosition, (vec3){ 10.0f, 1.0f, 10.0f } ); + componentid_t floorPhysics = entityAddComponent( + testEntities, floorEntity, COMPONENT_TYPE_PHYSICS + ); + entityPhysicsSetBodyType( + testEntities, floorEntity, floorPhysics, PHYSICS_BODY_STATIC + ); + entityPhysicsSetShape(testEntities, floorEntity, floorPhysics, (physicsshape_t){ + .type = PHYSICS_SHAPE_CUBE, + .data.cube.halfExtents = { 5.0f, 0.5f, 5.0f } + }); + componentid_t floorRenderable = entityAddComponent( + testEntities, floorEntity, COMPONENT_TYPE_RENDERABLE + ); + entityRenderableSetColor(testEntities, floorEntity, floorRenderable, COLOR_GRAY); + + // Three dynamic cubes, staggered above the floor with slight offsets so + // they tumble and land on each other instead of falling in perfect sync. + vec3 cubeStartPositions[3] = { + { -0.3f, 1.5f, 0.1f }, + { 0.1f, 3.0f, -0.2f }, + { -0.1f, 4.5f, 0.2f }, + }; + color_t cubeColors[3] = { COLOR_RED, COLOR_GREEN, COLOR_BLUE }; + + for(uint8_t i = 0; i < 3; i++) { + entityid_t cubeEntity = entityManagerAdd(testEntities); + componentid_t cubePosition = entityAddComponent( + testEntities, cubeEntity, COMPONENT_TYPE_POSITION + ); + entityPositionSetLocalPosition( + testEntities, cubeEntity, cubePosition, cubeStartPositions[i] + ); + entityAddComponent(testEntities, cubeEntity, COMPONENT_TYPE_PHYSICS); + componentid_t cubeRenderable = entityAddComponent( + testEntities, cubeEntity, COMPONENT_TYPE_RENDERABLE + ); + entityRenderableSetColor( + testEntities, cubeEntity, cubeRenderable, cubeColors[i] + ); + } networkRequestConnection( engineNetworkOnConnected, @@ -207,16 +253,3 @@ void engineNetworkOnDisconnect(errorret_t error, void *user) { void engineNetworkDisconnectTestOnComplete(void *user) { consolePrint("Network disconnect test complete"); } - -void engineTestCubeRotate( - entitymanager_t *mgr, - const entityid_t entityId, - const componentid_t componentId, - void *user -) { - ENGINE.testCubeRotation += TIME.delta; - entityPositionSetLocalRotation( - mgr, entityId, componentId, - (vec3){ 0.0f, ENGINE.testCubeRotation, 0.0f } - ); -} diff --git a/src/dusk/engine/engine.h b/src/dusk/engine/engine.h index 3e4b0c38..31fc6667 100644 --- a/src/dusk/engine/engine.h +++ b/src/dusk/engine/engine.h @@ -23,11 +23,9 @@ typedef struct { bool_t networkDisconnectTestPending; float_t networkDisconnectTestAt; - // Test: a spinning cube, viewed by a static camera. + // Test: three colored cubes falling onto each other and a static floor, + // viewed by a static camera. sceneid_t testSceneId; - entityid_t testCubeEntity; - componentid_t testCubePositionComponent; - float_t testCubeRotation; } engine_t; extern engine_t ENGINE; @@ -98,21 +96,3 @@ void engineNetworkOnDisconnect(errorret_t error, void *user); * @param user Unused. */ void engineNetworkDisconnectTestOnComplete(void *user); - -/** - * Entity update callback that spins the test cube around Y. Registered - * once in engineInit() via entityUpdateAdd(), so it only runs from - * sceneFixedUpdate() -- once per fixed timestep, never once per rendered - * frame. - * - * @param mgr The entity manager that owns the entity. - * @param entityId The test cube's entity ID. - * @param componentId The test cube's position component ID. - * @param user Unused. - */ -void engineTestCubeRotate( - entitymanager_t *mgr, - const entityid_t entityId, - const componentid_t componentId, - void *user -); diff --git a/src/dusk/entity/component/display/entityrenderable.c b/src/dusk/entity/component/display/entityrenderable.c index 6f320358..03af2edf 100644 --- a/src/dusk/entity/component/display/entityrenderable.c +++ b/src/dusk/entity/component/display/entityrenderable.c @@ -64,6 +64,22 @@ void entityRenderableSetPriority( r->priority = priority; } +void entityRenderableSetColor( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + const color_t color +) { + entityrenderable_t *r = componentGetData( + mgr, entityId, componentId, COMPONENT_TYPE_RENDERABLE + ); + assertTrue( + r->type == ENTITY_RENDERABLE_TYPE_SHADER_MATERIAL, + "Renderable must be ENTITY_RENDERABLE_TYPE_SHADER_MATERIAL to set color" + ); + r->data.material.material.unlit.color = color; +} + void entityRenderableSetDraw( entitymanager_t *mgr, const entityid_t entityId, diff --git a/src/dusk/entity/component/display/entityrenderable.h b/src/dusk/entity/component/display/entityrenderable.h index fa0d5037..0fa01183 100644 --- a/src/dusk/entity/component/display/entityrenderable.h +++ b/src/dusk/entity/component/display/entityrenderable.h @@ -124,6 +124,23 @@ void entityRenderableSetPriority( const int8_t priority ); +/** + * Sets the unlit material color. Only meaningful when the renderable is + * (or defaults to) ENTITY_RENDERABLE_TYPE_SHADER_MATERIAL -- asserts + * otherwise. + * + * @param mgr The entity manager that owns the entity. + * @param entityId The entity to configure. + * @param componentId The renderable component. + * @param color The color to tint the material with. + */ +void entityRenderableSetColor( + entitymanager_t *mgr, + const entityid_t entityId, + const componentid_t componentId, + const color_t color +); + /** * Sets the draw callback, switching the type to * ENTITY_RENDERABLE_TYPE_CUSTOM. diff --git a/src/dusk/physics/physicsshapemesh.c b/src/dusk/physics/physicsshapemesh.c index e91c05dd..994a2225 100644 --- a/src/dusk/physics/physicsshapemesh.c +++ b/src/dusk/physics/physicsshapemesh.c @@ -21,6 +21,39 @@ void physicsShapeMeshGetVertex( out[2] = p[2]; } +void physicsShapeMeshComputeBounds(physicsshapemesh_t *mesh) { + assertNotNull(mesh, "Mesh cannot be null"); + + const uint32_t vertexCount = mesh->triangleCount * 3; + if(vertexCount == 0) { + glm_vec3_zero(mesh->boundsCenter); + mesh->boundsRadius = 0.0f; + return; + } + + vec3 minPoint, maxPoint; + physicsShapeMeshGetVertex(mesh, 0, minPoint); + glm_vec3_copy(minPoint, maxPoint); + + for(uint32_t i = 1; i < vertexCount; i++) { + vec3 v; + physicsShapeMeshGetVertex(mesh, i, v); + if(v[0] < minPoint[0]) minPoint[0] = v[0]; + if(v[1] < minPoint[1]) minPoint[1] = v[1]; + if(v[2] < minPoint[2]) minPoint[2] = v[2]; + if(v[0] > maxPoint[0]) maxPoint[0] = v[0]; + if(v[1] > maxPoint[1]) maxPoint[1] = v[1]; + if(v[2] > maxPoint[2]) maxPoint[2] = v[2]; + } + + glm_vec3_add(minPoint, maxPoint, mesh->boundsCenter); + glm_vec3_scale(mesh->boundsCenter, 0.5f, mesh->boundsCenter); + + vec3 extent; + glm_vec3_sub(maxPoint, mesh->boundsCenter, extent); + mesh->boundsRadius = glm_vec3_norm(extent); +} + bool_t physicsShapeMeshTestSphere( const physicsshapemesh_t *mesh, const vec3 meshPos, const vec3 sphereCenter, const float_t sphereRadius, @@ -28,6 +61,15 @@ bool_t physicsShapeMeshTestSphere( ) { assertNotNull(mesh, "Mesh cannot be null"); + // Broad-phase: reject in O(1) against the cached bounding sphere before + // scanning every triangle. + vec3 worldBoundsCenter; + glm_vec3_add((float_t *)mesh->boundsCenter, (float_t *)meshPos, worldBoundsCenter); + float_t rejectRadius = mesh->boundsRadius + sphereRadius; + vec3 toCenter; + glm_vec3_sub((float_t *)sphereCenter, worldBoundsCenter, toCenter); + if(glm_vec3_norm2(toCenter) >= rejectRadius * rejectRadius) return false; + bool_t found = false; float_t bestDist2 = 0.0f; vec3 bestPoint; @@ -77,6 +119,19 @@ bool_t physicsShapeMeshTestCapsule( const float_t capsuleHalfHeight, vec3 outNormal, float_t *outDepth ) { + assertNotNull(mesh, "Mesh cannot be null"); + + // Broad-phase: reject the whole capsule (skipping every sample sphere + // below) against the cached bounding sphere before doing any real work. + // The capsule's own bounding sphere (radius + half-height) is a looser + // bound than its true shape, but cheap and always conservative. + vec3 worldBoundsCenter; + glm_vec3_add((float_t *)mesh->boundsCenter, (float_t *)meshPos, worldBoundsCenter); + float_t rejectRadius = mesh->boundsRadius + capsuleRadius + capsuleHalfHeight; + vec3 toCenter; + glm_vec3_sub((float_t *)capsuleCenter, worldBoundsCenter, toCenter); + if(glm_vec3_norm2(toCenter) >= rejectRadius * rejectRadius) return false; + vec3 capA = { capsuleCenter[0], capsuleCenter[1] - capsuleHalfHeight, capsuleCenter[2] }; @@ -142,8 +197,9 @@ bool_t physicsShapeMeshTest( } } -physicsshape_t physicsShapeMeshCreate(const physicsshapemesh_t *mesh) { +physicsshape_t physicsShapeMeshCreate(physicsshapemesh_t *mesh) { assertNotNull(mesh, "Mesh cannot be null"); + physicsShapeMeshComputeBounds(mesh); return (physicsshape_t){ .type = PHYSICS_SHAPE_CUSTOM, .data.custom = { .userData = (void *)mesh, .test = physicsShapeMeshTest } diff --git a/src/dusk/physics/physicsshapemesh.h b/src/dusk/physics/physicsshapemesh.h index 37f882bc..154ab61b 100644 --- a/src/dusk/physics/physicsshapemesh.h +++ b/src/dusk/physics/physicsshapemesh.h @@ -29,10 +29,13 @@ * outlive any physicsshape_t built from it, so it's typically a field * alongside whatever owns the mesh (e.g. a terrain chunk). * - * Every query scans all triangleCount triangles (no broad-phase/BVH), so - * for good performance keep each mesh shape scoped to one reasonably-sized - * chunk rather than an entire world, and only add nearby chunks' physics - * bodies to the scene. + * A query first rejects against the cached local-space bounding sphere + * (boundsCenter/boundsRadius, see physicsShapeMeshComputeBounds) in O(1); + * only once that overlaps does it fall through to scanning all + * triangleCount triangles. There's still no finer broad-phase/BVH within + * a mesh, so for good performance keep each mesh shape scoped to one + * reasonably-sized chunk rather than an entire world, and only add nearby + * chunks' physics bodies to the scene. */ typedef struct { /** Pointer to the first triangle vertex's position (3 consecutive float_t). */ @@ -41,6 +44,15 @@ typedef struct { size_t stride; /** Number of triangles. `vertices` holds triangleCount * 3 positions. */ uint32_t triangleCount; + + /** + * Cached local-space bounding sphere, set by + * physicsShapeMeshComputeBounds() (called once by physicsShapeMeshCreate). + * Used to reject a query in O(1) before scanning every triangle. + */ + vec3 boundsCenter; + /** Cached local-space bounding sphere radius. See boundsCenter. */ + float_t boundsRadius; } physicsshapemesh_t; /** @@ -56,6 +68,18 @@ void physicsShapeMeshGetVertex( const physicsshapemesh_t *mesh, const uint32_t index, vec3 out ); +/** + * Computes and caches mesh's local-space bounding sphere (boundsCenter/ + * boundsRadius) from its current vertices, by scanning every vertex once. + * Called automatically by physicsShapeMeshCreate(); call again yourself + * only if you mutate the underlying vertex buffer afterward (e.g. terrain + * deformation) and need the cached bounds to stay accurate. + * + * @param mesh The mesh to compute bounds for; boundsCenter/boundsRadius + * are written in place. + */ +void physicsShapeMeshComputeBounds(physicsshapemesh_t *mesh); + /** * Tests a sphere against every triangle in the mesh, keeping only the * single closest triangle. outNormal points from the mesh surface toward @@ -120,9 +144,12 @@ bool_t physicsShapeMeshTest( /** * Builds a PHYSICS_SHAPE_CUSTOM shape backed by the given triangle mesh. + * Calls physicsShapeMeshComputeBounds(mesh) once to populate its cached + * bounding sphere. * - * @param mesh Mesh descriptor. Must outlive the returned shape (see + * @param mesh Mesh descriptor; boundsCenter/boundsRadius are computed and + * written in place. Must outlive the returned shape (see * physicsshapemesh_t). * @return A physicsshape_t wired to test against this mesh. */ -physicsshape_t physicsShapeMeshCreate(const physicsshapemesh_t *mesh); +physicsshape_t physicsShapeMeshCreate(physicsshapemesh_t *mesh); diff --git a/src/dusk/physics/physicstest.c b/src/dusk/physics/physicstest.c index 8ab29307..737d25f7 100644 --- a/src/dusk/physics/physicstest.c +++ b/src/dusk/physics/physicstest.c @@ -321,12 +321,12 @@ bool_t physicsTestCapsuleVsCapsule( } bool_t physicsTestDispatch( - const vec3 aPos, const physicsshape_t aShape, - const vec3 bPos, const physicsshape_t bShape, + 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; + physicshapetype_t ta = aShape->type; + physicshapetype_t tb = bShape->type; assertFalse( ta == PHYSICS_SHAPE_CUSTOM && tb == PHYSICS_SHAPE_CUSTOM, @@ -337,8 +337,8 @@ bool_t physicsTestDispatch( // 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 + 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; @@ -348,30 +348,30 @@ bool_t physicsTestDispatch( 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 + 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; + 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, + aPos, aShape->data.cube.halfExtents, pn, pd, outNormal, outDepth ); case PHYSICS_SHAPE_SPHERE: return physicsTestSphereVsPlane( - aPos, aShape.data.sphere.radius, + aPos, aShape->data.sphere.radius, pn, pd, outNormal, outDepth ); case PHYSICS_SHAPE_CAPSULE: return physicsTestCapsuleVsPlane( aPos, - aShape.data.capsule.radius, - aShape.data.capsule.halfHeight, + aShape->data.capsule.radius, + aShape->data.capsule.halfHeight, pn, pd, outNormal, outDepth ); default: @@ -392,18 +392,18 @@ bool_t physicsTestDispatch( switch(ta) { case PHYSICS_SHAPE_CUBE: { const float_t *ac = aPos; - const float_t *ah = aShape.data.cube.halfExtents; + const float_t *ah = aShape->data.cube.halfExtents; switch(tb) { case PHYSICS_SHAPE_CUBE: return physicsTestAabbVsAabb( ac, ah, - bPos, bShape.data.cube.halfExtents, + bPos, bShape->data.cube.halfExtents, outNormal, outDepth ); case PHYSICS_SHAPE_SPHERE: { vec3 tmp; float_t d; if(!physicsTestSphereVsAabb( - bPos, bShape.data.sphere.radius, + bPos, bShape->data.sphere.radius, ac, ah, tmp, &d )) return false; glm_vec3_scale(tmp, -1.0f, outNormal); @@ -414,8 +414,8 @@ bool_t physicsTestDispatch( vec3 tmp; float_t d; if(!physicsTestCapsuleVsAabb( bPos, - bShape.data.capsule.radius, - bShape.data.capsule.halfHeight, + bShape->data.capsule.radius, + bShape->data.capsule.halfHeight, ac, ah, tmp, &d )) return false; glm_vec3_scale(tmp, -1.0f, outNormal); @@ -427,26 +427,26 @@ bool_t physicsTestDispatch( } case PHYSICS_SHAPE_SPHERE: { - const float_t sr = aShape.data.sphere.radius; + const float_t sr = aShape->data.sphere.radius; switch(tb) { case PHYSICS_SHAPE_CUBE: return physicsTestSphereVsAabb( aPos, sr, - bPos, bShape.data.cube.halfExtents, + bPos, bShape->data.cube.halfExtents, outNormal, outDepth ); case PHYSICS_SHAPE_SPHERE: return physicsTestSphereVsSphere( aPos, sr, - bPos, bShape.data.sphere.radius, + 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, + bShape->data.capsule.radius, + bShape->data.capsule.halfHeight, aPos, sr, tmp, &d )) return false; glm_vec3_scale(tmp, -1.0f, outNormal); @@ -458,27 +458,27 @@ bool_t physicsTestDispatch( } case PHYSICS_SHAPE_CAPSULE: { - const float_t cr = aShape.data.capsule.radius; - const float_t chh = aShape.data.capsule.halfHeight; + 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, + bPos, bShape->data.cube.halfExtents, outNormal, outDepth ); case PHYSICS_SHAPE_SPHERE: return physicsTestCapsuleVsSphere( aPos, cr, chh, - bPos, bShape.data.sphere.radius, + 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, + bShape->data.capsule.radius, + bShape->data.capsule.halfHeight, outNormal, outDepth ); default: return false; @@ -490,8 +490,8 @@ bool_t physicsTestDispatch( } bool_t physicsTestShapeVsShape( - const vec3 aPos, const physicsshape_t aShape, - const vec3 bPos, const physicsshape_t bShape, + const vec3 aPos, const physicsshape_t *aShape, + const vec3 bPos, const physicsshape_t *bShape, vec3 outNormal, float_t *outDepth ) { return physicsTestDispatch( diff --git a/src/dusk/physics/physicstest.h b/src/dusk/physics/physicstest.h index 3cca1772..cafaa6be 100644 --- a/src/dusk/physics/physicstest.h +++ b/src/dusk/physics/physicstest.h @@ -235,8 +235,8 @@ bool_t physicsTestCapsuleVsCapsule( * @return true if overlapping. */ bool_t physicsTestDispatch( - const vec3 aPos, const physicsshape_t aShape, - const vec3 bPos, const physicsshape_t bShape, + const vec3 aPos, const physicsshape_t *aShape, + const vec3 bPos, const physicsshape_t *bShape, vec3 outNormal, float_t *outDepth ); @@ -257,9 +257,9 @@ bool_t physicsTestDispatch( */ bool_t physicsTestShapeVsShape( const vec3 aPos, - const physicsshape_t aShape, + const physicsshape_t *aShape, const vec3 bPos, - const physicsshape_t bShape, + const physicsshape_t *bShape, vec3 outNormal, float_t *outDepth ); diff --git a/src/dusk/physics/physicsworld.c b/src/dusk/physics/physicsworld.c index 985bb326..9f181451 100644 --- a/src/dusk/physics/physicsworld.c +++ b/src/dusk/physics/physicsworld.c @@ -55,12 +55,28 @@ void physicsWorldStep( physBodies[i] = entityPhysicsGet(mgr, physEnts[i], physComps[i]); } + // Partition once into dynamic vs non-dynamic (static/kinematic) indices, + // so the phases below only ever iterate the subset they actually care + // about instead of re-scanning (and skipping past) every physics entity + // each time. + entityid_t dynamicIndices[ENTITY_COUNT_MAX]; + entityid_t otherIndices[ENTITY_COUNT_MAX]; + entityid_t dynamicCount = 0; + entityid_t otherCount = 0; + for(entityid_t i = 0; i < physCount; i++) { + if(physBodies[i]->type == PHYSICS_BODY_DYNAMIC) { + dynamicIndices[dynamicCount++] = i; + } else { + otherIndices[otherCount++] = 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++) { + for(entityid_t di = 0; di < dynamicCount; di++) { + entityid_t i = dynamicIndices[di]; if(!positions[i]) continue; entityphysics_t *phys = physBodies[i]; - if(phys->type != PHYSICS_BODY_DYNAMIC) continue; phys->onGround = false; @@ -75,22 +91,21 @@ void physicsWorldStep( } // Phase 2: dynamic vs static/kinematic. - for(entityid_t i = 0; i < physCount; i++) { + for(entityid_t di = 0; di < dynamicCount; di++) { + entityid_t i = dynamicIndices[di]; 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; + for(entityid_t oj = 0; oj < otherCount; oj++) { + entityid_t j = otherIndices[oj]; + if(!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, + pos, &phys->shape, + positions[j]->position, &otherPhys->shape, normal, &depth )) continue; @@ -110,23 +125,21 @@ void physicsWorldStep( } // Phase 3: dynamic vs dynamic. - for(entityid_t i = 0; i < physCount; i++) { + for(entityid_t di = 0; di < dynamicCount; di++) { + entityid_t i = dynamicIndices[di]; 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++) { + for(entityid_t dj = di + 1; dj < dynamicCount; dj++) { + entityid_t j = dynamicIndices[dj]; 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 + posA, &physA->shape, posB, &physB->shape, normal, &depth )) continue; posA[0] += normal[0] * depth * 0.5f; @@ -154,9 +167,9 @@ void physicsWorldStep( } // Rebuild transforms for all dynamic bodies once, after all phases. - for(entityid_t i = 0; i < physCount; i++) { + for(entityid_t di = 0; di < dynamicCount; di++) { + entityid_t i = dynamicIndices[di]; if(!positions[i]) continue; - if(physBodies[i]->type != PHYSICS_BODY_DYNAMIC) continue; entityPositionRebuild(mgr, positions[i]); } } diff --git a/src/dusk/system/system.c b/src/dusk/system/system.c index e8c07d68..3846d915 100644 --- a/src/dusk/system/system.c +++ b/src/dusk/system/system.c @@ -16,6 +16,10 @@ #error "systemInitPlatform is not defined" #endif +#ifndef systemGetCyclesPlatform + #error "systemGetCyclesPlatform is not defined" +#endif + errorret_t systemInit() { return systemInitPlatform(); } @@ -36,4 +40,8 @@ systemplatform_t systemGetPlatform(void) { #else return SYSTEM_PLATFORM_LINUX; #endif +} + +uint64_t systemGetCycles(void) { + return systemGetCyclesPlatform(); } \ No newline at end of file diff --git a/src/dusk/system/system.h b/src/dusk/system/system.h index d819f943..7dd69d7b 100644 --- a/src/dusk/system/system.h +++ b/src/dusk/system/system.h @@ -47,4 +47,15 @@ systemdialogtype_t systemGetActiveDialogType(); * * @return The current platform. */ -systemplatform_t systemGetPlatform(void); \ No newline at end of file +systemplatform_t systemGetPlatform(void); + +/** + * Returns the current CPU cycle counter value. This is intended for + * profiling only -- take the difference between two calls to measure + * elapsed cycles for a section of code. The counter's rate, width, and + * wrap-around behavior differ per platform, so the raw value itself is + * meaningless outside of such a comparison. + * + * @return The current CPU cycle count. + */ +uint64_t systemGetCycles(void); \ No newline at end of file diff --git a/src/duskdolphin/system/systemdolphin.c b/src/duskdolphin/system/systemdolphin.c index 19915b16..f9500fed 100644 --- a/src/duskdolphin/system/systemdolphin.c +++ b/src/duskdolphin/system/systemdolphin.c @@ -39,4 +39,8 @@ int32_t systemGetAspectRatioDolphin(void) { int32_t systemGetLanguageDolphin(void) { return CONF_GetLanguage(); +} + +uint64_t systemGetCyclesDolphin(void) { + return gettime(); } \ No newline at end of file diff --git a/src/duskdolphin/system/systemdolphin.h b/src/duskdolphin/system/systemdolphin.h index 41322641..bf9770e3 100644 --- a/src/duskdolphin/system/systemdolphin.h +++ b/src/duskdolphin/system/systemdolphin.h @@ -48,4 +48,12 @@ int32_t systemGetAspectRatioDolphin(void); int32_t systemGetLanguageDolphin(void); // There's actually a tonne more things Wii can return, this is it for now -// though. \ No newline at end of file +// though. + +/** + * Returns the current CPU cycle counter value, read directly from the + * PowerPC time base register. + * + * @return The current CPU cycle count. + */ +uint64_t systemGetCyclesDolphin(void); \ No newline at end of file diff --git a/src/duskdolphin/system/systemplatform.h b/src/duskdolphin/system/systemplatform.h index 483f7eef..41b8fdfa 100644 --- a/src/duskdolphin/system/systemplatform.h +++ b/src/duskdolphin/system/systemplatform.h @@ -9,4 +9,5 @@ #include "system/systemdolphin.h" #define systemInitPlatform systemInitDolphin -#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeDolphin \ No newline at end of file +#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeDolphin +#define systemGetCyclesPlatform systemGetCyclesDolphin \ No newline at end of file diff --git a/src/dusklinux/system/systemlinux.c b/src/dusklinux/system/systemlinux.c index cec2f252..49ba0789 100644 --- a/src/dusklinux/system/systemlinux.c +++ b/src/dusklinux/system/systemlinux.c @@ -7,10 +7,30 @@ #include "systemlinux.h" +#if defined(__x86_64__) || defined(__i386__) + #include +#else + #include +#endif + errorret_t systemInitLinux() { errorOk(); } systemdialogtype_t systemGetActiveDialogTypeLinux() { return SYSTEM_DIALOG_TYPE_NONE; +} + +uint64_t systemGetCyclesLinux(void) { + #if defined(__x86_64__) || defined(__i386__) + return __rdtsc(); + #elif defined(__aarch64__) + uint64_t cycles; + __asm__ volatile("mrs %0, cntvct_el0" : "=r" (cycles)); + return cycles; + #else + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (uint64_t)ts.tv_sec * 1000000000ULL + (uint64_t)ts.tv_nsec; + #endif } \ No newline at end of file diff --git a/src/dusklinux/system/systemlinux.h b/src/dusklinux/system/systemlinux.h index 6bb739be..6ec05f2a 100644 --- a/src/dusklinux/system/systemlinux.h +++ b/src/dusklinux/system/systemlinux.h @@ -15,7 +15,16 @@ errorret_t systemInitLinux(void); /** * Currently just returns SYSTEM_DIALOG_TYPE_NONE. - * + * * @return Currently open system dialog type. */ -systemdialogtype_t systemGetActiveDialogTypeLinux(); \ No newline at end of file +systemdialogtype_t systemGetActiveDialogTypeLinux(); + +/** + * Returns the current CPU cycle counter value. Uses the hardware + * timestamp counter on x86/x86_64 and the virtual counter register on + * ARM64, falling back to a monotonic nanosecond clock elsewhere. + * + * @return The current CPU cycle count. + */ +uint64_t systemGetCyclesLinux(void); \ No newline at end of file diff --git a/src/dusklinux/system/systemplatform.h b/src/dusklinux/system/systemplatform.h index 42e34dbe..145d4d1a 100644 --- a/src/dusklinux/system/systemplatform.h +++ b/src/dusklinux/system/systemplatform.h @@ -9,4 +9,5 @@ #include "system/systemlinux.h" #define systemInitPlatform systemInitLinux -#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeLinux \ No newline at end of file +#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeLinux +#define systemGetCyclesPlatform systemGetCyclesLinux \ No newline at end of file diff --git a/src/duskpsp/system/systemplatform.h b/src/duskpsp/system/systemplatform.h index 65a350c7..cef33847 100644 --- a/src/duskpsp/system/systemplatform.h +++ b/src/duskpsp/system/systemplatform.h @@ -9,4 +9,5 @@ #include "system/systempsp.h" #define systemInitPlatform systemInitPSP -#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypePSP \ No newline at end of file +#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypePSP +#define systemGetCyclesPlatform systemGetCyclesPSP \ No newline at end of file diff --git a/src/duskpsp/system/systempsp.c b/src/duskpsp/system/systempsp.c index b4825ef1..7b31725e 100644 --- a/src/duskpsp/system/systempsp.c +++ b/src/duskpsp/system/systempsp.c @@ -61,4 +61,10 @@ int_t systemPSPGetCrossButtonSetting() { return ( ret == 1 ? PSP_UTILITY_ACCEPT_CROSS : PSP_UTILITY_ACCEPT_CIRCLE ); +} + +uint64_t systemGetCyclesPSP(void) { + uint32_t count; + asm volatile("mfc0 %0, $9" : "=r" (count)); + return (uint64_t)count; } \ No newline at end of file diff --git a/src/duskpsp/system/systempsp.h b/src/duskpsp/system/systempsp.h index b2943148..ba53e791 100644 --- a/src/duskpsp/system/systempsp.h +++ b/src/duskpsp/system/systempsp.h @@ -37,4 +37,12 @@ int_t systemPSPGetLanguage(); * * @return PSP_UTILITY_ACCEPT_CROSS or PSP_UTILITY_ACCEPT_CIRCLE. */ -int_t systemPSPGetCrossButtonSetting(); \ No newline at end of file +int_t systemPSPGetCrossButtonSetting(); + +/** + * Returns the current CPU cycle counter value, read directly from the + * Allegrex COP0 Count register. + * + * @return The current CPU cycle count. + */ +uint64_t systemGetCyclesPSP(void); \ No newline at end of file diff --git a/test/physics/test_physicsshapemesh.c b/test/physics/test_physicsshapemesh.c index a136ae9e..f9747e9f 100644 --- a/test/physics/test_physicsshapemesh.c +++ b/test/physics/test_physicsshapemesh.c @@ -163,7 +163,7 @@ static void test_physicsShapeMeshCreateIntegratesWithDispatch(void **state) { // Landscape as B, matching physicsWorldStep's dynamic(A)-vs-static(B). vec3 normal; float_t depth; assert_true(physicsTestShapeVsShape( - sphereCenter, sphere, meshPos, landscape, normal, &depth + sphereCenter, &sphere, meshPos, &landscape, normal, &depth )); assert_float_equal(depth, 0.2f, 0.0001f); assert_float_equal(normal[1], 1.0f, 0.0001f); diff --git a/test/physics/test_physicstest.c b/test/physics/test_physicstest.c index 55e6b2e4..35abc780 100644 --- a/test/physics/test_physicstest.c +++ b/test/physics/test_physicstest.c @@ -91,12 +91,12 @@ static void test_dispatchSymmetryAndPlaneRouting(void **state) { vec3 normalAB; float_t depthAB; assert_true(physicsTestShapeVsShape( - aPos, cubeA, bPos, sphereB, normalAB, &depthAB + aPos, &cubeA, bPos, &sphereB, normalAB, &depthAB )); vec3 normalBA; float_t depthBA; assert_true(physicsTestShapeVsShape( - bPos, sphereB, aPos, cubeA, normalBA, &depthBA + bPos, &sphereB, aPos, &cubeA, normalBA, &depthBA )); // Swapping A/B should give the same depth and a negated normal. @@ -111,7 +111,7 @@ static void test_dispatchSymmetryAndPlaneRouting(void **state) { vec3 cubePos = { 0.0f, 0.4f, 0.0f }; vec3 normal; float_t depth; assert_true(physicsTestShapeVsShape( - cubePos, cubeA, bPos, plane, normal, &depth + cubePos, &cubeA, bPos, &plane, normal, &depth )); assert_float_equal(depth, 0.1f, 0.0001f); @@ -173,7 +173,7 @@ static void test_customShapeFlatLandscape(void **state) { // body as A, static/custom body as B). vec3 normal; float_t depth; assert_true(physicsTestShapeVsShape( - spherePos, sphere, landscapePos, landscape, normal, &depth + spherePos, &sphere, landscapePos, &landscape, normal, &depth )); assert_float_equal(depth, 0.2f, 0.0001f); assert_float_equal(normal[0], 0.0f, 0.0001f); @@ -183,7 +183,7 @@ static void test_customShapeFlatLandscape(void **state) { // Landscape as A: same depth, negated normal. vec3 normalSwapped; float_t depthSwapped; assert_true(physicsTestShapeVsShape( - landscapePos, landscape, spherePos, sphere, normalSwapped, &depthSwapped + landscapePos, &landscape, spherePos, &sphere, normalSwapped, &depthSwapped )); assert_float_equal(depthSwapped, depth, 0.0001f); assert_float_equal(normalSwapped[1], -normal[1], 0.0001f); @@ -191,7 +191,7 @@ static void test_customShapeFlatLandscape(void **state) { // Sphere far above the ground: no overlap. vec3 sphereFar = { 0.0f, 10.0f, 0.0f }; assert_false(physicsTestShapeVsShape( - sphereFar, sphere, landscapePos, landscape, normal, &depth + sphereFar, &sphere, landscapePos, &landscape, normal, &depth )); assert_int_equal(memoryGetAllocatedCount(), 0); @@ -208,7 +208,7 @@ static void test_customVsCustomAsserts(void **state) { vec3 normal; float_t depth; expect_assert_failure(physicsTestShapeVsShape( - pos, landscapeA, pos, landscapeB, normal, &depth + pos, &landscapeA, pos, &landscapeB, normal, &depth )); }