diff --git a/src/api/l_physics_world.c b/src/api/l_physics_world.c index 02aa8f26..82bb6fad 100644 --- a/src/api/l_physics_world.c +++ b/src/api/l_physics_world.c @@ -6,24 +6,32 @@ #include #include -static bool raycastCallback(Collider* collider, float position[3], float normal[3], uint32_t shape, void* userdata) { - lua_State* L = userdata; - lua_pushvalue(L, -1); - luax_pushtype(L, Collider, collider); - lua_pushnumber(L, position[0]); - lua_pushnumber(L, position[1]); - lua_pushnumber(L, position[2]); - lua_pushnumber(L, normal[0]); - lua_pushnumber(L, normal[1]); - lua_pushnumber(L, normal[2]); - lua_pushinteger(L, shape + 1); - lua_call(L, 8, 1); - bool shouldStop = lua_type(L, -1) == LUA_TBOOLEAN && !lua_toboolean(L, -1); - lua_pop(L, 1); - return shouldStop; +static void luax_pushcastresult(lua_State* L, CastResult* hit) { + luax_pushtype(L, Collider, hit->collider); + lua_pushnumber(L, hit->position[0]); + lua_pushnumber(L, hit->position[1]); + lua_pushnumber(L, hit->position[2]); + lua_pushnumber(L, hit->fraction); + lua_pushinteger(L, hit->part); } -static bool queryCallback(Collider* collider, void* userdata) { +static float castCallback(void* userdata, CastResult* hit) { + lua_State* L = userdata; + lua_pushvalue(L, -1); + luax_pushcastresult(L, hit); + lua_call(L, 6, 1); + bool stop = lua_type(L, -1) == LUA_TBOOLEAN && !lua_toboolean(L, -1); + lua_pop(L, 1); + return stop ? 0.f : 1.f; +} + +static float castClosestCallback(void* userdata, CastResult* hit) { + CastResult* closest = userdata; + *closest = *hit; + return hit->fraction; +} + +static bool queryCallback(void* userdata, Collider* collider) { lua_State* L = userdata; lua_pushvalue(L, -1); luax_pushtype(L, Collider, collider); @@ -193,14 +201,25 @@ static int l_lovrWorldUpdate(lua_State* L) { static int l_lovrWorldRaycast(lua_State* L) { World* world = luax_checktype(L, 1, World); - float start[3], end[3]; int index = 2; + float start[3], end[3]; index = luax_readvec3(L, index, start, NULL); index = luax_readvec3(L, index, end, NULL); - luaL_checktype(L, index, LUA_TFUNCTION); - lua_settop(L, index); - lovrWorldRaycast(world, start, end, raycastCallback, L); - return 0; + if (lua_isnoneornil(L, index)) { + CastResult closest; + if (lovrWorldRaycast(world, start, end, castClosestCallback, &closest)) { + luax_pushcastresult(L, &closest); + return 6; + } else { + lua_pushnil(L); + return 1; + } + } else { + luaL_checktype(L, index, LUA_TFUNCTION); + lua_settop(L, index); + lovrWorldRaycast(world, start, end, castCallback, L); + return 0; + } } static int l_lovrWorldQueryBox(lua_State* L) { diff --git a/src/modules/physics/physics.h b/src/modules/physics/physics.h index e1ec2d7a..b2b5f40f 100644 --- a/src/modules/physics/physics.h +++ b/src/modules/physics/physics.h @@ -28,9 +28,6 @@ typedef Joint DistanceJoint; typedef Joint HingeJoint; typedef Joint SliderJoint; -typedef bool CastCallback(Collider* collider, float position[3], float normal[3], uint32_t child, void* userdata); -typedef bool QueryCallback(Collider* collider, void* userdata); - bool lovrPhysicsInit(void); void lovrPhysicsDestroy(void); @@ -45,6 +42,16 @@ typedef struct { uint32_t tagCount; } WorldInfo; +typedef struct { + Collider* collider; + float position[3]; + float fraction; + uint32_t part; +} CastResult; + +typedef float CastCallback(void* userdata, CastResult* hit); +typedef bool QueryCallback(void* userdata, Collider* collider); + World* lovrWorldCreate(WorldInfo* info); void lovrWorldDestroy(void* ref); void lovrWorldDestroyData(World* world); @@ -53,7 +60,7 @@ uint32_t lovrWorldGetJointCount(World* world); Collider* lovrWorldGetColliders(World* world, Collider* collider); Joint* lovrWorldGetJoints(World* world, Joint* joint); void lovrWorldUpdate(World* world, float dt); -void lovrWorldRaycast(World* world, float start[3], float end[3], CastCallback* callback, void* userdata); +bool lovrWorldRaycast(World* world, float start[3], float end[3], CastCallback* callback, void* userdata); bool lovrWorldQueryBox(World* world, float position[3], float size[3], QueryCallback* callback, void* userdata); bool lovrWorldQuerySphere(World* world, float position[3], float radius, QueryCallback* callback, void* userdata); void lovrWorldGetGravity(World* world, float gravity[3]); diff --git a/src/modules/physics/physics_jolt.c b/src/modules/physics/physics_jolt.c index e404cab1..858da74c 100644 --- a/src/modules/physics/physics_jolt.c +++ b/src/modules/physics/physics_jolt.c @@ -78,18 +78,6 @@ static uint32_t findTag(World* world, const char* name) { return UNTAGGED; } -typedef struct { - World* world; - QueryCallback* callback; - void* userdata; -} QueryInfo; - -static void queryCallback(void* arg, JPH_BodyID id) { - QueryInfo* query = arg; - Collider* collider = (Collider*) (uintptr_t) JPH_BodyInterface_GetUserData(query->world->bodies, id); - if (query->callback) query->callback(query->userdata, collider); -} - bool lovrPhysicsInit(void) { if (state.initialized) return false; JPH_Init(32 * 1024 * 1024); @@ -189,41 +177,52 @@ void lovrWorldUpdate(World* world, float dt) { JPH_PhysicsSystem_Step(world->system, dt, world->collisionSteps); } -void lovrWorldRaycast(World* world, float start[3], float end[3], CastCallback* callback, void* userdata) { +typedef struct { + World* world; + float origin[3]; + float direction[3]; + CastCallback* callback; + void* userdata; +} CastInfo; + +static float raycastCallback(void* arg, JPH_RayCastResult* result) { + CastInfo* cast = arg; + CastResult hit; + hit.collider = (Collider*) (uintptr_t) JPH_BodyInterface_GetUserData(cast->world->bodies, result->bodyID); + hit.position[0] = cast->origin[0] + cast->direction[0] * result->fraction; + hit.position[1] = cast->origin[1] + cast->direction[1] * result->fraction; + hit.position[2] = cast->origin[2] + cast->direction[2] * result->fraction; + hit.fraction = result->fraction; + hit.part = result->subShapeID2; + return cast->callback(cast->userdata, &hit); +} + +bool lovrWorldRaycast(World* world, float start[3], float end[3], CastCallback* callback, void* userdata) { const JPH_NarrowPhaseQuery* query = JPC_PhysicsSystem_GetNarrowPhaseQueryNoLock(world->system); - const JPH_RVec3 origin = { start[0], start[1], start[2] }; - const JPH_Vec3 direction = { end[0] - start[0], end[1] - start[1], end[2] - start[2] }; - JPH_AllHit_CastRayCollector* collector = JPH_AllHit_CastRayCollector_Create(); - JPH_NarrowPhaseQuery_CastRayAll(query, &origin, &direction, collector, NULL, NULL, NULL); - size_t count; - JPH_RayCastResult* hits = JPH_AllHit_CastRayCollector_GetHits(collector, &count); + CastInfo raycast = { + .world = world, + .origin = { start[0], start[1], start[2] }, + .direction = { end[0] - start[0], end[1] - start[1], end[2] - start[2] }, + .callback = callback, + .userdata = userdata + }; - for (size_t i = 0; i < count; i++) { - Collider* collider = (Collider*) (uintptr_t) JPH_BodyInterface_GetUserData(world->bodies, hits[i].bodyID); - uint32_t child = 0; + JPH_RVec3* origin = vec3_toJolt(raycast.origin); + JPH_Vec3* direction = vec3_toJolt(raycast.direction); + return JPH_NarrowPhaseQuery_CastRay2(query, origin, direction, raycastCallback, &raycast, NULL, NULL, NULL); +} - if (collider->shape->type == SHAPE_COMPOUND) { - JPH_SubShapeID id = hits[i].subShapeID2; - JPH_SubShapeID remainder; - child = JPH_CompoundShape_GetSubShapeIndexFromID((JPH_CompoundShape*) collider->shape, id, &remainder); - } +typedef struct { + World* world; + QueryCallback* callback; + void* userdata; +} QueryInfo; - JPH_RVec3 position = { - start[0] + hits[i].fraction * direction.x, - start[1] + hits[i].fraction * direction.y, - start[2] + hits[i].fraction * direction.z - }; - - JPH_Vec3 normal; - JPH_Body_GetWorldSpaceSurfaceNormal(collider->body, hits[i].subShapeID2, &position, &normal); - - if (callback(collider, &position.x, &normal.x, child, userdata)) { - break; - } - } - - JPH_AllHit_CastRayCollector_Destroy(collector); +static void queryCallback(void* arg, JPH_BodyID id) { + QueryInfo* query = arg; + Collider* collider = (Collider*) (uintptr_t) JPH_BodyInterface_GetUserData(query->world->bodies, id); + if (query->callback) query->callback(query->userdata, collider); } bool lovrWorldQueryBox(World* world, float position[3], float size[3], QueryCallback* callback, void* userdata) { diff --git a/src/modules/physics/physics_ode.c b/src/modules/physics/physics_ode.c index 2801359f..531b0ee5 100644 --- a/src/modules/physics/physics_ode.c +++ b/src/modules/physics/physics_ode.c @@ -92,10 +92,9 @@ typedef struct { } RaycastData; static void raycastCallback(void* d, dGeomID a, dGeomID b) { + if (a == b) return; RaycastData* data = d; if (data->shouldStop) return; - CastCallback* callback = data->callback; - void* userdata = data->userdata; Shape* shape = dGeomGetData(b); Collider* collider = dBodyGetData(dGeomGetBody(b)); @@ -106,8 +105,14 @@ static void raycastCallback(void* d, dGeomID a, dGeomID b) { dContact contact[MAX_CONTACTS]; int count = dCollide(a, b, MAX_CONTACTS, &contact->geom, sizeof(dContact)); for (int i = 0; i < count; i++) { - dContactGeom g = contact[i].geom; - data->shouldStop = callback(collider, g.pos, g.normal, 0, userdata); + CastResult hit; + hit.collider = collider; + vec3_init(hit.position, contact[i].geom.pos); + hit.fraction = 0.f; + hit.part = 0; + + data->shouldStop = data->callback(data->userdata, &hit); + if (data->shouldStop) break; } } @@ -276,16 +281,21 @@ void lovrWorldSetStepCount(World* world, int iterations) { dWorldSetQuickStepNumIterations(world->id, iterations); } -void lovrWorldRaycast(World* world, float start[3], float end[3], CastCallback* callback, void* userdata) { - RaycastData data = { .callback = callback, .userdata = userdata, .shouldStop = false }; - float dx = start[0] - end[0]; - float dy = start[1] - end[1]; - float dz = start[2] - end[2]; - float length = sqrtf(dx * dx + dy * dy + dz * dz); - dGeomID ray = dCreateRay(world->space, length); - dGeomRaySet(ray, start[0], start[1], start[2], end[0], end[1], end[2]); - dSpaceCollide2(ray, (dGeomID) world->space, &data, raycastCallback); - dGeomDestroy(ray); +bool lovrWorldRaycast(World* world, float start[3], float end[3], CastCallback* callback, void* userdata) { + if (callback) { + RaycastData data = { .callback = callback, .userdata = userdata, .shouldStop = false }; + float dx = start[0] - end[0]; + float dy = start[1] - end[1]; + float dz = start[2] - end[2]; + float length = sqrtf(dx * dx + dy * dy + dz * dz); + dGeomID ray = dCreateRay(world->space, length); + dGeomRaySet(ray, start[0], start[1], start[2], end[0], end[1], end[2]); + dSpaceCollide2(ray, (dGeomID) world->space, &data, raycastCallback); + dGeomDestroy(ray); + return true; + } + + return false; } bool lovrWorldQueryBox(World* world, float position[3], float size[3], QueryCallback* callback, void* userdata) {