From a78cf626f2608ad69568f62c04afa49d4516fde5 Mon Sep 17 00:00:00 2001 From: Victor Blomqvist Date: Mon, 4 Sep 2023 22:37:15 +0200 Subject: [PATCH 1/2] poc of overrideable joints --- dump/joint.py | 45 ++++++++++ pymunk/_callbacks.py | 34 +++++++- pymunk/cffi/callbacks_cdef.h | 12 ++- pymunk/cffi/chipmunk_cdef.h | 158 +++++++++++++++++++++-------------- 4 files changed, 183 insertions(+), 66 deletions(-) create mode 100644 dump/joint.py diff --git a/dump/joint.py b/dump/joint.py new file mode 100644 index 00000000..1d464f3d --- /dev/null +++ b/dump/joint.py @@ -0,0 +1,45 @@ +from typing import Callable, Optional + +import pymunk + + +class OverrideableConstraint(pymunk.Constraint): + _pre_step_func: Callable[[pymunk.Constraint, float], None] + _apply_cached_impulse_func: Callable[[pymunk.Constraint, float], None] + _apply_impulse_func: Callable[[pymunk.Constraint, float], None] + _get_impulse_func: Callable[[pymunk.Constraint], float] + + def __init__(self, a: pymunk.Body, b: pymunk.Body) -> None: + _constraint = pymunk.ffi.new("cpConstraint *") + klass = pymunk.ffi.new("cpConstraintClass *") + klass.preStep = pymunk.cp.ext_cpConstraintPreStepImpl + klass.applyCachedImpulse = pymunk.cp.ext_cpConstraintApplyCachedImpulseImpl + klass.applyImpulse = pymunk.cp.ext_cpConstraintApplyImpulseImpl + klass.getImpulse = pymunk.cp.ext_cpConstraintGetImpulseImpl + pymunk.cp.cpConstraintInit(_constraint, klass, a._body, b._body) + + self._init(a, b, _constraint) + + def pre_step(self, dt): + print(f"pre_step {c} {dt}") + + def apply_cached_impulse(self, dt): + print(f"apply_cached_impulse {c} {dt}") + + def apply_impulse(self, dt): + print(f"apply_impulse {c} {dt}") + + def get_impulse(self): + print(f"get_impulse {c}") + return 0 + + +a, b = pymunk.Body(1, 2), pymunk.Body(1, 2) +c = OverrideableConstraint(a, b) + +space = pymunk.Space() + +space.add(a, b, c) + +space.step(1) +space.step(0.1) diff --git a/pymunk/_callbacks.py b/pymunk/_callbacks.py index 0cb92c5b..ed38c678 100644 --- a/pymunk/_callbacks.py +++ b/pymunk/_callbacks.py @@ -292,9 +292,41 @@ def ext_cpConstraintPostSolveFunc( constraint._post_solve_func(constraint, constraint.a.space) +# custom constraints + + +@ffi.def_extern() +def ext_cpConstraintPreStepImpl(cp_constraint: ffi.CData, dt: float) -> None: + print("prestep", cp_constraint, dt) + constraint = ffi.from_handle(lib.cpConstraintGetUserData(cp_constraint)) + constraint.pre_step(dt) + + +@ffi.def_extern() +def ext_cpConstraintApplyCachedImpulseImpl( + cp_constraint: ffi.CData, dt_coef: float +) -> None: + print("apply cached impulse", cp_constraint, dt_coef) + constraint = ffi.from_handle(lib.cpConstraintGetUserData(cp_constraint)) + constraint.apply_cached_impulse(dt_coef) + + +@ffi.def_extern() +def ext_cpConstraintApplyImpulseImpl(cp_constraint: ffi.CData, dt: float) -> None: + print("apply impulse", cp_constraint, dt) + constraint = ffi.from_handle(lib.cpConstraintGetUserData(cp_constraint)) + constraint.apply_impulse(dt) + + +@ffi.def_extern() +def ext_cpConstraintGetImpulseImpl(cp_constraint: ffi.CData) -> float: + print("get impulse", cp_constraint) + constraint = ffi.from_handle(lib.cpConstraintGetUserData(cp_constraint)) + return constraint.get_impulse() + # Pickle of Arbiters @ffi.def_extern() def ext_cpArbiterIteratorFunc(_arbiter, data): # type: ignore arbiters = ffi.from_handle(data) - arbiters.append(_arbiter) \ No newline at end of file + arbiters.append(_arbiter) diff --git a/pymunk/cffi/callbacks_cdef.h b/pymunk/cffi/callbacks_cdef.h index b4bce472..1719e031 100644 --- a/pymunk/cffi/callbacks_cdef.h +++ b/pymunk/cffi/callbacks_cdef.h @@ -1,10 +1,10 @@ -extern "Python" { +extern "Python" +{ // cpConstraint.h void ext_cpConstraintPreSolveFunc(cpConstraint *constraint, cpSpace *space); void ext_cpConstraintPostSolveFunc(cpConstraint *constraint, cpSpace *space); - // cpBody.h void ext_cpBodyVelocityFunc(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); void ext_cpBodyPositionFunc(cpBody *body, cpFloat dt); @@ -13,7 +13,6 @@ extern "Python" { void ext_cpBodyConstraintIteratorFunc(cpBody *body, cpConstraint *constraint, void *data); void ext_cpBodyArbiterIteratorFunc(cpBody *body, cpArbiter *arbiter, void *data); - // cpSpace.h cpBool ext_cpCollisionBeginFunc(cpArbiter *arb, cpSpace *space, cpDataPointer userData); @@ -41,4 +40,9 @@ extern "Python" { cpFloat ext_cpMarchSampleFunc(cpVect point, void *data); void ext_cpMarchSegmentFunc(cpVect v0, cpVect v1, void *data); -} \ No newline at end of file + // chipmunk_structs.h + void ext_cpConstraintPreStepImpl(cpConstraint *constraint, cpFloat dt); + void ext_cpConstraintApplyCachedImpulseImpl(cpConstraint *constraint, cpFloat dt_coef); + void ext_cpConstraintApplyImpulseImpl(cpConstraint *constraint, cpFloat dt); + cpFloat ext_cpConstraintGetImpulseImpl(cpConstraint *constraint); +} diff --git a/pymunk/cffi/chipmunk_cdef.h b/pymunk/cffi/chipmunk_cdef.h index efafb023..2f220770 100644 --- a/pymunk/cffi/chipmunk_cdef.h +++ b/pymunk/cffi/chipmunk_cdef.h @@ -53,67 +53,6 @@ typedef struct cpArbiter cpArbiter; typedef struct cpSpace cpSpace; -/////////////////////////////////////////// -// chipmunk_structs.h -/////////////////////////////////////////// - -enum cpArbiterState -{ - // Arbiter is active and its the first collision. - CP_ARBITER_STATE_FIRST_COLLISION, - // Arbiter is active and its not the first collision. - CP_ARBITER_STATE_NORMAL, - // Collision has been explicitly ignored. - // Either by returning false from a begin collision handler or calling cpArbiterIgnore(). - CP_ARBITER_STATE_IGNORE, - // Collison is no longer active. A space will cache an arbiter for up to cpSpace.collisionPersistence more steps. - CP_ARBITER_STATE_CACHED, - // Collison arbiter is invalid because one of the shapes was removed. - CP_ARBITER_STATE_INVALIDATED, -}; - -struct cpArbiterThread -{ - struct cpArbiter *next, *prev; -}; - -struct cpContact -{ - cpVect r1, r2; - - cpFloat nMass, tMass; - cpFloat bounce; // TODO: look for an alternate bounce solution. - - cpFloat jnAcc, jtAcc, jBias; - cpFloat bias; - - cpHashValue hash; -}; - -struct cpArbiter -{ - cpFloat e; - cpFloat u; - cpVect surface_vr; - - cpDataPointer data; - - const cpShape *a, *b; - cpBody *body_a, *body_b; - struct cpArbiterThread thread_a, thread_b; - - int count; - struct cpContact *contacts; - cpVect n; - - // Regular, wildcard A and wildcard B collision handlers. - cpCollisionHandler *handler, *handlerA, *handlerB; - cpBool swapped; - - cpTimestamp stamp; - enum cpArbiterState state; -}; - /////////////////////////////////////////// // cpVect.h /////////////////////////////////////////// @@ -1516,3 +1455,100 @@ typedef void (*cpHashSetIteratorFunc)(void *elt, void *data); void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data); cpArbiter *cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); + +void cpConstraintInit(cpConstraint *constraint, const struct cpConstraintClass *klass, cpBody *a, cpBody *b); + +/////////////////////////////////////////// +// chipmunk_structs.h +/////////////////////////////////////////// + +enum cpArbiterState +{ + // Arbiter is active and its the first collision. + CP_ARBITER_STATE_FIRST_COLLISION, + // Arbiter is active and its not the first collision. + CP_ARBITER_STATE_NORMAL, + // Collision has been explicitly ignored. + // Either by returning false from a begin collision handler or calling cpArbiterIgnore(). + CP_ARBITER_STATE_IGNORE, + // Collison is no longer active. A space will cache an arbiter for up to cpSpace.collisionPersistence more steps. + CP_ARBITER_STATE_CACHED, + // Collison arbiter is invalid because one of the shapes was removed. + CP_ARBITER_STATE_INVALIDATED, +}; + +struct cpArbiterThread +{ + struct cpArbiter *next, *prev; +}; + +struct cpContact +{ + cpVect r1, r2; + + cpFloat nMass, tMass; + cpFloat bounce; // TODO: look for an alternate bounce solution. + + cpFloat jnAcc, jtAcc, jBias; + cpFloat bias; + + cpHashValue hash; +}; + +struct cpArbiter +{ + cpFloat e; + cpFloat u; + cpVect surface_vr; + + cpDataPointer data; + + const cpShape *a, *b; + cpBody *body_a, *body_b; + struct cpArbiterThread thread_a, thread_b; + + int count; + struct cpContact *contacts; + cpVect n; + + // Regular, wildcard A and wildcard B collision handlers. + cpCollisionHandler *handler, *handlerA, *handlerB; + cpBool swapped; + + cpTimestamp stamp; + enum cpArbiterState state; +}; + +typedef void (*cpConstraintPreStepImpl)(cpConstraint *constraint, cpFloat dt); +typedef void (*cpConstraintApplyCachedImpulseImpl)(cpConstraint *constraint, cpFloat dt_coef); +typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint, cpFloat dt); +typedef cpFloat (*cpConstraintGetImpulseImpl)(cpConstraint *constraint); + +typedef struct cpConstraintClass +{ + cpConstraintPreStepImpl preStep; + cpConstraintApplyCachedImpulseImpl applyCachedImpulse; + cpConstraintApplyImpulseImpl applyImpulse; + cpConstraintGetImpulseImpl getImpulse; +} cpConstraintClass; + +struct cpConstraint +{ + const cpConstraintClass *klass; + + cpSpace *space; + + cpBody *a, *b; + cpConstraint *next_a, *next_b; + + cpFloat maxForce; + cpFloat errorBias; + cpFloat maxBias; + + cpBool collideBodies; + + cpConstraintPreSolveFunc preSolve; + cpConstraintPostSolveFunc postSolve; + + cpDataPointer userData; +}; \ No newline at end of file From f042691ff4badf1fff7524d3778a6912e3fe7677 Mon Sep 17 00:00:00 2001 From: Victor Blomqvist Date: Mon, 4 Sep 2023 22:41:13 +0200 Subject: [PATCH 2/2] restructure custom constraint code --- dump/joint.py | 36 +----------------------------------- pymunk/constraints.py | 34 +++++++++++++++++++++++++++++++++- 2 files changed, 34 insertions(+), 36 deletions(-) diff --git a/dump/joint.py b/dump/joint.py index 1d464f3d..a572bd32 100644 --- a/dump/joint.py +++ b/dump/joint.py @@ -1,41 +1,7 @@ -from typing import Callable, Optional - import pymunk - -class OverrideableConstraint(pymunk.Constraint): - _pre_step_func: Callable[[pymunk.Constraint, float], None] - _apply_cached_impulse_func: Callable[[pymunk.Constraint, float], None] - _apply_impulse_func: Callable[[pymunk.Constraint, float], None] - _get_impulse_func: Callable[[pymunk.Constraint], float] - - def __init__(self, a: pymunk.Body, b: pymunk.Body) -> None: - _constraint = pymunk.ffi.new("cpConstraint *") - klass = pymunk.ffi.new("cpConstraintClass *") - klass.preStep = pymunk.cp.ext_cpConstraintPreStepImpl - klass.applyCachedImpulse = pymunk.cp.ext_cpConstraintApplyCachedImpulseImpl - klass.applyImpulse = pymunk.cp.ext_cpConstraintApplyImpulseImpl - klass.getImpulse = pymunk.cp.ext_cpConstraintGetImpulseImpl - pymunk.cp.cpConstraintInit(_constraint, klass, a._body, b._body) - - self._init(a, b, _constraint) - - def pre_step(self, dt): - print(f"pre_step {c} {dt}") - - def apply_cached_impulse(self, dt): - print(f"apply_cached_impulse {c} {dt}") - - def apply_impulse(self, dt): - print(f"apply_impulse {c} {dt}") - - def get_impulse(self): - print(f"get_impulse {c}") - return 0 - - a, b = pymunk.Body(1, 2), pymunk.Body(1, 2) -c = OverrideableConstraint(a, b) +c = pymunk.CustomConstraint(a, b) space = pymunk.Space() diff --git a/pymunk/constraints.py b/pymunk/constraints.py index 65524bed..677202ac 100644 --- a/pymunk/constraints.py +++ b/pymunk/constraints.py @@ -66,6 +66,7 @@ "RatchetJoint", "GearJoint", "SimpleMotor", + "CustomConstraint", ] import logging @@ -450,7 +451,7 @@ def __init__( b: "Body", *args: Union[ Tuple[float, float], Tuple[Tuple[float, float], Tuple[float, float]] - ] + ], ) -> None: """a and b are the two bodies to connect, and pivot is the point in world coordinates of the pivot. @@ -861,3 +862,34 @@ def _set_rate(self, rate: float) -> None: rate = property( _get_rate, _set_rate, doc="""The desired relative angular velocity""" ) + + +class CustomConstraint(Constraint): + _pre_step_func: Callable[[Constraint, float], None] + _apply_cached_impulse_func: Callable[[Constraint, float], None] + _apply_impulse_func: Callable[[Constraint, float], None] + _get_impulse_func: Callable[[Constraint], float] + + def __init__(self, a: "Body", b: "Body") -> None: + _constraint = ffi.new("cpConstraint *") + klass = ffi.new("cpConstraintClass *") + klass.preStep = lib.ext_cpConstraintPreStepImpl + klass.applyCachedImpulse = lib.ext_cpConstraintApplyCachedImpulseImpl + klass.applyImpulse = lib.ext_cpConstraintApplyImpulseImpl + klass.getImpulse = lib.ext_cpConstraintGetImpulseImpl + lib.cpConstraintInit(_constraint, klass, a._body, b._body) + + self._init(a, b, _constraint) + + def pre_step(self, dt): + print(f"pre_step {self} {dt}") + + def apply_cached_impulse(self, dt): + print(f"apply_cached_impulse {self} {dt}") + + def apply_impulse(self, dt): + print(f"apply_impulse {self} {dt}") + + def get_impulse(self): + print(f"get_impulse {self}") + return 0