From 3aaadbd2851cd827a72e6c2f634772be5c0dc69a Mon Sep 17 00:00:00 2001
From: Victor Blomqvist <vb@viblo.se>
Date: Thu, 16 Nov 2023 22:07:44 +0100
Subject: [PATCH] Fix to support sensor shapes in batch api #238

---
 TODO.txt                   | 10 +++++++-
 pymunk/tests/test_batch.py | 38 +++++++++++++++++++++++++++++
 pymunk_cffi/extensions.c   | 50 ++++++++++++++++++--------------------
 3 files changed, 71 insertions(+), 27 deletions(-)

diff --git a/TODO.txt b/TODO.txt
index 8fa66e24..8ed3434e 100644
--- a/TODO.txt
+++ b/TODO.txt
@@ -38,9 +38,17 @@ v6.x
 v7+ (all potentially breaking changes)
 ---
 - Think about split between pymunk.util and pymunk modules
+
+
+Chipmunk improvements
+---------------------
 - Investigate the SPOOK solver, which could be much more stable than what is currently used. (but probably very difficult to switch to). https://github.com/viblo/pymunk/issues/216
 - Investigate "Affine Body Dynamics Fast, Stable & Intersection-free Simulation of Stiff Materials"
-
+- optimize step function:
+  - streamline build - benchmark
+  - https://www.microsoft.com/en-us/research/wp-content/uploads/2016/12/ccds.pdf
+- collision detection:
+  - r-tree https://www.sebastiansylvan.com/post/r-trees--adapting-out-of-core-techniques-to-modern-memory-architectures/
 
 Typing a existing project - learnings
 -------------------------------------
diff --git a/pymunk/tests/test_batch.py b/pymunk/tests/test_batch.py
index fa84ce64..6632adf2 100644
--- a/pymunk/tests/test_batch.py
+++ b/pymunk/tests/test_batch.py
@@ -147,3 +147,41 @@ def check_arb_data(arb: pymunk.Arbiter) -> None:
         b1.each_arbiter(check_arb_data)
         b2.each_arbiter(check_arb_data)
         b3.each_arbiter(check_arb_data)
+
+    def test_get_arbiter_sensor(self):
+        s = pymunk.Space()
+
+        b1 = pymunk.Body(1, 1)
+        s1 = pymunk.Circle(b1, 40)
+        s.add(b1, s1)
+
+        b2 = pymunk.Body(1, 1)
+        b2.position = 1,0
+        s2 = pymunk.Poly.create_box(b2)
+        s2.sensor = True
+        s.add(b2, s2)
+
+        s.step(0.1)
+
+        data = pymunk.batch.Buffer()
+        pymunk.batch.get_space_arbiters(
+            s,
+            pymunk.batch.ArbiterFields.BODY_A_ID 
+            | pymunk.batch.ArbiterFields.BODY_B_ID
+            | pymunk.batch.ArbiterFields.CONTACT_COUNT
+            | pymunk.batch.ArbiterFields.DISTANCE_1
+            | pymunk.batch.ArbiterFields.DISTANCE_2
+            | pymunk.batch.ArbiterFields.POINT_A_1
+            | pymunk.batch.ArbiterFields.POINT_A_2
+            | pymunk.batch.ArbiterFields.POINT_B_1
+            | pymunk.batch.ArbiterFields.POINT_B_2,
+            data,
+        )
+
+        
+        self.assertEqual(list(memoryview(data.int_buf()).cast("P")), [b1.id, b2.id, 0])
+
+        self.assertEqual(
+            list(memoryview(data.float_buf()).cast("d")),
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        )
diff --git a/pymunk_cffi/extensions.c b/pymunk_cffi/extensions.c
index 81ba1d75..28067144 100644
--- a/pymunk_cffi/extensions.c
+++ b/pymunk_cffi/extensions.c
@@ -63,29 +63,6 @@ struct pmBatchedData
     pmBatchableBodyFields fields;
 };
 
-// cpVectArray *
-// cpVectArrayNew(int size)
-// {
-//     cpVectArray *arr = (cpVectArray *)cpcalloc(1, sizeof(cpVectArray));
-
-//     arr->num = 0;
-//     arr->max = (size ? size : 4);
-//     arr->arr = (cpVect *)cpcalloc(arr->max, sizeof(cpVect));
-
-//     return arr;
-// }
-
-// void cpVectArrayFree(cpVectArray *arr)
-// {
-//     if (arr)
-//     {
-//         cpfree(arr->arr);
-//         arr->arr = NULL;
-
-//         cpfree(arr);
-//     }
-// }
-
 pmFloatArray *
 pmFloatArrayNew(int size)
 {
@@ -234,15 +211,36 @@ void pmSpaceArbiterIteratorFuncBatched(cpArbiter *arbiter, void *data)
     }
     if (d->fields & POINT_A_1)
     {
-        pmFloatArrayPushVect(d->floatArray, cpArbiterGetPointA(arbiter, 0));
+        if (cpArbiterGetCount(arbiter) > 0)
+        {
+            pmFloatArrayPushVect(d->floatArray, cpArbiterGetPointA(arbiter, 0));
+        }
+        else
+        {
+            pmFloatArrayPushVect(d->floatArray, cpv(0, 0));
+        }
     }
     if (d->fields & POINT_B_1)
     {
-        pmFloatArrayPushVect(d->floatArray, cpArbiterGetPointB(arbiter, 0));
+        if (cpArbiterGetCount(arbiter) > 0)
+        {
+            pmFloatArrayPushVect(d->floatArray, cpArbiterGetPointB(arbiter, 0));
+        }
+        else
+        {
+            pmFloatArrayPushVect(d->floatArray, cpv(0, 0));
+        }
     }
     if (d->fields & DISTANCE_1)
     {
-        pmFloatArrayPush(d->floatArray, cpArbiterGetDepth(arbiter, 0));
+        if (cpArbiterGetCount(arbiter) > 0)
+        {
+            pmFloatArrayPush(d->floatArray, cpArbiterGetDepth(arbiter, 0));
+        }
+        else
+        {
+            pmFloatArrayPush(d->floatArray, 0);
+        }
     }
     if (d->fields & POINT_A_2)
     {