From 3879da9f26a0b32af9c6e7d07bdbd581f1f42e7b Mon Sep 17 00:00:00 2001 From: spartanj Date: Wed, 12 Jan 2011 04:34:54 -0300 Subject: [PATCH] Started working on the Physics Engine, a Chipmunk OOP wrapper. --- Makefile | 30 +- src/ee.h | 20 + src/helper/chipmunk/chipmunk.c | 150 ++++ src/helper/chipmunk/chipmunk.h | 171 ++++ src/helper/chipmunk/chipmunk_ffi.h | 59 ++ src/helper/chipmunk/chipmunk_private.h | 150 ++++ src/helper/chipmunk/chipmunk_types.h | 151 ++++ src/helper/chipmunk/chipmunk_unsafe.h | 52 ++ .../chipmunk/constraints/cpConstraint.c | 58 ++ .../chipmunk/constraints/cpConstraint.h | 102 +++ .../constraints/cpDampedRotarySpring.c | 105 +++ .../constraints/cpDampedRotarySpring.h | 46 + .../chipmunk/constraints/cpDampedSpring.c | 115 +++ .../chipmunk/constraints/cpDampedSpring.h | 53 ++ src/helper/chipmunk/constraints/cpGearJoint.c | 113 +++ src/helper/chipmunk/constraints/cpGearJoint.h | 41 + .../chipmunk/constraints/cpGrooveJoint.c | 161 ++++ .../chipmunk/constraints/cpGrooveJoint.h | 48 + src/helper/chipmunk/constraints/cpPinJoint.c | 116 +++ src/helper/chipmunk/constraints/cpPinJoint.h | 43 + .../chipmunk/constraints/cpPivotJoint.c | 114 +++ .../chipmunk/constraints/cpPivotJoint.h | 42 + .../chipmunk/constraints/cpRatchetJoint.c | 126 +++ .../chipmunk/constraints/cpRatchetJoint.h | 40 + .../chipmunk/constraints/cpRotaryLimitJoint.c | 120 +++ .../chipmunk/constraints/cpRotaryLimitJoint.h | 39 + .../chipmunk/constraints/cpSimpleMotor.c | 97 ++ .../chipmunk/constraints/cpSimpleMotor.h | 37 + .../chipmunk/constraints/cpSlideJoint.c | 129 +++ .../chipmunk/constraints/cpSlideJoint.h | 44 + src/helper/chipmunk/constraints/util.h | 130 +++ src/helper/chipmunk/cpArbiter.c | 256 ++++++ src/helper/chipmunk/cpArbiter.h | 181 ++++ src/helper/chipmunk/cpArray.c | 140 +++ src/helper/chipmunk/cpBB.c | 47 + src/helper/chipmunk/cpBB.h | 102 +++ src/helper/chipmunk/cpBBTree.c | 847 ++++++++++++++++++ src/helper/chipmunk/cpBody.c | 166 ++++ src/helper/chipmunk/cpBody.h | 212 +++++ src/helper/chipmunk/cpCollision.c | 411 +++++++++ src/helper/chipmunk/cpHashSet.c | 265 ++++++ src/helper/chipmunk/cpPolyShape.c | 235 +++++ src/helper/chipmunk/cpPolyShape.h | 56 ++ src/helper/chipmunk/cpShape.c | 393 ++++++++ src/helper/chipmunk/cpShape.h | 174 ++++ src/helper/chipmunk/cpSpace.c | 522 +++++++++++ src/helper/chipmunk/cpSpace.h | 181 ++++ src/helper/chipmunk/cpSpaceComponent.c | 290 ++++++ src/helper/chipmunk/cpSpaceHash.c | 646 +++++++++++++ src/helper/chipmunk/cpSpaceQuery.c | 246 +++++ src/helper/chipmunk/cpSpaceStep.c | 373 ++++++++ src/helper/chipmunk/cpSpatialIndex.c | 50 ++ src/helper/chipmunk/cpSpatialIndex.h | 159 ++++ src/helper/chipmunk/cpVect.c | 71 ++ src/helper/chipmunk/cpVect.h | 207 +++++ src/helper/chipmunk/prime.h | 68 ++ src/physics/base.hpp | 24 + src/physics/cbody.cpp | 170 ++++ src/physics/cbody.hpp | 97 ++ src/physics/constraints/cconstraint.cpp | 59 ++ src/physics/constraints/cconstraint.hpp | 44 + .../constraints/cdampedrotaryspring.cpp | 38 + .../constraints/cdampedrotaryspring.hpp | 29 + src/physics/constraints/cdampedspring.cpp | 86 ++ src/physics/constraints/cdampedspring.hpp | 37 + src/physics/constraints/cgearjoint.cpp | 30 + src/physics/constraints/cgearjoint.hpp | 25 + src/physics/constraints/cgroovejoint.cpp | 54 ++ src/physics/constraints/cgroovejoint.hpp | 29 + src/physics/constraints/cpinjoint.cpp | 54 ++ src/physics/constraints/cpinjoint.hpp | 29 + src/physics/constraints/cpivotjoint.cpp | 46 + src/physics/constraints/cpivotjoint.hpp | 27 + src/physics/constraints/cratchetjoint.cpp | 38 + src/physics/constraints/cratchetjoint.hpp | 29 + src/physics/constraints/crotarylimitjoint.cpp | 30 + src/physics/constraints/crotarylimitjoint.hpp | 25 + src/physics/constraints/csimplemotor.cpp | 22 + src/physics/constraints/csimplemotor.hpp | 21 + src/physics/constraints/cslidejoint.cpp | 62 ++ src/physics/constraints/cslidejoint.hpp | 33 + src/physics/cphysicsmanager.cpp | 48 + src/physics/cphysicsmanager.hpp | 57 ++ src/physics/cshape.cpp | 143 +++ src/physics/cshape.hpp | 86 ++ src/physics/cshapecircle.cpp | 57 ++ src/physics/cshapecircle.hpp | 25 + src/physics/cshapepoly.cpp | 52 ++ src/physics/cshapepoly.hpp | 27 + src/physics/cshapesegment.cpp | 91 ++ src/physics/cshapesegment.hpp | 35 + src/physics/cspace.cpp | 238 +++++ src/physics/cspace.hpp | 82 ++ src/physics/physicshelper.hpp | 132 +++ src/test/ee.cpp | 148 ++- src/ui/cuimanager.hpp | 1 + src/ui/cuiwindow.cpp | 14 +- src/ui/cuiwindow.hpp | 2 + src/utils/vector2.hpp | 1 + src/utils/vector3.hpp | 1 + 100 files changed, 11363 insertions(+), 15 deletions(-) create mode 100644 src/helper/chipmunk/chipmunk.c create mode 100644 src/helper/chipmunk/chipmunk.h create mode 100644 src/helper/chipmunk/chipmunk_ffi.h create mode 100644 src/helper/chipmunk/chipmunk_private.h create mode 100644 src/helper/chipmunk/chipmunk_types.h create mode 100644 src/helper/chipmunk/chipmunk_unsafe.h create mode 100644 src/helper/chipmunk/constraints/cpConstraint.c create mode 100644 src/helper/chipmunk/constraints/cpConstraint.h create mode 100644 src/helper/chipmunk/constraints/cpDampedRotarySpring.c create mode 100644 src/helper/chipmunk/constraints/cpDampedRotarySpring.h create mode 100644 src/helper/chipmunk/constraints/cpDampedSpring.c create mode 100644 src/helper/chipmunk/constraints/cpDampedSpring.h create mode 100644 src/helper/chipmunk/constraints/cpGearJoint.c create mode 100644 src/helper/chipmunk/constraints/cpGearJoint.h create mode 100644 src/helper/chipmunk/constraints/cpGrooveJoint.c create mode 100644 src/helper/chipmunk/constraints/cpGrooveJoint.h create mode 100644 src/helper/chipmunk/constraints/cpPinJoint.c create mode 100644 src/helper/chipmunk/constraints/cpPinJoint.h create mode 100644 src/helper/chipmunk/constraints/cpPivotJoint.c create mode 100644 src/helper/chipmunk/constraints/cpPivotJoint.h create mode 100644 src/helper/chipmunk/constraints/cpRatchetJoint.c create mode 100644 src/helper/chipmunk/constraints/cpRatchetJoint.h create mode 100644 src/helper/chipmunk/constraints/cpRotaryLimitJoint.c create mode 100644 src/helper/chipmunk/constraints/cpRotaryLimitJoint.h create mode 100644 src/helper/chipmunk/constraints/cpSimpleMotor.c create mode 100644 src/helper/chipmunk/constraints/cpSimpleMotor.h create mode 100644 src/helper/chipmunk/constraints/cpSlideJoint.c create mode 100644 src/helper/chipmunk/constraints/cpSlideJoint.h create mode 100644 src/helper/chipmunk/constraints/util.h create mode 100644 src/helper/chipmunk/cpArbiter.c create mode 100644 src/helper/chipmunk/cpArbiter.h create mode 100644 src/helper/chipmunk/cpArray.c create mode 100644 src/helper/chipmunk/cpBB.c create mode 100644 src/helper/chipmunk/cpBB.h create mode 100644 src/helper/chipmunk/cpBBTree.c create mode 100644 src/helper/chipmunk/cpBody.c create mode 100644 src/helper/chipmunk/cpBody.h create mode 100644 src/helper/chipmunk/cpCollision.c create mode 100644 src/helper/chipmunk/cpHashSet.c create mode 100644 src/helper/chipmunk/cpPolyShape.c create mode 100644 src/helper/chipmunk/cpPolyShape.h create mode 100644 src/helper/chipmunk/cpShape.c create mode 100644 src/helper/chipmunk/cpShape.h create mode 100644 src/helper/chipmunk/cpSpace.c create mode 100644 src/helper/chipmunk/cpSpace.h create mode 100644 src/helper/chipmunk/cpSpaceComponent.c create mode 100644 src/helper/chipmunk/cpSpaceHash.c create mode 100644 src/helper/chipmunk/cpSpaceQuery.c create mode 100644 src/helper/chipmunk/cpSpaceStep.c create mode 100644 src/helper/chipmunk/cpSpatialIndex.c create mode 100644 src/helper/chipmunk/cpSpatialIndex.h create mode 100644 src/helper/chipmunk/cpVect.c create mode 100644 src/helper/chipmunk/cpVect.h create mode 100644 src/helper/chipmunk/prime.h create mode 100644 src/physics/base.hpp create mode 100644 src/physics/cbody.cpp create mode 100644 src/physics/cbody.hpp create mode 100644 src/physics/constraints/cconstraint.cpp create mode 100644 src/physics/constraints/cconstraint.hpp create mode 100644 src/physics/constraints/cdampedrotaryspring.cpp create mode 100644 src/physics/constraints/cdampedrotaryspring.hpp create mode 100644 src/physics/constraints/cdampedspring.cpp create mode 100644 src/physics/constraints/cdampedspring.hpp create mode 100644 src/physics/constraints/cgearjoint.cpp create mode 100644 src/physics/constraints/cgearjoint.hpp create mode 100644 src/physics/constraints/cgroovejoint.cpp create mode 100644 src/physics/constraints/cgroovejoint.hpp create mode 100644 src/physics/constraints/cpinjoint.cpp create mode 100644 src/physics/constraints/cpinjoint.hpp create mode 100644 src/physics/constraints/cpivotjoint.cpp create mode 100644 src/physics/constraints/cpivotjoint.hpp create mode 100644 src/physics/constraints/cratchetjoint.cpp create mode 100644 src/physics/constraints/cratchetjoint.hpp create mode 100644 src/physics/constraints/crotarylimitjoint.cpp create mode 100644 src/physics/constraints/crotarylimitjoint.hpp create mode 100644 src/physics/constraints/csimplemotor.cpp create mode 100644 src/physics/constraints/csimplemotor.hpp create mode 100644 src/physics/constraints/cslidejoint.cpp create mode 100644 src/physics/constraints/cslidejoint.hpp create mode 100644 src/physics/cphysicsmanager.cpp create mode 100644 src/physics/cphysicsmanager.hpp create mode 100644 src/physics/cshape.cpp create mode 100644 src/physics/cshape.hpp create mode 100644 src/physics/cshapecircle.cpp create mode 100644 src/physics/cshapecircle.hpp create mode 100644 src/physics/cshapepoly.cpp create mode 100644 src/physics/cshapepoly.hpp create mode 100644 src/physics/cshapesegment.cpp create mode 100644 src/physics/cshapesegment.hpp create mode 100644 src/physics/cspace.cpp create mode 100644 src/physics/cspace.hpp create mode 100644 src/physics/physicshelper.hpp diff --git a/Makefile b/Makefile index 5dc252436..3fcb85f8b 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,7 @@ ifeq ($(DEBUGBUILD), yes) DEBUGFLAGS = -g -DDEBUG -DEE_DEBUG -DEE_MEMORY_MANAGER RELEASETYPE = debug else - DEBUGFLAGS = -fno-strict-aliasing -O3 -s -DNDEBUG + DEBUGFLAGS = -fno-strict-aliasing -O3 -s -DNDEBUG -ffast-math RELEASETYPE = release endif @@ -32,7 +32,7 @@ export CC = gcc export CPP = g++ endif -export CFLAGS = -Wall $(DEBUGFLAGS) $(BUILDFLAGS) +export CFLAGS = -Wall -Wno-unknown-pragmas $(DEBUGFLAGS) $(BUILDFLAGS) export CFLAGSEXT = $(DEBUGFLAGS) $(BUILDFLAGS) export LDFLAGS = $(LINKFLAGS) export LIBPATH = ./ @@ -61,6 +61,8 @@ OTHERINC = -I/usr/include/freetype2 endif endif +HELPERSINC = -I./src/helper/chipmunk + EXE = eetest-$(RELEASETYPE) EXEIV = eeiv-$(RELEASETYPE) EXEFLUID = eefluid-$(RELEASETYPE) @@ -70,6 +72,7 @@ SRCSOIL = $(wildcard ./src/helper/SOIL/*.c) SRCSTBVORBIS = $(wildcard ./src/helper/stb_vorbis/*.c) SRCZLIB = $(wildcard ./src/helper/zlib/*.c) SRCLIBZIP = $(wildcard ./src/helper/libzip/*.c) +SRCCHIPMUNK = $(wildcard ./src/helper/chipmunk/*.c) $(wildcard ./src/helper/chipmunk/constraints/*.c) SRCHAIKUTTF = $(wildcard ./src/helper/haikuttf/*.cpp) SRCBASE = $(wildcard ./src/base/*.cpp) @@ -81,13 +84,14 @@ SRCSYSTEM = $(wildcard ./src/system/*.cpp) SRCUI = $(wildcard ./src/ui/*.cpp) SRCUTILS = $(wildcard ./src/utils/*.cpp) SRCWINDOW = $(wildcard ./src/window/*.cpp) +SRCPHYSICS = $(wildcard ./src/physics/*.cpp) $(wildcard ./src/physics/constraints/*.cpp) SRCTEST = $(wildcard ./src/test/*.cpp) SRCEEIV = $(wildcard ./src/eeiv/*.cpp) SRCFLUID = $(wildcard ./src/fluid/*.cpp) -SRCHELPERS = $(SRCGLEW) $(SRCSOIL) $(SRCSTBVORBIS) $(SRCZLIB) $(SRCLIBZIP) -SRCMODULES = $(SRCHAIKUTTF) $(SRCBASE) $(SRCAUDIO) $(SRCGAMING) $(SRCGRAPHICS) $(SRCMATH) $(SRCSYSTEM) $(SRCUI) $(SRCUTILS) $(SRCWINDOW) +SRCHELPERS = $(SRCGLEW) $(SRCSOIL) $(SRCSTBVORBIS) $(SRCZLIB) $(SRCLIBZIP) $(SRCCHIPMUNK) +SRCMODULES = $(SRCHAIKUTTF) $(SRCBASE) $(SRCAUDIO) $(SRCGAMING) $(SRCGRAPHICS) $(SRCMATH) $(SRCSYSTEM) $(SRCUI) $(SRCUTILS) $(SRCWINDOW) $(SRCPHYSICS) SRCALL = $(SRCMODULES) $(SRCHELPERS) $(SRCTEST) $(SRCEEIV) SRCHPPALL = $(SRCALL:.cpp=.hpp) SRCHALL = $(SRCALL:.c=.h) @@ -97,6 +101,7 @@ OBJSOIL = $(SRCSOIL:.c=.o) OBJSTBVORBIS = $(SRCSTBVORBIS:.c=.o) OBJZLIB = $(SRCZLIB:.c=.o) OBJLIBZIP = $(SRCLIBZIP:.c=.o) +OBJCHIPMUNK = $(SRCCHIPMUNK:.c=.o) OBJHAIKUTTF = $(SRCHAIKUTTF:.cpp=.o) OBJBASE = $(SRCBASE:.cpp=.o) @@ -108,9 +113,10 @@ OBJSYSTEM = $(SRCSYSTEM:.cpp=.o) OBJUI = $(SRCUI:.cpp=.o) OBJUTILS = $(SRCUTILS:.cpp=.o) OBJWINDOW = $(SRCWINDOW:.cpp=.o) +OBJPHYSICS = $(SRCPHYSICS:.cpp=.o) -OBJHELPERS = $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) -OBJMODULES = $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) +OBJHELPERS = $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) $(OBJCHIPMUNK) +OBJMODULES = $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) $(OBJPHYSICS) OBJTEST = $(SRCTEST:.cpp=.o) OBJEEIV = $(SRCEEIV:.cpp=.o) @@ -118,8 +124,8 @@ OBJFLUID = $(SRCFLUID:.cpp=.o) OBJDIR = obj/$(OS)/$(RELEASETYPE)/ -FOBJHELPERS = $(patsubst ./%, $(OBJDIR)%, $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) ) -FOBJMODULES = $(patsubst ./%, $(OBJDIR)%, $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) ) +FOBJHELPERS = $(patsubst ./%, $(OBJDIR)%, $(OBJGLEW) $(OBJSOIL) $(OBJSTBVORBIS) $(OBJZLIB) $(OBJLIBZIP) $(OBJCHIPMUNK) ) +FOBJMODULES = $(patsubst ./%, $(OBJDIR)%, $(OBJHAIKUTTF) $(OBJBASE) $(OBJUTILS) $(OBJMATH) $(OBJSYSTEM) $(OBJAUDIO) $(OBJWINDOW) $(OBJGRAPHICS) $(OBJGAMING) $(OBJUI) $(OBJPHYSICS) ) FOBJTEST = $(patsubst ./%, $(OBJDIR)%, $(SRCTEST:.cpp=.o) ) FOBJEEIV = $(patsubst ./%, $(OBJDIR)%, $(SRCEEIV:.cpp=.o) ) @@ -140,6 +146,8 @@ dirs: @mkdir -p $(OBJDIR)/src/helper/stb_vorbis @mkdir -p $(OBJDIR)/src/helper/zlib @mkdir -p $(OBJDIR)/src/helper/libzip + @mkdir -p $(OBJDIR)/src/helper/chipmunk + @mkdir -p $(OBJDIR)/src/helper/chipmunk/constraints @mkdir -p $(OBJDIR)/src/helper/haikuttf @mkdir -p $(OBJDIR)/src/base @mkdir -p $(OBJDIR)/src/audio @@ -150,6 +158,8 @@ dirs: @mkdir -p $(OBJDIR)/src/ui @mkdir -p $(OBJDIR)/src/utils @mkdir -p $(OBJDIR)/src/window + @mkdir -p $(OBJDIR)/src/physics + @mkdir -p $(OBJDIR)/src/physics/constraints @mkdir -p $(OBJDIR)/src/test @mkdir -p $(OBJDIR)/src/eeiv @mkdir -p $(OBJDIR)/src/fluid @@ -161,8 +171,8 @@ $(FOBJMODULES): @$(CPP) -MT $@ -MM $(patsubst $(OBJDIR)%.o,%.cpp,$@) $(OTHERINC) > $(patsubst %.o,%.d,$@) $(FOBJHELPERS): - $(CC) -o $@ -c $(patsubst $(OBJDIR)%.o,%.c,$@) $(CFLAGSEXT) -DSTBI_FAILURE_USERMSG - @$(CC) -MT $@ -MM $(patsubst $(OBJDIR)%.o,%.c,$@) -DSTBI_FAILURE_USERMSG > $(patsubst %.o,%.d,$@) + $(CC) -o $@ -c $(patsubst $(OBJDIR)%.o,%.c,$@) $(CFLAGSEXT) -DSTBI_FAILURE_USERMSG -std=gnu99 $(HELPERSINC) + @$(CC) -MT $@ -MM $(patsubst $(OBJDIR)%.o,%.c,$@) -DSTBI_FAILURE_USERMSG > $(patsubst %.o,%.d,$@) $(HELPERSINC) $(FOBJTEST): $(CPP) -o $@ -c $(patsubst $(OBJDIR)%.o,%.cpp,$@) $(CFLAGS) $(OTHERINC) diff --git a/src/ee.h b/src/ee.h index 26c2f7e0e..088436a4d 100755 --- a/src/ee.h +++ b/src/ee.h @@ -173,4 +173,24 @@ #include "ui/cuigenericgrid.hpp" #include "ui/cuiwindow.hpp" using namespace EE::UI; + + #include "physics/cphysicsmanager.hpp" + #include "physics/cshape.hpp" + #include "physics/cshapecircle.hpp" + #include "physics/cshapesegment.hpp" + #include "physics/cshapepoly.hpp" + #include "physics/cspace.hpp" + #include "physics/cbody.hpp" + #include "physics/constraints/cconstraint.hpp" + #include "physics/constraints/cdampedrotaryspring.hpp" + #include "physics/constraints/cdampedspring.hpp" + #include "physics/constraints/cgearjoint.hpp" + #include "physics/constraints/cgroovejoint.hpp" + #include "physics/constraints/cpinjoint.hpp" + #include "physics/constraints/cpivotjoint.hpp" + #include "physics/constraints/cratchetjoint.hpp" + #include "physics/constraints/crotarylimitjoint.hpp" + #include "physics/constraints/csimplemotor.hpp" + #include "physics/constraints/cslidejoint.hpp" + using namespace EE::Physics; #endif diff --git a/src/helper/chipmunk/chipmunk.c b/src/helper/chipmunk/chipmunk.c new file mode 100644 index 000000000..058930800 --- /dev/null +++ b/src/helper/chipmunk/chipmunk.c @@ -0,0 +1,150 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include "chipmunk.h" + +#ifdef __cplusplus +extern "C" { +#endif + void cpInitCollisionFuncs(void); +#ifdef __cplusplus +} +#endif + +void +cpMessage(const char *message, const char *condition, const char *file, int line, int isError) +{ + fprintf(stderr, (isError ? "Aborting due to Chipmunk error: %s\n" : "Chipmunk warning: %s\n"), message); + fprintf(stderr, "\tFailed condition: %s\n", condition); + fprintf(stderr, "\tSource:%s:%d\n", file, line); + + if(isError) abort(); +} + + +const char *cpVersionString = "5.3.4"; + +void +cpInitChipmunk(void) +{ +#ifndef NDEBUG + printf("Initializing Chipmunk v%s (Debug Enabled)\n", cpVersionString); + printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n"); +#endif + + cpInitCollisionFuncs(); +} + +cpFloat +cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) +{ + return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset)); +} + +cpFloat +cpAreaForCircle(cpFloat r1, cpFloat r2) +{ + return 2.0f*(cpFloat)M_PI*cpfabs(r1*r1 - r2*r2); +} + +cpFloat +cpMomentForSegment(cpFloat m, cpVect a, cpVect b) +{ + cpFloat length = cpvlength(cpvsub(b, a)); + cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f); + + return m*(length*length/12.0f + cpvlengthsq(offset)); +} + +cpFloat +cpAreaForSegment(cpVect a, cpVect b, cpFloat r) +{ + return 2.0f*r*((cpFloat)M_PI*r + cpvdist(a, b)); +} + +cpFloat +cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset) +{ + cpFloat sum1 = 0.0f; + cpFloat sum2 = 0.0f; + for(int i=0; i= 1600 + #define MAKE_REF(name) decltype(name) *_##name = name + #else + #define MAKE_REF(name) + #endif +#else + #define MAKE_REF(name) __typeof__(name) *_##name = name +#endif + +MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv() +MAKE_REF(cpveql); +MAKE_REF(cpvadd); +MAKE_REF(cpvneg); +MAKE_REF(cpvsub); +MAKE_REF(cpvmult); +MAKE_REF(cpvdot); +MAKE_REF(cpvcross); +MAKE_REF(cpvperp); +MAKE_REF(cpvrperp); +MAKE_REF(cpvproject); +MAKE_REF(cpvrotate); +MAKE_REF(cpvunrotate); +MAKE_REF(cpvlengthsq); +MAKE_REF(cpvlerp); +MAKE_REF(cpvnormalize); +MAKE_REF(cpvnormalize_safe); +MAKE_REF(cpvclamp); +MAKE_REF(cpvlerpconst); +MAKE_REF(cpvdist); +MAKE_REF(cpvdistsq); +MAKE_REF(cpvnear); + +MAKE_REF(cpBBNew); +MAKE_REF(cpBBintersects); +MAKE_REF(cpBBcontainsBB); +MAKE_REF(cpBBcontainsVect); +MAKE_REF(cpBBmerge); +MAKE_REF(cpBBexpand); + +MAKE_REF(cpBodyWorld2Local); +MAKE_REF(cpBodyLocal2World); +MAKE_REF(cpBodyApplyImpulse); +MAKE_REF(cpBodyIsSleeping); +MAKE_REF(cpBodyIsRogue); +MAKE_REF(cpBodyKineticEnergy); + +MAKE_REF(cpArbiterIsFirstContact); +MAKE_REF(cpArbiterGetShapes); +MAKE_REF(cpArbiterGetNormal); +MAKE_REF(cpArbiterGetPoint); + +MAKE_REF(cpConstraintGetImpulse); + +MAKE_REF(cpSegmentQueryHitPoint); +MAKE_REF(cpSegmentQueryHitDist); diff --git a/src/helper/chipmunk/chipmunk_private.h b/src/helper/chipmunk/chipmunk_private.h new file mode 100644 index 000000000..418655492 --- /dev/null +++ b/src/helper/chipmunk/chipmunk_private.h @@ -0,0 +1,150 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#define CP_ALLOW_PRIVATE_ACCESS 1 +#include "chipmunk.h" + +#pragma mark cpArray + +struct cpArray { + int num, max; + void **arr; +}; + +// TODO get rid of reference versions? +cpArray *cpArrayAlloc(void); +cpArray *cpArrayInit(cpArray *arr, int size); +cpArray *cpArrayNew(int size); + +void cpArrayDestroy(cpArray *arr); +void cpArrayFree(cpArray *arr); + +void cpArrayPush(cpArray *arr, void *object); +void *cpArrayPop(cpArray *arr); +void cpArrayDeleteObj(cpArray *arr, void *obj); +cpBool cpArrayContains(cpArray *arr, void *ptr); + +void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); + +#pragma mark Foreach loops + +#define CP_BODY_FOREACH_CONSTRAINT(body, var)\ + for(cpConstraint *var = body->constraintList; var; var = (var->a == body ? var->nextA : var->nextB)) + +#define CP_BODY_FOREACH_ARBITER(body, var)\ + for(cpArbiter *var = body->arbiterList; var; var = (var->a->body == body ? var->nextA : var->nextB)) + +#define CP_BODY_FOREACH_SHAPE(body, var)\ + for(cpShape *var = body->shapeList; var; var = var->next) + +#define CP_BODY_FOREACH_GROUP(root, var)\ + for(cpBody *var = root; var; var = var->node.next) + +#pragma mark cpHashSet + +typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt); +typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); + +// TODO get rid of reference versions? +cpHashSet *cpHashSetAlloc(void); +cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *defaultValue); +cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, void *defaultValue); + +void cpHashSetDestroy(cpHashSet *set); +void cpHashSetFree(cpHashSet *set); + +int cpHashSetCount(cpHashSet *set); +void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans); +void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr); +void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr); + +typedef void (*cpHashSetIterFunc)(void *elt, void *data); +void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data); + +typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data); +void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data); + +#pragma mark Arbiters + +cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash); +cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); + +void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b); +void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv); +void cpArbiterApplyCachedImpulse(cpArbiter *arb); +void cpArbiterApplyImpulse(cpArbiter *arb); + +#pragma mark Collision Functions + +int cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr); + +static inline cpFloat +cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) +{ + cpVect *verts = poly->CP_PRIVATE(tVerts); + cpFloat min = cpvdot(n, verts[0]); + + for(int i=1; iCP_PRIVATE(numVerts); i++){ + min = cpfmin(min, cpvdot(n, verts[i])); + } + + return min - d; +} + +static inline cpBool +cpPolyShapeContainsVert(const cpPolyShape *poly, const cpVect v) +{ + cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes); + + for(int i=0; iCP_PRIVATE(numVerts); i++){ + cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; + if(dist > 0.0f) return cpFalse; + } + + return cpTrue; +} + +static inline cpBool +cpPolyShapeContainsVertPartial(const cpPolyShape *poly, const cpVect v, const cpVect n) +{ + cpPolyShapeAxis *axes = poly->CP_PRIVATE(tAxes); + + for(int i=0; iCP_PRIVATE(numVerts); i++){ + if(cpvdot(axes[i].n, n) < 0.0f) continue; + cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; + if(dist > 0.0f) return cpFalse; + } + + return cpTrue; +} + +#pragma mark Spatial Index Functions + +cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + +#pragma mark Space Functions + +cpContact *cpContactBufferGetArray(cpSpace *space); +void cpSpacePushContacts(cpSpace *space, int count); +void cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space); +void cpSpaceActivateBody(cpSpace *space, cpBody *body); +void cpSpaceLock(cpSpace *space); +void cpSpaceUnlock(cpSpace *space); diff --git a/src/helper/chipmunk/chipmunk_types.h b/src/helper/chipmunk/chipmunk_types.h new file mode 100644 index 000000000..a9d9a4172 --- /dev/null +++ b/src/helper/chipmunk/chipmunk_types.h @@ -0,0 +1,151 @@ +#ifdef __APPLE__ + #import "TargetConditionals.h" +#endif + +#if (defined TARGET_OS_IPHONE) && (!defined CP_USE_CGPOINTS) + #define CP_USE_CGPOINTS +#endif + +#ifdef CP_USE_CGPOINTS + #if TARGET_OS_IPHONE + #import + #elif TARGET_OS_MAC + #import + #endif + + #if defined(__LP64__) && __LP64__ + #define CP_USE_DOUBLES 1 + #else + #define CP_USE_DOUBLES 0 + #endif +#endif + +#ifndef CP_USE_DOUBLES + // use doubles by default for higher precision + #define CP_USE_DOUBLES 1 +#endif + +#if CP_USE_DOUBLES + typedef double cpFloat; + #define cpfsqrt sqrt + #define cpfsin sin + #define cpfcos cos + #define cpfacos acos + #define cpfatan2 atan2 + #define cpfmod fmod + #define cpfexp exp + #define cpfpow pow + #define cpffloor floor + #define cpfceil ceil +#else + typedef float cpFloat; + #define cpfsqrt sqrtf + #define cpfsin sinf + #define cpfcos cosf + #define cpfacos acosf + #define cpfatan2 atan2f + #define cpfmod fmodf + #define cpfexp expf + #define cpfpow powf + #define cpffloor floorf + #define cpfceil ceilf +#endif + +static inline cpFloat +cpfmax(cpFloat a, cpFloat b) +{ + return (a > b) ? a : b; +} + +static inline cpFloat +cpfmin(cpFloat a, cpFloat b) +{ + return (a < b) ? a : b; +} + +static inline cpFloat +cpfabs(cpFloat n) +{ + return (n < 0) ? -n : n; +} + +static inline cpFloat +cpfclamp(cpFloat f, cpFloat min, cpFloat max) +{ + return cpfmin(cpfmax(f, min), max); +} + +static inline cpFloat +cpflerp(cpFloat f1, cpFloat f2, cpFloat t) +{ + return f1*(1.0f - t) + f2*t; +} + +static inline cpFloat +cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) +{ + return f1 + cpfclamp(f2 - f1, -d, d); +} + +// CGPoints are structurally the same, and allow +// easy interoperability with other Cocoa libraries +#ifdef CP_USE_CGPOINTS + typedef CGPoint cpVect; +#else + typedef struct cpVect{cpFloat x,y;} cpVect; +#endif + +typedef unsigned int cpHashValue; + +// Oh C, how we love to define our own boolean types to get compiler compatibility +#ifdef CP_BOOL_TYPE + typedef CP_BOOL_TYPE cpBool; +#else + typedef int cpBool; +#endif + +#ifndef cpTrue + #define cpTrue 1 +#endif + +#ifndef cpFalse + #define cpFalse 0 +#endif + +#ifdef CP_DATA_POINTER_TYPE + typedef CP_DATA_POINTER_TYPE cpDataPointer; +#else + typedef void * cpDataPointer; +#endif + +#ifdef CP_COLLISION_TYPE_TYPE + typedef CP_COLLISION_TYPE_TYPE cpCollisionType; +#else + typedef unsigned int cpCollisionType; +#endif + +#ifdef CP_GROUP_TYPE + typedef CP_GROUP_TYPE cpGroup; +#else + typedef unsigned int cpGroup; +#endif + +#ifdef CP_LAYERS_TYPE + typedef CP_GROUP_TYPE cpLayers; +#else + typedef unsigned int cpLayers; +#endif + +#ifdef CP_TIMESTAMP_TYPE + typedef CP_TIMESTAMP_TYPE cpTimestamp; +#else + typedef unsigned int cpTimestamp; +#endif + +#ifndef CP_NO_GROUP + #define CP_NO_GROUP ((cpGroup)0) +#endif + +#ifndef CP_ALL_LAYERS + #define CP_ALL_LAYERS (~(cpLayers)0) +#endif diff --git a/src/helper/chipmunk/chipmunk_unsafe.h b/src/helper/chipmunk/chipmunk_unsafe.h new file mode 100644 index 000000000..d15b6712d --- /dev/null +++ b/src/helper/chipmunk/chipmunk_unsafe.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/* This header defines a number of "unsafe" operations on Chipmunk objects. + * In this case "unsafe" is referring to operations which may reduce the + * physical accuracy or numerical stability of the simulation, but will not + * cause crashes. + * + * The prime example is mutating collision shapes. Chipmunk does not support + * this directly. Mutating shapes using this API will caused objects in contact + * to be pushed apart using Chipmunk's overlap solver, but not using real + * persistent velocities. Probably not what you meant, but perhaps close enough. + */ + +#ifndef CHIPMUNK_UNSAFE_HEADER +#define CHIPMUNK_UNSAFE_HEADER + +#ifdef __cplusplus +extern "C" { +#endif + +void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius); +void cpCircleShapeSetOffset(cpShape *shape, cpVect offset); + +void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b); +void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius); + +void cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/helper/chipmunk/constraints/cpConstraint.c b/src/helper/chipmunk/constraints/cpConstraint.c new file mode 100644 index 000000000..2b65deeeb --- /dev/null +++ b/src/helper/chipmunk/constraints/cpConstraint.c @@ -0,0 +1,58 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +// TODO: Comment me! + +cpFloat cp_constraint_bias_coef = 0.1f; + +void cpConstraintDestroy(cpConstraint *constraint){} + +void +cpConstraintFree(cpConstraint *constraint) +{ + if(constraint){ + cpConstraintDestroy(constraint); + cpfree(constraint); + } +} + +// *** defined in util.h + +void +cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b) +{ + constraint->klass = klass; + + constraint->a = a; + constraint->b = b; + + constraint->nextA = NULL; + constraint->nextB = NULL; + + constraint->maxForce = (cpFloat)INFINITY; + constraint->biasCoef = cp_constraint_bias_coef; + constraint->maxBias = (cpFloat)INFINITY; +} diff --git a/src/helper/chipmunk/constraints/cpConstraint.h b/src/helper/chipmunk/constraints/cpConstraint.h new file mode 100644 index 000000000..6646ef3db --- /dev/null +++ b/src/helper/chipmunk/constraints/cpConstraint.h @@ -0,0 +1,102 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// TODO: refactoring needed here + +extern cpFloat cp_constraint_bias_coef; + +typedef struct cpConstraintClass cpConstraintClass; + +typedef void (*cpConstraintPreStepFunction)(cpConstraint *constraint, cpFloat dt, cpFloat dt_inv); +typedef void (*cpConstraintApplyImpulseFunction)(cpConstraint *constraint); +typedef cpFloat (*cpConstraintGetImpulseFunction)(cpConstraint *constraint); + +struct cpConstraintClass { + cpConstraintPreStepFunction preStep; + cpConstraintApplyImpulseFunction applyImpulse; + cpConstraintGetImpulseFunction getImpulse; +}; + + + +struct cpConstraint { + CP_PRIVATE(const cpConstraintClass *klass); + + cpBody *a, *b; + cpConstraint CP_PRIVATE(*nextA), CP_PRIVATE(*nextB); + + cpFloat maxForce; + cpFloat biasCoef; + cpFloat maxBias; + + cpDataPointer data; +}; + +void cpConstraintDestroy(cpConstraint *constraint); +void cpConstraintFree(cpConstraint *constraint); + +static inline void +cpConstraintActivateBodies(cpConstraint *constraint) +{ + cpBody *a = constraint->a; if(a) cpBodyActivate(a); + cpBody *b = constraint->b; if(b) cpBodyActivate(b); +} + +static inline cpFloat +cpConstraintGetImpulse(cpConstraint *constraint) +{ + return constraint->CP_PRIVATE(klass)->getImpulse(constraint); +} + +#define cpConstraintCheckCast(constraint, struct) \ + cpAssert(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct); + + +#define CP_DefineConstraintGetter(struct, type, member, name) \ +static inline type \ +struct##Get##name(const cpConstraint *constraint){ \ + cpConstraintCheckCast(constraint, struct); \ + return ((struct *)constraint)->member; \ +} \ + +#define CP_DefineConstraintSetter(struct, type, member, name) \ +static inline void \ +struct##Set##name(cpConstraint *constraint, type value){ \ + cpConstraintCheckCast(constraint, struct); \ + cpConstraintActivateBodies(constraint); \ + ((struct *)constraint)->member = value; \ +} \ + +#define CP_DefineConstraintProperty(struct, type, member, name) \ +CP_DefineConstraintGetter(struct, type, member, name) \ +CP_DefineConstraintSetter(struct, type, member, name) + +// Built in Joint types +#include "cpPinJoint.h" +#include "cpSlideJoint.h" +#include "cpPivotJoint.h" +#include "cpGrooveJoint.h" +#include "cpDampedSpring.h" +#include "cpDampedRotarySpring.h" +#include "cpRotaryLimitJoint.h" +#include "cpRatchetJoint.h" +#include "cpGearJoint.h" +#include "cpSimpleMotor.h" diff --git a/src/helper/chipmunk/constraints/cpDampedRotarySpring.c b/src/helper/chipmunk/constraints/cpDampedRotarySpring.c new file mode 100644 index 000000000..499103dba --- /dev/null +++ b/src/helper/chipmunk/constraints/cpDampedRotarySpring.c @@ -0,0 +1,105 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static cpFloat +defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ + return (relativeAngle - spring->restAngle)*spring->stiffness; +} + +static void +preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(spring, a, b); + + cpFloat moment = a->i_inv + b->i_inv; + spring->iSum = 1.0f/moment; + + spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); + spring->target_wrn = 0.0f; + + // apply spring torque + cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; + a->w -= j_spring*a->i_inv; + b->w += j_spring*b->i_inv; +} + +static void +applyImpulse(cpDampedRotarySpring *spring) +{ + CONSTRAINT_BEGIN(spring, a, b); + + // compute relative velocity + cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + cpFloat w_damp = wrn*spring->w_coef; + spring->target_wrn = wrn - w_damp; + + //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); + cpFloat j_damp = w_damp*spring->iSum; + a->w -= j_damp*a->i_inv; + b->w += j_damp*b->i_inv; +} + +static cpFloat +getImpulse(cpConstraint *constraint) +{ + return 0.0f; +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpDampedRotarySpring) + +cpDampedRotarySpring * +cpDampedRotarySpringAlloc(void) +{ + return (cpDampedRotarySpring *)cpmalloc(sizeof(cpDampedRotarySpring)); +} + +cpDampedRotarySpring * +cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit((cpConstraint *)spring, &klass, a, b); + + spring->restAngle = restAngle; + spring->stiffness = stiffness; + spring->damping = damping; + spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque; + + return spring; +} + +cpConstraint * +cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) +{ + return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping); +} diff --git a/src/helper/chipmunk/constraints/cpDampedRotarySpring.h b/src/helper/chipmunk/constraints/cpDampedRotarySpring.h new file mode 100644 index 000000000..b446a3a51 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpDampedRotarySpring.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); + +const cpConstraintClass *cpDampedRotarySpringGetClass(); + +typedef struct cpDampedRotarySpring { + cpConstraint constraint; + cpFloat restAngle; + cpFloat stiffness; + cpFloat damping; + cpDampedRotarySpringTorqueFunc springTorqueFunc; + + cpFloat target_wrn; + cpFloat w_coef; + + cpFloat iSum; +} cpDampedRotarySpring; + +cpDampedRotarySpring *cpDampedRotarySpringAlloc(void); +cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); +cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); + +CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle); +CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness); +CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping); +CP_DefineConstraintProperty(cpDampedRotarySpring, cpDampedRotarySpringTorqueFunc, springTorqueFunc, SpringTorqueFunc); diff --git a/src/helper/chipmunk/constraints/cpDampedSpring.c b/src/helper/chipmunk/constraints/cpDampedSpring.c new file mode 100644 index 000000000..406af3af6 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpDampedSpring.c @@ -0,0 +1,115 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static cpFloat +defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ + return (spring->restLength - dist)*spring->stiffness; +} + +static void +preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(spring, a, b); + + spring->r1 = cpvrotate(spring->anchr1, a->rot); + spring->r2 = cpvrotate(spring->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); + cpFloat dist = cpvlength(delta); + spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); + spring->nMass = 1.0f/k; + + spring->target_vrn = 0.0f; + spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); + + // apply spring force + cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); + apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt)); +} + +static void +applyImpulse(cpDampedSpring *spring) +{ + CONSTRAINT_BEGIN(spring, a, b); + + cpVect n = spring->n; + cpVect r1 = spring->r1; + cpVect r2 = spring->r2; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + cpFloat v_damp = -vrn*spring->v_coef; + spring->target_vrn = vrn + v_damp; + + apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); +} + +static cpFloat +getImpulse(cpConstraint *constraint) +{ + return 0.0f; +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpDampedSpring) + +cpDampedSpring * +cpDampedSpringAlloc(void) +{ + return (cpDampedSpring *)cpmalloc(sizeof(cpDampedSpring)); +} + +cpDampedSpring * +cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit((cpConstraint *)spring, cpDampedSpringGetClass(), a, b); + + spring->anchr1 = anchr1; + spring->anchr2 = anchr2; + + spring->restLength = restLength; + spring->stiffness = stiffness; + spring->damping = damping; + spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce; + + return spring; +} + +cpConstraint * +cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping); +} diff --git a/src/helper/chipmunk/constraints/cpDampedSpring.h b/src/helper/chipmunk/constraints/cpDampedSpring.h new file mode 100644 index 000000000..aed871e51 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpDampedSpring.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef struct cpDampedSpring cpDampedSpring; + +typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist); + +const cpConstraintClass *cpDampedSpringGetClass(); + +struct cpDampedSpring { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat restLength; + cpFloat stiffness; + cpFloat damping; + cpDampedSpringForceFunc springForceFunc; + + cpFloat target_vrn; + cpFloat v_coef; + + cpVect r1, r2; + cpFloat nMass; + cpVect n; +}; + +cpDampedSpring *cpDampedSpringAlloc(void); +cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); +cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); + +CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpDampedSpring, cpFloat, restLength, RestLength); +CP_DefineConstraintProperty(cpDampedSpring, cpFloat, stiffness, Stiffness); +CP_DefineConstraintProperty(cpDampedSpring, cpFloat, damping, Damping); +CP_DefineConstraintProperty(cpDampedSpring, cpDampedSpringForceFunc, springForceFunc, SpringForceFunc); diff --git a/src/helper/chipmunk/constraints/cpGearJoint.c b/src/helper/chipmunk/constraints/cpGearJoint.c new file mode 100644 index 000000000..dd694d35b --- /dev/null +++ b/src/helper/chipmunk/constraints/cpGearJoint.c @@ -0,0 +1,113 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // apply joint torque + cpFloat j = joint->jAcc; + a->w -= j*a->i_inv*joint->ratio_inv; + b->w += j*b->i_inv; +} + +static void +applyImpulse(cpGearJoint *joint) +{ + CONSTRAINT_BEGIN(joint, a, b); + + // compute relative rotational velocity + cpFloat wr = b->w*joint->ratio - a->w; + + // compute normal impulse + cpFloat j = (joint->bias - wr)*joint->iSum; + cpFloat jOld = joint->jAcc; + joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax); + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv*joint->ratio_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpGearJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpGearJoint) + +cpGearJoint * +cpGearJointAlloc(void) +{ + return (cpGearJoint *)cpmalloc(sizeof(cpGearJoint)); +} + +cpGearJoint * +cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->phase = phase; + joint->ratio = ratio; + joint->ratio_inv = 1.0f/ratio; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) +{ + return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio); +} + +void +cpGearJointSetRatio(cpConstraint *constraint, cpFloat value) +{ + cpConstraintCheckCast(constraint, cpGearJoint); + ((cpGearJoint *)constraint)->ratio = value; + ((cpGearJoint *)constraint)->ratio_inv = 1.0f/value; + cpConstraintActivateBodies(constraint); +} diff --git a/src/helper/chipmunk/constraints/cpGearJoint.h b/src/helper/chipmunk/constraints/cpGearJoint.h new file mode 100644 index 000000000..f0317c1d4 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpGearJoint.h @@ -0,0 +1,41 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpGearJointGetClass(); + +typedef struct cpGearJoint { + cpConstraint constraint; + cpFloat phase, ratio; + cpFloat ratio_inv; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc, jMax; +} cpGearJoint; + +cpGearJoint *cpGearJointAlloc(void); +cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); +cpConstraint *cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); + +CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase); +CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio); +void cpGearJointSetRatio(cpConstraint *constraint, cpFloat value); diff --git a/src/helper/chipmunk/constraints/cpGrooveJoint.c b/src/helper/chipmunk/constraints/cpGrooveJoint.c new file mode 100644 index 000000000..4a067ef92 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpGrooveJoint.c @@ -0,0 +1,161 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + // calculate endpoints in worldspace + cpVect ta = cpBodyLocal2World(a, joint->grv_a); + cpVect tb = cpBodyLocal2World(a, joint->grv_b); + + // calculate axis + cpVect n = cpvrotate(joint->grv_n, a->rot); + cpFloat d = cpvdot(ta, n); + + joint->grv_tn = n; + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + // calculate tangential distance along the axis of r2 + cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n); + // calculate clamping factor and r2 + if(td <= cpvcross(ta, n)){ + joint->clamp = 1.0f; + joint->r1 = cpvsub(ta, a->p); + } else if(td >= cpvcross(tb, n)){ + joint->clamp = -1.0f; + joint->r1 = cpvsub(tb, a->p); + } else { + joint->clamp = 0.0f; + joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + } + + // Calculate mass tensor + k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); + + // compute max impulse + joint->jMaxLen = J_MAX(joint, dt); + + // calculate bias velocity + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + // apply accumulated impulse + apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); +} + +static inline cpVect +grooveConstrain(cpGrooveJoint *joint, cpVect j){ + cpVect n = joint->grv_tn; + cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); + return cpvclamp(jClamp, joint->jMaxLen); +} + +static void +applyImpulse(cpGrooveJoint *joint) +{ + CONSTRAINT_BEGIN(joint, a, b); + + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute impulse + cpVect vr = relative_velocity(a, b, r1, r2); + + cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); + cpVect jOld = joint->jAcc; + joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j)); + j = cpvsub(joint->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static cpFloat +getImpulse(cpGrooveJoint *joint) +{ + return cpvlength(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpGrooveJoint) + +cpGrooveJoint * +cpGrooveJointAlloc(void) +{ + return (cpGrooveJoint *)cpmalloc(sizeof(cpGrooveJoint)); +} + +cpGrooveJoint * +cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->grv_a = groove_a; + joint->grv_b = groove_b; + joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); + joint->anchr2 = anchr2; + + joint->jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) +{ + return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); +} + +void +cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value) +{ + cpGrooveJoint *g = (cpGrooveJoint *)constraint; + cpConstraintCheckCast(constraint, cpGrooveJoint); + + g->grv_a = value; + g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value))); + + cpConstraintActivateBodies(constraint); +} + +void +cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value) +{ + cpGrooveJoint *g = (cpGrooveJoint *)constraint; + cpConstraintCheckCast(constraint, cpGrooveJoint); + + g->grv_b = value; + g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a))); + + cpConstraintActivateBodies(constraint); +} + diff --git a/src/helper/chipmunk/constraints/cpGrooveJoint.h b/src/helper/chipmunk/constraints/cpGrooveJoint.h new file mode 100644 index 000000000..831e3b68c --- /dev/null +++ b/src/helper/chipmunk/constraints/cpGrooveJoint.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpGrooveJointGetClass(); + +typedef struct cpGrooveJoint { + cpConstraint constraint; + cpVect grv_n, grv_a, grv_b; + cpVect anchr2; + + cpVect grv_tn; + cpFloat clamp; + cpVect r1, r2; + cpVect k1, k2; + + cpVect jAcc; + cpFloat jMaxLen; + cpVect bias; +} cpGrooveJoint; + +cpGrooveJoint *cpGrooveJointAlloc(void); +cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); +cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); + + +CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA); +void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value); +CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB); +void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value); +CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2); diff --git a/src/helper/chipmunk/constraints/cpPinJoint.c b/src/helper/chipmunk/constraints/cpPinJoint.c new file mode 100644 index 000000000..3955143ff --- /dev/null +++ b/src/helper/chipmunk/constraints/cpPinJoint.c @@ -0,0 +1,116 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +//#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + joint->r1 = cpvrotate(joint->anchr1, a->rot); + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + cpFloat dist = cpvlength(delta); + joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY)); + + // calculate mass normal + joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias); + + // compute max impulse + joint->jnMax = J_MAX(joint, dt); + + // apply accumulated impulse + cpVect j = cpvmult(joint->n, joint->jnAcc); + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static void +applyImpulse(cpPinJoint *joint) +{ + CONSTRAINT_BEGIN(joint, a, b); + cpVect n = joint->n; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n); + + // compute normal impulse + cpFloat jn = (joint->bias - vrn)*joint->nMass; + cpFloat jnOld = joint->jnAcc; + joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax); + jn = joint->jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpPinJoint *joint) +{ + return cpfabs(joint->jnAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpPinJoint); + + +cpPinJoint * +cpPinJointAlloc(void) +{ + return (cpPinJoint *)cpmalloc(sizeof(cpPinJoint)); +} + +cpPinJoint * +cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + + // STATIC_BODY_CHECK + cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1); + cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2); + joint->dist = cpvlength(cpvsub(p2, p1)); + + joint->jnAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); +} diff --git a/src/helper/chipmunk/constraints/cpPinJoint.h b/src/helper/chipmunk/constraints/cpPinJoint.h new file mode 100644 index 000000000..5b257a190 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpPinJoint.h @@ -0,0 +1,43 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpPinJointGetClass(); + +typedef struct cpPinJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat dist; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc, jnMax; + cpFloat bias; +} cpPinJoint; + +cpPinJoint *cpPinJointAlloc(void); +cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); + +CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist); diff --git a/src/helper/chipmunk/constraints/cpPivotJoint.c b/src/helper/chipmunk/constraints/cpPivotJoint.c new file mode 100644 index 000000000..634bd0233 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpPivotJoint.c @@ -0,0 +1,114 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + joint->r1 = cpvrotate(joint->anchr1, a->rot); + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + // Calculate mass tensor + k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); + + // compute max impulse + joint->jMaxLen = J_MAX(joint, dt); + + // calculate bias velocity + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + // apply accumulated impulse + apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); +} + +static void +applyImpulse(cpPivotJoint *joint) +{ + CONSTRAINT_BEGIN(joint, a, b); + + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + + // compute normal impulse + cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); + cpVect jOld = joint->jAcc; + joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen); + j = cpvsub(joint->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpvlength(((cpPivotJoint *)joint)->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpPivotJoint) + +cpPivotJoint * +cpPivotJointAlloc(void) +{ + return (cpPivotJoint *)cpmalloc(sizeof(cpPivotJoint)); +} + +cpPivotJoint * +cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + + joint->jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2); +} + +cpConstraint * +cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) +{ + cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot); + cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot); + return cpPivotJointNew2(a, b, anchr1, anchr2); +} diff --git a/src/helper/chipmunk/constraints/cpPivotJoint.h b/src/helper/chipmunk/constraints/cpPivotJoint.h new file mode 100644 index 000000000..e71c7a015 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpPivotJoint.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpPivotJointGetClass(); + +typedef struct cpPivotJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + + cpVect r1, r2; + cpVect k1, k2; + + cpVect jAcc; + cpFloat jMaxLen; + cpVect bias; +} cpPivotJoint; + +cpPivotJoint *cpPivotJointAlloc(void); +cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); +cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); + +CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2); diff --git a/src/helper/chipmunk/constraints/cpRatchetJoint.c b/src/helper/chipmunk/constraints/cpRatchetJoint.c new file mode 100644 index 000000000..a7103c7f0 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpRatchetJoint.c @@ -0,0 +1,126 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + cpFloat angle = joint->angle; + cpFloat phase = joint->phase; + cpFloat ratchet = joint->ratchet; + + cpFloat delta = b->a - a->a; + cpFloat diff = angle - delta; + cpFloat pdist = 0.0f; + + if(diff*ratchet > 0.0f){ + pdist = diff; + } else { + joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; + } + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // If the bias is 0, the joint is not at a limit. Reset the impulse. + if(!joint->bias) + joint->jAcc = 0.0f; + + // apply joint torque + a->w -= joint->jAcc*a->i_inv; + b->w += joint->jAcc*b->i_inv; +} + +static void +applyImpulse(cpRatchetJoint *joint) +{ + if(!joint->bias) return; // early exit + + CONSTRAINT_BEGIN(joint, a, b); + + // compute relative rotational velocity + cpFloat wr = b->w - a->w; + cpFloat ratchet = joint->ratchet; + + // compute normal impulse + cpFloat j = -(joint->bias + wr)*joint->iSum; + cpFloat jOld = joint->jAcc; + joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint->jMax*cpfabs(ratchet))/ratchet; + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpRatchetJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpRatchetJoint) + +cpRatchetJoint * +cpRatchetJointAlloc(void) +{ + return (cpRatchetJoint *)cpmalloc(sizeof(cpRatchetJoint)); +} + +cpRatchetJoint * +cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->angle = 0.0f; + joint->phase = phase; + joint->ratchet = ratchet; + + // STATIC_BODY_CHECK + joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f); + + return joint; +} + +cpConstraint * +cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) +{ + return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet); +} diff --git a/src/helper/chipmunk/constraints/cpRatchetJoint.h b/src/helper/chipmunk/constraints/cpRatchetJoint.h new file mode 100644 index 000000000..ecb026ce7 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpRatchetJoint.h @@ -0,0 +1,40 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpRatchetJointGetClass(); + +typedef struct cpRatchetJoint { + cpConstraint constraint; + cpFloat angle, phase, ratchet; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc, jMax; +} cpRatchetJoint; + +cpRatchetJoint *cpRatchetJointAlloc(void); +cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); +cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); + +CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle); +CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase); +CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet); diff --git a/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c new file mode 100644 index 000000000..9bfa1ea70 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.c @@ -0,0 +1,120 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + cpFloat dist = b->a - a->a; + cpFloat pdist = 0.0f; + if(dist > joint->max) { + pdist = joint->max - dist; + } else if(dist < joint->min) { + pdist = joint->min - dist; + } + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // If the bias is 0, the joint is not at a limit. Reset the impulse. + if(!joint->bias) + joint->jAcc = 0.0f; + + // apply joint torque + a->w -= joint->jAcc*a->i_inv; + b->w += joint->jAcc*b->i_inv; +} + +static void +applyImpulse(cpRotaryLimitJoint *joint) +{ + if(!joint->bias) return; // early exit + + CONSTRAINT_BEGIN(joint, a, b); + + // compute relative rotational velocity + cpFloat wr = b->w - a->w; + + // compute normal impulse + cpFloat j = -(joint->bias + wr)*joint->iSum; + cpFloat jOld = joint->jAcc; + if(joint->bias < 0.0f){ + joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax); + } else { + joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f); + } + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpRotaryLimitJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpRotaryLimitJoint) + +cpRotaryLimitJoint * +cpRotaryLimitJointAlloc(void) +{ + return (cpRotaryLimitJoint *)cpmalloc(sizeof(cpRotaryLimitJoint)); +} + +cpRotaryLimitJoint * +cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->min = min; + joint->max = max; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max) +{ + return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max); +} diff --git a/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h new file mode 100644 index 000000000..9062f85f0 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpRotaryLimitJoint.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpRotaryLimitJointGetClass(); + +typedef struct cpRotaryLimitJoint { + cpConstraint constraint; + cpFloat min, max; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc, jMax; +} cpRotaryLimitJoint; + +cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void); +cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); +cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); + +CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min); +CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max); diff --git a/src/helper/chipmunk/constraints/cpSimpleMotor.c b/src/helper/chipmunk/constraints/cpSimpleMotor.c new file mode 100644 index 000000000..c4f771e44 --- /dev/null +++ b/src/helper/chipmunk/constraints/cpSimpleMotor.c @@ -0,0 +1,97 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // apply joint torque + a->w -= joint->jAcc*a->i_inv; + b->w += joint->jAcc*b->i_inv; +} + +static void +applyImpulse(cpSimpleMotor *joint) +{ + CONSTRAINT_BEGIN(joint, a, b); + + // compute relative rotational velocity + cpFloat wr = b->w - a->w + joint->rate; + + // compute normal impulse + cpFloat j = -wr*joint->iSum; + cpFloat jOld = joint->jAcc; + joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax); + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpSimpleMotor *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpSimpleMotor) + +cpSimpleMotor * +cpSimpleMotorAlloc(void) +{ + return (cpSimpleMotor *)cpmalloc(sizeof(cpSimpleMotor)); +} + +cpSimpleMotor * +cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->rate = rate; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate) +{ + return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate); +} diff --git a/src/helper/chipmunk/constraints/cpSimpleMotor.h b/src/helper/chipmunk/constraints/cpSimpleMotor.h new file mode 100644 index 000000000..1611e640d --- /dev/null +++ b/src/helper/chipmunk/constraints/cpSimpleMotor.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpSimpleMotorGetClass(); + +typedef struct cpSimpleMotor { + cpConstraint constraint; + cpFloat rate; + + cpFloat iSum; + + cpFloat jAcc, jMax; +} cpSimpleMotor; + +cpSimpleMotor *cpSimpleMotorAlloc(void); +cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); +cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); + +CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate); diff --git a/src/helper/chipmunk/constraints/cpSlideJoint.c b/src/helper/chipmunk/constraints/cpSlideJoint.c new file mode 100644 index 000000000..79c627c7f --- /dev/null +++ b/src/helper/chipmunk/constraints/cpSlideJoint.c @@ -0,0 +1,129 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +static void +preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + CONSTRAINT_BEGIN(joint, a, b); + + joint->r1 = cpvrotate(joint->anchr1, a->rot); + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + cpFloat dist = cpvlength(delta); + cpFloat pdist = 0.0f; + if(dist > joint->max) { + pdist = dist - joint->max; + } else if(dist < joint->min) { + pdist = joint->min - dist; + dist = -dist; + } + joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY)); + + // calculate mass normal + joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); + + // compute max impulse + joint->jnMax = J_MAX(joint, dt); + + // apply accumulated impulse + if(!joint->bias) //{ + // if bias is 0, then the joint is not at a limit. + joint->jnAcc = 0.0f; +// } else { + cpVect j = cpvmult(joint->n, joint->jnAcc); + apply_impulses(a, b, joint->r1, joint->r2, j); +// } +} + +static void +applyImpulse(cpSlideJoint *joint) +{ + if(!joint->bias) return; // early exit + + CONSTRAINT_BEGIN(joint, a, b); + + cpVect n = joint->n; + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + cpFloat vrn = cpvdot(vr, n); + + // compute normal impulse + cpFloat jn = (joint->bias - vrn)*joint->nMass; + cpFloat jnOld = joint->jnAcc; + joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f); + jn = joint->jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpfabs(((cpSlideJoint *)joint)->jnAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpSlideJoint) + +cpSlideJoint * +cpSlideJointAlloc(void) +{ + return (cpSlideJoint *)cpmalloc(sizeof(cpSlideJoint)); +} + +cpSlideJoint * +cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + joint->min = min; + joint->max = max; + + joint->jnAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) +{ + return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max); +} diff --git a/src/helper/chipmunk/constraints/cpSlideJoint.h b/src/helper/chipmunk/constraints/cpSlideJoint.h new file mode 100644 index 000000000..c5ffa4e3d --- /dev/null +++ b/src/helper/chipmunk/constraints/cpSlideJoint.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpSlideJointGetClass(); + +typedef struct cpSlideJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat min, max; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc, jnMax; + cpFloat bias; +} cpSlideJoint; + +cpSlideJoint *cpSlideJointAlloc(void); +cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); +cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); + +CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min); +CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max); diff --git a/src/helper/chipmunk/constraints/util.h b/src/helper/chipmunk/constraints/util.h new file mode 100644 index 000000000..8d402db63 --- /dev/null +++ b/src/helper/chipmunk/constraints/util.h @@ -0,0 +1,130 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(){return (cpConstraintClass *)&klass;} + +void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b); + +#define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt)) + +// Get valid body pointers and exit early if the bodies are idle +#define CONSTRAINT_BEGIN(constraint, a_var, b_var) \ +cpBody *a_var, *b_var; { \ + a_var = ((cpConstraint *)constraint)->a; \ + b_var = ((cpConstraint *)constraint)->b; \ +} + +static inline cpVect +relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ + cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); + cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); + + return cpvsub(v2_sum, v1_sum); +} + +static inline cpFloat +normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ + return cpvdot(relative_velocity(a, b, r1, r2), n); +} + +static inline void +apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + cpBodyApplyImpulse(a, cpvneg(j), r1); + cpBodyApplyImpulse(b, j, r2); +} + +static inline void +apply_bias_impulse(cpBody *body, cpVect j, cpVect r) +{ + body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); + body->w_bias += body->i_inv*cpvcross(r, j); +} + +static inline void +apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + apply_bias_impulse(a, cpvneg(j), r1); + apply_bias_impulse(b, j, r2); +} + +static inline cpVect +clamp_vect(cpVect v, cpFloat len) +{ + return cpvclamp(v, len); +// return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +static inline cpFloat +k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) +{ + cpFloat mass_sum = a->m_inv + b->m_inv; + cpFloat r1cn = cpvcross(r1, n); + cpFloat r2cn = cpvcross(r2, n); + + cpFloat value = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; + cpAssert(value != 0.0, "Unsolvable collision or constraint."); + + return value; +} + +static inline void +k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2) +{ + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + cpFloat k11, k12, k21, k22; + cpFloat m_sum = a->m_inv + b->m_inv; + + // start with I*m_sum + k11 = m_sum; k12 = 0.0f; + k21 = 0.0f; k22 = m_sum; + + // add the influence from r1 + cpFloat a_i_inv = a->i_inv; + cpFloat r1xsq = r1.x * r1.x * a_i_inv; + cpFloat r1ysq = r1.y * r1.y * a_i_inv; + cpFloat r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + cpFloat b_i_inv = b->i_inv; + cpFloat r2xsq = r2.x * r2.x * b_i_inv; + cpFloat r2ysq = r2.y * r2.y * b_i_inv; + cpFloat r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + cpFloat determinant = k11*k22 - k12*k21; + cpAssert(determinant != 0.0, "Unsolvable constraint."); + + cpFloat det_inv = 1.0f/determinant; + *k1 = cpv( k22*det_inv, -k12*det_inv); + *k2 = cpv(-k21*det_inv, k11*det_inv); +} + +static inline cpVect +mult_k(cpVect vr, cpVect k1, cpVect k2) +{ + return cpv(cpvdot(vr, k1), cpvdot(vr, k2)); +} diff --git a/src/helper/chipmunk/cpArbiter.c b/src/helper/chipmunk/cpArbiter.c new file mode 100644 index 000000000..1efcb448e --- /dev/null +++ b/src/helper/chipmunk/cpArbiter.c @@ -0,0 +1,256 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "constraints/util.h" + +cpFloat cp_bias_coef = 0.1f; +cpFloat cp_collision_slop = 0.1f; + +cpContact* +cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash) +{ + con->p = p; + con->n = n; + con->dist = dist; + + con->jnAcc = 0.0f; + con->jtAcc = 0.0f; + con->jBias = 0.0f; + + con->hash = hash; + + return con; +} + +cpVect +cpArbiterTotalImpulse(cpArbiter *arb) +{ + cpContact *contacts = arb->contacts; + cpVect sum = cpvzero; + + for(int i=0, count=arb->numContacts; in, con->jnAcc)); + } + + return sum; +} + +cpVect +cpArbiterTotalImpulseWithFriction(cpArbiter *arb) +{ + cpContact *contacts = arb->contacts; + cpVect sum = cpvzero; + + for(int i=0, count=arb->numContacts; in, cpv(con->jnAcc, con->jtAcc))); + } + + return sum; +} + +cpFloat +cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts) +{ + cpFloat fsum = 0.0f; + cpVect vsum = cpvzero; + + for(int i=0; in, cpv(con->jnAcc, con->jtAcc)); + + fsum += cpvlength(j); + vsum = cpvadd(vsum, j); + } + + cpFloat vmag = cpvlength(vsum); + return (1.0f - vmag/fsum); +} + +void +cpArbiterIgnore(cpArbiter *arb) +{ + arb->state = cpArbiterStateIgnore; +} + +cpArbiter* +cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) +{ + arb->handler = NULL; + arb->swappedColl = cpFalse; + + arb->e = 0.0f; + arb->u = 0.0f; + arb->surface_vr = cpvzero; + + arb->numContacts = 0; + arb->contacts = NULL; + + arb->a = a; + arb->b = b; + + arb->nextA = NULL; + arb->nextB = NULL; + + arb->stamp = 0; + arb->state = cpArbiterStateFirstColl; + + return arb; +} + +void +cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b) +{ + // Arbiters without contact data may exist if a collision function rejected the collision. + if(arb->contacts){ + // Iterate over the possible pairs to look for hash value matches. + for(int i=0; inumContacts; i++){ + cpContact *old = &arb->contacts[i]; + + for(int j=0; jhash == old->hash){ + // Copy the persistant contact information. + new_contact->jnAcc = old->jnAcc; + new_contact->jtAcc = old->jtAcc; + } + } + } + } + + arb->contacts = contacts; + arb->numContacts = numContacts; + + arb->handler = handler; + arb->swappedColl = (a->collision_type != handler->a); + + arb->e = a->e * b->e; + arb->u = a->u * b->u; + arb->surface_vr = cpvsub(a->surface_v, b->surface_v); + + // For collisions between two similar primitive types, the order could have been swapped. + arb->a = a; + arb->b = b; + + // mark it as new if it's been cached + if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl; +} + +void +cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) +{ + cpBody *a = arb->a->body; + cpBody *b = arb->b->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + + // Calculate the offsets. + con->r1 = cpvsub(con->p, a->p); + con->r2 = cpvsub(con->p, b->p); + + // Calculate the mass normal and mass tangent. + con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n); + con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n)); + + // Calculate the target bias velocity. + con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop); + con->jBias = 0.0f; + + // Calculate the target bounce velocity. + con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;//cpvdot(con->n, cpvsub(v2, v1))*e; + } +} + +void +cpArbiterApplyCachedImpulse(cpArbiter *arb) +{ + cpShape *shapea = arb->a; + cpShape *shapeb = arb->b; + + arb->u = shapea->u * shapeb->u; + arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v); + + cpBody *a = shapea->body; + cpBody *b = shapeb->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + apply_impulses(a, b, con->r1, con->r2, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc))); + } +} + +void +cpArbiterApplyImpulse(cpArbiter *arb) +{ + cpBody *a = arb->a->body; + cpBody *b = arb->b->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + cpVect n = con->n; + cpVect r1 = con->r1; + cpVect r2 = con->r2; + + // Calculate the relative bias velocities. + cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); + cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); + cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); + + // Calculate and clamp the bias impulse. + cpFloat jbn = (con->bias - vbn)*con->nMass; + cpFloat jbnOld = con->jBias; + con->jBias = cpfmax(jbnOld + jbn, 0.0f); + jbn = con->jBias - jbnOld; + + // Apply the bias impulse. + apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn)); + + // Calculate the relative velocity. + cpVect vr = relative_velocity(a, b, r1, r2); + cpFloat vrn = cpvdot(vr, n); + + // Calculate and clamp the normal impulse. + cpFloat jn = -(con->bounce + vrn)*con->nMass; + cpFloat jnOld = con->jnAcc; + con->jnAcc = cpfmax(jnOld + jn, 0.0f); + jn = con->jnAcc - jnOld; + + // Calculate the relative tangent velocity. + cpFloat vrt = cpvdot(cpvadd(vr, arb->surface_vr), cpvperp(n)); + + // Calculate and clamp the friction impulse. + cpFloat jtMax = arb->u*con->jnAcc; + cpFloat jt = -vrt*con->tMass; + cpFloat jtOld = con->jtAcc; + con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); + jt = con->jtAcc - jtOld; + + // Apply the final impulse. + apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt))); + } +} diff --git a/src/helper/chipmunk/cpArbiter.h b/src/helper/chipmunk/cpArbiter.h new file mode 100644 index 000000000..1432cd980 --- /dev/null +++ b/src/helper/chipmunk/cpArbiter.h @@ -0,0 +1,181 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Determines how fast penetrations resolve themselves expressed as a percentage per step. Defaults to 0.1. +extern cpFloat cp_bias_coef; + +// Amount of allowed penetration. Used to reduce oscillating contacts and keep the collision cache warm. Defaults to 0.1. +extern cpFloat cp_collision_slop; + +// User collision handler function types. +typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, cpSpace *space, void *data); +typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, cpSpace *space, void *data); +typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, cpSpace *space, void *data); +typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, cpSpace *space, void *data); + +// Structure for holding collision handler function callback information. +typedef struct cpCollisionHandler { + cpCollisionType a; + cpCollisionType b; + cpCollisionBeginFunc begin; + cpCollisionPreSolveFunc preSolve; + cpCollisionPostSolveFunc postSolve; + cpCollisionSeparateFunc separate; + void *data; +} cpCollisionHandler; + +// Data structure for contact points. +typedef struct cpContact { + // Contact point and normal. + cpVect CP_PRIVATE(p), CP_PRIVATE(n); + // Penetration distance. + CP_PRIVATE(cpFloat dist); + + // Calculated by cpArbiterPreStep(). + cpVect CP_PRIVATE(r1), CP_PRIVATE(r2); + cpFloat CP_PRIVATE(nMass), CP_PRIVATE(tMass), CP_PRIVATE(bounce); + + // Persistant contact information. + cpFloat CP_PRIVATE(jnAcc), CP_PRIVATE(jtAcc), CP_PRIVATE(jBias); + CP_PRIVATE(cpFloat bias); + + // Hash value used to (mostly) uniquely identify a contact. + CP_PRIVATE(cpHashValue hash); +} cpContact; + +#define CP_MAX_CONTACTS_PER_ARBITER 4 + +typedef enum cpArbiterState { + cpArbiterStateNormal, + cpArbiterStateFirstColl, + cpArbiterStateIgnore, + cpArbiterStateCached, +} cpArbiterState; + +// Data structure for tracking collisions between shapes. +struct cpArbiter { + // Information on the contact points between the objects. + CP_PRIVATE(int numContacts); + CP_PRIVATE(cpContact *contacts); + + // The two shapes and bodies involved in the collision. + // These variables are NOT in the order defined by the collision handler. + cpShape CP_PRIVATE(*a), CP_PRIVATE(*b); + cpArbiter CP_PRIVATE(*nextA), CP_PRIVATE(*nextB); + + // Calculated before calling the pre-solve collision handler + // Override them with custom values if you want specialized behavior + CP_PRIVATE(cpFloat e); + CP_PRIVATE(cpFloat u); + // Used for surface_v calculations, implementation may change + CP_PRIVATE(cpVect surface_vr); + + // Time stamp of the arbiter. (from cpSpace) + CP_PRIVATE(cpTimestamp stamp); + + CP_PRIVATE(cpCollisionHandler *handler); + + // Are the shapes swapped in relation to the collision handler? + CP_PRIVATE(cpBool swappedColl); + CP_PRIVATE(cpArbiterState state); +}; + +// Arbiter Helper Functions +cpVect cpArbiterTotalImpulse(cpArbiter *arb); +cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb); +void cpArbiterIgnore(cpArbiter *arb); + + +static inline void +cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) +{ + if(arb->CP_PRIVATE(swappedColl)){ + (*a) = arb->CP_PRIVATE(b), (*b) = arb->CP_PRIVATE(a); + } else { + (*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b); + } +} +#define CP_ARBITER_GET_SHAPES(arb, a, b) cpShape *a, *b; cpArbiterGetShapes(arb, &a, &b); + +static inline void +cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) +{ + CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b); + (*a) = shape_a->body; + (*b) = shape_b->body; +} +#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b); + +static inline cpBool +cpArbiterIsFirstContact(const cpArbiter *arb) +{ + return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl; +} + +static inline int +cpArbiterGetCount(const cpArbiter *arb) +{ + return arb->CP_PRIVATE(numContacts); +} + +static inline cpVect +cpArbiterGetNormal(const cpArbiter *arb, int i) +{ + cpVect n = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); + return arb->CP_PRIVATE(swappedColl) ? cpvneg(n) : n; +} + +static inline cpVect +cpArbiterGetPoint(const cpArbiter *arb, int i) +{ + return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p); +} + +static inline cpFloat +cpArbiteGetDepth(const cpArbiter *arb, int i) +{ + return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); +} + +typedef struct cpContactPointSet { + int count; + + struct { + cpVect point, normal; + cpFloat dist; + } points[CP_MAX_CONTACTS_PER_ARBITER]; +} cpContactPointSet; + +static inline cpContactPointSet +cpArbiterGetContactPointSet(const cpArbiter *arb) +{ + cpContactPointSet set; + set.count = cpArbiterGetCount(arb); + + int i; + for(i=0; iCP_PRIVATE(contacts)[i].CP_PRIVATE(p); + set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); + set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); + } + + return set; +} diff --git a/src/helper/chipmunk/cpArray.c b/src/helper/chipmunk/cpArray.c new file mode 100644 index 000000000..787d62577 --- /dev/null +++ b/src/helper/chipmunk/cpArray.c @@ -0,0 +1,140 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" + + +cpArray* +cpArrayAlloc(void) +{ + return (cpArray *)cpcalloc(1, sizeof(cpArray)); +} + +cpArray* +cpArrayInit(cpArray *arr, int size) +{ + arr->num = 0; + arr->max = (size ? size : 4); + arr->arr = (void **)cpmalloc(arr->max*sizeof(void**)); + + return arr; +} + +cpArray* +cpArrayNew(int size) +{ + return cpArrayInit(cpArrayAlloc(), size); +} + +void +cpArrayDestroy(cpArray *arr) +{ + cpfree(arr->arr); + arr->arr = NULL; +} + +void +cpArrayFree(cpArray *arr) +{ + if(arr){ + cpArrayDestroy(arr); + cpfree(arr); + } +} + +void +cpArrayPush(cpArray *arr, void *object) +{ + if(arr->num == arr->max){ + arr->max *= 2; + arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**)); + } + + arr->arr[arr->num] = object; + arr->num++; +} + +void * +cpArrayPop(cpArray *arr) +{ + arr->num--; + + void *value = arr->arr[arr->num]; + arr->arr[arr->num] = NULL; + + return value; +} + +//static void +//cpArrayDeleteIndex(cpArray *arr, int idx) +//{ +// arr->num--; +// +// arr->arr[idx] = arr->arr[arr->num]; +// arr->arr[arr->num] = NULL; +//} + +void +cpArrayDeleteObj(cpArray *arr, void *obj) +{ + for(int i=0; inum; i++){ + if(arr->arr[i] == obj){ + arr->num--; + + arr->arr[i] = arr->arr[arr->num]; + arr->arr[arr->num] = NULL; + + return; + } + } +} + +//void +//cpArrayAppend(cpArray *arr, cpArray *other) +//{ +// void *tail = &arr->arr[arr->num]; +// +// arr->num += other->num; +// if(arr->num >= arr->max){ +// arr->max = arr->num; +// arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**)); +// } +// +// memcpy(tail, other->arr, other->num*sizeof(void**)); +//} + +void +cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)) +{ + for(int i=0; inum; i++) freeFunc(arr->arr[i]); +} + +cpBool +cpArrayContains(cpArray *arr, void *ptr) +{ + for(int i=0; inum; i++) + if(arr->arr[i] == ptr) return cpTrue; + + return cpFalse; +} diff --git a/src/helper/chipmunk/cpBB.c b/src/helper/chipmunk/cpBB.c new file mode 100644 index 000000000..6418e0310 --- /dev/null +++ b/src/helper/chipmunk/cpBB.c @@ -0,0 +1,47 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + +cpVect +cpBBClampVect(const cpBB bb, const cpVect v) +{ + cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r); + cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t); + return cpv(x, y); +} + +cpVect +cpBBWrapVect(const cpBB bb, const cpVect v) +{ + cpFloat ix = cpfabs(bb.r - bb.l); + cpFloat modx = cpfmod(v.x - bb.l, ix); + cpFloat x = (modx > 0.0f) ? modx : modx + ix; + + cpFloat iy = cpfabs(bb.t - bb.b); + cpFloat mody = cpfmod(v.y - bb.b, iy); + cpFloat y = (mody > 0.0f) ? mody : mody + iy; + + return cpv(x + bb.l, y + bb.b); +} diff --git a/src/helper/chipmunk/cpBB.h b/src/helper/chipmunk/cpBB.h new file mode 100644 index 000000000..300f8cd54 --- /dev/null +++ b/src/helper/chipmunk/cpBB.h @@ -0,0 +1,102 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// TODO fix naming +typedef struct cpBB{ + cpFloat l, b, r ,t; +} cpBB; + +static inline cpBB +cpBBNew(const cpFloat l, const cpFloat b, + const cpFloat r, const cpFloat t) +{ + cpBB bb = {l, b, r, t}; + return bb; +} + +static inline cpBool +cpBBintersects(const cpBB a, const cpBB b) +{ + return (a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t); +} + +static inline cpBool +cpBBcontainsBB(const cpBB bb, const cpBB other) +{ + return (bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t); +} + +static inline cpBool +cpBBcontainsVect(const cpBB bb, const cpVect v) +{ + return (bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y); +} + +static inline cpBB +cpBBmerge(const cpBB a, const cpBB b){ + return cpBBNew( + cpfmin(a.l, b.l), + cpfmin(a.b, b.b), + cpfmax(a.r, b.r), + cpfmax(a.t, b.t) + ); +} + +static inline cpBB +cpBBexpand(const cpBB bb, const cpVect v){ + return cpBBNew( + cpfmin(bb.l, v.x), + cpfmin(bb.b, v.y), + cpfmax(bb.r, v.x), + cpfmax(bb.t, v.y) + ); +} + +static inline cpFloat +cpBBArea(cpBB bb) +{ + return (bb.r - bb.l)*(bb.t - bb.b); +} + +static inline cpFloat +cpBBMergedArea(cpBB a, cpBB b) +{ + return (cpfmax(a.r, b.r) - cpfmin(a.l, b.l))*(cpfmax(a.t, b.t) - cpfmin(a.b, b.b)); +} + +static inline cpBool +cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b) +{ + cpBB seg_bb = cpBBNew(cpfmin(a.x, b.x), cpfmin(a.y, b.y), cpfmax(a.x, b.x), cpfmax(a.y, b.y)); + if(cpBBintersects(bb, seg_bb)){ + cpVect axis = cpv(b.y - a.y, a.x - b.x); + cpVect offset = cpv((a.x + b.x - bb.r - bb.l), (a.y + b.y - bb.t - bb.b)); + cpVect extents = cpv(bb.r - bb.l, bb.t - bb.b); + + return (cpfabs(cpvdot(axis, offset)) < cpfabs(axis.x*extents.x) + cpfabs(axis.y*extents.y)); + } + + return cpFalse; +} + +cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox +// TODO edge case issue +cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox diff --git a/src/helper/chipmunk/cpBBTree.c b/src/helper/chipmunk/cpBBTree.c new file mode 100644 index 000000000..f6f78e85a --- /dev/null +++ b/src/helper/chipmunk/cpBBTree.c @@ -0,0 +1,847 @@ +#include "stdlib.h" +#include "stdio.h" + +#include "chipmunk_private.h" + +static cpSpatialIndexClass klass; + +typedef struct Node Node; +typedef struct Pair Pair; + +struct cpBBTree { + cpSpatialIndex spatialIndex; + cpBBTreeVelocityFunc velocityFunc; + + cpHashSet *leaves; + Node *root; + + Node *pooledNodes; + Pair *pooledPairs; + cpArray *allocatedBuffers; + + cpTimestamp stamp; +}; + +struct Node { + void *obj; + cpBB bb; + Node *parent; + + union { + // Internal nodes + struct { Node *a, *b; }; + + // Leaves + struct { + cpTimestamp stamp; + Pair *pairs; + }; + }; +}; + +typedef struct Thread { + Pair *prev; + Node *leaf; + Pair *next; +} Thread; + +struct Pair { Thread a, b; }; + +#pragma mark Misc Functions + +//static inline cpFloat +//cpBBProximity(cpBB a, cpBB b) +//{ +// return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + b.t - b.b - b.t); +//} + +static inline cpBB +GetBB(cpBBTree *tree, void *obj) +{ + cpBB bb = tree->spatialIndex.bbfunc(obj); + + cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc; + if(velocityFunc){ + cpFloat coef = 0.1f; + cpFloat x = (bb.r - bb.l)*coef; + cpFloat y = (bb.t - bb.b)*coef; + + cpVect v = cpvmult(velocityFunc(obj), 0.1f); + return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y)); + } else { + return bb; + } +} + +static inline cpBBTree * +GetTree(cpSpatialIndex *index) +{ + return (index && index->klass == &klass ? (cpBBTree *)index : NULL); +} + +static inline Node * +GetRootIfTree(cpSpatialIndex *index){ + return (index && index->klass == &klass ? ((cpBBTree *)index)->root : NULL); +} + +static inline cpTimestamp +GetStamp(cpBBTree *tree) +{ + cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); + return (dynamicTree ? dynamicTree->stamp : tree->stamp); +} + +static inline void +IncrementStamp(cpBBTree *tree) +{ + cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); + if(dynamicTree){ + dynamicTree->stamp++; + } else { + tree->stamp++; + } +} + +#pragma mark Pair/Thread Functions + +static void +PairRecycle(cpBBTree *tree, Pair *pair) +{ + pair->a.next = tree->pooledPairs; + tree->pooledPairs = pair; +} + +static Pair * +PairFromPool(cpBBTree *tree) +{ + Pair *pair = tree->pooledPairs; + + if(pair){ + tree->pooledPairs = pair->a.next; + return pair; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(Pair); + cpAssert(count, "Buffer size is too small."); + + Pair *buffer = (Pair *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(tree->allocatedBuffers, buffer); + + // push all but the first one, return the first instead + for(int i=1; ia.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev; + } + + if(prev){ + if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next; + } else { + thread.leaf->pairs = next; + } +} + +static void +PairsClear(Node *leaf, cpBBTree *tree) +{ + Pair *pair = leaf->pairs; + leaf->pairs = NULL; + + while(pair){ + if(pair->a.leaf == leaf){ + Pair *next = pair->a.next; + ThreadUnlink(pair->b); + PairRecycle(tree, pair); + pair = next; + } else { + Pair *next = pair->b.next; + ThreadUnlink(pair->a); + PairRecycle(tree, pair); + pair = next; + } + } +} + +static void +PairInsert(Node *a, Node *b, cpBBTree *tree) +{ + Pair *pair = PairFromPool(tree); + Pair *nextA = a->pairs, *nextB = b->pairs; + + if(nextA){ + if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair; + } + + if(nextB){ + if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair; + } + + (*pair) = (Pair){{NULL, a, nextA},{NULL, b, nextB}}; + a->pairs = b->pairs = pair; +} + + +#pragma mark Node Functions + +static void +NodeRecycle(cpBBTree *tree, Node *node) +{ + node->parent = tree->pooledNodes; + tree->pooledNodes = node; +} + +static Node * +NodeFromPool(cpBBTree *tree) +{ + Node *node = tree->pooledNodes; + + if(node){ + tree->pooledNodes = node->parent; + return node; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(Node); + cpAssert(count, "Buffer size is too small."); + + Node *buffer = (Node *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(tree->allocatedBuffers, buffer); + + // push all but the first one, return the first instead + for(int i=1; ia = value; + value->parent = node; +} + +static inline void +NodeSetB(Node *node, Node *value) +{ + node->b = value; + value->parent = node; +} + +static Node * +NodeNew(cpBBTree *tree, Node *a, Node *b) +{ + Node *node = NodeFromPool(tree); + + node->obj = NULL; + node->bb = cpBBmerge(a->bb, b->bb); + node->parent = NULL; + + NodeSetA(node, a); + NodeSetB(node, b); + + return node; +} + +static inline cpBool +NodeIsLeaf(Node *node) +{ + return (node->obj != NULL); +} + +static inline Node * +NodeOther(Node *node, Node *child) +{ + return (node->a == child ? node->b : node->a); +} + +static inline void +NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) +{ + cpAssert(!NodeIsLeaf(parent), "Cannot replace child of a leaf."); + cpAssert(child == parent->a || child == parent->b, "Node is not a child of parent."); + + if(parent->a == child){ + NodeRecycle(tree, parent->a); + NodeSetA(parent, value); + } else { + NodeRecycle(tree, parent->b); + NodeSetB(parent, value); + } + + for(Node *node=parent; node; node = node->parent){ + node->bb = cpBBmerge(node->a->bb, node->b->bb); + } +} + +#pragma mark Subtree Functions + +static Node * +SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree) +{ + if(subtree == NULL){ + return leaf; + } else if(NodeIsLeaf(subtree)){ + return NodeNew(tree, leaf, subtree); + } else { + cpFloat cost_a = cpBBArea(subtree->b->bb) + cpBBMergedArea(subtree->a->bb, leaf->bb); + cpFloat cost_b = cpBBArea(subtree->a->bb) + cpBBMergedArea(subtree->b->bb, leaf->bb); + +// cpFloat cost_a = cpBBProximity(subtree->a->bb, leaf->bb); +// cpFloat cost_b = cpBBProximity(subtree->b->bb, leaf->bb); + + if(cost_b < cost_a){ + NodeSetB(subtree, SubtreeInsert(subtree->b, leaf, tree)); + } else { + NodeSetA(subtree, SubtreeInsert(subtree->a, leaf, tree)); + } + + subtree->bb = cpBBmerge(subtree->bb, leaf->bb); + return subtree; + } +} + +static void +SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +{ + if(cpBBintersects(subtree->bb, bb)){ + if(NodeIsLeaf(subtree)){ + func(obj, subtree->obj, data); + } else { + SubtreeQuery(subtree->a, obj, bb, func, data); + SubtreeQuery(subtree->b, obj, bb, func, data); + } + } +} + +static void +SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpSpatialIndexSegmentQueryCallback func, void *data) +{ + if(cpBBIntersectsSegment(subtree->bb, a, b)){ + if(NodeIsLeaf(subtree)){ + func(obj, subtree->obj, data); + } else { + SubtreeSegmentQuery(subtree->a, obj, a, b, func, data); + SubtreeSegmentQuery(subtree->b, obj, a, b, func, data); + } + } +} + +static void +SubtreeRecycle(cpBBTree *tree, Node *node) +{ + if(!NodeIsLeaf(node)){ + SubtreeRecycle(tree, node->a); + SubtreeRecycle(tree, node->b); + NodeRecycle(tree, node); + } +} + +static inline Node * +SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree) +{ + if(leaf == subtree){ + return NULL; + } else { + Node *parent = leaf->parent; + if(parent == subtree){ + Node *other = NodeOther(subtree, leaf); + other->parent = subtree->parent; + NodeRecycle(tree, subtree); + return other; + } else { + NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree); + return subtree; + } + } +} + +#pragma mark Marking Functions + +typedef struct MarkContext { + cpBBTree *tree; + Node *staticRoot; + cpSpatialIndexQueryCallback func; + void *data; +} MarkContext; + +static void +MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context) +{ + if(cpBBintersects(leaf->bb, subtree->bb)){ + if(NodeIsLeaf(subtree)){ + if(left){ + PairInsert(leaf, subtree, context->tree); + } else { + if(subtree->stamp < leaf->stamp) PairInsert(subtree, leaf, context->tree); + context->func(leaf->obj, subtree->obj, context->data); + } + } else { + MarkLeafQuery(subtree->a, leaf, left, context); + MarkLeafQuery(subtree->b, leaf, left, context); + } + } +} + +static void +MarkLeaf(Node *leaf, MarkContext *context) +{ + cpBBTree *tree = context->tree; + if(leaf->stamp == GetStamp(tree)){ + Node *staticRoot = context->staticRoot; + if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context); + + for(Node *node = leaf; node->parent; node = node->parent){ + if(node == node->parent->a){ + MarkLeafQuery(node->parent->b, leaf, cpTrue, context); + } else { + MarkLeafQuery(node->parent->a, leaf, cpFalse, context); + } + } + } else { + Pair *pair = leaf->pairs; + while(pair){ + if(leaf == pair->b.leaf){ + context->func(pair->a.leaf->obj, leaf->obj, context->data); + pair = pair->b.next; + } else { + pair = pair->a.next; + } + } + } +} + +static void +MarkSubtree(Node *subtree, MarkContext *context) +{ + if(NodeIsLeaf(subtree)){ + MarkLeaf(subtree, context); + } else { + MarkSubtree(subtree->a, context); + MarkSubtree(subtree->b, context); + } +} + +#pragma mark Leaf Functions + +static Node * +LeafNew(cpBBTree *tree, void *obj, cpBB bb) +{ + Node *node = NodeFromPool(tree); + node->obj = obj; + node->bb = GetBB(tree, obj); + + node->parent = NULL; + node->stamp = 0; + node->pairs = NULL; + + return node; +} + +static cpBool +LeafUpdate(Node *leaf, cpBBTree *tree) +{ + Node *root = tree->root; + cpBB bb = tree->spatialIndex.bbfunc(leaf->obj); + + if(!cpBBcontainsBB(leaf->bb, bb)){ + leaf->bb = GetBB(tree, leaf->obj); + + root = SubtreeRemove(root, leaf, tree); + tree->root = SubtreeInsert(root, leaf, tree); + + PairsClear(leaf, tree); + leaf->stamp = GetStamp(tree); + + return cpTrue; + } + + return cpFalse; +} + +static void VoidQueryCallback(void *obj1, void *obj2, void *data){} + +static void +LeafAddPairs(Node *leaf, cpBBTree *tree) +{ + cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex; + if(dynamicIndex){ + cpBBTree *dynamicTree = GetTree(dynamicIndex); + Node *dynamicRoot = dynamicTree->root; + if(dynamicRoot){ + MarkContext context = {dynamicTree, NULL, NULL, NULL}; + MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context); + } + } else { + Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex); + MarkContext context = {tree, staticRoot, VoidQueryCallback, NULL}; + MarkLeaf(leaf, &context); + } +} + +#pragma mark Memory Management Functions + +cpBBTree * +cpBBTreeAlloc(void) +{ + return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree)); +} + +static int +leafSetEql(void *obj, Node *node) +{ + return (obj == node->obj); +} + +static void * +leafSetTrans(void *obj, cpBBTree *tree) +{ + return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj)); +} + +cpSpatialIndex * +cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + cpSpatialIndexInit((cpSpatialIndex *)tree, &klass, bbfunc, staticIndex); + + tree->velocityFunc = NULL; + + tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql, NULL); + tree->root = NULL; + + tree->pooledNodes = NULL; + tree->allocatedBuffers = cpArrayNew(0); + + tree->stamp = 0; + + return (cpSpatialIndex *)tree; +} + +void +cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func) +{ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index."); + return; + } + + ((cpBBTree *)index)->velocityFunc = func; +} + +cpSpatialIndex * +cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex); +} + +static void +cpBBTreeDestroy(cpBBTree *tree) +{ + cpHashSetFree(tree->leaves); + + cpArrayFreeEach(tree->allocatedBuffers, cpfree); + cpArrayFree(tree->allocatedBuffers); +} + +#pragma mark Insert/Remove + +static void +cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + Node *leaf = cpHashSetInsert(tree->leaves, hashid, obj, tree, (cpHashSetTransFunc)leafSetTrans); + + Node *root = tree->root; + tree->root = SubtreeInsert(root, leaf, tree); + + leaf->stamp = GetStamp(tree); + LeafAddPairs(leaf, tree); + IncrementStamp(tree); +} + +static void +cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + Node *leaf = cpHashSetRemove(tree->leaves, hashid, obj); + + tree->root = SubtreeRemove(tree->root, leaf, tree); + PairsClear(leaf, tree); + NodeRecycle(tree, leaf); +} + +static cpBool +cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + return (cpHashSetFind(tree->leaves, hashid, obj) != NULL); +} + +#pragma mark Reindex + +static void +cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryCallback func, void *data) +{ + if(!tree->root) return; + + // LeafUpdate() may modify tree->root. Don't cache it. + cpHashSetEach(tree->leaves, (cpHashSetIterFunc)LeafUpdate, tree); + + cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex; + Node *staticRoot = (staticIndex && staticIndex->klass == &klass ? ((cpBBTree *)staticIndex)->root : NULL); + + MarkContext context = {tree, staticRoot, func, data}; + MarkSubtree(tree->root, &context); + if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data); + + IncrementStamp(tree); +} + +static void +cpBBTreeReindex(cpBBTree *tree) +{ + cpBBTreeReindexQuery(tree, VoidQueryCallback, NULL); +} + +static void +cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid) +{ + Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj); + if(leaf){ + if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree); + IncrementStamp(tree); + } +} + +#pragma mark Query + +static void +cpBBTreePointQuery(cpBBTree *tree, cpVect point, cpSpatialIndexQueryCallback func, void *data) +{ + Node *root = tree->root; + if(root) SubtreeQuery(root, &point, cpBBNew(point.x, point.y, point.x, point.y), func, data); +} + +static void +cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data) +{ + Node *root = tree->root; + if(root) SubtreeSegmentQuery(root, obj, a, b, func, data); +} + +static void +cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +{ + if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data); +} + +#pragma mark Misc + +static int +cpBBTreeCount(cpBBTree *tree) +{ + return cpHashSetCount(tree->leaves); +} + +typedef struct eachContext { + cpSpatialIndexIterator func; + void *data; +} eachContext; + +static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);} + +static void +cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIterator func, void *data) +{ + eachContext context = {func, data}; + cpHashSetEach(tree->leaves, (cpHashSetIterFunc)each_helper, &context); +} + +static cpSpatialIndexClass klass = { + (cpSpatialIndexDestroyFunc)cpBBTreeDestroy, + + (cpSpatialIndexCountFunc)cpBBTreeCount, + (cpSpatialIndexEachFunc)cpBBTreeEach, + + (cpSpatialIndexContainsFunc)cpBBTreeContains, + (cpSpatialIndexInsertFunc)cpBBTreeInsert, + (cpSpatialIndexRemoveFunc)cpBBTreeRemove, + + (cpSpatialIndexReindexFunc)cpBBTreeReindex, + (cpSpatialIndexReindexObjectFunc)cpBBTreeReindexObject, + (cpSpatialIndexReindexQueryFunc)cpBBTreeReindexQuery, + + (cpSpatialIndexPointQueryFunc)cpBBTreePointQuery, + (cpSpatialIndexSegmentQueryFunc)cpBBTreeSegmentQuery, + (cpSpatialIndexQueryFunc)cpBBTreeQuery, +}; + +#pragma mark Tree Optimization + +static int +cpfcompare(const cpFloat *a, const cpFloat *b){ + return (*a < *b ? -1 : (*b < *a ? 1 : 0)); +} + +static void +fillNodeArray(Node *node, Node ***cursor){ + (**cursor) = node; + (*cursor)++; +} + +static Node * +partitionNodes(cpBBTree *tree, Node **nodes, int count) +{ + if(count == 1){ + return nodes[0]; + } else if(count == 2) { + return NodeNew(tree, nodes[0], nodes[1]); + } + + // Find the AABB for these nodes + cpBB bb = nodes[0]->bb; + for(int i=1; ibb); + + // Split it on it's longest axis + cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b); + + // Sort the bounds and use the median as the splitting point + cpFloat bounds[count*2]; + if(splitWidth){ + for(int i=0; ibb.l; + bounds[2*i + 1] = nodes[i]->bb.r; + } + } else { + for(int i=0; ibb.b; + bounds[2*i + 1] = nodes[i]->bb.t; + } + } + + qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare); + cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split + + // Generate the child BBs + cpBB a = bb, b = bb; + if(splitWidth) a.r = b.l = split; else a.t = b.b = split; + + // Partition the nodes + int right = count; + for(int left=0; left < right;){ + Node *node = nodes[left]; + if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){ +// if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){ + right--; + nodes[left] = nodes[right]; + nodes[right] = node; + } else { + left++; + } + } + + if(right == count){ + Node *node = NULL; + for(int i=0; iroot; +// Node *node = root; +// int bit = 0; +// unsigned int path = tree->opath; +// +// while(!NodeIsLeaf(node)){ +// node = (path&(1<a : node->b); +// bit = (bit + 1)&(sizeof(unsigned int)*8 - 1); +// } +// +// root = subtreeRemove(root, node, tree); +// tree->root = subtreeInsert(root, node, tree); +// } +//} + +void +cpBBTreeOptimize(cpSpatialIndex *index) +{ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index."); + return; + } + + cpBBTree *tree = (cpBBTree *)index; + Node *root = tree->root; + if(!root) return; + + int count = cpBBTreeCount(tree); + Node *nodes[count]; + Node **cursor = nodes; + + cpHashSetEach(tree->leaves, (cpHashSetIterFunc)fillNodeArray, &cursor); + + SubtreeRecycle(tree, root); + tree->root = partitionNodes(tree, nodes, count); +} + +#pragma mark Debug Draw + +#define CP_BBTREE_DEBUG_DRAW +#ifdef CP_BBTREE_DEBUG_DRAW +#include +#include +#include + +static void +NodeRender(Node *node, int depth) +{ + if(!NodeIsLeaf(node) && depth <= 10){ + NodeRender(node->a, depth + 1); + NodeRender(node->b, depth + 1); + } + + cpBB bb = node->bb; + +// GLfloat v = depth/2.0f; +// glColor3f(1.0f - v, v, 0.0f); + glLineWidth(cpfmax(5.0f - depth, 1.0f)); + glBegin(GL_LINES); { + glVertex2f(bb.l, bb.b); + glVertex2f(bb.l, bb.t); + + glVertex2f(bb.l, bb.t); + glVertex2f(bb.r, bb.t); + + glVertex2f(bb.r, bb.t); + glVertex2f(bb.r, bb.b); + + glVertex2f(bb.r, bb.b); + glVertex2f(bb.l, bb.b); + }; glEnd(); +} + +void +cpBBTreeRenderDebug(cpSpatialIndex *index){ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index."); + return; + } + + cpBBTree *tree = (cpBBTree *)index; + if(tree->root) NodeRender(tree->root, 0); +} +#endif diff --git a/src/helper/chipmunk/cpBody.c b/src/helper/chipmunk/cpBody.c new file mode 100644 index 000000000..c3caeda0c --- /dev/null +++ b/src/helper/chipmunk/cpBody.c @@ -0,0 +1,166 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include "chipmunk_private.h" + +// initialized in cpInitChipmunk() +cpBody cpStaticBodySingleton; + +cpBody* +cpBodyAlloc(void) +{ + return (cpBody *)cpmalloc(sizeof(cpBody)); +} + +cpBodyVelocityFunc cpBodyUpdateVelocityDefault = cpBodyUpdateVelocity; +cpBodyPositionFunc cpBodyUpdatePositionDefault = cpBodyUpdatePosition; + +cpBody* +cpBodyInit(cpBody *body, cpFloat m, cpFloat i) +{ + body->velocity_func = cpBodyUpdateVelocityDefault; + body->position_func = cpBodyUpdatePositionDefault; + + cpBodySetMass(body, m); + cpBodySetMoment(body, i); + + body->p = cpvzero; + body->v = cpvzero; + body->f = cpvzero; + + cpBodySetAngle(body, 0.0f); + body->w = 0.0f; + body->t = 0.0f; + + body->v_bias = cpvzero; + body->w_bias = 0.0f; + + body->data = NULL; + body->v_limit = (cpFloat)INFINITY; + body->w_limit = (cpFloat)INFINITY; + + body->space = NULL; + body->shapeList = NULL; + body->arbiterList = NULL; + body->constraintList = NULL; + + cpComponentNode node = {NULL, NULL, 0.0f}; + body->node = node; + + return body; +} + +cpBody* +cpBodyNew(cpFloat m, cpFloat i) +{ + return cpBodyInit(cpBodyAlloc(), m, i); +} + +cpBody * +cpBodyInitStatic(cpBody *body) +{ + cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY); + body->node.idleTime = (cpFloat)INFINITY; + + return body; +} + +cpBody * +cpBodyNewStatic() +{ + return cpBodyInitStatic(cpBodyAlloc()); +} + +void cpBodyDestroy(cpBody *body){} + +void +cpBodyFree(cpBody *body) +{ + if(body){ + cpBodyDestroy(body); + cpfree(body); + } +} + +void +cpBodySetMass(cpBody *body, cpFloat mass) +{ + body->m = mass; + body->m_inv = 1.0f/mass; +} + +void +cpBodySetMoment(cpBody *body, cpFloat moment) +{ + body->i = moment; + body->i_inv = 1.0f/moment; +} + +void +cpBodySetAngle(cpBody *body, cpFloat angle) +{ + body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f); + body->rot = cpvforangle(angle); +} + +void +cpBodySlew(cpBody *body, cpVect pos, cpFloat dt) +{ + cpVect delta = cpvsub(pos, body->p); + body->v = cpvmult(delta, 1.0f/dt); +} + +void +cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) +{ + body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), body->v_limit); + + cpFloat w_limit = body->w_limit; + body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit); +} + +void +cpBodyUpdatePosition(cpBody *body, cpFloat dt) +{ + body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); + cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt); + + body->v_bias = cpvzero; + body->w_bias = 0.0f; +} + +void +cpBodyResetForces(cpBody *body) +{ + body->f = cpvzero; + body->t = 0.0f; +} + +void +cpBodyApplyForce(cpBody *body, cpVect force, cpVect r) +{ + body->f = cpvadd(body->f, force); + body->t += cpvcross(r, force); +} diff --git a/src/helper/chipmunk/cpBody.h b/src/helper/chipmunk/cpBody.h new file mode 100644 index 000000000..a75aca980 --- /dev/null +++ b/src/helper/chipmunk/cpBody.h @@ -0,0 +1,212 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef void (*cpBodyVelocityFunc)(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +typedef void (*cpBodyPositionFunc)(cpBody *body, cpFloat dt); + +extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault; +extern cpBodyPositionFunc cpBodyUpdatePositionDefault; + +// Structure to hold information about the contact graph components +// when putting groups of objects to sleep. +// No interesting user accessible fields. +typedef struct cpComponentNode { + cpBody *root; + cpBody *next; + cpFloat idleTime; +} cpComponentNode; + +struct cpBody { + // *** Integration Functions. + + // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) + cpBodyVelocityFunc velocity_func; + + // Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) + cpBodyPositionFunc position_func; + + // *** Mass Properties + + // Mass and it's inverse. + // Always use cpBodySetMass() whenever changing the mass as these values must agree. + cpFloat m, m_inv; + + // Moment of inertia and it's inverse. + // Always use cpBodySetMoment() whenever changing the moment as these values must agree. + cpFloat i, i_inv; + + // *** Positional Properties + + // Linear components of motion (position, velocity, and force) + cpVect p, v, f; + + // Angular components of motion (angle, angular velocity, and torque) + // Always use cpBodySetAngle() to set the angle of the body as a and rot must agree. + cpFloat a, w, t; + + // Cached unit length vector representing the angle of the body. + // Used for fast vector rotation using cpvrotate(). + cpVect rot; + + // *** User Definable Fields + + // User defined data pointer. + cpDataPointer data; + + // *** Other Fields + + // Maximum velocities this body can move at after integrating velocity + cpFloat v_limit, w_limit; + + // *** Internally Used Fields + + // Velocity bias values used when solving penetrations and correcting constraints. + CP_PRIVATE(cpVect v_bias); + CP_PRIVATE(cpFloat w_bias); + + // Space this body has been added to + CP_PRIVATE(cpSpace *space); + + // Pointer to the shape list. + // Shapes form a linked list using cpShape.next when added to a space. + CP_PRIVATE(cpShape *shapeList); + CP_PRIVATE(cpArbiter *arbiterList); + CP_PRIVATE(cpConstraint *constraintList); + + // Used by cpSpaceStep() to store contact graph information. + CP_PRIVATE(cpComponentNode node); +}; + +// Basic allocation/destruction functions +cpBody *cpBodyAlloc(void); +cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); +cpBody *cpBodyNew(cpFloat m, cpFloat i); + +cpBody *cpBodyInitStatic(cpBody *body); +cpBody *cpBodyNewStatic(); + +void cpBodyDestroy(cpBody *body); +void cpBodyFree(cpBody *body); + +// Wake up a sleeping or idle body. (defined in cpSpace.c) +void cpBodyActivate(cpBody *body); + +// Force a body to sleep; +// defined in cpSpaceComponent.c +void cpBodySleep(cpBody *body); +void cpBodySleepWithGroup(cpBody *body, cpBody *group); + +static inline cpBool +cpBodyIsSleeping(const cpBody *body) +{ + return (CP_PRIVATE(body->node).root != ((cpBody*)0)); +} + +static inline cpBool +cpBodyIsStatic(const cpBody *body) +{ + return CP_PRIVATE(body->node).idleTime == INFINITY; +} + +static inline cpBool +cpBodyIsRogue(const cpBody *body) +{ + return (body->CP_PRIVATE(space) == ((cpSpace*)0)); +} + + +#define CP_DefineBodyGetter(type, member, name) \ +static inline type cpBodyGet##name(const cpBody *body){return body->member;} + +#define CP_DefineBodySetter(type, member, name) \ +static inline void \ +cpBodySet##name(cpBody *body, const type value){ \ + cpBodyActivate(body); \ + body->member = value; \ +} \ + +#define CP_DefineBodyProperty(type, member, name) \ +CP_DefineBodyGetter(type, member, name) \ +CP_DefineBodySetter(type, member, name) + + +// Accessors for cpBody struct members +CP_DefineBodyGetter(cpFloat, m, Mass); +void cpBodySetMass(cpBody *body, cpFloat m); + +CP_DefineBodyGetter(cpFloat, i, Moment); +void cpBodySetMoment(cpBody *body, cpFloat i); + + +CP_DefineBodyProperty(cpVect, p, Pos); +CP_DefineBodyProperty(cpVect, v, Vel); +CP_DefineBodyProperty(cpVect, f, Force); +CP_DefineBodyGetter(cpFloat, a, Angle); +void cpBodySetAngle(cpBody *body, cpFloat a); +CP_DefineBodyProperty(cpFloat, w, AngVel); +CP_DefineBodyProperty(cpFloat, t, Torque); +CP_DefineBodyGetter(cpVect, rot, Rot); +CP_DefineBodyProperty(cpFloat, v_limit, VelLimit); +CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit); + +// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep. +// Intended for objects that are moved manually with a custom velocity integration function. +void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt); + +// Default Integration functions. +void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +void cpBodyUpdatePosition(cpBody *body, cpFloat dt); + +// Convert body local to world coordinates +static inline cpVect +cpBodyLocal2World(const cpBody *body, const cpVect v) +{ + return cpvadd(body->p, cpvrotate(v, body->rot)); +} + +// Convert world to body local coordinates +static inline cpVect +cpBodyWorld2Local(const cpBody *body, const cpVect v) +{ + return cpvunrotate(cpvsub(v, body->p), body->rot); +} + +// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). +static inline void +cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r) +{ + body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); + body->w += body->i_inv*cpvcross(r, j); +} + +// Zero the forces on a body. +void cpBodyResetForces(cpBody *body); +// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates). +void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); + +static inline cpFloat +cpBodyKineticEnergy(const cpBody *body) +{ + // Need to do some fudging to avoid NaNs + cpFloat vsq = cpvdot(body->v, body->v); + cpFloat wsq = body->w*body->w; + return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f); +} diff --git a/src/helper/chipmunk/cpCollision.c b/src/helper/chipmunk/cpCollision.c new file mode 100644 index 000000000..a67ba25e4 --- /dev/null +++ b/src/helper/chipmunk/cpCollision.c @@ -0,0 +1,411 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +//#include + +#include "chipmunk_private.h" + +typedef int (*collisionFunc)(const cpShape *, const cpShape *, cpContact *); + +// Add contact points for circle to circle collisions. +// Used by several collision tests. +static int +circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con) +{ + cpFloat mindist = r1 + r2; + cpVect delta = cpvsub(p2, p1); + cpFloat distsq = cpvlengthsq(delta); + if(distsq >= mindist*mindist) return 0; + + cpFloat dist = cpfsqrt(distsq); + + // Allocate and initialize the contact. + cpContactInit( + con, + cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))), + (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)), + dist - mindist, + 0 + ); + + return 1; +} + +// Collide circle shapes. +static int +circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr) +{ + cpCircleShape *circ1 = (cpCircleShape *)shape1; + cpCircleShape *circ2 = (cpCircleShape *)shape2; + + return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr); +} + +// Collide circles to segment shapes. +static int +circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con) +{ + cpCircleShape *circ = (cpCircleShape *)circleShape; + cpSegmentShape *seg = (cpSegmentShape *)segmentShape; + + // Radius sum + cpFloat rsum = circ->r + seg->r; + + // Calculate normal distance from segment. + cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn); + cpFloat dist = cpfabs(dn) - rsum; + if(dist > 0.0f) return 0; + + // Calculate tangential distance along segment. + cpFloat dt = -cpvcross(seg->tn, circ->tc); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + // Decision tree to decide which feature of the segment to collide with. + if(dt < dtMin){ + if(dt < (dtMin - rsum)){ + return 0; + } else { + return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con); + } + } else { + if(dt < dtMax){ + cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn); + cpContactInit( + con, + cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)), + n, + dist, + 0 + ); + return 1; + } else { + if(dt < (dtMax + rsum)) { + return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con); + } else { + return 0; + } + } + } + + return 1; +} + +// Helper function for working with contact buffers +// This used to malloc/realloc memory on the fly but was repurposed. +static cpContact * +nextContactPoint(cpContact *arr, int *numPtr) +{ + int index = *numPtr; + + if(index < CP_MAX_CONTACTS_PER_ARBITER){ + (*numPtr) = index + 1; + return &arr[index]; + } else { + return &arr[CP_MAX_CONTACTS_PER_ARBITER - 1]; + } +} + +// Find the minimum separating axis for the give poly and axis list. +static inline int +findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out) +{ + int min_index = 0; + cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d); + if(min > 0.0f) return -1; + + for(int i=1; i 0.0f) { + return -1; + } else if(dist > min){ + min = dist; + min_index = i; + } + } + + (*min_out) = min; + return min_index; +} + +// Add contacts for probably penetrating vertexes. +// This handles the degenerate case where an overlap was detected, but no vertexes fall inside +// the opposing polygon. (like a star of david) +static inline int +findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) +{ + int num = 0; + + for(int i=0; inumVerts; i++){ + cpVect v = poly1->tVerts[i]; + if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) + cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); + } + + for(int i=0; inumVerts; i++){ + cpVect v = poly2->tVerts[i]; + if(cpPolyShapeContainsVertPartial(poly1, v, n)) + cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); + } + + return num; +} + +// Add contacts for penetrating vertexes. +static inline int +findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) +{ + int num = 0; + + for(int i=0; inumVerts; i++){ + cpVect v = poly1->tVerts[i]; + if(cpPolyShapeContainsVert(poly2, v)) + cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i)); + } + + for(int i=0; inumVerts; i++){ + cpVect v = poly2->tVerts[i]; + if(cpPolyShapeContainsVert(poly1, v)) + cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i)); + } + + return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist)); +} + +// Collide poly shapes together. +static int +poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) +{ + cpPolyShape *poly1 = (cpPolyShape *)shape1; + cpPolyShape *poly2 = (cpPolyShape *)shape2; + + cpFloat min1; + int mini1 = findMSA(poly2, poly1->tAxes, poly1->numVerts, &min1); + if(mini1 == -1) return 0; + + cpFloat min2; + int mini2 = findMSA(poly1, poly2->tAxes, poly2->numVerts, &min2); + if(mini2 == -1) return 0; + + // There is overlap, find the penetrating verts + if(min1 > min2) + return findVerts(arr, poly1, poly2, poly1->tAxes[mini1].n, min1); + else + return findVerts(arr, poly1, poly2, cpvneg(poly2->tAxes[mini2].n), min2); +} + +// Like cpPolyValueOnAxis(), but for segments. +static inline cpFloat +segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const cpFloat d) +{ + cpFloat a = cpvdot(n, seg->ta) - seg->r; + cpFloat b = cpvdot(n, seg->tb) - seg->r; + return cpfmin(a, b) - d; +} + +// Identify vertexes that have penetrated the segment. +static inline void +findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const cpFloat coef) +{ + cpFloat dta = cpvcross(seg->tn, seg->ta); + cpFloat dtb = cpvcross(seg->tn, seg->tb); + cpVect n = cpvmult(seg->tn, coef); + + for(int i=0; inumVerts; i++){ + cpVect v = poly->tVerts[i]; + if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){ + cpFloat dt = cpvcross(seg->tn, v); + if(dta >= dt && dt >= dtb){ + cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly->shape.hashid, i)); + } + } + } +} + +// This one is complicated and gross. Just don't go there... +// TODO: Comment me! +static int +seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape1; + cpPolyShape *poly = (cpPolyShape *)shape2; + cpPolyShapeAxis *axes = poly->tAxes; + + cpFloat segD = cpvdot(seg->tn, seg->ta); + cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; + cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r; + if(minNeg > 0.0f || minNorm > 0.0f) return 0; + + int mini = 0; + cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d); + if(poly_min > 0.0f) return 0; + for(int i=0; inumVerts; i++){ + cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); + if(dist > 0.0f){ + return 0; + } else if(dist > poly_min){ + poly_min = dist; + mini = i; + } + } + + int num = 0; + + cpVect poly_n = cpvneg(axes[mini].n); + + cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); + cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); + if(cpPolyShapeContainsVert(poly, va)) + cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 0)); + if(cpPolyShapeContainsVert(poly, vb)) + cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 1)); + + // Floating point precision problems here. + // This will have to do for now. + poly_min -= cp_collision_slop; + if(minNorm >= poly_min || minNeg >= poly_min) { + if(minNorm > minNeg) + findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f); + else + findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f); + } + + // If no other collision points are found, try colliding endpoints. + if(num == 0){ + cpVect poly_a = poly->tVerts[mini]; + cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts]; + + if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) + return 1; + + if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) + return 1; + + if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) + return 1; + + if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) + return 1; + } + + return num; +} + +// This one is less gross, but still gross. +// TODO: Comment me! +static int +circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) +{ + cpCircleShape *circ = (cpCircleShape *)shape1; + cpPolyShape *poly = (cpPolyShape *)shape2; + cpPolyShapeAxis *axes = poly->tAxes; + + int mini = 0; + cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r; + for(int i=0; inumVerts; i++){ + cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r; + if(dist > 0.0f){ + return 0; + } else if(dist > min) { + min = dist; + mini = i; + } + } + + cpVect n = axes[mini].n; + cpVect a = poly->tVerts[mini]; + cpVect b = poly->tVerts[(mini + 1)%poly->numVerts]; + cpFloat dta = cpvcross(n, a); + cpFloat dtb = cpvcross(n, b); + cpFloat dt = cpvcross(n, circ->tc); + + if(dt < dtb){ + return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con); + } else if(dt < dta) { + cpContactInit( + con, + cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)), + cpvneg(n), + min, + 0 + ); + + return 1; + } else { + return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con); + } +} + +//static const collisionFunc builtinCollisionFuncs[9] = { +// circle2circle, +// NULL, +// NULL, +// circle2segment, +// NULL, +// NULL, +// circle2poly, +// seg2poly, +// poly2poly, +//}; +//static const collisionFunc *colfuncs = builtinCollisionFuncs; + +static collisionFunc *colfuncs = NULL; + +static void +addColFunc(const cpShapeType a, const cpShapeType b, const collisionFunc func) +{ + colfuncs[a + b*CP_NUM_SHAPES] = func; +} + +#ifdef __cplusplus +extern "C" { +#endif + void cpInitCollisionFuncs(void); + + // Initializes the array of collision functions. + // Called by cpInitChipmunk(). + void + cpInitCollisionFuncs(void) + { + if(!colfuncs) + colfuncs = (collisionFunc *)cpcalloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc)); + + addColFunc(CP_CIRCLE_SHAPE, CP_CIRCLE_SHAPE, circle2circle); + addColFunc(CP_CIRCLE_SHAPE, CP_SEGMENT_SHAPE, circle2segment); + addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE, seg2poly); + addColFunc(CP_CIRCLE_SHAPE, CP_POLY_SHAPE, circle2poly); + addColFunc(CP_POLY_SHAPE, CP_POLY_SHAPE, poly2poly); + } +#ifdef __cplusplus +} +#endif + +int +cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr) +{ + // Their shape types must be in order. + cpAssert(a->klass->type <= b->klass->type, "Collision shapes passed to cpCollideShapes() are not sorted."); + + collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES]; + return (cfunc) ? cfunc(a, b, arr) : 0; +} diff --git a/src/helper/chipmunk/cpHashSet.c b/src/helper/chipmunk/cpHashSet.c new file mode 100644 index 000000000..dc2d4d4c1 --- /dev/null +++ b/src/helper/chipmunk/cpHashSet.c @@ -0,0 +1,265 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" +#include "prime.h" + +typedef struct cpHashSetBin { + void *elt; + cpHashValue hash; + struct cpHashSetBin *next; +} cpHashSetBin; + +struct cpHashSet { + int entries, size; + + cpHashSetEqlFunc eql; + void *default_value; + + cpHashSetBin **table; + cpHashSetBin *pooledBins; + + cpArray *allocatedBuffers; +}; + +void +cpHashSetDestroy(cpHashSet *set) +{ + cpfree(set->table); + + cpArrayFreeEach(set->allocatedBuffers, cpfree); + cpArrayFree(set->allocatedBuffers); +} + +void +cpHashSetFree(cpHashSet *set) +{ + if(set){ + cpHashSetDestroy(set); + cpfree(set); + } +} + +cpHashSet * +cpHashSetAlloc(void) +{ + return (cpHashSet *)cpcalloc(1, sizeof(cpHashSet)); +} + +cpHashSet * +cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, void *default_value) +{ + set->size = next_prime(size); + set->entries = 0; + + set->eql = eqlFunc; + set->default_value = default_value; + + set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *)); + set->pooledBins = NULL; + + set->allocatedBuffers = cpArrayNew(0); + + return set; +} + +cpHashSet * +cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, void *default_value) +{ + return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, default_value); +} + +static int +setIsFull(cpHashSet *set) +{ + return (set->entries >= set->size); +} + +static void +cpHashSetResize(cpHashSet *set) +{ + // Get the next approximate doubled prime. + int newSize = next_prime(set->size + 1); + // Allocate a new table. + cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *)); + + // Iterate over the chains. + for(int i=0; isize; i++){ + // Rehash the bins into the new table. + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + + int idx = bin->hash%newSize; + bin->next = newTable[idx]; + newTable[idx] = bin; + + bin = next; + } + } + + cpfree(set->table); + + set->table = newTable; + set->size = newSize; +} + +static inline void +recycleBin(cpHashSet *set, cpHashSetBin *bin) +{ + bin->next = set->pooledBins; + set->pooledBins = bin; + bin->elt = NULL; +} + +static cpHashSetBin * +getUnusedBin(cpHashSet *set) +{ + cpHashSetBin *bin = set->pooledBins; + + if(bin){ + set->pooledBins = bin->next; + return bin; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin); + cpAssert(count, "Buffer size is too small."); + + cpHashSetBin *buffer = (cpHashSetBin *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(set->allocatedBuffers, buffer); + + // push all but the first one, return it instead + for(int i=1; ientries; +} + +void * +cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans) +{ + int idx = hash%set->size; + + // Find the bin with the matching element. + cpHashSetBin *bin = set->table[idx]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + // Create it if necessary. + if(!bin){ + bin = getUnusedBin(set); + bin->hash = hash; + bin->elt = (trans ? trans(ptr, data) : data); + + bin->next = set->table[idx]; + set->table[idx] = bin; + + set->entries++; + if(setIsFull(set)) cpHashSetResize(set); + } + + return bin->elt; +} + +void * +cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr) +{ + int idx = hash%set->size; + + cpHashSetBin **prev_ptr = &set->table[idx]; + cpHashSetBin *bin = set->table[idx]; + + // Find the bin + while(bin && !set->eql(ptr, bin->elt)){ + prev_ptr = &bin->next; + bin = bin->next; + } + + // Remove it if it exists. + if(bin){ + // Update the previous linked list pointer + (*prev_ptr) = bin->next; + set->entries--; + + void *elt = bin->elt; + recycleBin(set, bin); + + return elt; + } + + return NULL; +} + +void * +cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) +{ + int idx = hash%set->size; + cpHashSetBin *bin = set->table[idx]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + return (bin ? bin->elt : set->default_value); +} + +void +cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data) +{ + for(int i=0; isize; i++){ + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + func(bin->elt, data); + bin = next; + } + } +} + +void +cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data) +{ + for(int i=0; isize; i++){ + // The rest works similarly to cpHashSetRemove() above. + cpHashSetBin **prev_ptr = &set->table[i]; + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + + if(func(bin->elt, data)){ + prev_ptr = &bin->next; + } else { + (*prev_ptr) = next; + + set->entries--; + recycleBin(set, bin); + } + + bin = next; + } + } +} diff --git a/src/helper/chipmunk/cpPolyShape.c b/src/helper/chipmunk/cpPolyShape.c new file mode 100644 index 000000000..4cffb56b5 --- /dev/null +++ b/src/helper/chipmunk/cpPolyShape.c @@ -0,0 +1,235 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" +#include "chipmunk_unsafe.h" + +cpPolyShape * +cpPolyShapeAlloc(void) +{ + return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape)); +} + +static cpBB +cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpVect *src = poly->verts; + cpVect *dst = poly->tVerts; + + cpFloat l = INFINITY, r = -INFINITY; + cpFloat b = INFINITY, t = -INFINITY; + + for(int i=0; inumVerts; i++){ + cpVect v = cpvadd(p, cpvrotate(src[i], rot)); + + dst[i] = v; + l = cpfmin(l, v.x); + r = cpfmax(r, v.x); + b = cpfmin(b, v.y); + t = cpfmax(t, v.y); + } + + return cpBBNew(l, b, r, t); +} + +static void +cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpPolyShapeAxis *src = poly->axes; + cpPolyShapeAxis *dst = poly->tAxes; + + for(int i=0; inumVerts; i++){ + cpVect n = cpvrotate(src[i].n, rot); + dst[i].n = n; + dst[i].d = cpvdot(p, n) + src[i].d; + } +} + +static cpBB +cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + + cpPolyShapeTransformAxes(poly, p, rot); + cpBB bb = shape->bb = cpPolyShapeTransformVerts(poly, p, rot); + + return bb; +} + +static void +cpPolyShapeDestroy(cpShape *shape) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + + cpfree(poly->verts); + cpfree(poly->tVerts); + + cpfree(poly->axes); + cpfree(poly->tAxes); +} + +static cpBool +cpPolyShapePointQuery(cpShape *shape, cpVect p){ + return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p); +} + +static void +cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + cpPolyShapeAxis *axes = poly->tAxes; + cpVect *verts = poly->tVerts; + int numVerts = poly->numVerts; + + for(int i=0; i an) continue; + + cpFloat bn = cpvdot(b, n); + cpFloat t = (axes[i].d - an)/(bn - an); + if(t < 0.0f || 1.0f < t) continue; + + cpVect point = cpvlerp(a, b, t); + cpFloat dt = -cpvcross(n, point); + cpFloat dtMin = -cpvcross(n, verts[i]); + cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]); + + if(dtMin <= dt && dt <= dtMax){ + info->shape = shape; + info->t = t; + info->n = n; + } + } +} + +static const cpShapeClass polyClass = { + CP_POLY_SHAPE, + cpPolyShapeCacheData, + cpPolyShapeDestroy, + cpPolyShapePointQuery, + cpPolyShapeSegmentQuery, +}; + +cpBool +cpPolyValidate(const cpVect *verts, const int numVerts) +{ + for(int i=0; i 0.0f) + return cpFalse; + } + + return cpTrue; +} + +int +cpPolyShapeGetNumVerts(cpShape *shape) +{ + cpAssert(shape->klass == &polyClass, "Shape is not a poly shape."); + return ((cpPolyShape *)shape)->numVerts; +} + +cpVect +cpPolyShapeGetVert(cpShape *shape, int idx) +{ + cpAssert(shape->klass == &polyClass, "Shape is not a poly shape."); + cpAssert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range."); + + return ((cpPolyShape *)shape)->verts[idx]; +} + + +static void +setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset) +{ + poly->numVerts = numVerts; + + poly->verts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect)); + poly->tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect)); + poly->axes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis)); + poly->tAxes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis)); + + for(int i=0; iverts[i] = a; + poly->axes[i].n = n; + poly->axes[i].d = cpvdot(n, a); + } +} + +cpPolyShape * +cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset) +{ + // Fail if the user attempts to pass a concave poly, or a bad winding. + cpAssert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding."); + + setUpVerts(poly, numVerts, verts, offset); + cpShapeInit((cpShape *)poly, &polyClass, body); + + return poly; +} + +cpShape * +cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset) +{ + return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, numVerts, verts, offset); +} + +cpPolyShape * +cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height) +{ + cpFloat hw = width/2.0f; + cpFloat hh = height/2.0f; + + cpVect verts[] = { + cpv(-hw,-hh), + cpv(-hw, hh), + cpv( hw, hh), + cpv( hw,-hh), + }; + + return cpPolyShapeInit(poly, body, 4, verts, cpvzero); +} + +cpShape * +cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height) +{ + return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height); +} + +// Unsafe API (chipmunk_unsafe.h) + +void +cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset) +{ + cpAssert(shape->klass == &polyClass, "Shape is not a poly shape."); + cpPolyShapeDestroy(shape); + setUpVerts((cpPolyShape *)shape, numVerts, verts, offset); +} diff --git a/src/helper/chipmunk/cpPolyShape.h b/src/helper/chipmunk/cpPolyShape.h new file mode 100644 index 000000000..16a704cb9 --- /dev/null +++ b/src/helper/chipmunk/cpPolyShape.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Axis structure used by cpPolyShape. +typedef struct cpPolyShapeAxis { + // normal + cpVect n; + // distance from origin + cpFloat d; +} cpPolyShapeAxis; + +// Convex polygon shape structure. +typedef struct cpPolyShape { + CP_PRIVATE(cpShape shape); + + // Vertex and axis lists. + CP_PRIVATE(int numVerts); + CP_PRIVATE(cpVect *verts); + CP_PRIVATE(cpPolyShapeAxis *axes); + + // Transformed vertex and axis lists. + CP_PRIVATE(cpVect *tVerts); + CP_PRIVATE(cpPolyShapeAxis *tAxes); +} cpPolyShape; + +// Basic allocation functions. +cpPolyShape *cpPolyShapeAlloc(void); +cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset); +cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset); + +cpPolyShape *cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height); +cpShape *cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height); + +// Check that a set of vertexes has a correct winding and that they are convex +cpBool cpPolyValidate(const cpVect *verts, const int numVerts); + +int cpPolyShapeGetNumVerts(cpShape *shape); +cpVect cpPolyShapeGetVert(cpShape *shape, int idx); diff --git a/src/helper/chipmunk/cpShape.c b/src/helper/chipmunk/cpShape.c new file mode 100644 index 000000000..a16370a91 --- /dev/null +++ b/src/helper/chipmunk/cpShape.c @@ -0,0 +1,393 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include "chipmunk_private.h" +#include "chipmunk_unsafe.h" + +#define CP_DefineShapeGetter(struct, type, member, name) \ +CP_DeclareShapeGetter(struct, type, name){ \ + cpAssert(shape->klass == &struct##Class, "shape is not a "#struct); \ + return ((struct *)shape)->member; \ +} +cpHashValue SHAPE_ID_COUNTER = 0; + +void +cpResetShapeIdCounter(void) +{ + SHAPE_ID_COUNTER = 0; +} + + +cpShape* +cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) +{ + shape->klass = klass; + + shape->hashid = SHAPE_ID_COUNTER; + SHAPE_ID_COUNTER++; + + shape->body = body; + shape->sensor = 0; + + shape->e = 0.0f; + shape->u = 0.0f; + shape->surface_v = cpvzero; + + shape->collision_type = 0; + shape->group = CP_NO_GROUP; + shape->layers = CP_ALL_LAYERS; + + shape->data = NULL; + shape->next = NULL; + +// cpShapeCacheBB(shape); + + return shape; +} + +void +cpShapeDestroy(cpShape *shape) +{ + if(shape->klass->destroy) shape->klass->destroy(shape); +} + +void +cpShapeFree(cpShape *shape) +{ + if(shape){ + cpShapeDestroy(shape); + cpfree(shape); + } +} + +cpBB +cpShapeCacheBB(cpShape *shape) +{ + cpBody *body = shape->body; + return cpShapeUpdate(shape, body->p, body->rot); +} + +cpBB +cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot) +{ + return (shape->bb = shape->klass->cacheData(shape, pos, rot)); +} + +cpBool +cpShapePointQuery(cpShape *shape, cpVect p){ + return shape->klass->pointQuery(shape, p); +} + +cpBool +cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){ + cpSegmentQueryInfo blank = {NULL, 0.0f, cpvzero}; + (*info) = blank; + + shape->klass->segmentQuery(shape, a, b, info); + return (info->shape != NULL); +} + +cpCircleShape * +cpCircleShapeAlloc(void) +{ + return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape)); +} + +static inline cpBB +bbFromCircle(const cpVect c, const cpFloat r) +{ + return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r); +} + +static cpBB +cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); + return bbFromCircle(circle->tc, circle->r); +} + +static cpBool +cpCircleShapePointQuery(cpShape *shape, cpVect p){ + cpCircleShape *circle = (cpCircleShape *)shape; + return cpvnear(circle->tc, p, circle->r); +} + +static void +circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + // offset the line to be relative to the circle + a = cpvsub(a, center); + b = cpvsub(b, center); + + cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b); + cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b); + cpFloat qc = cpvdot(a, a) - r*r; + + cpFloat det = qb*qb - 4.0f*qa*qc; + + if(det >= 0.0f){ + cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa); + if(0.0f<= t && t <= 1.0f){ + info->shape = shape; + info->t = t; + info->n = cpvnormalize(cpvlerp(a, b, t)); + } + } +} + +static void +cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + cpCircleShape *circle = (cpCircleShape *)shape; + circleSegmentQuery(shape, circle->tc, circle->r, a, b, info); +} + +static const cpShapeClass cpCircleShapeClass = { + CP_CIRCLE_SHAPE, + cpCircleShapeCacheData, + NULL, + cpCircleShapePointQuery, + cpCircleShapeSegmentQuery, +}; + +cpCircleShape * +cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) +{ + circle->c = offset; + circle->r = radius; + + cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body); + + return circle; +} + +cpShape * +cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) +{ + return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); +} + +CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset) +CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius) + +cpSegmentShape * +cpSegmentShapeAlloc(void) +{ + return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape)); +} + +static cpBB +cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->ta = cpvadd(p, cpvrotate(seg->a, rot)); + seg->tb = cpvadd(p, cpvrotate(seg->b, rot)); + seg->tn = cpvrotate(seg->n, rot); + + cpFloat l,r,s,t; + + if(seg->ta.x < seg->tb.x){ + l = seg->ta.x; + r = seg->tb.x; + } else { + l = seg->tb.x; + r = seg->ta.x; + } + + if(seg->ta.y < seg->tb.y){ + s = seg->ta.y; + t = seg->tb.y; + } else { + s = seg->tb.y; + t = seg->ta.y; + } + + cpFloat rad = seg->r; + return cpBBNew(l - rad, s - rad, r + rad, t + rad); +} + +static cpBool +cpSegmentShapePointQuery(cpShape *shape, cpVect p){ + if(!cpBBcontainsVect(shape->bb, p)) return cpFalse; + + cpSegmentShape *seg = (cpSegmentShape *)shape; + + // Calculate normal distance from segment. + cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); + cpFloat dist = cpfabs(dn) - seg->r; + if(dist > 0.0f) return cpFalse; + + // Calculate tangential distance along segment. + cpFloat dt = -cpvcross(seg->tn, p); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + // Decision tree to decide which feature of the segment to collide with. + if(dt <= dtMin){ + if(dt < (dtMin - seg->r)){ + return cpFalse; + } else { + return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r); + } + } else { + if(dt < dtMax){ + return cpTrue; + } else { + if(dt < (dtMax + seg->r)) { + return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r); + } else { + return cpFalse; + } + } + } + + return cpTrue; +} + +static inline cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);} + +static void +cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + // TODO this function could be optimized better. + + cpSegmentShape *seg = (cpSegmentShape *)shape; + cpVect n = seg->tn; + // flip n if a is behind the axis + if(cpvdot(a, n) < cpvdot(seg->ta, n)) + n = cpvneg(n); + + cpFloat an = cpvdot(a, n); + cpFloat bn = cpvdot(b, n); + + if(an != bn){ + cpFloat d = cpvdot(seg->ta, n) + seg->r; + cpFloat t = (d - an)/(bn - an); + + if(0.0f < t && t < 1.0f){ + cpVect point = cpvlerp(a, b, t); + cpFloat dt = -cpvcross(seg->tn, point); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + if(dtMin < dt && dt < dtMax){ + info->shape = shape; + info->t = t; + info->n = n; + + return; // don't continue on and check endcaps + } + } + } + + if(seg->r) { + cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero}; + cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero}; + circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1); + circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2); + + if(info1.t < info2.t){ + (*info) = info1; + } else { + (*info) = info2; + } + } +} + +static const cpShapeClass cpSegmentShapeClass = { + CP_SEGMENT_SHAPE, + cpSegmentShapeCacheData, + NULL, + cpSegmentShapePointQuery, + cpSegmentShapeSegmentQuery, +}; + +cpSegmentShape * +cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + seg->a = a; + seg->b = b; + seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); + + seg->r = r; + + cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body); + + return seg; +} + +cpShape* +cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); +} + +CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A) +CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B) +CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal) +CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius) + +// Unsafe API (chipmunk_unsafe.h) + +void +cpCircleShapeSetRadius(cpShape *shape, cpFloat radius) +{ + cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->r = radius; +} + +void +cpCircleShapeSetOffset(cpShape *shape, cpVect offset) +{ + cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->c = offset; +} + +void +cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b) +{ + cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->a = a; + seg->b = b; + seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); +} + +void +cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius) +{ + cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->r = radius; +} diff --git a/src/helper/chipmunk/cpShape.h b/src/helper/chipmunk/cpShape.h new file mode 100644 index 000000000..c5bc238c5 --- /dev/null +++ b/src/helper/chipmunk/cpShape.h @@ -0,0 +1,174 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Forward declarations required for defining other structs. +typedef struct cpShapeClass cpShapeClass; + +typedef struct cpSegmentQueryInfo { + cpShape *shape; // shape that was hit, NULL if no collision + cpFloat t; // Distance along query segment, will always be in the range [0, 1]. + cpVect n; // normal of hit surface +} cpSegmentQueryInfo; + +// Enumeration of shape types. +typedef enum cpShapeType{ + CP_CIRCLE_SHAPE, + CP_SEGMENT_SHAPE, + CP_POLY_SHAPE, + CP_NUM_SHAPES +} cpShapeType; + +// Shape class. Holds function pointers and type data. +struct cpShapeClass { + cpShapeType type; + + // Called by cpShapeCacheBB(). + cpBB (*cacheData)(cpShape *shape, cpVect p, cpVect rot); + // Called to by cpShapeDestroy(). + void (*destroy)(cpShape *shape); + + // called by cpShapePointQuery(). + cpBool (*pointQuery)(cpShape *shape, cpVect p); + + // called by cpShapeSegmentQuery() + void (*segmentQuery)(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); +}; + +// Basic shape struct that the others inherit from. +struct cpShape { + // The "class" of a shape as defined above + CP_PRIVATE(const cpShapeClass *klass); + + // cpBody that the shape is attached to. + cpBody *body; + + // Cached BBox for the shape. + cpBB bb; + + // Sensors invoke callbacks, but do not generate collisions + cpBool sensor; + + // *** Surface properties. + + // Coefficient of restitution. (elasticity) + cpFloat e; + // Coefficient of friction. + cpFloat u; + // Surface velocity used when solving for friction. + cpVect surface_v; + + // *** User Definable Fields + + // User defined data pointer for the shape. + cpDataPointer data; + + // User defined collision type for the shape. + cpCollisionType collision_type; + // User defined collision group for the shape. + cpGroup group; + // User defined layer bitmask for the shape. + cpLayers layers; + + // *** Internally Used Fields + + // Shapes form a linked list when added to space on a non-NULL body + CP_PRIVATE(cpShape *next); + + // Unique id used as the hash value. + CP_PRIVATE(cpHashValue hashid); +}; + +// Low level shape initialization func. +cpShape* cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body); + +// Basic destructor functions. (allocation functions are not shared) +void cpShapeDestroy(cpShape *shape); +void cpShapeFree(cpShape *shape); + +// Cache the BBox of the shape. +cpBB cpShapeCacheBB(cpShape *shape); +cpBB cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot); + +// Test if a point lies within a shape. +cpBool cpShapePointQuery(cpShape *shape, cpVect p); + +#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape) + +// Circle shape structure. +typedef struct cpCircleShape { + CP_PRIVATE(cpShape shape); + + // Center in body space coordinates + CP_PRIVATE(cpVect c); + // Radius. + CP_PRIVATE(cpFloat r); + + // Transformed center. (world space coordinates) + CP_PRIVATE(cpVect tc); +} cpCircleShape; + +// Basic allocation functions for cpCircleShape. +cpCircleShape *cpCircleShapeAlloc(void); +cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); +cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); + +CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset); +CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius); + +// Segment shape structure. +typedef struct cpSegmentShape { + CP_PRIVATE(cpShape shape); + + // Endpoints and normal of the segment. (body space coordinates) + cpVect CP_PRIVATE(a), CP_PRIVATE(b), CP_PRIVATE(n); + // Radius of the segment. (Thickness) + cpFloat CP_PRIVATE(r); + + // Transformed endpoints and normal. (world space coordinates) + cpVect CP_PRIVATE(ta), CP_PRIVATE(tb), CP_PRIVATE(tn); +} cpSegmentShape; + +// Basic allocation functions for cpSegmentShape. +cpSegmentShape* cpSegmentShapeAlloc(void); +cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); +cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); + +CP_DeclareShapeGetter(cpSegmentShape, cpVect, A); +CP_DeclareShapeGetter(cpSegmentShape, cpVect, B); +CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal); +CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius); + +// For determinism, you can reset the shape id counter. +void cpResetShapeIdCounter(void); + +cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); + +static inline cpVect +cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) +{ + return cpvlerp(start, end, info.t); +} + +static inline cpFloat +cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) +{ + return cpvdist(start, end)*info.t; +} diff --git a/src/helper/chipmunk/cpSpace.c b/src/helper/chipmunk/cpSpace.c new file mode 100644 index 000000000..0d1b819f4 --- /dev/null +++ b/src/helper/chipmunk/cpSpace.c @@ -0,0 +1,522 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include + +#include "chipmunk_private.h" + +cpTimestamp cp_contact_persistence = 3; + +#pragma mark Contact Set Helpers + +// Equal function for contactSet. +static cpBool +contactSetEql(cpShape **shapes, cpArbiter *arb) +{ + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); +} + +#pragma mark Collision Pair Function Helpers + +// Equals function for collFuncSet. +static cpBool +collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) +{ + return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b)); +} + +// Transformation function for collFuncSet. +static void * +collFuncSetTrans(cpCollisionHandler *handler, void *unused) +{ + cpCollisionHandler *copy = (cpCollisionHandler *)cpmalloc(sizeof(cpCollisionHandler)); + (*copy) = (*handler); + + return copy; +} + +#pragma mark Misc Helper Funcs + +// Default collision functions. +static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;} +static void nothing(cpArbiter *arb, cpSpace *space, void *data){} + +// BBfunc callback for the spatial hash. +static cpBB shapeBBFunc(cpShape *shape){return shape->bb;} +static cpVect shapeVelocityFunc(cpShape *shape){return shape->body->v;} + +// Iterator functions for destructors. +static void freeWrap(void *ptr, void *unused){ cpfree(ptr);} +static void shapeFreeWrap(cpShape *ptr, void *unused){ cpShapeFree(ptr);} + +void +cpSpaceLock(cpSpace *space) +{ + space->locked++; +} + +void +cpSpaceUnlock(cpSpace *space) +{ + space->locked--; + cpAssert(space->locked >= 0, "Internal error:Space lock underflow."); + + if(!space->locked){ + cpArray *waking = space->rousedBodies; + for(int i=0, count=waking->num; iarr[i]); + } + + waking->num = 0; + } +} + + +#pragma mark Memory Management Functions + +cpSpace * +cpSpaceAlloc(void) +{ + return (cpSpace *)cpcalloc(1, sizeof(cpSpace)); +} + +#define DEFAULT_DIM_SIZE 100.0f +#define DEFAULT_COUNT 1000 +#define DEFAULT_ITERATIONS 10 +#define DEFAULT_ELASTIC_ITERATIONS 0 + +cpCollisionHandler defaultHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL}; + +cpSpace* +cpSpaceInit(cpSpace *space) +{ + space->iterations = DEFAULT_ITERATIONS; + + space->gravity = cpvzero; + space->damping = 1.0f; + + space->locked = 0; + space->stamp = 0; + + if(0){ + space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpatialIndexBBFunc)shapeBBFunc, NULL); + space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpatialIndexBBFunc)shapeBBFunc, space->staticShapes); + } else { + space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)shapeBBFunc, NULL); + space->activeShapes = cpBBTreeNew((cpSpatialIndexBBFunc)shapeBBFunc, space->staticShapes); + cpBBTreeSetVelocityFunc(space->activeShapes, (cpBBTreeVelocityFunc)shapeVelocityFunc); + } + + space->allocatedBuffers = cpArrayNew(0); + + space->bodies = cpArrayNew(0); + space->sleepingComponents = cpArrayNew(0); + space->rousedBodies = cpArrayNew(0); + + space->sleepTimeThreshold = INFINITY; + space->idleSpeedThreshold = 0.0f; + + space->arbiters = cpArrayNew(0); + space->pooledArbiters = cpArrayNew(0); + + space->contactBuffersHead = NULL; + space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, NULL); + + space->constraints = cpArrayNew(0); + + space->defaultHandler = defaultHandler; + space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, &space->defaultHandler); + + space->postStepCallbacks = NULL; + + cpBodyInitStatic(&space->_staticBody); + space->staticBody = &space->_staticBody; + + return space; +} + +cpSpace* +cpSpaceNew(void) +{ + return cpSpaceInit(cpSpaceAlloc()); +} + +void +cpSpaceDestroy(cpSpace *space) +{ + cpSpatialIndexFree(space->staticShapes); + cpSpatialIndexFree(space->activeShapes); + + cpArrayFree(space->bodies); + cpArrayFree(space->sleepingComponents); + cpArrayFree(space->rousedBodies); + + cpArrayFree(space->constraints); + + cpHashSetFree(space->contactSet); + + cpArrayFree(space->arbiters); + cpArrayFree(space->pooledArbiters); + + if(space->allocatedBuffers){ + cpArrayFreeEach(space->allocatedBuffers, cpfree); + cpArrayFree(space->allocatedBuffers); + } + + if(space->postStepCallbacks){ + cpHashSetEach(space->postStepCallbacks, freeWrap, NULL); + cpHashSetFree(space->postStepCallbacks); + } + + if(space->collFuncSet){ + cpHashSetEach(space->collFuncSet, freeWrap, NULL); + cpHashSetFree(space->collFuncSet); + } +} + +void +cpSpaceFree(cpSpace *space) +{ + if(space){ + cpSpaceDestroy(space); + cpfree(space); + } +} + +void +cpSpaceFreeChildren(cpSpace *space) +{ + cpArray *components = space->sleepingComponents; + while(components->num) cpBodyActivate((cpBody *)components->arr[0]); + + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIterator)&shapeFreeWrap, NULL); + cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIterator)&shapeFreeWrap, NULL); + + cpArrayFreeEach(space->bodies, (void (*)(void*))cpBodyFree); + cpArrayFreeEach(space->constraints, (void (*)(void*))cpConstraintFree); +} + +#pragma mark Collision Handler Function Management + +void +cpSpaceAddCollisionHandler( + cpSpace *space, + cpCollisionType a, cpCollisionType b, + cpCollisionBeginFunc begin, + cpCollisionPreSolveFunc preSolve, + cpCollisionPostSolveFunc postSolve, + cpCollisionSeparateFunc separate, + void *data +){ + // Remove any old function so the new one will get added. + cpSpaceRemoveCollisionHandler(space, a, b); + + cpCollisionHandler handler = { + a, b, + begin ? begin : alwaysCollide, + preSolve ? preSolve : alwaysCollide, + postSolve ? postSolve : nothing, + separate ? separate : nothing, + data + }; + + cpHashSetInsert(space->collFuncSet, CP_HASH_PAIR(a, b), &handler, NULL, (cpHashSetTransFunc)collFuncSetTrans); +} + +void +cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) +{ + struct { cpCollisionType a, b; } ids = {a, b}; + cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collFuncSet, CP_HASH_PAIR(a, b), &ids); + cpfree(old_handler); +} + +void +cpSpaceSetDefaultCollisionHandler( + cpSpace *space, + cpCollisionBeginFunc begin, + cpCollisionPreSolveFunc preSolve, + cpCollisionPostSolveFunc postSolve, + cpCollisionSeparateFunc separate, + void *data +){ + cpCollisionHandler handler = { + 0, 0, + begin ? begin : alwaysCollide, + preSolve ? preSolve : alwaysCollide, + postSolve ? postSolve : nothing, + separate ? separate : nothing, + data + }; + + space->defaultHandler = handler; +} + +#pragma mark Body, Shape, and Joint Management + +#define cpAssertSpaceUnlocked(space) \ + cpAssert(!space->locked, \ + "This addition/removal cannot be done safely during a call to cpSpaceStep() or during a query. " \ + "Put these calls into a post-step callback." \ + ); + +static void +cpBodyRemoveShape(cpBody *body, cpShape *shape) +{ + cpShape **prev_ptr = &body->shapeList; + cpShape *node = body->shapeList; + + while(node && node != shape){ + prev_ptr = &node->next; + node = node->next; + } + + cpAssert(node, "Attempted to remove a shape from a body it was never attached to."); + (*prev_ptr) = node->next; +} + +cpShape * +cpSpaceAddShape(cpSpace *space, cpShape *shape) +{ + cpBody *body = shape->body; + if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape); + + cpAssert(!cpSpatialIndexContains(space->activeShapes, shape, shape->hashid), + "Cannot add the same shape more than once."); + cpAssertSpaceUnlocked(space); + + cpBodyActivate(body); + + // Push onto the head of the body's shape list + shape->next = body->shapeList; body->shapeList = shape; + + cpShapeUpdate(shape, body->p, body->rot); + cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); + + return shape; +} + +cpShape * +cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) +{ + cpAssert(!cpSpatialIndexContains(space->staticShapes, shape, shape->hashid), + "Cannot add the same static shape more than once."); + cpAssertSpaceUnlocked(space); + + cpBody *body = shape->body; + cpShapeUpdate(shape, body->p, body->rot); + cpSpaceActivateShapesTouchingShape(space, shape); + cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); + + return shape; +} + +cpBody * +cpSpaceAddBody(cpSpace *space, cpBody *body) +{ + cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); + cpAssert(!body->space, "Cannot add a body to a more than one space or to the same space twice."); +// cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback + + cpArrayPush(space->bodies, body); + body->space = space; + + return body; +} + +static void +cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint) +{ + cpConstraint **prev_ptr = &body->constraintList; + cpConstraint *node = body->constraintList; + + while(node && node != constraint){ + prev_ptr = (node->a == body ? &node->nextA : &node->nextB); + node = *prev_ptr; + } + + cpAssert(node, "Attempted to remove a constraint from a body it was never attached to."); + (*prev_ptr) = (node->a == body ? node->nextA : node->nextB); +} + + + +cpConstraint * +cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) +{ + cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once."); + cpAssertSpaceUnlocked(space); + + cpBodyActivate(constraint->a); + cpBodyActivate(constraint->b); + cpArrayPush(space->constraints, constraint); + + // Push onto the heads of the bodies' constraint lists + cpBody *a = constraint->a, *b = constraint->b; + constraint->nextA = a->constraintList; a->constraintList = constraint; + constraint->nextB = b->constraintList; b->constraintList = constraint; + + return constraint; +} + +typedef struct removalContext { + cpSpace *space; + cpShape *shape; +} removalContext; + +// Hashset filter func to throw away old arbiters. +static cpBool +contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context) +{ + if(context->shape == arb->a || context->shape == arb->b){ + if(arb->state != cpArbiterStateCached){ + arb->handler->separate(arb, context->space, arb->handler->data); + } + + cpArrayPush(context->space->pooledArbiters, arb); + return cpFalse; + } + + return cpTrue; +} + +void +cpSpaceRemoveShape(cpSpace *space, cpShape *shape) +{ + cpBody *body = shape->body; + if(cpBodyIsStatic(body)){ + cpSpaceRemoveStaticShape(space, shape); + return; + } + + cpBodyActivate(body); + + cpAssert(cpSpatialIndexContains(space->activeShapes, shape, shape->hashid), + "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); + cpAssertSpaceUnlocked(space); + + cpBodyRemoveShape(body, shape); + + removalContext context = {space, shape}; + cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context); + cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); +} + +void +cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) +{ + cpAssert(cpSpatialIndexContains(space->staticShapes, shape, shape->hashid), + "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)"); + cpAssertSpaceUnlocked(space); + + removalContext context = {space, shape}; + cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context); + cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); + + cpSpaceActivateShapesTouchingShape(space, shape); +} + +void +cpSpaceRemoveBody(cpSpace *space, cpBody *body) +{ + cpAssertWarn(body->space == space, + "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); + cpAssertSpaceUnlocked(space); + + cpBodyActivate(body); + cpArrayDeleteObj(space->bodies, body); + body->space = NULL; +} + +void +cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) +{ + cpAssertWarn(cpArrayContains(space->constraints, constraint), + "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); + cpAssertSpaceUnlocked(space); + + cpBodyActivate(constraint->a); + cpBodyActivate(constraint->b); + cpArrayDeleteObj(space->constraints, constraint); + + cpBodyRemoveConstraint(constraint->a, constraint); + cpBodyRemoveConstraint(constraint->b, constraint); +} + +#pragma mark Iteration + +// TODO copy fix from 5.x +void +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) +{ + cpArray *bodies = space->bodies; + + for(int i=0; inum; i++) + func((cpBody *)bodies->arr[i], data); +} + +#pragma mark Spatial Hash Management + +static void +updateBBCache(cpShape *shape, void *unused) +{ + cpBody *body = shape->body; + cpShapeUpdate(shape, body->p, body->rot); +} + +void +cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) +{ +// TODO spatial hash specific + cpSpaceHashResize((cpSpaceHash *)space->staticShapes, dim, count); + cpSpatialIndexReindex(space->staticShapes); +} + +void +cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count) +{ +// TODO spatial hash specific + cpSpaceHashResize((cpSpaceHash *)space->activeShapes, dim, count); +} + +void +cpSpaceRehashStatic(cpSpace *space) +{ + cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIterator)&updateBBCache, NULL); + cpSpatialIndexReindex(space->staticShapes); +} + +void +cpSpaceRehashShape(cpSpace *space, cpShape *shape) +{ + cpBody *body = shape->body; + cpShapeUpdate(shape, body->p, body->rot); + + // attempt to rehash the shape in both hashes + cpSpatialIndexReindexObject(space->activeShapes, shape, shape->hashid); + cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid); +} diff --git a/src/helper/chipmunk/cpSpace.h b/src/helper/chipmunk/cpSpace.h new file mode 100644 index 000000000..5b921060d --- /dev/null +++ b/src/helper/chipmunk/cpSpace.h @@ -0,0 +1,181 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Number of frames that contact information should persist. +extern cpTimestamp cp_contact_persistence; + +typedef struct cpContactBufferHeader cpContactBufferHeader; + +struct cpSpace { + // *** User definable fields + + // Number of iterations to use in the impulse solver to solve contacts. + int iterations; + + // Default gravity to supply when integrating rigid body motions. + cpVect gravity; + + // Default damping to supply when integrating rigid body motions. + cpFloat damping; + + // Speed threshold for a body to be considered idle. + // The default value of 0 means to let the space guess a good threshold based on gravity. + cpFloat idleSpeedThreshold; + + // Time a group of bodies must remain idle in order to fall asleep + // The default value of INFINITY disables the sleeping algorithm. + cpFloat sleepTimeThreshold; + + // *** Internally Used Fields + + // When the space lock count is non zero you cannot add or remove objects + CP_PRIVATE(int locked); + + // Time stamp. Is incremented on every call to cpSpaceStep(). + CP_PRIVATE(cpTimestamp stamp); + + // The static and active shape spatial hashes. + CP_PRIVATE(cpSpatialIndex *staticShapes); + CP_PRIVATE(cpSpatialIndex *activeShapes); + + // List of bodies in the system. + CP_PRIVATE(cpArray *bodies); + + // List of groups of sleeping bodies. + CP_PRIVATE(cpArray *sleepingComponents); + + // List of bodies that have been flagged to be awoken. + CP_PRIVATE(cpArray *rousedBodies); + + // List of active arbiters for the impulse solver. + CP_PRIVATE(cpArray *arbiters); + CP_PRIVATE(cpArray *pooledArbiters); + + // Linked list ring of contact buffers. + // Head is the newest buffer, and each buffer points to a newer buffer. + // Head wraps around and points to the oldest (tail) buffer. + CP_PRIVATE(cpContactBufferHeader *contactBuffersHead); + + // List of buffers to be free()ed when destroying the space. + CP_PRIVATE(cpArray *allocatedBuffers); + + // Persistant contact set. + CP_PRIVATE(cpHashSet *contactSet); + + // List of constraints in the system. + CP_PRIVATE(cpArray *constraints); + + // Set of collisionpair functions. + CP_PRIVATE(cpHashSet *collFuncSet); + // Default collision handler. + CP_PRIVATE(cpCollisionHandler defaultHandler); + + CP_PRIVATE(cpHashSet *postStepCallbacks); + + CP_PRIVATE(cpBody _staticBody); + + cpDataPointer data; + cpBody *staticBody; +}; + +// Basic allocation/destruction functions. +cpSpace* cpSpaceAlloc(void); +cpSpace* cpSpaceInit(cpSpace *space); +cpSpace* cpSpaceNew(void); + +void cpSpaceDestroy(cpSpace *space); +void cpSpaceFree(cpSpace *space); + +// Convenience function. Frees all referenced entities. (bodies, shapes and constraints) +void cpSpaceFreeChildren(cpSpace *space); + +// Collision handler management functions. +void cpSpaceSetDefaultCollisionHandler( + cpSpace *space, + cpCollisionBeginFunc begin, + cpCollisionPreSolveFunc preSolve, + cpCollisionPostSolveFunc postSolve, + cpCollisionSeparateFunc separate, + void *data +); +void cpSpaceAddCollisionHandler( + cpSpace *space, + cpCollisionType a, cpCollisionType b, + cpCollisionBeginFunc begin, + cpCollisionPreSolveFunc preSolve, + cpCollisionPostSolveFunc postSolve, + cpCollisionSeparateFunc separate, + void *data +); +void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b); + +// Add and remove entities from the system. +cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape); +cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); +cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body); +cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); + +void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); +void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); +void cpSpaceRemoveBody(cpSpace *space, cpBody *body); +void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); + +// Post Step function definition +typedef void (*cpPostStepFunc)(cpSpace *space, void *obj, void *data); +// Register a post step function to be called after cpSpaceStep() has finished. +// obj is used a key, you can only register one callback per unique value for obj +void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data); + +// Point query callback function +typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data); +void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data); +cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group); + +// Segment query callback function +typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data); +void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data); +cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out); + +// BB query callback function +typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data); +void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data); + +// Shape query callback function +typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data); +cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data); + + +void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape); + + +// Iterator function for iterating the bodies in a space. +typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data); +void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data); + +// Spatial hash management functions. +void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count); +void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count); +void cpSpaceRehashStatic(cpSpace *space); + +void cpSpaceRehashShape(cpSpace *space, cpShape *shape); + +// Update the space. +void cpSpaceStep(cpSpace *space, cpFloat dt); diff --git a/src/helper/chipmunk/cpSpaceComponent.c b/src/helper/chipmunk/cpSpaceComponent.c new file mode 100644 index 000000000..6a00358e5 --- /dev/null +++ b/src/helper/chipmunk/cpSpaceComponent.c @@ -0,0 +1,290 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include "chipmunk_private.h" + +#pragma mark Sleeping Functions + +void +cpSpaceActivateBody(cpSpace *space, cpBody *body) +{ + if(space->locked){ + // cpSpaceActivateBody() is called again once the space is unlocked + cpArrayPush(space->rousedBodies, body); + } else { + cpArrayPush(space->bodies, body); + + CP_BODY_FOREACH_SHAPE(body, shape){ + cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); + cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); + } + + CP_BODY_FOREACH_ARBITER(body, arb){ + cpBody *bodyA = arb->a->body; + if(body == bodyA || cpBodyIsStatic(bodyA)){ + int numContacts = arb->numContacts; + cpContact *contacts = arb->contacts; + + // Restore contact values back to the space's contact buffer memory + arb->contacts = cpContactBufferGetArray(space); + memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact)); + cpSpacePushContacts(space, numContacts); + + cpShape *a = arb->a, *b = arb->b; + cpShape *shape_pair[] = {a, b}; + cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b); + cpHashSetInsert(space->contactSet, arbHashID, shape_pair, arb, NULL); + cpArrayPush(space->arbiters, arb); + arb->stamp = space->stamp; + + arb->stamp = space->stamp; + cpArrayPush(space->arbiters, arb); + + cpfree(contacts); + } + } + + CP_BODY_FOREACH_CONSTRAINT(body, constraint){ + cpBody *bodyA = constraint->a; + if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayPush(space->constraints, constraint); + } + } +} + +static void +cpSpaceDeactivateBody(cpSpace *space, cpBody *body) +{ + cpArrayDeleteObj(space->bodies, body); + + CP_BODY_FOREACH_SHAPE(body, shape){ + cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); + cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); + } + + CP_BODY_FOREACH_ARBITER(body, arb){ + cpBody *bodyA = arb->a->body; + if(body == bodyA || cpBodyIsStatic(bodyA)){ + cpShape *a = arb->a, *b = arb->b; + cpShape *shape_pair[] = {a, b}; + cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b); + cpHashSetRemove(space->contactSet, arbHashID, shape_pair); + cpArrayDeleteObj(space->arbiters, arb); + + // Save contact values to a new block of memory so they won't time out + size_t bytes = arb->numContacts*sizeof(cpContact); + cpContact *contacts = (cpContact *)cpmalloc(bytes); + memcpy(contacts, arb->contacts, bytes); + arb->contacts = contacts; + } + } + + CP_BODY_FOREACH_CONSTRAINT(body, constraint){ + cpBody *bodyA = constraint->a; + if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayDeleteObj(space->constraints, constraint); + } +} + +static inline void +ComponentActivate(cpBody *root) +{ + if(!root || !cpBodyIsSleeping(root)) return; + cpAssert(!cpBodyIsRogue(root), "Internal Error: ComponentActivate() called on a rogue body."); + + cpSpace *space = root->space; + CP_BODY_FOREACH_GROUP(root, body){ + body->node.root = NULL; + cpSpaceActivateBody(space, body); + } + + cpArrayDeleteObj(space->sleepingComponents, root); +} + +static inline void +ComponentAdd(cpBody *root, cpBody *body){ + body->node.root = root; + + if(body != root){ + body->node.next = root->node.next; + root->node.next = body; + } +} + +void +cpBodyActivate(cpBody *body) +{ + if(!cpBodyIsStatic(body)){ + body->node.idleTime = 0.0f; + ComponentActivate(body->node.root); + } +} + +static inline cpBool +ComponentActive(cpBody *root, cpFloat threshold) +{ + CP_BODY_FOREACH_GROUP(root, body){ + if(body->node.idleTime < threshold) return cpTrue; + } + + return cpFalse; +} + +static inline void +DepthFirstSearch(cpBody *root, cpBody *body) +{ + if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){ + cpBody *other_root = body->node.root; + if(other_root == NULL){ + ComponentAdd(root, body); + CP_BODY_FOREACH_ARBITER(body, arb) DepthFirstSearch(root, (body == arb->a->body ? arb->b->body : arb->a->body)); + CP_BODY_FOREACH_CONSTRAINT(body, constraint) DepthFirstSearch(root, (body == constraint->a ? constraint->b : constraint->a)); + } else if(other_root != root){ + cpAssert(cpFalse, "Uh oh. This shouldn't happen?"); + } + } +} + +static inline void +cpBodyPushArbiter(cpBody *body, cpArbiter *arb) +{ + if(!cpBodyIsStatic(body) && !cpBodyIsRogue(body)){ + if(body == arb->a->body){ + arb->nextA = body->arbiterList; + } else { + arb->nextB = body->arbiterList; + } + + body->arbiterList = arb; + } +} + +// TODO this function needs more commenting. +void +cpSpaceProcessComponents(cpSpace *space, cpFloat dt) +{ + cpFloat dv = space->idleSpeedThreshold; + cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); + + // update idling and reset arbiter list and component nodes + cpArray *bodies = space->bodies; + for(int i=0; inum; i++){ + cpBody *body = (cpBody*)bodies->arr[i]; + + // Need to deal with infinite mass objects + cpFloat thresh = (dvsq ? body->m*dvsq : 0.0f); + body->node.idleTime = (cpBodyKineticEnergy(body) > thresh ? 0.0f : body->node.idleTime + dt); + + body->arbiterList = NULL; + body->node.next = NULL; + } + + // Add arbiters to body lists and awaken any sleeping bodies found + cpArray *arbiters = space->arbiters; + for(int i=0, count=arbiters->num; iarr[i]; + cpBody *a = arb->a->body, *b = arb->b->body; + + if(cpBodyIsSleeping(a) || (cpBodyIsRogue(b) && !cpBodyIsStatic(b))) cpBodyActivate(a); + if(cpBodyIsSleeping(b) || (cpBodyIsRogue(a) && !cpBodyIsStatic(a))) cpBodyActivate(b); + + cpBodyPushArbiter(a, arb); + cpBodyPushArbiter(b, arb); + } + + cpArray *constraints = space->constraints; + for(int i=0; inum; i++){ + cpConstraint *constraint = constraints->arr[i]; + cpBody *a = constraint->a, *b = constraint->b; + + if(cpBodyIsSleeping(a) || (cpBodyIsRogue(b) && !cpBodyIsStatic(b))) cpBodyActivate(a); + if(cpBodyIsSleeping(b) || (cpBodyIsRogue(a) && !cpBodyIsStatic(a))) cpBodyActivate(b); + } + + for(int i=0; inum;){ + cpBody *body = (cpBody*)bodies->arr[i]; + + if(body->node.root == NULL){ + DepthFirstSearch(body, body); + + if(!ComponentActive(body, space->sleepTimeThreshold)){ + cpArrayPush(space->sleepingComponents, body); + CP_BODY_FOREACH_GROUP(body, other) cpSpaceDeactivateBody(space, other); + + // cpSpaceDeactivateBody() removed the current body from the list. + // Skip the index increment and don't reset the root pointer. + continue; + } + } + + i++; + body->node.root = NULL; + } +} + +void +cpBodySleep(cpBody *body) +{ + cpBodySleepWithGroup(body, NULL); +} + +void +cpBodySleepWithGroup(cpBody *body, cpBody *group){ + cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep."); + + cpSpace *space = body->space; + cpAssert(space, "Cannot put a rogue body to sleep."); + cpAssert(!space->locked, "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback."); + cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); + cpAssert(!group || cpBodyIsSleeping(body), "Cannot add a body that is already sleeping to a group."); + + CP_BODY_FOREACH_SHAPE(body, shape) cpShapeUpdate(shape, body->p, body->rot); + cpSpaceDeactivateBody(space, body); + + if(group){ + cpBody *root = group->node.root; + + cpComponentNode node = {root, root->node.next, 0.0f}; + body->node = node; + + root->node.next = body; + } else { + cpComponentNode node = {body, NULL, 0.0f}; + body->node = node; + + cpArrayPush(space->sleepingComponents, body); + } + + cpArrayDeleteObj(space->bodies, body); +} + +static void +activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){ + cpBodyActivate(shape->body); +} + +void +cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){ + cpArray *bodies = NULL; + cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)activateTouchingHelper, &bodies); +} diff --git a/src/helper/chipmunk/cpSpaceHash.c b/src/helper/chipmunk/cpSpaceHash.c new file mode 100644 index 000000000..b6c1abb64 --- /dev/null +++ b/src/helper/chipmunk/cpSpaceHash.c @@ -0,0 +1,646 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk_private.h" +#include "prime.h" + +typedef struct cpSpaceHashBin cpSpaceHashBin; +typedef struct cpHandle cpHandle; + +struct cpSpaceHash { + cpSpatialIndex spatialIndex; + + int numcells; + cpFloat celldim; + + cpSpaceHashBin **table; + cpHashSet *handleSet; + + cpSpaceHashBin *pooledBins; + cpArray *pooledHandles; + cpArray *allocatedBuffers; + + cpTimestamp stamp; +}; + + +#pragma mark Handle Functions + +struct cpHandle { + void *obj; + int retain; + cpTimestamp stamp; +}; + +static cpHandle* +cpHandleInit(cpHandle *hand, void *obj) +{ + hand->obj = obj; + hand->retain = 0; + hand->stamp = 0; + + return hand; +} + +static inline void cpHandleRetain(cpHandle *hand){hand->retain++;} + +static inline void +cpHandleRelease(cpHandle *hand, cpArray *pooledHandles) +{ + hand->retain--; + if(hand->retain == 0) cpArrayPush(pooledHandles, hand); +} + +static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);} + +static void * +handleSetTrans(void *obj, cpSpaceHash *hash) +{ + if(hash->pooledHandles->num == 0){ + // handle pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpHandle); + cpAssert(count, "Buffer size is too small."); + + cpHandle *buffer = (cpHandle *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(hash->allocatedBuffers, buffer); + + for(int i=0; ipooledHandles, buffer + i); + } + + cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj); + cpHandleRetain(hand); + + return hand; +} + +#pragma mark Bin Functions + +struct cpSpaceHashBin { + cpHandle *handle; + cpSpaceHashBin *next; +}; + +static inline void +recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin) +{ + bin->next = hash->pooledBins; + hash->pooledBins = bin; +} + +static inline void +clearTableCell(cpSpaceHash *hash, int idx) +{ + cpSpaceHashBin *bin = hash->table[idx]; + while(bin){ + cpSpaceHashBin *next = bin->next; + + cpHandleRelease(bin->handle, hash->pooledHandles); + recycleBin(hash, bin); + + bin = next; + } + + hash->table[idx] = NULL; +} + +static void +clearTable(cpSpaceHash *hash) +{ + for(int i=0; inumcells; i++) clearTableCell(hash, i); +} + +// Get a recycled or new bin. +static inline cpSpaceHashBin * +getEmptyBin(cpSpaceHash *hash) +{ + cpSpaceHashBin *bin = hash->pooledBins; + + if(bin){ + hash->pooledBins = bin->next; + return bin; + } else { + // Pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin); + cpAssert(count, "Buffer size is too small."); + + cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(hash->allocatedBuffers, buffer); + + // push all but the first one, return the first instead + for(int i=1; itable); + + hash->numcells = numcells; + hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *)); +} + +static cpSpatialIndexClass klass; + +cpSpatialIndex * +cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + cpSpatialIndexInit((cpSpatialIndex *)hash, &klass, bbfunc, staticIndex); + + cpSpaceHashAllocTable(hash, next_prime(numcells)); + hash->celldim = celldim; + + hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql, NULL); + hash->pooledHandles = cpArrayNew(0); + + hash->pooledBins = NULL; + hash->allocatedBuffers = cpArrayNew(0); + + hash->stamp = 1; + + return (cpSpatialIndex *)hash; +} + +cpSpatialIndex * +cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex); +} + +static void +cpSpaceHashDestroy(cpSpaceHash *hash) +{ + clearTable(hash); + + cpHashSetFree(hash->handleSet); + + cpArrayFreeEach(hash->allocatedBuffers, cpfree); + cpArrayFree(hash->allocatedBuffers); + cpArrayFree(hash->pooledHandles); + + cpfree(hash->table); +} + +#pragma mark Helper Functions + +static inline cpBool +containsHandle(cpSpaceHashBin *bin, cpHandle *hand) +{ + while(bin){ + if(bin->handle == hand) return cpTrue; + bin = bin->next; + } + + return cpFalse; +} + +// The hash function itself. +static inline cpHashValue +hash_func(cpHashValue x, cpHashValue y, cpHashValue n) +{ + return (x*1640531513ul ^ y*2654435789ul) % n; +} + +// Much faster than (int)floor(f) +// Profiling showed floor() to be a sizable performance hog +static inline int +floor_int(cpFloat f) +{ + int i = (int)f; + return (f < 0.0f && f != i ? i - 1 : i); +} + +static inline void +hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) +{ + // Find the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = floor_int(bb.l/dim); // Fix by ShiftZ + int r = floor_int(bb.r/dim); + int b = floor_int(bb.b/dim); + int t = floor_int(bb.t/dim); + + int n = hash->numcells; + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int idx = hash_func(i,j,n); + cpSpaceHashBin *bin = hash->table[idx]; + + // Don't add an object twice to the same cell. + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); + // Insert a new bin for the handle in this cell. + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + hash->table[idx] = newBin; + } + } +} + +#pragma mark Basic Operations + +static void +cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, hash, (cpHashSetTransFunc)handleSetTrans); + hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj)); +} + +static void +cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand, hash->pooledHandles); + + cpSpaceHashInsert(hash, obj, hashid); + } +} + +static void +rehash_helper(cpHandle *hand, cpSpaceHash *hash) +{ + hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj)); +} + +static void +cpSpaceHashRehash(cpSpaceHash *hash) +{ + clearTable(hash); + cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)rehash_helper, hash); +} + +static void +cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand, hash->pooledHandles); + } +} + +typedef struct eachContext { + cpSpatialIndexIterator func; + void *data; +} eachContext; + +static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);} + +static void +cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIterator func, void *data) +{ + eachContext context = {func, data}; + cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)eachHelper, &context); +} + +static void +remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) +{ + cpSpaceHashBin *bin = *bin_ptr; + while(bin){ + cpHandle *hand = bin->handle; + cpSpaceHashBin *next = bin->next; + + if(!hand->obj){ + // orphaned handle, unlink and recycle the bin + (*bin_ptr) = bin->next; + recycleBin(hash, bin); + + cpHandleRelease(hand, hash->pooledHandles); + } else { + bin_ptr = &bin->next; + } + + bin = next; + } +} + +#pragma mark Query Functions + +static inline void +query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryCallback func, void *data) +{ + restart: + for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ + cpHandle *hand = bin->handle; + void *other = hand->obj; + + if(hand->stamp == hash->stamp || obj == other){ + continue; + } else if(other){ + func(obj, other, data); + hand->stamp = hash->stamp; + } else { + // The object for this handle has been removed + // cleanup this cell and restart the query + remove_orphaned_handles(hash, bin_ptr); + goto restart; // GCC not smart enough/able to tail call an inlined function. + } + } +} + +static void +cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpatialIndexQueryCallback func, void *data) +{ + cpFloat dim = hash->celldim; + int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash->numcells); // Fix by ShiftZ + + query_helper(hash, &hash->table[idx], &point, func, data); + hash->stamp++; +} + +static void +cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +{ + // Get the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = floor_int(bb.l/dim); // Fix by ShiftZ + int r = floor_int(bb.r/dim); + int b = floor_int(bb.b/dim); + int t = floor_int(bb.t/dim); + + int n = hash->numcells; + cpSpaceHashBin **table = hash->table; + + // Iterate over the cells and query them. + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + query_helper(hash, &table[hash_func(i,j,n)], obj, func, data); + } + } + + hash->stamp++; +} + +// Similar to struct eachPair above. +typedef struct queryRehashContext { + cpSpaceHash *hash; + cpSpatialIndexQueryCallback func; + void *data; +} queryRehashContext; + +// Hashset iterator func used with cpSpaceHashQueryRehash(). +static void +queryRehash_helper(cpHandle *hand, queryRehashContext *context) +{ + cpSpaceHash *hash = context->hash; + cpSpatialIndexQueryCallback func = context->func; + void *data = context->data; + + cpFloat dim = hash->celldim; + int n = hash->numcells; + + void *obj = hand->obj; + cpBB bb = hash->spatialIndex.bbfunc(obj); + + int l = floor_int(bb.l/dim); + int r = floor_int(bb.r/dim); + int b = floor_int(bb.b/dim); + int t = floor_int(bb.t/dim); + + cpSpaceHashBin **table = hash->table; + + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int idx = hash_func(i,j,n); + cpSpaceHashBin *bin = table[idx]; + + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); // this MUST be done first in case the object is removed in func() + query_helper(hash, &bin, obj, func, data); + + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + table[idx] = newBin; + } + } + + // Increment the stamp for each object hashed. + hash->stamp++; +} + +static void +cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryCallback func, void *data) +{ + clearTable(hash); + + queryRehashContext context = {hash, func, data}; + cpHashSetEach(hash->handleSet, (cpHashSetIterFunc)queryRehash_helper, &context); + + cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data); +} + +static inline cpFloat +segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryCallback func, void *data) +{ + cpFloat t = 1.0f; + + restart: + for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ + cpHandle *hand = bin->handle; + void *other = hand->obj; + + // Skip over certain conditions + if(hand->stamp == hash->stamp){ + continue; + } else if(other){ + t = cpfmin(t, func(obj, other, data)); + hand->stamp = hash->stamp; + } else { + // The object for this handle has been removed + // cleanup this cell and restart the query + remove_orphaned_handles(hash, bin_ptr); + goto restart; // GCC not smart enough/able to tail call an inlined function. + } + } + + return t; +} + +// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html +void +cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data) +{ + a = cpvmult(a, 1.0f/hash->celldim); + b = cpvmult(b, 1.0f/hash->celldim); + + int cell_x = floor_int(a.x), cell_y = floor_int(a.y); + + cpFloat t = 0; + + int x_inc, y_inc; + cpFloat temp_v, temp_h; + + if (b.x > a.x){ + x_inc = 1; + temp_h = (cpffloor(a.x + 1.0f) - a.x); + } else { + x_inc = -1; + temp_h = (a.x - cpffloor(a.x)); + } + + if (b.y > a.y){ + y_inc = 1; + temp_v = (cpffloor(a.y + 1.0f) - a.y); + } else { + y_inc = -1; + temp_v = (a.y - cpffloor(a.y)); + } + + // Division by zero is *very* slow on ARM + cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y); + cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY); + + // fix NANs in horizontal directions + cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx); + cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy); + + int n = hash->numcells; + cpSpaceHashBin **table = hash->table; + + while(t < t_exit){ + int idx = hash_func(cell_x, cell_y, n); + t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data)); + + if (next_v < next_h){ + cell_y += y_inc; + t = next_v; + next_v += dt_dy; + } else { + cell_x += x_inc; + t = next_h; + next_h += dt_dx; + } + } + + hash->stamp++; +} + +#pragma mark Misc + +void +cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) +{ + if(hash->spatialIndex.klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index."); + return; + } + + clearTable(hash); + + hash->celldim = celldim; + cpSpaceHashAllocTable(hash, next_prime(numcells)); +} + +static int +cpSpaceHashCount(cpSpaceHash *hash) +{ + return cpHashSetCount(hash->handleSet); +} + +static int +cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid) +{ + return cpHashSetFind(hash->handleSet, hashid, obj) != NULL; +} + +static cpSpatialIndexClass klass = { + (cpSpatialIndexDestroyFunc)cpSpaceHashDestroy, + + (cpSpatialIndexCountFunc)cpSpaceHashCount, + (cpSpatialIndexEachFunc)cpSpaceHashEach, + (cpSpatialIndexContainsFunc)cpSpaceHashContains, + + (cpSpatialIndexInsertFunc)cpSpaceHashInsert, + (cpSpatialIndexRemoveFunc)cpSpaceHashRemove, + + (cpSpatialIndexReindexFunc)cpSpaceHashRehash, + (cpSpatialIndexReindexObjectFunc)cpSpaceHashRehashObject, + (cpSpatialIndexReindexQueryFunc)cpSpaceHashReindexQuery, + + (cpSpatialIndexPointQueryFunc)cpSpaceHashPointQuery, + (cpSpatialIndexSegmentQueryFunc)cpSpaceHashSegmentQuery, + (cpSpatialIndexQueryFunc)cpSpaceHashQuery, +}; + +#pragma mark Debug Drawing + +#define CP_BBTREE_DEBUG_DRAW +#ifdef CP_BBTREE_DEBUG_DRAW +#include +#include +#include + +void +cpSpaceHashRenderDebug(cpSpatialIndex *index) +{ + if(index->klass != &klass){ + cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index."); + return; + } + + cpSpaceHash *hash = (cpSpaceHash *)index; + cpBB bb = cpBBNew(-320, -240, 320, 240); + + cpFloat dim = hash->celldim; + int n = hash->numcells; + + int l = (int)floor(bb.l/dim); + int r = (int)floor(bb.r/dim); + int b = (int)floor(bb.b/dim); + int t = (int)floor(bb.t/dim); + + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int cell_count = 0; + + int index = hash_func(i,j,n); + for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next) + cell_count++; + + GLfloat v = 1.0f - (GLfloat)cell_count/10.0f; + glColor3f(v,v,v); + glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim); + } + } +} +#endif diff --git a/src/helper/chipmunk/cpSpaceQuery.c b/src/helper/chipmunk/cpSpaceQuery.c new file mode 100644 index 000000000..eaa4a132c --- /dev/null +++ b/src/helper/chipmunk/cpSpaceQuery.c @@ -0,0 +1,246 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk_private.h" + +#pragma mark Point Query Functions + +typedef struct pointQueryContext { + cpLayers layers; + cpGroup group; + cpSpacePointQueryFunc func; + void *data; +} pointQueryContext; + +static void +pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context) +{ + if( + !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && + cpShapePointQuery(shape, *point) + ){ + context->func(shape, context->data); + } +} + +void +cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data) +{ + pointQueryContext context = {layers, group, func, data}; + + cpSpaceLock(space); { + cpSpatialIndexPointQuery(space->activeShapes, point, (cpSpatialIndexQueryCallback)pointQueryHelper, &context); + cpSpatialIndexPointQuery(space->staticShapes, point, (cpSpatialIndexQueryCallback)pointQueryHelper, &context); + } cpSpaceUnlock(space); +} + +static void +rememberLastPointQuery(cpShape *shape, cpShape **outShape) +{ + if(!shape->sensor) *outShape = shape; +} + +cpShape * +cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group) +{ + cpShape *shape = NULL; + cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape); + + return shape; +} + + +#pragma mark Segment Query Functions + +typedef struct segQueryContext { + cpVect start, end; + cpLayers layers; + cpGroup group; + cpSpaceSegmentQueryFunc func; +} segQueryContext; + +static cpFloat +segQueryFunc(segQueryContext *context, cpShape *shape, void *data) +{ + cpSegmentQueryInfo info; + + if( + !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && + cpShapeSegmentQuery(shape, context->start, context->end, &info) + ){ + context->func(shape, info.t, info.n, data); + } + + return 1.0f; +} + +void +cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data) +{ + segQueryContext context = { + start, end, + layers, group, + func, + }; + + cpSpaceLock(space); { + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFunc, data); + cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFunc, data); + } cpSpaceUnlock(space); +} + +typedef struct segQueryFirstContext { + cpVect start, end; + cpLayers layers; + cpGroup group; +} segQueryFirstContext; + +static cpFloat +segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out) +{ + cpSegmentQueryInfo info; + + if( + !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && + !shape->sensor && + cpShapeSegmentQuery(shape, context->start, context->end, &info) && + info.t < out->t + ){ + *out = info; + } + + return out->t; +} + +cpShape * +cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out) +{ + cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero}; + if(out){ + (*out) = info; + } else { + out = &info; + } + + segQueryFirstContext context = { + start, end, + layers, group + }; + + cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryCallback)segQueryFirst, out); + cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryCallback)segQueryFirst, out); + + return out->shape; +} + +#pragma mark BB Query Functions + +typedef struct bbQueryContext { + cpLayers layers; + cpGroup group; + cpSpaceBBQueryFunc func; + void *data; +} bbQueryContext; + +static void +bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context) +{ + if( + !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && + cpBBintersects(*bb, shape->bb) + ){ + context->func(shape, context->data); + } +} + +void +cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data) +{ + bbQueryContext context = {layers, group, func, data}; + + cpSpaceLock(space); { + cpSpatialIndexQuery(space->activeShapes, &bb, bb, (cpSpatialIndexQueryCallback)bbQueryHelper, &context); + cpSpatialIndexQuery(space->staticShapes, &bb, bb, (cpSpatialIndexQueryCallback)bbQueryHelper, &context); + } cpSpaceUnlock(space); +} + +#pragma mark Shape Query Functions + +typedef struct shapeQueryContext { + cpSpaceShapeQueryFunc func; + void *data; + cpBool anyCollision; +} shapeQueryContext; + +// Callback from the spatial hash. +static void +shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context) +{ + // Reject any of the simple cases + if( + (a->group && a->group == b->group) || + !(a->layers & b->layers) || + a->sensor || b->sensor + ) return; + + cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER]; + int numContacts = 0; + + // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) + if(a->klass->type <= b->klass->type){ + numContacts = cpCollideShapes(a, b, contacts); + } else { + numContacts = cpCollideShapes(b, a, contacts); + for(int i=0; ianyCollision = cpTrue; + + if(context->func){ + cpContactPointSet set = {numContacts, {}}; + for(int i=0; ifunc(b, &set, context->data); + } + } +} + +cpBool +cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data) +{ + cpBody *body = shape->body; + cpBB bb = (body ? cpShapeUpdate(shape, body->p, body->rot) : shape->bb); + shapeQueryContext context = {func, data, cpFalse}; + + cpSpaceLock(space); { + cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryCallback)shapeQueryHelper, &context); + cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryCallback)shapeQueryHelper, &context); + } cpSpaceUnlock(space); + + return context.anyCollision; +} diff --git a/src/helper/chipmunk/cpSpaceStep.c b/src/helper/chipmunk/cpSpaceStep.c new file mode 100644 index 000000000..d6eb87c48 --- /dev/null +++ b/src/helper/chipmunk/cpSpaceStep.c @@ -0,0 +1,373 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include "chipmunk_private.h" + +#pragma mark Post Step Callback Functions + +typedef struct postStepCallback { + cpPostStepFunc func; + void *obj; + void *data; +} postStepCallback; + +static cpBool +postStepFuncSetEql(postStepCallback *a, postStepCallback *b){ + return a->obj == b->obj; +} + +static void * +postStepFuncSetTrans(postStepCallback *callback, void *ignored) +{ + postStepCallback *value = (postStepCallback *)cpmalloc(sizeof(postStepCallback)); + (*value) = (*callback); + + return value; +} + +void +cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data) +{ + if(!space->postStepCallbacks){ + space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, NULL); + } + + postStepCallback callback = {func, obj, data}; + cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL, (cpHashSetTransFunc)postStepFuncSetTrans); +} + +#pragma mark Contact Buffer Functions + +struct cpContactBufferHeader { + cpTimestamp stamp; + cpContactBufferHeader *next; + unsigned int numContacts; +}; + +#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact)) +typedef struct cpContactBuffer { + cpContactBufferHeader header; + cpContact contacts[CP_CONTACTS_BUFFER_SIZE]; +} cpContactBuffer; + +static cpContactBufferHeader * +cpSpaceAllocContactBuffer(cpSpace *space) +{ + cpContactBuffer *buffer = (cpContactBuffer *)cpmalloc(sizeof(cpContactBuffer)); + cpArrayPush(space->allocatedBuffers, buffer); + return (cpContactBufferHeader *)buffer; +} + +static cpContactBufferHeader * +cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice) +{ + header->stamp = stamp; + header->next = (splice ? splice->next : header); + header->numContacts = 0; + + return header; +} + +static void +cpSpacePushFreshContactBuffer(cpSpace *space) +{ + cpTimestamp stamp = space->stamp; + + cpContactBufferHeader *head = space->contactBuffersHead; + + if(!head){ + // No buffers have been allocated, make one + space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL); + } else if(stamp - head->next->stamp > cp_contact_persistence){ + // The tail buffer is available, rotate the ring + cpContactBufferHeader *tail = head->next; + space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); + } else { + // Allocate a new buffer and push it into the ring + cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head); + space->contactBuffersHead = head->next = buffer; + } +} + + +cpContact * +cpContactBufferGetArray(cpSpace *space) +{ + if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){ + // contact buffer could overflow on the next collision, push a fresh one. + cpSpacePushFreshContactBuffer(space); + } + + cpContactBufferHeader *head = space->contactBuffersHead; + return ((cpContactBuffer *)head)->contacts + head->numContacts; +} + +void +cpSpacePushContacts(cpSpace *space, int count) +{ + cpAssert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error:contact buffer overflow!"); + space->contactBuffersHead->numContacts += count; +} + +static inline void +cpSpacePopContacts(cpSpace *space, int count){ + space->contactBuffersHead->numContacts -= count; +} + +#pragma mark Collision Detection Functions + +static void * +contactSetTrans(cpShape **shapes, cpSpace *space) +{ + if(space->pooledArbiters->num == 0){ + // arbiter pool is exhausted, make more + int count = CP_BUFFER_BYTES/sizeof(cpArbiter); + cpAssert(count, "Buffer size too small."); + + cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES); + cpArrayPush(space->allocatedBuffers, buffer); + + for(int i=0; ipooledArbiters, buffer + i); + } + + return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); +} + +static inline cpBool +queryReject(cpShape *a, cpShape *b) +{ + return ( + // BBoxes must overlap + !cpBBintersects(a->bb, b->bb) + // Don't collide shapes attached to the same body. + || a->body == b->body + // Don't collide objects in the same non-zero group + || (a->group && a->group == b->group) + // Don't collide objects that don't share at least on layer. + || !(a->layers & b->layers) + ); +} + +// Callback from the spatial hash. +void +cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space) +{ + // Reject any of the simple cases + if(queryReject(a,b)) return; + + // Find the collision pair function for the shapes. + cpCollisionType types[] = {a->collision_type, b->collision_type}; + cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type); + cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, types); + + cpBool sensor = a->sensor || b->sensor; + if(sensor && handler == &space->defaultHandler) return; + + // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) + if(a->klass->type > b->klass->type){ + cpShape *temp = a; + a = b; + b = temp; + } + + // Narrow-phase collision detection. + cpContact *contacts = cpContactBufferGetArray(space); + int numContacts = cpCollideShapes(a, b, contacts); + if(!numContacts) return; // Shapes are not colliding. + cpSpacePushContacts(space, numContacts); + + // Get an arbiter from space->contactSet for the two shapes. + // This is where the persistant contact magic comes from. + cpShape *shape_pair[] = {a, b}; + cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b); + cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space, (cpHashSetTransFunc)contactSetTrans); + cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); + + // Call the begin function first if it's the first step + if(arb->state == cpArbiterStateFirstColl && !handler->begin(arb, space, handler->data)){ + cpArbiterIgnore(arb); // permanently ignore the collision until separation + } + + if( + // Ignore the arbiter if it has been flagged + (arb->state != cpArbiterStateIgnore) && + // Call preSolve + handler->preSolve(arb, space, handler->data) && + // Process, but don't add collisions for sensors. + !sensor + ){ + cpArrayPush(space->arbiters, arb); + } else { + cpSpacePopContacts(space, numContacts); + + arb->contacts = NULL; + arb->numContacts = 0; + + // Normally arbiters are set as used after calling the post-step callback. + // However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve. + if(arb->state != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal; + } + + // Time stamp the arbiter so we know it was used recently. + arb->stamp = space->stamp; +} + +// Hashset filter func to throw away old arbiters. +static cpBool +contactSetFilter(cpArbiter *arb, cpSpace *space) +{ + cpTimestamp ticks = space->stamp - arb->stamp; + + // was used last frame, but not this one + if(ticks >= 1 && arb->state != cpArbiterStateCached){ + arb->handler->separate(arb, space, arb->handler->data); + arb->state = cpArbiterStateCached; + } + + if(ticks >= cp_contact_persistence){ + arb->contacts = NULL; + arb->numContacts = 0; + + cpArrayPush(space->pooledArbiters, arb); + return cpFalse; + } + + return cpTrue; +} + +// Hashset filter func to call and throw away post step callbacks. +static void +postStepCallbackSetIter(postStepCallback *callback, cpSpace *space) +{ + callback->func(space, callback->obj, callback->data); + cpfree(callback); +} + +#pragma mark All Important cpSpaceStep() Function + +void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); + +static void +updateBBCache(cpShape *shape, void *unused) +{ + cpBody *body = shape->body; + cpShapeUpdate(shape, body->p, body->rot); +} + +void +cpSpaceStep(cpSpace *space, cpFloat dt) +{ + if(!dt) return; // don't step if the timestep is 0! + cpFloat dt_inv = 1.0f/dt; + + cpArray *bodies = space->bodies; + cpArray *constraints = space->constraints; + + // Empty the arbiter list. + space->arbiters->num = 0; + + // Integrate positions + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->position_func(body, dt); + } + + // Pre-cache BBoxes and shape data. + cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIterator)updateBBCache, NULL); + + + // Collide! + cpSpaceLock(space); + cpSpacePushFreshContactBuffer(space); + cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryCallback)cpSpaceCollideShapes, space); + cpSpaceUnlock(space); + + // If body sleeping is enabled, do that now. + if(space->sleepTimeThreshold != INFINITY){ + cpSpaceProcessComponents(space, dt); +// bodies = space->bodies; // rebuilt by processContactComponents() + } + + // Clear out old cached arbiters and call separate callbacks + cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space); + + // Prestep the arbiters. + cpArray *arbiters = space->arbiters; + for(int i=0; inum; i++) + cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv); + + // Prestep the constraints. + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + constraint->klass->preStep(constraint, dt, dt_inv); + } + + // Integrate velocities. + cpFloat damping = cpfpow(space->damping, dt); + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->velocity_func(body, space->gravity, damping, dt); + } + + for(int i=0; inum; i++) + cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]); + + // Run the impulse solver. + for(int i=0; iiterations; i++){ + for(int j=0; jnum; j++) + cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]); + + for(int j=0; jnum; j++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; + constraint->klass->applyImpulse(constraint); + } + } + + + // run the post solve callbacks + cpSpaceLock(space); + for(int i=0; inum; i++){ + cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; + + cpCollisionHandler *handler = arb->handler; + handler->postSolve(arb, space, handler->data); + + arb->state = cpArbiterStateNormal; + } + cpSpaceUnlock(space); + + // Run the post step callbacks + // Loop because post step callbacks may create more post step callbacks + while(space->postStepCallbacks){ + cpHashSet *callbacks = space->postStepCallbacks; + space->postStepCallbacks = NULL; + + cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space); + cpHashSetFree(callbacks); + } + + // Increment the stamp. + space->stamp++; +} diff --git a/src/helper/chipmunk/cpSpatialIndex.c b/src/helper/chipmunk/cpSpatialIndex.c new file mode 100644 index 000000000..887082b5d --- /dev/null +++ b/src/helper/chipmunk/cpSpatialIndex.c @@ -0,0 +1,50 @@ +#include + +#include "chipmunk.h" + +void +cpSpatialIndexFree(cpSpatialIndex *index) +{ + if(index){ + cpSpatialIndexDestroy(index); + cpfree(index); + } +} + +typedef struct dynamicToStaticContext { + cpSpatialIndexBBFunc bbfunc; + cpSpatialIndex *staticIndex; + cpSpatialIndexQueryCallback queryFunc; + void *data; +} dynamicToStaticContext; + +cpSpatialIndex * +cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) +{ + index->klass = klass; + index->bbfunc = bbfunc; + index->staticIndex = staticIndex; + + if(staticIndex){ + cpAssert(!staticIndex->dynamicIndex, "This static index is already is already associated with a dynamic index."); + staticIndex->dynamicIndex = index; + } + + return index; +} + +static void +dynamicToStaticIter(void *obj, dynamicToStaticContext *context) +{ + cpSpatialIndexQuery(context->staticIndex, obj, context->bbfunc(obj), context->queryFunc, context->data); +} + +void +cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryCallback func, void *data) +{ + if(cpSpatialIndexCount(staticIndex) > 0){ + dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data}; + cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIterator)dynamicToStaticIter, &context); + } +} + diff --git a/src/helper/chipmunk/cpSpatialIndex.h b/src/helper/chipmunk/cpSpatialIndex.h new file mode 100644 index 000000000..9d6ec0185 --- /dev/null +++ b/src/helper/chipmunk/cpSpatialIndex.h @@ -0,0 +1,159 @@ +#pragma mark Spatial Index + +typedef cpBB (*cpSpatialIndexBBFunc)(void *obj); +typedef void (*cpSpatialIndexIterator)(void *obj, void *data); +typedef void (*cpSpatialIndexQueryCallback)(void *obj1, void *obj2, void *data); +typedef cpFloat (*cpSpatialIndexSegmentQueryCallback)(void *obj1, void *obj2, void *data); + + +typedef struct cpSpatialIndexClass cpSpatialIndexClass; +typedef struct cpSpatialIndex cpSpatialIndex; + +struct cpSpatialIndex { + cpSpatialIndexClass *klass; + + cpSpatialIndexBBFunc bbfunc; + + cpSpatialIndex *staticIndex, *dynamicIndex; +}; + + +#pragma mark Spatial Hash + +typedef struct cpSpaceHash cpSpaceHash; + +cpSpaceHash *cpSpaceHashAlloc(void); +cpSpatialIndex *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +cpSpatialIndex *cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + +void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); + +#pragma mark AABB Tree + +typedef struct cpBBTree cpBBTree; + +cpBBTree *cpBBTreeAlloc(void); +cpSpatialIndex *cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); +cpSpatialIndex *cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); + +void cpBBTreeOptimize(cpSpatialIndex *index); + +typedef cpVect (*cpBBTreeVelocityFunc)(void *obj); +void cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func); + +#pragma mark Spatial Index Implementation + +typedef void (*cpSpatialIndexDestroyFunc)(cpSpatialIndex *index); + +typedef int (*cpSpatialIndexCountFunc)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexEachFunc)(cpSpatialIndex *index, cpSpatialIndexIterator func, void *data); + +typedef cpBool (*cpSpatialIndexContainsFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexInsertFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexRemoveFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); + +typedef void (*cpSpatialIndexReindexFunc)(cpSpatialIndex *index); +typedef void (*cpSpatialIndexReindexObjectFunc)(cpSpatialIndex *index, void *obj, cpHashValue hashid); +typedef void (*cpSpatialIndexReindexQueryFunc)(cpSpatialIndex *index, cpSpatialIndexQueryCallback func, void *data); + +typedef void (*cpSpatialIndexPointQueryFunc)(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryCallback func, void *data); +typedef void (*cpSpatialIndexSegmentQueryFunc)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data); +typedef void (*cpSpatialIndexQueryFunc)(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data); + +struct cpSpatialIndexClass { + cpSpatialIndexDestroyFunc destroy; + + cpSpatialIndexCountFunc count; + cpSpatialIndexEachFunc each; + + cpSpatialIndexContainsFunc contains; + cpSpatialIndexInsertFunc insert; + cpSpatialIndexRemoveFunc remove; + + cpSpatialIndexReindexFunc reindex; + cpSpatialIndexReindexObjectFunc reindexObject; + cpSpatialIndexReindexQueryFunc reindexQuery; + + cpSpatialIndexPointQueryFunc pointQuery; + cpSpatialIndexSegmentQueryFunc segmentQuery; + cpSpatialIndexQueryFunc query; +}; + + +void cpSpatialIndexFree(cpSpatialIndex *index); +void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryCallback func, void *data); + +static inline void +cpSpatialIndexDestroy(cpSpatialIndex *index) +{ + index->klass->destroy(index); +} + +static inline int +cpSpatialIndexCount(cpSpatialIndex *index) +{ + return index->klass->count(index); +} + +static inline void +cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIterator func, void *data) +{ + index->klass->each(index, func, data); +} + +static inline cpBool +cpSpatialIndexContains(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + return index->klass->contains(index, obj, hashid); +} + +static inline void +cpSpatialIndexInsert(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + index->klass->insert(index, obj, hashid); +} + +static inline void +cpSpatialIndexRemove(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + index->klass->remove(index, obj, hashid); +} + +static inline void +cpSpatialIndexReindex(cpSpatialIndex *index) +{ + index->klass->reindex(index); +} + +static inline void +cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, cpHashValue hashid) +{ + index->klass->reindexObject(index, obj, hashid); +} + +// TODO make sure to doc the callback type, a pointer to the point is passed as obj1 +static inline void +cpSpatialIndexPointQuery(cpSpatialIndex *index, cpVect point, cpSpatialIndexQueryCallback func, void *data) +{ + index->klass->pointQuery(index, point, func, data); +} + +// TODO make sure to doc the callback type +static inline void +cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryCallback func, void *data) +{ + index->klass->segmentQuery(index, obj, a, b, t_exit, func, data); +} + + +static inline void +cpSpatialIndexQuery(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryCallback func, void *data) +{ + index->klass->query(index, obj, bb, func, data); +} + +static inline void +cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryCallback func, void *data) +{ + index->klass->reindexQuery(index, func, data); +} diff --git a/src/helper/chipmunk/cpVect.c b/src/helper/chipmunk/cpVect.c new file mode 100644 index 000000000..39554e620 --- /dev/null +++ b/src/helper/chipmunk/cpVect.c @@ -0,0 +1,71 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + +cpFloat +cpvlength(const cpVect v) +{ + return cpfsqrt( cpvdot(v, v) ); +} + +inline cpVect +cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) +{ + cpFloat omega = cpfacos(cpvdot(v1, v2)); + + if(omega){ + cpFloat denom = 1.0f/cpfsin(omega); + return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); + } else { + return v1; + } +} + +cpVect +cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a) +{ + cpFloat angle = cpfacos(cpvdot(v1, v2)); + return cpvslerp(v1, v2, cpfmin(a, angle)/angle); +} + +cpVect +cpvforangle(const cpFloat a) +{ + return cpv(cpfcos(a), cpfsin(a)); +} + +cpFloat +cpvtoangle(const cpVect v) +{ + return cpfatan2(v.y, v.x); +} + +char* +cpvstr(const cpVect v) +{ + static char str[256]; + sprintf(str, "(% .3f, % .3f)", v.x, v.y); + return str; +} diff --git a/src/helper/chipmunk/cpVect.h b/src/helper/chipmunk/cpVect.h new file mode 100644 index 000000000..261cec522 --- /dev/null +++ b/src/helper/chipmunk/cpVect.h @@ -0,0 +1,207 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/// Constant for the zero vector. +static const cpVect cpvzero = {0.0f,0.0f}; + +/// Convenience constructor for cpVect structs. +static inline cpVect +cpv(const cpFloat x, const cpFloat y) +{ + cpVect v = {x, y}; + return v; +} + +// non-inlined functions + +/// Returns the length of v. +cpFloat cpvlength(const cpVect v); + +/// Spherical linearly interpolate between v1 and v2. +cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t); + +/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians +cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a); + +/// Returns the unit length vector for the given angle (in radians). +cpVect cpvforangle(const cpFloat a); + +/// Returns the angular direction v is pointing in (in radians). +cpFloat cpvtoangle(const cpVect v); + +/** + Returns a string representation of v. Intended mostly for debugging purposes and not production use. + + @attention The string points to a static local and is reset every time the function is called. + If you want to print more than one vector you will have to split up your printing onto separate lines. +*/ +char *cpvstr(const cpVect v); + +/// Check if two vectors are equal. (Be careful when comparing floating point numbers!) +static inline cpBool +cpveql(const cpVect v1, const cpVect v2) +{ + return (v1.x == v2.x && v1.y == v2.y); +} + +/// Add two vectors +static inline cpVect +cpvadd(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x + v2.x, v1.y + v2.y); +} + +/// Negate a vector. +static inline cpVect +cpvneg(const cpVect v) +{ + return cpv(-v.x, -v.y); +} + +/// Subtract two vectors. +static inline cpVect +cpvsub(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x - v2.x, v1.y - v2.y); +} + +/// Scalar multiplication. +static inline cpVect +cpvmult(const cpVect v, const cpFloat s) +{ + return cpv(v.x*s, v.y*s); +} + +/// Vector dot product. +static inline cpFloat +cpvdot(const cpVect v1, const cpVect v2) +{ + return v1.x*v2.x + v1.y*v2.y; +} + +/** + 2D vector cross product analog. + The cross product of 2D vectors results in a 3D vector with only a z component. + This function returns the magnitude of the z value. +*/ +static inline cpFloat +cpvcross(const cpVect v1, const cpVect v2) +{ + return v1.x*v2.y - v1.y*v2.x; +} + +/// Returns a perpendicular vector. (90 degree rotation) +static inline cpVect +cpvperp(const cpVect v) +{ + return cpv(-v.y, v.x); +} + +/// Returns a perpendicular vector. (-90 degree rotation) +static inline cpVect +cpvrperp(const cpVect v) +{ + return cpv(v.y, -v.x); +} + +/// Returns the vector projection of v1 onto v2. +static inline cpVect +cpvproject(const cpVect v1, const cpVect v2) +{ + return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); +} + +/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector. +static inline cpVect +cpvrotate(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); +} + +/// Inverse of cpvrotate(). +static inline cpVect +cpvunrotate(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); +} + +/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths. +static inline cpFloat +cpvlengthsq(const cpVect v) +{ + return cpvdot(v, v); +} + +/// Linearly interpolate between v1 and v2. +static inline cpVect +cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t) +{ + return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t)); +} + +/// Returns a normalized copy of v. +static inline cpVect +cpvnormalize(const cpVect v) +{ + return cpvmult(v, 1.0f/cpvlength(v)); +} + +/// Returns a normalized copy of v or cpvzero if v was already cpvzero. Protects against divide by zero errors. +static inline cpVect +cpvnormalize_safe(const cpVect v) +{ + return (v.x == 0.0f && v.y == 0.0f ? cpvzero : cpvnormalize(v)); +} + +/// Clamp v to length len. +static inline cpVect +cpvclamp(const cpVect v, const cpFloat len) +{ + return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +/// Linearly interpolate between v1 towards v2 by distance d. +static inline cpVect +cpvlerpconst(cpVect v1, cpVect v2, cpFloat d) +{ + return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)); +} + +/// Returns the distance between v1 and v2. +static inline cpFloat +cpvdist(const cpVect v1, const cpVect v2) +{ + return cpvlength(cpvsub(v1, v2)); +} + +/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances. +static inline cpFloat +cpvdistsq(const cpVect v1, const cpVect v2) +{ + return cpvlengthsq(cpvsub(v1, v2)); +} + +/// Returns true if the distance between v1 and v2 is less than dist. +static inline cpBool +cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) +{ + return cpvdistsq(v1, v2) < dist*dist; +} diff --git a/src/helper/chipmunk/prime.h b/src/helper/chipmunk/prime.h new file mode 100644 index 000000000..423ac331e --- /dev/null +++ b/src/helper/chipmunk/prime.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Used for resizing hash tables. +// Values approximately double. +// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html +static int primes[] = { + 5, + 13, + 23, + 47, + 97, + 193, + 389, + 769, + 1543, + 3079, + 6151, + 12289, + 24593, + 49157, + 98317, + 196613, + 393241, + 786433, + 1572869, + 3145739, + 6291469, + 12582917, + 25165843, + 50331653, + 100663319, + 201326611, + 402653189, + 805306457, + 1610612741, + 0, +}; + +static inline int +next_prime(int n) +{ + int i = 0; + while(n > primes[i]){ + i++; + cpAssert(primes[i], "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen + } + + return primes[i]; +} diff --git a/src/physics/base.hpp b/src/physics/base.hpp new file mode 100644 index 000000000..020e972fd --- /dev/null +++ b/src/physics/base.hpp @@ -0,0 +1,24 @@ +#ifndef EE_PHYSICS_BASE +#define EE_PHYSICS_BASE + +#include "../base.hpp" + +#include "../math/math.hpp" +using namespace EE::Math; + +#include "../window/cengine.hpp" +using namespace EE::Window; + +#include "../system/tsingleton.hpp" +using namespace EE::System; + +#include "../graphics/cprimitives.hpp" + +#define CP_ALLOW_PRIVATE_ACCESS 1 +#include "../helper/chipmunk/chipmunk_private.h" +#include "../helper/chipmunk/chipmunk_unsafe.h" +#include "../helper/chipmunk/chipmunk.h" + +#include "physicshelper.hpp" + +#endif diff --git a/src/physics/cbody.cpp b/src/physics/cbody.cpp new file mode 100644 index 000000000..f292b6fde --- /dev/null +++ b/src/physics/cbody.cpp @@ -0,0 +1,170 @@ +#include "cbody.hpp" + +namespace EE { namespace Physics { + +cBody::cBody( cpBody * body ) { + mBody = body; + mBody->data = (void*)this; +} + +cBody::cBody( cpFloat m, cpFloat i ) { + mBody = cpBodyNew( m, i ); + mBody->data = (void*)this; +} + +cBody::cBody() { + mBody = cpBodyNewStatic(); +} + +cBody::~cBody() { +} + +void cBody::Activate() { + cpBodyActivate( mBody ); +} + +void cBody::Sleep() { + cpBodySleep( mBody ); +} + +void cBody::SleepWithGroup( cBody * Group ) { + cpBodySleepWithGroup( mBody, Group->Body() ); +} + +bool cBody::IsSleeping() { + return cpFalse != cpBodyIsSleeping( mBody ); +} + +bool cBody::IsStatic() { + return cpFalse != cpBodyIsStatic( mBody ); +} + +bool cBody::IsRogue() { + return cpFalse != cpBodyIsRogue( mBody ); +} + +cpBody * cBody::Body() const { + return mBody; +} + +cpFloat cBody::Mass() const { + return cpBodyGetMass( mBody ); +} + +void cBody::Mass( const cpFloat& mass ) { + cpBodySetMass( mBody, mass ); +} + +cpFloat cBody::Moment() const { + return cpBodyGetMoment( mBody ); +} + +void cBody::Moment( const cpFloat& i ) { + cpBodySetMoment( mBody, i ); +} + +cpVect cBody::Pos() const { + return cpBodyGetPos( mBody ); +} + +void cBody::Pos( const cpVect& pos ) { + cpBodySetPos( mBody, pos ); +} + +cpVect cBody::Vel() const { + return cpBodyGetVel( mBody ); +} + +void cBody::Vel( const cpVect& vel ) { + cpBodySetVel( mBody, vel ); +} + +cpVect cBody::Force() const { + return cpBodyGetForce( mBody ); +} + +void cBody::Force( const cpVect& force ) { + cpBodySetForce( mBody, force ); +} + +cpFloat cBody::Angle() const { + return cpBodyGetAngle( mBody ); +} + +void cBody::Angle( const cpFloat& rads ) { + cpBodySetAngle( mBody, rads ); +} + +cpFloat cBody::AngleDeg() { + return Degrees( mBody->a ); +} + +void cBody::AngleDeg( const cpFloat& angle ) { + Angle( Radians( angle ) ); +} + +cpFloat cBody::AngVel() const { + return cpBodyGetAngVel( mBody ); +} + +void cBody::AngVel( const cpFloat& rotVel ) { + cpBodySetAngVel( mBody, rotVel ); +} + +cpVect cBody::Rot() const { + return cpBodyGetRot( mBody ); +} + +cpFloat cBody::VelLimit() const { + return cpBodyGetVelLimit( mBody ); +} + +void cBody::VelLimit( const cpFloat& speed ) { + cpBodySetVelLimit( mBody, speed ); +} + +cpFloat cBody::AngVelLimit() const { + return cpBodyGetAngVelLimit( mBody ); +} + +void cBody::AngVelLimit( const cpFloat& speed ) { + cpBodySetAngVelLimit( mBody, speed ); +} + +void cBody::Slew( cpVect pos, cpFloat dt ) { + cpBodySlew( mBody, pos, dt ); +} + +void cBody::UpdateVelocity( cpVect gravity, cpFloat damping, cpFloat dt ) { + cpBodyUpdateVelocity( mBody, gravity, damping, dt ); +} + +void cBody::UpdatePosition( cpFloat dt ) { + cpBodyUpdatePosition( mBody, dt ); +} + +cpVect cBody::Local2World( const cpVect v ) { + return cpBodyLocal2World( mBody, v ); +} + +cpVect cBody::World2Local( const cpVect v ) { + return cpBodyWorld2Local( mBody, v ); +} + +void cBody::ApplyImpulse( const cpVect j, const cpVect r ) { + cpBodyApplyImpulse( mBody, j, r ); +} + +void cBody::ResetForces() { + cpBodyResetForces( mBody ); +} + +void cBody::ApplyForce( const cpVect f, const cpVect r ) { + cpBodyApplyForce( mBody, f, r ); +} + +cpFloat cBody::KineticEnergy() { + return cpBodyKineticEnergy( mBody ); +} + +}} diff --git a/src/physics/cbody.hpp b/src/physics/cbody.hpp new file mode 100644 index 000000000..184a01bb2 --- /dev/null +++ b/src/physics/cbody.hpp @@ -0,0 +1,97 @@ +#ifndef EE_PHYSICS_CBODY_HPP +#define EE_PHYSICS_CBODY_HPP + +#include "base.hpp" + +namespace EE { namespace Physics { + +class cBody { + public: + cBody( cpBody * body ); + + cBody( cpFloat m, cpFloat i ); + + cBody(); + + ~cBody(); + + void Activate(); + + void Sleep(); + + void SleepWithGroup( cBody * Group ); + + bool IsSleeping(); + + bool IsStatic(); + + bool IsRogue(); + + cpBody * Body() const; + + cpFloat Mass() const; + + void Mass( const cpFloat& mass ); + + cpFloat Moment() const; + + void Moment( const cpFloat& i ); + + cpVect Pos() const; + + void Pos( const cpVect& pos ); + + cpVect Vel() const; + + void Vel( const cpVect& vel ); + + cpVect Force() const; + + void Force( const cpVect& force ); + + cpFloat Angle() const; + + void Angle( const cpFloat& rads ); + + cpFloat AngleDeg(); + + void AngleDeg( const cpFloat& angle ); + + cpFloat AngVel() const; + + void AngVel( const cpFloat& angVel ); + + cpVect Rot() const; + + cpFloat VelLimit() const; + + void VelLimit( const cpFloat& speed ); + + cpFloat AngVelLimit() const; + + void AngVelLimit( const cpFloat& speed ); + + void Slew( cpVect pos, cpFloat dt ); + + void UpdateVelocity( cpVect gravity, cpFloat damping, cpFloat dt ); + + void UpdatePosition( cpFloat dt ); + + cpVect Local2World( const cpVect v ); + + cpVect World2Local( const cpVect v ); + + void ApplyImpulse( const cpVect j, const cpVect r ); + + void ResetForces(); + + void ApplyForce( const cpVect f, const cpVect r ); + + cpFloat KineticEnergy(); + protected: + cpBody * mBody; +}; + +}} + +#endif diff --git a/src/physics/constraints/cconstraint.cpp b/src/physics/constraints/cconstraint.cpp new file mode 100644 index 000000000..ebe58d50e --- /dev/null +++ b/src/physics/constraints/cconstraint.cpp @@ -0,0 +1,59 @@ +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +cConstraint::cConstraint( cpConstraint * Constraint ) { + mConstraint = Constraint; + SetData(); +} + +cConstraint::cConstraint() { +} + +cConstraint::~cConstraint() { +} + +void cConstraint::SetData() { + mConstraint->data = (void*)this; +} + +cpConstraint * cConstraint::Constraint() const { + return mConstraint; +} + +cBody * cConstraint::A() { + return reinterpret_cast( mConstraint->a->data ); +} + +cBody * cConstraint::B() { + return reinterpret_cast( mConstraint->b->data ); +} + +cpFloat cConstraint::MaxForce() { + return mConstraint->maxForce; +} + +void cConstraint::MaxForce( const cpFloat& maxforce ) { + mConstraint->maxForce = maxforce; +} + +cpFloat cConstraint::BiasCoef() { + return mConstraint->biasCoef; +} + +void cConstraint::BiasCoef( const cpFloat& biascoef ) { + mConstraint->biasCoef = biascoef; +} + +cpFloat cConstraint::MaxBias() { + return mConstraint->maxBias; +} + +void cConstraint::MaxBias( const cpFloat& maxbias ) { + mConstraint->maxBias = maxbias; +} + +void cConstraint::Draw() { +} + +}} diff --git a/src/physics/constraints/cconstraint.hpp b/src/physics/constraints/cconstraint.hpp new file mode 100644 index 000000000..4820d7ad5 --- /dev/null +++ b/src/physics/constraints/cconstraint.hpp @@ -0,0 +1,44 @@ +#ifndef EE_PHYSICS_CCONSTRAINT_HPP +#define EE_PHYSICS_CCONSTRAINT_HPP + +#include "../base.hpp" +#include "../cbody.hpp" + +namespace EE { namespace Physics { + +class cConstraint { + public: + cConstraint( cpConstraint * Constraint ); + + cConstraint(); + + ~cConstraint(); + + cpConstraint * Constraint() const; + + cBody * A(); + + cBody * B(); + + cpFloat MaxForce(); + + void MaxForce( const cpFloat& maxforce ); + + cpFloat BiasCoef(); + + void BiasCoef( const cpFloat& biascoef ); + + cpFloat MaxBias(); + + void MaxBias( const cpFloat& maxbias ); + + virtual void Draw(); + protected: + cpConstraint * mConstraint; + + void SetData(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cdampedrotaryspring.cpp b/src/physics/constraints/cdampedrotaryspring.cpp new file mode 100644 index 000000000..a00334ba0 --- /dev/null +++ b/src/physics/constraints/cdampedrotaryspring.cpp @@ -0,0 +1,38 @@ +#include "cdampedrotaryspring.hpp" + +namespace EE { namespace Physics { + +cDampedRotarySpring::cDampedRotarySpring( cBody * a, cBody * b, cpFloat restAngle, cpFloat stiffness, cpFloat damping ) { + mConstraint = cpDampedRotarySpringNew( a->Body(), b->Body(), restAngle, stiffness, damping ); + SetData(); +} + +cpFloat cDampedRotarySpring::RestAngle() { + return cpDampedRotarySpringGetRestAngle( mConstraint ); +} + +void cDampedRotarySpring::RestAngle( const cpFloat& restangle ) { + cpDampedRotarySpringSetRestAngle( mConstraint, restangle ); +} + +cpFloat cDampedRotarySpring::Stiffness() { + return cpDampedRotarySpringGetStiffness( mConstraint ); +} + +void cDampedRotarySpring::Stiffness( const cpFloat& stiffness ) { + cpDampedRotarySpringSetStiffness( mConstraint, stiffness ); +} + +cpFloat cDampedRotarySpring::Damping() { + return cpDampedRotarySpringGetDamping( mConstraint ); +} + +void cDampedRotarySpring::Damping( const cpFloat& damping ) { + cpDampedRotarySpringSetDamping( mConstraint, damping ); +} + +void cDampedRotarySpring::Draw() { + // Not implemented +} + +}} diff --git a/src/physics/constraints/cdampedrotaryspring.hpp b/src/physics/constraints/cdampedrotaryspring.hpp new file mode 100644 index 000000000..cec8c3242 --- /dev/null +++ b/src/physics/constraints/cdampedrotaryspring.hpp @@ -0,0 +1,29 @@ +#ifndef EE_PHYSICS_CDAMPEDROTARYSPRING_HPP +#define EE_PHYSICS_CDAMPEDROTARYSPRING_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cDampedRotarySpring : public cConstraint { + public: + cDampedRotarySpring( cBody * a, cBody * b, cpFloat restAngle, cpFloat stiffness, cpFloat damping ); + + cpFloat RestAngle(); + + void RestAngle( const cpFloat& restangle ); + + cpFloat Stiffness(); + + void Stiffness( const cpFloat& stiffness ); + + cpFloat Damping(); + + void Damping( const cpFloat& damping ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cdampedspring.cpp b/src/physics/constraints/cdampedspring.cpp new file mode 100644 index 000000000..f14ac6b01 --- /dev/null +++ b/src/physics/constraints/cdampedspring.cpp @@ -0,0 +1,86 @@ +#include "cdampedspring.hpp" + +namespace EE { namespace Physics { + +cDampedSpring::cDampedSpring( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping ) { + mConstraint = cpDampedSpringNew( a->Body(), b->Body(), anchr1, anchr2, restLength, stiffness, damping ); + SetData(); +} + +cpVect cDampedSpring::Anchr1() { + return cpDampedSpringGetAnchr1( mConstraint ); +} + +void cDampedSpring::Anchr1( const cpVect& anchr1 ) { + cpDampedSpringSetAnchr1( mConstraint, anchr1 ); +} + +cpVect cDampedSpring::Anchr2() { + return cpDampedSpringGetAnchr2( mConstraint ); +} + +void cDampedSpring::Anchr2( const cpVect& anchr2 ) { + cpDampedSpringSetAnchr2( mConstraint, anchr2 ); +} + +cpFloat cDampedSpring::RestLength() { + return cpDampedSpringGetRestLength( mConstraint ); +} + +void cDampedSpring::RestLength( const cpFloat& restlength ) { + cpDampedSpringSetRestLength( mConstraint, restlength ); +} + +cpFloat cDampedSpring::Stiffness() { + return cpDampedSpringGetStiffness( mConstraint ); +} + +void cDampedSpring::Stiffness( const cpFloat& stiffness ) { + cpDampedSpringSetStiffness( mConstraint, stiffness ); +} + +cpFloat cDampedSpring::Damping() { + return cpDampedSpringGetDamping( mConstraint ); +} + +void cDampedSpring::Damping( const cpFloat& damping ) { + cpDampedSpringSetDamping( mConstraint, damping ); +} + +void cDampedSpring::Draw() { + cpDampedSpring * spring = (cpDampedSpring*)mConstraint; + cpBody * body_a = mConstraint->a; + cpBody * body_b = mConstraint->b; + + cpVect a = cpvadd(body_a->p, cpvrotate(spring->anchr1, body_a->rot)); + cpVect b = cpvadd(body_b->p, cpvrotate(spring->anchr2, body_b->rot)); + + glPointSize(5.0f); + glBegin(GL_POINTS); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); + + cpVect delta = cpvsub(b, a); + + glVertexPointer(2, GL_FLOAT, 0, springVAR); + glPushMatrix(); { + GLfloat x = a.x; + GLfloat y = a.y; + GLfloat cos = delta.x; + GLfloat sin = delta.y; + GLfloat s = 1.0f/cpvlength(delta); + + const GLfloat matrix[] = { + cos, sin, 0.0f, 0.0f, + -sin*s, cos*s, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + x, y, 0.0f, 1.0f, + }; + + glMultMatrixf(matrix); + glDrawArrays(GL_LINE_STRIP, 0, springVAR_count); + } glPopMatrix(); +} + +}} diff --git a/src/physics/constraints/cdampedspring.hpp b/src/physics/constraints/cdampedspring.hpp new file mode 100644 index 000000000..aad2200d5 --- /dev/null +++ b/src/physics/constraints/cdampedspring.hpp @@ -0,0 +1,37 @@ +#ifndef EE_PHYSICS_CDAMPEDSPRING_HPP +#define EE_PHYSICS_CDAMPEDSPRING_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cDampedSpring : public cConstraint { + public: + cDampedSpring( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping ); + + cpVect Anchr1(); + + void Anchr1( const cpVect& anchr1 ); + + cpVect Anchr2(); + + void Anchr2( const cpVect& anchr2 ); + + cpFloat RestLength(); + + void RestLength( const cpFloat& restlength ); + + cpFloat Stiffness(); + + void Stiffness( const cpFloat& stiffness ); + + cpFloat Damping(); + + void Damping( const cpFloat& damping ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cgearjoint.cpp b/src/physics/constraints/cgearjoint.cpp new file mode 100644 index 000000000..72e002160 --- /dev/null +++ b/src/physics/constraints/cgearjoint.cpp @@ -0,0 +1,30 @@ +#include "cgearjoint.hpp" + +namespace EE { namespace Physics { + +cGearJoint::cGearJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratio ) { + mConstraint = cpGearJointNew( a->Body(), b->Body(), phase, ratio ); + SetData(); +} + +cpFloat cGearJoint::Phase() { + return cpGearJointGetPhase( mConstraint ); +} + +void cGearJoint::Phase( const cpFloat& phase ) { + cpGearJointSetPhase( mConstraint, phase ); +} + +cpFloat cGearJoint::Ratio() { + return cpGearJointGetRatio( mConstraint ); +} + +void cGearJoint::Ratio( const cpFloat& ratio ) { + cpGearJointSetRatio( mConstraint, ratio ); +} + +void cGearJoint::Draw() { + // Not implemented +} + +}} diff --git a/src/physics/constraints/cgearjoint.hpp b/src/physics/constraints/cgearjoint.hpp new file mode 100644 index 000000000..475d8442a --- /dev/null +++ b/src/physics/constraints/cgearjoint.hpp @@ -0,0 +1,25 @@ +#ifndef EE_PHYSICS_CGEARJOINT_HPP +#define EE_PHYSICS_CGEARJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cGearJoint : public cConstraint { + public: + cGearJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratio ); + + cpFloat Phase(); + + void Phase( const cpFloat& phase ); + + cpFloat Ratio(); + + void Ratio( const cpFloat& ratio ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cgroovejoint.cpp b/src/physics/constraints/cgroovejoint.cpp new file mode 100644 index 000000000..18fbd136c --- /dev/null +++ b/src/physics/constraints/cgroovejoint.cpp @@ -0,0 +1,54 @@ +#include "cgroovejoint.hpp" + +namespace EE { namespace Physics { + +cGrooveJoint::cGrooveJoint( cBody * a, cBody * b, cpVect groove_a, cpVect groove_b, cpVect anchr2 ) { + mConstraint = cpGrooveJointNew( a->Body(), b->Body(), groove_a, groove_b, anchr2 ); + SetData(); +} + +cpVect cGrooveJoint::Anchr2() { + return cpGrooveJointGetAnchr2( mConstraint ); +} + +void cGrooveJoint::Anchr2( const cpVect& anchr2 ) { + cpGrooveJointSetAnchr2( mConstraint, anchr2 ); +} + +cpVect cGrooveJoint::GrooveA() { + return cpGrooveJointGetGrooveA( mConstraint ); +} + +void cGrooveJoint::GrooveA( const cpVect& groove_a ) { + cpGrooveJointSetGrooveA( mConstraint, groove_a ); +} + +cpVect cGrooveJoint::GrooveB() { + return cpGrooveJointGetGrooveB( mConstraint ); +} + +void cGrooveJoint::GrooveB( const cpVect& groove_b ) { + cpGrooveJointSetGrooveB( mConstraint, groove_b ); +} + +void cGrooveJoint::Draw() { + cpGrooveJoint *joint = (cpGrooveJoint *)mConstraint; + cpBody * body_a = mConstraint->a; + cpBody * body_b = mConstraint->b; + + cpVect a = cpvadd(body_a->p, cpvrotate(joint->grv_a, body_a->rot)); + cpVect b = cpvadd(body_a->p, cpvrotate(joint->grv_b, body_a->rot)); + cpVect c = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot)); + + glPointSize(5.0f); + glBegin(GL_POINTS); { + glVertex2f(c.x, c.y); + } glEnd(); + + glBegin(GL_LINES); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); +} + +}} diff --git a/src/physics/constraints/cgroovejoint.hpp b/src/physics/constraints/cgroovejoint.hpp new file mode 100644 index 000000000..8149d542f --- /dev/null +++ b/src/physics/constraints/cgroovejoint.hpp @@ -0,0 +1,29 @@ +#ifndef EE_PHYSICS_CGROOVEJOINT_HPP +#define EE_PHYSICS_CGROOVEJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cGrooveJoint : public cConstraint { + public: + cGrooveJoint( cBody * a, cBody * b, cpVect groove_a, cpVect groove_b, cpVect anchr2 ); + + cpVect Anchr2(); + + void Anchr2( const cpVect& anchr2 ); + + cpVect GrooveA(); + + void GrooveA( const cpVect& groove_a ); + + cpVect GrooveB(); + + void GrooveB( const cpVect& groove_b ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cpinjoint.cpp b/src/physics/constraints/cpinjoint.cpp new file mode 100644 index 000000000..1782fa451 --- /dev/null +++ b/src/physics/constraints/cpinjoint.cpp @@ -0,0 +1,54 @@ +#include "cpinjoint.hpp" + +namespace EE { namespace Physics { + +cPinJoint::cPinJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 ) { + mConstraint = cpPinJointNew( a->Body(), b->Body(), anchr1, anchr2 ); + SetData(); +} + +cpVect cPinJoint::Anchr1() { + return cpPinJointGetAnchr1( mConstraint ); +} + +void cPinJoint::Anchr1( const cpVect& anchr1 ) { + cpPinJointSetAnchr1( mConstraint, anchr1 ); +} + +cpVect cPinJoint::Anchr2() { + return cpPinJointGetAnchr2( mConstraint ); +} + +void cPinJoint::Anchr2( const cpVect& anchr2 ) { + cpPinJointSetAnchr2( mConstraint, anchr2 ); +} + +cpFloat cPinJoint::Dist() { + return cpPinJointGetDist( mConstraint ); +} + +void cPinJoint::Dist( const cpFloat& dist ) { + cpPinJointSetDist( mConstraint, dist ); +} + +void cPinJoint::Draw() { + cpPinJoint *joint = (cpPinJoint *)mConstraint; + cpBody * body_a = mConstraint->a; + cpBody * body_b = mConstraint->b; + + cpVect a = cpvadd(body_a->p, cpvrotate(joint->anchr1, body_a->rot)); + cpVect b = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot)); + + glPointSize(5.0f); + glBegin(GL_POINTS); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); + + glBegin(GL_LINES); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); +} + +}} diff --git a/src/physics/constraints/cpinjoint.hpp b/src/physics/constraints/cpinjoint.hpp new file mode 100644 index 000000000..64870214c --- /dev/null +++ b/src/physics/constraints/cpinjoint.hpp @@ -0,0 +1,29 @@ +#ifndef EE_PHYSICS_CPINJOINT_HPP +#define EE_PHYSICS_CPINJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cPinJoint : public cConstraint { + public: + cPinJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 ); + + cpVect Anchr1(); + + void Anchr1( const cpVect& anchr1 ); + + cpVect Anchr2(); + + void Anchr2( const cpVect& anchr2 ); + + cpFloat Dist(); + + void Dist( const cpFloat& dist ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cpivotjoint.cpp b/src/physics/constraints/cpivotjoint.cpp new file mode 100644 index 000000000..79c6c3c85 --- /dev/null +++ b/src/physics/constraints/cpivotjoint.cpp @@ -0,0 +1,46 @@ +#include "cpivotjoint.hpp" + +namespace EE { namespace Physics { + +cPivotJoint::cPivotJoint( cBody * a, cBody * b, cpVect pivot ) { + mConstraint = cpPivotJointNew( a->Body(), b->Body(), pivot ); + SetData(); +} + +cPivotJoint::cPivotJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 ) { + mConstraint = cpPivotJointNew2( a->Body(), b->Body(), anchr1, anchr2 ); + SetData(); +} + +cpVect cPivotJoint::Anchr1() { + return cpPivotJointGetAnchr1( mConstraint ); +} + +void cPivotJoint::Anchr1( const cpVect& anchr1 ) { + cpPivotJointSetAnchr1( mConstraint, anchr1 ); +} + +cpVect cPivotJoint::Anchr2() { + return cpPivotJointGetAnchr2( mConstraint ); +} + +void cPivotJoint::Anchr2( const cpVect& anchr2 ) { + cpPivotJointSetAnchr2( mConstraint, anchr2 ); +} + +void cPivotJoint::Draw() { + cpBody * body_a = mConstraint->a; + cpBody * body_b = mConstraint->b; + cpPivotJoint *joint = (cpPivotJoint *)mConstraint; + + cpVect a = cpvadd(body_a->p, cpvrotate(joint->anchr1, body_a->rot)); + cpVect b = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot)); + + glPointSize(10.0f); + glBegin(GL_POINTS); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); +} + +}} diff --git a/src/physics/constraints/cpivotjoint.hpp b/src/physics/constraints/cpivotjoint.hpp new file mode 100644 index 000000000..fd58cb95b --- /dev/null +++ b/src/physics/constraints/cpivotjoint.hpp @@ -0,0 +1,27 @@ +#ifndef EE_PHYSICS_CPIVOTJOINT_HPP +#define EE_PHYSICS_CPIVOTJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cPivotJoint : public cConstraint { + public: + cPivotJoint( cBody * a, cBody * b, cpVect pivot ); + + cPivotJoint( cBody * a, cBody * b, cpVect anchr1, cpVect anchr2 ); + + cpVect Anchr1(); + + void Anchr1( const cpVect& anchr1 ); + + cpVect Anchr2(); + + void Anchr2( const cpVect& anchr2 ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cratchetjoint.cpp b/src/physics/constraints/cratchetjoint.cpp new file mode 100644 index 000000000..1573b3b83 --- /dev/null +++ b/src/physics/constraints/cratchetjoint.cpp @@ -0,0 +1,38 @@ +#include "cratchetjoint.hpp" + +namespace EE { namespace Physics { + +cRatchetJoint::cRatchetJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratchet ) { + mConstraint = cpRatchetJointNew( a->Body(), b->Body(), phase, ratchet ); + SetData(); +} + +cpFloat cRatchetJoint::Angle() { + return cpRatchetJointGetAngle( mConstraint ); +} + +void cRatchetJoint::Angle( const cpFloat& angle ) { + cpRatchetJointSetAngle( mConstraint, angle ); +} + +cpFloat cRatchetJoint::Phase() { + return cpRatchetJointGetPhase( mConstraint ); +} + +void cRatchetJoint::Phase( const cpFloat& phase ) { + cpRatchetJointSetPhase( mConstraint, phase ); +} + +cpFloat cRatchetJoint::Ratchet() { + return cpRatchetJointGetRatchet( mConstraint ); +} + +void cRatchetJoint::Ratchet( const cpFloat& ratchet ) { + cpRatchetJointSetRatchet( mConstraint, ratchet ); +} + +void cRatchetJoint::Draw() { + // Not implemented +} + +}} diff --git a/src/physics/constraints/cratchetjoint.hpp b/src/physics/constraints/cratchetjoint.hpp new file mode 100644 index 000000000..406804ae8 --- /dev/null +++ b/src/physics/constraints/cratchetjoint.hpp @@ -0,0 +1,29 @@ +#ifndef EE_PHYSICS_CRATCHETJOINT_HPP +#define EE_PHYSICS_CRATCHETJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cRatchetJoint : public cConstraint { + public: + cRatchetJoint( cBody * a, cBody * b, cpFloat phase, cpFloat ratchet ); + + cpFloat Angle(); + + void Angle( const cpFloat& angle ); + + cpFloat Phase(); + + void Phase( const cpFloat& phase ); + + cpFloat Ratchet(); + + void Ratchet( const cpFloat& ratchet ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/crotarylimitjoint.cpp b/src/physics/constraints/crotarylimitjoint.cpp new file mode 100644 index 000000000..918cc7158 --- /dev/null +++ b/src/physics/constraints/crotarylimitjoint.cpp @@ -0,0 +1,30 @@ +#include "crotarylimitjoint.hpp" + +namespace EE { namespace Physics { + +cRotaryLimitJoint::cRotaryLimitJoint( cBody * a, cBody * b, cpFloat min, cpFloat max ) { + mConstraint = cpRotaryLimitJointNew( a->Body(), b->Body(), min, max ); + SetData(); +} + +cpFloat cRotaryLimitJoint::Min() { + return cpRotaryLimitJointGetMin( mConstraint ); +} + +void cRotaryLimitJoint::Min( const cpFloat& min ) { + cpRotaryLimitJointSetMin( mConstraint, min ); +} + +cpFloat cRotaryLimitJoint::Max() { + return cpRotaryLimitJointGetMax( mConstraint ); +} + +void cRotaryLimitJoint::Max( const cpFloat& max ) { + cpRotaryLimitJointSetMax( mConstraint, max ); +} + +void cRotaryLimitJoint::Draw() { + // Not implemented +} + +}} diff --git a/src/physics/constraints/crotarylimitjoint.hpp b/src/physics/constraints/crotarylimitjoint.hpp new file mode 100644 index 000000000..42f8ef689 --- /dev/null +++ b/src/physics/constraints/crotarylimitjoint.hpp @@ -0,0 +1,25 @@ +#ifndef EE_PHYSICS_CROTARYLIMITJOINT_HPP +#define EE_PHYSICS_CROTARYLIMITJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cRotaryLimitJoint : public cConstraint { + public: + cRotaryLimitJoint( cBody * a, cBody * b, cpFloat min, cpFloat max ); + + cpFloat Min(); + + void Min( const cpFloat& min ); + + cpFloat Max(); + + void Max( const cpFloat& max ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/csimplemotor.cpp b/src/physics/constraints/csimplemotor.cpp new file mode 100644 index 000000000..707bad282 --- /dev/null +++ b/src/physics/constraints/csimplemotor.cpp @@ -0,0 +1,22 @@ +#include "csimplemotor.hpp" + +namespace EE { namespace Physics { + +cSimpleMotor::cSimpleMotor( cBody * a, cBody * b, cpFloat rate ) { + mConstraint = cpSimpleMotorNew( a->Body(), b->Body(), rate ); + SetData(); +} + +cpFloat cSimpleMotor::Rate() { + return cpSimpleMotorGetRate( mConstraint ); +} + +void cSimpleMotor::Rate( const cpFloat& rate ) { + cpSimpleMotorSetRate( mConstraint, rate ); +} + +void cSimpleMotor::Draw() { + // Not implemented +} + +}} diff --git a/src/physics/constraints/csimplemotor.hpp b/src/physics/constraints/csimplemotor.hpp new file mode 100644 index 000000000..e43287115 --- /dev/null +++ b/src/physics/constraints/csimplemotor.hpp @@ -0,0 +1,21 @@ +#ifndef EE_PHYSICS_CSIMPLEMOTOR_HPP +#define EE_PHYSICS_CSIMPLEMOTOR_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cSimpleMotor : public cConstraint { + public: + cSimpleMotor( cBody * a, cBody * b, cpFloat rate ); + + cpFloat Rate(); + + void Rate( const cpFloat& rate ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/constraints/cslidejoint.cpp b/src/physics/constraints/cslidejoint.cpp new file mode 100644 index 000000000..f94002e01 --- /dev/null +++ b/src/physics/constraints/cslidejoint.cpp @@ -0,0 +1,62 @@ +#include "cslidejoint.hpp" + +namespace EE { namespace Physics { + +cSlideJoint::cSlideJoint( cBody * a, cBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max ) { + mConstraint = cpSlideJointNew( a->Body(), b->Body(), anchr1, anchr2, min, max ); + SetData(); +} + +cpVect cSlideJoint::Anchr1() { + return cpSlideJointGetAnchr1( mConstraint ); +} + +void cSlideJoint::Anchr1( const cpVect& anchr1 ) { + cpSlideJointSetAnchr1( mConstraint, anchr1 ); +} + +cpVect cSlideJoint::Anchr2() { + return cpSlideJointGetAnchr2( mConstraint ); +} + +void cSlideJoint::Anchr2( const cpVect& anchr2 ) { + cpSlideJointSetAnchr2( mConstraint, anchr2 ); +} + +cpFloat cSlideJoint::Min() { + return cpSlideJointGetMin( mConstraint ); +} + +void cSlideJoint::Min( const cpFloat& min ) { + cpSlideJointSetMin( mConstraint, min ); +} + +cpFloat cSlideJoint::Max() { + return cpSlideJointGetMax( mConstraint ); +} + +void cSlideJoint::Max( const cpFloat& max ) { + cpSlideJointSetMax( mConstraint, max ); +} + +void cSlideJoint::Draw() { + cpBody * body_a = mConstraint->a; + cpBody * body_b = mConstraint->b; + cpSlideJoint *joint = (cpSlideJoint *)mConstraint; + + cpVect a = cpvadd(body_a->p, cpvrotate(joint->anchr1, body_a->rot)); + cpVect b = cpvadd(body_b->p, cpvrotate(joint->anchr2, body_b->rot)); + + glPointSize(5.0f); + glBegin(GL_POINTS); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); + + glBegin(GL_LINES); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); +} + +}} diff --git a/src/physics/constraints/cslidejoint.hpp b/src/physics/constraints/cslidejoint.hpp new file mode 100644 index 000000000..386c4b5a4 --- /dev/null +++ b/src/physics/constraints/cslidejoint.hpp @@ -0,0 +1,33 @@ +#ifndef EE_PHYSICS_CSLIDEJOINT_HPP +#define EE_PHYSICS_CSLIDEJOINT_HPP + +#include "cconstraint.hpp" + +namespace EE { namespace Physics { + +class cSlideJoint : public cConstraint { + public: + cSlideJoint( cBody * a, cBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max ); + + cpVect Anchr1(); + + void Anchr1( const cpVect& anchr1 ); + + cpVect Anchr2(); + + void Anchr2( const cpVect& anchr2 ); + + cpFloat Min(); + + void Min( const cpFloat& min ); + + cpFloat Max(); + + void Max( const cpFloat& max ); + + virtual void Draw(); +}; + +}} + +#endif diff --git a/src/physics/cphysicsmanager.cpp b/src/physics/cphysicsmanager.cpp new file mode 100644 index 000000000..7a35f9009 --- /dev/null +++ b/src/physics/cphysicsmanager.cpp @@ -0,0 +1,48 @@ +#include "cphysicsmanager.hpp" + +namespace EE { namespace Physics { + +cPhysicsManager::cPhysicsManager() { + cpInitChipmunk(); +} + +cPhysicsManager::~cPhysicsManager() { +} + +const cpFloat& cPhysicsManager::BiasCoef() const { + return cp_bias_coef; +} + +void cPhysicsManager::BiasCoef( const cpFloat& biasCoef ) { + cp_bias_coef = biasCoef; +} + +const cpFloat& cPhysicsManager::ConstraintBiasCoef() const { + return cp_constraint_bias_coef; +} + +void cPhysicsManager::ConstraintBiasCoef( const cpFloat& constraintBiasCoef ) { + cp_constraint_bias_coef = constraintBiasCoef; +} + +const cpTimestamp& cPhysicsManager::ContactPersistence() const { + return cp_contact_persistence; +} + +void cPhysicsManager::ContactPersistence( const cpTimestamp& timestamp ) { + cp_contact_persistence = timestamp; +} + +const cpFloat& cPhysicsManager::CollisionSlop() const { + return cp_collision_slop; +} + +void cPhysicsManager::CollisionSlop( const cpFloat& slop ) { + cp_collision_slop = slop; +} + +cPhysicsManager::cDrawSpaceOptions * cPhysicsManager::GetDrawOptions() { + return &mOptions; +} + +}} diff --git a/src/physics/cphysicsmanager.hpp b/src/physics/cphysicsmanager.hpp new file mode 100644 index 000000000..80f2d228a --- /dev/null +++ b/src/physics/cphysicsmanager.hpp @@ -0,0 +1,57 @@ +#ifndef EE_PHYSICS_PHYSICSMANAGER_HPP +#define EE_PHYSICS__PHYSICSMANAGER_HPP + +#include "base.hpp" + +namespace EE { namespace Physics { + +class EE_API cPhysicsManager : public tSingleton { + friend class tSingleton; + public: + class cDrawSpaceOptions { + public: + cDrawSpaceOptions() : + DrawHash( false ), + DrawBBs( false ), + DrawShapes( true ), + CollisionPointSize( 4.0f ), + BodyPointSize( 0.0f ), + LineThickness( 1.5f ) + {} + + int DrawHash; + int DrawBBs; + int DrawShapes; + cpFloat CollisionPointSize; + cpFloat BodyPointSize; + cpFloat LineThickness; + }; + + cPhysicsManager(); + + ~cPhysicsManager(); + + const cpFloat& BiasCoef() const; + + void BiasCoef( const cpFloat& biasCoef ); + + const cpFloat& ConstraintBiasCoef() const; + + void ConstraintBiasCoef( const cpFloat& constraintBiasCoef ); + + const cpTimestamp& ContactPersistence() const; + + void ContactPersistence( const cpTimestamp& timestamp ); + + const cpFloat& CollisionSlop() const; + + void CollisionSlop( const cpFloat& slop ); + + cPhysicsManager::cDrawSpaceOptions * GetDrawOptions(); + protected: + cDrawSpaceOptions mOptions; +}; + +}} + +#endif diff --git a/src/physics/cshape.cpp b/src/physics/cshape.cpp new file mode 100644 index 000000000..cae8dae12 --- /dev/null +++ b/src/physics/cshape.cpp @@ -0,0 +1,143 @@ +#include "cshape.hpp" +#include "cshapecircle.hpp" +#include "cshapesegment.hpp" +#include "cshapepoly.hpp" + +namespace EE { namespace Physics { + +void cShape::ResetShapeIdCounter() { + cpResetShapeIdCounter(); +} + +cShape::cShape() { +} + +cShape::~cShape() { +} + +void cShape::SetData() { + mShape->data = (void*)this; +} + +cpShape * cShape::Shape() const { + return mShape; +} + +cpBB cShape::CacheBB() { + return cpShapeCacheBB( mShape ); +} + +cpBB cShape::Update( cpVect pos, cpVect rot ) { + return cpShapeUpdate( mShape, pos, rot ); +} + +bool cShape::PointQuery( cpVect p ) { + return 0 != cpShapePointQuery( mShape, p ); +} + +cBody * cShape::Body() const { + return reinterpret_cast( mShape->body->data ); +} + +void cShape::Body( cBody * body ) { + mShape->body = body->Body(); +} + +cpBB cShape::BB() const { + return mShape->bb; +} + +void cShape::BB( const cpBB& bb ) { + mShape->bb = bb; +} + +bool cShape::Sensor() { + return 0 != mShape->sensor; +} + +void cShape::Sensor( const bool& sensor ) { + mShape->sensor = sensor ? 1 : 0; +} + +cpFloat cShape::e() const { + return mShape->e; +} + +void cShape::e( const cpFloat& e ) { + mShape->e = e; +} + +cpFloat cShape::u() const { + return mShape->u; +} + +void cShape::u( const cpFloat& u ) { + mShape->u = u; +} + +cpVect cShape::SurfaceVel() const { + return mShape->surface_v; +} + +void cShape::SurfaceVel( const cpVect& vel ) { + mShape->surface_v = vel; +} + +cpCollisionType cShape::CollisionType() const { + return mShape->collision_type; +} + +void cShape::CollisionType( const cpCollisionType& type ) { + mShape->collision_type = type; +} + +cpGroup cShape::Group() const { + return mShape->group; +} + +void cShape::Group( const cpGroup& group ) { + mShape->group = group; +} + +cpLayers cShape::Layers() const { + return mShape->layers; +} + +void cShape::Layers( const cpLayers& layers ) { + mShape->layers = layers; +} + +cpShapeType cShape::Type() const { + return mShape->klass->type; +} + +cShapePoly * cShape::GetAsPoly() { + eeASSERT( Type() == CP_POLY_SHAPE ); + + return reinterpret_cast( this ); +} + +cShapeCircle * cShape::GetAsCircle() { + eeASSERT( Type() == CP_CIRCLE_SHAPE ); + + return reinterpret_cast( this ); +} + +cShapeSegment * cShape::GetAsSegment() { + eeASSERT( Type() == CP_SEGMENT_SHAPE ); + + return reinterpret_cast( this ); +} + +void cShape::DrawBB() { + glBegin(GL_LINE_LOOP); + + glVertex2f(mShape->bb.l, mShape->bb.b); + glVertex2f(mShape->bb.l, mShape->bb.t); + glVertex2f(mShape->bb.r, mShape->bb.t); + glVertex2f(mShape->bb.r, mShape->bb.b); + + glEnd(); +} + +}} diff --git a/src/physics/cshape.hpp b/src/physics/cshape.hpp new file mode 100644 index 000000000..efd16ef7c --- /dev/null +++ b/src/physics/cshape.hpp @@ -0,0 +1,86 @@ +#ifndef EE_PHYSICS_CSHAPE_HPP +#define EE_PHYSICS_CSHAPE_HPP + +#include "base.hpp" +#include "cbody.hpp" + +namespace EE { namespace Physics { + +class cShapeCircle; +class cShapeSegment; +class cShapePoly; +class cSpace; + +class cShape { + public: + static void ResetShapeIdCounter(); + + cpShape * Shape() const; + + ~cShape(); + + cBody * Body() const; + + void Body( cBody * body ); + + cpBB BB() const; + + void BB( const cpBB& bb ); + + bool Sensor(); + + void Sensor( const bool& sensor ); + + cpFloat e() const; + + void e( const cpFloat& e ); + + cpFloat u() const; + + void u( const cpFloat& u ); + + cpVect SurfaceVel() const; + + void SurfaceVel( const cpVect& vel ); + + cpCollisionType CollisionType() const; + + void CollisionType( const cpCollisionType& type ); + + cpGroup Group() const; + + void Group( const cpGroup& group ); + + cpLayers Layers() const; + + void Layers( const cpLayers& layers ); + + cpBB CacheBB(); + + cpBB Update( cpVect pos, cpVect rot ); + + bool PointQuery( cpVect p ); + + cpShapeType Type() const; + + cShapePoly * GetAsPoly(); + + cShapeCircle * GetAsCircle(); + + cShapeSegment * GetAsSegment(); + + virtual void Draw( cSpace * space ) = 0; + + virtual void DrawBB(); + protected: + cShape(); + + cpShape * mShape; + cBody * mBody; + + void SetData(); +}; + +}} + +#endif diff --git a/src/physics/cshapecircle.cpp b/src/physics/cshapecircle.cpp new file mode 100644 index 000000000..a6d5eba86 --- /dev/null +++ b/src/physics/cshapecircle.cpp @@ -0,0 +1,57 @@ +#include "cshapecircle.hpp" +#include "cspace.hpp" + +namespace EE { namespace Physics { + +cShapeCircle::cShapeCircle( cBody * body, cpFloat radius, cpVect offset ) { + mShape = cpCircleShapeNew( body->Body(), radius, offset ); + SetData(); +} + +cpVect cShapeCircle::Offset() { + return cpCircleShapeGetOffset( mShape ); +} + +void cShapeCircle::Offset( const cpVect &offset ) { + cpCircleShapeSetOffset( mShape, offset ); +} + +cpFloat cShapeCircle::Radius() { + return cpCircleShapeGetRadius( mShape ); +} + +void cShapeCircle::Radius( const cpFloat& radius ) { + cpCircleShapeSetRadius( mShape, radius ); +} + +void cShapeCircle::Draw( cSpace * space ) { + cPrimitives p; + + cpCircleShape * cs = (cpCircleShape*)mShape; + + p.DrawCircle( cs->tc.x, cs->tc.y, cs->r ); + + /*glVertexPointer( 2, GL_FLOAT, 0, circleVAR ); + + cpCircleShape * cs = (cpCircleShape*)mShape; + + glPushMatrix(); + + cpVect center = cs->tc; + + glTranslatef( center.x, center.y, 0.0f ); + glRotatef( space->StaticBody()->Body()->a * 180.0f / M_PI, 0.0f, 0.0f, 1.0f); + + glScalef( cs->r, cs->r, 1.0f); + + if(!cs->shape.sensor){ + glColor_for_shape( mShape, space->Space() ); + glDrawArrays(GL_TRIANGLE_FAN, 0, circleVAR_count - 1); + } + + glColor3f(LINE_COLOR); + glDrawArrays(GL_LINE_STRIP, 0, circleVAR_count); + glPopMatrix();*/ +} + +}} diff --git a/src/physics/cshapecircle.hpp b/src/physics/cshapecircle.hpp new file mode 100644 index 000000000..cb1fa095f --- /dev/null +++ b/src/physics/cshapecircle.hpp @@ -0,0 +1,25 @@ +#ifndef EE_PHYSICS_CSHAPECIRCLE_HPP +#define EE_PHYSICS_CSHAPECIRCLE_HPP + +#include "cshape.hpp" + +namespace EE { namespace Physics { + +class cShapeCircle : public cShape { + public: + cShapeCircle( cBody * body, cpFloat radius, cpVect offset ); + + cpVect Offset(); + + void Offset( const cpVect& offset ); + + cpFloat Radius(); + + void Radius( const cpFloat& radius ); + + virtual void Draw( cSpace * space ); +}; + +}} + +#endif diff --git a/src/physics/cshapepoly.cpp b/src/physics/cshapepoly.cpp new file mode 100644 index 000000000..83d3add8e --- /dev/null +++ b/src/physics/cshapepoly.cpp @@ -0,0 +1,52 @@ +#include "cshapepoly.hpp" +#include "cspace.hpp" + +namespace EE { namespace Physics { + +cShapePoly::cShapePoly( cBody * body, int numVerts, cpVect *verts, cpVect offset ) { + mShape = cpPolyShapeNew( body->Body(), numVerts, verts, offset ); + SetData(); +} + +cShapePoly::cShapePoly( cBody * body, cpFloat width, cpFloat height ) : + cShape() +{ + mShape = cpBoxShapeNew( body->Body(), width, height ); +} + +bool cShapePoly::Validate( const cpVect * verts, const int numVerts ) { + return 0 != cpPolyValidate( verts, numVerts ); +} + +int cShapePoly::GetNumVerts() { + return cpPolyShapeGetNumVerts( mShape ); +} + +cpVect cShapePoly::GetVert( int idx ) { + return cpPolyShapeGetVert( mShape, idx ); +} + +void cShapePoly::SetVerts( int numVerts, cpVect *verts, cpVect offset ) { + cpPolyShapeSetVerts( mShape, numVerts, verts, offset ); +} + +void cShapePoly::Draw( cSpace * space ) { + cpPolyShape *poly = (cpPolyShape*)mShape; + + int count = poly->numVerts; +#if CP_USE_DOUBLES + glVertexPointer(2, GL_DOUBLE, 0, poly->tVerts); +#else + glVertexPointer(2, GL_FLOAT, 0, poly->tVerts); +#endif + + if(!poly->shape.sensor){ + glColor_for_shape((cpShape *)poly, space->Space()); + glDrawArrays(GL_TRIANGLE_FAN, 0, count); + } + + glColor3f(LINE_COLOR); + glDrawArrays(GL_LINE_LOOP, 0, count); +} + +}} diff --git a/src/physics/cshapepoly.hpp b/src/physics/cshapepoly.hpp new file mode 100644 index 000000000..53d217496 --- /dev/null +++ b/src/physics/cshapepoly.hpp @@ -0,0 +1,27 @@ +#ifndef EE_PHYSICS_CSHAPEPOLY_HPP +#define EE_PHYSICS_CSHAPEPOLY_HPP + +#include "cshape.hpp" + +namespace EE { namespace Physics { + +class cShapePoly : public cShape { + public: + cShapePoly( cBody * body, int numVerts, cpVect *verts, cpVect offset ); + + cShapePoly( cBody * body, cpFloat width, cpFloat height ); + + static bool Validate( const cpVect * verts, const int numVerts ); + + int GetNumVerts(); + + cpVect GetVert( int idx ); + + void SetVerts( int numVerts, cpVect *verts, cpVect offset ); + + virtual void Draw( cSpace * space ); +}; + +}} + +#endif diff --git a/src/physics/cshapesegment.cpp b/src/physics/cshapesegment.cpp new file mode 100644 index 000000000..3d6e147ce --- /dev/null +++ b/src/physics/cshapesegment.cpp @@ -0,0 +1,91 @@ +#include "cshapesegment.hpp" +#include "cspace.hpp" + +namespace EE { namespace Physics { + +cShapeSegment::cShapeSegment( cBody * body, cpVect a, cpVect b, cpFloat radius ) { + mShape = cpSegmentShapeNew( body->Body(), a, b, radius ); + SetData(); +} + +cpVect cShapeSegment::A() { + return cpSegmentShapeGetA( mShape ); +} + +cpVect cShapeSegment::B() { + return cpSegmentShapeGetB( mShape ); +} + +cpVect cShapeSegment::Normal() { + return cpSegmentShapeGetNormal( mShape ); +} + +cpFloat cShapeSegment::Radius() { + return cpSegmentShapeGetRadius( mShape ); +} + +void cShapeSegment::Radius( const cpFloat& radius ) { + cpSegmentShapeSetRadius( mShape, radius ); +} + +void cShapeSegment::Endpoints( const cpVect& a, const cpVect& b ) { + cpSegmentShapeSetEndpoints( mShape, a, b ); +} + +bool cShapeSegment::Query( cpVect a, cpVect b, cpSegmentQueryInfo * info ) { + return 0 != cpShapeSegmentQuery( mShape, a, b, info ); +} + +cpVect cShapeSegment::HitPoint( const cpVect start, const cpVect end, const cpSegmentQueryInfo info ) { + return cpSegmentQueryHitPoint( start, end, info ); +} + +cpFloat cShapeSegment::HitDist( const cpVect start, const cpVect end, const cpSegmentQueryInfo info ) { + return cpSegmentQueryHitDist( start, end, info ); +} + +void cShapeSegment::Draw( cSpace * space ) { + cPrimitives p; + + cpSegmentShape * seg = (cpSegmentShape *)mShape; + cpVect a = seg->ta; + cpVect b = seg->tb; + + p.DrawLine( eeVector2f( a.x, a.y ), eeVector2f( b.x, b.y ) ); + /*cpSegmentShape * seg = (cpSegmentShape *)mShape; + + cpVect a = seg->ta; + cpVect b = seg->tb; + + if(seg->r){ + glVertexPointer(3, GL_FLOAT, 0, pillVAR); + glPushMatrix(); { + cpVect d = cpvsub(b, a); + cpVect r = cpvmult(d, seg->r/cpvlength(d)); + + const GLfloat matrix[] = { + r.x, r.y, 0.0f, 0.0f, + -r.y, r.x, 0.0f, 0.0f, + d.x, d.y, 0.0f, 0.0f, + a.x, a.y, 0.0f, 1.0f, + }; + glMultMatrixf(matrix); + + if(!seg->shape.sensor){ + glColor_for_shape((cpShape *)seg, space->Space()); + glDrawArrays(GL_TRIANGLE_FAN, 0, pillVAR_count); + } + + glColor3f(LINE_COLOR); + glDrawArrays(GL_LINE_LOOP, 0, pillVAR_count); + } glPopMatrix(); + } else { + glColor3f(LINE_COLOR); + glBegin(GL_LINES); { + glVertex2f(a.x, a.y); + glVertex2f(b.x, b.y); + } glEnd(); + }*/ +} + +}} diff --git a/src/physics/cshapesegment.hpp b/src/physics/cshapesegment.hpp new file mode 100644 index 000000000..dda65aabf --- /dev/null +++ b/src/physics/cshapesegment.hpp @@ -0,0 +1,35 @@ +#ifndef EE_PHYSICS_CSHAPESEGMENT_HPP +#define EE_PHYSICS_CSHAPESEGMENT_HPP + +#include "cshape.hpp" + +namespace EE { namespace Physics { + +class cShapeSegment : public cShape { + public: + cShapeSegment( cBody * body, cpVect a, cpVect b, cpFloat radius ); + + cpVect A(); + + cpVect B(); + + cpVect Normal(); + + cpFloat Radius(); + + void Radius( const cpFloat& radius ); + + void Endpoints( const cpVect& a, const cpVect& b ); + + bool Query( cpVect a, cpVect b, cpSegmentQueryInfo * info ); + + static cpVect HitPoint( const cpVect start, const cpVect end, const cpSegmentQueryInfo info ); + + static cpFloat HitDist( const cpVect start, const cpVect end, const cpSegmentQueryInfo info ); + + virtual void Draw( cSpace * space ); +}; + +}} + +#endif diff --git a/src/physics/cspace.cpp b/src/physics/cspace.cpp new file mode 100644 index 000000000..861a383bf --- /dev/null +++ b/src/physics/cspace.cpp @@ -0,0 +1,238 @@ +#include "cspace.hpp" +#include "cphysicsmanager.hpp" + +namespace EE { namespace Physics { + +cSpace::cSpace() { + mSpace = cpSpaceNew(); + mSpace->data = (void*)this; + mStaticBody = eeNew( cBody, ( mSpace->staticBody ) ); +} + +cSpace::~cSpace() { + cpSpaceFreeChildren( mSpace ); + cpSpaceFree( mSpace ); + + std::list::iterator itc = mConstraints.begin(); + for ( ; itc != mConstraints.end(); itc++ ) + eeSAFE_DELETE( *itc ); + + std::list::iterator its = mShapes.begin(); + for ( ; its != mShapes.end(); its++ ) + eeSAFE_DELETE( *its ); + + std::list::iterator itb = mBodys.begin(); + for ( ; itb != mBodys.end(); itb++ ) + eeSAFE_DELETE( *itb ); + + eeSAFE_DELETE( mStaticBody ); +} + +void cSpace::Update( const cpFloat& dt ) { + cpSpaceStep( mSpace, dt ); +} + +void cSpace::Update() { + Update( cEngine::instance()->Elapsed() / 1000 ); +} + +void cSpace::ResizeStaticHash( cpFloat dim, int count ) { + cpSpaceResizeStaticHash( mSpace, dim, count ); +} + +void cSpace::ResizeActiveHash( cpFloat dim, int count ) { + cpSpaceResizeActiveHash( mSpace, dim, count ); +} + +void cSpace::RehashStatic() { + cpSpaceRehashStatic( mSpace ); +} + +const int& cSpace::Iterations() const { + return mSpace->iterations; +} + +void cSpace::Iterations( const int& iterations ) { + mSpace->iterations = iterations; +} + +const cpVect& cSpace::Gravity() const { + return mSpace->gravity; +} + +void cSpace::Gravity( const cpVect& gravity ) { + mSpace->gravity = gravity; +} + +const cpFloat& cSpace::Damping() const { + return mSpace->damping; +} + +void cSpace::Damping( const cpFloat& damping ) { + mSpace->damping = damping; +} + +const cpFloat& cSpace::IdleSpeedThreshold() const { + return mSpace->idleSpeedThreshold; +} + +void cSpace::IdleSpeedThreshold( const cpFloat& idleSpeedThreshold ) { + mSpace->idleSpeedThreshold = idleSpeedThreshold; +} + +const cpFloat& cSpace::SleepTimeThreshold() const { + return mSpace->sleepTimeThreshold; +} + +void cSpace::SleepTimeThreshold( const cpFloat& sleepTimeThreshold ) { + mSpace->sleepTimeThreshold = sleepTimeThreshold; +} + +cBody * cSpace::StaticBody() const { + return mStaticBody; +} + +cShape * cSpace::AddShape( cShape * shape ) { + cpSpaceAddShape( mSpace, shape->Shape() ); + mShapes.push_back( shape ); + return shape; +} + +cShape * cSpace::AddStaticShape( cShape * shape ) { + cpSpaceAddStaticShape( mSpace, shape->Shape() ); + mShapes.push_back( shape ); + return shape; +} + +cBody * cSpace::AddBody( cBody * body ) { + cpSpaceAddBody( mSpace, body->Body() ); + mBodys.push_back( body ); + return body; +} + +cConstraint * cSpace::AddConstraint( cConstraint * constraint ) { + cpSpaceAddConstraint( mSpace, constraint->Constraint() ); + mConstraints.push_back( constraint ); + return constraint; +} + +void cSpace::RemoveShape( cShape * shape ) { + cpSpaceRemoveShape( mSpace, shape->Shape() ); + mShapes.remove( shape ); +} + +void cSpace::RemoveStaticShape( cShape * shape ) { + cpSpaceRemoveStaticShape( mSpace, shape->Shape() ); + mShapes.remove( shape ); +} + +void cSpace::RemoveBody( cBody * body ) { + cpSpaceRemoveBody( mSpace, body->Body() ); + mBodys.remove( body ); +} + +void cSpace::RemoveConstraint( cConstraint * constraint ) { + cpSpaceRemoveConstraint( mSpace, constraint->Constraint() ); + mConstraints.remove( constraint ); +} + +cpSpace * cSpace::Space() const { + return mSpace; +} + +void cSpace::ActivateShapesTouchingShape( cShape * shape ) { + cpSpaceActivateShapesTouchingShape( mSpace, shape->Shape() ); +} + +void cSpace::RehashShape( cShape * shape ) { + cpSpaceRehashShape( mSpace, shape->Shape() ); +} + +static void drawObject( cpShape * shape, cpSpace * space ) { + reinterpret_cast ( shape->data )->Draw( reinterpret_cast( space->data ) ); +} + +static void drawBB( cpShape *shape, void * unused ) { + reinterpret_cast ( shape->data )->DrawBB(); +} + +static void drawConstraint( cpConstraint *constraint ) { + reinterpret_cast ( constraint->data )->Draw(); +} + +void cSpace::Draw() { + cPhysicsManager::cDrawSpaceOptions * options = cPhysicsManager::instance()->GetDrawOptions(); + + if( options->DrawHash ) { + glColor3f(0.5, 0.5, 0.5); + //cpBBTreeRenderDebug( mSpace->staticShapes ); + + glColor3f(0, 1, 0); + //cpBBTreeRenderDebug( mSpace->activeShapes ); + } + + glLineWidth( options->LineThickness ); + + if( options->DrawShapes ) { + cpSpatialIndexEach( mSpace->activeShapes, (cpSpatialIndexIterator)drawObject, mSpace ); + cpSpatialIndexEach( mSpace->staticShapes, (cpSpatialIndexIterator)drawObject, mSpace ); + } + + glLineWidth( 1.0f ); + + if( options->DrawBBs ){ + glColor3f( 0.3f, 0.5f, 0.3f ); + + cpSpatialIndexEach(mSpace->activeShapes, (cpSpatialIndexIterator)drawBB, NULL); + cpSpatialIndexEach(mSpace->staticShapes, (cpSpatialIndexIterator)drawBB, NULL); + } + + cpArray * constraints = mSpace->constraints; + + glColor3f(0.5f, 1.0f, 0.5f); + + for( int i=0, count = constraints->num; iarr[i] ); + } + + if( options->BodyPointSize ){ + glPointSize( options->BodyPointSize ); + + glBegin(GL_POINTS); + + glColor3f(LINE_COLOR); + + cpArray * bodies = mSpace->bodies; + + for( int i=0, count = bodies->num; iarr[i]; + + glVertex2f( body->p.x, body->p.y ); + } + + glEnd(); + } + + if( options->CollisionPointSize ){ + glPointSize( options->CollisionPointSize ); + + glBegin( GL_POINTS ); + + cpArray * arbiters = mSpace->arbiters; + + for( int i = 0; i < arbiters->num; i++ ){ + cpArbiter *arb = (cpArbiter*)arbiters->arr[i]; + + glColor3f( COLLISION_COLOR ); + + for(int i=0; inumContacts; i++){ + cpVect v = arb->contacts[i].p; + glVertex2f( v.x, v.y ); + } + } + + glEnd(); + } +} + +}} diff --git a/src/physics/cspace.hpp b/src/physics/cspace.hpp new file mode 100644 index 000000000..ddaf5b6f2 --- /dev/null +++ b/src/physics/cspace.hpp @@ -0,0 +1,82 @@ +#ifndef EE_PHYSICS_CSPACE_HPP +#define EE_PHYSICS_CSPACE_HPP + +#include "base.hpp" +#include "cbody.hpp" +#include "cshape.hpp" +#include "constraints/cconstraint.hpp" + +namespace EE { namespace Physics { + +class cSpace { + public: + cSpace(); + + ~cSpace(); + + void Update( const cpFloat& dt ); + + void Update(); + + cBody * StaticBody() const; + + const int& Iterations() const; + + void Iterations( const int& iterations ); + + const cpVect& Gravity() const; + + void Gravity( const cpVect& gravity ); + + const cpFloat& Damping() const; + + void Damping( const cpFloat& damping ); + + const cpFloat& IdleSpeedThreshold() const; + + void IdleSpeedThreshold( const cpFloat& idleSpeedThreshold ); + + const cpFloat& SleepTimeThreshold() const; + + void SleepTimeThreshold( const cpFloat& sleepTimeThreshold ); + + cShape * AddShape( cShape *shape ); + + cShape * AddStaticShape( cShape *shape ); + + cBody * AddBody( cBody * body ); + + cConstraint * AddConstraint( cConstraint * constraint ); + + void RemoveShape( cShape * shape); + + void RemoveStaticShape( cShape * shape ); + + void RemoveBody( cBody * body ); + + void RemoveConstraint( cConstraint * constraint ); + + void ResizeStaticHash( cpFloat dim, int count ); + + void ResizeActiveHash( cpFloat dim, int count ); + + void RehashStatic(); + + void RehashShape( cShape * shape ); + + cpSpace * Space() const; + + void ActivateShapesTouchingShape( cShape * shape ); + + virtual void Draw(); + protected: + cpSpace * mSpace; + cBody * mStaticBody; + std::list mBodys; + std::list mShapes; + std::list mConstraints; +}; + +}} + +#endif diff --git a/src/physics/physicshelper.hpp b/src/physics/physicshelper.hpp new file mode 100644 index 000000000..0bb838c32 --- /dev/null +++ b/src/physics/physicshelper.hpp @@ -0,0 +1,132 @@ +#ifndef EE_PHYSICS_HELPER +#define EE_PHYSICS_HELPER + +#define LINE_COLOR 1.0f, 1.0f, 1.0f +#define COLLISION_COLOR 1.0f, 0.0f, 0.0f +#define BODY_COLOR 0.0f, 0.0f, 1.0f + +static const GLfloat circleVAR[] = { + 0.0000f, 1.0000f, + 0.2588f, 0.9659f, + 0.5000f, 0.8660f, + 0.7071f, 0.7071f, + 0.8660f, 0.5000f, + 0.9659f, 0.2588f, + 1.0000f, 0.0000f, + 0.9659f, -0.2588f, + 0.8660f, -0.5000f, + 0.7071f, -0.7071f, + 0.5000f, -0.8660f, + 0.2588f, -0.9659f, + 0.0000f, -1.0000f, + -0.2588f, -0.9659f, + -0.5000f, -0.8660f, + -0.7071f, -0.7071f, + -0.8660f, -0.5000f, + -0.9659f, -0.2588f, + -1.0000f, -0.0000f, + -0.9659f, 0.2588f, + -0.8660f, 0.5000f, + -0.7071f, 0.7071f, + -0.5000f, 0.8660f, + -0.2588f, 0.9659f, + 0.0000f, 1.0000f, + 0.0f, 0.0f, // For an extra line to see the rotation. +}; + +static const int circleVAR_count = sizeof(circleVAR)/sizeof(GLfloat)/2; + +static const GLfloat pillVAR[] = { + 0.0000f, 1.0000f, 1.0f, + 0.2588f, 0.9659f, 1.0f, + 0.5000f, 0.8660f, 1.0f, + 0.7071f, 0.7071f, 1.0f, + 0.8660f, 0.5000f, 1.0f, + 0.9659f, 0.2588f, 1.0f, + 1.0000f, 0.0000f, 1.0f, + 0.9659f, -0.2588f, 1.0f, + 0.8660f, -0.5000f, 1.0f, + 0.7071f, -0.7071f, 1.0f, + 0.5000f, -0.8660f, 1.0f, + 0.2588f, -0.9659f, 1.0f, + 0.0000f, -1.0000f, 1.0f, + + 0.0000f, -1.0000f, 0.0f, + -0.2588f, -0.9659f, 0.0f, + -0.5000f, -0.8660f, 0.0f, + -0.7071f, -0.7071f, 0.0f, + -0.8660f, -0.5000f, 0.0f, + -0.9659f, -0.2588f, 0.0f, + -1.0000f, -0.0000f, 0.0f, + -0.9659f, 0.2588f, 0.0f, + -0.8660f, 0.5000f, 0.0f, + -0.7071f, 0.7071f, 0.0f, + -0.5000f, 0.8660f, 0.0f, + -0.2588f, 0.9659f, 0.0f, + 0.0000f, 1.0000f, 0.0f, +}; +static const int pillVAR_count = sizeof(pillVAR)/sizeof(GLfloat)/3; + +inline void glColor_from_pointer(void *ptr) { + unsigned long val = (long)ptr; + + // hash the pointer up nicely + val = (val+0x7ed55d16) + (val<<12); + val = (val^0xc761c23c) ^ (val>>19); + val = (val+0x165667b1) + (val<<5); + val = (val+0xd3a2646c) ^ (val<<9); + val = (val+0xfd7046c5) + (val<<3); + val = (val^0xb55a4f09) ^ (val>>16); + + GLubyte r = (val>>0) & 0xFF; + GLubyte g = (val>>8) & 0xFF; + GLubyte b = (val>>16) & 0xFF; + + GLubyte max = r>g ? (r>b ? r : b) : (g>b ? g : b); + + const int mult = 127; + const int add = 63; + r = (r*mult)/max + add; + g = (g*mult)/max + add; + b = (b*mult)/max + add; + + glColor3ub(r, g, b); +} + +inline void glColor_for_shape( cpShape *shape, cpSpace *space ) { + cpBody *body = shape->body; + if(body){ + if(cpBodyIsSleeping(body)){ + GLfloat v = 0.25f; + glColor3f(v,v,v); + return; + } else if(body->node.idleTime > space->sleepTimeThreshold) { + GLfloat v = 0.9f; + glColor3f(v,v,v); + return; + } + } + + glColor_from_pointer(shape); +} + +static const GLfloat springVAR[] = { + 0.00f, 0.0f, + 0.20f, 0.0f, + 0.25f, 3.0f, + 0.30f,-6.0f, + 0.35f, 6.0f, + 0.40f,-6.0f, + 0.45f, 6.0f, + 0.50f,-6.0f, + 0.55f, 6.0f, + 0.60f,-6.0f, + 0.65f, 6.0f, + 0.70f,-3.0f, + 0.75f, 6.0f, + 0.80f, 0.0f, + 1.00f, 0.0f, +}; +static const int springVAR_count = sizeof(springVAR)/sizeof(GLfloat)/2; + +#endif diff --git a/src/test/ee.cpp b/src/test/ee.cpp index b6afd7886..b61c3de40 100644 --- a/src/test/ee.cpp +++ b/src/test/ee.cpp @@ -96,7 +96,7 @@ class cEETest : private cThread { std::vector TN; std::vector TNP; - std::vector Tiles; + std::vector Tiles; eeVector2i Mouse; eeVector2f Mousef; @@ -218,6 +218,15 @@ class cEETest : private cThread { cTextCache mEEText; cTextCache mFBOText; cTextCache mInfoText; + + cSpace * mSpace; + cBody * mMouseBody; + cpVect mMousePoint; + cpVect mMousePoint_last; + cConstraint * mMouseJoint; + void PhysicsCreate(); + void PhysicsUpdate(); + void PhysicsDestroy(); }; void cEETest::CreateAquaTextureAtlas() { @@ -344,6 +353,8 @@ void cEETest::Init() { mVBO->Compile(); + PhysicsCreate(); + Launch(); } else { std::cout << "Failed to start EE++" << std::endl; @@ -722,6 +733,7 @@ void cEETest::CreateUI() { cUIWindow::CreateParams WinParams; WinParams.Flags = UI_HALIGN_CENTER; + WinParams.WinFlags |= cUIWindow::UI_WIN_MAXIMIZE_BUTTON; WinParams.PosSet( 200, 50 ); WinParams.Size = eeSize( 530, 380 ); WinParams.ButtonsPositionFixer.x = -4; @@ -936,7 +948,7 @@ void cEETest::LoadTextures() { TreeTilingCreated = false; CreateTiling(Wireframe); - cGlobalShapeGroup::instance()->Add( eeNew( cShape, ( TF->Load( MyPath + "data/aqua/aqua_button_ok.png" ), "aqua_button_ok" ) ) ); + cGlobalShapeGroup::instance()->Add( eeNew( Graphics::cShape, ( TF->Load( MyPath + "data/aqua/aqua_button_ok.png" ), "aqua_button_ok" ) ) ); } void cEETest::RandomizeHeights() { @@ -1287,6 +1299,8 @@ void cEETest::Render() { } } + PhysicsUpdate(); + cUIManager::instance()->Update(); cUIManager::instance()->Draw(); @@ -1532,6 +1546,8 @@ void cEETest::Process() { void cEETest::End() { Wait(); + PhysicsDestroy(); + Mus->Stop(); eeSAFE_DELETE( Mus ); eeSAFE_DELETE( mTGL ); @@ -1567,6 +1583,134 @@ void cEETest::Particles() { PS[i].Draw(); } +#define GRABABLE_MASK_BIT (1<<31) +#define NOT_GRABABLE_MASK (~GRABABLE_MASK_BIT) + +void cEETest::PhysicsCreate() { + cPhysicsManager::CreateSingleton(); + cPhysicsManager::instance()->CollisionSlop( 0.2 ); + + mMouseJoint = NULL; + + mMouseBody = eeNew( cBody, ( INFINITY, INFINITY ) ); + + // Create a space, a space is a simulation world. It simulates the motions of rigid bodies, + // handles collisions between them, and simulates the joints between them. + mSpace = eeNew( cSpace, () ); + + // Lets set some parameters of the space: + // More iterations make the simulation more accurate but slower + mSpace->Iterations( 10 ); + + // These parameters tune the efficiency of the collision detection. + // For more info: http://code.google.com/p/chipmunk-physics/wiki/cpSpace + mSpace->ResizeStaticHash( 30.0f, 1000 ); + mSpace->ResizeActiveHash( 30.0f, 1000 ); + + // Give it some gravity + mSpace->Gravity( cpv(0, 1000) ); + mSpace->SleepTimeThreshold( 0.5f ); + + // Create A ground segment along the bottom of the screen + // By attaching it to &space->staticBody instead of a body, we make it a static shape. + Physics::cShapeSegment * ground = eeNew( cShapeSegment, ( mSpace->StaticBody(), cpv(0,640), cpv(640, 640), 0.0f ) ); + + // Set some parameters of the shape. + // For more info: http://code.google.com/p/chipmunk-physics/wiki/cpShape + ground->e( 1.0f ); + ground->u( 1.0f ); + ground->Layers( NOT_GRABABLE_MASK ); // Used by the Demo mouse grabbing code + + // Add the shape to the space as a static shape + // If a shape never changes position, add it as static so Chipmunk knows it only needs to + // calculate collision information for it once when it is added. + // Do not change the postion of a static shape after adding it. + mSpace->AddShape( ground ); + + // Add a moving circle object. + cpFloat radius = 15.0f; + cpFloat mass = 10.0f; + + // This time we need to give a mass and moment of inertia when creating the circle. + cBody * ballBody = eeNew( cBody, ( mass, cpMomentForCircle(mass, 0.0f, radius, cpvzero) ) ); + + // Set some parameters of the body: + // For more info: http://code.google.com/p/chipmunk-physics/wiki/cpBody + ballBody->Pos( cpv(320, 240 + radius+50) ); + ballBody->Vel( cpv(0, 20) ); + + // Add the body to the space so it will be simulated and move around. + mSpace->AddBody( ballBody ); + + // Add a circle shape for the ball. + // Shapes are always defined relative to the center of gravity of the body they are attached to. + // When the body moves or rotates, the shape will move with it. + // Additionally, all of the cpSpaceAdd*() functions return the thing they added so you can create and add in one go. + Physics::cShapeCircle * ballShape = eeNew( Physics::cShapeCircle, ( ballBody, radius, cpvzero ) ); + mSpace->AddShape( ballShape ); + ballShape->e( 0.0f ); + ballShape->u( 0.9f ); + + ballBody = eeNew( cBody, ( mass, cpMomentForCircle( mass, 0.0f, radius, cpvzero ) ) ); + + // Set some parameters of the body: + // For more info: http://code.google.com/p/chipmunk-physics/wiki/cpBody + ballBody->Pos( cpv(320, 240 + radius + 50 + 200) ); + ballBody->Vel( cpv(0, 20) ); + + // Add the body to the space so it will be simulated and move around. + mSpace->AddBody( ballBody ); + + // Add a circle shape for the ball. + // Shapes are always defined relative to the center of gravity of the body they are attached to. + // When the body moves or rotates, the shape will move with it. + // Additionally, all of the cpSpaceAdd*() functions return the thing they added so you can create and add in one go. + ballShape = eeNew( Physics::cShapeCircle, ( ballBody, radius, cpvzero ) ); + ballShape->e( 0.0f ); + ballShape->u( 0.9f ); + mSpace->AddShape( ballShape ); +} + +void cEETest::PhysicsUpdate() { + cGlobalBatchRenderer::instance()->Draw(); + + mMousePoint = cpv( KM->GetMousePosf().x, KM->GetMousePosf().y ); + cpVect newPoint = cpvlerp(mMousePoint_last, mMousePoint, 0.25f); + mMouseBody->Pos( newPoint ); + mMouseBody->Vel( cpvmult( cpvsub( newPoint, mMousePoint_last ), 60.0f ) ); + mMousePoint_last = newPoint; + + if ( KM->MouseLeftPressed() ) { + if ( NULL == mMouseJoint ) { + cpVect point = cpv( KM->GetMousePosf().x, KM->GetMousePosf().y ); + + cpShape * shape = cpSpacePointQueryFirst( mSpace->Space(), point, GRABABLE_MASK_BIT, CP_NO_GROUP ); + + if( shape ){ + cpBody * body = shape->body; + mMouseJoint = eeNew( cConstraint, ( cpPivotJointNew2( mMouseBody->Body(), body, cpvzero, cpBodyWorld2Local( body, point ) ) ) ); + mMouseJoint->MaxForce( 50000.0f ); + mMouseJoint->BiasCoef( 0.15f ); + mSpace->AddConstraint( mMouseJoint ); + } + } + } else if ( NULL != mMouseJoint ) { + mSpace->RemoveConstraint( mMouseJoint ); + cpConstraintFree( mMouseJoint->Constraint() ); + eeSAFE_DELETE( mMouseJoint ); + } + + mSpace->Update(); + mSpace->Draw(); +} + +void cEETest::PhysicsDestroy() { + eeSAFE_DELETE( mSpace ); + eeSAFE_DELETE( mMouseJoint ); + eeSAFE_DELETE( mMouseBody ); + cPhysicsManager::DestroySingleton(); +} + int main (int argc, char * argv []) { cEETest * Test = eeNew( cEETest, () ); diff --git a/src/ui/cuimanager.hpp b/src/ui/cuimanager.hpp index 296fdd3ea..2e90df385 100644 --- a/src/ui/cuimanager.hpp +++ b/src/ui/cuimanager.hpp @@ -12,6 +12,7 @@ class EE_API cUIManager : public tSingleton { friend class tSingleton; public: cUIManager(); + ~cUIManager(); cUIControlAnim * MainControl() const; diff --git a/src/ui/cuiwindow.cpp b/src/ui/cuiwindow.cpp index 9999d5f33..487067def 100644 --- a/src/ui/cuiwindow.cpp +++ b/src/ui/cuiwindow.cpp @@ -110,15 +110,15 @@ void cUIWindow::ButtonCloseClick( const cUIEvent * Event ) { void cUIWindow::ButtonMaximizeClick( const cUIEvent * Event ) { cUIControl * Ctrl = cUIManager::instance()->MainControl(); - if ( Ctrl->Size() == cUIControl::Size() ) { + if ( Ctrl->Size() == mSize ) { Pos( mNonMaxPos ); - Size( mNonMaxSize ); + InternalSize( mNonMaxSize ); } else { mNonMaxPos = mPos; mNonMaxSize = mSize; Pos( 0, 0 ); - Size( cUIManager::instance()->MainControl()->Size() ); + InternalSize( cUIManager::instance()->MainControl()->Size() ); } } @@ -622,4 +622,12 @@ cUITextBox * cUIWindow::TitleTextBox() const { return mTitle; } +Uint32 cUIWindow::OnMouseDoubleClick( const eeVector2i &Pos, Uint32 Flags ) { + if ( ( mWinFlags & UI_WIN_RESIZEABLE ) && ( NULL != mButtonMaximize ) && ( Flags & EE_BUTTON_LMASK ) ) { + ButtonMaximizeClick( NULL ); + } + + return 1; +} + }} diff --git a/src/ui/cuiwindow.hpp b/src/ui/cuiwindow.hpp index ac3db78e3..e35e067fb 100644 --- a/src/ui/cuiwindow.hpp +++ b/src/ui/cuiwindow.hpp @@ -158,6 +158,8 @@ class cUIWindow : public cUIComplexControl { void GetMinWinSize(); void FixTitleSize(); + + Uint32 OnMouseDoubleClick( const eeVector2i &Pos, Uint32 Flags ); }; }} diff --git a/src/utils/vector2.hpp b/src/utils/vector2.hpp index 04edfd230..a2eaab2bb 100755 --- a/src/utils/vector2.hpp +++ b/src/utils/vector2.hpp @@ -208,6 +208,7 @@ T Vector2::Distance( const Vector2& Vec ) { typedef Vector2 eeVector2if; typedef Vector2 eeVector2i; typedef Vector2 eeVector2f; +typedef Vector2 eeVector2d; }} diff --git a/src/utils/vector3.hpp b/src/utils/vector3.hpp index 4b4f94125..ff28e8cea 100755 --- a/src/utils/vector3.hpp +++ b/src/utils/vector3.hpp @@ -89,6 +89,7 @@ bool operator !=(const Vector3& V1, const Vector3& V2) { // Define the most common types typedef Vector3 eeVector3i; typedef Vector3 eeVector3f; +typedef Vector3 eeVector3d; }}