diff --git a/src/physics/physics.c b/src/physics/physics.c index 61775e85..65415f7c 100644 --- a/src/physics/physics.c +++ b/src/physics/physics.c @@ -2,6 +2,33 @@ #include "math/quat.h" #include +static void nearCallback(void* data, dGeomID shapeA, dGeomID shapeB) { + World* world = data; + + dBodyID bodyA = dGeomGetBody(shapeA); + dBodyID bodyB = dGeomGetBody(shapeB); + if (bodyA && bodyB && dAreConnectedExcluding(bodyA, bodyB, dJointTypeContact)) { + return; + } + + dContact contacts[8]; + + for (int i = 0; i < 8; i++) { + contacts[i].surface.mode = 0; + contacts[i].surface.mu = dInfinity; + contacts[i].surface.mu2 = 0; + } + + int contactCount; + + contactCount = dCollide(shapeA, shapeB, 8, &contacts[0].geom, sizeof(dContact)); + + for (int i = 0; i < contactCount; i++) { + dJointID joint = dJointCreateContact(world->id, world->contactGroup, &contacts[i]); + dJointAttach(joint, bodyA, bodyB); + } +} + void lovrPhysicsInit() { dInitODE(); @@ -21,6 +48,9 @@ World* lovrWorldCreate() { if (!world) return NULL; world->id = dWorldCreate(); + world->space = dHashSpaceCreate(0); + dHashSpaceSetLevels(world->space, -4, 8); + world->contactGroup = dJointGroupCreate(0); return world; } @@ -72,7 +102,9 @@ void lovrWorldSetSleepingAllowed(World* world, int allowed) { } void lovrWorldUpdate(World* world, float dt) { + dSpaceCollide(world->space, world, nearCallback); dWorldQuickStep(world->id, dt); + dJointGroupEmpty(world->contactGroup); } Body* lovrBodyCreate(World* world) { @@ -322,6 +354,17 @@ Body* lovrShapeGetBody(Shape* shape) { void lovrShapeSetBody(Shape* shape, Body* body) { shape->body = body; dGeomSetBody(shape->id, body ? body->id : 0); + + dSpaceID oldSpace = dGeomGetSpace(shape->id); + dSpaceID newSpace = body ? body->world->space : 0; + + if (oldSpace && oldSpace != newSpace) { + dSpaceRemove(oldSpace, shape->id); + } + + if (newSpace && newSpace != oldSpace) { + dSpaceAdd(newSpace, shape->id); + } } int lovrShapeIsEnabled(Shape* shape) { diff --git a/src/physics/physics.h b/src/physics/physics.h index 0cd9a777..6767d150 100644 --- a/src/physics/physics.h +++ b/src/physics/physics.h @@ -12,6 +12,8 @@ typedef enum { typedef struct { Ref ref; dWorldID id; + dSpaceID space; + dJointGroupID contactGroup; } World; typedef struct {