Feature/physics fix

engine/src/toast/physics/broad_phase.hpp

@@ -69,6 +69,11 @@ class BroadPhase {
 	[[nodiscard]]
 	auto stats() const -> const BroadPhaseStats&;
 
+	[[nodiscard]]
+	auto snapshotTree() const -> AABBTree {
+		return m_tree;
+	}
+
 private:
 	struct ShapeLeaf {
 		TreeNodeID node = null_node;

engine/src/toast/physics/overlap.cpp

@@ -1,3 +1,4 @@
+#include "query_snapshot.hpp"
 #include "raycast.hpp"
 #include "toast/physics/shape_query.hpp"
 #include "toast/physics/simulator.hpp"
@@ -6,30 +7,17 @@ namespace physics {
 
 auto sphereOverlap(glm::vec3 position, float radius, std::optional<LayerFilter> layers) -> std::optional<SphereHit> {
 	ZoneScoped;
-	Simulator* simulator = Simulator::current();
-	if (simulator == nullptr || !std::isfinite(radius) || radius <= 0.0f || !std::isfinite(position.x) ||
-	    !std::isfinite(position.y) || !std::isfinite(position.z)) {
+	const auto snapshot = Simulator::querySnapshot();
+	if (!snapshot || !std::isfinite(radius) || radius <= 0.0f || !std::isfinite(position.x) || !std::isfinite(position.y) ||
+	    !std::isfinite(position.z)) {
 		return std::nullopt;
 	}
-
-	std::vector<QueryContact> contacts;
-	if (!simulator->overlapSphere(SphereShape {.radius = radius}, position, 0.0f, contacts, layers)) {
-		return std::nullopt;
-	}
-
-	const QueryContact& deepest = *std::ranges::max_element(contacts, {}, &QueryContact::penetration);
-	return SphereHit {
-	  .node = Simulator::nodeFor(deepest.body),
-	  .position = deepest.point,
-	  .normal = deepest.normal,
-	  .penetration = deepest.penetration,
-	};
+	return snapshotSphereOverlap(*snapshot, position, radius, layers);
 }
 
 auto overlapAABB(glm::vec3 min, glm::vec3 max) -> bool {
-	Simulator* simulator = Simulator::current();
-
-	return simulator->overlapAABB(AABB {.min = min, .max = max});
+	const auto snapshot = Simulator::querySnapshot();
+	return snapshot && snapshotAABBOverlap(*snapshot, AABB {.min = min, .max = max});
 }
 
 }

engine/src/toast/physics/query_snapshot.hpp

@@ -0,0 +1,76 @@
+#pragma once
+
+#include "aabb_tree.hpp"
+#include "body.hpp"
+#include "raycast.hpp"
+#include "shape.hpp"
+#include "voxel_shape_data.hpp"
+
+#include <memory>
+#include <toast/world/box.hpp>
+#include <utility>
+#include <vector>
+
+namespace physics {
+
+/// All data used by gameplay collision queries from one completed physics tick.
+struct QuerySnapshot {
+	AABBTree tree;
+	std::vector<BodySlot> bodies;
+	std::vector<ShapeSlot> shapes;
+	std::vector<VoxelShapeSlot> voxels;
+	std::vector<toast::Box<toast::Node>> collider_nodes;
+	std::vector<toast::Box<toast::Node>> body_nodes;
+	float fat_margin = 0.0f;
+	uint64_t tick = 0;
+
+	template<typename Visit>
+	void raycast(glm::vec3 origin, glm::vec3 inv_dir, float max_distance, Visit&& visit) const {
+		tree.raycast(origin, inv_dir, max_distance, std::forward<Visit>(visit));
+	}
+
+	[[nodiscard]]
+	auto tryGetShape(ShapeID id) const -> const Shape* {
+		if (id.slot >= shapes.size()) {
+			return nullptr;
+		}
+		const ShapeSlot& slot = shapes[id.slot];
+		return slot.occupied && slot.generation == id.generation ? &slot.shape : nullptr;
+	}
+
+	[[nodiscard]]
+	auto tryGetBody(BodyID id) const -> const Body* {
+		if (id.slot >= bodies.size()) {
+			return nullptr;
+		}
+		const BodySlot& slot = bodies[id.slot];
+		return slot.occupied && slot.generation == id.generation ? &slot.body : nullptr;
+	}
+
+	[[nodiscard]]
+	auto tryGetVoxelDataConst(VoxelDataID id) const -> const VoxelShapeData* {
+		if (id.slot >= voxels.size()) {
+			return nullptr;
+		}
+		const VoxelShapeSlot& slot = voxels[id.slot];
+		return slot.data && slot.generation == id.generation ? &*slot.data : nullptr;
+	}
+
+	[[nodiscard]]
+	auto colliderFor(ShapeID id) const -> toast::Box<toast::Node> {
+		return id.slot < collider_nodes.size() ? collider_nodes[id.slot] : toast::Box<toast::Node> {};
+	}
+
+	[[nodiscard]]
+	auto nodeFor(BodyID id) const -> toast::Box<toast::Node> {
+		return id.slot < body_nodes.size() ? body_nodes[id.slot] : toast::Box<toast::Node> {};
+	}
+};
+
+[[nodiscard]]
+auto snapshotSphereOverlap(const QuerySnapshot& snapshot, glm::vec3 position, float radius, std::optional<LayerFilter> layers)
+    -> std::optional<SphereHit>;
+[[nodiscard]]
+auto snapshotAABBOverlap(const QuerySnapshot& snapshot, const AABB& bounds) -> bool;
+
+}

engine/src/toast/physics/raycast.cpp

@@ -1,4 +1,5 @@
 #include "nodes/rigidbody.hpp"
+#include "query_snapshot.hpp"
 #include "toast/physics/broad_phase.hpp"
 #include "toast/physics/shape.hpp"
 #include "toast/physics/simulator.hpp"
@@ -162,19 +163,24 @@ namespace physics {
 
 template<typename OnContact>
 auto Simulator::castRay(const RayQuery& query, OnContact&& on_contact) const -> uint32_t {
+	return castRayUsing(*this, query, std::forward<OnContact>(on_contact));
+}
+
+template<typename Source, typename OnContact>
+auto Simulator::castRayUsing(const Source& source, const RayQuery& query, OnContact&& on_contact) -> uint32_t {
 	const glm::vec3 inverse_direction = 1.0f / query.direction;
 	float clip = query.max_distance;
 	uint32_t tested = 0;
 
-	m_broad_phase.raycast(query.origin, inverse_direction, clip, [&](ShapeID shape_id, float) -> float {
-		const Shape* shape = tryGetShape(shape_id);
+	const auto visit = [&](ShapeID shape_id, float) -> float {
+		const Shape* shape = source.tryGetShape(shape_id);
 		if (shape == nullptr || not shape->enabled || shape->owner == query.ignored) {
 			return clip;
 		}
 		if (query.voxels_only && shape->type != ShapeType::voxel) {
 			return clip;
 		}
-		const Body* body = tryGetBody(shape->owner);
+		const Body* body = source.tryGetBody(shape->owner);
 		if (body == nullptr || not body->enabled || (body->layers.included & query.layers) == 0) {
 			return clip;
 		}
@@ -211,7 +217,7 @@ auto Simulator::castRay(const RayQuery& query, OnContact&& on_contact) const ->
 				report_local(raycastCapsule(shape->capsule.radius, shape->capsule.height, local_origin, local_direction, clip));
 				break;
 			case ShapeType::voxel: {
-				const VoxelShapeData* data = tryGetVoxelDataConst(shape->voxel.data);
+				const VoxelShapeData* data = source.tryGetVoxelDataConst(shape->voxel.data);
 				if (data == nullptr || data->volume == nullptr) {
 					break;
 				}
@@ -232,7 +238,12 @@ auto Simulator::castRay(const RayQuery& query, OnContact&& on_contact) const ->
 			}
 		}
 		return clip;
-	});
+	};
+	if constexpr (std::is_same_v<Source, Simulator>) {
+		source.m_broad_phase.raycast(query.origin, inverse_direction, clip, visit);
+	} else {
+		source.raycast(query.origin, inverse_direction, clip, visit);
+	}
 
 	return tested;
 }
@@ -303,13 +314,14 @@ auto Simulator::raycast(glm::vec3 origin, glm::vec3 direction, float max_distanc
     -> std::optional<RayHit> {
 	ZoneScopedN("physics::Raycast");
 	const std::optional<RayQuery> query = makeRayQuery(origin, direction, max_distance, ignored, layers);
-	if (instance == nullptr || not query) {
+	const auto snapshot = querySnapshot();
+	if (not snapshot || not query) {
 		return std::nullopt;
 	}
 
 	// Only nearest contact matters
 	std::optional<RayContact> nearest;
-	const uint32_t tested = instance->castRay(*query, [&](const RayContact& contact) {
+	const uint32_t tested = castRayUsing(*snapshot, *query, [&](const RayContact& contact) {
 		if (not nearest || contact.distance < nearest->distance) {
 			nearest = contact;
 		}
@@ -320,7 +332,7 @@ auto Simulator::raycast(glm::vec3 origin, glm::vec3 direction, float max_distanc
 	if (not nearest) {
 		return std::nullopt;
 	}
-	return makeRayHit(*query, *nearest, colliderFor(nearest->body, nearest->shape));
+	return makeRayHit(*query, *nearest, snapshot->colliderFor(nearest->shape));
 }
 
 auto Simulator::linecast(glm::vec3 from, glm::vec3 to, int max_hits, BodyID ignored, uint32_t layers) -> std::vector<RayHit> {
@@ -335,7 +347,8 @@ auto Simulator::raymulticast(
 ) -> std::vector<RayHit> {
 	ZoneScopedN("physics::RayMulticast");
 	const std::optional<RayQuery> query = makeRayQuery(origin, direction, max_distance, ignored, layers);
-	if (instance == nullptr || not query || max_hits == 0) {
+	const auto snapshot = querySnapshot();
+	if (not snapshot || not query || max_hits == 0) {
 		return {};
 	}
 
@@ -347,7 +360,7 @@ auto Simulator::raymulticast(
 		contacts.reserve(limit + 1);
 	}
 
-	const uint32_t tested = instance->castRay(*query, [&](const RayContact& contact) {
+	const uint32_t tested = castRayUsing(*snapshot, *query, [&](const RayContact& contact) {
 		contacts.push_back(contact);
 		if (not limited) {
 			return query->max_distance;
@@ -370,7 +383,7 @@ auto Simulator::raymulticast(
 	for (const RayContact& contact : contacts) {
 		auto known = std::ranges::find(nodes, contact.shape, &std::pair<ShapeID, toast::Box<toast::Node>>::first);
 		if (known == nodes.end()) {
-			nodes.emplace_back(contact.shape, colliderFor(contact.body, contact.shape));
+			nodes.emplace_back(contact.shape, snapshot->colliderFor(contact.shape));
 			known = nodes.end() - 1;
 		}
 		hits.push_back(makeRayHit(*query, contact, known->second));

engine/src/toast/physics/raycast.hpp

@@ -58,6 +58,7 @@ struct SphereHit {
 	glm::vec3 position;
 	glm::vec3 normal;
 	float penetration;
+	ShapeType shape_type = ShapeType::sphere;
 };
 
 /// The optional layer filter limits which bodies can overlap the sphere.

engine/src/toast/physics/shape_query.cpp

@@ -1,6 +1,7 @@
 #include "broad_phase.hpp"
 #include "manifold.hpp"
 #include "physics_settings.hpp"
+#include "query_snapshot.hpp"
 #include "simulator.hpp"
 #include "voxel_query.hpp"
 
@@ -137,6 +138,127 @@ auto Simulator::current() -> Simulator* {
 	return instance;
 }
 
+auto snapshotSphereOverlap(const QuerySnapshot& snapshot, glm::vec3 position, float radius, std::optional<LayerFilter> layers)
+    -> std::optional<SphereHit> {
+	const Shape probe_shape = sphereProbe(SphereShape {.radius = radius});
+	const Body probe_body = probeBody(position, glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	const AABB bounds = worldShapeBounds(probe_body, probe_shape).expanded(snapshot.fat_margin);
+	std::optional<QueryContact> deepest;
+	std::vector<Manifold> manifolds;
+	for (ShapeID id : snapshot.tree.query(bounds)) {
+		const Shape* shape = snapshot.tryGetShape(id);
+		const Body* body = shape ? snapshot.tryGetBody(shape->owner) : nullptr;
+		if (!shape || !body || !shape->enabled || !body->enabled || (layers && !layersCollide(*layers, body->layers))) {
+			continue;
+		}
+		const BroadPhasePair pair {
+		  .a = {},
+        .b = {.body = shape->owner, .shape = id}
+		};
+		const CollisionElement probe {probe_shape, probe_body};
+		const CollisionElement other {*shape, *body};
+		manifolds.clear();
+		switch (shape->type) {
+			case ShapeType::sphere:
+				if (auto m = collideSpheres(pair, probe, other)) {
+					manifolds.push_back(*m);
+				}
+				break;
+			case ShapeType::box:
+				if (auto m = collideSphereBox(pair, probe, other)) {
+					manifolds.push_back(*m);
+				}
+				break;
+			case ShapeType::capsule:
+				if (auto m = collideSphereCapsule(pair, probe, other)) {
+					manifolds.push_back(*m);
+				}
+				break;
+			case ShapeType::voxel:
+				if (const auto* data = snapshot.tryGetVoxelDataConst(shape->voxel.data); data && data->volume) {
+					collideSphereVoxel(pair, probe, other, *data, manifolds);
+				}
+				break;
+		}
+		for (const Manifold& manifold : manifolds) {
+			for (const ContactPoint& point : std::span {manifold.contacts}.first(manifold.contact_count)) {
+				if (point.penetration > 0.0f && (!deepest || point.penetration > deepest->penetration)) {
+					deepest = QueryContact {
+					  .body = shape->owner,
+					  .shape = id,
+					  .normal = -manifold.normal,
+					  .point = point.position,
+					  .penetration = point.penetration
+					};
+				}
+			}
+		}
+	}
+	if (!deepest) {
+		return std::nullopt;
+	}
+	const Shape* shape = snapshot.tryGetShape(deepest->shape);
+	return SphereHit {
+	  .node = snapshot.nodeFor(deepest->body),
+	  .position = deepest->point,
+	  .normal = deepest->normal,
+	  .penetration = deepest->penetration,
+	  .shape_type = shape->type
+	};
+}
+
+auto snapshotAABBOverlap(const QuerySnapshot& snapshot, const AABB& bounds) -> bool {
+	const glm::vec3 size = bounds.max - bounds.min;
+	if (!std::isfinite(bounds.min.x) || !std::isfinite(bounds.min.y) || !std::isfinite(bounds.min.z) ||
+	    !std::isfinite(bounds.max.x) || !std::isfinite(bounds.max.y) || !std::isfinite(bounds.max.z) || size.x < 0.0f ||
+	    size.y < 0.0f || size.z < 0.0f) {
+		return false;
+	}
+	const Shape probe_shape = boxProbe(bounds);
+	const Body probe_body = probeBody((bounds.min + bounds.max) * 0.5f, glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	std::vector<Manifold> manifolds;
+	for (ShapeID id : snapshot.tree.query(bounds.expanded(snapshot.fat_margin))) {
+		const Shape* shape = snapshot.tryGetShape(id);
+		const Body* body = shape ? snapshot.tryGetBody(shape->owner) : nullptr;
+		if (!shape || !body || !shape->enabled || !body->enabled) {
+			continue;
+		}
+		const BroadPhasePair pair {
+		  .a = {},
+        .b = {.body = shape->owner, .shape = id}
+		};
+		const CollisionElement probe {probe_shape, probe_body};
+		const CollisionElement other {*shape, *body};
+		manifolds.clear();
+		switch (shape->type) {
+			case ShapeType::sphere:
+				if (auto m = collideSphereBox(pair, other, probe)) {
+					manifolds.push_back(*m);
+				}
+				break;
+			case ShapeType::box:
+				if (auto m = collideBoxes(pair, probe, other)) {
+					manifolds.push_back(*m);
+				}
+				break;
+			case ShapeType::capsule:
+				if (auto m = collideCapsuleBox(pair, other, probe)) {
+					manifolds.push_back(*m);
+				}
+				break;
+			case ShapeType::voxel:
+				if (const auto* data = snapshot.tryGetVoxelDataConst(shape->voxel.data); data && data->volume) {
+					collideBoxVoxel(pair, probe, other, *data, manifolds);
+				}
+				break;
+		}
+		if (std::ranges::any_of(manifolds, [](const Manifold& m) { return m.contact_count != 0; })) {
+			return true;
+		}
+	}
+	return false;
+}
+
 auto Simulator::queryCandidates(BodyID ignored, const AABB& bounds, std::optional<LayerFilter> layers) const
     -> std::vector<ShapeID> {
 	std::vector<ShapeID> result;