From b47f5210e0bab8cca412450e44feec3860297730 Mon Sep 17 00:00:00 2001
From: Lynix <lynix680@gmail.com>
Date: Sat, 20 Jan 2018 19:36:21 +0100
Subject: [PATCH] Physics3D/PhysWorld3D: Add ForEachBodyInAABB method

---
 include/Nazara/Physics3D/PhysWorld3D.hpp |  4 ++++
 src/Nazara/Physics3D/PhysWorld3D.cpp     | 12 ++++++++++++
 2 files changed, 16 insertions(+)

diff --git a/include/Nazara/Physics3D/PhysWorld3D.hpp b/include/Nazara/Physics3D/PhysWorld3D.hpp
index 887982c98..428685b3f 100644
--- a/include/Nazara/Physics3D/PhysWorld3D.hpp
+++ b/include/Nazara/Physics3D/PhysWorld3D.hpp
@@ -9,6 +9,7 @@
 
 #include <Nazara/Prerequisites.hpp>
 #include <Nazara/Core/String.hpp>
+#include <Nazara/Math/Box.hpp>
 #include <Nazara/Math/Vector3.hpp>
 #include <Nazara/Physics3D/Config.hpp>
 #include <unordered_map>
@@ -25,6 +26,7 @@ namespace Nz
 	class NAZARA_PHYSICS3D_API PhysWorld3D
 	{
 		public:
+			using BodyIterator = std::function<bool(const RigidBody3D& body)>;
 			using AABBOverlapCallback = std::function<bool(const RigidBody3D& firstBody, const RigidBody3D& secondBody)>;
 			using CollisionCallback = std::function<bool(const RigidBody3D& firstBody, const RigidBody3D& secondBody)>;
 
@@ -35,6 +37,8 @@ namespace Nz
 
 			int CreateMaterial(Nz::String name = Nz::String());
 
+			void ForEachBodyInAABB(const Nz::Boxf& box, BodyIterator iterator);
+
 			Vector3f GetGravity() const;
 			NewtonWorld* GetHandle() const;
 			int GetMaterial(const Nz::String& name);
diff --git a/src/Nazara/Physics3D/PhysWorld3D.cpp b/src/Nazara/Physics3D/PhysWorld3D.cpp
index 4613b7f32..84264112b 100644
--- a/src/Nazara/Physics3D/PhysWorld3D.cpp
+++ b/src/Nazara/Physics3D/PhysWorld3D.cpp
@@ -36,6 +36,18 @@ namespace Nz
 		return materialId;
 	}
 
+	void PhysWorld3D::ForEachBodyInAABB(const Nz::Boxf& box, BodyIterator iterator)
+	{
+		auto NewtonCallback = [](const NewtonBody* const body, void* const userdata) -> int
+		{
+			BodyIterator& iterator = *static_cast<BodyIterator*>(userdata);
+			RigidBody3D* nzBody = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body));
+			return iterator(*nzBody);
+		};
+
+		NewtonWorldForEachBodyInAABBDo(m_world, box.GetMinimum(), box.GetMaximum(), NewtonCallback, &iterator);
+	}
+
 	Vector3f PhysWorld3D::GetGravity() const
 	{
 		return m_gravity;