diff --git a/pymunk/tests/test_batch.py b/pymunk/tests/test_batch.py index 66233aa9..2880caad 100644 --- a/pymunk/tests/test_batch.py +++ b/pymunk/tests/test_batch.py @@ -7,6 +7,35 @@ class UnitTestBatch(unittest.TestCase): + def test_set_pose_with_offset_center_of_gravity(self) -> None: + fields = pymunk.batch.BodyFields + for selected in [fields.POSITION | fields.ANGLE, fields.ALL]: + with self.subTest(fields=selected): + space = pymunk.Space() + bodies = [pymunk.Body(2, 3), pymunk.Body(4, 5)] + for i, body in enumerate(bodies): + body.center_of_gravity = (2 + i, 3 - i) + body.angle = 0.5 + i + body.position = (7 + i, 8 - i) + body.velocity = (1 + i, 2 - i) + space.add(body) + + data = pymunk.batch.Buffer() + pymunk.batch.get_space_bodies(space, selected, data) + saved = bytes(data.float_buf()) + for body in bodies: + body.angle += 1 + body.position = (-10, -20) + + pymunk.batch.set_space_bodies(space, selected, data) + + for i, body in enumerate(bodies): + self.assertAlmostEqual(body.position.x, 7 + i) + self.assertAlmostEqual(body.position.y, 8 - i) + self.assertEqual(body.angle, 0.5 + i) + self.assertEqual(body.velocity, (1 + i, 2 - i)) + self.assertEqual(bytes(data.float_buf()), saved) + def test_buffer(self) -> None: b = pymunk.batch.Buffer() arr = array.array("d", [1.2, 3.4]) diff --git a/pymunk_cffi/extensions.c b/pymunk_cffi/extensions.c index a8b8d379..77f82595 100644 --- a/pymunk_cffi/extensions.c +++ b/pymunk_cffi/extensions.c @@ -209,6 +209,7 @@ void pmSpaceBodyGetIteratorFuncBatched(cpBody *body, void *data) void pmSpaceBodySetIteratorFuncBatched(cpBody *body, void *data) { pmBatchedData *d = (pmBatchedData *)data; + cpVect position = cpvzero; if (d->fields & BODY_ID) { @@ -216,12 +217,17 @@ void pmSpaceBodySetIteratorFuncBatched(cpBody *body, void *data) } if (d->fields & POSITION) { - cpBodySetPosition(body, pmFloatArrayPopVect(d->floatArray)); + position = pmFloatArrayPopVect(d->floatArray); } if (d->fields & ANGLE) { cpBodySetAngle(body, pmFloatArrayPop(d->floatArray)); } + if (d->fields & POSITION) + { + // Changing angle moves the origin of bodies with an offset center of gravity. + cpBodySetPosition(body, position); + } if (d->fields & VELOCITY) { cpBodySetVelocity(body, pmFloatArrayPopVect(d->floatArray));