|
|
|
@@ -8,8 +8,8 @@
|
|
|
|
|
#include "physicsworld.h"
|
|
|
|
|
#include "assert/assert.h"
|
|
|
|
|
#include "util/memory.h"
|
|
|
|
|
// #include "entity/entity.h"
|
|
|
|
|
// #include "entity/component.h"
|
|
|
|
|
#include "entity/entity.h"
|
|
|
|
|
#include "entity/component.h"
|
|
|
|
|
#include "physicstest.h"
|
|
|
|
|
|
|
|
|
|
physicsworld_t PHYSICS_WORLD;
|
|
|
|
@@ -23,129 +23,129 @@ void physicsWorldInit() {
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void physicsWorldStep(const float_t dt) {
|
|
|
|
|
// assertTrue(dt > 0.0f, "Delta time must be positive");
|
|
|
|
|
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(
|
|
|
|
|
// COMPONENT_TYPE_PHYSICS, physEnts, physComps
|
|
|
|
|
// );
|
|
|
|
|
entityid_t physEnts[ENTITY_COUNT_MAX];
|
|
|
|
|
componentid_t physComps[ENTITY_COUNT_MAX];
|
|
|
|
|
entityid_t physCount = componentGetEntitiesWithComponent(
|
|
|
|
|
COMPONENT_TYPE_PHYSICS, physEnts, physComps
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
// /* 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++) {
|
|
|
|
|
// componentid_t posComp = entityGetComponent(
|
|
|
|
|
// physEnts[i], COMPONENT_TYPE_POSITION
|
|
|
|
|
// );
|
|
|
|
|
// positions[i] = (posComp != 0xFF)
|
|
|
|
|
// ? entityPositionGet(physEnts[i], posComp)
|
|
|
|
|
// : NULL;
|
|
|
|
|
// physBodies[i] = entityPhysicsGet(physEnts[i], physComps[i]);
|
|
|
|
|
// }
|
|
|
|
|
/* 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++) {
|
|
|
|
|
componentid_t posComp = entityGetComponent(
|
|
|
|
|
physEnts[i], COMPONENT_TYPE_POSITION
|
|
|
|
|
);
|
|
|
|
|
positions[i] = (posComp != 0xFF)
|
|
|
|
|
? entityPositionGet(physEnts[i], posComp)
|
|
|
|
|
: NULL;
|
|
|
|
|
physBodies[i] = entityPhysicsGet(physEnts[i], 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;
|
|
|
|
|
/* 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;
|
|
|
|
|
phys->onGround = false;
|
|
|
|
|
|
|
|
|
|
// phys->velocity[0] += PHYSICS_WORLD.gravity[0] * phys->gravityScale * dt;
|
|
|
|
|
// phys->velocity[1] += PHYSICS_WORLD.gravity[1] * phys->gravityScale * dt;
|
|
|
|
|
// phys->velocity[2] += PHYSICS_WORLD.gravity[2] * phys->gravityScale * dt;
|
|
|
|
|
phys->velocity[0] += PHYSICS_WORLD.gravity[0] * phys->gravityScale * dt;
|
|
|
|
|
phys->velocity[1] += PHYSICS_WORLD.gravity[1] * phys->gravityScale * dt;
|
|
|
|
|
phys->velocity[2] += PHYSICS_WORLD.gravity[2] * phys->gravityScale * dt;
|
|
|
|
|
|
|
|
|
|
// float_t *pos = positions[i]->position;
|
|
|
|
|
// pos[0] += phys->velocity[0] * dt;
|
|
|
|
|
// pos[1] += phys->velocity[1] * dt;
|
|
|
|
|
// pos[2] += phys->velocity[2] * 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;
|
|
|
|
|
/* 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;
|
|
|
|
|
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;
|
|
|
|
|
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;
|
|
|
|
|
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;
|
|
|
|
|
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];
|
|
|
|
|
// }
|
|
|
|
|
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;
|
|
|
|
|
// }
|
|
|
|
|
// }
|
|
|
|
|
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;
|
|
|
|
|
/* 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;
|
|
|
|
|
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;
|
|
|
|
|
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;
|
|
|
|
|
float_t *posB = positions[j]->position;
|
|
|
|
|
|
|
|
|
|
// vec3 normal; float_t depth;
|
|
|
|
|
// if(!physicsTestShapeVsShape(
|
|
|
|
|
// posA, physA->shape, posB, physB->shape, normal, &depth
|
|
|
|
|
// )) continue;
|
|
|
|
|
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;
|
|
|
|
|
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;
|
|
|
|
|
posB[0] -= normal[0] * depth * 0.5f;
|
|
|
|
|
posB[1] -= normal[1] * depth * 0.5f;
|
|
|
|
|
posB[2] -= normal[2] * depth * 0.5f;
|
|
|
|
|
|
|
|
|
|
// float_t v_rel = glm_vec3_dot(physA->velocity, normal)
|
|
|
|
|
// - glm_vec3_dot(physB->velocity, normal);
|
|
|
|
|
// if(v_rel < 0.0f) {
|
|
|
|
|
// physA->velocity[0] -= v_rel * normal[0];
|
|
|
|
|
// physA->velocity[1] -= v_rel * normal[1];
|
|
|
|
|
// physA->velocity[2] -= v_rel * normal[2];
|
|
|
|
|
// physB->velocity[0] += v_rel * normal[0];
|
|
|
|
|
// physB->velocity[1] += v_rel * normal[1];
|
|
|
|
|
// physB->velocity[2] += v_rel * normal[2];
|
|
|
|
|
// }
|
|
|
|
|
float_t v_rel = glm_vec3_dot(physA->velocity, normal)
|
|
|
|
|
- glm_vec3_dot(physB->velocity, normal);
|
|
|
|
|
if(v_rel < 0.0f) {
|
|
|
|
|
physA->velocity[0] -= v_rel * normal[0];
|
|
|
|
|
physA->velocity[1] -= v_rel * normal[1];
|
|
|
|
|
physA->velocity[2] -= v_rel * normal[2];
|
|
|
|
|
physB->velocity[0] += v_rel * normal[0];
|
|
|
|
|
physB->velocity[1] += v_rel * normal[1];
|
|
|
|
|
physB->velocity[2] += v_rel * normal[2];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// if( normal[1] > PHYSICS_GROUND_THRESHOLD) physA->onGround = true;
|
|
|
|
|
// if(-normal[1] > PHYSICS_GROUND_THRESHOLD) physB->onGround = true;
|
|
|
|
|
// }
|
|
|
|
|
// }
|
|
|
|
|
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(positions[i]);
|
|
|
|
|
// }
|
|
|
|
|
/* 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]);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|