Physics position optimization

src/dusk/entity/component/display/entityposition.c

@@ -115,6 +115,15 @@ void entityPositionSetScale(
   entityPositionRebuild(pos);
 }
 
+entityposition_t *entityPositionGet(
+  const entityid_t entityId,
+  const componentid_t componentId
+) {
+  return componentGetData(
+    entityId, componentId, COMPONENT_TYPE_POSITION
+  );
+}
+
 void entityPositionRebuild(entityposition_t *pos) {
   glm_mat4_identity(pos->transform);
   glm_translate(pos->transform, pos->position);

src/dusk/entity/component/display/entityposition.h

@@ -135,6 +135,20 @@ void entityPositionSetScale(
   vec3 scale
 );
 
+/**
+ * Returns a direct pointer to the entity position component data.
+ * After modifying position, rotation, or scale directly, call
+ * entityPositionRebuild() to update the transform matrix.
+ *
+ * @param entityId     The entity ID.
+ * @param componentId  The component ID.
+ * @return Pointer to the entity position component data.
+ */
+entityposition_t *entityPositionGet(
+  const entityid_t entityId,
+  const componentid_t componentId
+);
+
 /**
  * Internal function to rebuild the transform matrix of the entity position
  * component based on the current position, rotation, and scale.

src/dusk/physics/physicsworld.c

@@ -31,13 +31,24 @@ void physicsWorldStep(const float_t dt) {
     COMPONENT_TYPE_PHYSICS, physEnts, physComps
   );
 
-  /* Phase 1: integrate dynamic bodies (gravity + velocity → position). */
+  /* Pre-fetch all position and physics pointers once. */
+  entityposition_t *positions[ENTITY_COUNT_MAX];
+  entityphysics_t  *physBodies[ENTITY_COUNT_MAX];
   for(entityid_t i = 0; i < physCount; i++) {
-    entityid_t id = physEnts[i];
+    componentid_t posComp = entityGetComponent(
-    componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
+      physEnts[i], COMPONENT_TYPE_POSITION
-    if(posComp == 0xFF) continue;
+    );
+    positions[i] = (posComp != 0xFF)
+      ? entityPositionGet(physEnts[i], posComp)
+      : NULL;
+    physBodies[i] = entityPhysicsGet(physEnts[i], physComps[i]);
+  }
 
-    entityphysics_t *phys = entityPhysicsGet(id, physComps[i]);
+  /* Phase 1: integrate dynamic bodies (gravity + velocity → position).
+   * Writes directly to pos->position — matrix rebuilt at 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;
@@ -46,49 +57,35 @@ void physicsWorldStep(const float_t dt) {
     phys->velocity[1] += PHYSICS_WORLD.gravity[1] * phys->gravityScale * dt;
     phys->velocity[2] += PHYSICS_WORLD.gravity[2] * phys->gravityScale * dt;
 
-    vec3 pos;
+    float_t *pos = positions[i]->position;
-    entityPositionGetPosition(id, posComp, pos);
     pos[0] += phys->velocity[0] * dt;
     pos[1] += phys->velocity[1] * dt;
     pos[2] += phys->velocity[2] * dt;
-    entityPositionSetPosition(id, posComp, pos);
   }
 
-  // Phase 2: Dynamic vs Static/Kinematic.
+  /* Phase 2: dynamic vs static/kinematic. */
   for(entityid_t i = 0; i < physCount; i++) {
-    entityid_t id = physEnts[i];
+    if(!positions[i]) continue;
-    componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
+    entityphysics_t *phys = physBodies[i];
-    if(posComp == 0xFF) continue;
-
-    entityphysics_t *phys = entityPhysicsGet(id, physComps[i]);
     if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
 
-    vec3 pos;
+    float_t *pos = positions[i]->position;
-    entityPositionGetPosition(id, posComp, pos);
 
     for(entityid_t j = 0; j < physCount; j++) {
-      if(i == j) continue;
+      if(i == j || !positions[j]) continue;
-      entityid_t otherId = physEnts[j];
+      entityphysics_t *otherPhys = physBodies[j];
-      componentid_t otherPosComp = entityGetComponent(
-        otherId, COMPONENT_TYPE_POSITION
-      );
-      if(otherPosComp == 0xFF) continue;
-
-      entityphysics_t *otherPhys = entityPhysicsGet(otherId, physComps[j]);
       if(otherPhys->type == PHYSICS_BODY_DYNAMIC) continue;
 
-      vec3 otherPos;
-      entityPositionGetPosition(otherId, otherPosComp, otherPos);
-
       vec3 normal; float_t depth;
       if(!physicsTestShapeVsShape(
-        pos, phys->shape, otherPos, otherPhys->shape, normal, &depth
+        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;
-      entityPositionSetPosition(id, posComp, pos);
 
       float_t vn = glm_vec3_dot(phys->velocity, normal);
       if(vn < 0.0f) {
@@ -101,28 +98,20 @@ void physicsWorldStep(const float_t dt) {
     }
   }
 
-  // Phase 3: Dynamic vs Dynamic
+  /* Phase 3: dynamic vs dynamic. */
   for(entityid_t i = 0; i < physCount; i++) {
-    entityid_t idA = physEnts[i];
+    if(!positions[i]) continue;
-    componentid_t posCompA = entityGetComponent(idA, COMPONENT_TYPE_POSITION);
+    entityphysics_t *physA = physBodies[i];
-    if(posCompA == 0xFF) continue;
-
-    entityphysics_t *physA = entityPhysicsGet(idA, physComps[i]);
     if(physA->type != PHYSICS_BODY_DYNAMIC) continue;
 
-    vec3 posA;
+    float_t *posA = positions[i]->position;
-    entityPositionGetPosition(idA, posCompA, posA);
 
     for(entityid_t j = i + 1; j < physCount; j++) {
-      entityid_t idB = physEnts[j];
+      if(!positions[j]) continue;
-      componentid_t posCompB = entityGetComponent(idB, COMPONENT_TYPE_POSITION);
+      entityphysics_t *physB = physBodies[j];
-      if(posCompB == 0xFF) continue;
-
-      entityphysics_t *physB = entityPhysicsGet(idB, physComps[j]);
       if(physB->type != PHYSICS_BODY_DYNAMIC) continue;
 
-      vec3 posB;
+      float_t *posB = positions[j]->position;
-      entityPositionGetPosition(idB, posCompB, posB);
 
       vec3 normal; float_t depth;
       if(!physicsTestShapeVsShape(
@@ -132,12 +121,10 @@ void physicsWorldStep(const float_t dt) {
       posA[0] += normal[0] * depth * 0.5f;
       posA[1] += normal[1] * depth * 0.5f;
       posA[2] += normal[2] * depth * 0.5f;
-      entityPositionSetPosition(idA, posCompA, posA);
 
       posB[0] -= normal[0] * depth * 0.5f;
       posB[1] -= normal[1] * depth * 0.5f;
       posB[2] -= normal[2] * depth * 0.5f;
-      entityPositionSetPosition(idB, posCompB, posB);
 
       float_t v_rel = glm_vec3_dot(physA->velocity, normal)
                     - glm_vec3_dot(physB->velocity, normal);
@@ -154,4 +141,11 @@ void physicsWorldStep(const float_t dt) {
       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(positions[i]);
+  }
 }