Free games for graphing calculators: NumWorks, TI-Nspire CX, Casio fx-CG50 and soon TI-84 Plus CE. Balatro, Celeste, Hollow Knight, Minecraft and more.
C calculatorcalculator-gamescasiochesscrossy-roaddrive-madfx-cg50gamesgeometry-dashgraphing-calculatorndlessnumworksnumworks-appsnumworks-calculatornumworks-gamenumworks-gamestetristi-84-plus-ceti-nspire
Mr Bullet: first playable build
Mason36398ddcc85a5eb
diff --git a/games/mrbullet/.gitignore b/games/mrbullet/.gitignorenew file mode 100644index 00000000..e48c9cd3--- /dev/null+++ b/games/mrbullet/.gitignore@@ -0,0 +1,4 @@+output/+build/+__pycache__/+*.pycdiff --git a/games/mrbullet/Makefile b/games/mrbullet/Makefilenew file mode 100644index 00000000..4a793b29--- /dev/null+++ b/games/mrbullet/Makefile@@ -0,0 +1,118 @@+# Mr Bullet on the NumWorks calculator (C, and the physics in C++ on Box2D 2.3.1: src/box2d).+# make output/mrbullet.nwa+# make check link it like the calculator does (output/mrbullet.bin): it must fit the app space and+# 133652 bytes of RAM+# make host build/play: the game on a computer, for tests and pictures+# make test physics checks, the first levels solved, every level loaded and played with random keys, saves+# (tests/play.c, with the sanitizers)+# make data src/data.bin and src/data.h from the game's own files (REF=the APK's assets folder; Pillow)+Q ?= @+# the ARM toolchain on the PATH, or in TOOLCHAIN=<its bin folder>+TOOLCHAIN ?=+BIN = $(if $(TOOLCHAIN),$(TOOLCHAIN)/)+CC = $(BIN)arm-none-eabi-gcc+CXX = $(BIN)arm-none-eabi-g+++NWLINK ?= npx --yes -- [email protected]+STRIP ?= $(BIN)arm-none-eabi-strip --strip-unneeded+BUILD_DIR = output+L = ../../launcher/src+REF ?=++# drawing runs every frame at -O2; the rest is small at -Os+HOT = src/draw.c+COLD = src/main.c src/ui.c src/game.c src/res.c src/save.c src/inflate.c $(L)/gfx.c $(L)/font_data.c $(L)/xfont.c+COLD_OBJ = $(patsubst %.c,$(BUILD_DIR)/cold/%.o,$(notdir $(COLD)))+HEADERS = $(wildcard src/*.h) $(L)/gfx.h $(L)/font.h $(L)/np.h ../common/epsilon_app.h ../common/epsilon_files.h+BOX2D = $(wildcard src/box2d/Box2D/*/*.cpp src/box2d/Box2D/*/*/*.cpp)+BOX2D_H = $(wildcard src/box2d/Box2D/*/*.h src/box2d/Box2D/*/*/*.h)+PHYS = src/world.cpp $(BOX2D)+PHYS_OBJ = $(patsubst src/%.cpp,$(BUILD_DIR)/phys/%.o,$(PHYS))++CFLAGS = -std=gnu11 $(shell $(NWLINK) eadk-cflags-device) -DEPSILON_XL=1 -I$(L) -Isrc+CFLAGS += -Wall -Wextra -Wno-unused-parameter -fno-math-errno -fsingle-precision-constant+CFLAGS += -fno-tree-loop-distribute-patterns -flto -fno-fat-lto-objects -fwhole-program -fvisibility=internal+CFLAGS += -ffunction-sections -fdata-sections $(EXTRA)+CXXFLAGS = $(filter-out -std=gnu11,$(CFLAGS)) -std=gnu++11 -O2 -fno-exceptions -fno-rtti -fno-threadsafe-statics+CXXFLAGS += -fno-use-cxa-atexit -DNDEBUG -Isrc/box2d -Wno-unused-but-set-variable -Wno-class-memaccess -fno-strict-aliasing+CXXFLAGS += -Wno-unused-variable+LDFLAGS = -Wl,--relocatable -nostartfiles --specs=nano.specs+LDFLAGS += -Wl,-e,main -Wl,-u,eadk_app_name -Wl,-u,eadk_app_icon -Wl,-u,eadk_api_level+LDFLAGS += -Wl,--gc-sections -flinker-output=nolto-rel++.PHONY: build check clean host test data+build: $(BUILD_DIR)/mrbullet.nwa++# apps get 133652 bytes of RAM on custom builds of Epsilon 25.2 (148928 or more on official ones); installed on its+# own, the app must fit the calculator's space for apps (0x90200000 to 0x903F0000)+RAM_LENGTH = 133652+APP_SPACE = 2031616+check: $(BUILD_DIR)/mrbullet.nwa+ $(Q) $(NWLINK) nwa-bin --ram-length $(RAM_LENGTH) $< $(BUILD_DIR)/mrbullet.bin+ @n=$$(wc -c < $(BUILD_DIR)/mrbullet.bin); \+ echo "BIN $(BUILD_DIR)/mrbullet.bin: $$n bytes, $$(( $(APP_SPACE) - n )) to spare of the app space"; \+ [ $$n -le $(APP_SPACE) ] || { echo "mrbullet.nwa is bigger than the calculator's app space ($(APP_SPACE) bytes)"; exit 1; }++$(BUILD_DIR)/data.o: src/data.S src/data.bin | $(BUILD_DIR)+ $(Q) $(CC) -c -DDATA_BIN='"src/data.bin"' $< -o $@++$(BUILD_DIR)/cold/%.o: src/%.c $(HEADERS)+ @mkdir -p $(@D)+ $(Q) $(CC) $(CFLAGS) -Os -c $< -o $@+$(BUILD_DIR)/cold/%.o: $(L)/%.c $(HEADERS)+ @mkdir -p $(@D)+ $(Q) $(CC) $(CFLAGS) -Os -c $< -o $@++$(BUILD_DIR)/phys/%.o: src/%.cpp $(HEADERS) $(BOX2D_H)+ @mkdir -p $(@D)+ $(Q) $(CXX) $(CXXFLAGS) -c $< -o $@++$(BUILD_DIR)/mrbullet.nwa: $(HOT) src/plat_eadk.c $(COLD_OBJ) $(PHYS_OBJ) $(HEADERS) $(BUILD_DIR)/icon.o $(BUILD_DIR)/data.o+ @echo "LD $@"+ $(Q) $(CC) $(CFLAGS) -O2 $(LDFLAGS) $(HOT) src/plat_eadk.c $(COLD_OBJ) $(PHYS_OBJ) $(BUILD_DIR)/data.o \+ $(BUILD_DIR)/icon.o -lm -o $@+ $(Q) $(STRIP) $@++$(BUILD_DIR)/icon.o: src/icon.png | $(BUILD_DIR)+ @echo "ICON $<"+ $(Q) $(NWLINK) png-icon-o $< $@++$(BUILD_DIR):+ $(Q) mkdir -p $@++# on a computer: tests/play.c checks the physics, solves the first levels, plays every level with random keys, and+# saves pictures+HOST_FLAGS = -std=gnu11 -O2 -g -Wall -Wextra -Wno-unused-parameter -ffp-contract=off -Isrc -I$(L) -DHOST+HOST_CXXFLAGS = -std=gnu++11 -O2 -g -Wall -Wno-unused-parameter -Wno-unused-variable -Wno-unused-but-set-variable \+ -Wno-unused-private-field -ffp-contract=off -Isrc -Isrc/box2d -DHOST -DNDEBUG -fno-exceptions -fno-rtti+HOST_SAN = -fsanitize=address,undefined -fno-sanitize-recover=undefined+HOST_SRC = tests/play.c src/ui.c src/draw.c src/game.c src/res.c src/save.c src/inflate.c+HOST_LSRC = gfx.c font_data.c xfont.c+HOST_COBJ = $(patsubst %.c,build/host/c/%.o,$(HOST_SRC)) $(patsubst %.c,build/host/l/%.o,$(HOST_LSRC))+HOST_PHYS = $(patsubst src/%.cpp,build/host/%.o,$(PHYS))+host: build/play+build/host/%.o: src/%.cpp $(HEADERS) $(BOX2D_H)+ $(Q) mkdir -p $(@D)+ $(Q) c++ $(HOST_CXXFLAGS) $(HOST_SAN) -c $< -o $@+# (the C files with cc: g++ takes them for C++ even after -x c)+build/host/c/%.o: %.c $(HEADERS)+ $(Q) mkdir -p $(@D)+ $(Q) cc $(HOST_FLAGS) $(HOST_SAN) -c $< -o $@+build/host/l/%.o: $(L)/%.c $(HEADERS)+ $(Q) mkdir -p $(@D)+ $(Q) cc $(HOST_FLAGS) $(HOST_SAN) -c $< -o $@+build/host/data.o: src/data.S src/data.bin+ $(Q) mkdir -p $(@D)+ $(Q) cc -c -DDATA_BIN='"src/data.bin"' $< -o $@+build/play: $(HOST_COBJ) $(HOST_PHYS) build/host/data.o+ $(Q) c++ $(HOST_SAN) $(HOST_COBJ) $(HOST_PHYS) build/host/data.o -lm -o $@+ @echo "HOST $@"++test: build/play+ build/play test build++data:+ @test -n "$(REF)" || { echo "make data REF=<the APK's assets folder>"; exit 1; }+ python3 -I tools/pack.py $(REF)++clean:+ $(Q) rm -rf $(BUILD_DIR) builddiff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.cppnew file mode 100644index 00000000..a9c2e1d5--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp@@ -0,0 +1,193 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>+#include <new>+#include <string.h>++b2ChainShape::~b2ChainShape()+{+ Clear();+}++void b2ChainShape::Clear()+{+ b2Free(m_vertices);+ m_vertices = NULL;+ m_count = 0;+}++void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)+{+ b2Assert(m_vertices == NULL && m_count == 0);+ b2Assert(count >= 3);+ for (int32 i = 1; i < count; ++i)+ {+ b2Vec2 v1 = vertices[i-1];+ b2Vec2 v2 = vertices[i];+ // If the code crashes here, it means your vertices are too close together.+ b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);+ }++ m_count = count + 1;+ m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));+ memcpy(m_vertices, vertices, count * sizeof(b2Vec2));+ m_vertices[count] = m_vertices[0];+ m_prevVertex = m_vertices[m_count - 2];+ m_nextVertex = m_vertices[1];+ m_hasPrevVertex = true;+ m_hasNextVertex = true;+}++void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)+{+ b2Assert(m_vertices == NULL && m_count == 0);+ b2Assert(count >= 2);+ for (int32 i = 1; i < count; ++i)+ {+ // If the code crashes here, it means your vertices are too close together.+ b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop);+ }++ m_count = count;+ m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));+ memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));++ m_hasPrevVertex = false;+ m_hasNextVertex = false;++ m_prevVertex.SetZero();+ m_nextVertex.SetZero();+}++void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)+{+ m_prevVertex = prevVertex;+ m_hasPrevVertex = true;+}++void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)+{+ m_nextVertex = nextVertex;+ m_hasNextVertex = true;+}++b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const+{+ void* mem = allocator->Allocate(sizeof(b2ChainShape));+ b2ChainShape* clone = new (mem) b2ChainShape;+ clone->CreateChain(m_vertices, m_count);+ clone->m_prevVertex = m_prevVertex;+ clone->m_nextVertex = m_nextVertex;+ clone->m_hasPrevVertex = m_hasPrevVertex;+ clone->m_hasNextVertex = m_hasNextVertex;+ return clone;+}++int32 b2ChainShape::GetChildCount() const+{+ // edge count = vertex count - 1+ return m_count - 1;+}++void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const+{+ b2Assert(0 <= index && index < m_count - 1);+ edge->m_type = b2Shape::e_edge;+ edge->m_radius = m_radius;++ edge->m_vertex1 = m_vertices[index + 0];+ edge->m_vertex2 = m_vertices[index + 1];++ if (index > 0)+ {+ edge->m_vertex0 = m_vertices[index - 1];+ edge->m_hasVertex0 = true;+ }+ else+ {+ edge->m_vertex0 = m_prevVertex;+ edge->m_hasVertex0 = m_hasPrevVertex;+ }++ if (index < m_count - 2)+ {+ edge->m_vertex3 = m_vertices[index + 2];+ edge->m_hasVertex3 = true;+ }+ else+ {+ edge->m_vertex3 = m_nextVertex;+ edge->m_hasVertex3 = m_hasNextVertex;+ }+}++bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const+{+ B2_NOT_USED(xf);+ B2_NOT_USED(p);+ return false;+}++bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& xf, int32 childIndex) const+{+ b2Assert(childIndex < m_count);++ b2EdgeShape edgeShape;++ int32 i1 = childIndex;+ int32 i2 = childIndex + 1;+ if (i2 == m_count)+ {+ i2 = 0;+ }++ edgeShape.m_vertex1 = m_vertices[i1];+ edgeShape.m_vertex2 = m_vertices[i2];++ return edgeShape.RayCast(output, input, xf, 0);+}++void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const+{+ b2Assert(childIndex < m_count);++ int32 i1 = childIndex;+ int32 i2 = childIndex + 1;+ if (i2 == m_count)+ {+ i2 = 0;+ }++ b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);+ b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);++ aabb->lowerBound = b2Min(v1, v2);+ aabb->upperBound = b2Max(v1, v2);+}++void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const+{+ B2_NOT_USED(density);++ massData->mass = 0.0f;+ massData->center.SetZero();+ massData->I = 0.0f;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.h b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.hnew file mode 100644index 00000000..8b378bc8--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.h@@ -0,0 +1,105 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CHAIN_SHAPE_H+#define B2_CHAIN_SHAPE_H++#include <Box2D/Collision/Shapes/b2Shape.h>++class b2EdgeShape;++/// A chain shape is a free form sequence of line segments.+/// The chain has two-sided collision, so you can use inside and outside collision.+/// Therefore, you may use any winding order.+/// Since there may be many vertices, they are allocated using b2Alloc.+/// Connectivity information is used to create smooth collisions.+/// WARNING: The chain will not collide properly if there are self-intersections.+class b2ChainShape : public b2Shape+{+public:+ b2ChainShape();++ /// The destructor frees the vertices using b2Free.+ ~b2ChainShape();++ /// Clear all data.+ void Clear();++ /// Create a loop. This automatically adjusts connectivity.+ /// @param vertices an array of vertices, these are copied+ /// @param count the vertex count+ void CreateLoop(const b2Vec2* vertices, int32 count);++ /// Create a chain with isolated end vertices.+ /// @param vertices an array of vertices, these are copied+ /// @param count the vertex count+ void CreateChain(const b2Vec2* vertices, int32 count);++ /// Establish connectivity to a vertex that precedes the first vertex.+ /// Don't call this for loops.+ void SetPrevVertex(const b2Vec2& prevVertex);++ /// Establish connectivity to a vertex that follows the last vertex.+ /// Don't call this for loops.+ void SetNextVertex(const b2Vec2& nextVertex);++ /// Implement b2Shape. Vertices are cloned using b2Alloc.+ b2Shape* Clone(b2BlockAllocator* allocator) const;++ /// @see b2Shape::GetChildCount+ int32 GetChildCount() const;++ /// Get a child edge.+ void GetChildEdge(b2EdgeShape* edge, int32 index) const;++ /// This always return false.+ /// @see b2Shape::TestPoint+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;++ /// Implement b2Shape.+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeAABB+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;++ /// Chains have zero mass.+ /// @see b2Shape::ComputeMass+ void ComputeMass(b2MassData* massData, float32 density) const;++ /// The vertices. Owned by this class.+ b2Vec2* m_vertices;++ /// The vertex count.+ int32 m_count;++ b2Vec2 m_prevVertex, m_nextVertex;+ bool m_hasPrevVertex, m_hasNextVertex;+};++inline b2ChainShape::b2ChainShape()+{+ m_type = e_chain;+ m_radius = b2_polygonRadius;+ m_vertices = NULL;+ m_count = 0;+ m_hasPrevVertex = false;+ m_hasNextVertex = false;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.cppnew file mode 100644index 00000000..47e5b372--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp@@ -0,0 +1,99 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <new>++b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const+{+ void* mem = allocator->Allocate(sizeof(b2CircleShape));+ b2CircleShape* clone = new (mem) b2CircleShape;+ *clone = *this;+ return clone;+}++int32 b2CircleShape::GetChildCount() const+{+ return 1;+}++bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const+{+ b2Vec2 center = transform.p + b2Mul(transform.q, m_p);+ b2Vec2 d = p - center;+ return b2Dot(d, d) <= m_radius * m_radius;+}++// Collision Detection in Interactive 3D Environments by Gino van den Bergen+// From Section 3.1.2+// x = s + a * r+// norm(x) = radius+bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& transform, int32 childIndex) const+{+ B2_NOT_USED(childIndex);++ b2Vec2 position = transform.p + b2Mul(transform.q, m_p);+ b2Vec2 s = input.p1 - position;+ float32 b = b2Dot(s, s) - m_radius * m_radius;++ // Solve quadratic equation.+ b2Vec2 r = input.p2 - input.p1;+ float32 c = b2Dot(s, r);+ float32 rr = b2Dot(r, r);+ float32 sigma = c * c - rr * b;++ // Check for negative discriminant and short segment.+ if (sigma < 0.0f || rr < b2_epsilon)+ {+ return false;+ }++ // Find the point of intersection of the line with the circle.+ float32 a = -(c + b2Sqrt(sigma));++ // Is the intersection point on the segment?+ if (0.0f <= a && a <= input.maxFraction * rr)+ {+ a /= rr;+ output->fraction = a;+ output->normal = s + a * r;+ output->normal.Normalize();+ return true;+ }++ return false;+}++void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const+{+ B2_NOT_USED(childIndex);++ b2Vec2 p = transform.p + b2Mul(transform.q, m_p);+ aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);+ aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);+}++void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const+{+ massData->mass = density * b2_pi * m_radius * m_radius;+ massData->center = m_p;++ // inertia about the local origin+ massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.h b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.hnew file mode 100644index 00000000..6c1fd543--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.h@@ -0,0 +1,91 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CIRCLE_SHAPE_H+#define B2_CIRCLE_SHAPE_H++#include <Box2D/Collision/Shapes/b2Shape.h>++/// A circle shape.+class b2CircleShape : public b2Shape+{+public:+ b2CircleShape();++ /// Implement b2Shape.+ b2Shape* Clone(b2BlockAllocator* allocator) const;++ /// @see b2Shape::GetChildCount+ int32 GetChildCount() const;++ /// Implement b2Shape.+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;++ /// Implement b2Shape.+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeAABB+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeMass+ void ComputeMass(b2MassData* massData, float32 density) const;++ /// Get the supporting vertex index in the given direction.+ int32 GetSupport(const b2Vec2& d) const;++ /// Get the supporting vertex in the given direction.+ const b2Vec2& GetSupportVertex(const b2Vec2& d) const;++ /// Get the vertex count.+ int32 GetVertexCount() const { return 1; }++ /// Get a vertex by index. Used by b2Distance.+ const b2Vec2& GetVertex(int32 index) const;++ /// Position+ b2Vec2 m_p;+};++inline b2CircleShape::b2CircleShape()+{+ m_type = e_circle;+ m_radius = 0.0f;+ m_p.SetZero();+}++inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const+{+ B2_NOT_USED(d);+ return 0;+}++inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const+{+ B2_NOT_USED(d);+ return m_p;+}++inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const+{+ B2_NOT_USED(index);+ b2Assert(index == 0);+ return m_p;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.cppnew file mode 100644index 00000000..3514b2ed--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp@@ -0,0 +1,138 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/Shapes/b2EdgeShape.h>+#include <new>++void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)+{+ m_vertex1 = v1;+ m_vertex2 = v2;+ m_hasVertex0 = false;+ m_hasVertex3 = false;+}++b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const+{+ void* mem = allocator->Allocate(sizeof(b2EdgeShape));+ b2EdgeShape* clone = new (mem) b2EdgeShape;+ *clone = *this;+ return clone;+}++int32 b2EdgeShape::GetChildCount() const+{+ return 1;+}++bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const+{+ B2_NOT_USED(xf);+ B2_NOT_USED(p);+ return false;+}++// p = p1 + t * d+// v = v1 + s * e+// p1 + t * d = v1 + s * e+// s * e - t * d = p1 - v1+bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& xf, int32 childIndex) const+{+ B2_NOT_USED(childIndex);++ // Put the ray into the edge's frame of reference.+ b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);+ b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);+ b2Vec2 d = p2 - p1;++ b2Vec2 v1 = m_vertex1;+ b2Vec2 v2 = m_vertex2;+ b2Vec2 e = v2 - v1;+ b2Vec2 normal(e.y, -e.x);+ normal.Normalize();++ // q = p1 + t * d+ // dot(normal, q - v1) = 0+ // dot(normal, p1 - v1) + t * dot(normal, d) = 0+ float32 numerator = b2Dot(normal, v1 - p1);+ float32 denominator = b2Dot(normal, d);++ if (denominator == 0.0f)+ {+ return false;+ }++ float32 t = numerator / denominator;+ if (t < 0.0f || input.maxFraction < t)+ {+ return false;+ }++ b2Vec2 q = p1 + t * d;++ // q = v1 + s * r+ // s = dot(q - v1, r) / dot(r, r)+ b2Vec2 r = v2 - v1;+ float32 rr = b2Dot(r, r);+ if (rr == 0.0f)+ {+ return false;+ }++ float32 s = b2Dot(q - v1, r) / rr;+ if (s < 0.0f || 1.0f < s)+ {+ return false;+ }++ output->fraction = t;+ if (numerator > 0.0f)+ {+ output->normal = -b2Mul(xf.q, normal);+ }+ else+ {+ output->normal = b2Mul(xf.q, normal);+ }+ return true;+}++void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const+{+ B2_NOT_USED(childIndex);++ b2Vec2 v1 = b2Mul(xf, m_vertex1);+ b2Vec2 v2 = b2Mul(xf, m_vertex2);++ b2Vec2 lower = b2Min(v1, v2);+ b2Vec2 upper = b2Max(v1, v2);++ b2Vec2 r(m_radius, m_radius);+ aabb->lowerBound = lower - r;+ aabb->upperBound = upper + r;+}++void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const+{+ B2_NOT_USED(density);++ massData->mass = 0.0f;+ massData->center = 0.5f * (m_vertex1 + m_vertex2);+ massData->I = 0.0f;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.h b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.hnew file mode 100644index 00000000..99f822be--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.h@@ -0,0 +1,74 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_EDGE_SHAPE_H+#define B2_EDGE_SHAPE_H++#include <Box2D/Collision/Shapes/b2Shape.h>++/// A line segment (edge) shape. These can be connected in chains or loops+/// to other edge shapes. The connectivity information is used to ensure+/// correct contact normals.+class b2EdgeShape : public b2Shape+{+public:+ b2EdgeShape();++ /// Set this as an isolated edge.+ void Set(const b2Vec2& v1, const b2Vec2& v2);++ /// Implement b2Shape.+ b2Shape* Clone(b2BlockAllocator* allocator) const;++ /// @see b2Shape::GetChildCount+ int32 GetChildCount() const;++ /// @see b2Shape::TestPoint+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;++ /// Implement b2Shape.+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeAABB+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeMass+ void ComputeMass(b2MassData* massData, float32 density) const;++ /// These are the edge vertices+ b2Vec2 m_vertex1, m_vertex2;++ /// Optional adjacent vertices. These are used for smooth collision.+ b2Vec2 m_vertex0, m_vertex3;+ bool m_hasVertex0, m_hasVertex3;+};++inline b2EdgeShape::b2EdgeShape()+{+ m_type = e_edge;+ m_radius = b2_polygonRadius;+ m_vertex0.x = 0.0f;+ m_vertex0.y = 0.0f;+ m_vertex3.x = 0.0f;+ m_vertex3.y = 0.0f;+ m_hasVertex0 = false;+ m_hasVertex3 = false;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.cppnew file mode 100644index 00000000..2ef6529d--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp@@ -0,0 +1,467 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/Shapes/b2PolygonShape.h>+#include <new>++b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const+{+ void* mem = allocator->Allocate(sizeof(b2PolygonShape));+ b2PolygonShape* clone = new (mem) b2PolygonShape;+ *clone = *this;+ return clone;+}++void b2PolygonShape::SetAsBox(float32 hx, float32 hy)+{+ m_count = 4;+ m_vertices[0].Set(-hx, -hy);+ m_vertices[1].Set( hx, -hy);+ m_vertices[2].Set( hx, hy);+ m_vertices[3].Set(-hx, hy);+ m_normals[0].Set(0.0f, -1.0f);+ m_normals[1].Set(1.0f, 0.0f);+ m_normals[2].Set(0.0f, 1.0f);+ m_normals[3].Set(-1.0f, 0.0f);+ m_centroid.SetZero();+}++void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)+{+ m_count = 4;+ m_vertices[0].Set(-hx, -hy);+ m_vertices[1].Set( hx, -hy);+ m_vertices[2].Set( hx, hy);+ m_vertices[3].Set(-hx, hy);+ m_normals[0].Set(0.0f, -1.0f);+ m_normals[1].Set(1.0f, 0.0f);+ m_normals[2].Set(0.0f, 1.0f);+ m_normals[3].Set(-1.0f, 0.0f);+ m_centroid = center;++ b2Transform xf;+ xf.p = center;+ xf.q.Set(angle);++ // Transform vertices and normals.+ for (int32 i = 0; i < m_count; ++i)+ {+ m_vertices[i] = b2Mul(xf, m_vertices[i]);+ m_normals[i] = b2Mul(xf.q, m_normals[i]);+ }+}++int32 b2PolygonShape::GetChildCount() const+{+ return 1;+}++static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)+{+ b2Assert(count >= 3);++ b2Vec2 c; c.Set(0.0f, 0.0f);+ float32 area = 0.0f;++ // pRef is the reference point for forming triangles.+ // It's location doesn't change the result (except for rounding error).+ b2Vec2 pRef(0.0f, 0.0f);+#if 0+ // This code would put the reference point inside the polygon.+ for (int32 i = 0; i < count; ++i)+ {+ pRef += vs[i];+ }+ pRef *= 1.0f / count;+#endif++ const float32 inv3 = 1.0f / 3.0f;++ for (int32 i = 0; i < count; ++i)+ {+ // Triangle vertices.+ b2Vec2 p1 = pRef;+ b2Vec2 p2 = vs[i];+ b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];++ b2Vec2 e1 = p2 - p1;+ b2Vec2 e2 = p3 - p1;++ float32 D = b2Cross(e1, e2);++ float32 triangleArea = 0.5f * D;+ area += triangleArea;++ // Area weighted centroid+ c += triangleArea * inv3 * (p1 + p2 + p3);+ }++ // Centroid+ b2Assert(area > b2_epsilon);+ c *= 1.0f / area;+ return c;+}++void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)+{+ b2Assert(3 <= count && count <= b2_maxPolygonVertices);+ if (count < 3)+ {+ SetAsBox(1.0f, 1.0f);+ return;+ }++ int32 n = b2Min(count, b2_maxPolygonVertices);++ // Perform welding and copy vertices into local buffer.+ b2Vec2 ps[b2_maxPolygonVertices];+ int32 tempCount = 0;+ for (int32 i = 0; i < n; ++i)+ {+ b2Vec2 v = vertices[i];++ bool unique = true;+ for (int32 j = 0; j < tempCount; ++j)+ {+ if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)+ {+ unique = false;+ break;+ }+ }++ if (unique)+ {+ ps[tempCount++] = v;+ }+ }++ n = tempCount;+ if (n < 3)+ {+ // Polygon is degenerate.+ b2Assert(false);+ SetAsBox(1.0f, 1.0f);+ return;+ }++ // Create the convex hull using the Gift wrapping algorithm+ // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm++ // Find the right most point on the hull+ int32 i0 = 0;+ float32 x0 = ps[0].x;+ for (int32 i = 1; i < n; ++i)+ {+ float32 x = ps[i].x;+ if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))+ {+ i0 = i;+ x0 = x;+ }+ }++ int32 hull[b2_maxPolygonVertices];+ int32 m = 0;+ int32 ih = i0;++ for (;;)+ {+ hull[m] = ih;++ int32 ie = 0;+ for (int32 j = 1; j < n; ++j)+ {+ if (ie == ih)+ {+ ie = j;+ continue;+ }++ b2Vec2 r = ps[ie] - ps[hull[m]];+ b2Vec2 v = ps[j] - ps[hull[m]];+ float32 c = b2Cross(r, v);+ if (c < 0.0f)+ {+ ie = j;+ }++ // Collinearity check+ if (c == 0.0f && v.LengthSquared() > r.LengthSquared())+ {+ ie = j;+ }+ }++ ++m;+ ih = ie;++ if (ie == i0)+ {+ break;+ }+ }++ if (m < 3)+ {+ // Polygon is degenerate.+ b2Assert(false);+ SetAsBox(1.0f, 1.0f);+ return;+ }++ m_count = m;++ // Copy vertices.+ for (int32 i = 0; i < m; ++i)+ {+ m_vertices[i] = ps[hull[i]];+ }++ // Compute normals. Ensure the edges have non-zero length.+ for (int32 i = 0; i < m; ++i)+ {+ int32 i1 = i;+ int32 i2 = i + 1 < m ? i + 1 : 0;+ b2Vec2 edge = m_vertices[i2] - m_vertices[i1];+ b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);+ m_normals[i] = b2Cross(edge, 1.0f);+ m_normals[i].Normalize();+ }++ // Compute the polygon centroid.+ m_centroid = ComputeCentroid(m_vertices, m);+}++bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const+{+ b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);++ for (int32 i = 0; i < m_count; ++i)+ {+ float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);+ if (dot > 0.0f)+ {+ return false;+ }+ }++ return true;+}++bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& xf, int32 childIndex) const+{+ B2_NOT_USED(childIndex);++ // Put the ray into the polygon's frame of reference.+ b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);+ b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);+ b2Vec2 d = p2 - p1;++ float32 lower = 0.0f, upper = input.maxFraction;++ int32 index = -1;++ for (int32 i = 0; i < m_count; ++i)+ {+ // p = p1 + a * d+ // dot(normal, p - v) = 0+ // dot(normal, p1 - v) + a * dot(normal, d) = 0+ float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);+ float32 denominator = b2Dot(m_normals[i], d);++ if (denominator == 0.0f)+ {+ if (numerator < 0.0f)+ {+ return false;+ }+ }+ else+ {+ // Note: we want this predicate without division:+ // lower < numerator / denominator, where denominator < 0+ // Since denominator < 0, we have to flip the inequality:+ // lower < numerator / denominator <==> denominator * lower > numerator.+ if (denominator < 0.0f && numerator < lower * denominator)+ {+ // Increase lower.+ // The segment enters this half-space.+ lower = numerator / denominator;+ index = i;+ }+ else if (denominator > 0.0f && numerator < upper * denominator)+ {+ // Decrease upper.+ // The segment exits this half-space.+ upper = numerator / denominator;+ }+ }++ // The use of epsilon here causes the assert on lower to trip+ // in some cases. Apparently the use of epsilon was to make edge+ // shapes work, but now those are handled separately.+ //if (upper < lower - b2_epsilon)+ if (upper < lower)+ {+ return false;+ }+ }++ b2Assert(0.0f <= lower && lower <= input.maxFraction);++ if (index >= 0)+ {+ output->fraction = lower;+ output->normal = b2Mul(xf.q, m_normals[index]);+ return true;+ }++ return false;+}++void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const+{+ B2_NOT_USED(childIndex);++ b2Vec2 lower = b2Mul(xf, m_vertices[0]);+ b2Vec2 upper = lower;++ for (int32 i = 1; i < m_count; ++i)+ {+ b2Vec2 v = b2Mul(xf, m_vertices[i]);+ lower = b2Min(lower, v);+ upper = b2Max(upper, v);+ }++ b2Vec2 r(m_radius, m_radius);+ aabb->lowerBound = lower - r;+ aabb->upperBound = upper + r;+}++void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const+{+ // Polygon mass, centroid, and inertia.+ // Let rho be the polygon density in mass per unit area.+ // Then:+ // mass = rho * int(dA)+ // centroid.x = (1/mass) * rho * int(x * dA)+ // centroid.y = (1/mass) * rho * int(y * dA)+ // I = rho * int((x*x + y*y) * dA)+ //+ // We can compute these integrals by summing all the integrals+ // for each triangle of the polygon. To evaluate the integral+ // for a single triangle, we make a change of variables to+ // the (u,v) coordinates of the triangle:+ // x = x0 + e1x * u + e2x * v+ // y = y0 + e1y * u + e2y * v+ // where 0 <= u && 0 <= v && u + v <= 1.+ //+ // We integrate u from [0,1-v] and then v from [0,1].+ // We also need to use the Jacobian of the transformation:+ // D = cross(e1, e2)+ //+ // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)+ //+ // The rest of the derivation is handled by computer algebra.++ b2Assert(m_count >= 3);++ b2Vec2 center; center.Set(0.0f, 0.0f);+ float32 area = 0.0f;+ float32 I = 0.0f;++ // s is the reference point for forming triangles.+ // It's location doesn't change the result (except for rounding error).+ b2Vec2 s(0.0f, 0.0f);++ // This code would put the reference point inside the polygon.+ for (int32 i = 0; i < m_count; ++i)+ {+ s += m_vertices[i];+ }+ s *= 1.0f / m_count;++ const float32 k_inv3 = 1.0f / 3.0f;++ for (int32 i = 0; i < m_count; ++i)+ {+ // Triangle vertices.+ b2Vec2 e1 = m_vertices[i] - s;+ b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;++ float32 D = b2Cross(e1, e2);++ float32 triangleArea = 0.5f * D;+ area += triangleArea;++ // Area weighted centroid+ center += triangleArea * k_inv3 * (e1 + e2);++ float32 ex1 = e1.x, ey1 = e1.y;+ float32 ex2 = e2.x, ey2 = e2.y;++ float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;+ float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;++ I += (0.25f * k_inv3 * D) * (intx2 + inty2);+ }++ // Total mass+ massData->mass = density * area;++ // Center of mass+ b2Assert(area > b2_epsilon);+ center *= 1.0f / area;+ massData->center = center + s;++ // Inertia tensor relative to the local origin (point s).+ massData->I = density * I;++ // Shift to center of mass then to original body origin.+ massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));+}++bool b2PolygonShape::Validate() const+{+ for (int32 i = 0; i < m_count; ++i)+ {+ int32 i1 = i;+ int32 i2 = i < m_count - 1 ? i1 + 1 : 0;+ b2Vec2 p = m_vertices[i1];+ b2Vec2 e = m_vertices[i2] - p;++ for (int32 j = 0; j < m_count; ++j)+ {+ if (j == i1 || j == i2)+ {+ continue;+ }++ b2Vec2 v = m_vertices[j] - p;+ float32 c = b2Cross(e, v);+ if (c < 0.0f)+ {+ return false;+ }+ }+ }++ return true;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.h b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.hnew file mode 100644index 00000000..d5c0e742--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.h@@ -0,0 +1,101 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_POLYGON_SHAPE_H+#define B2_POLYGON_SHAPE_H++#include <Box2D/Collision/Shapes/b2Shape.h>++/// A convex polygon. It is assumed that the interior of the polygon is to+/// the left of each edge.+/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.+/// In most cases you should not need many vertices for a convex polygon.+class b2PolygonShape : public b2Shape+{+public:+ b2PolygonShape();++ /// Implement b2Shape.+ b2Shape* Clone(b2BlockAllocator* allocator) const;++ /// @see b2Shape::GetChildCount+ int32 GetChildCount() const;++ /// Create a convex hull from the given array of local points.+ /// The count must be in the range [3, b2_maxPolygonVertices].+ /// @warning the points may be re-ordered, even if they form a convex polygon+ /// @warning collinear points are handled but not removed. Collinear points+ /// may lead to poor stacking behavior.+ void Set(const b2Vec2* points, int32 count);++ /// Build vertices to represent an axis-aligned box centered on the local origin.+ /// @param hx the half-width.+ /// @param hy the half-height.+ void SetAsBox(float32 hx, float32 hy);++ /// Build vertices to represent an oriented box.+ /// @param hx the half-width.+ /// @param hy the half-height.+ /// @param center the center of the box in local coordinates.+ /// @param angle the rotation of the box in local coordinates.+ void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);++ /// @see b2Shape::TestPoint+ bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;++ /// Implement b2Shape.+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeAABB+ void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;++ /// @see b2Shape::ComputeMass+ void ComputeMass(b2MassData* massData, float32 density) const;++ /// Get the vertex count.+ int32 GetVertexCount() const { return m_count; }++ /// Get a vertex by index.+ const b2Vec2& GetVertex(int32 index) const;++ /// Validate convexity. This is a very time consuming operation.+ /// @returns true if valid+ bool Validate() const;++ b2Vec2 m_centroid;+ b2Vec2 m_vertices[b2_maxPolygonVertices];+ b2Vec2 m_normals[b2_maxPolygonVertices];+ int32 m_count;+};++inline b2PolygonShape::b2PolygonShape()+{+ m_type = e_polygon;+ m_radius = b2_polygonRadius;+ m_count = 0;+ m_centroid.SetZero();+}++inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const+{+ b2Assert(0 <= index && index < m_count);+ return m_vertices[index];+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2Shape.h b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2Shape.hnew file mode 100644index 00000000..fd7de262--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2Shape.h@@ -0,0 +1,101 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_SHAPE_H+#define B2_SHAPE_H++#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Common/b2Math.h>+#include <Box2D/Collision/b2Collision.h>++/// This holds the mass data computed for a shape.+struct b2MassData+{+ /// The mass of the shape, usually in kilograms.+ float32 mass;++ /// The position of the shape's centroid relative to the shape's origin.+ b2Vec2 center;++ /// The rotational inertia of the shape about the local origin.+ float32 I;+};++/// A shape is used for collision detection. You can create a shape however you like.+/// Shapes used for simulation in b2World are created automatically when a b2Fixture+/// is created. Shapes may encapsulate a one or more child shapes.+class b2Shape+{+public:++ enum Type+ {+ e_circle = 0,+ e_edge = 1,+ e_polygon = 2,+ e_chain = 3,+ e_typeCount = 4+ };++ virtual ~b2Shape() {}++ /// Clone the concrete shape using the provided allocator.+ virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;++ /// Get the type of this shape. You can use this to down cast to the concrete shape.+ /// @return the shape type.+ Type GetType() const;++ /// Get the number of child primitives.+ virtual int32 GetChildCount() const = 0;++ /// Test a point for containment in this shape. This only works for convex shapes.+ /// @param xf the shape world transform.+ /// @param p a point in world coordinates.+ virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;++ /// Cast a ray against a child shape.+ /// @param output the ray-cast results.+ /// @param input the ray-cast input parameters.+ /// @param transform the transform to be applied to the shape.+ /// @param childIndex the child shape index+ virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,+ const b2Transform& transform, int32 childIndex) const = 0;++ /// Given a transform, compute the associated axis aligned bounding box for a child shape.+ /// @param aabb returns the axis aligned box.+ /// @param xf the world transform of the shape.+ /// @param childIndex the child shape+ virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;++ /// Compute the mass properties of this shape using its dimensions and density.+ /// The inertia tensor is computed about the local origin.+ /// @param massData returns the mass data for this shape.+ /// @param density the density in kilograms per meter squared.+ virtual void ComputeMass(b2MassData* massData, float32 density) const = 0;++ Type m_type;+ float32 m_radius;+};++inline b2Shape::Type b2Shape::GetType() const+{+ return m_type;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.cppnew file mode 100644index 00000000..6cddf719--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.cpp@@ -0,0 +1,119 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2BroadPhase.h>++b2BroadPhase::b2BroadPhase()+{+ m_proxyCount = 0;++ m_pairCapacity = 16;+ m_pairCount = 0;+ m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));++ m_moveCapacity = 16;+ m_moveCount = 0;+ m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));+}++b2BroadPhase::~b2BroadPhase()+{+ b2Free(m_moveBuffer);+ b2Free(m_pairBuffer);+}++int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)+{+ int32 proxyId = m_tree.CreateProxy(aabb, userData);+ ++m_proxyCount;+ BufferMove(proxyId);+ return proxyId;+}++void b2BroadPhase::DestroyProxy(int32 proxyId)+{+ UnBufferMove(proxyId);+ --m_proxyCount;+ m_tree.DestroyProxy(proxyId);+}++void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)+{+ bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);+ if (buffer)+ {+ BufferMove(proxyId);+ }+}++void b2BroadPhase::TouchProxy(int32 proxyId)+{+ BufferMove(proxyId);+}++void b2BroadPhase::BufferMove(int32 proxyId)+{+ if (m_moveCount == m_moveCapacity)+ {+ int32* oldBuffer = m_moveBuffer;+ m_moveCapacity *= 2;+ m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));+ memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));+ b2Free(oldBuffer);+ }++ m_moveBuffer[m_moveCount] = proxyId;+ ++m_moveCount;+}++void b2BroadPhase::UnBufferMove(int32 proxyId)+{+ for (int32 i = 0; i < m_moveCount; ++i)+ {+ if (m_moveBuffer[i] == proxyId)+ {+ m_moveBuffer[i] = e_nullProxy;+ }+ }+}++// This is called from b2DynamicTree::Query when we are gathering pairs.+bool b2BroadPhase::QueryCallback(int32 proxyId)+{+ // A proxy cannot form a pair with itself.+ if (proxyId == m_queryProxyId)+ {+ return true;+ }++ // Grow the pair buffer as needed.+ if (m_pairCount == m_pairCapacity)+ {+ b2Pair* oldBuffer = m_pairBuffer;+ m_pairCapacity *= 2;+ m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));+ memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));+ b2Free(oldBuffer);+ }++ m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);+ m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);+ ++m_pairCount;++ return true;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.h b/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.hnew file mode 100644index 00000000..49057104--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.h@@ -0,0 +1,257 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_BROAD_PHASE_H+#define B2_BROAD_PHASE_H++#include <Box2D/Common/b2Settings.h>+#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/b2DynamicTree.h>+#include <algorithm>++struct b2Pair+{+ int32 proxyIdA;+ int32 proxyIdB;+};++/// The broad-phase is used for computing pairs and performing volume queries and ray casts.+/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.+/// It is up to the client to consume the new pairs and to track subsequent overlap.+class b2BroadPhase+{+public:++ enum+ {+ e_nullProxy = -1+ };++ b2BroadPhase();+ ~b2BroadPhase();++ /// Create a proxy with an initial AABB. Pairs are not reported until+ /// UpdatePairs is called.+ int32 CreateProxy(const b2AABB& aabb, void* userData);++ /// Destroy a proxy. It is up to the client to remove any pairs.+ void DestroyProxy(int32 proxyId);++ /// Call MoveProxy as many times as you like, then when you are done+ /// call UpdatePairs to finalized the proxy pairs (for your time step).+ void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);++ /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.+ void TouchProxy(int32 proxyId);++ /// Get the fat AABB for a proxy.+ const b2AABB& GetFatAABB(int32 proxyId) const;++ /// Get user data from a proxy. Returns NULL if the id is invalid.+ void* GetUserData(int32 proxyId) const;++ /// Test overlap of fat AABBs.+ bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;++ /// Get the number of proxies.+ int32 GetProxyCount() const;++ /// Update the pairs. This results in pair callbacks. This can only add pairs.+ template <typename T>+ void UpdatePairs(T* callback);++ /// Query an AABB for overlapping proxies. The callback class+ /// is called for each proxy that overlaps the supplied AABB.+ template <typename T>+ void Query(T* callback, const b2AABB& aabb) const;++ /// Ray-cast against the proxies in the tree. This relies on the callback+ /// to perform a exact ray-cast in the case were the proxy contains a shape.+ /// The callback also performs the any collision filtering. This has performance+ /// roughly equal to k * log(n), where k is the number of collisions and n is the+ /// number of proxies in the tree.+ /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).+ /// @param callback a callback class that is called for each proxy that is hit by the ray.+ template <typename T>+ void RayCast(T* callback, const b2RayCastInput& input) const;++ /// Get the height of the embedded tree.+ int32 GetTreeHeight() const;++ /// Get the balance of the embedded tree.+ int32 GetTreeBalance() const;++ /// Get the quality metric of the embedded tree.+ float32 GetTreeQuality() const;++ /// Shift the world origin. Useful for large worlds.+ /// The shift formula is: position -= newOrigin+ /// @param newOrigin the new origin with respect to the old origin+ void ShiftOrigin(const b2Vec2& newOrigin);++private:++ friend class b2DynamicTree;++ void BufferMove(int32 proxyId);+ void UnBufferMove(int32 proxyId);++ bool QueryCallback(int32 proxyId);++ b2DynamicTree m_tree;++ int32 m_proxyCount;++ int32* m_moveBuffer;+ int32 m_moveCapacity;+ int32 m_moveCount;++ b2Pair* m_pairBuffer;+ int32 m_pairCapacity;+ int32 m_pairCount;++ int32 m_queryProxyId;+};++/// This is used to sort pairs.+inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)+{+ if (pair1.proxyIdA < pair2.proxyIdA)+ {+ return true;+ }++ if (pair1.proxyIdA == pair2.proxyIdA)+ {+ return pair1.proxyIdB < pair2.proxyIdB;+ }++ return false;+}++inline void* b2BroadPhase::GetUserData(int32 proxyId) const+{+ return m_tree.GetUserData(proxyId);+}++inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const+{+ const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);+ const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);+ return b2TestOverlap(aabbA, aabbB);+}++inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const+{+ return m_tree.GetFatAABB(proxyId);+}++inline int32 b2BroadPhase::GetProxyCount() const+{+ return m_proxyCount;+}++inline int32 b2BroadPhase::GetTreeHeight() const+{+ return m_tree.GetHeight();+}++inline int32 b2BroadPhase::GetTreeBalance() const+{+ return m_tree.GetMaxBalance();+}++inline float32 b2BroadPhase::GetTreeQuality() const+{+ return m_tree.GetAreaRatio();+}++template <typename T>+void b2BroadPhase::UpdatePairs(T* callback)+{+ // Reset pair buffer+ m_pairCount = 0;++ // Perform tree queries for all moving proxies.+ for (int32 i = 0; i < m_moveCount; ++i)+ {+ m_queryProxyId = m_moveBuffer[i];+ if (m_queryProxyId == e_nullProxy)+ {+ continue;+ }++ // We have to query the tree with the fat AABB so that+ // we don't fail to create a pair that may touch later.+ const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);++ // Query tree, create pairs and add them pair buffer.+ m_tree.Query(this, fatAABB);+ }++ // Reset move buffer+ m_moveCount = 0;++ // Sort the pair buffer to expose duplicates.+ std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);++ // Send the pairs back to the client.+ int32 i = 0;+ while (i < m_pairCount)+ {+ b2Pair* primaryPair = m_pairBuffer + i;+ void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);+ void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);++ callback->AddPair(userDataA, userDataB);+ ++i;++ // Skip any duplicate pairs.+ while (i < m_pairCount)+ {+ b2Pair* pair = m_pairBuffer + i;+ if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)+ {+ break;+ }+ ++i;+ }+ }++ // Try to keep the tree balanced.+ //m_tree.Rebalance(4);+}++template <typename T>+inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const+{+ m_tree.Query(callback, aabb);+}++template <typename T>+inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const+{+ m_tree.RayCast(callback, input);+}++inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)+{+ m_tree.ShiftOrigin(newOrigin);+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2CollideCircle.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2CollideCircle.cppnew file mode 100644index 00000000..e60b2086--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2CollideCircle.cpp@@ -0,0 +1,154 @@+/*+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>++void b2CollideCircles(+ b2Manifold* manifold,+ const b2CircleShape* circleA, const b2Transform& xfA,+ const b2CircleShape* circleB, const b2Transform& xfB)+{+ manifold->pointCount = 0;++ b2Vec2 pA = b2Mul(xfA, circleA->m_p);+ b2Vec2 pB = b2Mul(xfB, circleB->m_p);++ b2Vec2 d = pB - pA;+ float32 distSqr = b2Dot(d, d);+ float32 rA = circleA->m_radius, rB = circleB->m_radius;+ float32 radius = rA + rB;+ if (distSqr > radius * radius)+ {+ return;+ }++ manifold->type = b2Manifold::e_circles;+ manifold->localPoint = circleA->m_p;+ manifold->localNormal.SetZero();+ manifold->pointCount = 1;++ manifold->points[0].localPoint = circleB->m_p;+ manifold->points[0].id.key = 0;+}++void b2CollidePolygonAndCircle(+ b2Manifold* manifold,+ const b2PolygonShape* polygonA, const b2Transform& xfA,+ const b2CircleShape* circleB, const b2Transform& xfB)+{+ manifold->pointCount = 0;++ // Compute circle position in the frame of the polygon.+ b2Vec2 c = b2Mul(xfB, circleB->m_p);+ b2Vec2 cLocal = b2MulT(xfA, c);++ // Find the min separating edge.+ int32 normalIndex = 0;+ float32 separation = -b2_maxFloat;+ float32 radius = polygonA->m_radius + circleB->m_radius;+ int32 vertexCount = polygonA->m_count;+ const b2Vec2* vertices = polygonA->m_vertices;+ const b2Vec2* normals = polygonA->m_normals;++ for (int32 i = 0; i < vertexCount; ++i)+ {+ float32 s = b2Dot(normals[i], cLocal - vertices[i]);++ if (s > radius)+ {+ // Early out.+ return;+ }++ if (s > separation)+ {+ separation = s;+ normalIndex = i;+ }+ }++ // Vertices that subtend the incident face.+ int32 vertIndex1 = normalIndex;+ int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;+ b2Vec2 v1 = vertices[vertIndex1];+ b2Vec2 v2 = vertices[vertIndex2];++ // If the center is inside the polygon ...+ if (separation < b2_epsilon)+ {+ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_faceA;+ manifold->localNormal = normals[normalIndex];+ manifold->localPoint = 0.5f * (v1 + v2);+ manifold->points[0].localPoint = circleB->m_p;+ manifold->points[0].id.key = 0;+ return;+ }++ // Compute barycentric coordinates+ float32 u1 = b2Dot(cLocal - v1, v2 - v1);+ float32 u2 = b2Dot(cLocal - v2, v1 - v2);+ if (u1 <= 0.0f)+ {+ if (b2DistanceSquared(cLocal, v1) > radius * radius)+ {+ return;+ }++ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_faceA;+ manifold->localNormal = cLocal - v1;+ manifold->localNormal.Normalize();+ manifold->localPoint = v1;+ manifold->points[0].localPoint = circleB->m_p;+ manifold->points[0].id.key = 0;+ }+ else if (u2 <= 0.0f)+ {+ if (b2DistanceSquared(cLocal, v2) > radius * radius)+ {+ return;+ }++ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_faceA;+ manifold->localNormal = cLocal - v2;+ manifold->localNormal.Normalize();+ manifold->localPoint = v2;+ manifold->points[0].localPoint = circleB->m_p;+ manifold->points[0].id.key = 0;+ }+ else+ {+ b2Vec2 faceCenter = 0.5f * (v1 + v2);+ float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);+ if (separation > radius)+ {+ return;+ }++ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_faceA;+ manifold->localNormal = normals[vertIndex1];+ manifold->localPoint = faceCenter;+ manifold->points[0].localPoint = circleB->m_p;+ manifold->points[0].id.key = 0;+ }+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2CollideEdge.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2CollideEdge.cppnew file mode 100644index 00000000..c8276be7--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2CollideEdge.cpp@@ -0,0 +1,698 @@+/*+ * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org+ *+ * This software is provided 'as-is', without any express or implied+ * warranty. In no event will the authors be held liable for any damages+ * arising from the use of this software.+ * Permission is granted to anyone to use this software for any purpose,+ * including commercial applications, and to alter it and redistribute it+ * freely, subject to the following restrictions:+ * 1. The origin of this software must not be misrepresented; you must not+ * claim that you wrote the original software. If you use this software+ * in a product, an acknowledgment in the product documentation would be+ * appreciated but is not required.+ * 2. Altered source versions must be plainly marked as such, and must not be+ * misrepresented as being the original software.+ * 3. This notice may not be removed or altered from any source distribution.+ */++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>+++// Compute contact points for edge versus circle.+// This accounts for edge connectivity.+void b2CollideEdgeAndCircle(b2Manifold* manifold,+ const b2EdgeShape* edgeA, const b2Transform& xfA,+ const b2CircleShape* circleB, const b2Transform& xfB)+{+ manifold->pointCount = 0;++ // Compute circle in frame of edge+ b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));++ b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;+ b2Vec2 e = B - A;++ // Barycentric coordinates+ float32 u = b2Dot(e, B - Q);+ float32 v = b2Dot(e, Q - A);++ float32 radius = edgeA->m_radius + circleB->m_radius;++ b2ContactFeature cf;+ cf.indexB = 0;+ cf.typeB = b2ContactFeature::e_vertex;++ // Region A+ if (v <= 0.0f)+ {+ b2Vec2 P = A;+ b2Vec2 d = Q - P;+ float32 dd = b2Dot(d, d);+ if (dd > radius * radius)+ {+ return;+ }++ // Is there an edge connected to A?+ if (edgeA->m_hasVertex0)+ {+ b2Vec2 A1 = edgeA->m_vertex0;+ b2Vec2 B1 = A;+ b2Vec2 e1 = B1 - A1;+ float32 u1 = b2Dot(e1, B1 - Q);++ // Is the circle in Region AB of the previous edge?+ if (u1 > 0.0f)+ {+ return;+ }+ }++ cf.indexA = 0;+ cf.typeA = b2ContactFeature::e_vertex;+ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_circles;+ manifold->localNormal.SetZero();+ manifold->localPoint = P;+ manifold->points[0].id.key = 0;+ manifold->points[0].id.cf = cf;+ manifold->points[0].localPoint = circleB->m_p;+ return;+ }++ // Region B+ if (u <= 0.0f)+ {+ b2Vec2 P = B;+ b2Vec2 d = Q - P;+ float32 dd = b2Dot(d, d);+ if (dd > radius * radius)+ {+ return;+ }++ // Is there an edge connected to B?+ if (edgeA->m_hasVertex3)+ {+ b2Vec2 B2 = edgeA->m_vertex3;+ b2Vec2 A2 = B;+ b2Vec2 e2 = B2 - A2;+ float32 v2 = b2Dot(e2, Q - A2);++ // Is the circle in Region AB of the next edge?+ if (v2 > 0.0f)+ {+ return;+ }+ }++ cf.indexA = 1;+ cf.typeA = b2ContactFeature::e_vertex;+ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_circles;+ manifold->localNormal.SetZero();+ manifold->localPoint = P;+ manifold->points[0].id.key = 0;+ manifold->points[0].id.cf = cf;+ manifold->points[0].localPoint = circleB->m_p;+ return;+ }++ // Region AB+ float32 den = b2Dot(e, e);+ b2Assert(den > 0.0f);+ b2Vec2 P = (1.0f / den) * (u * A + v * B);+ b2Vec2 d = Q - P;+ float32 dd = b2Dot(d, d);+ if (dd > radius * radius)+ {+ return;+ }++ b2Vec2 n(-e.y, e.x);+ if (b2Dot(n, Q - A) < 0.0f)+ {+ n.Set(-n.x, -n.y);+ }+ n.Normalize();++ cf.indexA = 0;+ cf.typeA = b2ContactFeature::e_face;+ manifold->pointCount = 1;+ manifold->type = b2Manifold::e_faceA;+ manifold->localNormal = n;+ manifold->localPoint = A;+ manifold->points[0].id.key = 0;+ manifold->points[0].id.cf = cf;+ manifold->points[0].localPoint = circleB->m_p;+}++// This structure is used to keep track of the best separating axis.+struct b2EPAxis+{+ enum Type+ {+ e_unknown,+ e_edgeA,+ e_edgeB+ };++ Type type;+ int32 index;+ float32 separation;+};++// This holds polygon B expressed in frame A.+struct b2TempPolygon+{+ b2Vec2 vertices[b2_maxPolygonVertices];+ b2Vec2 normals[b2_maxPolygonVertices];+ int32 count;+};++// Reference face used for clipping+struct b2ReferenceFace+{+ int32 i1, i2;++ b2Vec2 v1, v2;++ b2Vec2 normal;++ b2Vec2 sideNormal1;+ float32 sideOffset1;++ b2Vec2 sideNormal2;+ float32 sideOffset2;+};++// This class collides and edge and a polygon, taking into account edge adjacency.+struct b2EPCollider+{+ void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,+ const b2PolygonShape* polygonB, const b2Transform& xfB);+ b2EPAxis ComputeEdgeSeparation();+ b2EPAxis ComputePolygonSeparation();++ enum VertexType+ {+ e_isolated,+ e_concave,+ e_convex+ };++ b2TempPolygon m_polygonB;++ b2Transform m_xf;+ b2Vec2 m_centroidB;+ b2Vec2 m_v0, m_v1, m_v2, m_v3;+ b2Vec2 m_normal0, m_normal1, m_normal2;+ b2Vec2 m_normal;+ VertexType m_type1, m_type2;+ b2Vec2 m_lowerLimit, m_upperLimit;+ float32 m_radius;+ bool m_front;+};++// Algorithm:+// 1. Classify v1 and v2+// 2. Classify polygon centroid as front or back+// 3. Flip normal if necessary+// 4. Initialize normal range to [-pi, pi] about face normal+// 5. Adjust normal range according to adjacent edges+// 6. Visit each separating axes, only accept axes within the range+// 7. Return if _any_ axis indicates separation+// 8. Clip+void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,+ const b2PolygonShape* polygonB, const b2Transform& xfB)+{+ m_xf = b2MulT(xfA, xfB);++ m_centroidB = b2Mul(m_xf, polygonB->m_centroid);++ m_v0 = edgeA->m_vertex0;+ m_v1 = edgeA->m_vertex1;+ m_v2 = edgeA->m_vertex2;+ m_v3 = edgeA->m_vertex3;++ bool hasVertex0 = edgeA->m_hasVertex0;+ bool hasVertex3 = edgeA->m_hasVertex3;++ b2Vec2 edge1 = m_v2 - m_v1;+ edge1.Normalize();+ m_normal1.Set(edge1.y, -edge1.x);+ float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);+ float32 offset0 = 0.0f, offset2 = 0.0f;+ bool convex1 = false, convex2 = false;++ // Is there a preceding edge?+ if (hasVertex0)+ {+ b2Vec2 edge0 = m_v1 - m_v0;+ edge0.Normalize();+ m_normal0.Set(edge0.y, -edge0.x);+ convex1 = b2Cross(edge0, edge1) >= 0.0f;+ offset0 = b2Dot(m_normal0, m_centroidB - m_v0);+ }++ // Is there a following edge?+ if (hasVertex3)+ {+ b2Vec2 edge2 = m_v3 - m_v2;+ edge2.Normalize();+ m_normal2.Set(edge2.y, -edge2.x);+ convex2 = b2Cross(edge1, edge2) > 0.0f;+ offset2 = b2Dot(m_normal2, m_centroidB - m_v2);+ }++ // Determine front or back collision. Determine collision normal limits.+ if (hasVertex0 && hasVertex3)+ {+ if (convex1 && convex2)+ {+ m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = m_normal0;+ m_upperLimit = m_normal2;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = -m_normal1;+ m_upperLimit = -m_normal1;+ }+ }+ else if (convex1)+ {+ m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = m_normal0;+ m_upperLimit = m_normal1;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = -m_normal2;+ m_upperLimit = -m_normal1;+ }+ }+ else if (convex2)+ {+ m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = m_normal1;+ m_upperLimit = m_normal2;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = -m_normal1;+ m_upperLimit = -m_normal0;+ }+ }+ else+ {+ m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = m_normal1;+ m_upperLimit = m_normal1;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = -m_normal2;+ m_upperLimit = -m_normal0;+ }+ }+ }+ else if (hasVertex0)+ {+ if (convex1)+ {+ m_front = offset0 >= 0.0f || offset1 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = m_normal0;+ m_upperLimit = -m_normal1;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = m_normal1;+ m_upperLimit = -m_normal1;+ }+ }+ else+ {+ m_front = offset0 >= 0.0f && offset1 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = m_normal1;+ m_upperLimit = -m_normal1;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = m_normal1;+ m_upperLimit = -m_normal0;+ }+ }+ }+ else if (hasVertex3)+ {+ if (convex2)+ {+ m_front = offset1 >= 0.0f || offset2 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = -m_normal1;+ m_upperLimit = m_normal2;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = -m_normal1;+ m_upperLimit = m_normal1;+ }+ }+ else+ {+ m_front = offset1 >= 0.0f && offset2 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = -m_normal1;+ m_upperLimit = m_normal1;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = -m_normal2;+ m_upperLimit = m_normal1;+ }+ }+ }+ else+ {+ m_front = offset1 >= 0.0f;+ if (m_front)+ {+ m_normal = m_normal1;+ m_lowerLimit = -m_normal1;+ m_upperLimit = -m_normal1;+ }+ else+ {+ m_normal = -m_normal1;+ m_lowerLimit = m_normal1;+ m_upperLimit = m_normal1;+ }+ }++ // Get polygonB in frameA+ m_polygonB.count = polygonB->m_count;+ for (int32 i = 0; i < polygonB->m_count; ++i)+ {+ m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);+ m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);+ }++ m_radius = 2.0f * b2_polygonRadius;++ manifold->pointCount = 0;++ b2EPAxis edgeAxis = ComputeEdgeSeparation();++ // If no valid normal can be found than this edge should not collide.+ if (edgeAxis.type == b2EPAxis::e_unknown)+ {+ return;+ }++ if (edgeAxis.separation > m_radius)+ {+ return;+ }++ b2EPAxis polygonAxis = ComputePolygonSeparation();+ if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)+ {+ return;+ }++ // Use hysteresis for jitter reduction.+ const float32 k_relativeTol = 0.98f;+ const float32 k_absoluteTol = 0.001f;++ b2EPAxis primaryAxis;+ if (polygonAxis.type == b2EPAxis::e_unknown)+ {+ primaryAxis = edgeAxis;+ }+ else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)+ {+ primaryAxis = polygonAxis;+ }+ else+ {+ primaryAxis = edgeAxis;+ }++ b2ClipVertex ie[2];+ b2ReferenceFace rf;+ if (primaryAxis.type == b2EPAxis::e_edgeA)+ {+ manifold->type = b2Manifold::e_faceA;++ // Search for the polygon normal that is most anti-parallel to the edge normal.+ int32 bestIndex = 0;+ float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);+ for (int32 i = 1; i < m_polygonB.count; ++i)+ {+ float32 value = b2Dot(m_normal, m_polygonB.normals[i]);+ if (value < bestValue)+ {+ bestValue = value;+ bestIndex = i;+ }+ }++ int32 i1 = bestIndex;+ int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;++ ie[0].v = m_polygonB.vertices[i1];+ ie[0].id.cf.indexA = 0;+ ie[0].id.cf.indexB = static_cast<uint8>(i1);+ ie[0].id.cf.typeA = b2ContactFeature::e_face;+ ie[0].id.cf.typeB = b2ContactFeature::e_vertex;++ ie[1].v = m_polygonB.vertices[i2];+ ie[1].id.cf.indexA = 0;+ ie[1].id.cf.indexB = static_cast<uint8>(i2);+ ie[1].id.cf.typeA = b2ContactFeature::e_face;+ ie[1].id.cf.typeB = b2ContactFeature::e_vertex;++ if (m_front)+ {+ rf.i1 = 0;+ rf.i2 = 1;+ rf.v1 = m_v1;+ rf.v2 = m_v2;+ rf.normal = m_normal1;+ }+ else+ {+ rf.i1 = 1;+ rf.i2 = 0;+ rf.v1 = m_v2;+ rf.v2 = m_v1;+ rf.normal = -m_normal1;+ }+ }+ else+ {+ manifold->type = b2Manifold::e_faceB;++ ie[0].v = m_v1;+ ie[0].id.cf.indexA = 0;+ ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);+ ie[0].id.cf.typeA = b2ContactFeature::e_vertex;+ ie[0].id.cf.typeB = b2ContactFeature::e_face;++ ie[1].v = m_v2;+ ie[1].id.cf.indexA = 0;+ ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);+ ie[1].id.cf.typeA = b2ContactFeature::e_vertex;+ ie[1].id.cf.typeB = b2ContactFeature::e_face;++ rf.i1 = primaryAxis.index;+ rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;+ rf.v1 = m_polygonB.vertices[rf.i1];+ rf.v2 = m_polygonB.vertices[rf.i2];+ rf.normal = m_polygonB.normals[rf.i1];+ }++ rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);+ rf.sideNormal2 = -rf.sideNormal1;+ rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);+ rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);++ // Clip incident edge against extruded edge1 side edges.+ b2ClipVertex clipPoints1[2];+ b2ClipVertex clipPoints2[2];+ int32 np;++ // Clip to box side 1+ np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);++ if (np < b2_maxManifoldPoints)+ {+ return;+ }++ // Clip to negative box side 1+ np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);++ if (np < b2_maxManifoldPoints)+ {+ return;+ }++ // Now clipPoints2 contains the clipped points.+ if (primaryAxis.type == b2EPAxis::e_edgeA)+ {+ manifold->localNormal = rf.normal;+ manifold->localPoint = rf.v1;+ }+ else+ {+ manifold->localNormal = polygonB->m_normals[rf.i1];+ manifold->localPoint = polygonB->m_vertices[rf.i1];+ }++ int32 pointCount = 0;+ for (int32 i = 0; i < b2_maxManifoldPoints; ++i)+ {+ float32 separation;++ separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);++ if (separation <= m_radius)+ {+ b2ManifoldPoint* cp = manifold->points + pointCount;++ if (primaryAxis.type == b2EPAxis::e_edgeA)+ {+ cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);+ cp->id = clipPoints2[i].id;+ }+ else+ {+ cp->localPoint = clipPoints2[i].v;+ cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;+ cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;+ cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;+ cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;+ }++ ++pointCount;+ }+ }++ manifold->pointCount = pointCount;+}++b2EPAxis b2EPCollider::ComputeEdgeSeparation()+{+ b2EPAxis axis;+ axis.type = b2EPAxis::e_edgeA;+ axis.index = m_front ? 0 : 1;+ axis.separation = FLT_MAX;++ for (int32 i = 0; i < m_polygonB.count; ++i)+ {+ float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);+ if (s < axis.separation)+ {+ axis.separation = s;+ }+ }++ return axis;+}++b2EPAxis b2EPCollider::ComputePolygonSeparation()+{+ b2EPAxis axis;+ axis.type = b2EPAxis::e_unknown;+ axis.index = -1;+ axis.separation = -FLT_MAX;++ b2Vec2 perp(-m_normal.y, m_normal.x);++ for (int32 i = 0; i < m_polygonB.count; ++i)+ {+ b2Vec2 n = -m_polygonB.normals[i];++ float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);+ float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);+ float32 s = b2Min(s1, s2);++ if (s > m_radius)+ {+ // No collision+ axis.type = b2EPAxis::e_edgeB;+ axis.index = i;+ axis.separation = s;+ return axis;+ }++ // Adjacency+ if (b2Dot(n, perp) >= 0.0f)+ {+ if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)+ {+ continue;+ }+ }+ else+ {+ if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)+ {+ continue;+ }+ }++ if (s > axis.separation)+ {+ axis.type = b2EPAxis::e_edgeB;+ axis.index = i;+ axis.separation = s;+ }+ }++ return axis;+}++void b2CollideEdgeAndPolygon( b2Manifold* manifold,+ const b2EdgeShape* edgeA, const b2Transform& xfA,+ const b2PolygonShape* polygonB, const b2Transform& xfB)+{+ b2EPCollider collider;+ collider.Collide(manifold, edgeA, xfA, polygonB, xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2CollidePolygon.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2CollidePolygon.cppnew file mode 100644index 00000000..b24d01e7--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2CollidePolygon.cpp@@ -0,0 +1,239 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>++// Find the max separation between poly1 and poly2 using edge normals from poly1.+static float32 b2FindMaxSeparation(int32* edgeIndex,+ const b2PolygonShape* poly1, const b2Transform& xf1,+ const b2PolygonShape* poly2, const b2Transform& xf2)+{+ int32 count1 = poly1->m_count;+ int32 count2 = poly2->m_count;+ const b2Vec2* n1s = poly1->m_normals;+ const b2Vec2* v1s = poly1->m_vertices;+ const b2Vec2* v2s = poly2->m_vertices;+ b2Transform xf = b2MulT(xf2, xf1);++ int32 bestIndex = 0;+ float32 maxSeparation = -b2_maxFloat;+ for (int32 i = 0; i < count1; ++i)+ {+ // Get poly1 normal in frame2.+ b2Vec2 n = b2Mul(xf.q, n1s[i]);+ b2Vec2 v1 = b2Mul(xf, v1s[i]);++ // Find deepest point for normal i.+ float32 si = b2_maxFloat;+ for (int32 j = 0; j < count2; ++j)+ {+ float32 sij = b2Dot(n, v2s[j] - v1);+ if (sij < si)+ {+ si = sij;+ }+ }++ if (si > maxSeparation)+ {+ maxSeparation = si;+ bestIndex = i;+ }+ }++ *edgeIndex = bestIndex;+ return maxSeparation;+}++static void b2FindIncidentEdge(b2ClipVertex c[2],+ const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,+ const b2PolygonShape* poly2, const b2Transform& xf2)+{+ const b2Vec2* normals1 = poly1->m_normals;++ int32 count2 = poly2->m_count;+ const b2Vec2* vertices2 = poly2->m_vertices;+ const b2Vec2* normals2 = poly2->m_normals;++ b2Assert(0 <= edge1 && edge1 < poly1->m_count);++ // Get the normal of the reference edge in poly2's frame.+ b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));++ // Find the incident edge on poly2.+ int32 index = 0;+ float32 minDot = b2_maxFloat;+ for (int32 i = 0; i < count2; ++i)+ {+ float32 dot = b2Dot(normal1, normals2[i]);+ if (dot < minDot)+ {+ minDot = dot;+ index = i;+ }+ }++ // Build the clip vertices for the incident edge.+ int32 i1 = index;+ int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;++ c[0].v = b2Mul(xf2, vertices2[i1]);+ c[0].id.cf.indexA = (uint8)edge1;+ c[0].id.cf.indexB = (uint8)i1;+ c[0].id.cf.typeA = b2ContactFeature::e_face;+ c[0].id.cf.typeB = b2ContactFeature::e_vertex;++ c[1].v = b2Mul(xf2, vertices2[i2]);+ c[1].id.cf.indexA = (uint8)edge1;+ c[1].id.cf.indexB = (uint8)i2;+ c[1].id.cf.typeA = b2ContactFeature::e_face;+ c[1].id.cf.typeB = b2ContactFeature::e_vertex;+}++// Find edge normal of max separation on A - return if separating axis is found+// Find edge normal of max separation on B - return if separation axis is found+// Choose reference edge as min(minA, minB)+// Find incident edge+// Clip++// The normal points from 1 to 2+void b2CollidePolygons(b2Manifold* manifold,+ const b2PolygonShape* polyA, const b2Transform& xfA,+ const b2PolygonShape* polyB, const b2Transform& xfB)+{+ manifold->pointCount = 0;+ float32 totalRadius = polyA->m_radius + polyB->m_radius;++ int32 edgeA = 0;+ float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);+ if (separationA > totalRadius)+ return;++ int32 edgeB = 0;+ float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);+ if (separationB > totalRadius)+ return;++ const b2PolygonShape* poly1; // reference polygon+ const b2PolygonShape* poly2; // incident polygon+ b2Transform xf1, xf2;+ int32 edge1; // reference edge+ uint8 flip;+ const float32 k_tol = 0.1f * b2_linearSlop;++ if (separationB > separationA + k_tol)+ {+ poly1 = polyB;+ poly2 = polyA;+ xf1 = xfB;+ xf2 = xfA;+ edge1 = edgeB;+ manifold->type = b2Manifold::e_faceB;+ flip = 1;+ }+ else+ {+ poly1 = polyA;+ poly2 = polyB;+ xf1 = xfA;+ xf2 = xfB;+ edge1 = edgeA;+ manifold->type = b2Manifold::e_faceA;+ flip = 0;+ }++ b2ClipVertex incidentEdge[2];+ b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);++ int32 count1 = poly1->m_count;+ const b2Vec2* vertices1 = poly1->m_vertices;++ int32 iv1 = edge1;+ int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;++ b2Vec2 v11 = vertices1[iv1];+ b2Vec2 v12 = vertices1[iv2];++ b2Vec2 localTangent = v12 - v11;+ localTangent.Normalize();++ b2Vec2 localNormal = b2Cross(localTangent, 1.0f);+ b2Vec2 planePoint = 0.5f * (v11 + v12);++ b2Vec2 tangent = b2Mul(xf1.q, localTangent);+ b2Vec2 normal = b2Cross(tangent, 1.0f);++ v11 = b2Mul(xf1, v11);+ v12 = b2Mul(xf1, v12);++ // Face offset.+ float32 frontOffset = b2Dot(normal, v11);++ // Side offsets, extended by polytope skin thickness.+ float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;+ float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;++ // Clip incident edge against extruded edge1 side edges.+ b2ClipVertex clipPoints1[2];+ b2ClipVertex clipPoints2[2];+ int np;++ // Clip to box side 1+ np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);++ if (np < 2)+ return;++ // Clip to negative box side 1+ np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);++ if (np < 2)+ {+ return;+ }++ // Now clipPoints2 contains the clipped points.+ manifold->localNormal = localNormal;+ manifold->localPoint = planePoint;++ int32 pointCount = 0;+ for (int32 i = 0; i < b2_maxManifoldPoints; ++i)+ {+ float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;++ if (separation <= totalRadius)+ {+ b2ManifoldPoint* cp = manifold->points + pointCount;+ cp->localPoint = b2MulT(xf2, clipPoints2[i].v);+ cp->id = clipPoints2[i].id;+ if (flip)+ {+ // Swap features+ b2ContactFeature cf = cp->id.cf;+ cp->id.cf.indexA = cf.indexB;+ cp->id.cf.indexB = cf.indexA;+ cp->id.cf.typeA = cf.typeB;+ cp->id.cf.typeB = cf.typeA;+ }+ ++pointCount;+ }+ }++ manifold->pointCount = pointCount;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2Collision.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2Collision.cppnew file mode 100644index 00000000..bd3e78b8--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2Collision.cpp@@ -0,0 +1,252 @@+/*+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/b2Distance.h>++void b2WorldManifold::Initialize(const b2Manifold* manifold,+ const b2Transform& xfA, float32 radiusA,+ const b2Transform& xfB, float32 radiusB)+{+ if (manifold->pointCount == 0)+ {+ return;+ }++ switch (manifold->type)+ {+ case b2Manifold::e_circles:+ {+ normal.Set(1.0f, 0.0f);+ b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);+ b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);+ if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)+ {+ normal = pointB - pointA;+ normal.Normalize();+ }++ b2Vec2 cA = pointA + radiusA * normal;+ b2Vec2 cB = pointB - radiusB * normal;+ points[0] = 0.5f * (cA + cB);+ separations[0] = b2Dot(cB - cA, normal);+ }+ break;++ case b2Manifold::e_faceA:+ {+ normal = b2Mul(xfA.q, manifold->localNormal);+ b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);++ for (int32 i = 0; i < manifold->pointCount; ++i)+ {+ b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);+ b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;+ b2Vec2 cB = clipPoint - radiusB * normal;+ points[i] = 0.5f * (cA + cB);+ separations[i] = b2Dot(cB - cA, normal);+ }+ }+ break;++ case b2Manifold::e_faceB:+ {+ normal = b2Mul(xfB.q, manifold->localNormal);+ b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);++ for (int32 i = 0; i < manifold->pointCount; ++i)+ {+ b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);+ b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;+ b2Vec2 cA = clipPoint - radiusA * normal;+ points[i] = 0.5f * (cA + cB);+ separations[i] = b2Dot(cA - cB, normal);+ }++ // Ensure normal points from A to B.+ normal = -normal;+ }+ break;+ }+}++void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],+ const b2Manifold* manifold1, const b2Manifold* manifold2)+{+ for (int32 i = 0; i < b2_maxManifoldPoints; ++i)+ {+ state1[i] = b2_nullState;+ state2[i] = b2_nullState;+ }++ // Detect persists and removes.+ for (int32 i = 0; i < manifold1->pointCount; ++i)+ {+ b2ContactID id = manifold1->points[i].id;++ state1[i] = b2_removeState;++ for (int32 j = 0; j < manifold2->pointCount; ++j)+ {+ if (manifold2->points[j].id.key == id.key)+ {+ state1[i] = b2_persistState;+ break;+ }+ }+ }++ // Detect persists and adds.+ for (int32 i = 0; i < manifold2->pointCount; ++i)+ {+ b2ContactID id = manifold2->points[i].id;++ state2[i] = b2_addState;++ for (int32 j = 0; j < manifold1->pointCount; ++j)+ {+ if (manifold1->points[j].id.key == id.key)+ {+ state2[i] = b2_persistState;+ break;+ }+ }+ }+}++// From Real-time Collision Detection, p179.+bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const+{+ float32 tmin = -b2_maxFloat;+ float32 tmax = b2_maxFloat;++ b2Vec2 p = input.p1;+ b2Vec2 d = input.p2 - input.p1;+ b2Vec2 absD = b2Abs(d);++ b2Vec2 normal;++ for (int32 i = 0; i < 2; ++i)+ {+ if (absD(i) < b2_epsilon)+ {+ // Parallel.+ if (p(i) < lowerBound(i) || upperBound(i) < p(i))+ {+ return false;+ }+ }+ else+ {+ float32 inv_d = 1.0f / d(i);+ float32 t1 = (lowerBound(i) - p(i)) * inv_d;+ float32 t2 = (upperBound(i) - p(i)) * inv_d;++ // Sign of the normal vector.+ float32 s = -1.0f;++ if (t1 > t2)+ {+ b2Swap(t1, t2);+ s = 1.0f;+ }++ // Push the min up+ if (t1 > tmin)+ {+ normal.SetZero();+ normal(i) = s;+ tmin = t1;+ }++ // Pull the max down+ tmax = b2Min(tmax, t2);++ if (tmin > tmax)+ {+ return false;+ }+ }+ }++ // Does the ray start inside the box?+ // Does the ray intersect beyond the max fraction?+ if (tmin < 0.0f || input.maxFraction < tmin)+ {+ return false;+ }++ // Intersection.+ output->fraction = tmin;+ output->normal = normal;+ return true;+}++// Sutherland-Hodgman clipping.+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],+ const b2Vec2& normal, float32 offset, int32 vertexIndexA)+{+ // Start with no output points+ int32 numOut = 0;++ // Calculate the distance of end points to the line+ float32 distance0 = b2Dot(normal, vIn[0].v) - offset;+ float32 distance1 = b2Dot(normal, vIn[1].v) - offset;++ // If the points are behind the plane+ if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];+ if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];++ // If the points are on different sides of the plane+ if (distance0 * distance1 < 0.0f)+ {+ // Find intersection point of edge and plane+ float32 interp = distance0 / (distance0 - distance1);+ vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);++ // VertexA is hitting edgeB.+ vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);+ vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;+ vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;+ vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;+ ++numOut;+ }++ return numOut;+}++bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,+ const b2Shape* shapeB, int32 indexB,+ const b2Transform& xfA, const b2Transform& xfB)+{+ b2DistanceInput input;+ input.proxyA.Set(shapeA, indexA);+ input.proxyB.Set(shapeB, indexB);+ input.transformA = xfA;+ input.transformB = xfB;+ input.useRadii = true;++ b2SimplexCache cache;+ cache.count = 0;++ b2DistanceOutput output;++ b2Distance(&output, &cache, &input);++ return output.distance < 10.0f * b2_epsilon;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2Collision.h b/games/mrbullet/src/box2d/Box2D/Collision/b2Collision.hnew file mode 100644index 00000000..8b0ed66a--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2Collision.h@@ -0,0 +1,277 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_COLLISION_H+#define B2_COLLISION_H++#include <Box2D/Common/b2Math.h>+#include <limits.h>++/// @file+/// Structures and functions used for computing contact points, distance+/// queries, and TOI queries.++class b2Shape;+class b2CircleShape;+class b2EdgeShape;+class b2PolygonShape;++const uint8 b2_nullFeature = UCHAR_MAX;++/// The features that intersect to form the contact point+/// This must be 4 bytes or less.+struct b2ContactFeature+{+ enum Type+ {+ e_vertex = 0,+ e_face = 1+ };++ uint8 indexA; ///< Feature index on shapeA+ uint8 indexB; ///< Feature index on shapeB+ uint8 typeA; ///< The feature type on shapeA+ uint8 typeB; ///< The feature type on shapeB+};++/// Contact ids to facilitate warm starting.+union b2ContactID+{+ b2ContactFeature cf;+ uint32 key; ///< Used to quickly compare contact ids.+};++/// A manifold point is a contact point belonging to a contact+/// manifold. It holds details related to the geometry and dynamics+/// of the contact points.+/// The local point usage depends on the manifold type:+/// -e_circles: the local center of circleB+/// -e_faceA: the local center of cirlceB or the clip point of polygonB+/// -e_faceB: the clip point of polygonA+/// This structure is stored across time steps, so we keep it small.+/// Note: the impulses are used for internal caching and may not+/// provide reliable contact forces, especially for high speed collisions.+struct b2ManifoldPoint+{+ b2Vec2 localPoint; ///< usage depends on manifold type+ float32 normalImpulse; ///< the non-penetration impulse+ float32 tangentImpulse; ///< the friction impulse+ b2ContactID id; ///< uniquely identifies a contact point between two shapes+};++/// A manifold for two touching convex shapes.+/// Box2D supports multiple types of contact:+/// - clip point versus plane with radius+/// - point versus point with radius (circles)+/// The local point usage depends on the manifold type:+/// -e_circles: the local center of circleA+/// -e_faceA: the center of faceA+/// -e_faceB: the center of faceB+/// Similarly the local normal usage:+/// -e_circles: not used+/// -e_faceA: the normal on polygonA+/// -e_faceB: the normal on polygonB+/// We store contacts in this way so that position correction can+/// account for movement, which is critical for continuous physics.+/// All contact scenarios must be expressed in one of these types.+/// This structure is stored across time steps, so we keep it small.+struct b2Manifold+{+ enum Type+ {+ e_circles,+ e_faceA,+ e_faceB+ };++ b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact+ b2Vec2 localNormal; ///< not use for Type::e_points+ b2Vec2 localPoint; ///< usage depends on manifold type+ Type type;+ int32 pointCount; ///< the number of manifold points+};++/// This is used to compute the current state of a contact manifold.+struct b2WorldManifold+{+ /// Evaluate the manifold with supplied transforms. This assumes+ /// modest motion from the original state. This does not change the+ /// point count, impulses, etc. The radii must come from the shapes+ /// that generated the manifold.+ void Initialize(const b2Manifold* manifold,+ const b2Transform& xfA, float32 radiusA,+ const b2Transform& xfB, float32 radiusB);++ b2Vec2 normal; ///< world vector pointing from A to B+ b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection)+ float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters+};++/// This is used for determining the state of contact points.+enum b2PointState+{+ b2_nullState, ///< point does not exist+ b2_addState, ///< point was added in the update+ b2_persistState, ///< point persisted across the update+ b2_removeState ///< point was removed in the update+};++/// Compute the point states given two manifolds. The states pertain to the transition from manifold1+/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.+void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],+ const b2Manifold* manifold1, const b2Manifold* manifold2);++/// Used for computing contact manifolds.+struct b2ClipVertex+{+ b2Vec2 v;+ b2ContactID id;+};++/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).+struct b2RayCastInput+{+ b2Vec2 p1, p2;+ float32 maxFraction;+};++/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2+/// come from b2RayCastInput.+struct b2RayCastOutput+{+ b2Vec2 normal;+ float32 fraction;+};++/// An axis aligned bounding box.+struct b2AABB+{+ /// Verify that the bounds are sorted.+ bool IsValid() const;++ /// Get the center of the AABB.+ b2Vec2 GetCenter() const+ {+ return 0.5f * (lowerBound + upperBound);+ }++ /// Get the extents of the AABB (half-widths).+ b2Vec2 GetExtents() const+ {+ return 0.5f * (upperBound - lowerBound);+ }++ /// Get the perimeter length+ float32 GetPerimeter() const+ {+ float32 wx = upperBound.x - lowerBound.x;+ float32 wy = upperBound.y - lowerBound.y;+ return 2.0f * (wx + wy);+ }++ /// Combine an AABB into this one.+ void Combine(const b2AABB& aabb)+ {+ lowerBound = b2Min(lowerBound, aabb.lowerBound);+ upperBound = b2Max(upperBound, aabb.upperBound);+ }++ /// Combine two AABBs into this one.+ void Combine(const b2AABB& aabb1, const b2AABB& aabb2)+ {+ lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);+ upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);+ }++ /// Does this aabb contain the provided AABB.+ bool Contains(const b2AABB& aabb) const+ {+ bool result = true;+ result = result && lowerBound.x <= aabb.lowerBound.x;+ result = result && lowerBound.y <= aabb.lowerBound.y;+ result = result && aabb.upperBound.x <= upperBound.x;+ result = result && aabb.upperBound.y <= upperBound.y;+ return result;+ }++ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;++ b2Vec2 lowerBound; ///< the lower vertex+ b2Vec2 upperBound; ///< the upper vertex+};++/// Compute the collision manifold between two circles.+void b2CollideCircles(b2Manifold* manifold,+ const b2CircleShape* circleA, const b2Transform& xfA,+ const b2CircleShape* circleB, const b2Transform& xfB);++/// Compute the collision manifold between a polygon and a circle.+void b2CollidePolygonAndCircle(b2Manifold* manifold,+ const b2PolygonShape* polygonA, const b2Transform& xfA,+ const b2CircleShape* circleB, const b2Transform& xfB);++/// Compute the collision manifold between two polygons.+void b2CollidePolygons(b2Manifold* manifold,+ const b2PolygonShape* polygonA, const b2Transform& xfA,+ const b2PolygonShape* polygonB, const b2Transform& xfB);++/// Compute the collision manifold between an edge and a circle.+void b2CollideEdgeAndCircle(b2Manifold* manifold,+ const b2EdgeShape* polygonA, const b2Transform& xfA,+ const b2CircleShape* circleB, const b2Transform& xfB);++/// Compute the collision manifold between an edge and a circle.+void b2CollideEdgeAndPolygon(b2Manifold* manifold,+ const b2EdgeShape* edgeA, const b2Transform& xfA,+ const b2PolygonShape* circleB, const b2Transform& xfB);++/// Clipping for contact manifolds.+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],+ const b2Vec2& normal, float32 offset, int32 vertexIndexA);++/// Determine if two generic shapes overlap.+bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,+ const b2Shape* shapeB, int32 indexB,+ const b2Transform& xfA, const b2Transform& xfB);++// ---------------- Inline Functions ------------------------------------------++inline bool b2AABB::IsValid() const+{+ b2Vec2 d = upperBound - lowerBound;+ bool valid = d.x >= 0.0f && d.y >= 0.0f;+ valid = valid && lowerBound.IsValid() && upperBound.IsValid();+ return valid;+}++inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b)+{+ b2Vec2 d1, d2;+ d1 = b.lowerBound - a.upperBound;+ d2 = a.lowerBound - b.upperBound;++ if (d1.x > 0.0f || d1.y > 0.0f)+ return false;++ if (d2.x > 0.0f || d2.y > 0.0f)+ return false;++ return true;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.cppnew file mode 100644index 00000000..faa4c87a--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.cpp@@ -0,0 +1,603 @@+/*+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2Distance.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>+#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>++// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.+int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;++void b2DistanceProxy::Set(const b2Shape* shape, int32 index)+{+ switch (shape->GetType())+ {+ case b2Shape::e_circle:+ {+ const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);+ m_vertices = &circle->m_p;+ m_count = 1;+ m_radius = circle->m_radius;+ }+ break;++ case b2Shape::e_polygon:+ {+ const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);+ m_vertices = polygon->m_vertices;+ m_count = polygon->m_count;+ m_radius = polygon->m_radius;+ }+ break;++ case b2Shape::e_chain:+ {+ const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);+ b2Assert(0 <= index && index < chain->m_count);++ m_buffer[0] = chain->m_vertices[index];+ if (index + 1 < chain->m_count)+ {+ m_buffer[1] = chain->m_vertices[index + 1];+ }+ else+ {+ m_buffer[1] = chain->m_vertices[0];+ }++ m_vertices = m_buffer;+ m_count = 2;+ m_radius = chain->m_radius;+ }+ break;++ case b2Shape::e_edge:+ {+ const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);+ m_vertices = &edge->m_vertex1;+ m_count = 2;+ m_radius = edge->m_radius;+ }+ break;++ default:+ b2Assert(false);+ }+}+++struct b2SimplexVertex+{+ b2Vec2 wA; // support point in proxyA+ b2Vec2 wB; // support point in proxyB+ b2Vec2 w; // wB - wA+ float32 a; // barycentric coordinate for closest point+ int32 indexA; // wA index+ int32 indexB; // wB index+};++struct b2Simplex+{+ void ReadCache( const b2SimplexCache* cache,+ const b2DistanceProxy* proxyA, const b2Transform& transformA,+ const b2DistanceProxy* proxyB, const b2Transform& transformB)+ {+ b2Assert(cache->count <= 3);++ // Copy data from cache.+ m_count = cache->count;+ b2SimplexVertex* vertices = &m_v1;+ for (int32 i = 0; i < m_count; ++i)+ {+ b2SimplexVertex* v = vertices + i;+ v->indexA = cache->indexA[i];+ v->indexB = cache->indexB[i];+ b2Vec2 wALocal = proxyA->GetVertex(v->indexA);+ b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);+ v->wA = b2Mul(transformA, wALocal);+ v->wB = b2Mul(transformB, wBLocal);+ v->w = v->wB - v->wA;+ v->a = 0.0f;+ }++ // Compute the new simplex metric, if it is substantially different than+ // old metric then flush the simplex.+ if (m_count > 1)+ {+ float32 metric1 = cache->metric;+ float32 metric2 = GetMetric();+ if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)+ {+ // Reset the simplex.+ m_count = 0;+ }+ }++ // If the cache is empty or invalid ...+ if (m_count == 0)+ {+ b2SimplexVertex* v = vertices + 0;+ v->indexA = 0;+ v->indexB = 0;+ b2Vec2 wALocal = proxyA->GetVertex(0);+ b2Vec2 wBLocal = proxyB->GetVertex(0);+ v->wA = b2Mul(transformA, wALocal);+ v->wB = b2Mul(transformB, wBLocal);+ v->w = v->wB - v->wA;+ v->a = 1.0f;+ m_count = 1;+ }+ }++ void WriteCache(b2SimplexCache* cache) const+ {+ cache->metric = GetMetric();+ cache->count = uint16(m_count);+ const b2SimplexVertex* vertices = &m_v1;+ for (int32 i = 0; i < m_count; ++i)+ {+ cache->indexA[i] = uint8(vertices[i].indexA);+ cache->indexB[i] = uint8(vertices[i].indexB);+ }+ }++ b2Vec2 GetSearchDirection() const+ {+ switch (m_count)+ {+ case 1:+ return -m_v1.w;++ case 2:+ {+ b2Vec2 e12 = m_v2.w - m_v1.w;+ float32 sgn = b2Cross(e12, -m_v1.w);+ if (sgn > 0.0f)+ {+ // Origin is left of e12.+ return b2Cross(1.0f, e12);+ }+ else+ {+ // Origin is right of e12.+ return b2Cross(e12, 1.0f);+ }+ }++ default:+ b2Assert(false);+ return b2Vec2_zero;+ }+ }++ b2Vec2 GetClosestPoint() const+ {+ switch (m_count)+ {+ case 0:+ b2Assert(false);+ return b2Vec2_zero;++ case 1:+ return m_v1.w;++ case 2:+ return m_v1.a * m_v1.w + m_v2.a * m_v2.w;++ case 3:+ return b2Vec2_zero;++ default:+ b2Assert(false);+ return b2Vec2_zero;+ }+ }++ void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const+ {+ switch (m_count)+ {+ case 0:+ b2Assert(false);+ break;++ case 1:+ *pA = m_v1.wA;+ *pB = m_v1.wB;+ break;++ case 2:+ *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;+ *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;+ break;++ case 3:+ *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;+ *pB = *pA;+ break;++ default:+ b2Assert(false);+ break;+ }+ }++ float32 GetMetric() const+ {+ switch (m_count)+ {+ case 0:+ b2Assert(false);+ return 0.0f;++ case 1:+ return 0.0f;++ case 2:+ return b2Distance(m_v1.w, m_v2.w);++ case 3:+ return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);++ default:+ b2Assert(false);+ return 0.0f;+ }+ }++ void Solve2();+ void Solve3();++ b2SimplexVertex m_v1, m_v2, m_v3;+ int32 m_count;+};+++// Solve a line segment using barycentric coordinates.+//+// p = a1 * w1 + a2 * w2+// a1 + a2 = 1+//+// The vector from the origin to the closest point on the line is+// perpendicular to the line.+// e12 = w2 - w1+// dot(p, e) = 0+// a1 * dot(w1, e) + a2 * dot(w2, e) = 0+//+// 2-by-2 linear system+// [1 1 ][a1] = [1]+// [w1.e12 w2.e12][a2] = [0]+//+// Define+// d12_1 = dot(w2, e12)+// d12_2 = -dot(w1, e12)+// d12 = d12_1 + d12_2+//+// Solution+// a1 = d12_1 / d12+// a2 = d12_2 / d12+void b2Simplex::Solve2()+{+ b2Vec2 w1 = m_v1.w;+ b2Vec2 w2 = m_v2.w;+ b2Vec2 e12 = w2 - w1;++ // w1 region+ float32 d12_2 = -b2Dot(w1, e12);+ if (d12_2 <= 0.0f)+ {+ // a2 <= 0, so we clamp it to 0+ m_v1.a = 1.0f;+ m_count = 1;+ return;+ }++ // w2 region+ float32 d12_1 = b2Dot(w2, e12);+ if (d12_1 <= 0.0f)+ {+ // a1 <= 0, so we clamp it to 0+ m_v2.a = 1.0f;+ m_count = 1;+ m_v1 = m_v2;+ return;+ }++ // Must be in e12 region.+ float32 inv_d12 = 1.0f / (d12_1 + d12_2);+ m_v1.a = d12_1 * inv_d12;+ m_v2.a = d12_2 * inv_d12;+ m_count = 2;+}++// Possible regions:+// - points[2]+// - edge points[0]-points[2]+// - edge points[1]-points[2]+// - inside the triangle+void b2Simplex::Solve3()+{+ b2Vec2 w1 = m_v1.w;+ b2Vec2 w2 = m_v2.w;+ b2Vec2 w3 = m_v3.w;++ // Edge12+ // [1 1 ][a1] = [1]+ // [w1.e12 w2.e12][a2] = [0]+ // a3 = 0+ b2Vec2 e12 = w2 - w1;+ float32 w1e12 = b2Dot(w1, e12);+ float32 w2e12 = b2Dot(w2, e12);+ float32 d12_1 = w2e12;+ float32 d12_2 = -w1e12;++ // Edge13+ // [1 1 ][a1] = [1]+ // [w1.e13 w3.e13][a3] = [0]+ // a2 = 0+ b2Vec2 e13 = w3 - w1;+ float32 w1e13 = b2Dot(w1, e13);+ float32 w3e13 = b2Dot(w3, e13);+ float32 d13_1 = w3e13;+ float32 d13_2 = -w1e13;++ // Edge23+ // [1 1 ][a2] = [1]+ // [w2.e23 w3.e23][a3] = [0]+ // a1 = 0+ b2Vec2 e23 = w3 - w2;+ float32 w2e23 = b2Dot(w2, e23);+ float32 w3e23 = b2Dot(w3, e23);+ float32 d23_1 = w3e23;+ float32 d23_2 = -w2e23;++ // Triangle123+ float32 n123 = b2Cross(e12, e13);++ float32 d123_1 = n123 * b2Cross(w2, w3);+ float32 d123_2 = n123 * b2Cross(w3, w1);+ float32 d123_3 = n123 * b2Cross(w1, w2);++ // w1 region+ if (d12_2 <= 0.0f && d13_2 <= 0.0f)+ {+ m_v1.a = 1.0f;+ m_count = 1;+ return;+ }++ // e12+ if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)+ {+ float32 inv_d12 = 1.0f / (d12_1 + d12_2);+ m_v1.a = d12_1 * inv_d12;+ m_v2.a = d12_2 * inv_d12;+ m_count = 2;+ return;+ }++ // e13+ if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)+ {+ float32 inv_d13 = 1.0f / (d13_1 + d13_2);+ m_v1.a = d13_1 * inv_d13;+ m_v3.a = d13_2 * inv_d13;+ m_count = 2;+ m_v2 = m_v3;+ return;+ }++ // w2 region+ if (d12_1 <= 0.0f && d23_2 <= 0.0f)+ {+ m_v2.a = 1.0f;+ m_count = 1;+ m_v1 = m_v2;+ return;+ }++ // w3 region+ if (d13_1 <= 0.0f && d23_1 <= 0.0f)+ {+ m_v3.a = 1.0f;+ m_count = 1;+ m_v1 = m_v3;+ return;+ }++ // e23+ if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)+ {+ float32 inv_d23 = 1.0f / (d23_1 + d23_2);+ m_v2.a = d23_1 * inv_d23;+ m_v3.a = d23_2 * inv_d23;+ m_count = 2;+ m_v1 = m_v3;+ return;+ }++ // Must be in triangle123+ float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);+ m_v1.a = d123_1 * inv_d123;+ m_v2.a = d123_2 * inv_d123;+ m_v3.a = d123_3 * inv_d123;+ m_count = 3;+}++void b2Distance(b2DistanceOutput* output,+ b2SimplexCache* cache,+ const b2DistanceInput* input)+{+ ++b2_gjkCalls;++ const b2DistanceProxy* proxyA = &input->proxyA;+ const b2DistanceProxy* proxyB = &input->proxyB;++ b2Transform transformA = input->transformA;+ b2Transform transformB = input->transformB;++ // Initialize the simplex.+ b2Simplex simplex;+ simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);++ // Get simplex vertices as an array.+ b2SimplexVertex* vertices = &simplex.m_v1;+ const int32 k_maxIters = 20;++ // These store the vertices of the last simplex so that we+ // can check for duplicates and prevent cycling.+ int32 saveA[3], saveB[3];+ int32 saveCount = 0;++ float32 distanceSqr1 = b2_maxFloat;+ float32 distanceSqr2 = distanceSqr1;++ // Main iteration loop.+ int32 iter = 0;+ while (iter < k_maxIters)+ {+ // Copy simplex so we can identify duplicates.+ saveCount = simplex.m_count;+ for (int32 i = 0; i < saveCount; ++i)+ {+ saveA[i] = vertices[i].indexA;+ saveB[i] = vertices[i].indexB;+ }++ switch (simplex.m_count)+ {+ case 1:+ break;++ case 2:+ simplex.Solve2();+ break;++ case 3:+ simplex.Solve3();+ break;++ default:+ b2Assert(false);+ }++ // If we have 3 points, then the origin is in the corresponding triangle.+ if (simplex.m_count == 3)+ {+ break;+ }++ // Compute closest point.+ b2Vec2 p = simplex.GetClosestPoint();+ distanceSqr2 = p.LengthSquared();++ // Ensure progress+ if (distanceSqr2 >= distanceSqr1)+ {+ //break;+ }+ distanceSqr1 = distanceSqr2;++ // Get search direction.+ b2Vec2 d = simplex.GetSearchDirection();++ // Ensure the search direction is numerically fit.+ if (d.LengthSquared() < b2_epsilon * b2_epsilon)+ {+ // The origin is probably contained by a line segment+ // or triangle. Thus the shapes are overlapped.++ // We can't return zero here even though there may be overlap.+ // In case the simplex is a point, segment, or triangle it is difficult+ // to determine if the origin is contained in the CSO or very close to it.+ break;+ }++ // Compute a tentative new simplex vertex using support points.+ b2SimplexVertex* vertex = vertices + simplex.m_count;+ vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));+ vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));+ b2Vec2 wBLocal;+ vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));+ vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));+ vertex->w = vertex->wB - vertex->wA;++ // Iteration count is equated to the number of support point calls.+ ++iter;+ ++b2_gjkIters;++ // Check for duplicate support points. This is the main termination criteria.+ bool duplicate = false;+ for (int32 i = 0; i < saveCount; ++i)+ {+ if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])+ {+ duplicate = true;+ break;+ }+ }++ // If we found a duplicate support point we must exit to avoid cycling.+ if (duplicate)+ {+ break;+ }++ // New vertex is ok and needed.+ ++simplex.m_count;+ }++ b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);++ // Prepare output.+ simplex.GetWitnessPoints(&output->pointA, &output->pointB);+ output->distance = b2Distance(output->pointA, output->pointB);+ output->iterations = iter;++ // Cache the simplex.+ simplex.WriteCache(cache);++ // Apply radii if requested.+ if (input->useRadii)+ {+ float32 rA = proxyA->m_radius;+ float32 rB = proxyB->m_radius;++ if (output->distance > rA + rB && output->distance > b2_epsilon)+ {+ // Shapes are still no overlapped.+ // Move the witness points to the outer surface.+ output->distance -= rA + rB;+ b2Vec2 normal = output->pointB - output->pointA;+ normal.Normalize();+ output->pointA += rA * normal;+ output->pointB -= rB * normal;+ }+ else+ {+ // Shapes are overlapped when radii are considered.+ // Move the witness points to the middle.+ b2Vec2 p = 0.5f * (output->pointA + output->pointB);+ output->pointA = p;+ output->pointB = p;+ output->distance = 0.0f;+ }+ }+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.h b/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.hnew file mode 100644index 00000000..54ed1e13--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.h@@ -0,0 +1,141 @@++/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_DISTANCE_H+#define B2_DISTANCE_H++#include <Box2D/Common/b2Math.h>++class b2Shape;++/// A distance proxy is used by the GJK algorithm.+/// It encapsulates any shape.+struct b2DistanceProxy+{+ b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {}++ /// Initialize the proxy using the given shape. The shape+ /// must remain in scope while the proxy is in use.+ void Set(const b2Shape* shape, int32 index);++ /// Get the supporting vertex index in the given direction.+ int32 GetSupport(const b2Vec2& d) const;++ /// Get the supporting vertex in the given direction.+ const b2Vec2& GetSupportVertex(const b2Vec2& d) const;++ /// Get the vertex count.+ int32 GetVertexCount() const;++ /// Get a vertex by index. Used by b2Distance.+ const b2Vec2& GetVertex(int32 index) const;++ b2Vec2 m_buffer[2];+ const b2Vec2* m_vertices;+ int32 m_count;+ float32 m_radius;+};++/// Used to warm start b2Distance.+/// Set count to zero on first call.+struct b2SimplexCache+{+ float32 metric; ///< length or area+ uint16 count;+ uint8 indexA[3]; ///< vertices on shape A+ uint8 indexB[3]; ///< vertices on shape B+};++/// Input for b2Distance.+/// You have to option to use the shape radii+/// in the computation. Even+struct b2DistanceInput+{+ b2DistanceProxy proxyA;+ b2DistanceProxy proxyB;+ b2Transform transformA;+ b2Transform transformB;+ bool useRadii;+};++/// Output for b2Distance.+struct b2DistanceOutput+{+ b2Vec2 pointA; ///< closest point on shapeA+ b2Vec2 pointB; ///< closest point on shapeB+ float32 distance;+ int32 iterations; ///< number of GJK iterations used+};++/// Compute the closest points between two shapes. Supports any combination of:+/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.+/// On the first call set b2SimplexCache.count to zero.+void b2Distance(b2DistanceOutput* output,+ b2SimplexCache* cache,+ const b2DistanceInput* input);+++//////////////////////////////////////////////////////////////////////////++inline int32 b2DistanceProxy::GetVertexCount() const+{+ return m_count;+}++inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const+{+ b2Assert(0 <= index && index < m_count);+ return m_vertices[index];+}++inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const+{+ int32 bestIndex = 0;+ float32 bestValue = b2Dot(m_vertices[0], d);+ for (int32 i = 1; i < m_count; ++i)+ {+ float32 value = b2Dot(m_vertices[i], d);+ if (value > bestValue)+ {+ bestIndex = i;+ bestValue = value;+ }+ }++ return bestIndex;+}++inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const+{+ int32 bestIndex = 0;+ float32 bestValue = b2Dot(m_vertices[0], d);+ for (int32 i = 1; i < m_count; ++i)+ {+ float32 value = b2Dot(m_vertices[i], d);+ if (value > bestValue)+ {+ bestIndex = i;+ bestValue = value;+ }+ }++ return m_vertices[bestIndex];+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.cppnew file mode 100644index 00000000..cc41b285--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.cpp@@ -0,0 +1,778 @@+/*+* Copyright (c) 2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2DynamicTree.h>+#include <string.h>++b2DynamicTree::b2DynamicTree()+{+ m_root = b2_nullNode;++ m_nodeCapacity = 16;+ m_nodeCount = 0;+ m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));+ memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));++ // Build a linked list for the free list.+ for (int32 i = 0; i < m_nodeCapacity - 1; ++i)+ {+ m_nodes[i].next = i + 1;+ m_nodes[i].height = -1;+ }+ m_nodes[m_nodeCapacity-1].next = b2_nullNode;+ m_nodes[m_nodeCapacity-1].height = -1;+ m_freeList = 0;++ m_path = 0;++ m_insertionCount = 0;+}++b2DynamicTree::~b2DynamicTree()+{+ // This frees the entire tree in one shot.+ b2Free(m_nodes);+}++// Allocate a node from the pool. Grow the pool if necessary.+int32 b2DynamicTree::AllocateNode()+{+ // Expand the node pool as needed.+ if (m_freeList == b2_nullNode)+ {+ b2Assert(m_nodeCount == m_nodeCapacity);++ // The free list is empty. Rebuild a bigger pool.+ b2TreeNode* oldNodes = m_nodes;+ m_nodeCapacity *= 2;+ m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));+ memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));+ b2Free(oldNodes);++ // Build a linked list for the free list. The parent+ // pointer becomes the "next" pointer.+ for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)+ {+ m_nodes[i].next = i + 1;+ m_nodes[i].height = -1;+ }+ m_nodes[m_nodeCapacity-1].next = b2_nullNode;+ m_nodes[m_nodeCapacity-1].height = -1;+ m_freeList = m_nodeCount;+ }++ // Peel a node off the free list.+ int32 nodeId = m_freeList;+ m_freeList = m_nodes[nodeId].next;+ m_nodes[nodeId].parent = b2_nullNode;+ m_nodes[nodeId].child1 = b2_nullNode;+ m_nodes[nodeId].child2 = b2_nullNode;+ m_nodes[nodeId].height = 0;+ m_nodes[nodeId].userData = NULL;+ ++m_nodeCount;+ return nodeId;+}++// Return a node to the pool.+void b2DynamicTree::FreeNode(int32 nodeId)+{+ b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);+ b2Assert(0 < m_nodeCount);+ m_nodes[nodeId].next = m_freeList;+ m_nodes[nodeId].height = -1;+ m_freeList = nodeId;+ --m_nodeCount;+}++// Create a proxy in the tree as a leaf node. We return the index+// of the node instead of a pointer so that we can grow+// the node pool.+int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)+{+ int32 proxyId = AllocateNode();++ // Fatten the aabb.+ b2Vec2 r(b2_aabbExtension, b2_aabbExtension);+ m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;+ m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;+ m_nodes[proxyId].userData = userData;+ m_nodes[proxyId].height = 0;++ InsertLeaf(proxyId);++ return proxyId;+}++void b2DynamicTree::DestroyProxy(int32 proxyId)+{+ b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);+ b2Assert(m_nodes[proxyId].IsLeaf());++ RemoveLeaf(proxyId);+ FreeNode(proxyId);+}++bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)+{+ b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);++ b2Assert(m_nodes[proxyId].IsLeaf());++ if (m_nodes[proxyId].aabb.Contains(aabb))+ {+ return false;+ }++ RemoveLeaf(proxyId);++ // Extend AABB.+ b2AABB b = aabb;+ b2Vec2 r(b2_aabbExtension, b2_aabbExtension);+ b.lowerBound = b.lowerBound - r;+ b.upperBound = b.upperBound + r;++ // Predict AABB displacement.+ b2Vec2 d = b2_aabbMultiplier * displacement;++ if (d.x < 0.0f)+ {+ b.lowerBound.x += d.x;+ }+ else+ {+ b.upperBound.x += d.x;+ }++ if (d.y < 0.0f)+ {+ b.lowerBound.y += d.y;+ }+ else+ {+ b.upperBound.y += d.y;+ }++ m_nodes[proxyId].aabb = b;++ InsertLeaf(proxyId);+ return true;+}++void b2DynamicTree::InsertLeaf(int32 leaf)+{+ ++m_insertionCount;++ if (m_root == b2_nullNode)+ {+ m_root = leaf;+ m_nodes[m_root].parent = b2_nullNode;+ return;+ }++ // Find the best sibling for this node+ b2AABB leafAABB = m_nodes[leaf].aabb;+ int32 index = m_root;+ while (m_nodes[index].IsLeaf() == false)+ {+ int32 child1 = m_nodes[index].child1;+ int32 child2 = m_nodes[index].child2;++ float32 area = m_nodes[index].aabb.GetPerimeter();++ b2AABB combinedAABB;+ combinedAABB.Combine(m_nodes[index].aabb, leafAABB);+ float32 combinedArea = combinedAABB.GetPerimeter();++ // Cost of creating a new parent for this node and the new leaf+ float32 cost = 2.0f * combinedArea;++ // Minimum cost of pushing the leaf further down the tree+ float32 inheritanceCost = 2.0f * (combinedArea - area);++ // Cost of descending into child1+ float32 cost1;+ if (m_nodes[child1].IsLeaf())+ {+ b2AABB aabb;+ aabb.Combine(leafAABB, m_nodes[child1].aabb);+ cost1 = aabb.GetPerimeter() + inheritanceCost;+ }+ else+ {+ b2AABB aabb;+ aabb.Combine(leafAABB, m_nodes[child1].aabb);+ float32 oldArea = m_nodes[child1].aabb.GetPerimeter();+ float32 newArea = aabb.GetPerimeter();+ cost1 = (newArea - oldArea) + inheritanceCost;+ }++ // Cost of descending into child2+ float32 cost2;+ if (m_nodes[child2].IsLeaf())+ {+ b2AABB aabb;+ aabb.Combine(leafAABB, m_nodes[child2].aabb);+ cost2 = aabb.GetPerimeter() + inheritanceCost;+ }+ else+ {+ b2AABB aabb;+ aabb.Combine(leafAABB, m_nodes[child2].aabb);+ float32 oldArea = m_nodes[child2].aabb.GetPerimeter();+ float32 newArea = aabb.GetPerimeter();+ cost2 = newArea - oldArea + inheritanceCost;+ }++ // Descend according to the minimum cost.+ if (cost < cost1 && cost < cost2)+ {+ break;+ }++ // Descend+ if (cost1 < cost2)+ {+ index = child1;+ }+ else+ {+ index = child2;+ }+ }++ int32 sibling = index;++ // Create a new parent.+ int32 oldParent = m_nodes[sibling].parent;+ int32 newParent = AllocateNode();+ m_nodes[newParent].parent = oldParent;+ m_nodes[newParent].userData = NULL;+ m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);+ m_nodes[newParent].height = m_nodes[sibling].height + 1;++ if (oldParent != b2_nullNode)+ {+ // The sibling was not the root.+ if (m_nodes[oldParent].child1 == sibling)+ {+ m_nodes[oldParent].child1 = newParent;+ }+ else+ {+ m_nodes[oldParent].child2 = newParent;+ }++ m_nodes[newParent].child1 = sibling;+ m_nodes[newParent].child2 = leaf;+ m_nodes[sibling].parent = newParent;+ m_nodes[leaf].parent = newParent;+ }+ else+ {+ // The sibling was the root.+ m_nodes[newParent].child1 = sibling;+ m_nodes[newParent].child2 = leaf;+ m_nodes[sibling].parent = newParent;+ m_nodes[leaf].parent = newParent;+ m_root = newParent;+ }++ // Walk back up the tree fixing heights and AABBs+ index = m_nodes[leaf].parent;+ while (index != b2_nullNode)+ {+ index = Balance(index);++ int32 child1 = m_nodes[index].child1;+ int32 child2 = m_nodes[index].child2;++ b2Assert(child1 != b2_nullNode);+ b2Assert(child2 != b2_nullNode);++ m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);+ m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);++ index = m_nodes[index].parent;+ }++ //Validate();+}++void b2DynamicTree::RemoveLeaf(int32 leaf)+{+ if (leaf == m_root)+ {+ m_root = b2_nullNode;+ return;+ }++ int32 parent = m_nodes[leaf].parent;+ int32 grandParent = m_nodes[parent].parent;+ int32 sibling;+ if (m_nodes[parent].child1 == leaf)+ {+ sibling = m_nodes[parent].child2;+ }+ else+ {+ sibling = m_nodes[parent].child1;+ }++ if (grandParent != b2_nullNode)+ {+ // Destroy parent and connect sibling to grandParent.+ if (m_nodes[grandParent].child1 == parent)+ {+ m_nodes[grandParent].child1 = sibling;+ }+ else+ {+ m_nodes[grandParent].child2 = sibling;+ }+ m_nodes[sibling].parent = grandParent;+ FreeNode(parent);++ // Adjust ancestor bounds.+ int32 index = grandParent;+ while (index != b2_nullNode)+ {+ index = Balance(index);++ int32 child1 = m_nodes[index].child1;+ int32 child2 = m_nodes[index].child2;++ m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);+ m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);++ index = m_nodes[index].parent;+ }+ }+ else+ {+ m_root = sibling;+ m_nodes[sibling].parent = b2_nullNode;+ FreeNode(parent);+ }++ //Validate();+}++// Perform a left or right rotation if node A is imbalanced.+// Returns the new root index.+int32 b2DynamicTree::Balance(int32 iA)+{+ b2Assert(iA != b2_nullNode);++ b2TreeNode* A = m_nodes + iA;+ if (A->IsLeaf() || A->height < 2)+ {+ return iA;+ }++ int32 iB = A->child1;+ int32 iC = A->child2;+ b2Assert(0 <= iB && iB < m_nodeCapacity);+ b2Assert(0 <= iC && iC < m_nodeCapacity);++ b2TreeNode* B = m_nodes + iB;+ b2TreeNode* C = m_nodes + iC;++ int32 balance = C->height - B->height;++ // Rotate C up+ if (balance > 1)+ {+ int32 iF = C->child1;+ int32 iG = C->child2;+ b2TreeNode* F = m_nodes + iF;+ b2TreeNode* G = m_nodes + iG;+ b2Assert(0 <= iF && iF < m_nodeCapacity);+ b2Assert(0 <= iG && iG < m_nodeCapacity);++ // Swap A and C+ C->child1 = iA;+ C->parent = A->parent;+ A->parent = iC;++ // A's old parent should point to C+ if (C->parent != b2_nullNode)+ {+ if (m_nodes[C->parent].child1 == iA)+ {+ m_nodes[C->parent].child1 = iC;+ }+ else+ {+ b2Assert(m_nodes[C->parent].child2 == iA);+ m_nodes[C->parent].child2 = iC;+ }+ }+ else+ {+ m_root = iC;+ }++ // Rotate+ if (F->height > G->height)+ {+ C->child2 = iF;+ A->child2 = iG;+ G->parent = iA;+ A->aabb.Combine(B->aabb, G->aabb);+ C->aabb.Combine(A->aabb, F->aabb);++ A->height = 1 + b2Max(B->height, G->height);+ C->height = 1 + b2Max(A->height, F->height);+ }+ else+ {+ C->child2 = iG;+ A->child2 = iF;+ F->parent = iA;+ A->aabb.Combine(B->aabb, F->aabb);+ C->aabb.Combine(A->aabb, G->aabb);++ A->height = 1 + b2Max(B->height, F->height);+ C->height = 1 + b2Max(A->height, G->height);+ }++ return iC;+ }++ // Rotate B up+ if (balance < -1)+ {+ int32 iD = B->child1;+ int32 iE = B->child2;+ b2TreeNode* D = m_nodes + iD;+ b2TreeNode* E = m_nodes + iE;+ b2Assert(0 <= iD && iD < m_nodeCapacity);+ b2Assert(0 <= iE && iE < m_nodeCapacity);++ // Swap A and B+ B->child1 = iA;+ B->parent = A->parent;+ A->parent = iB;++ // A's old parent should point to B+ if (B->parent != b2_nullNode)+ {+ if (m_nodes[B->parent].child1 == iA)+ {+ m_nodes[B->parent].child1 = iB;+ }+ else+ {+ b2Assert(m_nodes[B->parent].child2 == iA);+ m_nodes[B->parent].child2 = iB;+ }+ }+ else+ {+ m_root = iB;+ }++ // Rotate+ if (D->height > E->height)+ {+ B->child2 = iD;+ A->child1 = iE;+ E->parent = iA;+ A->aabb.Combine(C->aabb, E->aabb);+ B->aabb.Combine(A->aabb, D->aabb);++ A->height = 1 + b2Max(C->height, E->height);+ B->height = 1 + b2Max(A->height, D->height);+ }+ else+ {+ B->child2 = iE;+ A->child1 = iD;+ D->parent = iA;+ A->aabb.Combine(C->aabb, D->aabb);+ B->aabb.Combine(A->aabb, E->aabb);++ A->height = 1 + b2Max(C->height, D->height);+ B->height = 1 + b2Max(A->height, E->height);+ }++ return iB;+ }++ return iA;+}++int32 b2DynamicTree::GetHeight() const+{+ if (m_root == b2_nullNode)+ {+ return 0;+ }++ return m_nodes[m_root].height;+}++//+float32 b2DynamicTree::GetAreaRatio() const+{+ if (m_root == b2_nullNode)+ {+ return 0.0f;+ }++ const b2TreeNode* root = m_nodes + m_root;+ float32 rootArea = root->aabb.GetPerimeter();++ float32 totalArea = 0.0f;+ for (int32 i = 0; i < m_nodeCapacity; ++i)+ {+ const b2TreeNode* node = m_nodes + i;+ if (node->height < 0)+ {+ // Free node in pool+ continue;+ }++ totalArea += node->aabb.GetPerimeter();+ }++ return totalArea / rootArea;+}++// Compute the height of a sub-tree.+int32 b2DynamicTree::ComputeHeight(int32 nodeId) const+{+ b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);+ b2TreeNode* node = m_nodes + nodeId;++ if (node->IsLeaf())+ {+ return 0;+ }++ int32 height1 = ComputeHeight(node->child1);+ int32 height2 = ComputeHeight(node->child2);+ return 1 + b2Max(height1, height2);+}++int32 b2DynamicTree::ComputeHeight() const+{+ int32 height = ComputeHeight(m_root);+ return height;+}++void b2DynamicTree::ValidateStructure(int32 index) const+{+ if (index == b2_nullNode)+ {+ return;+ }++ if (index == m_root)+ {+ b2Assert(m_nodes[index].parent == b2_nullNode);+ }++ const b2TreeNode* node = m_nodes + index;++ int32 child1 = node->child1;+ int32 child2 = node->child2;++ if (node->IsLeaf())+ {+ b2Assert(child1 == b2_nullNode);+ b2Assert(child2 == b2_nullNode);+ b2Assert(node->height == 0);+ return;+ }++ b2Assert(0 <= child1 && child1 < m_nodeCapacity);+ b2Assert(0 <= child2 && child2 < m_nodeCapacity);++ b2Assert(m_nodes[child1].parent == index);+ b2Assert(m_nodes[child2].parent == index);++ ValidateStructure(child1);+ ValidateStructure(child2);+}++void b2DynamicTree::ValidateMetrics(int32 index) const+{+ if (index == b2_nullNode)+ {+ return;+ }++ const b2TreeNode* node = m_nodes + index;++ int32 child1 = node->child1;+ int32 child2 = node->child2;++ if (node->IsLeaf())+ {+ b2Assert(child1 == b2_nullNode);+ b2Assert(child2 == b2_nullNode);+ b2Assert(node->height == 0);+ return;+ }++ b2Assert(0 <= child1 && child1 < m_nodeCapacity);+ b2Assert(0 <= child2 && child2 < m_nodeCapacity);++ int32 height1 = m_nodes[child1].height;+ int32 height2 = m_nodes[child2].height;+ int32 height;+ height = 1 + b2Max(height1, height2);+ b2Assert(node->height == height);++ b2AABB aabb;+ aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);++ b2Assert(aabb.lowerBound == node->aabb.lowerBound);+ b2Assert(aabb.upperBound == node->aabb.upperBound);++ ValidateMetrics(child1);+ ValidateMetrics(child2);+}++void b2DynamicTree::Validate() const+{+ ValidateStructure(m_root);+ ValidateMetrics(m_root);++ int32 freeCount = 0;+ int32 freeIndex = m_freeList;+ while (freeIndex != b2_nullNode)+ {+ b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);+ freeIndex = m_nodes[freeIndex].next;+ ++freeCount;+ }++ b2Assert(GetHeight() == ComputeHeight());++ b2Assert(m_nodeCount + freeCount == m_nodeCapacity);+}++int32 b2DynamicTree::GetMaxBalance() const+{+ int32 maxBalance = 0;+ for (int32 i = 0; i < m_nodeCapacity; ++i)+ {+ const b2TreeNode* node = m_nodes + i;+ if (node->height <= 1)+ {+ continue;+ }++ b2Assert(node->IsLeaf() == false);++ int32 child1 = node->child1;+ int32 child2 = node->child2;+ int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);+ maxBalance = b2Max(maxBalance, balance);+ }++ return maxBalance;+}++void b2DynamicTree::RebuildBottomUp()+{+ int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));+ int32 count = 0;++ // Build array of leaves. Free the rest.+ for (int32 i = 0; i < m_nodeCapacity; ++i)+ {+ if (m_nodes[i].height < 0)+ {+ // free node in pool+ continue;+ }++ if (m_nodes[i].IsLeaf())+ {+ m_nodes[i].parent = b2_nullNode;+ nodes[count] = i;+ ++count;+ }+ else+ {+ FreeNode(i);+ }+ }++ while (count > 1)+ {+ float32 minCost = b2_maxFloat;+ int32 iMin = -1, jMin = -1;+ for (int32 i = 0; i < count; ++i)+ {+ b2AABB aabbi = m_nodes[nodes[i]].aabb;++ for (int32 j = i + 1; j < count; ++j)+ {+ b2AABB aabbj = m_nodes[nodes[j]].aabb;+ b2AABB b;+ b.Combine(aabbi, aabbj);+ float32 cost = b.GetPerimeter();+ if (cost < minCost)+ {+ iMin = i;+ jMin = j;+ minCost = cost;+ }+ }+ }++ int32 index1 = nodes[iMin];+ int32 index2 = nodes[jMin];+ b2TreeNode* child1 = m_nodes + index1;+ b2TreeNode* child2 = m_nodes + index2;++ int32 parentIndex = AllocateNode();+ b2TreeNode* parent = m_nodes + parentIndex;+ parent->child1 = index1;+ parent->child2 = index2;+ parent->height = 1 + b2Max(child1->height, child2->height);+ parent->aabb.Combine(child1->aabb, child2->aabb);+ parent->parent = b2_nullNode;++ child1->parent = parentIndex;+ child2->parent = parentIndex;++ nodes[jMin] = nodes[count-1];+ nodes[iMin] = parentIndex;+ --count;+ }++ m_root = nodes[0];+ b2Free(nodes);++ Validate();+}++void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)+{+ // Build array of leaves. Free the rest.+ for (int32 i = 0; i < m_nodeCapacity; ++i)+ {+ m_nodes[i].aabb.lowerBound -= newOrigin;+ m_nodes[i].aabb.upperBound -= newOrigin;+ }+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.h b/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.hnew file mode 100644index 00000000..97aa461b--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.h@@ -0,0 +1,289 @@+/*+* Copyright (c) 2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_DYNAMIC_TREE_H+#define B2_DYNAMIC_TREE_H++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Common/b2GrowableStack.h>++#define b2_nullNode (-1)++/// A node in the dynamic tree. The client does not interact with this directly.+struct b2TreeNode+{+ bool IsLeaf() const+ {+ return child1 == b2_nullNode;+ }++ /// Enlarged AABB+ b2AABB aabb;++ void* userData;++ union+ {+ int32 parent;+ int32 next;+ };++ int32 child1;+ int32 child2;++ // leaf = 0, free node = -1+ int32 height;+};++/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.+/// A dynamic tree arranges data in a binary tree to accelerate+/// queries such as volume queries and ray casts. Leafs are proxies+/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor+/// so that the proxy AABB is bigger than the client object. This allows the client+/// object to move by small amounts without triggering a tree update.+///+/// Nodes are pooled and relocatable, so we use node indices rather than pointers.+class b2DynamicTree+{+public:+ /// Constructing the tree initializes the node pool.+ b2DynamicTree();++ /// Destroy the tree, freeing the node pool.+ ~b2DynamicTree();++ /// Create a proxy. Provide a tight fitting AABB and a userData pointer.+ int32 CreateProxy(const b2AABB& aabb, void* userData);++ /// Destroy a proxy. This asserts if the id is invalid.+ void DestroyProxy(int32 proxyId);++ /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,+ /// then the proxy is removed from the tree and re-inserted. Otherwise+ /// the function returns immediately.+ /// @return true if the proxy was re-inserted.+ bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);++ /// Get proxy user data.+ /// @return the proxy user data or 0 if the id is invalid.+ void* GetUserData(int32 proxyId) const;++ /// Get the fat AABB for a proxy.+ const b2AABB& GetFatAABB(int32 proxyId) const;++ /// Query an AABB for overlapping proxies. The callback class+ /// is called for each proxy that overlaps the supplied AABB.+ template <typename T>+ void Query(T* callback, const b2AABB& aabb) const;++ /// Ray-cast against the proxies in the tree. This relies on the callback+ /// to perform a exact ray-cast in the case were the proxy contains a shape.+ /// The callback also performs the any collision filtering. This has performance+ /// roughly equal to k * log(n), where k is the number of collisions and n is the+ /// number of proxies in the tree.+ /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).+ /// @param callback a callback class that is called for each proxy that is hit by the ray.+ template <typename T>+ void RayCast(T* callback, const b2RayCastInput& input) const;++ /// Validate this tree. For testing.+ void Validate() const;++ /// Compute the height of the binary tree in O(N) time. Should not be+ /// called often.+ int32 GetHeight() const;++ /// Get the maximum balance of an node in the tree. The balance is the difference+ /// in height of the two children of a node.+ int32 GetMaxBalance() const;++ /// Get the ratio of the sum of the node areas to the root area.+ float32 GetAreaRatio() const;++ /// Build an optimal tree. Very expensive. For testing.+ void RebuildBottomUp();++ /// Shift the world origin. Useful for large worlds.+ /// The shift formula is: position -= newOrigin+ /// @param newOrigin the new origin with respect to the old origin+ void ShiftOrigin(const b2Vec2& newOrigin);++private:++ int32 AllocateNode();+ void FreeNode(int32 node);++ void InsertLeaf(int32 node);+ void RemoveLeaf(int32 node);++ int32 Balance(int32 index);++ int32 ComputeHeight() const;+ int32 ComputeHeight(int32 nodeId) const;++ void ValidateStructure(int32 index) const;+ void ValidateMetrics(int32 index) const;++ int32 m_root;++ b2TreeNode* m_nodes;+ int32 m_nodeCount;+ int32 m_nodeCapacity;++ int32 m_freeList;++ /// This is used to incrementally traverse the tree for re-balancing.+ uint32 m_path;++ int32 m_insertionCount;+};++inline void* b2DynamicTree::GetUserData(int32 proxyId) const+{+ b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);+ return m_nodes[proxyId].userData;+}++inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const+{+ b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);+ return m_nodes[proxyId].aabb;+}++template <typename T>+inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const+{+ b2GrowableStack<int32, 256> stack;+ stack.Push(m_root);++ while (stack.GetCount() > 0)+ {+ int32 nodeId = stack.Pop();+ if (nodeId == b2_nullNode)+ {+ continue;+ }++ const b2TreeNode* node = m_nodes + nodeId;++ if (b2TestOverlap(node->aabb, aabb))+ {+ if (node->IsLeaf())+ {+ bool proceed = callback->QueryCallback(nodeId);+ if (proceed == false)+ {+ return;+ }+ }+ else+ {+ stack.Push(node->child1);+ stack.Push(node->child2);+ }+ }+ }+}++template <typename T>+inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const+{+ b2Vec2 p1 = input.p1;+ b2Vec2 p2 = input.p2;+ b2Vec2 r = p2 - p1;+ b2Assert(r.LengthSquared() > 0.0f);+ r.Normalize();++ // v is perpendicular to the segment.+ b2Vec2 v = b2Cross(1.0f, r);+ b2Vec2 abs_v = b2Abs(v);++ // Separating axis for segment (Gino, p80).+ // |dot(v, p1 - c)| > dot(|v|, h)++ float32 maxFraction = input.maxFraction;++ // Build a bounding box for the segment.+ b2AABB segmentAABB;+ {+ b2Vec2 t = p1 + maxFraction * (p2 - p1);+ segmentAABB.lowerBound = b2Min(p1, t);+ segmentAABB.upperBound = b2Max(p1, t);+ }++ b2GrowableStack<int32, 256> stack;+ stack.Push(m_root);++ while (stack.GetCount() > 0)+ {+ int32 nodeId = stack.Pop();+ if (nodeId == b2_nullNode)+ {+ continue;+ }++ const b2TreeNode* node = m_nodes + nodeId;++ if (b2TestOverlap(node->aabb, segmentAABB) == false)+ {+ continue;+ }++ // Separating axis for segment (Gino, p80).+ // |dot(v, p1 - c)| > dot(|v|, h)+ b2Vec2 c = node->aabb.GetCenter();+ b2Vec2 h = node->aabb.GetExtents();+ float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);+ if (separation > 0.0f)+ {+ continue;+ }++ if (node->IsLeaf())+ {+ b2RayCastInput subInput;+ subInput.p1 = input.p1;+ subInput.p2 = input.p2;+ subInput.maxFraction = maxFraction;++ float32 value = callback->RayCastCallback(subInput, nodeId);++ if (value == 0.0f)+ {+ // The client has terminated the ray cast.+ return;+ }++ if (value > 0.0f)+ {+ // Update segment bounding box.+ maxFraction = value;+ b2Vec2 t = p1 + maxFraction * (p2 - p1);+ segmentAABB.lowerBound = b2Min(p1, t);+ segmentAABB.upperBound = b2Max(p1, t);+ }+ }+ else+ {+ stack.Push(node->child1);+ stack.Push(node->child2);+ }+ }+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.cppnew file mode 100644index 00000000..fa2248c4--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.cpp@@ -0,0 +1,486 @@+/*+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/b2Distance.h>+#include <Box2D/Collision/b2TimeOfImpact.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>+#include <Box2D/Common/b2Timer.h>++#include <stdio.h>++float32 b2_toiTime, b2_toiMaxTime;+int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;+int32 b2_toiRootIters, b2_toiMaxRootIters;++//+struct b2SeparationFunction+{+ enum Type+ {+ e_points,+ e_faceA,+ e_faceB+ };++ // TODO_ERIN might not need to return the separation++ float32 Initialize(const b2SimplexCache* cache,+ const b2DistanceProxy* proxyA, const b2Sweep& sweepA,+ const b2DistanceProxy* proxyB, const b2Sweep& sweepB,+ float32 t1)+ {+ m_proxyA = proxyA;+ m_proxyB = proxyB;+ int32 count = cache->count;+ b2Assert(0 < count && count < 3);++ m_sweepA = sweepA;+ m_sweepB = sweepB;++ b2Transform xfA, xfB;+ m_sweepA.GetTransform(&xfA, t1);+ m_sweepB.GetTransform(&xfB, t1);++ if (count == 1)+ {+ m_type = e_points;+ b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);+ b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);+ b2Vec2 pointA = b2Mul(xfA, localPointA);+ b2Vec2 pointB = b2Mul(xfB, localPointB);+ m_axis = pointB - pointA;+ float32 s = m_axis.Normalize();+ return s;+ }+ else if (cache->indexA[0] == cache->indexA[1])+ {+ // Two points on B and one on A.+ m_type = e_faceB;+ b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);+ b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);++ m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);+ m_axis.Normalize();+ b2Vec2 normal = b2Mul(xfB.q, m_axis);++ m_localPoint = 0.5f * (localPointB1 + localPointB2);+ b2Vec2 pointB = b2Mul(xfB, m_localPoint);++ b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);+ b2Vec2 pointA = b2Mul(xfA, localPointA);++ float32 s = b2Dot(pointA - pointB, normal);+ if (s < 0.0f)+ {+ m_axis = -m_axis;+ s = -s;+ }+ return s;+ }+ else+ {+ // Two points on A and one or two points on B.+ m_type = e_faceA;+ b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);+ b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);++ m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);+ m_axis.Normalize();+ b2Vec2 normal = b2Mul(xfA.q, m_axis);++ m_localPoint = 0.5f * (localPointA1 + localPointA2);+ b2Vec2 pointA = b2Mul(xfA, m_localPoint);++ b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);+ b2Vec2 pointB = b2Mul(xfB, localPointB);++ float32 s = b2Dot(pointB - pointA, normal);+ if (s < 0.0f)+ {+ m_axis = -m_axis;+ s = -s;+ }+ return s;+ }+ }++ //+ float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const+ {+ b2Transform xfA, xfB;+ m_sweepA.GetTransform(&xfA, t);+ m_sweepB.GetTransform(&xfB, t);++ switch (m_type)+ {+ case e_points:+ {+ b2Vec2 axisA = b2MulT(xfA.q, m_axis);+ b2Vec2 axisB = b2MulT(xfB.q, -m_axis);++ *indexA = m_proxyA->GetSupport(axisA);+ *indexB = m_proxyB->GetSupport(axisB);++ b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);+ b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);++ b2Vec2 pointA = b2Mul(xfA, localPointA);+ b2Vec2 pointB = b2Mul(xfB, localPointB);++ float32 separation = b2Dot(pointB - pointA, m_axis);+ return separation;+ }++ case e_faceA:+ {+ b2Vec2 normal = b2Mul(xfA.q, m_axis);+ b2Vec2 pointA = b2Mul(xfA, m_localPoint);++ b2Vec2 axisB = b2MulT(xfB.q, -normal);++ *indexA = -1;+ *indexB = m_proxyB->GetSupport(axisB);++ b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);+ b2Vec2 pointB = b2Mul(xfB, localPointB);++ float32 separation = b2Dot(pointB - pointA, normal);+ return separation;+ }++ case e_faceB:+ {+ b2Vec2 normal = b2Mul(xfB.q, m_axis);+ b2Vec2 pointB = b2Mul(xfB, m_localPoint);++ b2Vec2 axisA = b2MulT(xfA.q, -normal);++ *indexB = -1;+ *indexA = m_proxyA->GetSupport(axisA);++ b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);+ b2Vec2 pointA = b2Mul(xfA, localPointA);++ float32 separation = b2Dot(pointA - pointB, normal);+ return separation;+ }++ default:+ b2Assert(false);+ *indexA = -1;+ *indexB = -1;+ return 0.0f;+ }+ }++ //+ float32 Evaluate(int32 indexA, int32 indexB, float32 t) const+ {+ b2Transform xfA, xfB;+ m_sweepA.GetTransform(&xfA, t);+ m_sweepB.GetTransform(&xfB, t);++ switch (m_type)+ {+ case e_points:+ {+ b2Vec2 localPointA = m_proxyA->GetVertex(indexA);+ b2Vec2 localPointB = m_proxyB->GetVertex(indexB);++ b2Vec2 pointA = b2Mul(xfA, localPointA);+ b2Vec2 pointB = b2Mul(xfB, localPointB);+ float32 separation = b2Dot(pointB - pointA, m_axis);++ return separation;+ }++ case e_faceA:+ {+ b2Vec2 normal = b2Mul(xfA.q, m_axis);+ b2Vec2 pointA = b2Mul(xfA, m_localPoint);++ b2Vec2 localPointB = m_proxyB->GetVertex(indexB);+ b2Vec2 pointB = b2Mul(xfB, localPointB);++ float32 separation = b2Dot(pointB - pointA, normal);+ return separation;+ }++ case e_faceB:+ {+ b2Vec2 normal = b2Mul(xfB.q, m_axis);+ b2Vec2 pointB = b2Mul(xfB, m_localPoint);++ b2Vec2 localPointA = m_proxyA->GetVertex(indexA);+ b2Vec2 pointA = b2Mul(xfA, localPointA);++ float32 separation = b2Dot(pointA - pointB, normal);+ return separation;+ }++ default:+ b2Assert(false);+ return 0.0f;+ }+ }++ const b2DistanceProxy* m_proxyA;+ const b2DistanceProxy* m_proxyB;+ b2Sweep m_sweepA, m_sweepB;+ Type m_type;+ b2Vec2 m_localPoint;+ b2Vec2 m_axis;+};++// CCD via the local separating axis method. This seeks progression+// by computing the largest time at which separation is maintained.+void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)+{+ b2Timer timer;++ ++b2_toiCalls;++ output->state = b2TOIOutput::e_unknown;+ output->t = input->tMax;++ const b2DistanceProxy* proxyA = &input->proxyA;+ const b2DistanceProxy* proxyB = &input->proxyB;++ b2Sweep sweepA = input->sweepA;+ b2Sweep sweepB = input->sweepB;++ // Large rotations can make the root finder fail, so we normalize the+ // sweep angles.+ sweepA.Normalize();+ sweepB.Normalize();++ float32 tMax = input->tMax;++ float32 totalRadius = proxyA->m_radius + proxyB->m_radius;+ float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);+ float32 tolerance = 0.25f * b2_linearSlop;+ b2Assert(target > tolerance);++ float32 t1 = 0.0f;+ const int32 k_maxIterations = 20; // TODO_ERIN b2Settings+ int32 iter = 0;++ // Prepare input for distance query.+ b2SimplexCache cache;+ cache.count = 0;+ b2DistanceInput distanceInput;+ distanceInput.proxyA = input->proxyA;+ distanceInput.proxyB = input->proxyB;+ distanceInput.useRadii = false;++ // The outer loop progressively attempts to compute new separating axes.+ // This loop terminates when an axis is repeated (no progress is made).+ for(;;)+ {+ b2Transform xfA, xfB;+ sweepA.GetTransform(&xfA, t1);+ sweepB.GetTransform(&xfB, t1);++ // Get the distance between shapes. We can also use the results+ // to get a separating axis.+ distanceInput.transformA = xfA;+ distanceInput.transformB = xfB;+ b2DistanceOutput distanceOutput;+ b2Distance(&distanceOutput, &cache, &distanceInput);++ // If the shapes are overlapped, we give up on continuous collision.+ if (distanceOutput.distance <= 0.0f)+ {+ // Failure!+ output->state = b2TOIOutput::e_overlapped;+ output->t = 0.0f;+ break;+ }++ if (distanceOutput.distance < target + tolerance)+ {+ // Victory!+ output->state = b2TOIOutput::e_touching;+ output->t = t1;+ break;+ }++ // Initialize the separating axis.+ b2SeparationFunction fcn;+ fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);+#if 0+ // Dump the curve seen by the root finder+ {+ const int32 N = 100;+ float32 dx = 1.0f / N;+ float32 xs[N+1];+ float32 fs[N+1];++ float32 x = 0.0f;++ for (int32 i = 0; i <= N; ++i)+ {+ sweepA.GetTransform(&xfA, x);+ sweepB.GetTransform(&xfB, x);+ float32 f = fcn.Evaluate(xfA, xfB) - target;++ printf("%g %g\n", x, f);++ xs[i] = x;+ fs[i] = f;++ x += dx;+ }+ }+#endif++ // Compute the TOI on the separating axis. We do this by successively+ // resolving the deepest point. This loop is bounded by the number of vertices.+ bool done = false;+ float32 t2 = tMax;+ int32 pushBackIter = 0;+ for (;;)+ {+ // Find the deepest point at t2. Store the witness point indices.+ int32 indexA, indexB;+ float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);++ // Is the final configuration separated?+ if (s2 > target + tolerance)+ {+ // Victory!+ output->state = b2TOIOutput::e_separated;+ output->t = tMax;+ done = true;+ break;+ }++ // Has the separation reached tolerance?+ if (s2 > target - tolerance)+ {+ // Advance the sweeps+ t1 = t2;+ break;+ }++ // Compute the initial separation of the witness points.+ float32 s1 = fcn.Evaluate(indexA, indexB, t1);++ // Check for initial overlap. This might happen if the root finder+ // runs out of iterations.+ if (s1 < target - tolerance)+ {+ output->state = b2TOIOutput::e_failed;+ output->t = t1;+ done = true;+ break;+ }++ // Check for touching+ if (s1 <= target + tolerance)+ {+ // Victory! t1 should hold the TOI (could be 0.0).+ output->state = b2TOIOutput::e_touching;+ output->t = t1;+ done = true;+ break;+ }++ // Compute 1D root of: f(x) - target = 0+ int32 rootIterCount = 0;+ float32 a1 = t1, a2 = t2;+ for (;;)+ {+ // Use a mix of the secant rule and bisection.+ float32 t;+ if (rootIterCount & 1)+ {+ // Secant rule to improve convergence.+ t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);+ }+ else+ {+ // Bisection to guarantee progress.+ t = 0.5f * (a1 + a2);+ }++ ++rootIterCount;+ ++b2_toiRootIters;++ float32 s = fcn.Evaluate(indexA, indexB, t);++ if (b2Abs(s - target) < tolerance)+ {+ // t2 holds a tentative value for t1+ t2 = t;+ break;+ }++ // Ensure we continue to bracket the root.+ if (s > target)+ {+ a1 = t;+ s1 = s;+ }+ else+ {+ a2 = t;+ s2 = s;+ }++ if (rootIterCount == 50)+ {+ break;+ }+ }++ b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);++ ++pushBackIter;++ if (pushBackIter == b2_maxPolygonVertices)+ {+ break;+ }+ }++ ++iter;+ ++b2_toiIters;++ if (done)+ {+ break;+ }++ if (iter == k_maxIterations)+ {+ // Root finder got stuck. Semi-victory.+ output->state = b2TOIOutput::e_failed;+ output->t = t1;+ break;+ }+ }++ b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);++ float32 time = timer.GetMilliseconds();+ b2_toiMaxTime = b2Max(b2_toiMaxTime, time);+ b2_toiTime += time;+}diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.h b/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.hnew file mode 100644index 00000000..179a1700--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.h@@ -0,0 +1,58 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_TIME_OF_IMPACT_H+#define B2_TIME_OF_IMPACT_H++#include <Box2D/Common/b2Math.h>+#include <Box2D/Collision/b2Distance.h>++/// Input parameters for b2TimeOfImpact+struct b2TOIInput+{+ b2DistanceProxy proxyA;+ b2DistanceProxy proxyB;+ b2Sweep sweepA;+ b2Sweep sweepB;+ float32 tMax; // defines sweep interval [0, tMax]+};++// Output parameters for b2TimeOfImpact.+struct b2TOIOutput+{+ enum State+ {+ e_unknown,+ e_failed,+ e_overlapped,+ e_touching,+ e_separated+ };++ State state;+ float32 t;+};++/// Compute the upper bound on time before two shapes penetrate. Time is represented as+/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,+/// non-tunneling collision. If you change the time interval, you should call this function+/// again.+/// Note: use b2Distance to compute the contact point and normal at the time of impact.+void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.cppnew file mode 100644index 00000000..c9fcbfbb--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.cpp@@ -0,0 +1,215 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Common/b2BlockAllocator.h>+#include <limits.h>+#include <string.h>+#include <stddef.h>++int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] =+{+ 16, // 0+ 32, // 1+ 64, // 2+ 96, // 3+ 128, // 4+ 160, // 5+ 192, // 6+ 224, // 7+ 256, // 8+ 320, // 9+ 384, // 10+ 448, // 11+ 512, // 12+ 640, // 13+};+uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];+bool b2BlockAllocator::s_blockSizeLookupInitialized;++struct b2Chunk+{+ int32 blockSize;+ b2Block* blocks;+};++struct b2Block+{+ b2Block* next;+};++b2BlockAllocator::b2BlockAllocator()+{+ b2Assert(b2_blockSizes < UCHAR_MAX);++ m_chunkSpace = b2_chunkArrayIncrement;+ m_chunkCount = 0;+ m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));++ memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));+ memset(m_freeLists, 0, sizeof(m_freeLists));++ if (s_blockSizeLookupInitialized == false)+ {+ int32 j = 0;+ for (int32 i = 1; i <= b2_maxBlockSize; ++i)+ {+ b2Assert(j < b2_blockSizes);+ if (i <= s_blockSizes[j])+ {+ s_blockSizeLookup[i] = (uint8)j;+ }+ else+ {+ ++j;+ s_blockSizeLookup[i] = (uint8)j;+ }+ }++ s_blockSizeLookupInitialized = true;+ }+}++b2BlockAllocator::~b2BlockAllocator()+{+ for (int32 i = 0; i < m_chunkCount; ++i)+ {+ b2Free(m_chunks[i].blocks);+ }++ b2Free(m_chunks);+}++void* b2BlockAllocator::Allocate(int32 size)+{+ if (size == 0)+ return NULL;++ b2Assert(0 < size);++ if (size > b2_maxBlockSize)+ {+ return b2Alloc(size);+ }++ int32 index = s_blockSizeLookup[size];+ b2Assert(0 <= index && index < b2_blockSizes);++ if (m_freeLists[index])+ {+ b2Block* block = m_freeLists[index];+ m_freeLists[index] = block->next;+ return block;+ }+ else+ {+ if (m_chunkCount == m_chunkSpace)+ {+ b2Chunk* oldChunks = m_chunks;+ m_chunkSpace += b2_chunkArrayIncrement;+ m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));+ memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));+ memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));+ b2Free(oldChunks);+ }++ b2Chunk* chunk = m_chunks + m_chunkCount;+ chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);+#if defined(_DEBUG)+ memset(chunk->blocks, 0xcd, b2_chunkSize);+#endif+ int32 blockSize = s_blockSizes[index];+ chunk->blockSize = blockSize;+ int32 blockCount = b2_chunkSize / blockSize;+ b2Assert(blockCount * blockSize <= b2_chunkSize);+ for (int32 i = 0; i < blockCount - 1; ++i)+ {+ b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);+ b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));+ block->next = next;+ }+ b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));+ last->next = NULL;++ m_freeLists[index] = chunk->blocks->next;+ ++m_chunkCount;++ return chunk->blocks;+ }+}++void b2BlockAllocator::Free(void* p, int32 size)+{+ if (size == 0)+ {+ return;+ }++ b2Assert(0 < size);++ if (size > b2_maxBlockSize)+ {+ b2Free(p);+ return;+ }++ int32 index = s_blockSizeLookup[size];+ b2Assert(0 <= index && index < b2_blockSizes);++#ifdef _DEBUG+ // Verify the memory address and size is valid.+ int32 blockSize = s_blockSizes[index];+ bool found = false;+ for (int32 i = 0; i < m_chunkCount; ++i)+ {+ b2Chunk* chunk = m_chunks + i;+ if (chunk->blockSize != blockSize)+ {+ b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks ||+ (int8*)chunk->blocks + b2_chunkSize <= (int8*)p);+ }+ else+ {+ if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)+ {+ found = true;+ }+ }+ }++ b2Assert(found);++ memset(p, 0xfd, blockSize);+#endif++ b2Block* block = (b2Block*)p;+ block->next = m_freeLists[index];+ m_freeLists[index] = block;+}++void b2BlockAllocator::Clear()+{+ for (int32 i = 0; i < m_chunkCount; ++i)+ {+ b2Free(m_chunks[i].blocks);+ }++ m_chunkCount = 0;+ memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));++ memset(m_freeLists, 0, sizeof(m_freeLists));+}diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.h b/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.hnew file mode 100644index 00000000..aeb18a18--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.h@@ -0,0 +1,63 @@+/* Altered for NumPlay's Mr Bullet (NumWorks calculator): 2 KB chunks */+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_BLOCK_ALLOCATOR_H+#define B2_BLOCK_ALLOCATOR_H++#include <Box2D/Common/b2Settings.h>++const int32 b2_chunkSize = 2 * 1024; // (16 KB in Box2D)+const int32 b2_maxBlockSize = 640;+const int32 b2_blockSizes = 14;+const int32 b2_chunkArrayIncrement = 128;++struct b2Block;+struct b2Chunk;++/// This is a small object allocator used for allocating small+/// objects that persist for more than one time step.+/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp+class b2BlockAllocator+{+public:+ b2BlockAllocator();+ ~b2BlockAllocator();++ /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.+ void* Allocate(int32 size);++ /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.+ void Free(void* p, int32 size);++ void Clear();++private:++ b2Chunk* m_chunks;+ int32 m_chunkCount;+ int32 m_chunkSpace;++ b2Block* m_freeLists[b2_blockSizes];++ static int32 s_blockSizes[b2_blockSizes];+ static uint8 s_blockSizeLookup[b2_maxBlockSize + 1];+ static bool s_blockSizeLookupInitialized;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Draw.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Draw.cppnew file mode 100644index 00000000..327b5807--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Draw.cpp@@ -0,0 +1,44 @@+/*+* Copyright (c) 2011 Erin Catto http://box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Common/b2Draw.h>++b2Draw::b2Draw()+{+ m_drawFlags = 0;+}++void b2Draw::SetFlags(uint32 flags)+{+ m_drawFlags = flags;+}++uint32 b2Draw::GetFlags() const+{+ return m_drawFlags;+}++void b2Draw::AppendFlags(uint32 flags)+{+ m_drawFlags |= flags;+}++void b2Draw::ClearFlags(uint32 flags)+{+ m_drawFlags &= ~flags;+}diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Draw.h b/games/mrbullet/src/box2d/Box2D/Common/b2Draw.hnew file mode 100644index 00000000..16c0b323--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Draw.h@@ -0,0 +1,86 @@+/*+* Copyright (c) 2011 Erin Catto http://box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_DRAW_H+#define B2_DRAW_H++#include <Box2D/Common/b2Math.h>++/// Color for debug drawing. Each value has the range [0,1].+struct b2Color+{+ b2Color() {}+ b2Color(float32 r, float32 g, float32 b, float32 a = 1.0f) : r(r), g(g), b(b), a(a) {}+ void Set(float32 ri, float32 gi, float32 bi, float32 ai = 1.0f) { r = ri; g = gi; b = bi; a = ai; }+ float32 r, g, b, a;+};++/// Implement and register this class with a b2World to provide debug drawing of physics+/// entities in your game.+class b2Draw+{+public:+ b2Draw();++ virtual ~b2Draw() {}++ enum+ {+ e_shapeBit = 0x0001, ///< draw shapes+ e_jointBit = 0x0002, ///< draw joint connections+ e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes+ e_pairBit = 0x0008, ///< draw broad-phase pairs+ e_centerOfMassBit = 0x0010 ///< draw center of mass frame+ };++ /// Set the drawing flags.+ void SetFlags(uint32 flags);++ /// Get the drawing flags.+ uint32 GetFlags() const;++ /// Append flags to the current flags.+ void AppendFlags(uint32 flags);++ /// Clear flags from the current flags.+ void ClearFlags(uint32 flags);++ /// Draw a closed polygon provided in CCW order.+ virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;++ /// Draw a solid closed polygon provided in CCW order.+ virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;++ /// Draw a circle.+ virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;++ /// Draw a solid circle.+ virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;++ /// Draw a line segment.+ virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;++ /// Draw a transform. Choose your own length scale.+ /// @param xf a transform.+ virtual void DrawTransform(const b2Transform& xf) = 0;++protected:+ uint32 m_drawFlags;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2GrowableStack.h b/games/mrbullet/src/box2d/Box2D/Common/b2GrowableStack.hnew file mode 100644index 00000000..80514f29--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2GrowableStack.h@@ -0,0 +1,85 @@+/*+* Copyright (c) 2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_GROWABLE_STACK_H+#define B2_GROWABLE_STACK_H+#include <Box2D/Common/b2Settings.h>+#include <memory.h>++/// This is a growable LIFO stack with an initial capacity of N.+/// If the stack size exceeds the initial capacity, the heap is used+/// to increase the size of the stack.+template <typename T, int32 N>+class b2GrowableStack+{+public:+ b2GrowableStack()+ {+ m_stack = m_array;+ m_count = 0;+ m_capacity = N;+ }++ ~b2GrowableStack()+ {+ if (m_stack != m_array)+ {+ b2Free(m_stack);+ m_stack = NULL;+ }+ }++ void Push(const T& element)+ {+ if (m_count == m_capacity)+ {+ T* old = m_stack;+ m_capacity *= 2;+ m_stack = (T*)b2Alloc(m_capacity * sizeof(T));+ memcpy(m_stack, old, m_count * sizeof(T));+ if (old != m_array)+ {+ b2Free(old);+ }+ }++ m_stack[m_count] = element;+ ++m_count;+ }++ T Pop()+ {+ b2Assert(m_count > 0);+ --m_count;+ return m_stack[m_count];+ }++ int32 GetCount()+ {+ return m_count;+ }++private:+ T* m_stack;+ T m_array[N];+ int32 m_count;+ int32 m_capacity;+};+++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Math.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Math.cppnew file mode 100644index 00000000..4974fe18--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Math.cpp@@ -0,0 +1,94 @@+/*+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Common/b2Math.h>++const b2Vec2 b2Vec2_zero(0.0f, 0.0f);++/// Solve A * x = b, where b is a column vector. This is more efficient+/// than computing the inverse in one-shot cases.+b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const+{+ float32 det = b2Dot(ex, b2Cross(ey, ez));+ if (det != 0.0f)+ {+ det = 1.0f / det;+ }+ b2Vec3 x;+ x.x = det * b2Dot(b, b2Cross(ey, ez));+ x.y = det * b2Dot(ex, b2Cross(b, ez));+ x.z = det * b2Dot(ex, b2Cross(ey, b));+ return x;+}++/// Solve A * x = b, where b is a column vector. This is more efficient+/// than computing the inverse in one-shot cases.+b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const+{+ float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;+ float32 det = a11 * a22 - a12 * a21;+ if (det != 0.0f)+ {+ det = 1.0f / det;+ }+ b2Vec2 x;+ x.x = det * (a22 * b.x - a12 * b.y);+ x.y = det * (a11 * b.y - a21 * b.x);+ return x;+}++///+void b2Mat33::GetInverse22(b2Mat33* M) const+{+ float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;+ float32 det = a * d - b * c;+ if (det != 0.0f)+ {+ det = 1.0f / det;+ }++ M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f;+ M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f;+ M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;+}++/// Returns the zero matrix if singular.+void b2Mat33::GetSymInverse33(b2Mat33* M) const+{+ float32 det = b2Dot(ex, b2Cross(ey, ez));+ if (det != 0.0f)+ {+ det = 1.0f / det;+ }++ float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;+ float32 a22 = ey.y, a23 = ez.y;+ float32 a33 = ez.z;++ M->ex.x = det * (a22 * a33 - a23 * a23);+ M->ex.y = det * (a13 * a23 - a12 * a33);+ M->ex.z = det * (a12 * a23 - a13 * a22);++ M->ey.x = M->ex.y;+ M->ey.y = det * (a11 * a33 - a13 * a13);+ M->ey.z = det * (a13 * a12 - a11 * a23);++ M->ez.x = M->ex.z;+ M->ez.y = M->ey.z;+ M->ez.z = det * (a11 * a22 - a12 * a12);+}diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Math.h b/games/mrbullet/src/box2d/Box2D/Common/b2Math.hnew file mode 100644index 00000000..a9b0813e--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Math.h@@ -0,0 +1,720 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_MATH_H+#define B2_MATH_H++#include <Box2D/Common/b2Settings.h>+#include <math.h>++/// This function is used to ensure that a floating point number is not a NaN or infinity.+inline bool b2IsValid(float32 x)+{+ int32 ix = *reinterpret_cast<int32*>(&x);+ return (ix & 0x7f800000) != 0x7f800000;+}++/// This is a approximate yet fast inverse square-root.+inline float32 b2InvSqrt(float32 x)+{+ union+ {+ float32 x;+ int32 i;+ } convert;++ convert.x = x;+ float32 xhalf = 0.5f * x;+ convert.i = 0x5f3759df - (convert.i >> 1);+ x = convert.x;+ x = x * (1.5f - xhalf * x * x);+ return x;+}++#define b2Sqrt(x) sqrtf(x)+#define b2Atan2(y, x) atan2f(y, x)++/// A 2D column vector.+struct b2Vec2+{+ /// Default constructor does nothing (for performance).+ b2Vec2() {}++ /// Construct using coordinates.+ b2Vec2(float32 x, float32 y) : x(x), y(y) {}++ /// Set this vector to all zeros.+ void SetZero() { x = 0.0f; y = 0.0f; }++ /// Set this vector to some specified coordinates.+ void Set(float32 x_, float32 y_) { x = x_; y = y_; }++ /// Negate this vector.+ b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }++ /// Read from and indexed element.+ float32 operator () (int32 i) const+ {+ return (&x)[i];+ }++ /// Write to an indexed element.+ float32& operator () (int32 i)+ {+ return (&x)[i];+ }++ /// Add a vector to this vector.+ void operator += (const b2Vec2& v)+ {+ x += v.x; y += v.y;+ }++ /// Subtract a vector from this vector.+ void operator -= (const b2Vec2& v)+ {+ x -= v.x; y -= v.y;+ }++ /// Multiply this vector by a scalar.+ void operator *= (float32 a)+ {+ x *= a; y *= a;+ }++ /// Get the length of this vector (the norm).+ float32 Length() const+ {+ return b2Sqrt(x * x + y * y);+ }++ /// Get the length squared. For performance, use this instead of+ /// b2Vec2::Length (if possible).+ float32 LengthSquared() const+ {+ return x * x + y * y;+ }++ /// Convert this vector into a unit vector. Returns the length.+ float32 Normalize()+ {+ float32 length = Length();+ if (length < b2_epsilon)+ {+ return 0.0f;+ }+ float32 invLength = 1.0f / length;+ x *= invLength;+ y *= invLength;++ return length;+ }++ /// Does this vector contain finite coordinates?+ bool IsValid() const+ {+ return b2IsValid(x) && b2IsValid(y);+ }++ /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)+ b2Vec2 Skew() const+ {+ return b2Vec2(-y, x);+ }++ float32 x, y;+};++/// A 2D column vector with 3 elements.+struct b2Vec3+{+ /// Default constructor does nothing (for performance).+ b2Vec3() {}++ /// Construct using coordinates.+ b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}++ /// Set this vector to all zeros.+ void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }++ /// Set this vector to some specified coordinates.+ void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }++ /// Negate this vector.+ b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }++ /// Add a vector to this vector.+ void operator += (const b2Vec3& v)+ {+ x += v.x; y += v.y; z += v.z;+ }++ /// Subtract a vector from this vector.+ void operator -= (const b2Vec3& v)+ {+ x -= v.x; y -= v.y; z -= v.z;+ }++ /// Multiply this vector by a scalar.+ void operator *= (float32 s)+ {+ x *= s; y *= s; z *= s;+ }++ float32 x, y, z;+};++/// A 2-by-2 matrix. Stored in column-major order.+struct b2Mat22+{+ /// The default constructor does nothing (for performance).+ b2Mat22() {}++ /// Construct this matrix using columns.+ b2Mat22(const b2Vec2& c1, const b2Vec2& c2)+ {+ ex = c1;+ ey = c2;+ }++ /// Construct this matrix using scalars.+ b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)+ {+ ex.x = a11; ex.y = a21;+ ey.x = a12; ey.y = a22;+ }++ /// Initialize this matrix using columns.+ void Set(const b2Vec2& c1, const b2Vec2& c2)+ {+ ex = c1;+ ey = c2;+ }++ /// Set this to the identity matrix.+ void SetIdentity()+ {+ ex.x = 1.0f; ey.x = 0.0f;+ ex.y = 0.0f; ey.y = 1.0f;+ }++ /// Set this matrix to all zeros.+ void SetZero()+ {+ ex.x = 0.0f; ey.x = 0.0f;+ ex.y = 0.0f; ey.y = 0.0f;+ }++ b2Mat22 GetInverse() const+ {+ float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;+ b2Mat22 B;+ float32 det = a * d - b * c;+ if (det != 0.0f)+ {+ det = 1.0f / det;+ }+ B.ex.x = det * d; B.ey.x = -det * b;+ B.ex.y = -det * c; B.ey.y = det * a;+ return B;+ }++ /// Solve A * x = b, where b is a column vector. This is more efficient+ /// than computing the inverse in one-shot cases.+ b2Vec2 Solve(const b2Vec2& b) const+ {+ float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;+ float32 det = a11 * a22 - a12 * a21;+ if (det != 0.0f)+ {+ det = 1.0f / det;+ }+ b2Vec2 x;+ x.x = det * (a22 * b.x - a12 * b.y);+ x.y = det * (a11 * b.y - a21 * b.x);+ return x;+ }++ b2Vec2 ex, ey;+};++/// A 3-by-3 matrix. Stored in column-major order.+struct b2Mat33+{+ /// The default constructor does nothing (for performance).+ b2Mat33() {}++ /// Construct this matrix using columns.+ b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)+ {+ ex = c1;+ ey = c2;+ ez = c3;+ }++ /// Set this matrix to all zeros.+ void SetZero()+ {+ ex.SetZero();+ ey.SetZero();+ ez.SetZero();+ }++ /// Solve A * x = b, where b is a column vector. This is more efficient+ /// than computing the inverse in one-shot cases.+ b2Vec3 Solve33(const b2Vec3& b) const;++ /// Solve A * x = b, where b is a column vector. This is more efficient+ /// than computing the inverse in one-shot cases. Solve only the upper+ /// 2-by-2 matrix equation.+ b2Vec2 Solve22(const b2Vec2& b) const;++ /// Get the inverse of this matrix as a 2-by-2.+ /// Returns the zero matrix if singular.+ void GetInverse22(b2Mat33* M) const;++ /// Get the symmetric inverse of this matrix as a 3-by-3.+ /// Returns the zero matrix if singular.+ void GetSymInverse33(b2Mat33* M) const;++ b2Vec3 ex, ey, ez;+};++/// Rotation+struct b2Rot+{+ b2Rot() {}++ /// Initialize from an angle in radians+ explicit b2Rot(float32 angle)+ {+ /// TODO_ERIN optimize+ s = sinf(angle);+ c = cosf(angle);+ }++ /// Set using an angle in radians.+ void Set(float32 angle)+ {+ /// TODO_ERIN optimize+ s = sinf(angle);+ c = cosf(angle);+ }++ /// Set to the identity rotation+ void SetIdentity()+ {+ s = 0.0f;+ c = 1.0f;+ }++ /// Get the angle in radians+ float32 GetAngle() const+ {+ return b2Atan2(s, c);+ }++ /// Get the x-axis+ b2Vec2 GetXAxis() const+ {+ return b2Vec2(c, s);+ }++ /// Get the u-axis+ b2Vec2 GetYAxis() const+ {+ return b2Vec2(-s, c);+ }++ /// Sine and cosine+ float32 s, c;+};++/// A transform contains translation and rotation. It is used to represent+/// the position and orientation of rigid frames.+struct b2Transform+{+ /// The default constructor does nothing.+ b2Transform() {}++ /// Initialize using a position vector and a rotation.+ b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}++ /// Set this to the identity transform.+ void SetIdentity()+ {+ p.SetZero();+ q.SetIdentity();+ }++ /// Set this based on the position and angle.+ void Set(const b2Vec2& position, float32 angle)+ {+ p = position;+ q.Set(angle);+ }++ b2Vec2 p;+ b2Rot q;+};++/// This describes the motion of a body/shape for TOI computation.+/// Shapes are defined with respect to the body origin, which may+/// no coincide with the center of mass. However, to support dynamics+/// we must interpolate the center of mass position.+struct b2Sweep+{+ /// Get the interpolated transform at a specific time.+ /// @param beta is a factor in [0,1], where 0 indicates alpha0.+ void GetTransform(b2Transform* xfb, float32 beta) const;++ /// Advance the sweep forward, yielding a new initial state.+ /// @param alpha the new initial time.+ void Advance(float32 alpha);++ /// Normalize the angles.+ void Normalize();++ b2Vec2 localCenter; ///< local center of mass position+ b2Vec2 c0, c; ///< center world positions+ float32 a0, a; ///< world angles++ /// Fraction of the current time step in the range [0,1]+ /// c0 and a0 are the positions at alpha0.+ float32 alpha0;+};++/// Useful constant+extern const b2Vec2 b2Vec2_zero;++/// Perform the dot product on two vectors.+inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)+{+ return a.x * b.x + a.y * b.y;+}++/// Perform the cross product on two vectors. In 2D this produces a scalar.+inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)+{+ return a.x * b.y - a.y * b.x;+}++/// Perform the cross product on a vector and a scalar. In 2D this produces+/// a vector.+inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)+{+ return b2Vec2(s * a.y, -s * a.x);+}++/// Perform the cross product on a scalar and a vector. In 2D this produces+/// a vector.+inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)+{+ return b2Vec2(-s * a.y, s * a.x);+}++/// Multiply a matrix times a vector. If a rotation matrix is provided,+/// then this transforms the vector from one frame to another.+inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)+{+ return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);+}++/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,+/// then this transforms the vector from one frame to another (inverse transform).+inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)+{+ return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));+}++/// Add two vectors component-wise.+inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)+{+ return b2Vec2(a.x + b.x, a.y + b.y);+}++/// Subtract two vectors component-wise.+inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)+{+ return b2Vec2(a.x - b.x, a.y - b.y);+}++inline b2Vec2 operator * (float32 s, const b2Vec2& a)+{+ return b2Vec2(s * a.x, s * a.y);+}++inline bool operator == (const b2Vec2& a, const b2Vec2& b)+{+ return a.x == b.x && a.y == b.y;+}++inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)+{+ b2Vec2 c = a - b;+ return c.Length();+}++inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)+{+ b2Vec2 c = a - b;+ return b2Dot(c, c);+}++inline b2Vec3 operator * (float32 s, const b2Vec3& a)+{+ return b2Vec3(s * a.x, s * a.y, s * a.z);+}++/// Add two vectors component-wise.+inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)+{+ return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);+}++/// Subtract two vectors component-wise.+inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)+{+ return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);+}++/// Perform the dot product on two vectors.+inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)+{+ return a.x * b.x + a.y * b.y + a.z * b.z;+}++/// Perform the cross product on two vectors.+inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)+{+ return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);+}++inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)+{+ return b2Mat22(A.ex + B.ex, A.ey + B.ey);+}++// A * B+inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)+{+ return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));+}++// A^T * B+inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)+{+ b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));+ b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));+ return b2Mat22(c1, c2);+}++/// Multiply a matrix times a vector.+inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)+{+ return v.x * A.ex + v.y * A.ey + v.z * A.ez;+}++/// Multiply a matrix times a vector.+inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)+{+ return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);+}++/// Multiply two rotations: q * r+inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)+{+ // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]+ // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]+ // s = qs * rc + qc * rs+ // c = qc * rc - qs * rs+ b2Rot qr;+ qr.s = q.s * r.c + q.c * r.s;+ qr.c = q.c * r.c - q.s * r.s;+ return qr;+}++/// Transpose multiply two rotations: qT * r+inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)+{+ // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]+ // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]+ // s = qc * rs - qs * rc+ // c = qc * rc + qs * rs+ b2Rot qr;+ qr.s = q.c * r.s - q.s * r.c;+ qr.c = q.c * r.c + q.s * r.s;+ return qr;+}++/// Rotate a vector+inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)+{+ return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);+}++/// Inverse rotate a vector+inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)+{+ return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);+}++inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)+{+ float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;+ float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;++ return b2Vec2(x, y);+}++inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)+{+ float32 px = v.x - T.p.x;+ float32 py = v.y - T.p.y;+ float32 x = (T.q.c * px + T.q.s * py);+ float32 y = (-T.q.s * px + T.q.c * py);++ return b2Vec2(x, y);+}++// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p+// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p+inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)+{+ b2Transform C;+ C.q = b2Mul(A.q, B.q);+ C.p = b2Mul(A.q, B.p) + A.p;+ return C;+}++// v2 = A.q' * (B.q * v1 + B.p - A.p)+// = A.q' * B.q * v1 + A.q' * (B.p - A.p)+inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)+{+ b2Transform C;+ C.q = b2MulT(A.q, B.q);+ C.p = b2MulT(A.q, B.p - A.p);+ return C;+}++template <typename T>+inline T b2Abs(T a)+{+ return a > T(0) ? a : -a;+}++inline b2Vec2 b2Abs(const b2Vec2& a)+{+ return b2Vec2(b2Abs(a.x), b2Abs(a.y));+}++inline b2Mat22 b2Abs(const b2Mat22& A)+{+ return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));+}++template <typename T>+inline T b2Min(T a, T b)+{+ return a < b ? a : b;+}++inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)+{+ return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));+}++template <typename T>+inline T b2Max(T a, T b)+{+ return a > b ? a : b;+}++inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)+{+ return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));+}++template <typename T>+inline T b2Clamp(T a, T low, T high)+{+ return b2Max(low, b2Min(a, high));+}++inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)+{+ return b2Max(low, b2Min(a, high));+}++template<typename T> inline void b2Swap(T& a, T& b)+{+ T tmp = a;+ a = b;+ b = tmp;+}++/// "Next Largest Power of 2+/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm+/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with+/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next+/// largest power of 2. For a 32-bit value:"+inline uint32 b2NextPowerOfTwo(uint32 x)+{+ x |= (x >> 1);+ x |= (x >> 2);+ x |= (x >> 4);+ x |= (x >> 8);+ x |= (x >> 16);+ return x + 1;+}++inline bool b2IsPowerOfTwo(uint32 x)+{+ bool result = x > 0 && (x & (x - 1)) == 0;+ return result;+}++inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const+{+ xf->p = (1.0f - beta) * c0 + beta * c;+ float32 angle = (1.0f - beta) * a0 + beta * a;+ xf->q.Set(angle);++ // Shift to origin+ xf->p -= b2Mul(xf->q, localCenter);+}++inline void b2Sweep::Advance(float32 alpha)+{+ b2Assert(alpha0 < 1.0f);+ float32 beta = (alpha - alpha0) / (1.0f - alpha0);+ c0 += beta * (c - c0);+ a0 += beta * (a - a0);+ alpha0 = alpha;+}++/// Normalize an angle in radians to be between -pi and pi+inline void b2Sweep::Normalize()+{+ float32 twoPi = 2.0f * b2_pi;+ float32 d = twoPi * floorf(a0 / twoPi);+ a0 -= d;+ a -= d;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Settings.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Settings.cppnew file mode 100644index 00000000..5ce1f78f--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Settings.cpp@@ -0,0 +1,53 @@+/* Altered for NumPlay's Mr Bullet (NumWorks calculator): b2Log prints nothing; the memory in use is counted */+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Common/b2Settings.h>+#include <stdio.h>+#include <stdarg.h>+#include <stdlib.h>++b2Version b2_version = {2, 3, 1};++// Memory allocators. Modify these to use your own allocator.+/* Box2D's memory in use and the most it has used (a word before each block keeps its size), and the limit past+ which no new contact is made (b2ContactManager::AddPair): a pair that would touch waits until there is room */+extern "C" { long b2_heap_now, b2_heap_peak, b2_heap_limit; }+void* b2Alloc(int32 size)+{+ long *p = (long *)malloc(size + sizeof(long) * 2);+ if (!p) return NULL;+ p[0] = size;+ b2_heap_now += size + sizeof(long) * 2;+ if (b2_heap_now > b2_heap_peak) b2_heap_peak = b2_heap_now;+ return p + 2;+}++void b2Free(void* mem)+{+ if (!mem) return;+ long *p = (long *)mem - 2;+ b2_heap_now -= p[0] + sizeof(long) * 2;+ free(p);+}++// You can modify this to use your logging facility.+void b2Log(const char* string, ...)+{+ (void)string;+}diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Settings.h b/games/mrbullet/src/box2d/Box2D/Common/b2Settings.hnew file mode 100644index 00000000..370088d5--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Settings.h@@ -0,0 +1,153 @@+/* Altered for NumPlay's Mr Bullet (NumWorks calculator): a+ * tighter fattening of boxes in the broad phase (fewer contacts that never touch) */+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_SETTINGS_H+#define B2_SETTINGS_H++#include <stddef.h>+#include <assert.h>+#include <float.h>++#define B2_NOT_USED(x) ((void)(x))+#define b2Assert(A) assert(A)++typedef signed char int8;+typedef signed short int16;+typedef signed int int32;+typedef unsigned char uint8;+typedef unsigned short uint16;+typedef unsigned int uint32;+typedef float float32;+typedef double float64;++#define b2_maxFloat FLT_MAX+#define b2_epsilon FLT_EPSILON+#define b2_pi 3.14159265359f++/// @file+/// Global tuning constants based on meters-kilograms-seconds (MKS) units.+///++// Collision++/// The maximum number of contact points between two convex shapes. Do+/// not change this value.+#define b2_maxManifoldPoints 2++/// The maximum number of vertices on a convex polygon. You cannot increase+/// this too much because b2BlockAllocator has a maximum object size.+#define b2_maxPolygonVertices 8++/// This is used to fatten AABBs in the dynamic tree. This allows proxies+/// to move by a small amount without triggering a tree adjustment.+/// This is in meters.+#define b2_aabbExtension 0.01f // (0.1 in Box2D: the levels' many small parts made too many contacts)++/// This is used to fatten AABBs in the dynamic tree. This is used to predict+/// the future position based on the current displacement.+/// This is a dimensionless multiplier.+#define b2_aabbMultiplier 1.0f // (2 in Box2D)++/// A small length used as a collision and constraint tolerance. Usually it is+/// chosen to be numerically significant, but visually insignificant.+#define b2_linearSlop 0.005f++/// A small angle used as a collision and constraint tolerance. Usually it is+/// chosen to be numerically significant, but visually insignificant.+#define b2_angularSlop (2.0f / 180.0f * b2_pi)++/// The radius of the polygon/edge shape skin. This should not be modified. Making+/// this smaller means polygons will have an insufficient buffer for continuous collision.+/// Making it larger may create artifacts for vertex collision.+#define b2_polygonRadius (2.0f * b2_linearSlop)++/// Maximum number of sub-steps per contact in continuous physics simulation.+#define b2_maxSubSteps 8+++// Dynamics++/// Maximum number of contacts to be handled to solve a TOI impact.+#define b2_maxTOIContacts 32++/// A velocity threshold for elastic collisions. Any collision with a relative linear+/// velocity below this threshold will be treated as inelastic.+#define b2_velocityThreshold 1.0f++/// The maximum linear position correction used when solving constraints. This helps to+/// prevent overshoot.+#define b2_maxLinearCorrection 0.2f++/// The maximum angular position correction used when solving constraints. This helps to+/// prevent overshoot.+#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi)++/// The maximum linear velocity of a body. This limit is very large and is used+/// to prevent numerical problems. You shouldn't need to adjust this.+#define b2_maxTranslation 2.0f+#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation)++/// The maximum angular velocity of a body. This limit is very large and is used+/// to prevent numerical problems. You shouldn't need to adjust this.+#define b2_maxRotation (0.5f * b2_pi)+#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation)++/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so+/// that overlap is removed in one time step. However using values close to 1 often lead+/// to overshoot.+#define b2_baumgarte 0.2f+#define b2_toiBaugarte 0.75f+++// Sleep++/// The time that a body must be still before it will go to sleep.+#define b2_timeToSleep 0.5f++/// A body cannot sleep if its linear velocity is above this tolerance.+#define b2_linearSleepTolerance 0.01f++/// A body cannot sleep if its angular velocity is above this tolerance.+#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi)++// Memory Allocation++/// Implement this function to use your own memory allocator.+void* b2Alloc(int32 size);++/// If you implement b2Alloc, you should also implement this function.+void b2Free(void* mem);++/// Logging function.+void b2Log(const char* string, ...);++/// Version numbering scheme.+/// See http://en.wikipedia.org/wiki/Software_versioning+struct b2Version+{+ int32 major; ///< significant changes+ int32 minor; ///< incremental changes+ int32 revision; ///< bug fixes+};++/// Current version.+extern b2Version b2_version;++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.cppnew file mode 100644index 00000000..144de7bf--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.cpp@@ -0,0 +1,85 @@+/* Altered for NumPlay's Mr Bullet (NumWorks calculator): allocations kept 8-byte aligned */+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Common/b2StackAllocator.h>+#include <Box2D/Common/b2Math.h>++b2StackAllocator::b2StackAllocator()+{+ m_index = 0;+ m_allocation = 0;+ m_maxAllocation = 0;+ m_entryCount = 0;+}++b2StackAllocator::~b2StackAllocator()+{+ b2Assert(m_index == 0);+ b2Assert(m_entryCount == 0);+}++void* b2StackAllocator::Allocate(int32 size)+{+ size = (size + 7) & ~7; // (NumPlay: kept 8-byte aligned)+ b2Assert(m_entryCount < b2_maxStackEntries);++ b2StackEntry* entry = m_entries + m_entryCount;+ entry->size = size;+ if (m_index + size > b2_stackSize)+ {+ entry->data = (char*)b2Alloc(size);+ entry->usedMalloc = true;+ }+ else+ {+ entry->data = m_data + m_index;+ entry->usedMalloc = false;+ m_index += size;+ }++ m_allocation += size;+ m_maxAllocation = b2Max(m_maxAllocation, m_allocation);+ ++m_entryCount;++ return entry->data;+}++void b2StackAllocator::Free(void* p)+{+ b2Assert(m_entryCount > 0);+ b2StackEntry* entry = m_entries + m_entryCount - 1;+ b2Assert(p == entry->data);+ if (entry->usedMalloc)+ {+ b2Free(p);+ }+ else+ {+ m_index -= entry->size;+ }+ m_allocation -= entry->size;+ --m_entryCount;++ p = NULL;+}++int32 b2StackAllocator::GetMaxAllocation() const+{+ return m_maxAllocation;+}diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.h b/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.hnew file mode 100644index 00000000..b7f8370c--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.h@@ -0,0 +1,61 @@+/* Altered for NumPlay's Mr Bullet (NumWorks calculator): an 8 KB stack */+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_STACK_ALLOCATOR_H+#define B2_STACK_ALLOCATOR_H++#include <Box2D/Common/b2Settings.h>++const int32 b2_stackSize = 8 * 1024; // (100k in Box2D; more comes from b2Alloc)+const int32 b2_maxStackEntries = 32;++struct b2StackEntry+{+ char* data;+ int32 size;+ bool usedMalloc;+};++// This is a stack allocator used for fast per step allocations.+// You must nest allocate/free pairs. The code will assert+// if you try to interleave multiple allocate/free pairs.+class b2StackAllocator+{+public:+ b2StackAllocator();+ ~b2StackAllocator();++ void* Allocate(int32 size);+ void Free(void* p);++ int32 GetMaxAllocation() const;++private:++ char m_data[b2_stackSize];+ int32 m_index;++ int32 m_allocation;+ int32 m_maxAllocation;++ b2StackEntry m_entries[b2_maxStackEntries];+ int32 m_entryCount;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Timer.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Timer.cppnew file mode 100644index 00000000..44dc5cea--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Timer.cpp@@ -0,0 +1,101 @@+/*+* Copyright (c) 2011 Erin Catto http://box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Common/b2Timer.h>++#if defined(_WIN32)++float64 b2Timer::s_invFrequency = 0.0f;++#define WIN32_LEAN_AND_MEAN+#include <windows.h>++b2Timer::b2Timer()+{+ LARGE_INTEGER largeInteger;++ if (s_invFrequency == 0.0f)+ {+ QueryPerformanceFrequency(&largeInteger);+ s_invFrequency = float64(largeInteger.QuadPart);+ if (s_invFrequency > 0.0f)+ {+ s_invFrequency = 1000.0f / s_invFrequency;+ }+ }++ QueryPerformanceCounter(&largeInteger);+ m_start = float64(largeInteger.QuadPart);+}++void b2Timer::Reset()+{+ LARGE_INTEGER largeInteger;+ QueryPerformanceCounter(&largeInteger);+ m_start = float64(largeInteger.QuadPart);+}++float32 b2Timer::GetMilliseconds() const+{+ LARGE_INTEGER largeInteger;+ QueryPerformanceCounter(&largeInteger);+ float64 count = float64(largeInteger.QuadPart);+ float32 ms = float32(s_invFrequency * (count - m_start));+ return ms;+}++#elif defined(__linux__) || defined (__APPLE__)++#include <sys/time.h>++b2Timer::b2Timer()+{+ Reset();+}++void b2Timer::Reset()+{+ timeval t;+ gettimeofday(&t, 0);+ m_start_sec = t.tv_sec;+ m_start_usec = t.tv_usec;+}++float32 b2Timer::GetMilliseconds() const+{+ timeval t;+ gettimeofday(&t, 0);+ return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec);+}++#else++b2Timer::b2Timer()+{+}++void b2Timer::Reset()+{+}++float32 b2Timer::GetMilliseconds() const+{+ return 0.0f;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Timer.h b/games/mrbullet/src/box2d/Box2D/Common/b2Timer.hnew file mode 100644index 00000000..823404fe--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Common/b2Timer.h@@ -0,0 +1,50 @@+/*+* Copyright (c) 2011 Erin Catto http://box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_TIMER_H+#define B2_TIMER_H++#include <Box2D/Common/b2Settings.h>++/// Timer for profiling. This has platform specific code and may+/// not work on every platform.+class b2Timer+{+public:++ /// Constructor+ b2Timer();++ /// Reset the timer.+ void Reset();++ /// Get the time since construction or the last reset.+ float32 GetMilliseconds() const;++private:++#if defined(_WIN32)+ float64 m_start;+ static float64 s_invFrequency;+#elif defined(__linux__) || defined (__APPLE__)+ unsigned long m_start_sec;+ unsigned long m_start_usec;+#endif+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cppnew file mode 100644index 00000000..5e2f0f05--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp@@ -0,0 +1,53 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>++#include <new>++b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));+ return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);+}++void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();+ allocator->Free(contact, sizeof(b2ChainAndCircleContact));+}++b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)+: b2Contact(fixtureA, indexA, fixtureB, indexB)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);+}++void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();+ b2EdgeShape edge;+ chain->GetChildEdge(&edge, m_indexA);+ b2CollideEdgeAndCircle( manifold, &edge, xfA,+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.hnew file mode 100644index 00000000..2dad0b64--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h@@ -0,0 +1,39 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H+#define B2_CHAIN_AND_CIRCLE_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2ChainAndCircleContact : public b2Contact+{+public:+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);+ ~b2ChainAndCircleContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cppnew file mode 100644index 00000000..a4a99b8c--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp@@ -0,0 +1,53 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>++#include <new>++b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));+ return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);+}++void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();+ allocator->Free(contact, sizeof(b2ChainAndPolygonContact));+}++b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)+: b2Contact(fixtureA, indexA, fixtureB, indexB)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);+}++void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();+ b2EdgeShape edge;+ chain->GetChildEdge(&edge, m_indexA);+ b2CollideEdgeAndPolygon( manifold, &edge, xfA,+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.hnew file mode 100644index 00000000..8f30ee86--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h@@ -0,0 +1,39 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H+#define B2_CHAIN_AND_POLYGON_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2ChainAndPolygonContact : public b2Contact+{+public:+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);+ ~b2ChainAndPolygonContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cppnew file mode 100644index 00000000..23b98a9f--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp@@ -0,0 +1,52 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2CircleContact.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2WorldCallbacks.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Collision/b2TimeOfImpact.h>++#include <new>++b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2CircleContact));+ return new (mem) b2CircleContact(fixtureA, fixtureB);+}++void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2CircleContact*)contact)->~b2CircleContact();+ allocator->Free(contact, sizeof(b2CircleContact));+}++b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)+ : b2Contact(fixtureA, 0, fixtureB, 0)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);+}++void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2CollideCircles(manifold,+ (b2CircleShape*)m_fixtureA->GetShape(), xfA,+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.hnew file mode 100644index 00000000..aac1f0bd--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h@@ -0,0 +1,39 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CIRCLE_CONTACT_H+#define B2_CIRCLE_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2CircleContact : public b2Contact+{+public:+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);+ ~b2CircleContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.cppnew file mode 100644index 00000000..8d0104e8--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp@@ -0,0 +1,247 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/Contacts/b2CircleContact.h>+#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>+#include <Box2D/Dynamics/Contacts/b2PolygonContact.h>+#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>+#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>+#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>+#include <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>+#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>++#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/b2TimeOfImpact.h>+#include <Box2D/Collision/Shapes/b2Shape.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2World.h>++b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];+bool b2Contact::s_initialized = false;++void b2Contact::InitializeRegisters()+{+ AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);+ AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);+ AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);+ AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);+ AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);+ AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle);+ AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon);+}++void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,+ b2Shape::Type type1, b2Shape::Type type2)+{+ b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);+ b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);++ s_registers[type1][type2].createFcn = createFcn;+ s_registers[type1][type2].destroyFcn = destoryFcn;+ s_registers[type1][type2].primary = true;++ if (type1 != type2)+ {+ s_registers[type2][type1].createFcn = createFcn;+ s_registers[type2][type1].destroyFcn = destoryFcn;+ s_registers[type2][type1].primary = false;+ }+}++b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)+{+ if (s_initialized == false)+ {+ InitializeRegisters();+ s_initialized = true;+ }++ b2Shape::Type type1 = fixtureA->GetType();+ b2Shape::Type type2 = fixtureB->GetType();++ b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);+ b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);++ b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;+ if (createFcn)+ {+ if (s_registers[type1][type2].primary)+ {+ return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);+ }+ else+ {+ return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);+ }+ }+ else+ {+ return NULL;+ }+}++void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ b2Assert(s_initialized == true);++ b2Fixture* fixtureA = contact->m_fixtureA;+ b2Fixture* fixtureB = contact->m_fixtureB;++ if (contact->m_manifold.pointCount > 0 &&+ fixtureA->IsSensor() == false &&+ fixtureB->IsSensor() == false)+ {+ fixtureA->GetBody()->SetAwake(true);+ fixtureB->GetBody()->SetAwake(true);+ }++ b2Shape::Type typeA = fixtureA->GetType();+ b2Shape::Type typeB = fixtureB->GetType();++ b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);+ b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);++ b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;+ destroyFcn(contact, allocator);+}++b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)+{+ m_flags = e_enabledFlag;++ m_fixtureA = fA;+ m_fixtureB = fB;++ m_indexA = indexA;+ m_indexB = indexB;++ m_manifold.pointCount = 0;++ m_prev = NULL;+ m_next = NULL;++ m_nodeA.contact = NULL;+ m_nodeA.prev = NULL;+ m_nodeA.next = NULL;+ m_nodeA.other = NULL;++ m_nodeB.contact = NULL;+ m_nodeB.prev = NULL;+ m_nodeB.next = NULL;+ m_nodeB.other = NULL;++ m_toiCount = 0;++ m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);+ m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);++ m_tangentSpeed = 0.0f;+}++// Update the contact manifold and touching status.+// Note: do not assume the fixture AABBs are overlapping or are valid.+void b2Contact::Update(b2ContactListener* listener)+{+ b2Manifold oldManifold = m_manifold;++ // Re-enable this contact.+ m_flags |= e_enabledFlag;++ bool touching = false;+ bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;++ bool sensorA = m_fixtureA->IsSensor();+ bool sensorB = m_fixtureB->IsSensor();+ bool sensor = sensorA || sensorB;++ b2Body* bodyA = m_fixtureA->GetBody();+ b2Body* bodyB = m_fixtureB->GetBody();+ const b2Transform& xfA = bodyA->GetTransform();+ const b2Transform& xfB = bodyB->GetTransform();++ // Is this contact a sensor?+ if (sensor)+ {+ const b2Shape* shapeA = m_fixtureA->GetShape();+ const b2Shape* shapeB = m_fixtureB->GetShape();+ touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);++ // Sensors don't generate manifolds.+ m_manifold.pointCount = 0;+ }+ else+ {+ Evaluate(&m_manifold, xfA, xfB);+ touching = m_manifold.pointCount > 0;++ // Match old contact ids to new contact ids and copy the+ // stored impulses to warm start the solver.+ for (int32 i = 0; i < m_manifold.pointCount; ++i)+ {+ b2ManifoldPoint* mp2 = m_manifold.points + i;+ mp2->normalImpulse = 0.0f;+ mp2->tangentImpulse = 0.0f;+ b2ContactID id2 = mp2->id;++ for (int32 j = 0; j < oldManifold.pointCount; ++j)+ {+ b2ManifoldPoint* mp1 = oldManifold.points + j;++ if (mp1->id.key == id2.key)+ {+ mp2->normalImpulse = mp1->normalImpulse;+ mp2->tangentImpulse = mp1->tangentImpulse;+ break;+ }+ }+ }++ if (touching != wasTouching)+ {+ bodyA->SetAwake(true);+ bodyB->SetAwake(true);+ }+ }++ if (touching)+ {+ m_flags |= e_touchingFlag;+ }+ else+ {+ m_flags &= ~e_touchingFlag;+ }++ if (wasTouching == false && touching == true && listener)+ {+ listener->BeginContact(this);+ }++ if (wasTouching == true && touching == false && listener)+ {+ listener->EndContact(this);+ }++ if (sensor == false && touching && listener)+ {+ listener->PreSolve(this, &oldManifold);+ }+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.hnew file mode 100644index 00000000..347da31e--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.h@@ -0,0 +1,349 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CONTACT_H+#define B2_CONTACT_H++#include <Box2D/Common/b2Math.h>+#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/Shapes/b2Shape.h>+#include <Box2D/Dynamics/b2Fixture.h>++class b2Body;+class b2Contact;+class b2Fixture;+class b2World;+class b2BlockAllocator;+class b2StackAllocator;+class b2ContactListener;++/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero.+/// For example, anything slides on ice.+inline float32 b2MixFriction(float32 friction1, float32 friction2)+{+ return b2Sqrt(friction1 * friction2);+}++/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.+/// For example, a superball bounces on anything.+inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)+{+ return restitution1 > restitution2 ? restitution1 : restitution2;+}++typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB,+ b2BlockAllocator* allocator);+typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);++struct b2ContactRegister+{+ b2ContactCreateFcn* createFcn;+ b2ContactDestroyFcn* destroyFcn;+ bool primary;+};++/// A contact edge is used to connect bodies and contacts together+/// in a contact graph where each body is a node and each contact+/// is an edge. A contact edge belongs to a doubly linked list+/// maintained in each attached body. Each contact has two contact+/// nodes, one for each attached body.+struct b2ContactEdge+{+ b2Body* other; ///< provides quick access to the other body attached.+ b2Contact* contact; ///< the contact+ b2ContactEdge* prev; ///< the previous contact edge in the body's contact list+ b2ContactEdge* next; ///< the next contact edge in the body's contact list+};++/// The class manages contact between two shapes. A contact exists for each overlapping+/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist+/// that has no contact points.+class b2Contact+{+public:++ /// Get the contact manifold. Do not modify the manifold unless you understand the+ /// internals of Box2D.+ b2Manifold* GetManifold();+ const b2Manifold* GetManifold() const;++ /// Get the world manifold.+ void GetWorldManifold(b2WorldManifold* worldManifold) const;++ /// Is this contact touching?+ bool IsTouching() const;++ /// Enable/disable this contact. This can be used inside the pre-solve+ /// contact listener. The contact is only disabled for the current+ /// time step (or sub-step in continuous collisions).+ void SetEnabled(bool flag);++ /// Has this contact been disabled?+ bool IsEnabled() const;++ /// Get the next contact in the world's contact list.+ b2Contact* GetNext();+ const b2Contact* GetNext() const;++ /// Get fixture A in this contact.+ b2Fixture* GetFixtureA();+ const b2Fixture* GetFixtureA() const;++ /// Get the child primitive index for fixture A.+ int32 GetChildIndexA() const;++ /// Get fixture B in this contact.+ b2Fixture* GetFixtureB();+ const b2Fixture* GetFixtureB() const;++ /// Get the child primitive index for fixture B.+ int32 GetChildIndexB() const;++ /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.+ /// This value persists until set or reset.+ void SetFriction(float32 friction);++ /// Get the friction.+ float32 GetFriction() const;++ /// Reset the friction mixture to the default value.+ void ResetFriction();++ /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.+ /// The value persists until you set or reset.+ void SetRestitution(float32 restitution);++ /// Get the restitution.+ float32 GetRestitution() const;++ /// Reset the restitution to the default value.+ void ResetRestitution();++ /// Set the desired tangent speed for a conveyor belt behavior. In meters per second.+ void SetTangentSpeed(float32 speed);++ /// Get the desired tangent speed. In meters per second.+ float32 GetTangentSpeed() const;++ /// Evaluate this contact with your own manifold and transforms.+ virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;++protected:+ friend class b2ContactManager;+ friend class b2World;+ friend class b2ContactSolver;+ friend class b2Body;+ friend class b2Fixture;++ // Flags stored in m_flags+ enum+ {+ // Used when crawling contact graph when forming islands.+ e_islandFlag = 0x0001,++ // Set when the shapes are touching.+ e_touchingFlag = 0x0002,++ // This contact can be disabled (by user)+ e_enabledFlag = 0x0004,++ // This contact needs filtering because a fixture filter was changed.+ e_filterFlag = 0x0008,++ // This bullet contact had a TOI event+ e_bulletHitFlag = 0x0010,++ // This contact has a valid TOI in m_toi+ e_toiFlag = 0x0020+ };++ /// Flag this contact for filtering. Filtering will occur the next time step.+ void FlagForFiltering();++ static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,+ b2Shape::Type typeA, b2Shape::Type typeB);+ static void InitializeRegisters();+ static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {}+ b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);+ virtual ~b2Contact() {}++ void Update(b2ContactListener* listener);++ static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];+ static bool s_initialized;++ uint32 m_flags;++ // World pool and list pointers.+ b2Contact* m_prev;+ b2Contact* m_next;++ // Nodes for connecting bodies.+ b2ContactEdge m_nodeA;+ b2ContactEdge m_nodeB;++ b2Fixture* m_fixtureA;+ b2Fixture* m_fixtureB;++ int32 m_indexA;+ int32 m_indexB;++ b2Manifold m_manifold;++ int32 m_toiCount;+ float32 m_toi;++ float32 m_friction;+ float32 m_restitution;++ float32 m_tangentSpeed;+};++inline b2Manifold* b2Contact::GetManifold()+{+ return &m_manifold;+}++inline const b2Manifold* b2Contact::GetManifold() const+{+ return &m_manifold;+}++inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const+{+ const b2Body* bodyA = m_fixtureA->GetBody();+ const b2Body* bodyB = m_fixtureB->GetBody();+ const b2Shape* shapeA = m_fixtureA->GetShape();+ const b2Shape* shapeB = m_fixtureB->GetShape();++ worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);+}++inline void b2Contact::SetEnabled(bool flag)+{+ if (flag)+ {+ m_flags |= e_enabledFlag;+ }+ else+ {+ m_flags &= ~e_enabledFlag;+ }+}++inline bool b2Contact::IsEnabled() const+{+ return (m_flags & e_enabledFlag) == e_enabledFlag;+}++inline bool b2Contact::IsTouching() const+{+ return (m_flags & e_touchingFlag) == e_touchingFlag;+}++inline b2Contact* b2Contact::GetNext()+{+ return m_next;+}++inline const b2Contact* b2Contact::GetNext() const+{+ return m_next;+}++inline b2Fixture* b2Contact::GetFixtureA()+{+ return m_fixtureA;+}++inline const b2Fixture* b2Contact::GetFixtureA() const+{+ return m_fixtureA;+}++inline b2Fixture* b2Contact::GetFixtureB()+{+ return m_fixtureB;+}++inline int32 b2Contact::GetChildIndexA() const+{+ return m_indexA;+}++inline const b2Fixture* b2Contact::GetFixtureB() const+{+ return m_fixtureB;+}++inline int32 b2Contact::GetChildIndexB() const+{+ return m_indexB;+}++inline void b2Contact::FlagForFiltering()+{+ m_flags |= e_filterFlag;+}++inline void b2Contact::SetFriction(float32 friction)+{+ m_friction = friction;+}++inline float32 b2Contact::GetFriction() const+{+ return m_friction;+}++inline void b2Contact::ResetFriction()+{+ m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);+}++inline void b2Contact::SetRestitution(float32 restitution)+{+ m_restitution = restitution;+}++inline float32 b2Contact::GetRestitution() const+{+ return m_restitution;+}++inline void b2Contact::ResetRestitution()+{+ m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);+}++inline void b2Contact::SetTangentSpeed(float32 speed)+{+ m_tangentSpeed = speed;+}++inline float32 b2Contact::GetTangentSpeed() const+{+ return m_tangentSpeed;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cppnew file mode 100644index 00000000..06cff524--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp@@ -0,0 +1,838 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>++#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Common/b2StackAllocator.h>++#define B2_DEBUG_SOLVER 0++bool g_blockSolve = true;++struct b2ContactPositionConstraint+{+ b2Vec2 localPoints[b2_maxManifoldPoints];+ b2Vec2 localNormal;+ b2Vec2 localPoint;+ int32 indexA;+ int32 indexB;+ float32 invMassA, invMassB;+ b2Vec2 localCenterA, localCenterB;+ float32 invIA, invIB;+ b2Manifold::Type type;+ float32 radiusA, radiusB;+ int32 pointCount;+};++b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)+{+ m_step = def->step;+ m_allocator = def->allocator;+ m_count = def->count;+ m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));+ m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));+ m_positions = def->positions;+ m_velocities = def->velocities;+ m_contacts = def->contacts;++ // Initialize position independent portions of the constraints.+ for (int32 i = 0; i < m_count; ++i)+ {+ b2Contact* contact = m_contacts[i];++ b2Fixture* fixtureA = contact->m_fixtureA;+ b2Fixture* fixtureB = contact->m_fixtureB;+ b2Shape* shapeA = fixtureA->GetShape();+ b2Shape* shapeB = fixtureB->GetShape();+ float32 radiusA = shapeA->m_radius;+ float32 radiusB = shapeB->m_radius;+ b2Body* bodyA = fixtureA->GetBody();+ b2Body* bodyB = fixtureB->GetBody();+ b2Manifold* manifold = contact->GetManifold();++ int32 pointCount = manifold->pointCount;+ b2Assert(pointCount > 0);++ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;+ vc->friction = contact->m_friction;+ vc->restitution = contact->m_restitution;+ vc->tangentSpeed = contact->m_tangentSpeed;+ vc->indexA = bodyA->m_islandIndex;+ vc->indexB = bodyB->m_islandIndex;+ vc->invMassA = bodyA->m_invMass;+ vc->invMassB = bodyB->m_invMass;+ vc->invIA = bodyA->m_invI;+ vc->invIB = bodyB->m_invI;+ vc->contactIndex = i;+ vc->pointCount = pointCount;+ vc->K.SetZero();+ vc->normalMass.SetZero();++ b2ContactPositionConstraint* pc = m_positionConstraints + i;+ pc->indexA = bodyA->m_islandIndex;+ pc->indexB = bodyB->m_islandIndex;+ pc->invMassA = bodyA->m_invMass;+ pc->invMassB = bodyB->m_invMass;+ pc->localCenterA = bodyA->m_sweep.localCenter;+ pc->localCenterB = bodyB->m_sweep.localCenter;+ pc->invIA = bodyA->m_invI;+ pc->invIB = bodyB->m_invI;+ pc->localNormal = manifold->localNormal;+ pc->localPoint = manifold->localPoint;+ pc->pointCount = pointCount;+ pc->radiusA = radiusA;+ pc->radiusB = radiusB;+ pc->type = manifold->type;++ for (int32 j = 0; j < pointCount; ++j)+ {+ b2ManifoldPoint* cp = manifold->points + j;+ b2VelocityConstraintPoint* vcp = vc->points + j;++ if (m_step.warmStarting)+ {+ vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;+ vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;+ }+ else+ {+ vcp->normalImpulse = 0.0f;+ vcp->tangentImpulse = 0.0f;+ }++ vcp->rA.SetZero();+ vcp->rB.SetZero();+ vcp->normalMass = 0.0f;+ vcp->tangentMass = 0.0f;+ vcp->velocityBias = 0.0f;++ pc->localPoints[j] = cp->localPoint;+ }+ }+}++b2ContactSolver::~b2ContactSolver()+{+ m_allocator->Free(m_velocityConstraints);+ m_allocator->Free(m_positionConstraints);+}++// Initialize position dependent portions of the velocity constraints.+void b2ContactSolver::InitializeVelocityConstraints()+{+ for (int32 i = 0; i < m_count; ++i)+ {+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;+ b2ContactPositionConstraint* pc = m_positionConstraints + i;++ float32 radiusA = pc->radiusA;+ float32 radiusB = pc->radiusB;+ b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();++ int32 indexA = vc->indexA;+ int32 indexB = vc->indexB;++ float32 mA = vc->invMassA;+ float32 mB = vc->invMassB;+ float32 iA = vc->invIA;+ float32 iB = vc->invIB;+ b2Vec2 localCenterA = pc->localCenterA;+ b2Vec2 localCenterB = pc->localCenterB;++ b2Vec2 cA = m_positions[indexA].c;+ float32 aA = m_positions[indexA].a;+ b2Vec2 vA = m_velocities[indexA].v;+ float32 wA = m_velocities[indexA].w;++ b2Vec2 cB = m_positions[indexB].c;+ float32 aB = m_positions[indexB].a;+ b2Vec2 vB = m_velocities[indexB].v;+ float32 wB = m_velocities[indexB].w;++ b2Assert(manifold->pointCount > 0);++ b2Transform xfA, xfB;+ xfA.q.Set(aA);+ xfB.q.Set(aB);+ xfA.p = cA - b2Mul(xfA.q, localCenterA);+ xfB.p = cB - b2Mul(xfB.q, localCenterB);++ b2WorldManifold worldManifold;+ worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);++ vc->normal = worldManifold.normal;++ int32 pointCount = vc->pointCount;+ for (int32 j = 0; j < pointCount; ++j)+ {+ b2VelocityConstraintPoint* vcp = vc->points + j;++ vcp->rA = worldManifold.points[j] - cA;+ vcp->rB = worldManifold.points[j] - cB;++ float32 rnA = b2Cross(vcp->rA, vc->normal);+ float32 rnB = b2Cross(vcp->rB, vc->normal);++ float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;++ vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;++ b2Vec2 tangent = b2Cross(vc->normal, 1.0f);++ float32 rtA = b2Cross(vcp->rA, tangent);+ float32 rtB = b2Cross(vcp->rB, tangent);++ float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;++ vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;++ // Setup a velocity bias for restitution.+ vcp->velocityBias = 0.0f;+ float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));+ if (vRel < -b2_velocityThreshold)+ {+ vcp->velocityBias = -vc->restitution * vRel;+ }+ }++ // If we have two points, then prepare the block solver.+ if (vc->pointCount == 2 && g_blockSolve)+ {+ b2VelocityConstraintPoint* vcp1 = vc->points + 0;+ b2VelocityConstraintPoint* vcp2 = vc->points + 1;++ float32 rn1A = b2Cross(vcp1->rA, vc->normal);+ float32 rn1B = b2Cross(vcp1->rB, vc->normal);+ float32 rn2A = b2Cross(vcp2->rA, vc->normal);+ float32 rn2B = b2Cross(vcp2->rB, vc->normal);++ float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;+ float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;+ float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;++ // Ensure a reasonable condition number.+ const float32 k_maxConditionNumber = 1000.0f;+ if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))+ {+ // K is safe to invert.+ vc->K.ex.Set(k11, k12);+ vc->K.ey.Set(k12, k22);+ vc->normalMass = vc->K.GetInverse();+ }+ else+ {+ // The constraints are redundant, just use one.+ // TODO_ERIN use deepest?+ vc->pointCount = 1;+ }+ }+ }+}++void b2ContactSolver::WarmStart()+{+ // Warm start.+ for (int32 i = 0; i < m_count; ++i)+ {+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;++ int32 indexA = vc->indexA;+ int32 indexB = vc->indexB;+ float32 mA = vc->invMassA;+ float32 iA = vc->invIA;+ float32 mB = vc->invMassB;+ float32 iB = vc->invIB;+ int32 pointCount = vc->pointCount;++ b2Vec2 vA = m_velocities[indexA].v;+ float32 wA = m_velocities[indexA].w;+ b2Vec2 vB = m_velocities[indexB].v;+ float32 wB = m_velocities[indexB].w;++ b2Vec2 normal = vc->normal;+ b2Vec2 tangent = b2Cross(normal, 1.0f);++ for (int32 j = 0; j < pointCount; ++j)+ {+ b2VelocityConstraintPoint* vcp = vc->points + j;+ b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;+ wA -= iA * b2Cross(vcp->rA, P);+ vA -= mA * P;+ wB += iB * b2Cross(vcp->rB, P);+ vB += mB * P;+ }++ m_velocities[indexA].v = vA;+ m_velocities[indexA].w = wA;+ m_velocities[indexB].v = vB;+ m_velocities[indexB].w = wB;+ }+}++void b2ContactSolver::SolveVelocityConstraints()+{+ for (int32 i = 0; i < m_count; ++i)+ {+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;++ int32 indexA = vc->indexA;+ int32 indexB = vc->indexB;+ float32 mA = vc->invMassA;+ float32 iA = vc->invIA;+ float32 mB = vc->invMassB;+ float32 iB = vc->invIB;+ int32 pointCount = vc->pointCount;++ b2Vec2 vA = m_velocities[indexA].v;+ float32 wA = m_velocities[indexA].w;+ b2Vec2 vB = m_velocities[indexB].v;+ float32 wB = m_velocities[indexB].w;++ b2Vec2 normal = vc->normal;+ b2Vec2 tangent = b2Cross(normal, 1.0f);+ float32 friction = vc->friction;++ b2Assert(pointCount == 1 || pointCount == 2);++ // Solve tangent constraints first because non-penetration is more important+ // than friction.+ for (int32 j = 0; j < pointCount; ++j)+ {+ b2VelocityConstraintPoint* vcp = vc->points + j;++ // Relative velocity at contact+ b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);++ // Compute tangent force+ float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;+ float32 lambda = vcp->tangentMass * (-vt);++ // b2Clamp the accumulated force+ float32 maxFriction = friction * vcp->normalImpulse;+ float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);+ lambda = newImpulse - vcp->tangentImpulse;+ vcp->tangentImpulse = newImpulse;++ // Apply contact impulse+ b2Vec2 P = lambda * tangent;++ vA -= mA * P;+ wA -= iA * b2Cross(vcp->rA, P);++ vB += mB * P;+ wB += iB * b2Cross(vcp->rB, P);+ }++ // Solve normal constraints+ if (pointCount == 1 || g_blockSolve == false)+ {+ for (int32 i = 0; i < pointCount; ++i)+ {+ b2VelocityConstraintPoint* vcp = vc->points + i;++ // Relative velocity at contact+ b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);++ // Compute normal impulse+ float32 vn = b2Dot(dv, normal);+ float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);++ // b2Clamp the accumulated impulse+ float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);+ lambda = newImpulse - vcp->normalImpulse;+ vcp->normalImpulse = newImpulse;++ // Apply contact impulse+ b2Vec2 P = lambda * normal;+ vA -= mA * P;+ wA -= iA * b2Cross(vcp->rA, P);++ vB += mB * P;+ wB += iB * b2Cross(vcp->rB, P);+ }+ }+ else+ {+ // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).+ // Build the mini LCP for this contact patch+ //+ // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2+ //+ // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )+ // b = vn0 - velocityBias+ //+ // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i+ // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases+ // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid+ // solution that satisfies the problem is chosen.+ //+ // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires+ // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).+ //+ // Substitute:+ //+ // x = a + d+ //+ // a := old total impulse+ // x := new total impulse+ // d := incremental impulse+ //+ // For the current iteration we extend the formula for the incremental impulse+ // to compute the new total impulse:+ //+ // vn = A * d + b+ // = A * (x - a) + b+ // = A * x + b - A * a+ // = A * x + b'+ // b' = b - A * a;++ b2VelocityConstraintPoint* cp1 = vc->points + 0;+ b2VelocityConstraintPoint* cp2 = vc->points + 1;++ b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);+ b2Assert(a.x >= 0.0f && a.y >= 0.0f);++ // Relative velocity at contact+ b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);+ b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);++ // Compute normal velocity+ float32 vn1 = b2Dot(dv1, normal);+ float32 vn2 = b2Dot(dv2, normal);++ b2Vec2 b;+ b.x = vn1 - cp1->velocityBias;+ b.y = vn2 - cp2->velocityBias;++ // Compute b'+ b -= b2Mul(vc->K, a);++ const float32 k_errorTol = 1e-3f;+ B2_NOT_USED(k_errorTol);++ for (;;)+ {+ //+ // Case 1: vn = 0+ //+ // 0 = A * x + b'+ //+ // Solve for x:+ //+ // x = - inv(A) * b'+ //+ b2Vec2 x = - b2Mul(vc->normalMass, b);++ if (x.x >= 0.0f && x.y >= 0.0f)+ {+ // Get the incremental impulse+ b2Vec2 d = x - a;++ // Apply incremental impulse+ b2Vec2 P1 = d.x * normal;+ b2Vec2 P2 = d.y * normal;+ vA -= mA * (P1 + P2);+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));++ vB += mB * (P1 + P2);+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));++ // Accumulate+ cp1->normalImpulse = x.x;+ cp2->normalImpulse = x.y;++#if B2_DEBUG_SOLVER == 1+ // Postconditions+ dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);+ dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);++ // Compute normal velocity+ vn1 = b2Dot(dv1, normal);+ vn2 = b2Dot(dv2, normal);++ b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);+ b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);+#endif+ break;+ }++ //+ // Case 2: vn1 = 0 and x2 = 0+ //+ // 0 = a11 * x1 + a12 * 0 + b1'+ // vn2 = a21 * x1 + a22 * 0 + b2'+ //+ x.x = - cp1->normalMass * b.x;+ x.y = 0.0f;+ vn1 = 0.0f;+ vn2 = vc->K.ex.y * x.x + b.y;++ if (x.x >= 0.0f && vn2 >= 0.0f)+ {+ // Get the incremental impulse+ b2Vec2 d = x - a;++ // Apply incremental impulse+ b2Vec2 P1 = d.x * normal;+ b2Vec2 P2 = d.y * normal;+ vA -= mA * (P1 + P2);+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));++ vB += mB * (P1 + P2);+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));++ // Accumulate+ cp1->normalImpulse = x.x;+ cp2->normalImpulse = x.y;++#if B2_DEBUG_SOLVER == 1+ // Postconditions+ dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);++ // Compute normal velocity+ vn1 = b2Dot(dv1, normal);++ b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);+#endif+ break;+ }+++ //+ // Case 3: vn2 = 0 and x1 = 0+ //+ // vn1 = a11 * 0 + a12 * x2 + b1'+ // 0 = a21 * 0 + a22 * x2 + b2'+ //+ x.x = 0.0f;+ x.y = - cp2->normalMass * b.y;+ vn1 = vc->K.ey.x * x.y + b.x;+ vn2 = 0.0f;++ if (x.y >= 0.0f && vn1 >= 0.0f)+ {+ // Resubstitute for the incremental impulse+ b2Vec2 d = x - a;++ // Apply incremental impulse+ b2Vec2 P1 = d.x * normal;+ b2Vec2 P2 = d.y * normal;+ vA -= mA * (P1 + P2);+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));++ vB += mB * (P1 + P2);+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));++ // Accumulate+ cp1->normalImpulse = x.x;+ cp2->normalImpulse = x.y;++#if B2_DEBUG_SOLVER == 1+ // Postconditions+ dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);++ // Compute normal velocity+ vn2 = b2Dot(dv2, normal);++ b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);+#endif+ break;+ }++ //+ // Case 4: x1 = 0 and x2 = 0+ //+ // vn1 = b1+ // vn2 = b2;+ x.x = 0.0f;+ x.y = 0.0f;+ vn1 = b.x;+ vn2 = b.y;++ if (vn1 >= 0.0f && vn2 >= 0.0f )+ {+ // Resubstitute for the incremental impulse+ b2Vec2 d = x - a;++ // Apply incremental impulse+ b2Vec2 P1 = d.x * normal;+ b2Vec2 P2 = d.y * normal;+ vA -= mA * (P1 + P2);+ wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));++ vB += mB * (P1 + P2);+ wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));++ // Accumulate+ cp1->normalImpulse = x.x;+ cp2->normalImpulse = x.y;++ break;+ }++ // No solution, give up. This is hit sometimes, but it doesn't seem to matter.+ break;+ }+ }++ m_velocities[indexA].v = vA;+ m_velocities[indexA].w = wA;+ m_velocities[indexB].v = vB;+ m_velocities[indexB].w = wB;+ }+}++void b2ContactSolver::StoreImpulses()+{+ for (int32 i = 0; i < m_count; ++i)+ {+ b2ContactVelocityConstraint* vc = m_velocityConstraints + i;+ b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();++ for (int32 j = 0; j < vc->pointCount; ++j)+ {+ manifold->points[j].normalImpulse = vc->points[j].normalImpulse;+ manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;+ }+ }+}++struct b2PositionSolverManifold+{+ void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)+ {+ b2Assert(pc->pointCount > 0);++ switch (pc->type)+ {+ case b2Manifold::e_circles:+ {+ b2Vec2 pointA = b2Mul(xfA, pc->localPoint);+ b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);+ normal = pointB - pointA;+ normal.Normalize();+ point = 0.5f * (pointA + pointB);+ separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;+ }+ break;++ case b2Manifold::e_faceA:+ {+ normal = b2Mul(xfA.q, pc->localNormal);+ b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);++ b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);+ separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;+ point = clipPoint;+ }+ break;++ case b2Manifold::e_faceB:+ {+ normal = b2Mul(xfB.q, pc->localNormal);+ b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);++ b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);+ separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;+ point = clipPoint;++ // Ensure normal points from A to B+ normal = -normal;+ }+ break;+ }+ }++ b2Vec2 normal;+ b2Vec2 point;+ float32 separation;+};++// Sequential solver.+bool b2ContactSolver::SolvePositionConstraints()+{+ float32 minSeparation = 0.0f;++ for (int32 i = 0; i < m_count; ++i)+ {+ b2ContactPositionConstraint* pc = m_positionConstraints + i;++ int32 indexA = pc->indexA;+ int32 indexB = pc->indexB;+ b2Vec2 localCenterA = pc->localCenterA;+ float32 mA = pc->invMassA;+ float32 iA = pc->invIA;+ b2Vec2 localCenterB = pc->localCenterB;+ float32 mB = pc->invMassB;+ float32 iB = pc->invIB;+ int32 pointCount = pc->pointCount;++ b2Vec2 cA = m_positions[indexA].c;+ float32 aA = m_positions[indexA].a;++ b2Vec2 cB = m_positions[indexB].c;+ float32 aB = m_positions[indexB].a;++ // Solve normal constraints+ for (int32 j = 0; j < pointCount; ++j)+ {+ b2Transform xfA, xfB;+ xfA.q.Set(aA);+ xfB.q.Set(aB);+ xfA.p = cA - b2Mul(xfA.q, localCenterA);+ xfB.p = cB - b2Mul(xfB.q, localCenterB);++ b2PositionSolverManifold psm;+ psm.Initialize(pc, xfA, xfB, j);+ b2Vec2 normal = psm.normal;++ b2Vec2 point = psm.point;+ float32 separation = psm.separation;++ b2Vec2 rA = point - cA;+ b2Vec2 rB = point - cB;++ // Track max constraint error.+ minSeparation = b2Min(minSeparation, separation);++ // Prevent large corrections and allow slop.+ float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);++ // Compute the effective mass.+ float32 rnA = b2Cross(rA, normal);+ float32 rnB = b2Cross(rB, normal);+ float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;++ // Compute normal impulse+ float32 impulse = K > 0.0f ? - C / K : 0.0f;++ b2Vec2 P = impulse * normal;++ cA -= mA * P;+ aA -= iA * b2Cross(rA, P);++ cB += mB * P;+ aB += iB * b2Cross(rB, P);+ }++ m_positions[indexA].c = cA;+ m_positions[indexA].a = aA;++ m_positions[indexB].c = cB;+ m_positions[indexB].a = aB;+ }++ // We can't expect minSpeparation >= -b2_linearSlop because we don't+ // push the separation above -b2_linearSlop.+ return minSeparation >= -3.0f * b2_linearSlop;+}++// Sequential position solver for position constraints.+bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)+{+ float32 minSeparation = 0.0f;++ for (int32 i = 0; i < m_count; ++i)+ {+ b2ContactPositionConstraint* pc = m_positionConstraints + i;++ int32 indexA = pc->indexA;+ int32 indexB = pc->indexB;+ b2Vec2 localCenterA = pc->localCenterA;+ b2Vec2 localCenterB = pc->localCenterB;+ int32 pointCount = pc->pointCount;++ float32 mA = 0.0f;+ float32 iA = 0.0f;+ if (indexA == toiIndexA || indexA == toiIndexB)+ {+ mA = pc->invMassA;+ iA = pc->invIA;+ }++ float32 mB = 0.0f;+ float32 iB = 0.;+ if (indexB == toiIndexA || indexB == toiIndexB)+ {+ mB = pc->invMassB;+ iB = pc->invIB;+ }++ b2Vec2 cA = m_positions[indexA].c;+ float32 aA = m_positions[indexA].a;++ b2Vec2 cB = m_positions[indexB].c;+ float32 aB = m_positions[indexB].a;++ // Solve normal constraints+ for (int32 j = 0; j < pointCount; ++j)+ {+ b2Transform xfA, xfB;+ xfA.q.Set(aA);+ xfB.q.Set(aB);+ xfA.p = cA - b2Mul(xfA.q, localCenterA);+ xfB.p = cB - b2Mul(xfB.q, localCenterB);++ b2PositionSolverManifold psm;+ psm.Initialize(pc, xfA, xfB, j);+ b2Vec2 normal = psm.normal;++ b2Vec2 point = psm.point;+ float32 separation = psm.separation;++ b2Vec2 rA = point - cA;+ b2Vec2 rB = point - cB;++ // Track max constraint error.+ minSeparation = b2Min(minSeparation, separation);++ // Prevent large corrections and allow slop.+ float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);++ // Compute the effective mass.+ float32 rnA = b2Cross(rA, normal);+ float32 rnB = b2Cross(rB, normal);+ float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;++ // Compute normal impulse+ float32 impulse = K > 0.0f ? - C / K : 0.0f;++ b2Vec2 P = impulse * normal;++ cA -= mA * P;+ aA -= iA * b2Cross(rA, P);++ cB += mB * P;+ aB += iB * b2Cross(rB, P);+ }++ m_positions[indexA].c = cA;+ m_positions[indexA].a = aA;++ m_positions[indexB].c = cB;+ m_positions[indexB].a = aB;+ }++ // We can't expect minSpeparation >= -b2_linearSlop because we don't+ // push the separation above -b2_linearSlop.+ return minSeparation >= -1.5f * b2_linearSlop;+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.hnew file mode 100644index 00000000..cb568a33--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h@@ -0,0 +1,95 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CONTACT_SOLVER_H+#define B2_CONTACT_SOLVER_H++#include <Box2D/Common/b2Math.h>+#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Dynamics/b2TimeStep.h>++class b2Contact;+class b2Body;+class b2StackAllocator;+struct b2ContactPositionConstraint;++struct b2VelocityConstraintPoint+{+ b2Vec2 rA;+ b2Vec2 rB;+ float32 normalImpulse;+ float32 tangentImpulse;+ float32 normalMass;+ float32 tangentMass;+ float32 velocityBias;+};++struct b2ContactVelocityConstraint+{+ b2VelocityConstraintPoint points[b2_maxManifoldPoints];+ b2Vec2 normal;+ b2Mat22 normalMass;+ b2Mat22 K;+ int32 indexA;+ int32 indexB;+ float32 invMassA, invMassB;+ float32 invIA, invIB;+ float32 friction;+ float32 restitution;+ float32 tangentSpeed;+ int32 pointCount;+ int32 contactIndex;+};++struct b2ContactSolverDef+{+ b2TimeStep step;+ b2Contact** contacts;+ int32 count;+ b2Position* positions;+ b2Velocity* velocities;+ b2StackAllocator* allocator;+};++class b2ContactSolver+{+public:+ b2ContactSolver(b2ContactSolverDef* def);+ ~b2ContactSolver();++ void InitializeVelocityConstraints();++ void WarmStart();+ void SolveVelocityConstraints();+ void StoreImpulses();++ bool SolvePositionConstraints();+ bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);++ b2TimeStep m_step;+ b2Position* m_positions;+ b2Velocity* m_velocities;+ b2StackAllocator* m_allocator;+ b2ContactPositionConstraint* m_positionConstraints;+ b2ContactVelocityConstraint* m_velocityConstraints;+ b2Contact** m_contacts;+ int m_count;+};++#endif+diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cppnew file mode 100644index 00000000..ee622685--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp@@ -0,0 +1,49 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Dynamics/b2Fixture.h>++#include <new>++b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));+ return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);+}++void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();+ allocator->Free(contact, sizeof(b2EdgeAndCircleContact));+}++b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)+: b2Contact(fixtureA, 0, fixtureB, 0)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);+}++void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2CollideEdgeAndCircle( manifold,+ (b2EdgeShape*)m_fixtureA->GetShape(), xfA,+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.hnew file mode 100644index 00000000..11b61b88--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h@@ -0,0 +1,39 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H+#define B2_EDGE_AND_CIRCLE_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2EdgeAndCircleContact : public b2Contact+{+public:+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);+ ~b2EdgeAndCircleContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cppnew file mode 100644index 00000000..379ce243--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp@@ -0,0 +1,49 @@+/*+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Dynamics/b2Fixture.h>++#include <new>++b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));+ return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);+}++void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();+ allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));+}++b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)+: b2Contact(fixtureA, 0, fixtureB, 0)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);+}++void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2CollideEdgeAndPolygon( manifold,+ (b2EdgeShape*)m_fixtureA->GetShape(), xfA,+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.hnew file mode 100644index 00000000..74b27ee0--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h@@ -0,0 +1,39 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_EDGE_AND_POLYGON_CONTACT_H+#define B2_EDGE_AND_POLYGON_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2EdgeAndPolygonContact : public b2Contact+{+public:+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);+ ~b2EdgeAndPolygonContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cppnew file mode 100644index 00000000..41da9f19--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp@@ -0,0 +1,49 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Dynamics/b2Fixture.h>++#include <new>++b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));+ return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);+}++void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();+ allocator->Free(contact, sizeof(b2PolygonAndCircleContact));+}++b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)+: b2Contact(fixtureA, 0, fixtureB, 0)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);+}++void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2CollidePolygonAndCircle( manifold,+ (b2PolygonShape*)m_fixtureA->GetShape(), xfA,+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.hnew file mode 100644index 00000000..6beca16a--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h@@ -0,0 +1,38 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H+#define B2_POLYGON_AND_CIRCLE_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2PolygonAndCircleContact : public b2Contact+{+public:+ static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);+ ~b2PolygonAndCircleContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cppnew file mode 100644index 00000000..e466190e--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp@@ -0,0 +1,52 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Contacts/b2PolygonContact.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Collision/b2TimeOfImpact.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2WorldCallbacks.h>++#include <new>++b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)+{+ void* mem = allocator->Allocate(sizeof(b2PolygonContact));+ return new (mem) b2PolygonContact(fixtureA, fixtureB);+}++void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)+{+ ((b2PolygonContact*)contact)->~b2PolygonContact();+ allocator->Free(contact, sizeof(b2PolygonContact));+}++b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)+ : b2Contact(fixtureA, 0, fixtureB, 0)+{+ b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);+}++void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)+{+ b2CollidePolygons( manifold,+ (b2PolygonShape*)m_fixtureA->GetShape(), xfA,+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.hnew file mode 100644index 00000000..45932148--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h@@ -0,0 +1,39 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_POLYGON_CONTACT_H+#define B2_POLYGON_CONTACT_H++#include <Box2D/Dynamics/Contacts/b2Contact.h>++class b2BlockAllocator;++class b2PolygonContact : public b2Contact+{+public:+ static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,+ b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);+ static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);++ b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);+ ~b2PolygonContact() {}++ void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cppnew file mode 100644index 00000000..1ced7d9d--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp@@ -0,0 +1,260 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// 1-D constrained system+// m (v2 - v1) = lambda+// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.+// x2 = x1 + h * v2++// 1-D mass-damper-spring system+// m (v2 - v1) + h * d * v2 + h * k *++// C = norm(p2 - p1) - L+// u = (p2 - p1) / norm(p2 - p1)+// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))+// J = [-u -cross(r1, u) u cross(r2, u)]+// K = J * invM * JT+// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2++void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,+ const b2Vec2& anchor1, const b2Vec2& anchor2)+{+ bodyA = b1;+ bodyB = b2;+ localAnchorA = bodyA->GetLocalPoint(anchor1);+ localAnchorB = bodyB->GetLocalPoint(anchor2);+ b2Vec2 d = anchor2 - anchor1;+ length = d.Length();+}++b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;+ m_length = def->length;+ m_frequencyHz = def->frequencyHz;+ m_dampingRatio = def->dampingRatio;+ m_impulse = 0.0f;+ m_gamma = 0.0f;+ m_bias = 0.0f;+}++void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ m_u = cB + m_rB - cA - m_rA;++ // Handle singularity.+ float32 length = m_u.Length();+ if (length > b2_linearSlop)+ {+ m_u *= 1.0f / length;+ }+ else+ {+ m_u.Set(0.0f, 0.0f);+ }++ float32 crAu = b2Cross(m_rA, m_u);+ float32 crBu = b2Cross(m_rB, m_u);+ float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;++ // Compute the effective mass matrix.+ m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;++ if (m_frequencyHz > 0.0f)+ {+ float32 C = length - m_length;++ // Frequency+ float32 omega = 2.0f * b2_pi * m_frequencyHz;++ // Damping coefficient+ float32 d = 2.0f * m_mass * m_dampingRatio * omega;++ // Spring stiffness+ float32 k = m_mass * omega * omega;++ // magic formulas+ float32 h = data.step.dt;+ m_gamma = h * (d + h * k);+ m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;+ m_bias = C * h * k * m_gamma;++ invMass += m_gamma;+ m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;+ }+ else+ {+ m_gamma = 0.0f;+ m_bias = 0.0f;+ }++ if (data.step.warmStarting)+ {+ // Scale the impulse to support a variable time step.+ m_impulse *= data.step.dtRatio;++ b2Vec2 P = m_impulse * m_u;+ vA -= m_invMassA * P;+ wA -= m_invIA * b2Cross(m_rA, P);+ vB += m_invMassB * P;+ wB += m_invIB * b2Cross(m_rB, P);+ }+ else+ {+ m_impulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ // Cdot = dot(u, v + cross(w, r))+ b2Vec2 vpA = vA + b2Cross(wA, m_rA);+ b2Vec2 vpB = vB + b2Cross(wB, m_rB);+ float32 Cdot = b2Dot(m_u, vpB - vpA);++ float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);+ m_impulse += impulse;++ b2Vec2 P = impulse * m_u;+ vA -= m_invMassA * P;+ wA -= m_invIA * b2Cross(m_rA, P);+ vB += m_invMassB * P;+ wB += m_invIB * b2Cross(m_rB, P);++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)+{+ if (m_frequencyHz > 0.0f)+ {+ // There is no position correction for soft distance constraints.+ return true;+ }++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ b2Vec2 u = cB + rB - cA - rA;++ float32 length = u.Normalize();+ float32 C = length - m_length;+ C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);++ float32 impulse = -m_mass * C;+ b2Vec2 P = impulse * u;++ cA -= m_invMassA * P;+ aA -= m_invIA * b2Cross(rA, P);+ cB += m_invMassB * P;+ aB += m_invIB * b2Cross(rB, P);++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return b2Abs(C) < b2_linearSlop;+}++b2Vec2 b2DistanceJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2DistanceJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const+{+ b2Vec2 F = (inv_dt * m_impulse) * m_u;+ return F;+}++float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const+{+ B2_NOT_USED(inv_dt);+ return 0.0f;+}++void b2DistanceJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2DistanceJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.length = %.15lef;\n", m_length);+ b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);+ b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.hnew file mode 100644index 00000000..9d0528b9--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h@@ -0,0 +1,169 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_DISTANCE_JOINT_H+#define B2_DISTANCE_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Distance joint definition. This requires defining an+/// anchor point on both bodies and the non-zero length of the+/// distance joint. The definition uses local anchor points+/// so that the initial configuration can violate the constraint+/// slightly. This helps when saving and loading a game.+/// @warning Do not use a zero or short length.+struct b2DistanceJointDef : public b2JointDef+{+ b2DistanceJointDef()+ {+ type = e_distanceJoint;+ localAnchorA.Set(0.0f, 0.0f);+ localAnchorB.Set(0.0f, 0.0f);+ length = 1.0f;+ frequencyHz = 0.0f;+ dampingRatio = 0.0f;+ }++ /// Initialize the bodies, anchors, and length using the world+ /// anchors.+ void Initialize(b2Body* bodyA, b2Body* bodyB,+ const b2Vec2& anchorA, const b2Vec2& anchorB);++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The natural length between the anchor points.+ float32 length;++ /// The mass-spring-damper frequency in Hertz. A value of 0+ /// disables softness.+ float32 frequencyHz;++ /// The damping ratio. 0 = no damping, 1 = critical damping.+ float32 dampingRatio;+};++/// A distance joint constrains two points on two bodies+/// to remain at a fixed distance from each other. You can view+/// this as a massless, rigid rod.+class b2DistanceJoint : public b2Joint+{+public:++ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ /// Get the reaction force given the inverse time step.+ /// Unit is N.+ b2Vec2 GetReactionForce(float32 inv_dt) const;++ /// Get the reaction torque given the inverse time step.+ /// Unit is N*m. This is always zero for a distance joint.+ float32 GetReactionTorque(float32 inv_dt) const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// Set/get the natural length.+ /// Manipulating the length can lead to non-physical behavior when the frequency is zero.+ void SetLength(float32 length);+ float32 GetLength() const;++ /// Set/get frequency in Hz.+ void SetFrequency(float32 hz);+ float32 GetFrequency() const;++ /// Set/get damping ratio.+ void SetDampingRatio(float32 ratio);+ float32 GetDampingRatio() const;++ /// Dump joint to dmLog+ void Dump();++protected:++ friend class b2Joint;+ b2DistanceJoint(const b2DistanceJointDef* data);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ float32 m_frequencyHz;+ float32 m_dampingRatio;+ float32 m_bias;++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ float32 m_gamma;+ float32 m_impulse;+ float32 m_length;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_u;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ float32 m_mass;+};++inline void b2DistanceJoint::SetLength(float32 length)+{+ m_length = length;+}++inline float32 b2DistanceJoint::GetLength() const+{+ return m_length;+}++inline void b2DistanceJoint::SetFrequency(float32 hz)+{+ m_frequencyHz = hz;+}++inline float32 b2DistanceJoint::GetFrequency() const+{+ return m_frequencyHz;+}++inline void b2DistanceJoint::SetDampingRatio(float32 ratio)+{+ m_dampingRatio = ratio;+}++inline float32 b2DistanceJoint::GetDampingRatio() const+{+ return m_dampingRatio;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cppnew file mode 100644index 00000000..075e72bb--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp@@ -0,0 +1,251 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Point-to-point constraint+// Cdot = v2 - v1+// = v2 + cross(w2, r2) - v1 - cross(w1, r1)+// J = [-I -r1_skew I r2_skew ]+// Identity used:+// w k % (rx i + ry j) = w * (-ry i + rx j)++// Angle constraint+// Cdot = w2 - w1+// J = [0 0 -1 0 0 1]+// K = invI1 + invI2++void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)+{+ bodyA = bA;+ bodyB = bB;+ localAnchorA = bodyA->GetLocalPoint(anchor);+ localAnchorB = bodyB->GetLocalPoint(anchor);+}++b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;++ m_linearImpulse.SetZero();+ m_angularImpulse = 0.0f;++ m_maxForce = def->maxForce;+ m_maxTorque = def->maxTorque;+}++void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ // Compute the effective mass matrix.+ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ // J = [-I -r1_skew I r2_skew]+ // [ 0 -1 0 1]+ // r_skew = [-ry; rx]++ // Matlab+ // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]+ // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]+ // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Mat22 K;+ K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;+ K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;+ K.ey.x = K.ex.y;+ K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;++ m_linearMass = K.GetInverse();++ m_angularMass = iA + iB;+ if (m_angularMass > 0.0f)+ {+ m_angularMass = 1.0f / m_angularMass;+ }++ if (data.step.warmStarting)+ {+ // Scale impulses to support a variable time step.+ m_linearImpulse *= data.step.dtRatio;+ m_angularImpulse *= data.step.dtRatio;++ b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);+ vA -= mA * P;+ wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);+ vB += mB * P;+ wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);+ }+ else+ {+ m_linearImpulse.SetZero();+ m_angularImpulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ float32 h = data.step.dt;++ // Solve angular friction+ {+ float32 Cdot = wB - wA;+ float32 impulse = -m_angularMass * Cdot;++ float32 oldImpulse = m_angularImpulse;+ float32 maxImpulse = h * m_maxTorque;+ m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);+ impulse = m_angularImpulse - oldImpulse;++ wA -= iA * impulse;+ wB += iB * impulse;+ }++ // Solve linear friction+ {+ b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);++ b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);+ b2Vec2 oldImpulse = m_linearImpulse;+ m_linearImpulse += impulse;++ float32 maxImpulse = h * m_maxForce;++ if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)+ {+ m_linearImpulse.Normalize();+ m_linearImpulse *= maxImpulse;+ }++ impulse = m_linearImpulse - oldImpulse;++ vA -= mA * impulse;+ wA -= iA * b2Cross(m_rA, impulse);++ vB += mB * impulse;+ wB += iB * b2Cross(m_rB, impulse);+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)+{+ B2_NOT_USED(data);++ return true;+}++b2Vec2 b2FrictionJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2FrictionJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const+{+ return inv_dt * m_linearImpulse;+}++float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * m_angularImpulse;+}++void b2FrictionJoint::SetMaxForce(float32 force)+{+ b2Assert(b2IsValid(force) && force >= 0.0f);+ m_maxForce = force;+}++float32 b2FrictionJoint::GetMaxForce() const+{+ return m_maxForce;+}++void b2FrictionJoint::SetMaxTorque(float32 torque)+{+ b2Assert(b2IsValid(torque) && torque >= 0.0f);+ m_maxTorque = torque;+}++float32 b2FrictionJoint::GetMaxTorque() const+{+ return m_maxTorque;+}++void b2FrictionJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2FrictionJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);+ b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.hnew file mode 100644index 00000000..785d9ac1--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h@@ -0,0 +1,119 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_FRICTION_JOINT_H+#define B2_FRICTION_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Friction joint definition.+struct b2FrictionJointDef : public b2JointDef+{+ b2FrictionJointDef()+ {+ type = e_frictionJoint;+ localAnchorA.SetZero();+ localAnchorB.SetZero();+ maxForce = 0.0f;+ maxTorque = 0.0f;+ }++ /// Initialize the bodies, anchors, axis, and reference angle using the world+ /// anchor and world axis.+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The maximum friction force in N.+ float32 maxForce;++ /// The maximum friction torque in N-m.+ float32 maxTorque;+};++/// Friction joint. This is used for top-down friction.+/// It provides 2D translational friction and angular friction.+class b2FrictionJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// Set the maximum friction force in N.+ void SetMaxForce(float32 force);++ /// Get the maximum friction force in N.+ float32 GetMaxForce() const;++ /// Set the maximum friction torque in N*m.+ void SetMaxTorque(float32 torque);++ /// Get the maximum friction torque in N*m.+ float32 GetMaxTorque() const;++ /// Dump joint to dmLog+ void Dump();++protected:++ friend class b2Joint;++ b2FrictionJoint(const b2FrictionJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;++ // Solver shared+ b2Vec2 m_linearImpulse;+ float32 m_angularImpulse;+ float32 m_maxForce;+ float32 m_maxTorque;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ b2Mat22 m_linearMass;+ float32 m_angularMass;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.cppnew file mode 100644index 00000000..233d972a--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp@@ -0,0 +1,419 @@+/*+* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2GearJoint.h>+#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>+#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Gear Joint:+// C0 = (coordinate1 + ratio * coordinate2)_initial+// C = (coordinate1 + ratio * coordinate2) - C0 = 0+// J = [J1 ratio * J2]+// K = J * invM * JT+// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T+//+// Revolute:+// coordinate = rotation+// Cdot = angularVelocity+// J = [0 0 1]+// K = J * invM * JT = invI+//+// Prismatic:+// coordinate = dot(p - pg, ug)+// Cdot = dot(v + cross(w, r), ug)+// J = [ug cross(r, ug)]+// K = J * invM * JT = invMass + invI * cross(r, ug)^2++b2GearJoint::b2GearJoint(const b2GearJointDef* def)+: b2Joint(def)+{+ m_joint1 = def->joint1;+ m_joint2 = def->joint2;++ m_typeA = m_joint1->GetType();+ m_typeB = m_joint2->GetType();++ b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);+ b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);++ float32 coordinateA, coordinateB;++ // TODO_ERIN there might be some problem with the joint edges in b2Joint.++ m_bodyC = m_joint1->GetBodyA();+ m_bodyA = m_joint1->GetBodyB();++ // Get geometry of joint1+ b2Transform xfA = m_bodyA->m_xf;+ float32 aA = m_bodyA->m_sweep.a;+ b2Transform xfC = m_bodyC->m_xf;+ float32 aC = m_bodyC->m_sweep.a;++ if (m_typeA == e_revoluteJoint)+ {+ b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;+ m_localAnchorC = revolute->m_localAnchorA;+ m_localAnchorA = revolute->m_localAnchorB;+ m_referenceAngleA = revolute->m_referenceAngle;+ m_localAxisC.SetZero();++ coordinateA = aA - aC - m_referenceAngleA;+ }+ else+ {+ b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;+ m_localAnchorC = prismatic->m_localAnchorA;+ m_localAnchorA = prismatic->m_localAnchorB;+ m_referenceAngleA = prismatic->m_referenceAngle;+ m_localAxisC = prismatic->m_localXAxisA;++ b2Vec2 pC = m_localAnchorC;+ b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));+ coordinateA = b2Dot(pA - pC, m_localAxisC);+ }++ m_bodyD = m_joint2->GetBodyA();+ m_bodyB = m_joint2->GetBodyB();++ // Get geometry of joint2+ b2Transform xfB = m_bodyB->m_xf;+ float32 aB = m_bodyB->m_sweep.a;+ b2Transform xfD = m_bodyD->m_xf;+ float32 aD = m_bodyD->m_sweep.a;++ if (m_typeB == e_revoluteJoint)+ {+ b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;+ m_localAnchorD = revolute->m_localAnchorA;+ m_localAnchorB = revolute->m_localAnchorB;+ m_referenceAngleB = revolute->m_referenceAngle;+ m_localAxisD.SetZero();++ coordinateB = aB - aD - m_referenceAngleB;+ }+ else+ {+ b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;+ m_localAnchorD = prismatic->m_localAnchorA;+ m_localAnchorB = prismatic->m_localAnchorB;+ m_referenceAngleB = prismatic->m_referenceAngle;+ m_localAxisD = prismatic->m_localXAxisA;++ b2Vec2 pD = m_localAnchorD;+ b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));+ coordinateB = b2Dot(pB - pD, m_localAxisD);+ }++ m_ratio = def->ratio;++ m_constant = coordinateA + m_ratio * coordinateB;++ m_impulse = 0.0f;+}++void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_indexC = m_bodyC->m_islandIndex;+ m_indexD = m_bodyD->m_islandIndex;+ m_lcA = m_bodyA->m_sweep.localCenter;+ m_lcB = m_bodyB->m_sweep.localCenter;+ m_lcC = m_bodyC->m_sweep.localCenter;+ m_lcD = m_bodyD->m_sweep.localCenter;+ m_mA = m_bodyA->m_invMass;+ m_mB = m_bodyB->m_invMass;+ m_mC = m_bodyC->m_invMass;+ m_mD = m_bodyD->m_invMass;+ m_iA = m_bodyA->m_invI;+ m_iB = m_bodyB->m_invI;+ m_iC = m_bodyC->m_invI;+ m_iD = m_bodyD->m_invI;++ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ float32 aC = data.positions[m_indexC].a;+ b2Vec2 vC = data.velocities[m_indexC].v;+ float32 wC = data.velocities[m_indexC].w;++ float32 aD = data.positions[m_indexD].a;+ b2Vec2 vD = data.velocities[m_indexD].v;+ float32 wD = data.velocities[m_indexD].w;++ b2Rot qA(aA), qB(aB), qC(aC), qD(aD);++ m_mass = 0.0f;++ if (m_typeA == e_revoluteJoint)+ {+ m_JvAC.SetZero();+ m_JwA = 1.0f;+ m_JwC = 1.0f;+ m_mass += m_iA + m_iC;+ }+ else+ {+ b2Vec2 u = b2Mul(qC, m_localAxisC);+ b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);+ m_JvAC = u;+ m_JwC = b2Cross(rC, u);+ m_JwA = b2Cross(rA, u);+ m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;+ }++ if (m_typeB == e_revoluteJoint)+ {+ m_JvBD.SetZero();+ m_JwB = m_ratio;+ m_JwD = m_ratio;+ m_mass += m_ratio * m_ratio * (m_iB + m_iD);+ }+ else+ {+ b2Vec2 u = b2Mul(qD, m_localAxisD);+ b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);+ m_JvBD = m_ratio * u;+ m_JwD = m_ratio * b2Cross(rD, u);+ m_JwB = m_ratio * b2Cross(rB, u);+ m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;+ }++ // Compute effective mass.+ m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;++ if (data.step.warmStarting)+ {+ vA += (m_mA * m_impulse) * m_JvAC;+ wA += m_iA * m_impulse * m_JwA;+ vB += (m_mB * m_impulse) * m_JvBD;+ wB += m_iB * m_impulse * m_JwB;+ vC -= (m_mC * m_impulse) * m_JvAC;+ wC -= m_iC * m_impulse * m_JwC;+ vD -= (m_mD * m_impulse) * m_JvBD;+ wD -= m_iD * m_impulse * m_JwD;+ }+ else+ {+ m_impulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+ data.velocities[m_indexC].v = vC;+ data.velocities[m_indexC].w = wC;+ data.velocities[m_indexD].v = vD;+ data.velocities[m_indexD].w = wD;+}++void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;+ b2Vec2 vC = data.velocities[m_indexC].v;+ float32 wC = data.velocities[m_indexC].w;+ b2Vec2 vD = data.velocities[m_indexD].v;+ float32 wD = data.velocities[m_indexD].w;++ float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);+ Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);++ float32 impulse = -m_mass * Cdot;+ m_impulse += impulse;++ vA += (m_mA * impulse) * m_JvAC;+ wA += m_iA * impulse * m_JwA;+ vB += (m_mB * impulse) * m_JvBD;+ wB += m_iB * impulse * m_JwB;+ vC -= (m_mC * impulse) * m_JvAC;+ wC -= m_iC * impulse * m_JwC;+ vD -= (m_mD * impulse) * m_JvBD;+ wD -= m_iD * impulse * m_JwD;++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+ data.velocities[m_indexC].v = vC;+ data.velocities[m_indexC].w = wC;+ data.velocities[m_indexD].v = vD;+ data.velocities[m_indexD].w = wD;+}++bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 cC = data.positions[m_indexC].c;+ float32 aC = data.positions[m_indexC].a;+ b2Vec2 cD = data.positions[m_indexD].c;+ float32 aD = data.positions[m_indexD].a;++ b2Rot qA(aA), qB(aB), qC(aC), qD(aD);++ float32 linearError = 0.0f;++ float32 coordinateA, coordinateB;++ b2Vec2 JvAC, JvBD;+ float32 JwA, JwB, JwC, JwD;+ float32 mass = 0.0f;++ if (m_typeA == e_revoluteJoint)+ {+ JvAC.SetZero();+ JwA = 1.0f;+ JwC = 1.0f;+ mass += m_iA + m_iC;++ coordinateA = aA - aC - m_referenceAngleA;+ }+ else+ {+ b2Vec2 u = b2Mul(qC, m_localAxisC);+ b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);+ JvAC = u;+ JwC = b2Cross(rC, u);+ JwA = b2Cross(rA, u);+ mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;++ b2Vec2 pC = m_localAnchorC - m_lcC;+ b2Vec2 pA = b2MulT(qC, rA + (cA - cC));+ coordinateA = b2Dot(pA - pC, m_localAxisC);+ }++ if (m_typeB == e_revoluteJoint)+ {+ JvBD.SetZero();+ JwB = m_ratio;+ JwD = m_ratio;+ mass += m_ratio * m_ratio * (m_iB + m_iD);++ coordinateB = aB - aD - m_referenceAngleB;+ }+ else+ {+ b2Vec2 u = b2Mul(qD, m_localAxisD);+ b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);+ JvBD = m_ratio * u;+ JwD = m_ratio * b2Cross(rD, u);+ JwB = m_ratio * b2Cross(rB, u);+ mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;++ b2Vec2 pD = m_localAnchorD - m_lcD;+ b2Vec2 pB = b2MulT(qD, rB + (cB - cD));+ coordinateB = b2Dot(pB - pD, m_localAxisD);+ }++ float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;++ float32 impulse = 0.0f;+ if (mass > 0.0f)+ {+ impulse = -C / mass;+ }++ cA += m_mA * impulse * JvAC;+ aA += m_iA * impulse * JwA;+ cB += m_mB * impulse * JvBD;+ aB += m_iB * impulse * JwB;+ cC -= m_mC * impulse * JvAC;+ aC -= m_iC * impulse * JwC;+ cD -= m_mD * impulse * JvBD;+ aD -= m_iD * impulse * JwD;++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;+ data.positions[m_indexC].c = cC;+ data.positions[m_indexC].a = aC;+ data.positions[m_indexD].c = cD;+ data.positions[m_indexD].a = aD;++ // TODO_ERIN not implemented+ return linearError < b2_linearSlop;+}++b2Vec2 b2GearJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2GearJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const+{+ b2Vec2 P = m_impulse * m_JvAC;+ return inv_dt * P;+}++float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const+{+ float32 L = m_impulse * m_JwA;+ return inv_dt * L;+}++void b2GearJoint::SetRatio(float32 ratio)+{+ b2Assert(b2IsValid(ratio));+ m_ratio = ratio;+}++float32 b2GearJoint::GetRatio() const+{+ return m_ratio;+}++void b2GearJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ int32 index1 = m_joint1->m_index;+ int32 index2 = m_joint2->m_index;++ b2Log(" b2GearJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.joint1 = joints[%d];\n", index1);+ b2Log(" jd.joint2 = joints[%d];\n", index2);+ b2Log(" jd.ratio = %.15lef;\n", m_ratio);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.hnew file mode 100644index 00000000..48cdfd88--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.h@@ -0,0 +1,125 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_GEAR_JOINT_H+#define B2_GEAR_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Gear joint definition. This definition requires two existing+/// revolute or prismatic joints (any combination will work).+struct b2GearJointDef : public b2JointDef+{+ b2GearJointDef()+ {+ type = e_gearJoint;+ joint1 = NULL;+ joint2 = NULL;+ ratio = 1.0f;+ }++ /// The first revolute/prismatic joint attached to the gear joint.+ b2Joint* joint1;++ /// The second revolute/prismatic joint attached to the gear joint.+ b2Joint* joint2;++ /// The gear ratio.+ /// @see b2GearJoint for explanation.+ float32 ratio;+};++/// A gear joint is used to connect two joints together. Either joint+/// can be a revolute or prismatic joint. You specify a gear ratio+/// to bind the motions together:+/// coordinate1 + ratio * coordinate2 = constant+/// The ratio can be negative or positive. If one joint is a revolute joint+/// and the other joint is a prismatic joint, then the ratio will have units+/// of length or units of 1/length.+/// @warning You have to manually destroy the gear joint if joint1 or joint2+/// is destroyed.+class b2GearJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// Get the first joint.+ b2Joint* GetJoint1() { return m_joint1; }++ /// Get the second joint.+ b2Joint* GetJoint2() { return m_joint2; }++ /// Set/Get the gear ratio.+ void SetRatio(float32 ratio);+ float32 GetRatio() const;++ /// Dump joint to dmLog+ void Dump();++protected:++ friend class b2Joint;+ b2GearJoint(const b2GearJointDef* data);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ b2Joint* m_joint1;+ b2Joint* m_joint2;++ b2JointType m_typeA;+ b2JointType m_typeB;++ // Body A is connected to body C+ // Body B is connected to body D+ b2Body* m_bodyC;+ b2Body* m_bodyD;++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ b2Vec2 m_localAnchorC;+ b2Vec2 m_localAnchorD;++ b2Vec2 m_localAxisC;+ b2Vec2 m_localAxisD;++ float32 m_referenceAngleA;+ float32 m_referenceAngleB;++ float32 m_constant;+ float32 m_ratio;++ float32 m_impulse;++ // Solver temp+ int32 m_indexA, m_indexB, m_indexC, m_indexD;+ b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;+ float32 m_mA, m_mB, m_mC, m_mD;+ float32 m_iA, m_iB, m_iC, m_iD;+ b2Vec2 m_JvAC, m_JvBD;+ float32 m_JwA, m_JwB, m_JwC, m_JwD;+ float32 m_mass;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.cppnew file mode 100644index 00000000..50919495--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.cpp@@ -0,0 +1,211 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2Joint.h>+#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>+#include <Box2D/Dynamics/Joints/b2WheelJoint.h>+#include <Box2D/Dynamics/Joints/b2MouseJoint.h>+#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>+#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>+#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>+#include <Box2D/Dynamics/Joints/b2GearJoint.h>+#include <Box2D/Dynamics/Joints/b2WeldJoint.h>+#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>+#include <Box2D/Dynamics/Joints/b2RopeJoint.h>+#include <Box2D/Dynamics/Joints/b2MotorJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Common/b2BlockAllocator.h>++#include <new>++b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)+{+ b2Joint* joint = NULL;++ switch (def->type)+ {+ case e_distanceJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2DistanceJoint));+ joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));+ }+ break;++ case e_mouseJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2MouseJoint));+ joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));+ }+ break;++ case e_prismaticJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));+ joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));+ }+ break;++ case e_revoluteJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));+ joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));+ }+ break;++ case e_pulleyJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2PulleyJoint));+ joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));+ }+ break;++ case e_gearJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2GearJoint));+ joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));+ }+ break;++ case e_wheelJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2WheelJoint));+ joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));+ }+ break;++ case e_weldJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2WeldJoint));+ joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));+ }+ break;++ case e_frictionJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2FrictionJoint));+ joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));+ }+ break;++ case e_ropeJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2RopeJoint));+ joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def));+ }+ break;++ case e_motorJoint:+ {+ void* mem = allocator->Allocate(sizeof(b2MotorJoint));+ joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));+ }+ break;++ default:+ b2Assert(false);+ break;+ }++ return joint;+}++void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)+{+ joint->~b2Joint();+ switch (joint->m_type)+ {+ case e_distanceJoint:+ allocator->Free(joint, sizeof(b2DistanceJoint));+ break;++ case e_mouseJoint:+ allocator->Free(joint, sizeof(b2MouseJoint));+ break;++ case e_prismaticJoint:+ allocator->Free(joint, sizeof(b2PrismaticJoint));+ break;++ case e_revoluteJoint:+ allocator->Free(joint, sizeof(b2RevoluteJoint));+ break;++ case e_pulleyJoint:+ allocator->Free(joint, sizeof(b2PulleyJoint));+ break;++ case e_gearJoint:+ allocator->Free(joint, sizeof(b2GearJoint));+ break;++ case e_wheelJoint:+ allocator->Free(joint, sizeof(b2WheelJoint));+ break;++ case e_weldJoint:+ allocator->Free(joint, sizeof(b2WeldJoint));+ break;++ case e_frictionJoint:+ allocator->Free(joint, sizeof(b2FrictionJoint));+ break;++ case e_ropeJoint:+ allocator->Free(joint, sizeof(b2RopeJoint));+ break;++ case e_motorJoint:+ allocator->Free(joint, sizeof(b2MotorJoint));+ break;++ default:+ b2Assert(false);+ break;+ }+}++b2Joint::b2Joint(const b2JointDef* def)+{+ b2Assert(def->bodyA != def->bodyB);++ m_type = def->type;+ m_prev = NULL;+ m_next = NULL;+ m_bodyA = def->bodyA;+ m_bodyB = def->bodyB;+ m_index = 0;+ m_collideConnected = def->collideConnected;+ m_islandFlag = false;+ m_userData = def->userData;++ m_edgeA.joint = NULL;+ m_edgeA.other = NULL;+ m_edgeA.prev = NULL;+ m_edgeA.next = NULL;++ m_edgeB.joint = NULL;+ m_edgeB.other = NULL;+ m_edgeB.prev = NULL;+ m_edgeB.next = NULL;+}++bool b2Joint::IsActive() const+{+ return m_bodyA->IsActive() && m_bodyB->IsActive();+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.hnew file mode 100644index 00000000..f6002e50--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.h@@ -0,0 +1,226 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_JOINT_H+#define B2_JOINT_H++#include <Box2D/Common/b2Math.h>++class b2Body;+class b2Joint;+struct b2SolverData;+class b2BlockAllocator;++enum b2JointType+{+ e_unknownJoint,+ e_revoluteJoint,+ e_prismaticJoint,+ e_distanceJoint,+ e_pulleyJoint,+ e_mouseJoint,+ e_gearJoint,+ e_wheelJoint,+ e_weldJoint,+ e_frictionJoint,+ e_ropeJoint,+ e_motorJoint+};++enum b2LimitState+{+ e_inactiveLimit,+ e_atLowerLimit,+ e_atUpperLimit,+ e_equalLimits+};++struct b2Jacobian+{+ b2Vec2 linear;+ float32 angularA;+ float32 angularB;+};++/// A joint edge is used to connect bodies and joints together+/// in a joint graph where each body is a node and each joint+/// is an edge. A joint edge belongs to a doubly linked list+/// maintained in each attached body. Each joint has two joint+/// nodes, one for each attached body.+struct b2JointEdge+{+ b2Body* other; ///< provides quick access to the other body attached.+ b2Joint* joint; ///< the joint+ b2JointEdge* prev; ///< the previous joint edge in the body's joint list+ b2JointEdge* next; ///< the next joint edge in the body's joint list+};++/// Joint definitions are used to construct joints.+struct b2JointDef+{+ b2JointDef()+ {+ type = e_unknownJoint;+ userData = NULL;+ bodyA = NULL;+ bodyB = NULL;+ collideConnected = false;+ }++ /// The joint type is set automatically for concrete joint types.+ b2JointType type;++ /// Use this to attach application specific data to your joints.+ void* userData;++ /// The first attached body.+ b2Body* bodyA;++ /// The second attached body.+ b2Body* bodyB;++ /// Set this flag to true if the attached bodies should collide.+ bool collideConnected;+};++/// The base joint class. Joints are used to constraint two bodies together in+/// various fashions. Some joints also feature limits and motors.+class b2Joint+{+public:++ /// Get the type of the concrete joint.+ b2JointType GetType() const;++ /// Get the first body attached to this joint.+ b2Body* GetBodyA();++ /// Get the second body attached to this joint.+ b2Body* GetBodyB();++ /// Get the anchor point on bodyA in world coordinates.+ virtual b2Vec2 GetAnchorA() const = 0;++ /// Get the anchor point on bodyB in world coordinates.+ virtual b2Vec2 GetAnchorB() const = 0;++ /// Get the reaction force on bodyB at the joint anchor in Newtons.+ virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;++ /// Get the reaction torque on bodyB in N*m.+ virtual float32 GetReactionTorque(float32 inv_dt) const = 0;++ /// Get the next joint the world joint list.+ b2Joint* GetNext();+ const b2Joint* GetNext() const;++ /// Get the user data pointer.+ void* GetUserData() const;++ /// Set the user data pointer.+ void SetUserData(void* data);++ /// Short-cut function to determine if either body is inactive.+ bool IsActive() const;++ /// Get collide connected.+ /// Note: modifying the collide connect flag won't work correctly because+ /// the flag is only checked when fixture AABBs begin to overlap.+ bool GetCollideConnected() const;++ /// Dump this joint to the log file.+ virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); }++ /// Shift the origin for any points stored in world coordinates.+ virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); }++protected:+ friend class b2World;+ friend class b2Body;+ friend class b2Island;+ friend class b2GearJoint;++ static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);+ static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);++ b2Joint(const b2JointDef* def);+ virtual ~b2Joint() {}++ virtual void InitVelocityConstraints(const b2SolverData& data) = 0;+ virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;++ // This returns true if the position errors are within tolerance.+ virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;++ b2JointType m_type;+ b2Joint* m_prev;+ b2Joint* m_next;+ b2JointEdge m_edgeA;+ b2JointEdge m_edgeB;+ b2Body* m_bodyA;+ b2Body* m_bodyB;++ int32 m_index;++ bool m_islandFlag;+ bool m_collideConnected;++ void* m_userData;+};++inline b2JointType b2Joint::GetType() const+{+ return m_type;+}++inline b2Body* b2Joint::GetBodyA()+{+ return m_bodyA;+}++inline b2Body* b2Joint::GetBodyB()+{+ return m_bodyB;+}++inline b2Joint* b2Joint::GetNext()+{+ return m_next;+}++inline const b2Joint* b2Joint::GetNext() const+{+ return m_next;+}++inline void* b2Joint::GetUserData() const+{+ return m_userData;+}++inline void b2Joint::SetUserData(void* data)+{+ m_userData = data;+}++inline bool b2Joint::GetCollideConnected() const+{+ return m_collideConnected;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.cppnew file mode 100644index 00000000..a07a6401--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.cpp@@ -0,0 +1,304 @@+/*+* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2MotorJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Point-to-point constraint+// Cdot = v2 - v1+// = v2 + cross(w2, r2) - v1 - cross(w1, r1)+// J = [-I -r1_skew I r2_skew ]+// Identity used:+// w k % (rx i + ry j) = w * (-ry i + rx j)++// Angle constraint+// Cdot = w2 - w1+// J = [0 0 -1 0 0 1]+// K = invI1 + invI2++void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB)+{+ bodyA = bA;+ bodyB = bB;+ b2Vec2 xB = bodyB->GetPosition();+ linearOffset = bodyA->GetLocalPoint(xB);++ float32 angleA = bodyA->GetAngle();+ float32 angleB = bodyB->GetAngle();+ angularOffset = angleB - angleA;+}++b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def)+: b2Joint(def)+{+ m_linearOffset = def->linearOffset;+ m_angularOffset = def->angularOffset;++ m_linearImpulse.SetZero();+ m_angularImpulse = 0.0f;++ m_maxForce = def->maxForce;+ m_maxTorque = def->maxTorque;+ m_correctionFactor = def->correctionFactor;+}++void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ // Compute the effective mass matrix.+ m_rA = b2Mul(qA, -m_localCenterA);+ m_rB = b2Mul(qB, -m_localCenterB);++ // J = [-I -r1_skew I r2_skew]+ // [ 0 -1 0 1]+ // r_skew = [-ry; rx]++ // Matlab+ // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]+ // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]+ // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Mat22 K;+ K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;+ K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;+ K.ey.x = K.ex.y;+ K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;++ m_linearMass = K.GetInverse();++ m_angularMass = iA + iB;+ if (m_angularMass > 0.0f)+ {+ m_angularMass = 1.0f / m_angularMass;+ }++ m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);+ m_angularError = aB - aA - m_angularOffset;++ if (data.step.warmStarting)+ {+ // Scale impulses to support a variable time step.+ m_linearImpulse *= data.step.dtRatio;+ m_angularImpulse *= data.step.dtRatio;++ b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);+ vA -= mA * P;+ wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);+ vB += mB * P;+ wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);+ }+ else+ {+ m_linearImpulse.SetZero();+ m_angularImpulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ float32 h = data.step.dt;+ float32 inv_h = data.step.inv_dt;++ // Solve angular friction+ {+ float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;+ float32 impulse = -m_angularMass * Cdot;++ float32 oldImpulse = m_angularImpulse;+ float32 maxImpulse = h * m_maxTorque;+ m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);+ impulse = m_angularImpulse - oldImpulse;++ wA -= iA * impulse;+ wB += iB * impulse;+ }++ // Solve linear friction+ {+ b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;++ b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);+ b2Vec2 oldImpulse = m_linearImpulse;+ m_linearImpulse += impulse;++ float32 maxImpulse = h * m_maxForce;++ if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)+ {+ m_linearImpulse.Normalize();+ m_linearImpulse *= maxImpulse;+ }++ impulse = m_linearImpulse - oldImpulse;++ vA -= mA * impulse;+ wA -= iA * b2Cross(m_rA, impulse);++ vB += mB * impulse;+ wB += iB * b2Cross(m_rB, impulse);+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data)+{+ B2_NOT_USED(data);++ return true;+}++b2Vec2 b2MotorJoint::GetAnchorA() const+{+ return m_bodyA->GetPosition();+}++b2Vec2 b2MotorJoint::GetAnchorB() const+{+ return m_bodyB->GetPosition();+}++b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const+{+ return inv_dt * m_linearImpulse;+}++float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * m_angularImpulse;+}++void b2MotorJoint::SetMaxForce(float32 force)+{+ b2Assert(b2IsValid(force) && force >= 0.0f);+ m_maxForce = force;+}++float32 b2MotorJoint::GetMaxForce() const+{+ return m_maxForce;+}++void b2MotorJoint::SetMaxTorque(float32 torque)+{+ b2Assert(b2IsValid(torque) && torque >= 0.0f);+ m_maxTorque = torque;+}++float32 b2MotorJoint::GetMaxTorque() const+{+ return m_maxTorque;+}++void b2MotorJoint::SetCorrectionFactor(float32 factor)+{+ b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);+ m_correctionFactor = factor;+}++float32 b2MotorJoint::GetCorrectionFactor() const+{+ return m_correctionFactor;+}++void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset)+{+ if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y)+ {+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_linearOffset = linearOffset;+ }+}++const b2Vec2& b2MotorJoint::GetLinearOffset() const+{+ return m_linearOffset;+}++void b2MotorJoint::SetAngularOffset(float32 angularOffset)+{+ if (angularOffset != m_angularOffset)+ {+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_angularOffset = angularOffset;+ }+}++float32 b2MotorJoint::GetAngularOffset() const+{+ return m_angularOffset;+}++void b2MotorJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2MotorJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y);+ b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset);+ b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);+ b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);+ b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.hnew file mode 100644index 00000000..486225b9--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.h@@ -0,0 +1,133 @@+/*+* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_MOTOR_JOINT_H+#define B2_MOTOR_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Motor joint definition.+struct b2MotorJointDef : public b2JointDef+{+ b2MotorJointDef()+ {+ type = e_motorJoint;+ linearOffset.SetZero();+ angularOffset = 0.0f;+ maxForce = 1.0f;+ maxTorque = 1.0f;+ correctionFactor = 0.3f;+ }++ /// Initialize the bodies and offsets using the current transforms.+ void Initialize(b2Body* bodyA, b2Body* bodyB);++ /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.+ b2Vec2 linearOffset;++ /// The bodyB angle minus bodyA angle in radians.+ float32 angularOffset;++ /// The maximum motor force in N.+ float32 maxForce;++ /// The maximum motor torque in N-m.+ float32 maxTorque;++ /// Position correction factor in the range [0,1].+ float32 correctionFactor;+};++/// A motor joint is used to control the relative motion+/// between two bodies. A typical usage is to control the movement+/// of a dynamic body with respect to the ground.+class b2MotorJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// Set/get the target linear offset, in frame A, in meters.+ void SetLinearOffset(const b2Vec2& linearOffset);+ const b2Vec2& GetLinearOffset() const;++ /// Set/get the target angular offset, in radians.+ void SetAngularOffset(float32 angularOffset);+ float32 GetAngularOffset() const;++ /// Set the maximum friction force in N.+ void SetMaxForce(float32 force);++ /// Get the maximum friction force in N.+ float32 GetMaxForce() const;++ /// Set the maximum friction torque in N*m.+ void SetMaxTorque(float32 torque);++ /// Get the maximum friction torque in N*m.+ float32 GetMaxTorque() const;++ /// Set the position correction factor in the range [0,1].+ void SetCorrectionFactor(float32 factor);++ /// Get the position correction factor in the range [0,1].+ float32 GetCorrectionFactor() const;++ /// Dump to b2Log+ void Dump();++protected:++ friend class b2Joint;++ b2MotorJoint(const b2MotorJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ // Solver shared+ b2Vec2 m_linearOffset;+ float32 m_angularOffset;+ b2Vec2 m_linearImpulse;+ float32 m_angularImpulse;+ float32 m_maxForce;+ float32 m_maxTorque;+ float32 m_correctionFactor;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ b2Vec2 m_linearError;+ float32 m_angularError;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ b2Mat22 m_linearMass;+ float32 m_angularMass;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cppnew file mode 100644index 00000000..eb1de9d3--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp@@ -0,0 +1,222 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2MouseJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// p = attached point, m = mouse point+// C = p - m+// Cdot = v+// = v + cross(w, r)+// J = [I r_skew]+// Identity used:+// w k % (rx i + ry j) = w * (-ry i + rx j)++b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)+: b2Joint(def)+{+ b2Assert(def->target.IsValid());+ b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);+ b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);+ b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);++ m_targetA = def->target;+ m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);++ m_maxForce = def->maxForce;+ m_impulse.SetZero();++ m_frequencyHz = def->frequencyHz;+ m_dampingRatio = def->dampingRatio;++ m_beta = 0.0f;+ m_gamma = 0.0f;+}++void b2MouseJoint::SetTarget(const b2Vec2& target)+{+ if (m_bodyB->IsAwake() == false)+ {+ m_bodyB->SetAwake(true);+ }+ m_targetA = target;+}++const b2Vec2& b2MouseJoint::GetTarget() const+{+ return m_targetA;+}++void b2MouseJoint::SetMaxForce(float32 force)+{+ m_maxForce = force;+}++float32 b2MouseJoint::GetMaxForce() const+{+ return m_maxForce;+}++void b2MouseJoint::SetFrequency(float32 hz)+{+ m_frequencyHz = hz;+}++float32 b2MouseJoint::GetFrequency() const+{+ return m_frequencyHz;+}++void b2MouseJoint::SetDampingRatio(float32 ratio)+{+ m_dampingRatio = ratio;+}++float32 b2MouseJoint::GetDampingRatio() const+{+ return m_dampingRatio;+}++void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassB = m_bodyB->m_invMass;+ m_invIB = m_bodyB->m_invI;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qB(aB);++ float32 mass = m_bodyB->GetMass();++ // Frequency+ float32 omega = 2.0f * b2_pi * m_frequencyHz;++ // Damping coefficient+ float32 d = 2.0f * mass * m_dampingRatio * omega;++ // Spring stiffness+ float32 k = mass * (omega * omega);++ // magic formulas+ // gamma has units of inverse mass.+ // beta has units of inverse time.+ float32 h = data.step.dt;+ b2Assert(d + h * k > b2_epsilon);+ m_gamma = h * (d + h * k);+ if (m_gamma != 0.0f)+ {+ m_gamma = 1.0f / m_gamma;+ }+ m_beta = h * k * m_gamma;++ // Compute the effective mass matrix.+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]+ // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]+ // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]+ b2Mat22 K;+ K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;+ K.ex.y = -m_invIB * m_rB.x * m_rB.y;+ K.ey.x = K.ex.y;+ K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;++ m_mass = K.GetInverse();++ m_C = cB + m_rB - m_targetA;+ m_C *= m_beta;++ // Cheat with some damping+ wB *= 0.98f;++ if (data.step.warmStarting)+ {+ m_impulse *= data.step.dtRatio;+ vB += m_invMassB * m_impulse;+ wB += m_invIB * b2Cross(m_rB, m_impulse);+ }+ else+ {+ m_impulse.SetZero();+ }++ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ // Cdot = v + cross(w, r)+ b2Vec2 Cdot = vB + b2Cross(wB, m_rB);+ b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));++ b2Vec2 oldImpulse = m_impulse;+ m_impulse += impulse;+ float32 maxImpulse = data.step.dt * m_maxForce;+ if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)+ {+ m_impulse *= maxImpulse / m_impulse.Length();+ }+ impulse = m_impulse - oldImpulse;++ vB += m_invMassB * impulse;+ wB += m_invIB * b2Cross(m_rB, impulse);++ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)+{+ B2_NOT_USED(data);+ return true;+}++b2Vec2 b2MouseJoint::GetAnchorA() const+{+ return m_targetA;+}++b2Vec2 b2MouseJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const+{+ return inv_dt * m_impulse;+}++float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * 0.0f;+}++void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)+{+ m_targetA -= newOrigin;+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.hnew file mode 100644index 00000000..aba70ec1--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h@@ -0,0 +1,129 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_MOUSE_JOINT_H+#define B2_MOUSE_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Mouse joint definition. This requires a world target point,+/// tuning parameters, and the time step.+struct b2MouseJointDef : public b2JointDef+{+ b2MouseJointDef()+ {+ type = e_mouseJoint;+ target.Set(0.0f, 0.0f);+ maxForce = 0.0f;+ frequencyHz = 5.0f;+ dampingRatio = 0.7f;+ }++ /// The initial world target point. This is assumed+ /// to coincide with the body anchor initially.+ b2Vec2 target;++ /// The maximum constraint force that can be exerted+ /// to move the candidate body. Usually you will express+ /// as some multiple of the weight (multiplier * mass * gravity).+ float32 maxForce;++ /// The response speed.+ float32 frequencyHz;++ /// The damping ratio. 0 = no damping, 1 = critical damping.+ float32 dampingRatio;+};++/// A mouse joint is used to make a point on a body track a+/// specified world point. This a soft constraint with a maximum+/// force. This allows the constraint to stretch and without+/// applying huge forces.+/// NOTE: this joint is not documented in the manual because it was+/// developed to be used in the testbed. If you want to learn how to+/// use the mouse joint, look at the testbed.+class b2MouseJoint : public b2Joint+{+public:++ /// Implements b2Joint.+ b2Vec2 GetAnchorA() const;++ /// Implements b2Joint.+ b2Vec2 GetAnchorB() const;++ /// Implements b2Joint.+ b2Vec2 GetReactionForce(float32 inv_dt) const;++ /// Implements b2Joint.+ float32 GetReactionTorque(float32 inv_dt) const;++ /// Use this to update the target point.+ void SetTarget(const b2Vec2& target);+ const b2Vec2& GetTarget() const;++ /// Set/get the maximum force in Newtons.+ void SetMaxForce(float32 force);+ float32 GetMaxForce() const;++ /// Set/get the frequency in Hertz.+ void SetFrequency(float32 hz);+ float32 GetFrequency() const;++ /// Set/get the damping ratio (dimensionless).+ void SetDampingRatio(float32 ratio);+ float32 GetDampingRatio() const;++ /// The mouse joint does not support dumping.+ void Dump() { b2Log("Mouse joint dumping is not supported.\n"); }++ /// Implement b2Joint::ShiftOrigin+ void ShiftOrigin(const b2Vec2& newOrigin);++protected:+ friend class b2Joint;++ b2MouseJoint(const b2MouseJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ b2Vec2 m_localAnchorB;+ b2Vec2 m_targetA;+ float32 m_frequencyHz;+ float32 m_dampingRatio;+ float32 m_beta;++ // Solver shared+ b2Vec2 m_impulse;+ float32 m_maxForce;+ float32 m_gamma;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterB;+ float32 m_invMassB;+ float32 m_invIB;+ b2Mat22 m_mass;+ b2Vec2 m_C;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cppnew file mode 100644index 00000000..2da77b84--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp@@ -0,0 +1,629 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Linear constraint (point-to-line)+// d = p2 - p1 = x2 + r2 - x1 - r1+// C = dot(perp, d)+// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))+// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)+// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]+//+// Angular constraint+// C = a2 - a1 + a_initial+// Cdot = w2 - w1+// J = [0 0 -1 0 0 1]+//+// K = J * invM * JT+//+// J = [-a -s1 a s2]+// [0 -1 0 1]+// a = perp+// s1 = cross(d + r1, a) = cross(p2 - x1, a)+// s2 = cross(r2, a) = cross(p2 - x2, a)+++// Motor/Limit linear constraint+// C = dot(ax1, d)+// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]++// Block Solver+// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even+// when the mass has poor distribution (leading to large torques about the joint anchor points).+//+// The Jacobian has 3 rows:+// J = [-uT -s1 uT s2] // linear+// [0 -1 0 1] // angular+// [-vT -a1 vT a2] // limit+//+// u = perp+// v = axis+// s1 = cross(d + r1, u), s2 = cross(r2, u)+// a1 = cross(d + r1, v), a2 = cross(r2, v)++// M * (v2 - v1) = JT * df+// J * v2 = bias+//+// v2 = v1 + invM * JT * df+// J * (v1 + invM * JT * df) = bias+// K * df = bias - J * v1 = -Cdot+// K = J * invM * JT+// Cdot = J * v1 - bias+//+// Now solve for f2.+// df = f2 - f1+// K * (f2 - f1) = -Cdot+// f2 = invK * (-Cdot) + f1+//+// Clamp accumulated limit impulse.+// lower: f2(3) = max(f2(3), 0)+// upper: f2(3) = min(f2(3), 0)+//+// Solve for correct f2(1:2)+// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1+// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)+// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)+// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)+//+// Now compute impulse to be applied:+// df = f2 - f1++void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)+{+ bodyA = bA;+ bodyB = bB;+ localAnchorA = bodyA->GetLocalPoint(anchor);+ localAnchorB = bodyB->GetLocalPoint(anchor);+ localAxisA = bodyA->GetLocalVector(axis);+ referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();+}++b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;+ m_localXAxisA = def->localAxisA;+ m_localXAxisA.Normalize();+ m_localYAxisA = b2Cross(1.0f, m_localXAxisA);+ m_referenceAngle = def->referenceAngle;++ m_impulse.SetZero();+ m_motorMass = 0.0f;+ m_motorImpulse = 0.0f;++ m_lowerTranslation = def->lowerTranslation;+ m_upperTranslation = def->upperTranslation;+ m_maxMotorForce = def->maxMotorForce;+ m_motorSpeed = def->motorSpeed;+ m_enableLimit = def->enableLimit;+ m_enableMotor = def->enableMotor;+ m_limitState = e_inactiveLimit;++ m_axis.SetZero();+ m_perp.SetZero();+}++void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ // Compute the effective masses.+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ b2Vec2 d = (cB - cA) + rB - rA;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ // Compute motor Jacobian and effective mass.+ {+ m_axis = b2Mul(qA, m_localXAxisA);+ m_a1 = b2Cross(d + rA, m_axis);+ m_a2 = b2Cross(rB, m_axis);++ m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;+ if (m_motorMass > 0.0f)+ {+ m_motorMass = 1.0f / m_motorMass;+ }+ }++ // Prismatic constraint.+ {+ m_perp = b2Mul(qA, m_localYAxisA);++ m_s1 = b2Cross(d + rA, m_perp);+ m_s2 = b2Cross(rB, m_perp);++ float32 s1test;+ s1test = b2Cross(rA, m_perp);++ float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;+ float32 k12 = iA * m_s1 + iB * m_s2;+ float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;+ float32 k22 = iA + iB;+ if (k22 == 0.0f)+ {+ // For bodies with fixed rotation.+ k22 = 1.0f;+ }+ float32 k23 = iA * m_a1 + iB * m_a2;+ float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;++ m_K.ex.Set(k11, k12, k13);+ m_K.ey.Set(k12, k22, k23);+ m_K.ez.Set(k13, k23, k33);+ }++ // Compute motor and limit terms.+ if (m_enableLimit)+ {+ float32 jointTranslation = b2Dot(m_axis, d);+ if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)+ {+ m_limitState = e_equalLimits;+ }+ else if (jointTranslation <= m_lowerTranslation)+ {+ if (m_limitState != e_atLowerLimit)+ {+ m_limitState = e_atLowerLimit;+ m_impulse.z = 0.0f;+ }+ }+ else if (jointTranslation >= m_upperTranslation)+ {+ if (m_limitState != e_atUpperLimit)+ {+ m_limitState = e_atUpperLimit;+ m_impulse.z = 0.0f;+ }+ }+ else+ {+ m_limitState = e_inactiveLimit;+ m_impulse.z = 0.0f;+ }+ }+ else+ {+ m_limitState = e_inactiveLimit;+ m_impulse.z = 0.0f;+ }++ if (m_enableMotor == false)+ {+ m_motorImpulse = 0.0f;+ }++ if (data.step.warmStarting)+ {+ // Account for variable time step.+ m_impulse *= data.step.dtRatio;+ m_motorImpulse *= data.step.dtRatio;++ b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;+ float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;+ float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;++ vA -= mA * P;+ wA -= iA * LA;++ vB += mB * P;+ wB += iB * LB;+ }+ else+ {+ m_impulse.SetZero();+ m_motorImpulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ // Solve linear motor constraint.+ if (m_enableMotor && m_limitState != e_equalLimits)+ {+ float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;+ float32 impulse = m_motorMass * (m_motorSpeed - Cdot);+ float32 oldImpulse = m_motorImpulse;+ float32 maxImpulse = data.step.dt * m_maxMotorForce;+ m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);+ impulse = m_motorImpulse - oldImpulse;++ b2Vec2 P = impulse * m_axis;+ float32 LA = impulse * m_a1;+ float32 LB = impulse * m_a2;++ vA -= mA * P;+ wA -= iA * LA;++ vB += mB * P;+ wB += iB * LB;+ }++ b2Vec2 Cdot1;+ Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;+ Cdot1.y = wB - wA;++ if (m_enableLimit && m_limitState != e_inactiveLimit)+ {+ // Solve prismatic and limit constraint in block form.+ float32 Cdot2;+ Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;+ b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);++ b2Vec3 f1 = m_impulse;+ b2Vec3 df = m_K.Solve33(-Cdot);+ m_impulse += df;++ if (m_limitState == e_atLowerLimit)+ {+ m_impulse.z = b2Max(m_impulse.z, 0.0f);+ }+ else if (m_limitState == e_atUpperLimit)+ {+ m_impulse.z = b2Min(m_impulse.z, 0.0f);+ }++ // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)+ b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);+ b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);+ m_impulse.x = f2r.x;+ m_impulse.y = f2r.y;++ df = m_impulse - f1;++ b2Vec2 P = df.x * m_perp + df.z * m_axis;+ float32 LA = df.x * m_s1 + df.y + df.z * m_a1;+ float32 LB = df.x * m_s2 + df.y + df.z * m_a2;++ vA -= mA * P;+ wA -= iA * LA;++ vB += mB * P;+ wB += iB * LB;+ }+ else+ {+ // Limit is inactive, just solve the prismatic constraint in block form.+ b2Vec2 df = m_K.Solve22(-Cdot1);+ m_impulse.x += df.x;+ m_impulse.y += df.y;++ b2Vec2 P = df.x * m_perp;+ float32 LA = df.x * m_s1 + df.y;+ float32 LB = df.x * m_s2 + df.y;++ vA -= mA * P;+ wA -= iA * LA;++ vB += mB * P;+ wB += iB * LB;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ // Compute fresh Jacobians+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ b2Vec2 d = cB + rB - cA - rA;++ b2Vec2 axis = b2Mul(qA, m_localXAxisA);+ float32 a1 = b2Cross(d + rA, axis);+ float32 a2 = b2Cross(rB, axis);+ b2Vec2 perp = b2Mul(qA, m_localYAxisA);++ float32 s1 = b2Cross(d + rA, perp);+ float32 s2 = b2Cross(rB, perp);++ b2Vec3 impulse;+ b2Vec2 C1;+ C1.x = b2Dot(perp, d);+ C1.y = aB - aA - m_referenceAngle;++ float32 linearError = b2Abs(C1.x);+ float32 angularError = b2Abs(C1.y);++ bool active = false;+ float32 C2 = 0.0f;+ if (m_enableLimit)+ {+ float32 translation = b2Dot(axis, d);+ if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)+ {+ // Prevent large angular corrections+ C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);+ linearError = b2Max(linearError, b2Abs(translation));+ active = true;+ }+ else if (translation <= m_lowerTranslation)+ {+ // Prevent large linear corrections and allow some slop.+ C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);+ linearError = b2Max(linearError, m_lowerTranslation - translation);+ active = true;+ }+ else if (translation >= m_upperTranslation)+ {+ // Prevent large linear corrections and allow some slop.+ C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);+ linearError = b2Max(linearError, translation - m_upperTranslation);+ active = true;+ }+ }++ if (active)+ {+ float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;+ float32 k12 = iA * s1 + iB * s2;+ float32 k13 = iA * s1 * a1 + iB * s2 * a2;+ float32 k22 = iA + iB;+ if (k22 == 0.0f)+ {+ // For fixed rotation+ k22 = 1.0f;+ }+ float32 k23 = iA * a1 + iB * a2;+ float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;++ b2Mat33 K;+ K.ex.Set(k11, k12, k13);+ K.ey.Set(k12, k22, k23);+ K.ez.Set(k13, k23, k33);++ b2Vec3 C;+ C.x = C1.x;+ C.y = C1.y;+ C.z = C2;++ impulse = K.Solve33(-C);+ }+ else+ {+ float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;+ float32 k12 = iA * s1 + iB * s2;+ float32 k22 = iA + iB;+ if (k22 == 0.0f)+ {+ k22 = 1.0f;+ }++ b2Mat22 K;+ K.ex.Set(k11, k12);+ K.ey.Set(k12, k22);++ b2Vec2 impulse1 = K.Solve(-C1);+ impulse.x = impulse1.x;+ impulse.y = impulse1.y;+ impulse.z = 0.0f;+ }++ b2Vec2 P = impulse.x * perp + impulse.z * axis;+ float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;+ float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;++ cA -= mA * P;+ aA -= iA * LA;+ cB += mB * P;+ aB += iB * LB;++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return linearError <= b2_linearSlop && angularError <= b2_angularSlop;+}++b2Vec2 b2PrismaticJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2PrismaticJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const+{+ return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis);+}++float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * m_impulse.y;+}++float32 b2PrismaticJoint::GetJointTranslation() const+{+ b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);+ b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);+ b2Vec2 d = pB - pA;+ b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA);++ float32 translation = b2Dot(d, axis);+ return translation;+}++float32 b2PrismaticJoint::GetJointSpeed() const+{+ b2Body* bA = m_bodyA;+ b2Body* bB = m_bodyB;++ b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);+ b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);+ b2Vec2 p1 = bA->m_sweep.c + rA;+ b2Vec2 p2 = bB->m_sweep.c + rB;+ b2Vec2 d = p2 - p1;+ b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);++ b2Vec2 vA = bA->m_linearVelocity;+ b2Vec2 vB = bB->m_linearVelocity;+ float32 wA = bA->m_angularVelocity;+ float32 wB = bB->m_angularVelocity;++ float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));+ return speed;+}++bool b2PrismaticJoint::IsLimitEnabled() const+{+ return m_enableLimit;+}++void b2PrismaticJoint::EnableLimit(bool flag)+{+ if (flag != m_enableLimit)+ {+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_enableLimit = flag;+ m_impulse.z = 0.0f;+ }+}++float32 b2PrismaticJoint::GetLowerLimit() const+{+ return m_lowerTranslation;+}++float32 b2PrismaticJoint::GetUpperLimit() const+{+ return m_upperTranslation;+}++void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)+{+ b2Assert(lower <= upper);+ if (lower != m_lowerTranslation || upper != m_upperTranslation)+ {+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_lowerTranslation = lower;+ m_upperTranslation = upper;+ m_impulse.z = 0.0f;+ }+}++bool b2PrismaticJoint::IsMotorEnabled() const+{+ return m_enableMotor;+}++void b2PrismaticJoint::EnableMotor(bool flag)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_enableMotor = flag;+}++void b2PrismaticJoint::SetMotorSpeed(float32 speed)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_motorSpeed = speed;+}++void b2PrismaticJoint::SetMaxMotorForce(float32 force)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_maxMotorForce = force;+}++float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const+{+ return inv_dt * m_motorImpulse;+}++void b2PrismaticJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2PrismaticJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);+ b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);+ b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit);+ b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation);+ b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation);+ b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);+ b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);+ b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.hnew file mode 100644index 00000000..8d0f3422--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h@@ -0,0 +1,196 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_PRISMATIC_JOINT_H+#define B2_PRISMATIC_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Prismatic joint definition. This requires defining a line of+/// motion using an axis and an anchor point. The definition uses local+/// anchor points and a local axis so that the initial configuration+/// can violate the constraint slightly. The joint translation is zero+/// when the local anchor points coincide in world space. Using local+/// anchors and a local axis helps when saving and loading a game.+struct b2PrismaticJointDef : public b2JointDef+{+ b2PrismaticJointDef()+ {+ type = e_prismaticJoint;+ localAnchorA.SetZero();+ localAnchorB.SetZero();+ localAxisA.Set(1.0f, 0.0f);+ referenceAngle = 0.0f;+ enableLimit = false;+ lowerTranslation = 0.0f;+ upperTranslation = 0.0f;+ enableMotor = false;+ maxMotorForce = 0.0f;+ motorSpeed = 0.0f;+ }++ /// Initialize the bodies, anchors, axis, and reference angle using the world+ /// anchor and unit world axis.+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The local translation unit axis in bodyA.+ b2Vec2 localAxisA;++ /// The constrained angle between the bodies: bodyB_angle - bodyA_angle.+ float32 referenceAngle;++ /// Enable/disable the joint limit.+ bool enableLimit;++ /// The lower translation limit, usually in meters.+ float32 lowerTranslation;++ /// The upper translation limit, usually in meters.+ float32 upperTranslation;++ /// Enable/disable the joint motor.+ bool enableMotor;++ /// The maximum motor torque, usually in N-m.+ float32 maxMotorForce;++ /// The desired motor speed in radians per second.+ float32 motorSpeed;+};++/// A prismatic joint. This joint provides one degree of freedom: translation+/// along an axis fixed in bodyA. Relative rotation is prevented. You can+/// use a joint limit to restrict the range of motion and a joint motor to+/// drive the motion or to model joint friction.+class b2PrismaticJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// The local joint axis relative to bodyA.+ const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }++ /// Get the reference angle.+ float32 GetReferenceAngle() const { return m_referenceAngle; }++ /// Get the current joint translation, usually in meters.+ float32 GetJointTranslation() const;++ /// Get the current joint translation speed, usually in meters per second.+ float32 GetJointSpeed() const;++ /// Is the joint limit enabled?+ bool IsLimitEnabled() const;++ /// Enable/disable the joint limit.+ void EnableLimit(bool flag);++ /// Get the lower joint limit, usually in meters.+ float32 GetLowerLimit() const;++ /// Get the upper joint limit, usually in meters.+ float32 GetUpperLimit() const;++ /// Set the joint limits, usually in meters.+ void SetLimits(float32 lower, float32 upper);++ /// Is the joint motor enabled?+ bool IsMotorEnabled() const;++ /// Enable/disable the joint motor.+ void EnableMotor(bool flag);++ /// Set the motor speed, usually in meters per second.+ void SetMotorSpeed(float32 speed);++ /// Get the motor speed, usually in meters per second.+ float32 GetMotorSpeed() const;++ /// Set the maximum motor force, usually in N.+ void SetMaxMotorForce(float32 force);+ float32 GetMaxMotorForce() const { return m_maxMotorForce; }++ /// Get the current motor force given the inverse time step, usually in N.+ float32 GetMotorForce(float32 inv_dt) const;++ /// Dump to b2Log+ void Dump();++protected:+ friend class b2Joint;+ friend class b2GearJoint;+ b2PrismaticJoint(const b2PrismaticJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ b2Vec2 m_localXAxisA;+ b2Vec2 m_localYAxisA;+ float32 m_referenceAngle;+ b2Vec3 m_impulse;+ float32 m_motorImpulse;+ float32 m_lowerTranslation;+ float32 m_upperTranslation;+ float32 m_maxMotorForce;+ float32 m_motorSpeed;+ bool m_enableLimit;+ bool m_enableMotor;+ b2LimitState m_limitState;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ b2Vec2 m_axis, m_perp;+ float32 m_s1, m_s2;+ float32 m_a1, m_a2;+ b2Mat33 m_K;+ float32 m_motorMass;+};++inline float32 b2PrismaticJoint::GetMotorSpeed() const+{+ return m_motorSpeed;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cppnew file mode 100644index 00000000..f8b22853--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp@@ -0,0 +1,348 @@+/*+* Copyright (c) 2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Pulley:+// length1 = norm(p1 - s1)+// length2 = norm(p2 - s2)+// C0 = (length1 + ratio * length2)_initial+// C = C0 - (length1 + ratio * length2)+// u1 = (p1 - s1) / norm(p1 - s1)+// u2 = (p2 - s2) / norm(p2 - s2)+// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))+// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]+// K = J * invM * JT+// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)++void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB,+ const b2Vec2& groundA, const b2Vec2& groundB,+ const b2Vec2& anchorA, const b2Vec2& anchorB,+ float32 r)+{+ bodyA = bA;+ bodyB = bB;+ groundAnchorA = groundA;+ groundAnchorB = groundB;+ localAnchorA = bodyA->GetLocalPoint(anchorA);+ localAnchorB = bodyB->GetLocalPoint(anchorB);+ b2Vec2 dA = anchorA - groundA;+ lengthA = dA.Length();+ b2Vec2 dB = anchorB - groundB;+ lengthB = dB.Length();+ ratio = r;+ b2Assert(ratio > b2_epsilon);+}++b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)+: b2Joint(def)+{+ m_groundAnchorA = def->groundAnchorA;+ m_groundAnchorB = def->groundAnchorB;+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;++ m_lengthA = def->lengthA;+ m_lengthB = def->lengthB;++ b2Assert(def->ratio != 0.0f);+ m_ratio = def->ratio;++ m_constant = def->lengthA + m_ratio * def->lengthB;++ m_impulse = 0.0f;+}++void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ // Get the pulley axes.+ m_uA = cA + m_rA - m_groundAnchorA;+ m_uB = cB + m_rB - m_groundAnchorB;++ float32 lengthA = m_uA.Length();+ float32 lengthB = m_uB.Length();++ if (lengthA > 10.0f * b2_linearSlop)+ {+ m_uA *= 1.0f / lengthA;+ }+ else+ {+ m_uA.SetZero();+ }++ if (lengthB > 10.0f * b2_linearSlop)+ {+ m_uB *= 1.0f / lengthB;+ }+ else+ {+ m_uB.SetZero();+ }++ // Compute effective mass.+ float32 ruA = b2Cross(m_rA, m_uA);+ float32 ruB = b2Cross(m_rB, m_uB);++ float32 mA = m_invMassA + m_invIA * ruA * ruA;+ float32 mB = m_invMassB + m_invIB * ruB * ruB;++ m_mass = mA + m_ratio * m_ratio * mB;++ if (m_mass > 0.0f)+ {+ m_mass = 1.0f / m_mass;+ }++ if (data.step.warmStarting)+ {+ // Scale impulses to support variable time steps.+ m_impulse *= data.step.dtRatio;++ // Warm starting.+ b2Vec2 PA = -(m_impulse) * m_uA;+ b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;++ vA += m_invMassA * PA;+ wA += m_invIA * b2Cross(m_rA, PA);+ vB += m_invMassB * PB;+ wB += m_invIB * b2Cross(m_rB, PB);+ }+ else+ {+ m_impulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Vec2 vpA = vA + b2Cross(wA, m_rA);+ b2Vec2 vpB = vB + b2Cross(wB, m_rB);++ float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);+ float32 impulse = -m_mass * Cdot;+ m_impulse += impulse;++ b2Vec2 PA = -impulse * m_uA;+ b2Vec2 PB = -m_ratio * impulse * m_uB;+ vA += m_invMassA * PA;+ wA += m_invIA * b2Cross(m_rA, PA);+ vB += m_invMassB * PB;+ wB += m_invIB * b2Cross(m_rB, PB);++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ // Get the pulley axes.+ b2Vec2 uA = cA + rA - m_groundAnchorA;+ b2Vec2 uB = cB + rB - m_groundAnchorB;++ float32 lengthA = uA.Length();+ float32 lengthB = uB.Length();++ if (lengthA > 10.0f * b2_linearSlop)+ {+ uA *= 1.0f / lengthA;+ }+ else+ {+ uA.SetZero();+ }++ if (lengthB > 10.0f * b2_linearSlop)+ {+ uB *= 1.0f / lengthB;+ }+ else+ {+ uB.SetZero();+ }++ // Compute effective mass.+ float32 ruA = b2Cross(rA, uA);+ float32 ruB = b2Cross(rB, uB);++ float32 mA = m_invMassA + m_invIA * ruA * ruA;+ float32 mB = m_invMassB + m_invIB * ruB * ruB;++ float32 mass = mA + m_ratio * m_ratio * mB;++ if (mass > 0.0f)+ {+ mass = 1.0f / mass;+ }++ float32 C = m_constant - lengthA - m_ratio * lengthB;+ float32 linearError = b2Abs(C);++ float32 impulse = -mass * C;++ b2Vec2 PA = -impulse * uA;+ b2Vec2 PB = -m_ratio * impulse * uB;++ cA += m_invMassA * PA;+ aA += m_invIA * b2Cross(rA, PA);+ cB += m_invMassB * PB;+ aB += m_invIB * b2Cross(rB, PB);++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return linearError < b2_linearSlop;+}++b2Vec2 b2PulleyJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2PulleyJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const+{+ b2Vec2 P = m_impulse * m_uB;+ return inv_dt * P;+}++float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const+{+ B2_NOT_USED(inv_dt);+ return 0.0f;+}++b2Vec2 b2PulleyJoint::GetGroundAnchorA() const+{+ return m_groundAnchorA;+}++b2Vec2 b2PulleyJoint::GetGroundAnchorB() const+{+ return m_groundAnchorB;+}++float32 b2PulleyJoint::GetLengthA() const+{+ return m_lengthA;+}++float32 b2PulleyJoint::GetLengthB() const+{+ return m_lengthB;+}++float32 b2PulleyJoint::GetRatio() const+{+ return m_ratio;+}++float32 b2PulleyJoint::GetCurrentLengthA() const+{+ b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);+ b2Vec2 s = m_groundAnchorA;+ b2Vec2 d = p - s;+ return d.Length();+}++float32 b2PulleyJoint::GetCurrentLengthB() const+{+ b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);+ b2Vec2 s = m_groundAnchorB;+ b2Vec2 d = p - s;+ return d.Length();+}++void b2PulleyJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2PulleyJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y);+ b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.lengthA = %.15lef;\n", m_lengthA);+ b2Log(" jd.lengthB = %.15lef;\n", m_lengthB);+ b2Log(" jd.ratio = %.15lef;\n", m_ratio);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}++void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin)+{+ m_groundAnchorA -= newOrigin;+ m_groundAnchorB -= newOrigin;+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.hnew file mode 100644index 00000000..2dcdd34a--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h@@ -0,0 +1,152 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_PULLEY_JOINT_H+#define B2_PULLEY_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++const float32 b2_minPulleyLength = 2.0f;++/// Pulley joint definition. This requires two ground anchors,+/// two dynamic body anchor points, and a pulley ratio.+struct b2PulleyJointDef : public b2JointDef+{+ b2PulleyJointDef()+ {+ type = e_pulleyJoint;+ groundAnchorA.Set(-1.0f, 1.0f);+ groundAnchorB.Set(1.0f, 1.0f);+ localAnchorA.Set(-1.0f, 0.0f);+ localAnchorB.Set(1.0f, 0.0f);+ lengthA = 0.0f;+ lengthB = 0.0f;+ ratio = 1.0f;+ collideConnected = true;+ }++ /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.+ void Initialize(b2Body* bodyA, b2Body* bodyB,+ const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB,+ const b2Vec2& anchorA, const b2Vec2& anchorB,+ float32 ratio);++ /// The first ground anchor in world coordinates. This point never moves.+ b2Vec2 groundAnchorA;++ /// The second ground anchor in world coordinates. This point never moves.+ b2Vec2 groundAnchorB;++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The a reference length for the segment attached to bodyA.+ float32 lengthA;++ /// The a reference length for the segment attached to bodyB.+ float32 lengthB;++ /// The pulley ratio, used to simulate a block-and-tackle.+ float32 ratio;+};++/// The pulley joint is connected to two bodies and two fixed ground points.+/// The pulley supports a ratio such that:+/// length1 + ratio * length2 <= constant+/// Yes, the force transmitted is scaled by the ratio.+/// Warning: the pulley joint can get a bit squirrelly by itself. They often+/// work better when combined with prismatic joints. You should also cover the+/// the anchor points with static shapes to prevent one side from going to+/// zero length.+class b2PulleyJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// Get the first ground anchor.+ b2Vec2 GetGroundAnchorA() const;++ /// Get the second ground anchor.+ b2Vec2 GetGroundAnchorB() const;++ /// Get the current length of the segment attached to bodyA.+ float32 GetLengthA() const;++ /// Get the current length of the segment attached to bodyB.+ float32 GetLengthB() const;++ /// Get the pulley ratio.+ float32 GetRatio() const;++ /// Get the current length of the segment attached to bodyA.+ float32 GetCurrentLengthA() const;++ /// Get the current length of the segment attached to bodyB.+ float32 GetCurrentLengthB() const;++ /// Dump joint to dmLog+ void Dump();++ /// Implement b2Joint::ShiftOrigin+ void ShiftOrigin(const b2Vec2& newOrigin);++protected:++ friend class b2Joint;+ b2PulleyJoint(const b2PulleyJointDef* data);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ b2Vec2 m_groundAnchorA;+ b2Vec2 m_groundAnchorB;+ float32 m_lengthA;+ float32 m_lengthB;++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ float32 m_constant;+ float32 m_ratio;+ float32 m_impulse;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_uA;+ b2Vec2 m_uB;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ float32 m_mass;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cppnew file mode 100644index 00000000..d48cae81--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp@@ -0,0 +1,502 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Point-to-point constraint+// C = p2 - p1+// Cdot = v2 - v1+// = v2 + cross(w2, r2) - v1 - cross(w1, r1)+// J = [-I -r1_skew I r2_skew ]+// Identity used:+// w k % (rx i + ry j) = w * (-ry i + rx j)++// Motor constraint+// Cdot = w2 - w1+// J = [0 0 -1 0 0 1]+// K = invI1 + invI2++void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)+{+ bodyA = bA;+ bodyB = bB;+ localAnchorA = bodyA->GetLocalPoint(anchor);+ localAnchorB = bodyB->GetLocalPoint(anchor);+ referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();+}++b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;+ m_referenceAngle = def->referenceAngle;++ m_impulse.SetZero();+ m_motorImpulse = 0.0f;++ m_lowerAngle = def->lowerAngle;+ m_upperAngle = def->upperAngle;+ m_maxMotorTorque = def->maxMotorTorque;+ m_motorSpeed = def->motorSpeed;+ m_enableLimit = def->enableLimit;+ m_enableMotor = def->enableMotor;+ m_limitState = e_inactiveLimit;+}++void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ // J = [-I -r1_skew I r2_skew]+ // [ 0 -1 0 1]+ // r_skew = [-ry; rx]++ // Matlab+ // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]+ // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]+ // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ bool fixedRotation = (iA + iB == 0.0f);++ m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;+ m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;+ m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;+ m_mass.ex.y = m_mass.ey.x;+ m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;+ m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;+ m_mass.ex.z = m_mass.ez.x;+ m_mass.ey.z = m_mass.ez.y;+ m_mass.ez.z = iA + iB;++ m_motorMass = iA + iB;+ if (m_motorMass > 0.0f)+ {+ m_motorMass = 1.0f / m_motorMass;+ }++ if (m_enableMotor == false || fixedRotation)+ {+ m_motorImpulse = 0.0f;+ }++ if (m_enableLimit && fixedRotation == false)+ {+ float32 jointAngle = aB - aA - m_referenceAngle;+ if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)+ {+ m_limitState = e_equalLimits;+ }+ else if (jointAngle <= m_lowerAngle)+ {+ if (m_limitState != e_atLowerLimit)+ {+ m_impulse.z = 0.0f;+ }+ m_limitState = e_atLowerLimit;+ }+ else if (jointAngle >= m_upperAngle)+ {+ if (m_limitState != e_atUpperLimit)+ {+ m_impulse.z = 0.0f;+ }+ m_limitState = e_atUpperLimit;+ }+ else+ {+ m_limitState = e_inactiveLimit;+ m_impulse.z = 0.0f;+ }+ }+ else+ {+ m_limitState = e_inactiveLimit;+ }++ if (data.step.warmStarting)+ {+ // Scale impulses to support a variable time step.+ m_impulse *= data.step.dtRatio;+ m_motorImpulse *= data.step.dtRatio;++ b2Vec2 P(m_impulse.x, m_impulse.y);++ vA -= mA * P;+ wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);++ vB += mB * P;+ wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);+ }+ else+ {+ m_impulse.SetZero();+ m_motorImpulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ bool fixedRotation = (iA + iB == 0.0f);++ // Solve motor constraint.+ if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)+ {+ float32 Cdot = wB - wA - m_motorSpeed;+ float32 impulse = -m_motorMass * Cdot;+ float32 oldImpulse = m_motorImpulse;+ float32 maxImpulse = data.step.dt * m_maxMotorTorque;+ m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);+ impulse = m_motorImpulse - oldImpulse;++ wA -= iA * impulse;+ wB += iB * impulse;+ }++ // Solve limit constraint.+ if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)+ {+ b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);+ float32 Cdot2 = wB - wA;+ b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);++ b2Vec3 impulse = -m_mass.Solve33(Cdot);++ if (m_limitState == e_equalLimits)+ {+ m_impulse += impulse;+ }+ else if (m_limitState == e_atLowerLimit)+ {+ float32 newImpulse = m_impulse.z + impulse.z;+ if (newImpulse < 0.0f)+ {+ b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);+ b2Vec2 reduced = m_mass.Solve22(rhs);+ impulse.x = reduced.x;+ impulse.y = reduced.y;+ impulse.z = -m_impulse.z;+ m_impulse.x += reduced.x;+ m_impulse.y += reduced.y;+ m_impulse.z = 0.0f;+ }+ else+ {+ m_impulse += impulse;+ }+ }+ else if (m_limitState == e_atUpperLimit)+ {+ float32 newImpulse = m_impulse.z + impulse.z;+ if (newImpulse > 0.0f)+ {+ b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);+ b2Vec2 reduced = m_mass.Solve22(rhs);+ impulse.x = reduced.x;+ impulse.y = reduced.y;+ impulse.z = -m_impulse.z;+ m_impulse.x += reduced.x;+ m_impulse.y += reduced.y;+ m_impulse.z = 0.0f;+ }+ else+ {+ m_impulse += impulse;+ }+ }++ b2Vec2 P(impulse.x, impulse.y);++ vA -= mA * P;+ wA -= iA * (b2Cross(m_rA, P) + impulse.z);++ vB += mB * P;+ wB += iB * (b2Cross(m_rB, P) + impulse.z);+ }+ else+ {+ // Solve point-to-point constraint+ b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);+ b2Vec2 impulse = m_mass.Solve22(-Cdot);++ m_impulse.x += impulse.x;+ m_impulse.y += impulse.y;++ vA -= mA * impulse;+ wA -= iA * b2Cross(m_rA, impulse);++ vB += mB * impulse;+ wB += iB * b2Cross(m_rB, impulse);+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ float32 angularError = 0.0f;+ float32 positionError = 0.0f;++ bool fixedRotation = (m_invIA + m_invIB == 0.0f);++ // Solve angular limit constraint.+ if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)+ {+ float32 angle = aB - aA - m_referenceAngle;+ float32 limitImpulse = 0.0f;++ if (m_limitState == e_equalLimits)+ {+ // Prevent large angular corrections+ float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);+ limitImpulse = -m_motorMass * C;+ angularError = b2Abs(C);+ }+ else if (m_limitState == e_atLowerLimit)+ {+ float32 C = angle - m_lowerAngle;+ angularError = -C;++ // Prevent large angular corrections and allow some slop.+ C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);+ limitImpulse = -m_motorMass * C;+ }+ else if (m_limitState == e_atUpperLimit)+ {+ float32 C = angle - m_upperAngle;+ angularError = C;++ // Prevent large angular corrections and allow some slop.+ C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);+ limitImpulse = -m_motorMass * C;+ }++ aA -= m_invIA * limitImpulse;+ aB += m_invIB * limitImpulse;+ }++ // Solve point-to-point constraint.+ {+ qA.Set(aA);+ qB.Set(aB);+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ b2Vec2 C = cB + rB - cA - rA;+ positionError = C.Length();++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Mat22 K;+ K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;+ K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;+ K.ey.x = K.ex.y;+ K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;++ b2Vec2 impulse = -K.Solve(C);++ cA -= mA * impulse;+ aA -= iA * b2Cross(rA, impulse);++ cB += mB * impulse;+ aB += iB * b2Cross(rB, impulse);+ }++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return positionError <= b2_linearSlop && angularError <= b2_angularSlop;+}++b2Vec2 b2RevoluteJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2RevoluteJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const+{+ b2Vec2 P(m_impulse.x, m_impulse.y);+ return inv_dt * P;+}++float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * m_impulse.z;+}++float32 b2RevoluteJoint::GetJointAngle() const+{+ b2Body* bA = m_bodyA;+ b2Body* bB = m_bodyB;+ return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;+}++float32 b2RevoluteJoint::GetJointSpeed() const+{+ b2Body* bA = m_bodyA;+ b2Body* bB = m_bodyB;+ return bB->m_angularVelocity - bA->m_angularVelocity;+}++bool b2RevoluteJoint::IsMotorEnabled() const+{+ return m_enableMotor;+}++void b2RevoluteJoint::EnableMotor(bool flag)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_enableMotor = flag;+}++float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const+{+ return inv_dt * m_motorImpulse;+}++void b2RevoluteJoint::SetMotorSpeed(float32 speed)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_motorSpeed = speed;+}++void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_maxMotorTorque = torque;+}++bool b2RevoluteJoint::IsLimitEnabled() const+{+ return m_enableLimit;+}++void b2RevoluteJoint::EnableLimit(bool flag)+{+ if (flag != m_enableLimit)+ {+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_enableLimit = flag;+ m_impulse.z = 0.0f;+ }+}++float32 b2RevoluteJoint::GetLowerLimit() const+{+ return m_lowerAngle;+}++float32 b2RevoluteJoint::GetUpperLimit() const+{+ return m_upperAngle;+}++void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)+{+ b2Assert(lower <= upper);++ if (lower != m_lowerAngle || upper != m_upperAngle)+ {+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_impulse.z = 0.0f;+ m_lowerAngle = lower;+ m_upperAngle = upper;+ }+}++void b2RevoluteJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2RevoluteJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);+ b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit);+ b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle);+ b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle);+ b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);+ b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);+ b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.hnew file mode 100644index 00000000..e62cdbcc--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h@@ -0,0 +1,204 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_REVOLUTE_JOINT_H+#define B2_REVOLUTE_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Revolute joint definition. This requires defining an+/// anchor point where the bodies are joined. The definition+/// uses local anchor points so that the initial configuration+/// can violate the constraint slightly. You also need to+/// specify the initial relative angle for joint limits. This+/// helps when saving and loading a game.+/// The local anchor points are measured from the body's origin+/// rather than the center of mass because:+/// 1. you might not know where the center of mass will be.+/// 2. if you add/remove shapes from a body and recompute the mass,+/// the joints will be broken.+struct b2RevoluteJointDef : public b2JointDef+{+ b2RevoluteJointDef()+ {+ type = e_revoluteJoint;+ localAnchorA.Set(0.0f, 0.0f);+ localAnchorB.Set(0.0f, 0.0f);+ referenceAngle = 0.0f;+ lowerAngle = 0.0f;+ upperAngle = 0.0f;+ maxMotorTorque = 0.0f;+ motorSpeed = 0.0f;+ enableLimit = false;+ enableMotor = false;+ }++ /// Initialize the bodies, anchors, and reference angle using a world+ /// anchor point.+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The bodyB angle minus bodyA angle in the reference state (radians).+ float32 referenceAngle;++ /// A flag to enable joint limits.+ bool enableLimit;++ /// The lower angle for the joint limit (radians).+ float32 lowerAngle;++ /// The upper angle for the joint limit (radians).+ float32 upperAngle;++ /// A flag to enable the joint motor.+ bool enableMotor;++ /// The desired motor speed. Usually in radians per second.+ float32 motorSpeed;++ /// The maximum motor torque used to achieve the desired motor speed.+ /// Usually in N-m.+ float32 maxMotorTorque;+};++/// A revolute joint constrains two bodies to share a common point while they+/// are free to rotate about the point. The relative rotation about the shared+/// point is the joint angle. You can limit the relative rotation with+/// a joint limit that specifies a lower and upper angle. You can use a motor+/// to drive the relative rotation about the shared point. A maximum motor torque+/// is provided so that infinite forces are not generated.+class b2RevoluteJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// Get the reference angle.+ float32 GetReferenceAngle() const { return m_referenceAngle; }++ /// Get the current joint angle in radians.+ float32 GetJointAngle() const;++ /// Get the current joint angle speed in radians per second.+ float32 GetJointSpeed() const;++ /// Is the joint limit enabled?+ bool IsLimitEnabled() const;++ /// Enable/disable the joint limit.+ void EnableLimit(bool flag);++ /// Get the lower joint limit in radians.+ float32 GetLowerLimit() const;++ /// Get the upper joint limit in radians.+ float32 GetUpperLimit() const;++ /// Set the joint limits in radians.+ void SetLimits(float32 lower, float32 upper);++ /// Is the joint motor enabled?+ bool IsMotorEnabled() const;++ /// Enable/disable the joint motor.+ void EnableMotor(bool flag);++ /// Set the motor speed in radians per second.+ void SetMotorSpeed(float32 speed);++ /// Get the motor speed in radians per second.+ float32 GetMotorSpeed() const;++ /// Set the maximum motor torque, usually in N-m.+ void SetMaxMotorTorque(float32 torque);+ float32 GetMaxMotorTorque() const { return m_maxMotorTorque; }++ /// Get the reaction force given the inverse time step.+ /// Unit is N.+ b2Vec2 GetReactionForce(float32 inv_dt) const;++ /// Get the reaction torque due to the joint limit given the inverse time step.+ /// Unit is N*m.+ float32 GetReactionTorque(float32 inv_dt) const;++ /// Get the current motor torque given the inverse time step.+ /// Unit is N*m.+ float32 GetMotorTorque(float32 inv_dt) const;++ /// Dump to b2Log.+ void Dump();++protected:++ friend class b2Joint;+ friend class b2GearJoint;++ b2RevoluteJoint(const b2RevoluteJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ b2Vec3 m_impulse;+ float32 m_motorImpulse;++ bool m_enableMotor;+ float32 m_maxMotorTorque;+ float32 m_motorSpeed;++ bool m_enableLimit;+ float32 m_referenceAngle;+ float32 m_lowerAngle;+ float32 m_upperAngle;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ b2Mat33 m_mass; // effective mass for point-to-point constraint.+ float32 m_motorMass; // effective mass for motor/limit angular constraint.+ b2LimitState m_limitState;+};++inline float32 b2RevoluteJoint::GetMotorSpeed() const+{+ return m_motorSpeed;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cppnew file mode 100644index 00000000..107ce1fa--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp@@ -0,0 +1,241 @@+/*+* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2RopeJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>+++// Limit:+// C = norm(pB - pA) - L+// u = (pB - pA) / norm(pB - pA)+// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))+// J = [-u -cross(rA, u) u cross(rB, u)]+// K = J * invM * JT+// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2++b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;++ m_maxLength = def->maxLength;++ m_mass = 0.0f;+ m_impulse = 0.0f;+ m_state = e_inactiveLimit;+ m_length = 0.0f;+}++void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ m_u = cB + m_rB - cA - m_rA;++ m_length = m_u.Length();++ float32 C = m_length - m_maxLength;+ if (C > 0.0f)+ {+ m_state = e_atUpperLimit;+ }+ else+ {+ m_state = e_inactiveLimit;+ }++ if (m_length > b2_linearSlop)+ {+ m_u *= 1.0f / m_length;+ }+ else+ {+ m_u.SetZero();+ m_mass = 0.0f;+ m_impulse = 0.0f;+ return;+ }++ // Compute effective mass.+ float32 crA = b2Cross(m_rA, m_u);+ float32 crB = b2Cross(m_rB, m_u);+ float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;++ m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;++ if (data.step.warmStarting)+ {+ // Scale the impulse to support a variable time step.+ m_impulse *= data.step.dtRatio;++ b2Vec2 P = m_impulse * m_u;+ vA -= m_invMassA * P;+ wA -= m_invIA * b2Cross(m_rA, P);+ vB += m_invMassB * P;+ wB += m_invIB * b2Cross(m_rB, P);+ }+ else+ {+ m_impulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ // Cdot = dot(u, v + cross(w, r))+ b2Vec2 vpA = vA + b2Cross(wA, m_rA);+ b2Vec2 vpB = vB + b2Cross(wB, m_rB);+ float32 C = m_length - m_maxLength;+ float32 Cdot = b2Dot(m_u, vpB - vpA);++ // Predictive constraint.+ if (C < 0.0f)+ {+ Cdot += data.step.inv_dt * C;+ }++ float32 impulse = -m_mass * Cdot;+ float32 oldImpulse = m_impulse;+ m_impulse = b2Min(0.0f, m_impulse + impulse);+ impulse = m_impulse - oldImpulse;++ b2Vec2 P = impulse * m_u;+ vA -= m_invMassA * P;+ wA -= m_invIA * b2Cross(m_rA, P);+ vB += m_invMassB * P;+ wB += m_invIB * b2Cross(m_rB, P);++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ b2Vec2 u = cB + rB - cA - rA;++ float32 length = u.Normalize();+ float32 C = length - m_maxLength;++ C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);++ float32 impulse = -m_mass * C;+ b2Vec2 P = impulse * u;++ cA -= m_invMassA * P;+ aA -= m_invIA * b2Cross(rA, P);+ cB += m_invMassB * P;+ aB += m_invIB * b2Cross(rB, P);++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return length - m_maxLength < b2_linearSlop;+}++b2Vec2 b2RopeJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2RopeJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const+{+ b2Vec2 F = (inv_dt * m_impulse) * m_u;+ return F;+}++float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const+{+ B2_NOT_USED(inv_dt);+ return 0.0f;+}++float32 b2RopeJoint::GetMaxLength() const+{+ return m_maxLength;+}++b2LimitState b2RopeJoint::GetLimitState() const+{+ return m_state;+}++void b2RopeJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2RopeJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.maxLength = %.15lef;\n", m_maxLength);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.hnew file mode 100644index 00000000..30252460--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h@@ -0,0 +1,114 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_ROPE_JOINT_H+#define B2_ROPE_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Rope joint definition. This requires two body anchor points and+/// a maximum lengths.+/// Note: by default the connected objects will not collide.+/// see collideConnected in b2JointDef.+struct b2RopeJointDef : public b2JointDef+{+ b2RopeJointDef()+ {+ type = e_ropeJoint;+ localAnchorA.Set(-1.0f, 0.0f);+ localAnchorB.Set(1.0f, 0.0f);+ maxLength = 0.0f;+ }++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The maximum length of the rope.+ /// Warning: this must be larger than b2_linearSlop or+ /// the joint will have no effect.+ float32 maxLength;+};++/// A rope joint enforces a maximum distance between two points+/// on two bodies. It has no other effect.+/// Warning: if you attempt to change the maximum length during+/// the simulation you will get some non-physical behavior.+/// A model that would allow you to dynamically modify the length+/// would have some sponginess, so I chose not to implement it+/// that way. See b2DistanceJoint if you want to dynamically+/// control length.+class b2RopeJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// Set/Get the maximum length of the rope.+ void SetMaxLength(float32 length) { m_maxLength = length; }+ float32 GetMaxLength() const;++ b2LimitState GetLimitState() const;++ /// Dump joint to dmLog+ void Dump();++protected:++ friend class b2Joint;+ b2RopeJoint(const b2RopeJointDef* data);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ float32 m_maxLength;+ float32 m_length;+ float32 m_impulse;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_u;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ float32 m_mass;+ b2LimitState m_state;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cppnew file mode 100644index 00000000..5c5614d8--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp@@ -0,0 +1,344 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2WeldJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Point-to-point constraint+// C = p2 - p1+// Cdot = v2 - v1+// = v2 + cross(w2, r2) - v1 - cross(w1, r1)+// J = [-I -r1_skew I r2_skew ]+// Identity used:+// w k % (rx i + ry j) = w * (-ry i + rx j)++// Angle constraint+// C = angle2 - angle1 - referenceAngle+// Cdot = w2 - w1+// J = [0 0 -1 0 0 1]+// K = invI1 + invI2++void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)+{+ bodyA = bA;+ bodyB = bB;+ localAnchorA = bodyA->GetLocalPoint(anchor);+ localAnchorB = bodyB->GetLocalPoint(anchor);+ referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();+}++b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;+ m_referenceAngle = def->referenceAngle;+ m_frequencyHz = def->frequencyHz;+ m_dampingRatio = def->dampingRatio;++ m_impulse.SetZero();+}++void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ // J = [-I -r1_skew I r2_skew]+ // [ 0 -1 0 1]+ // r_skew = [-ry; rx]++ // Matlab+ // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]+ // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]+ // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Mat33 K;+ K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;+ K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;+ K.ez.x = -m_rA.y * iA - m_rB.y * iB;+ K.ex.y = K.ey.x;+ K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;+ K.ez.y = m_rA.x * iA + m_rB.x * iB;+ K.ex.z = K.ez.x;+ K.ey.z = K.ez.y;+ K.ez.z = iA + iB;++ if (m_frequencyHz > 0.0f)+ {+ K.GetInverse22(&m_mass);++ float32 invM = iA + iB;+ float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;++ float32 C = aB - aA - m_referenceAngle;++ // Frequency+ float32 omega = 2.0f * b2_pi * m_frequencyHz;++ // Damping coefficient+ float32 d = 2.0f * m * m_dampingRatio * omega;++ // Spring stiffness+ float32 k = m * omega * omega;++ // magic formulas+ float32 h = data.step.dt;+ m_gamma = h * (d + h * k);+ m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;+ m_bias = C * h * k * m_gamma;++ invM += m_gamma;+ m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;+ }+ else if (K.ez.z == 0.0f)+ {+ K.GetInverse22(&m_mass);+ m_gamma = 0.0f;+ m_bias = 0.0f;+ }+ else+ {+ K.GetSymInverse33(&m_mass);+ m_gamma = 0.0f;+ m_bias = 0.0f;+ }++ if (data.step.warmStarting)+ {+ // Scale impulses to support a variable time step.+ m_impulse *= data.step.dtRatio;++ b2Vec2 P(m_impulse.x, m_impulse.y);++ vA -= mA * P;+ wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);++ vB += mB * P;+ wB += iB * (b2Cross(m_rB, P) + m_impulse.z);+ }+ else+ {+ m_impulse.SetZero();+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ if (m_frequencyHz > 0.0f)+ {+ float32 Cdot2 = wB - wA;++ float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);+ m_impulse.z += impulse2;++ wA -= iA * impulse2;+ wB += iB * impulse2;++ b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);++ b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);+ m_impulse.x += impulse1.x;+ m_impulse.y += impulse1.y;++ b2Vec2 P = impulse1;++ vA -= mA * P;+ wA -= iA * b2Cross(m_rA, P);++ vB += mB * P;+ wB += iB * b2Cross(m_rB, P);+ }+ else+ {+ b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);+ float32 Cdot2 = wB - wA;+ b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);++ b2Vec3 impulse = -b2Mul(m_mass, Cdot);+ m_impulse += impulse;++ b2Vec2 P(impulse.x, impulse.y);++ vA -= mA * P;+ wA -= iA * (b2Cross(m_rA, P) + impulse.z);++ vB += mB * P;+ wB += iB * (b2Cross(m_rB, P) + impulse.z);+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);++ float32 positionError, angularError;++ b2Mat33 K;+ K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;+ K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;+ K.ez.x = -rA.y * iA - rB.y * iB;+ K.ex.y = K.ey.x;+ K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;+ K.ez.y = rA.x * iA + rB.x * iB;+ K.ex.z = K.ez.x;+ K.ey.z = K.ez.y;+ K.ez.z = iA + iB;++ if (m_frequencyHz > 0.0f)+ {+ b2Vec2 C1 = cB + rB - cA - rA;++ positionError = C1.Length();+ angularError = 0.0f;++ b2Vec2 P = -K.Solve22(C1);++ cA -= mA * P;+ aA -= iA * b2Cross(rA, P);++ cB += mB * P;+ aB += iB * b2Cross(rB, P);+ }+ else+ {+ b2Vec2 C1 = cB + rB - cA - rA;+ float32 C2 = aB - aA - m_referenceAngle;++ positionError = C1.Length();+ angularError = b2Abs(C2);++ b2Vec3 C(C1.x, C1.y, C2);++ b2Vec3 impulse;+ if (K.ez.z > 0.0f)+ {+ impulse = -K.Solve33(C);+ }+ else+ {+ b2Vec2 impulse2 = -K.Solve22(C1);+ impulse.Set(impulse2.x, impulse2.y, 0.0f);+ }++ b2Vec2 P(impulse.x, impulse.y);++ cA -= mA * P;+ aA -= iA * (b2Cross(rA, P) + impulse.z);++ cB += mB * P;+ aB += iB * (b2Cross(rB, P) + impulse.z);+ }++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return positionError <= b2_linearSlop && angularError <= b2_angularSlop;+}++b2Vec2 b2WeldJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2WeldJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const+{+ b2Vec2 P(m_impulse.x, m_impulse.y);+ return inv_dt * P;+}++float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * m_impulse.z;+}++void b2WeldJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2WeldJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);+ b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);+ b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.hnew file mode 100644index 00000000..2adfc26a--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h@@ -0,0 +1,126 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_WELD_JOINT_H+#define B2_WELD_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Weld joint definition. You need to specify local anchor points+/// where they are attached and the relative body angle. The position+/// of the anchor points is important for computing the reaction torque.+struct b2WeldJointDef : public b2JointDef+{+ b2WeldJointDef()+ {+ type = e_weldJoint;+ localAnchorA.Set(0.0f, 0.0f);+ localAnchorB.Set(0.0f, 0.0f);+ referenceAngle = 0.0f;+ frequencyHz = 0.0f;+ dampingRatio = 0.0f;+ }++ /// Initialize the bodies, anchors, and reference angle using a world+ /// anchor point.+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The bodyB angle minus bodyA angle in the reference state (radians).+ float32 referenceAngle;++ /// The mass-spring-damper frequency in Hertz. Rotation only.+ /// Disable softness with a value of 0.+ float32 frequencyHz;++ /// The damping ratio. 0 = no damping, 1 = critical damping.+ float32 dampingRatio;+};++/// A weld joint essentially glues two bodies together. A weld joint may+/// distort somewhat because the island constraint solver is approximate.+class b2WeldJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// Get the reference angle.+ float32 GetReferenceAngle() const { return m_referenceAngle; }++ /// Set/get frequency in Hz.+ void SetFrequency(float32 hz) { m_frequencyHz = hz; }+ float32 GetFrequency() const { return m_frequencyHz; }++ /// Set/get damping ratio.+ void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; }+ float32 GetDampingRatio() const { return m_dampingRatio; }++ /// Dump to b2Log+ void Dump();++protected:++ friend class b2Joint;++ b2WeldJoint(const b2WeldJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ float32 m_frequencyHz;+ float32 m_dampingRatio;+ float32 m_bias;++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ float32 m_referenceAngle;+ float32 m_gamma;+ b2Vec3 m_impulse;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_rA;+ b2Vec2 m_rB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;+ b2Mat33 m_mass;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cppnew file mode 100644index 00000000..74eda681--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp@@ -0,0 +1,419 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/Joints/b2WheelJoint.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++// Linear constraint (point-to-line)+// d = pB - pA = xB + rB - xA - rA+// C = dot(ay, d)+// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))+// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)+// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]++// Spring linear constraint+// C = dot(ax, d)+// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)+// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]++// Motor rotational constraint+// Cdot = wB - wA+// J = [0 0 -1 0 0 1]++void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)+{+ bodyA = bA;+ bodyB = bB;+ localAnchorA = bodyA->GetLocalPoint(anchor);+ localAnchorB = bodyB->GetLocalPoint(anchor);+ localAxisA = bodyA->GetLocalVector(axis);+}++b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)+: b2Joint(def)+{+ m_localAnchorA = def->localAnchorA;+ m_localAnchorB = def->localAnchorB;+ m_localXAxisA = def->localAxisA;+ m_localYAxisA = b2Cross(1.0f, m_localXAxisA);++ m_mass = 0.0f;+ m_impulse = 0.0f;+ m_motorMass = 0.0f;+ m_motorImpulse = 0.0f;+ m_springMass = 0.0f;+ m_springImpulse = 0.0f;++ m_maxMotorTorque = def->maxMotorTorque;+ m_motorSpeed = def->motorSpeed;+ m_enableMotor = def->enableMotor;++ m_frequencyHz = def->frequencyHz;+ m_dampingRatio = def->dampingRatio;++ m_bias = 0.0f;+ m_gamma = 0.0f;++ m_ax.SetZero();+ m_ay.SetZero();+}++void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data)+{+ m_indexA = m_bodyA->m_islandIndex;+ m_indexB = m_bodyB->m_islandIndex;+ m_localCenterA = m_bodyA->m_sweep.localCenter;+ m_localCenterB = m_bodyB->m_sweep.localCenter;+ m_invMassA = m_bodyA->m_invMass;+ m_invMassB = m_bodyB->m_invMass;+ m_invIA = m_bodyA->m_invI;+ m_invIB = m_bodyB->m_invI;++ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;++ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ b2Rot qA(aA), qB(aB);++ // Compute the effective masses.+ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ b2Vec2 d = cB + rB - cA - rA;++ // Point to line constraint+ {+ m_ay = b2Mul(qA, m_localYAxisA);+ m_sAy = b2Cross(d + rA, m_ay);+ m_sBy = b2Cross(rB, m_ay);++ m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;++ if (m_mass > 0.0f)+ {+ m_mass = 1.0f / m_mass;+ }+ }++ // Spring constraint+ m_springMass = 0.0f;+ m_bias = 0.0f;+ m_gamma = 0.0f;+ if (m_frequencyHz > 0.0f)+ {+ m_ax = b2Mul(qA, m_localXAxisA);+ m_sAx = b2Cross(d + rA, m_ax);+ m_sBx = b2Cross(rB, m_ax);++ float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;++ if (invMass > 0.0f)+ {+ m_springMass = 1.0f / invMass;++ float32 C = b2Dot(d, m_ax);++ // Frequency+ float32 omega = 2.0f * b2_pi * m_frequencyHz;++ // Damping coefficient+ float32 d = 2.0f * m_springMass * m_dampingRatio * omega;++ // Spring stiffness+ float32 k = m_springMass * omega * omega;++ // magic formulas+ float32 h = data.step.dt;+ m_gamma = h * (d + h * k);+ if (m_gamma > 0.0f)+ {+ m_gamma = 1.0f / m_gamma;+ }++ m_bias = C * h * k * m_gamma;++ m_springMass = invMass + m_gamma;+ if (m_springMass > 0.0f)+ {+ m_springMass = 1.0f / m_springMass;+ }+ }+ }+ else+ {+ m_springImpulse = 0.0f;+ }++ // Rotational motor+ if (m_enableMotor)+ {+ m_motorMass = iA + iB;+ if (m_motorMass > 0.0f)+ {+ m_motorMass = 1.0f / m_motorMass;+ }+ }+ else+ {+ m_motorMass = 0.0f;+ m_motorImpulse = 0.0f;+ }++ if (data.step.warmStarting)+ {+ // Account for variable time step.+ m_impulse *= data.step.dtRatio;+ m_springImpulse *= data.step.dtRatio;+ m_motorImpulse *= data.step.dtRatio;++ b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;+ float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;+ float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;++ vA -= m_invMassA * P;+ wA -= m_invIA * LA;++ vB += m_invMassB * P;+ wB += m_invIB * LB;+ }+ else+ {+ m_impulse = 0.0f;+ m_springImpulse = 0.0f;+ m_motorImpulse = 0.0f;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)+{+ float32 mA = m_invMassA, mB = m_invMassB;+ float32 iA = m_invIA, iB = m_invIB;++ b2Vec2 vA = data.velocities[m_indexA].v;+ float32 wA = data.velocities[m_indexA].w;+ b2Vec2 vB = data.velocities[m_indexB].v;+ float32 wB = data.velocities[m_indexB].w;++ // Solve spring constraint+ {+ float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;+ float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);+ m_springImpulse += impulse;++ b2Vec2 P = impulse * m_ax;+ float32 LA = impulse * m_sAx;+ float32 LB = impulse * m_sBx;++ vA -= mA * P;+ wA -= iA * LA;++ vB += mB * P;+ wB += iB * LB;+ }++ // Solve rotational motor constraint+ {+ float32 Cdot = wB - wA - m_motorSpeed;+ float32 impulse = -m_motorMass * Cdot;++ float32 oldImpulse = m_motorImpulse;+ float32 maxImpulse = data.step.dt * m_maxMotorTorque;+ m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);+ impulse = m_motorImpulse - oldImpulse;++ wA -= iA * impulse;+ wB += iB * impulse;+ }++ // Solve point to line constraint+ {+ float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;+ float32 impulse = -m_mass * Cdot;+ m_impulse += impulse;++ b2Vec2 P = impulse * m_ay;+ float32 LA = impulse * m_sAy;+ float32 LB = impulse * m_sBy;++ vA -= mA * P;+ wA -= iA * LA;++ vB += mB * P;+ wB += iB * LB;+ }++ data.velocities[m_indexA].v = vA;+ data.velocities[m_indexA].w = wA;+ data.velocities[m_indexB].v = vB;+ data.velocities[m_indexB].w = wB;+}++bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)+{+ b2Vec2 cA = data.positions[m_indexA].c;+ float32 aA = data.positions[m_indexA].a;+ b2Vec2 cB = data.positions[m_indexB].c;+ float32 aB = data.positions[m_indexB].a;++ b2Rot qA(aA), qB(aB);++ b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);+ b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);+ b2Vec2 d = (cB - cA) + rB - rA;++ b2Vec2 ay = b2Mul(qA, m_localYAxisA);++ float32 sAy = b2Cross(d + rA, ay);+ float32 sBy = b2Cross(rB, ay);++ float32 C = b2Dot(d, ay);++ float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;++ float32 impulse;+ if (k != 0.0f)+ {+ impulse = - C / k;+ }+ else+ {+ impulse = 0.0f;+ }++ b2Vec2 P = impulse * ay;+ float32 LA = impulse * sAy;+ float32 LB = impulse * sBy;++ cA -= m_invMassA * P;+ aA -= m_invIA * LA;+ cB += m_invMassB * P;+ aB += m_invIB * LB;++ data.positions[m_indexA].c = cA;+ data.positions[m_indexA].a = aA;+ data.positions[m_indexB].c = cB;+ data.positions[m_indexB].a = aB;++ return b2Abs(C) <= b2_linearSlop;+}++b2Vec2 b2WheelJoint::GetAnchorA() const+{+ return m_bodyA->GetWorldPoint(m_localAnchorA);+}++b2Vec2 b2WheelJoint::GetAnchorB() const+{+ return m_bodyB->GetWorldPoint(m_localAnchorB);+}++b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const+{+ return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);+}++float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const+{+ return inv_dt * m_motorImpulse;+}++float32 b2WheelJoint::GetJointTranslation() const+{+ b2Body* bA = m_bodyA;+ b2Body* bB = m_bodyB;++ b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);+ b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);+ b2Vec2 d = pB - pA;+ b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);++ float32 translation = b2Dot(d, axis);+ return translation;+}++float32 b2WheelJoint::GetJointSpeed() const+{+ float32 wA = m_bodyA->m_angularVelocity;+ float32 wB = m_bodyB->m_angularVelocity;+ return wB - wA;+}++bool b2WheelJoint::IsMotorEnabled() const+{+ return m_enableMotor;+}++void b2WheelJoint::EnableMotor(bool flag)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_enableMotor = flag;+}++void b2WheelJoint::SetMotorSpeed(float32 speed)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_motorSpeed = speed;+}++void b2WheelJoint::SetMaxMotorTorque(float32 torque)+{+ m_bodyA->SetAwake(true);+ m_bodyB->SetAwake(true);+ m_maxMotorTorque = torque;+}++float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const+{+ return inv_dt * m_motorImpulse;+}++void b2WheelJoint::Dump()+{+ int32 indexA = m_bodyA->m_islandIndex;+ int32 indexB = m_bodyB->m_islandIndex;++ b2Log(" b2WheelJointDef jd;\n");+ b2Log(" jd.bodyA = bodies[%d];\n", indexA);+ b2Log(" jd.bodyB = bodies[%d];\n", indexB);+ b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);+ b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);+ b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);+ b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);+ b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);+ b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);+ b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);+ b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);+ b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);+ b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.hnew file mode 100644index 00000000..6e8b6454--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h@@ -0,0 +1,211 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_WHEEL_JOINT_H+#define B2_WHEEL_JOINT_H++#include <Box2D/Dynamics/Joints/b2Joint.h>++/// Wheel joint definition. This requires defining a line of+/// motion using an axis and an anchor point. The definition uses local+/// anchor points and a local axis so that the initial configuration+/// can violate the constraint slightly. The joint translation is zero+/// when the local anchor points coincide in world space. Using local+/// anchors and a local axis helps when saving and loading a game.+struct b2WheelJointDef : public b2JointDef+{+ b2WheelJointDef()+ {+ type = e_wheelJoint;+ localAnchorA.SetZero();+ localAnchorB.SetZero();+ localAxisA.Set(1.0f, 0.0f);+ enableMotor = false;+ maxMotorTorque = 0.0f;+ motorSpeed = 0.0f;+ frequencyHz = 2.0f;+ dampingRatio = 0.7f;+ }++ /// Initialize the bodies, anchors, axis, and reference angle using the world+ /// anchor and world axis.+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);++ /// The local anchor point relative to bodyA's origin.+ b2Vec2 localAnchorA;++ /// The local anchor point relative to bodyB's origin.+ b2Vec2 localAnchorB;++ /// The local translation axis in bodyA.+ b2Vec2 localAxisA;++ /// Enable/disable the joint motor.+ bool enableMotor;++ /// The maximum motor torque, usually in N-m.+ float32 maxMotorTorque;++ /// The desired motor speed in radians per second.+ float32 motorSpeed;++ /// Suspension frequency, zero indicates no suspension+ float32 frequencyHz;++ /// Suspension damping ratio, one indicates critical damping+ float32 dampingRatio;+};++/// A wheel joint. This joint provides two degrees of freedom: translation+/// along an axis fixed in bodyA and rotation in the plane. You can use a+/// joint limit to restrict the range of motion and a joint motor to drive+/// the rotation or to model rotational friction.+/// This joint is designed for vehicle suspensions.+class b2WheelJoint : public b2Joint+{+public:+ b2Vec2 GetAnchorA() const;+ b2Vec2 GetAnchorB() const;++ b2Vec2 GetReactionForce(float32 inv_dt) const;+ float32 GetReactionTorque(float32 inv_dt) const;++ /// The local anchor point relative to bodyA's origin.+ const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }++ /// The local anchor point relative to bodyB's origin.+ const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }++ /// The local joint axis relative to bodyA.+ const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }++ /// Get the current joint translation, usually in meters.+ float32 GetJointTranslation() const;++ /// Get the current joint translation speed, usually in meters per second.+ float32 GetJointSpeed() const;++ /// Is the joint motor enabled?+ bool IsMotorEnabled() const;++ /// Enable/disable the joint motor.+ void EnableMotor(bool flag);++ /// Set the motor speed, usually in radians per second.+ void SetMotorSpeed(float32 speed);++ /// Get the motor speed, usually in radians per second.+ float32 GetMotorSpeed() const;++ /// Set/Get the maximum motor force, usually in N-m.+ void SetMaxMotorTorque(float32 torque);+ float32 GetMaxMotorTorque() const;++ /// Get the current motor torque given the inverse time step, usually in N-m.+ float32 GetMotorTorque(float32 inv_dt) const;++ /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring.+ void SetSpringFrequencyHz(float32 hz);+ float32 GetSpringFrequencyHz() const;++ /// Set/Get the spring damping ratio+ void SetSpringDampingRatio(float32 ratio);+ float32 GetSpringDampingRatio() const;++ /// Dump to b2Log+ void Dump();++protected:++ friend class b2Joint;+ b2WheelJoint(const b2WheelJointDef* def);++ void InitVelocityConstraints(const b2SolverData& data);+ void SolveVelocityConstraints(const b2SolverData& data);+ bool SolvePositionConstraints(const b2SolverData& data);++ float32 m_frequencyHz;+ float32 m_dampingRatio;++ // Solver shared+ b2Vec2 m_localAnchorA;+ b2Vec2 m_localAnchorB;+ b2Vec2 m_localXAxisA;+ b2Vec2 m_localYAxisA;++ float32 m_impulse;+ float32 m_motorImpulse;+ float32 m_springImpulse;++ float32 m_maxMotorTorque;+ float32 m_motorSpeed;+ bool m_enableMotor;++ // Solver temp+ int32 m_indexA;+ int32 m_indexB;+ b2Vec2 m_localCenterA;+ b2Vec2 m_localCenterB;+ float32 m_invMassA;+ float32 m_invMassB;+ float32 m_invIA;+ float32 m_invIB;++ b2Vec2 m_ax, m_ay;+ float32 m_sAx, m_sBx;+ float32 m_sAy, m_sBy;++ float32 m_mass;+ float32 m_motorMass;+ float32 m_springMass;++ float32 m_bias;+ float32 m_gamma;+};++inline float32 b2WheelJoint::GetMotorSpeed() const+{+ return m_motorSpeed;+}++inline float32 b2WheelJoint::GetMaxMotorTorque() const+{+ return m_maxMotorTorque;+}++inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz)+{+ m_frequencyHz = hz;+}++inline float32 b2WheelJoint::GetSpringFrequencyHz() const+{+ return m_frequencyHz;+}++inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio)+{+ m_dampingRatio = ratio;+}++inline float32 b2WheelJoint::GetSpringDampingRatio() const+{+ return m_dampingRatio;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.cppnew file mode 100644index 00000000..d99fc49f--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.cpp@@ -0,0 +1,549 @@+/*+* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/Joints/b2Joint.h>++b2Body::b2Body(const b2BodyDef* bd, b2World* world)+{+ b2Assert(bd->position.IsValid());+ b2Assert(bd->linearVelocity.IsValid());+ b2Assert(b2IsValid(bd->angle));+ b2Assert(b2IsValid(bd->angularVelocity));+ b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);+ b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);++ m_flags = 0;++ if (bd->bullet)+ {+ m_flags |= e_bulletFlag;+ }+ if (bd->fixedRotation)+ {+ m_flags |= e_fixedRotationFlag;+ }+ if (bd->allowSleep)+ {+ m_flags |= e_autoSleepFlag;+ }+ if (bd->awake)+ {+ m_flags |= e_awakeFlag;+ }+ if (bd->active)+ {+ m_flags |= e_activeFlag;+ }++ m_world = world;++ m_xf.p = bd->position;+ m_xf.q.Set(bd->angle);++ m_sweep.localCenter.SetZero();+ m_sweep.c0 = m_xf.p;+ m_sweep.c = m_xf.p;+ m_sweep.a0 = bd->angle;+ m_sweep.a = bd->angle;+ m_sweep.alpha0 = 0.0f;++ m_jointList = NULL;+ m_contactList = NULL;+ m_prev = NULL;+ m_next = NULL;++ m_linearVelocity = bd->linearVelocity;+ m_angularVelocity = bd->angularVelocity;++ m_linearDamping = bd->linearDamping;+ m_angularDamping = bd->angularDamping;+ m_gravityScale = bd->gravityScale;++ m_force.SetZero();+ m_torque = 0.0f;++ m_sleepTime = 0.0f;++ m_type = bd->type;++ if (m_type == b2_dynamicBody)+ {+ m_mass = 1.0f;+ m_invMass = 1.0f;+ }+ else+ {+ m_mass = 0.0f;+ m_invMass = 0.0f;+ }++ m_I = 0.0f;+ m_invI = 0.0f;++ m_userData = bd->userData;++ m_fixtureList = NULL;+ m_fixtureCount = 0;+}++b2Body::~b2Body()+{+ // shapes and joints are destroyed in b2World::Destroy+}++void b2Body::SetType(b2BodyType type)+{+ b2Assert(m_world->IsLocked() == false);+ if (m_world->IsLocked() == true)+ {+ return;+ }++ if (m_type == type)+ {+ return;+ }++ m_type = type;++ ResetMassData();++ if (m_type == b2_staticBody)+ {+ m_linearVelocity.SetZero();+ m_angularVelocity = 0.0f;+ m_sweep.a0 = m_sweep.a;+ m_sweep.c0 = m_sweep.c;+ SynchronizeFixtures();+ }++ SetAwake(true);++ m_force.SetZero();+ m_torque = 0.0f;++ // Delete the attached contacts.+ b2ContactEdge* ce = m_contactList;+ while (ce)+ {+ b2ContactEdge* ce0 = ce;+ ce = ce->next;+ m_world->m_contactManager.Destroy(ce0->contact);+ }+ m_contactList = NULL;++ // Touch the proxies so that new contacts will be created (when appropriate)+ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ int32 proxyCount = f->m_proxyCount;+ for (int32 i = 0; i < proxyCount; ++i)+ {+ broadPhase->TouchProxy(f->m_proxies[i].proxyId);+ }+ }+}++b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)+{+ b2Assert(m_world->IsLocked() == false);+ if (m_world->IsLocked() == true)+ {+ return NULL;+ }++ b2BlockAllocator* allocator = &m_world->m_blockAllocator;++ void* memory = allocator->Allocate(sizeof(b2Fixture));+ b2Fixture* fixture = new (memory) b2Fixture;+ fixture->Create(allocator, this, def);++ if (m_flags & e_activeFlag)+ {+ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ fixture->CreateProxies(broadPhase, m_xf);+ }++ fixture->m_next = m_fixtureList;+ m_fixtureList = fixture;+ ++m_fixtureCount;++ fixture->m_body = this;++ // Adjust mass properties if needed.+ if (fixture->m_density > 0.0f)+ {+ ResetMassData();+ }++ // Let the world know we have a new fixture. This will cause new contacts+ // to be created at the beginning of the next time step.+ m_world->m_flags |= b2World::e_newFixture;++ return fixture;+}++b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)+{+ b2FixtureDef def;+ def.shape = shape;+ def.density = density;++ return CreateFixture(&def);+}++void b2Body::DestroyFixture(b2Fixture* fixture)+{+ b2Assert(m_world->IsLocked() == false);+ if (m_world->IsLocked() == true)+ {+ return;+ }++ b2Assert(fixture->m_body == this);++ // Remove the fixture from this body's singly linked list.+ b2Assert(m_fixtureCount > 0);+ b2Fixture** node = &m_fixtureList;+ bool found = false;+ while (*node != NULL)+ {+ if (*node == fixture)+ {+ *node = fixture->m_next;+ found = true;+ break;+ }++ node = &(*node)->m_next;+ }++ // You tried to remove a shape that is not attached to this body.+ b2Assert(found);++ // Destroy any contacts associated with the fixture.+ b2ContactEdge* edge = m_contactList;+ while (edge)+ {+ b2Contact* c = edge->contact;+ edge = edge->next;++ b2Fixture* fixtureA = c->GetFixtureA();+ b2Fixture* fixtureB = c->GetFixtureB();++ if (fixture == fixtureA || fixture == fixtureB)+ {+ // This destroys the contact and removes it from+ // this body's contact list.+ m_world->m_contactManager.Destroy(c);+ }+ }++ b2BlockAllocator* allocator = &m_world->m_blockAllocator;++ if (m_flags & e_activeFlag)+ {+ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ fixture->DestroyProxies(broadPhase);+ }++ fixture->Destroy(allocator);+ fixture->m_body = NULL;+ fixture->m_next = NULL;+ fixture->~b2Fixture();+ allocator->Free(fixture, sizeof(b2Fixture));++ --m_fixtureCount;++ // Reset the mass data.+ ResetMassData();+}++void b2Body::ResetMassData()+{+ // Compute mass data from shapes. Each shape has its own density.+ m_mass = 0.0f;+ m_invMass = 0.0f;+ m_I = 0.0f;+ m_invI = 0.0f;+ m_sweep.localCenter.SetZero();++ // Static and kinematic bodies have zero mass.+ if (m_type == b2_staticBody || m_type == b2_kinematicBody)+ {+ m_sweep.c0 = m_xf.p;+ m_sweep.c = m_xf.p;+ m_sweep.a0 = m_sweep.a;+ return;+ }++ b2Assert(m_type == b2_dynamicBody);++ // Accumulate mass over all fixtures.+ b2Vec2 localCenter = b2Vec2_zero;+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ if (f->m_density == 0.0f)+ {+ continue;+ }++ b2MassData massData;+ f->GetMassData(&massData);+ m_mass += massData.mass;+ localCenter += massData.mass * massData.center;+ m_I += massData.I;+ }++ // Compute center of mass.+ if (m_mass > 0.0f)+ {+ m_invMass = 1.0f / m_mass;+ localCenter *= m_invMass;+ }+ else+ {+ // Force all dynamic bodies to have a positive mass.+ m_mass = 1.0f;+ m_invMass = 1.0f;+ }++ if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)+ {+ // Center the inertia about the center of mass.+ m_I -= m_mass * b2Dot(localCenter, localCenter);+ b2Assert(m_I > 0.0f);+ m_invI = 1.0f / m_I;++ }+ else+ {+ m_I = 0.0f;+ m_invI = 0.0f;+ }++ // Move center of mass.+ b2Vec2 oldCenter = m_sweep.c;+ m_sweep.localCenter = localCenter;+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);++ // Update center of mass velocity.+ m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);+}++void b2Body::SetMassData(const b2MassData* massData)+{+ b2Assert(m_world->IsLocked() == false);+ if (m_world->IsLocked() == true)+ {+ return;+ }++ if (m_type != b2_dynamicBody)+ {+ return;+ }++ m_invMass = 0.0f;+ m_I = 0.0f;+ m_invI = 0.0f;++ m_mass = massData->mass;+ if (m_mass <= 0.0f)+ {+ m_mass = 1.0f;+ }++ m_invMass = 1.0f / m_mass;++ if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)+ {+ m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);+ b2Assert(m_I > 0.0f);+ m_invI = 1.0f / m_I;+ }++ // Move center of mass.+ b2Vec2 oldCenter = m_sweep.c;+ m_sweep.localCenter = massData->center;+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);++ // Update center of mass velocity.+ m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);+}++bool b2Body::ShouldCollide(const b2Body* other) const+{+ // At least one body should be dynamic.+ if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)+ {+ return false;+ }++ // Does a joint prevent collision?+ for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)+ {+ if (jn->other == other)+ {+ if (jn->joint->m_collideConnected == false)+ {+ return false;+ }+ }+ }++ return true;+}++void b2Body::SetTransform(const b2Vec2& position, float32 angle)+{+ b2Assert(m_world->IsLocked() == false);+ if (m_world->IsLocked() == true)+ {+ return;+ }++ m_xf.q.Set(angle);+ m_xf.p = position;++ m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);+ m_sweep.a = angle;++ m_sweep.c0 = m_sweep.c;+ m_sweep.a0 = angle;++ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ f->Synchronize(broadPhase, m_xf, m_xf);+ }+}++void b2Body::SynchronizeFixtures()+{+ b2Transform xf1;+ xf1.q.Set(m_sweep.a0);+ xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);++ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ f->Synchronize(broadPhase, xf1, m_xf);+ }+}++void b2Body::SetActive(bool flag)+{+ b2Assert(m_world->IsLocked() == false);++ if (flag == IsActive())+ {+ return;+ }++ if (flag)+ {+ m_flags |= e_activeFlag;++ // Create all proxies.+ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ f->CreateProxies(broadPhase, m_xf);+ }++ // Contacts are created the next time step.+ }+ else+ {+ m_flags &= ~e_activeFlag;++ // Destroy all proxies.+ b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ f->DestroyProxies(broadPhase);+ }++ // Destroy the attached contacts.+ b2ContactEdge* ce = m_contactList;+ while (ce)+ {+ b2ContactEdge* ce0 = ce;+ ce = ce->next;+ m_world->m_contactManager.Destroy(ce0->contact);+ }+ m_contactList = NULL;+ }+}++void b2Body::SetFixedRotation(bool flag)+{+ bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;+ if (status == flag)+ {+ return;+ }++ if (flag)+ {+ m_flags |= e_fixedRotationFlag;+ }+ else+ {+ m_flags &= ~e_fixedRotationFlag;+ }++ m_angularVelocity = 0.0f;++ ResetMassData();+}++void b2Body::Dump()+{+ int32 bodyIndex = m_islandIndex;++ b2Log("{\n");+ b2Log(" b2BodyDef bd;\n");+ b2Log(" bd.type = b2BodyType(%d);\n", m_type);+ b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);+ b2Log(" bd.angle = %.15lef;\n", m_sweep.a);+ b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);+ b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity);+ b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping);+ b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping);+ b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);+ b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag);+ b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);+ b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);+ b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag);+ b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale);+ b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);+ b2Log("\n");+ for (b2Fixture* f = m_fixtureList; f; f = f->m_next)+ {+ b2Log(" {\n");+ f->Dump(bodyIndex);+ b2Log(" }\n");+ }+ b2Log("}\n");+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.hnew file mode 100644index 00000000..5c4d96a5--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.h@@ -0,0 +1,860 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_BODY_H+#define B2_BODY_H++#include <Box2D/Common/b2Math.h>+#include <Box2D/Collision/Shapes/b2Shape.h>+#include <memory>++class b2Fixture;+class b2Joint;+class b2Contact;+class b2Controller;+class b2World;+struct b2FixtureDef;+struct b2JointEdge;+struct b2ContactEdge;++/// The body type.+/// static: zero mass, zero velocity, may be manually moved+/// kinematic: zero mass, non-zero velocity set by user, moved by solver+/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver+enum b2BodyType+{+ b2_staticBody = 0,+ b2_kinematicBody,+ b2_dynamicBody++ // TODO_ERIN+ //b2_bulletBody,+};++/// A body definition holds all the data needed to construct a rigid body.+/// You can safely re-use body definitions. Shapes are added to a body after construction.+struct b2BodyDef+{+ /// This constructor sets the body definition default values.+ b2BodyDef()+ {+ userData = NULL;+ position.Set(0.0f, 0.0f);+ angle = 0.0f;+ linearVelocity.Set(0.0f, 0.0f);+ angularVelocity = 0.0f;+ linearDamping = 0.0f;+ angularDamping = 0.0f;+ allowSleep = true;+ awake = true;+ fixedRotation = false;+ bullet = false;+ type = b2_staticBody;+ active = true;+ gravityScale = 1.0f;+ }++ /// The body type: static, kinematic, or dynamic.+ /// Note: if a dynamic body would have zero mass, the mass is set to one.+ b2BodyType type;++ /// The world position of the body. Avoid creating bodies at the origin+ /// since this can lead to many overlapping shapes.+ b2Vec2 position;++ /// The world angle of the body in radians.+ float32 angle;++ /// The linear velocity of the body's origin in world co-ordinates.+ b2Vec2 linearVelocity;++ /// The angular velocity of the body.+ float32 angularVelocity;++ /// Linear damping is use to reduce the linear velocity. The damping parameter+ /// can be larger than 1.0f but the damping effect becomes sensitive to the+ /// time step when the damping parameter is large.+ float32 linearDamping;++ /// Angular damping is use to reduce the angular velocity. The damping parameter+ /// can be larger than 1.0f but the damping effect becomes sensitive to the+ /// time step when the damping parameter is large.+ float32 angularDamping;++ /// Set this flag to false if this body should never fall asleep. Note that+ /// this increases CPU usage.+ bool allowSleep;++ /// Is this body initially awake or sleeping?+ bool awake;++ /// Should this body be prevented from rotating? Useful for characters.+ bool fixedRotation;++ /// Is this a fast moving body that should be prevented from tunneling through+ /// other moving bodies? Note that all bodies are prevented from tunneling through+ /// kinematic and static bodies. This setting is only considered on dynamic bodies.+ /// @warning You should use this flag sparingly since it increases processing time.+ bool bullet;++ /// Does this body start out active?+ bool active;++ /// Use this to store application specific body data.+ void* userData;++ /// Scale the gravity applied to this body.+ float32 gravityScale;+};++/// A rigid body. These are created via b2World::CreateBody.+class b2Body+{+public:+ /// Creates a fixture and attach it to this body. Use this function if you need+ /// to set some fixture parameters, like friction. Otherwise you can create the+ /// fixture directly from a shape.+ /// If the density is non-zero, this function automatically updates the mass of the body.+ /// Contacts are not created until the next time step.+ /// @param def the fixture definition.+ /// @warning This function is locked during callbacks.+ b2Fixture* CreateFixture(const b2FixtureDef* def);++ /// Creates a fixture from a shape and attach it to this body.+ /// This is a convenience function. Use b2FixtureDef if you need to set parameters+ /// like friction, restitution, user data, or filtering.+ /// If the density is non-zero, this function automatically updates the mass of the body.+ /// @param shape the shape to be cloned.+ /// @param density the shape density (set to zero for static bodies).+ /// @warning This function is locked during callbacks.+ b2Fixture* CreateFixture(const b2Shape* shape, float32 density);++ /// Destroy a fixture. This removes the fixture from the broad-phase and+ /// destroys all contacts associated with this fixture. This will+ /// automatically adjust the mass of the body if the body is dynamic and the+ /// fixture has positive density.+ /// All fixtures attached to a body are implicitly destroyed when the body is destroyed.+ /// @param fixture the fixture to be removed.+ /// @warning This function is locked during callbacks.+ void DestroyFixture(b2Fixture* fixture);++ /// Set the position of the body's origin and rotation.+ /// Manipulating a body's transform may cause non-physical behavior.+ /// Note: contacts are updated on the next call to b2World::Step.+ /// @param position the world position of the body's local origin.+ /// @param angle the world rotation in radians.+ void SetTransform(const b2Vec2& position, float32 angle);++ /// Get the body transform for the body's origin.+ /// @return the world transform of the body's origin.+ const b2Transform& GetTransform() const;++ /// Get the world body origin position.+ /// @return the world position of the body's origin.+ const b2Vec2& GetPosition() const;++ /// Get the angle in radians.+ /// @return the current world rotation angle in radians.+ float32 GetAngle() const;++ /// Get the world position of the center of mass.+ const b2Vec2& GetWorldCenter() const;++ /// Get the local position of the center of mass.+ const b2Vec2& GetLocalCenter() const;++ /// Set the linear velocity of the center of mass.+ /// @param v the new linear velocity of the center of mass.+ void SetLinearVelocity(const b2Vec2& v);++ /// Get the linear velocity of the center of mass.+ /// @return the linear velocity of the center of mass.+ const b2Vec2& GetLinearVelocity() const;++ /// Set the angular velocity.+ /// @param omega the new angular velocity in radians/second.+ void SetAngularVelocity(float32 omega);++ /// Get the angular velocity.+ /// @return the angular velocity in radians/second.+ float32 GetAngularVelocity() const;++ /// Apply a force at a world point. If the force is not+ /// applied at the center of mass, it will generate a torque and+ /// affect the angular velocity. This wakes up the body.+ /// @param force the world force vector, usually in Newtons (N).+ /// @param point the world position of the point of application.+ /// @param wake also wake up the body+ void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake);++ /// Apply a force to the center of mass. This wakes up the body.+ /// @param force the world force vector, usually in Newtons (N).+ /// @param wake also wake up the body+ void ApplyForceToCenter(const b2Vec2& force, bool wake);++ /// Apply a torque. This affects the angular velocity+ /// without affecting the linear velocity of the center of mass.+ /// This wakes up the body.+ /// @param torque about the z-axis (out of the screen), usually in N-m.+ /// @param wake also wake up the body+ void ApplyTorque(float32 torque, bool wake);++ /// Apply an impulse at a point. This immediately modifies the velocity.+ /// It also modifies the angular velocity if the point of application+ /// is not at the center of mass. This wakes up the body.+ /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.+ /// @param point the world position of the point of application.+ /// @param wake also wake up the body+ void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake);++ /// Apply an angular impulse.+ /// @param impulse the angular impulse in units of kg*m*m/s+ /// @param wake also wake up the body+ void ApplyAngularImpulse(float32 impulse, bool wake);++ /// Get the total mass of the body.+ /// @return the mass, usually in kilograms (kg).+ float32 GetMass() const;++ /// Get the rotational inertia of the body about the local origin.+ /// @return the rotational inertia, usually in kg-m^2.+ float32 GetInertia() const;++ /// Get the mass data of the body.+ /// @return a struct containing the mass, inertia and center of the body.+ void GetMassData(b2MassData* data) const;++ /// Set the mass properties to override the mass properties of the fixtures.+ /// Note that this changes the center of mass position.+ /// Note that creating or destroying fixtures can also alter the mass.+ /// This function has no effect if the body isn't dynamic.+ /// @param massData the mass properties.+ void SetMassData(const b2MassData* data);++ /// This resets the mass properties to the sum of the mass properties of the fixtures.+ /// This normally does not need to be called unless you called SetMassData to override+ /// the mass and you later want to reset the mass.+ void ResetMassData();++ /// Get the world coordinates of a point given the local coordinates.+ /// @param localPoint a point on the body measured relative the the body's origin.+ /// @return the same point expressed in world coordinates.+ b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const;++ /// Get the world coordinates of a vector given the local coordinates.+ /// @param localVector a vector fixed in the body.+ /// @return the same vector expressed in world coordinates.+ b2Vec2 GetWorldVector(const b2Vec2& localVector) const;++ /// Gets a local point relative to the body's origin given a world point.+ /// @param a point in world coordinates.+ /// @return the corresponding local point relative to the body's origin.+ b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const;++ /// Gets a local vector given a world vector.+ /// @param a vector in world coordinates.+ /// @return the corresponding local vector.+ b2Vec2 GetLocalVector(const b2Vec2& worldVector) const;++ /// Get the world linear velocity of a world point attached to this body.+ /// @param a point in world coordinates.+ /// @return the world velocity of a point.+ b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;++ /// Get the world velocity of a local point.+ /// @param a point in local coordinates.+ /// @return the world velocity of a point.+ b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;++ /// Get the linear damping of the body.+ float32 GetLinearDamping() const;++ /// Set the linear damping of the body.+ void SetLinearDamping(float32 linearDamping);++ /// Get the angular damping of the body.+ float32 GetAngularDamping() const;++ /// Set the angular damping of the body.+ void SetAngularDamping(float32 angularDamping);++ /// Get the gravity scale of the body.+ float32 GetGravityScale() const;++ /// Set the gravity scale of the body.+ void SetGravityScale(float32 scale);++ /// Set the type of this body. This may alter the mass and velocity.+ void SetType(b2BodyType type);++ /// Get the type of this body.+ b2BodyType GetType() const;++ /// Should this body be treated like a bullet for continuous collision detection?+ void SetBullet(bool flag);++ /// Is this body treated like a bullet for continuous collision detection?+ bool IsBullet() const;++ /// You can disable sleeping on this body. If you disable sleeping, the+ /// body will be woken.+ void SetSleepingAllowed(bool flag);++ /// Is this body allowed to sleep+ bool IsSleepingAllowed() const;++ /// Set the sleep state of the body. A sleeping body has very+ /// low CPU cost.+ /// @param flag set to true to wake the body, false to put it to sleep.+ void SetAwake(bool flag);++ /// Get the sleeping state of this body.+ /// @return true if the body is awake.+ bool IsAwake() const;++ /// Set the active state of the body. An inactive body is not+ /// simulated and cannot be collided with or woken up.+ /// If you pass a flag of true, all fixtures will be added to the+ /// broad-phase.+ /// If you pass a flag of false, all fixtures will be removed from+ /// the broad-phase and all contacts will be destroyed.+ /// Fixtures and joints are otherwise unaffected. You may continue+ /// to create/destroy fixtures and joints on inactive bodies.+ /// Fixtures on an inactive body are implicitly inactive and will+ /// not participate in collisions, ray-casts, or queries.+ /// Joints connected to an inactive body are implicitly inactive.+ /// An inactive body is still owned by a b2World object and remains+ /// in the body list.+ void SetActive(bool flag);++ /// Get the active state of the body.+ bool IsActive() const;++ /// Set this body to have fixed rotation. This causes the mass+ /// to be reset.+ void SetFixedRotation(bool flag);++ /// Does this body have fixed rotation?+ bool IsFixedRotation() const;++ /// Get the list of all fixtures attached to this body.+ b2Fixture* GetFixtureList();+ const b2Fixture* GetFixtureList() const;++ /// Get the list of all joints attached to this body.+ b2JointEdge* GetJointList();+ const b2JointEdge* GetJointList() const;++ /// Get the list of all contacts attached to this body.+ /// @warning this list changes during the time step and you may+ /// miss some collisions if you don't use b2ContactListener.+ b2ContactEdge* GetContactList();+ const b2ContactEdge* GetContactList() const;++ /// Get the next body in the world's body list.+ b2Body* GetNext();+ const b2Body* GetNext() const;++ /// Get the user data pointer that was provided in the body definition.+ void* GetUserData() const;++ /// Set the user data. Use this to store your application specific data.+ void SetUserData(void* data);++ /// Get the parent world of this body.+ b2World* GetWorld();+ const b2World* GetWorld() const;++ /// Dump this body to a log file+ void Dump();++private:++ friend class b2World;+ friend class b2Island;+ friend class b2ContactManager;+ friend class b2ContactSolver;+ friend class b2Contact;++ friend class b2DistanceJoint;+ friend class b2FrictionJoint;+ friend class b2GearJoint;+ friend class b2MotorJoint;+ friend class b2MouseJoint;+ friend class b2PrismaticJoint;+ friend class b2PulleyJoint;+ friend class b2RevoluteJoint;+ friend class b2RopeJoint;+ friend class b2WeldJoint;+ friend class b2WheelJoint;++ // m_flags+ enum+ {+ e_islandFlag = 0x0001,+ e_awakeFlag = 0x0002,+ e_autoSleepFlag = 0x0004,+ e_bulletFlag = 0x0008,+ e_fixedRotationFlag = 0x0010,+ e_activeFlag = 0x0020,+ e_toiFlag = 0x0040+ };++ b2Body(const b2BodyDef* bd, b2World* world);+ ~b2Body();++ void SynchronizeFixtures();+ void SynchronizeTransform();++ // This is used to prevent connected bodies from colliding.+ // It may lie, depending on the collideConnected flag.+ bool ShouldCollide(const b2Body* other) const;++ void Advance(float32 t);++ b2BodyType m_type;++ uint16 m_flags;++ int32 m_islandIndex;++ b2Transform m_xf; // the body origin transform+ b2Sweep m_sweep; // the swept motion for CCD++ b2Vec2 m_linearVelocity;+ float32 m_angularVelocity;++ b2Vec2 m_force;+ float32 m_torque;++ b2World* m_world;+ b2Body* m_prev;+ b2Body* m_next;++ b2Fixture* m_fixtureList;+ int32 m_fixtureCount;++ b2JointEdge* m_jointList;+ b2ContactEdge* m_contactList;++ float32 m_mass, m_invMass;++ // Rotational inertia about the center of mass.+ float32 m_I, m_invI;++ float32 m_linearDamping;+ float32 m_angularDamping;+ float32 m_gravityScale;++ float32 m_sleepTime;++ void* m_userData;+};++inline b2BodyType b2Body::GetType() const+{+ return m_type;+}++inline const b2Transform& b2Body::GetTransform() const+{+ return m_xf;+}++inline const b2Vec2& b2Body::GetPosition() const+{+ return m_xf.p;+}++inline float32 b2Body::GetAngle() const+{+ return m_sweep.a;+}++inline const b2Vec2& b2Body::GetWorldCenter() const+{+ return m_sweep.c;+}++inline const b2Vec2& b2Body::GetLocalCenter() const+{+ return m_sweep.localCenter;+}++inline void b2Body::SetLinearVelocity(const b2Vec2& v)+{+ if (m_type == b2_staticBody)+ {+ return;+ }++ if (b2Dot(v,v) > 0.0f)+ {+ SetAwake(true);+ }++ m_linearVelocity = v;+}++inline const b2Vec2& b2Body::GetLinearVelocity() const+{+ return m_linearVelocity;+}++inline void b2Body::SetAngularVelocity(float32 w)+{+ if (m_type == b2_staticBody)+ {+ return;+ }++ if (w * w > 0.0f)+ {+ SetAwake(true);+ }++ m_angularVelocity = w;+}++inline float32 b2Body::GetAngularVelocity() const+{+ return m_angularVelocity;+}++inline float32 b2Body::GetMass() const+{+ return m_mass;+}++inline float32 b2Body::GetInertia() const+{+ return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);+}++inline void b2Body::GetMassData(b2MassData* data) const+{+ data->mass = m_mass;+ data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);+ data->center = m_sweep.localCenter;+}++inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const+{+ return b2Mul(m_xf, localPoint);+}++inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const+{+ return b2Mul(m_xf.q, localVector);+}++inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const+{+ return b2MulT(m_xf, worldPoint);+}++inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const+{+ return b2MulT(m_xf.q, worldVector);+}++inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const+{+ return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);+}++inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const+{+ return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));+}++inline float32 b2Body::GetLinearDamping() const+{+ return m_linearDamping;+}++inline void b2Body::SetLinearDamping(float32 linearDamping)+{+ m_linearDamping = linearDamping;+}++inline float32 b2Body::GetAngularDamping() const+{+ return m_angularDamping;+}++inline void b2Body::SetAngularDamping(float32 angularDamping)+{+ m_angularDamping = angularDamping;+}++inline float32 b2Body::GetGravityScale() const+{+ return m_gravityScale;+}++inline void b2Body::SetGravityScale(float32 scale)+{+ m_gravityScale = scale;+}++inline void b2Body::SetBullet(bool flag)+{+ if (flag)+ {+ m_flags |= e_bulletFlag;+ }+ else+ {+ m_flags &= ~e_bulletFlag;+ }+}++inline bool b2Body::IsBullet() const+{+ return (m_flags & e_bulletFlag) == e_bulletFlag;+}++inline void b2Body::SetAwake(bool flag)+{+ if (flag)+ {+ if ((m_flags & e_awakeFlag) == 0)+ {+ m_flags |= e_awakeFlag;+ m_sleepTime = 0.0f;+ }+ }+ else+ {+ m_flags &= ~e_awakeFlag;+ m_sleepTime = 0.0f;+ m_linearVelocity.SetZero();+ m_angularVelocity = 0.0f;+ m_force.SetZero();+ m_torque = 0.0f;+ }+}++inline bool b2Body::IsAwake() const+{+ return (m_flags & e_awakeFlag) == e_awakeFlag;+}++inline bool b2Body::IsActive() const+{+ return (m_flags & e_activeFlag) == e_activeFlag;+}++inline bool b2Body::IsFixedRotation() const+{+ return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;+}++inline void b2Body::SetSleepingAllowed(bool flag)+{+ if (flag)+ {+ m_flags |= e_autoSleepFlag;+ }+ else+ {+ m_flags &= ~e_autoSleepFlag;+ SetAwake(true);+ }+}++inline bool b2Body::IsSleepingAllowed() const+{+ return (m_flags & e_autoSleepFlag) == e_autoSleepFlag;+}++inline b2Fixture* b2Body::GetFixtureList()+{+ return m_fixtureList;+}++inline const b2Fixture* b2Body::GetFixtureList() const+{+ return m_fixtureList;+}++inline b2JointEdge* b2Body::GetJointList()+{+ return m_jointList;+}++inline const b2JointEdge* b2Body::GetJointList() const+{+ return m_jointList;+}++inline b2ContactEdge* b2Body::GetContactList()+{+ return m_contactList;+}++inline const b2ContactEdge* b2Body::GetContactList() const+{+ return m_contactList;+}++inline b2Body* b2Body::GetNext()+{+ return m_next;+}++inline const b2Body* b2Body::GetNext() const+{+ return m_next;+}++inline void b2Body::SetUserData(void* data)+{+ m_userData = data;+}++inline void* b2Body::GetUserData() const+{+ return m_userData;+}++inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake)+{+ if (m_type != b2_dynamicBody)+ {+ return;+ }++ if (wake && (m_flags & e_awakeFlag) == 0)+ {+ SetAwake(true);+ }++ // Don't accumulate a force if the body is sleeping.+ if (m_flags & e_awakeFlag)+ {+ m_force += force;+ m_torque += b2Cross(point - m_sweep.c, force);+ }+}++inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake)+{+ if (m_type != b2_dynamicBody)+ {+ return;+ }++ if (wake && (m_flags & e_awakeFlag) == 0)+ {+ SetAwake(true);+ }++ // Don't accumulate a force if the body is sleeping+ if (m_flags & e_awakeFlag)+ {+ m_force += force;+ }+}++inline void b2Body::ApplyTorque(float32 torque, bool wake)+{+ if (m_type != b2_dynamicBody)+ {+ return;+ }++ if (wake && (m_flags & e_awakeFlag) == 0)+ {+ SetAwake(true);+ }++ // Don't accumulate a force if the body is sleeping+ if (m_flags & e_awakeFlag)+ {+ m_torque += torque;+ }+}++inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake)+{+ if (m_type != b2_dynamicBody)+ {+ return;+ }++ if (wake && (m_flags & e_awakeFlag) == 0)+ {+ SetAwake(true);+ }++ // Don't accumulate velocity if the body is sleeping+ if (m_flags & e_awakeFlag)+ {+ m_linearVelocity += m_invMass * impulse;+ m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);+ }+}++inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake)+{+ if (m_type != b2_dynamicBody)+ {+ return;+ }++ if (wake && (m_flags & e_awakeFlag) == 0)+ {+ SetAwake(true);+ }++ // Don't accumulate velocity if the body is sleeping+ if (m_flags & e_awakeFlag)+ {+ m_angularVelocity += m_invI * impulse;+ }+}++inline void b2Body::SynchronizeTransform()+{+ m_xf.q.Set(m_sweep.a);+ m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);+}++inline void b2Body::Advance(float32 alpha)+{+ // Advance to the new safe time. This doesn't sync the broad-phase.+ m_sweep.Advance(alpha);+ m_sweep.c = m_sweep.c0;+ m_sweep.a = m_sweep.a0;+ m_xf.q.Set(m_sweep.a);+ m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);+}++inline b2World* b2Body::GetWorld()+{+ return m_world;+}++inline const b2World* b2Body::GetWorld() const+{+ return m_world;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.cppnew file mode 100644index 00000000..caebc89e--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.cpp@@ -0,0 +1,305 @@+/* Altered for NumPlay's Mr Bullet (NumWorks calculator): no new contact past the memory limit */+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/b2ContactManager.h>++extern "C" { extern long b2_heap_now, b2_heap_limit; }+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2WorldCallbacks.h>+#include <Box2D/Dynamics/Contacts/b2Contact.h>++b2ContactFilter b2_defaultFilter;+b2ContactListener b2_defaultListener;++b2ContactManager::b2ContactManager()+{+ m_contactList = NULL;+ m_contactCount = 0;+ m_contactFilter = &b2_defaultFilter;+ m_contactListener = &b2_defaultListener;+ m_allocator = NULL;+}++void b2ContactManager::Destroy(b2Contact* c)+{+ b2Fixture* fixtureA = c->GetFixtureA();+ b2Fixture* fixtureB = c->GetFixtureB();+ b2Body* bodyA = fixtureA->GetBody();+ b2Body* bodyB = fixtureB->GetBody();++ if (m_contactListener && c->IsTouching())+ {+ m_contactListener->EndContact(c);+ }++ // Remove from the world.+ if (c->m_prev)+ {+ c->m_prev->m_next = c->m_next;+ }++ if (c->m_next)+ {+ c->m_next->m_prev = c->m_prev;+ }++ if (c == m_contactList)+ {+ m_contactList = c->m_next;+ }++ // Remove from body 1+ if (c->m_nodeA.prev)+ {+ c->m_nodeA.prev->next = c->m_nodeA.next;+ }++ if (c->m_nodeA.next)+ {+ c->m_nodeA.next->prev = c->m_nodeA.prev;+ }++ if (&c->m_nodeA == bodyA->m_contactList)+ {+ bodyA->m_contactList = c->m_nodeA.next;+ }++ // Remove from body 2+ if (c->m_nodeB.prev)+ {+ c->m_nodeB.prev->next = c->m_nodeB.next;+ }++ if (c->m_nodeB.next)+ {+ c->m_nodeB.next->prev = c->m_nodeB.prev;+ }++ if (&c->m_nodeB == bodyB->m_contactList)+ {+ bodyB->m_contactList = c->m_nodeB.next;+ }++ // Call the factory.+ b2Contact::Destroy(c, m_allocator);+ --m_contactCount;+}++// This is the top level collision call for the time step. Here+// all the narrow phase collision is processed for the world+// contact list.+void b2ContactManager::Collide()+{+ // Update awake contacts.+ b2Contact* c = m_contactList;+ while (c)+ {+ b2Fixture* fixtureA = c->GetFixtureA();+ b2Fixture* fixtureB = c->GetFixtureB();+ int32 indexA = c->GetChildIndexA();+ int32 indexB = c->GetChildIndexB();+ b2Body* bodyA = fixtureA->GetBody();+ b2Body* bodyB = fixtureB->GetBody();++ // Is this contact flagged for filtering?+ if (c->m_flags & b2Contact::e_filterFlag)+ {+ // Should these bodies collide?+ if (bodyB->ShouldCollide(bodyA) == false)+ {+ b2Contact* cNuke = c;+ c = cNuke->GetNext();+ Destroy(cNuke);+ continue;+ }++ // Check user filtering.+ if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)+ {+ b2Contact* cNuke = c;+ c = cNuke->GetNext();+ Destroy(cNuke);+ continue;+ }++ // Clear the filtering flag.+ c->m_flags &= ~b2Contact::e_filterFlag;+ }++ bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody;+ bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody;++ // At least one body must be awake and it must be dynamic or kinematic.+ if (activeA == false && activeB == false)+ {+ c = c->GetNext();+ continue;+ }++ int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;+ int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;+ bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);++ // Here we destroy contacts that cease to overlap in the broad-phase.+ if (overlap == false)+ {+ b2Contact* cNuke = c;+ c = cNuke->GetNext();+ Destroy(cNuke);+ continue;+ }++ // The contact persists.+ c->Update(m_contactListener);+ c = c->GetNext();+ }+}++void b2ContactManager::FindNewContacts()+{+ m_broadPhase.UpdatePairs(this);+}++void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)+{+ b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA;+ b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB;++ b2Fixture* fixtureA = proxyA->fixture;+ b2Fixture* fixtureB = proxyB->fixture;++ int32 indexA = proxyA->childIndex;+ int32 indexB = proxyB->childIndex;++ b2Body* bodyA = fixtureA->GetBody();+ b2Body* bodyB = fixtureB->GetBody();++ // Are the fixtures on the same body?+ if (bodyA == bodyB)+ {+ return;+ }++ // TODO_ERIN use a hash table to remove a potential bottleneck when both+ // bodies have a lot of contacts.+ // Does a contact already exist?+ b2ContactEdge* edge = bodyB->GetContactList();+ while (edge)+ {+ if (edge->other == bodyA)+ {+ b2Fixture* fA = edge->contact->GetFixtureA();+ b2Fixture* fB = edge->contact->GetFixtureB();+ int32 iA = edge->contact->GetChildIndexA();+ int32 iB = edge->contact->GetChildIndexB();++ if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)+ {+ // A contact already exists.+ return;+ }++ if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)+ {+ // A contact already exists.+ return;+ }+ }++ edge = edge->next;+ }++ // Does a joint override collision? Is at least one body dynamic?+ if (bodyB->ShouldCollide(bodyA) == false)+ {+ return;+ }++ // Check user filtering.+ if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)+ {+ return;+ }++ // (NumPlay: no new contact when Box2D's memory is near its limit)+ if (b2_heap_limit && b2_heap_now > b2_heap_limit)+ {+ return;+ }++ // Call the factory.+ b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);+ if (c == NULL)+ {+ return;+ }++ // Contact creation may swap fixtures.+ fixtureA = c->GetFixtureA();+ fixtureB = c->GetFixtureB();+ indexA = c->GetChildIndexA();+ indexB = c->GetChildIndexB();+ bodyA = fixtureA->GetBody();+ bodyB = fixtureB->GetBody();++ // Insert into the world.+ c->m_prev = NULL;+ c->m_next = m_contactList;+ if (m_contactList != NULL)+ {+ m_contactList->m_prev = c;+ }+ m_contactList = c;++ // Connect to island graph.++ // Connect to body A+ c->m_nodeA.contact = c;+ c->m_nodeA.other = bodyB;++ c->m_nodeA.prev = NULL;+ c->m_nodeA.next = bodyA->m_contactList;+ if (bodyA->m_contactList != NULL)+ {+ bodyA->m_contactList->prev = &c->m_nodeA;+ }+ bodyA->m_contactList = &c->m_nodeA;++ // Connect to body B+ c->m_nodeB.contact = c;+ c->m_nodeB.other = bodyA;++ c->m_nodeB.prev = NULL;+ c->m_nodeB.next = bodyB->m_contactList;+ if (bodyB->m_contactList != NULL)+ {+ bodyB->m_contactList->prev = &c->m_nodeB;+ }+ bodyB->m_contactList = &c->m_nodeB;++ // Wake up the bodies+ if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false)+ {+ bodyA->SetAwake(true);+ bodyB->SetAwake(true);+ }++ ++m_contactCount;+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.hnew file mode 100644index 00000000..dc1f77fe--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.h@@ -0,0 +1,52 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_CONTACT_MANAGER_H+#define B2_CONTACT_MANAGER_H++#include <Box2D/Collision/b2BroadPhase.h>++class b2Contact;+class b2ContactFilter;+class b2ContactListener;+class b2BlockAllocator;++// Delegate of b2World.+class b2ContactManager+{+public:+ b2ContactManager();++ // Broad-phase callback.+ void AddPair(void* proxyUserDataA, void* proxyUserDataB);++ void FindNewContacts();++ void Destroy(b2Contact* c);++ void Collide();++ b2BroadPhase m_broadPhase;+ b2Contact* m_contactList;+ int32 m_contactCount;+ b2ContactFilter* m_contactFilter;+ b2ContactListener* m_contactListener;+ b2BlockAllocator* m_allocator;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.cppnew file mode 100644index 00000000..d97aa448--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.cpp@@ -0,0 +1,303 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>+#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/b2BroadPhase.h>+#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Common/b2BlockAllocator.h>++b2Fixture::b2Fixture()+{+ m_userData = NULL;+ m_body = NULL;+ m_next = NULL;+ m_proxies = NULL;+ m_proxyCount = 0;+ m_shape = NULL;+ m_density = 0.0f;+}++void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def)+{+ m_userData = def->userData;+ m_friction = def->friction;+ m_restitution = def->restitution;++ m_body = body;+ m_next = NULL;++ m_filter = def->filter;++ m_isSensor = def->isSensor;++ m_shape = def->shape->Clone(allocator);++ // Reserve proxy space+ int32 childCount = m_shape->GetChildCount();+ m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy));+ for (int32 i = 0; i < childCount; ++i)+ {+ m_proxies[i].fixture = NULL;+ m_proxies[i].proxyId = b2BroadPhase::e_nullProxy;+ }+ m_proxyCount = 0;++ m_density = def->density;+}++void b2Fixture::Destroy(b2BlockAllocator* allocator)+{+ // The proxies must be destroyed before calling this.+ b2Assert(m_proxyCount == 0);++ // Free the proxy array.+ int32 childCount = m_shape->GetChildCount();+ allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy));+ m_proxies = NULL;++ // Free the child shape.+ switch (m_shape->m_type)+ {+ case b2Shape::e_circle:+ {+ b2CircleShape* s = (b2CircleShape*)m_shape;+ s->~b2CircleShape();+ allocator->Free(s, sizeof(b2CircleShape));+ }+ break;++ case b2Shape::e_edge:+ {+ b2EdgeShape* s = (b2EdgeShape*)m_shape;+ s->~b2EdgeShape();+ allocator->Free(s, sizeof(b2EdgeShape));+ }+ break;++ case b2Shape::e_polygon:+ {+ b2PolygonShape* s = (b2PolygonShape*)m_shape;+ s->~b2PolygonShape();+ allocator->Free(s, sizeof(b2PolygonShape));+ }+ break;++ case b2Shape::e_chain:+ {+ b2ChainShape* s = (b2ChainShape*)m_shape;+ s->~b2ChainShape();+ allocator->Free(s, sizeof(b2ChainShape));+ }+ break;++ default:+ b2Assert(false);+ break;+ }++ m_shape = NULL;+}++void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf)+{+ b2Assert(m_proxyCount == 0);++ // Create proxies in the broad-phase.+ m_proxyCount = m_shape->GetChildCount();++ for (int32 i = 0; i < m_proxyCount; ++i)+ {+ b2FixtureProxy* proxy = m_proxies + i;+ m_shape->ComputeAABB(&proxy->aabb, xf, i);+ proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy);+ proxy->fixture = this;+ proxy->childIndex = i;+ }+}++void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase)+{+ // Destroy proxies in the broad-phase.+ for (int32 i = 0; i < m_proxyCount; ++i)+ {+ b2FixtureProxy* proxy = m_proxies + i;+ broadPhase->DestroyProxy(proxy->proxyId);+ proxy->proxyId = b2BroadPhase::e_nullProxy;+ }++ m_proxyCount = 0;+}++void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2)+{+ if (m_proxyCount == 0)+ {+ return;+ }++ for (int32 i = 0; i < m_proxyCount; ++i)+ {+ b2FixtureProxy* proxy = m_proxies + i;++ // Compute an AABB that covers the swept shape (may miss some rotation effect).+ b2AABB aabb1, aabb2;+ m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex);+ m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex);++ proxy->aabb.Combine(aabb1, aabb2);++ b2Vec2 displacement = transform2.p - transform1.p;++ broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);+ }+}++void b2Fixture::SetFilterData(const b2Filter& filter)+{+ m_filter = filter;++ Refilter();+}++void b2Fixture::Refilter()+{+ if (m_body == NULL)+ {+ return;+ }++ // Flag associated contacts for filtering.+ b2ContactEdge* edge = m_body->GetContactList();+ while (edge)+ {+ b2Contact* contact = edge->contact;+ b2Fixture* fixtureA = contact->GetFixtureA();+ b2Fixture* fixtureB = contact->GetFixtureB();+ if (fixtureA == this || fixtureB == this)+ {+ contact->FlagForFiltering();+ }++ edge = edge->next;+ }++ b2World* world = m_body->GetWorld();++ if (world == NULL)+ {+ return;+ }++ // Touch each proxy so that new pairs may be created+ b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase;+ for (int32 i = 0; i < m_proxyCount; ++i)+ {+ broadPhase->TouchProxy(m_proxies[i].proxyId);+ }+}++void b2Fixture::SetSensor(bool sensor)+{+ if (sensor != m_isSensor)+ {+ m_body->SetAwake(true);+ m_isSensor = sensor;+ }+}++void b2Fixture::Dump(int32 bodyIndex)+{+ b2Log(" b2FixtureDef fd;\n");+ b2Log(" fd.friction = %.15lef;\n", m_friction);+ b2Log(" fd.restitution = %.15lef;\n", m_restitution);+ b2Log(" fd.density = %.15lef;\n", m_density);+ b2Log(" fd.isSensor = bool(%d);\n", m_isSensor);+ b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits);+ b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits);+ b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex);++ switch (m_shape->m_type)+ {+ case b2Shape::e_circle:+ {+ b2CircleShape* s = (b2CircleShape*)m_shape;+ b2Log(" b2CircleShape shape;\n");+ b2Log(" shape.m_radius = %.15lef;\n", s->m_radius);+ b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y);+ }+ break;++ case b2Shape::e_edge:+ {+ b2EdgeShape* s = (b2EdgeShape*)m_shape;+ b2Log(" b2EdgeShape shape;\n");+ b2Log(" shape.m_radius = %.15lef;\n", s->m_radius);+ b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y);+ b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y);+ b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y);+ b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y);+ b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0);+ b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3);+ }+ break;++ case b2Shape::e_polygon:+ {+ b2PolygonShape* s = (b2PolygonShape*)m_shape;+ b2Log(" b2PolygonShape shape;\n");+ b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices);+ for (int32 i = 0; i < s->m_count; ++i)+ {+ b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);+ }+ b2Log(" shape.Set(vs, %d);\n", s->m_count);+ }+ break;++ case b2Shape::e_chain:+ {+ b2ChainShape* s = (b2ChainShape*)m_shape;+ b2Log(" b2ChainShape shape;\n");+ b2Log(" b2Vec2 vs[%d];\n", s->m_count);+ for (int32 i = 0; i < s->m_count; ++i)+ {+ b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);+ }+ b2Log(" shape.CreateChain(vs, %d);\n", s->m_count);+ b2Log(" shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y);+ b2Log(" shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y);+ b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex);+ b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex);+ }+ break;++ default:+ return;+ }++ b2Log("\n");+ b2Log(" fd.shape = &shape;\n");+ b2Log("\n");+ b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex);+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.hnew file mode 100644index 00000000..5ebd0c3f--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.h@@ -0,0 +1,345 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_FIXTURE_H+#define B2_FIXTURE_H++#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/Shapes/b2Shape.h>++class b2BlockAllocator;+class b2Body;+class b2BroadPhase;+class b2Fixture;++/// This holds contact filtering data.+struct b2Filter+{+ b2Filter()+ {+ categoryBits = 0x0001;+ maskBits = 0xFFFF;+ groupIndex = 0;+ }++ /// The collision category bits. Normally you would just set one bit.+ uint16 categoryBits;++ /// The collision mask bits. This states the categories that this+ /// shape would accept for collision.+ uint16 maskBits;++ /// Collision groups allow a certain group of objects to never collide (negative)+ /// or always collide (positive). Zero means no collision group. Non-zero group+ /// filtering always wins against the mask bits.+ int16 groupIndex;+};++/// A fixture definition is used to create a fixture. This class defines an+/// abstract fixture definition. You can reuse fixture definitions safely.+struct b2FixtureDef+{+ /// The constructor sets the default fixture definition values.+ b2FixtureDef()+ {+ shape = NULL;+ userData = NULL;+ friction = 0.2f;+ restitution = 0.0f;+ density = 0.0f;+ isSensor = false;+ }++ /// The shape, this must be set. The shape will be cloned, so you+ /// can create the shape on the stack.+ const b2Shape* shape;++ /// Use this to store application specific fixture data.+ void* userData;++ /// The friction coefficient, usually in the range [0,1].+ float32 friction;++ /// The restitution (elasticity) usually in the range [0,1].+ float32 restitution;++ /// The density, usually in kg/m^2.+ float32 density;++ /// A sensor shape collects contact information but never generates a collision+ /// response.+ bool isSensor;++ /// Contact filtering data.+ b2Filter filter;+};++/// This proxy is used internally to connect fixtures to the broad-phase.+struct b2FixtureProxy+{+ b2AABB aabb;+ b2Fixture* fixture;+ int32 childIndex;+ int32 proxyId;+};++/// A fixture is used to attach a shape to a body for collision detection. A fixture+/// inherits its transform from its parent. Fixtures hold additional non-geometric data+/// such as friction, collision filters, etc.+/// Fixtures are created via b2Body::CreateFixture.+/// @warning you cannot reuse fixtures.+class b2Fixture+{+public:+ /// Get the type of the child shape. You can use this to down cast to the concrete shape.+ /// @return the shape type.+ b2Shape::Type GetType() const;++ /// Get the child shape. You can modify the child shape, however you should not change the+ /// number of vertices because this will crash some collision caching mechanisms.+ /// Manipulating the shape may lead to non-physical behavior.+ b2Shape* GetShape();+ const b2Shape* GetShape() const;++ /// Set if this fixture is a sensor.+ void SetSensor(bool sensor);++ /// Is this fixture a sensor (non-solid)?+ /// @return the true if the shape is a sensor.+ bool IsSensor() const;++ /// Set the contact filtering data. This will not update contacts until the next time+ /// step when either parent body is active and awake.+ /// This automatically calls Refilter.+ void SetFilterData(const b2Filter& filter);++ /// Get the contact filtering data.+ const b2Filter& GetFilterData() const;++ /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.+ void Refilter();++ /// Get the parent body of this fixture. This is NULL if the fixture is not attached.+ /// @return the parent body.+ b2Body* GetBody();+ const b2Body* GetBody() const;++ /// Get the next fixture in the parent body's fixture list.+ /// @return the next shape.+ b2Fixture* GetNext();+ const b2Fixture* GetNext() const;++ /// Get the user data that was assigned in the fixture definition. Use this to+ /// store your application specific data.+ void* GetUserData() const;++ /// Set the user data. Use this to store your application specific data.+ void SetUserData(void* data);++ /// Test a point for containment in this fixture.+ /// @param p a point in world coordinates.+ bool TestPoint(const b2Vec2& p) const;++ /// Cast a ray against this shape.+ /// @param output the ray-cast results.+ /// @param input the ray-cast input parameters.+ bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const;++ /// Get the mass data for this fixture. The mass data is based on the density and+ /// the shape. The rotational inertia is about the shape's origin. This operation+ /// may be expensive.+ void GetMassData(b2MassData* massData) const;++ /// Set the density of this fixture. This will _not_ automatically adjust the mass+ /// of the body. You must call b2Body::ResetMassData to update the body's mass.+ void SetDensity(float32 density);++ /// Get the density of this fixture.+ float32 GetDensity() const;++ /// Get the coefficient of friction.+ float32 GetFriction() const;++ /// Set the coefficient of friction. This will _not_ change the friction of+ /// existing contacts.+ void SetFriction(float32 friction);++ /// Get the coefficient of restitution.+ float32 GetRestitution() const;++ /// Set the coefficient of restitution. This will _not_ change the restitution of+ /// existing contacts.+ void SetRestitution(float32 restitution);++ /// Get the fixture's AABB. This AABB may be enlarge and/or stale.+ /// If you need a more accurate AABB, compute it using the shape and+ /// the body transform.+ const b2AABB& GetAABB(int32 childIndex) const;++ /// Dump this fixture to the log file.+ void Dump(int32 bodyIndex);++protected:++ friend class b2Body;+ friend class b2World;+ friend class b2Contact;+ friend class b2ContactManager;++ b2Fixture();++ // We need separation create/destroy functions from the constructor/destructor because+ // the destructor cannot access the allocator (no destructor arguments allowed by C++).+ void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def);+ void Destroy(b2BlockAllocator* allocator);++ // These support body activation/deactivation.+ void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf);+ void DestroyProxies(b2BroadPhase* broadPhase);++ void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);++ float32 m_density;++ b2Fixture* m_next;+ b2Body* m_body;++ b2Shape* m_shape;++ float32 m_friction;+ float32 m_restitution;++ b2FixtureProxy* m_proxies;+ int32 m_proxyCount;++ b2Filter m_filter;++ bool m_isSensor;++ void* m_userData;+};++inline b2Shape::Type b2Fixture::GetType() const+{+ return m_shape->GetType();+}++inline b2Shape* b2Fixture::GetShape()+{+ return m_shape;+}++inline const b2Shape* b2Fixture::GetShape() const+{+ return m_shape;+}++inline bool b2Fixture::IsSensor() const+{+ return m_isSensor;+}++inline const b2Filter& b2Fixture::GetFilterData() const+{+ return m_filter;+}++inline void* b2Fixture::GetUserData() const+{+ return m_userData;+}++inline void b2Fixture::SetUserData(void* data)+{+ m_userData = data;+}++inline b2Body* b2Fixture::GetBody()+{+ return m_body;+}++inline const b2Body* b2Fixture::GetBody() const+{+ return m_body;+}++inline b2Fixture* b2Fixture::GetNext()+{+ return m_next;+}++inline const b2Fixture* b2Fixture::GetNext() const+{+ return m_next;+}++inline void b2Fixture::SetDensity(float32 density)+{+ b2Assert(b2IsValid(density) && density >= 0.0f);+ m_density = density;+}++inline float32 b2Fixture::GetDensity() const+{+ return m_density;+}++inline float32 b2Fixture::GetFriction() const+{+ return m_friction;+}++inline void b2Fixture::SetFriction(float32 friction)+{+ m_friction = friction;+}++inline float32 b2Fixture::GetRestitution() const+{+ return m_restitution;+}++inline void b2Fixture::SetRestitution(float32 restitution)+{+ m_restitution = restitution;+}++inline bool b2Fixture::TestPoint(const b2Vec2& p) const+{+ return m_shape->TestPoint(m_body->GetTransform(), p);+}++inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const+{+ return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex);+}++inline void b2Fixture::GetMassData(b2MassData* massData) const+{+ m_shape->ComputeMass(massData, m_density);+}++inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const+{+ b2Assert(0 <= childIndex && childIndex < m_proxyCount);+ return m_proxies[childIndex].aabb;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.cppnew file mode 100644index 00000000..b0cb19aa--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.cpp@@ -0,0 +1,539 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Collision/b2Distance.h>+#include <Box2D/Dynamics/b2Island.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>+#include <Box2D/Dynamics/Joints/b2Joint.h>+#include <Box2D/Common/b2StackAllocator.h>+#include <Box2D/Common/b2Timer.h>++/*+Position Correction Notes+=========================+I tried the several algorithms for position correction of the 2D revolute joint.+I looked at these systems:+- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.+- suspension bridge with 30 1m long planks of length 1m.+- multi-link chain with 30 1m long links.++Here are the algorithms:++Baumgarte - A fraction of the position error is added to the velocity error. There is no+separate position solver.++Pseudo Velocities - After the velocity solver and position integration,+the position error, Jacobian, and effective mass are recomputed. Then+the velocity constraints are solved with pseudo velocities and a fraction+of the position error is added to the pseudo velocity error. The pseudo+velocities are initialized to zero and there is no warm-starting. After+the position solver, the pseudo velocities are added to the positions.+This is also called the First Order World method or the Position LCP method.++Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the+position error is re-computed for each constraint and the positions are updated+after the constraint is solved. The radius vectors (aka Jacobians) are+re-computed too (otherwise the algorithm has horrible instability). The pseudo+velocity states are not needed because they are effectively zero at the beginning+of each iteration. Since we have the current position error, we allow the+iterations to terminate early if the error becomes smaller than b2_linearSlop.++Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed+each time a constraint is solved.++Here are the results:+Baumgarte - this is the cheapest algorithm but it has some stability problems,+especially with the bridge. The chain links separate easily close to the root+and they jitter as they struggle to pull together. This is one of the most common+methods in the field. The big drawback is that the position correction artificially+affects the momentum, thus leading to instabilities and false bounce. I used a+bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller+factor makes joints and contacts more spongy.++Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is+stable. However, joints still separate with large angular velocities. Drag the+simple pendulum in a circle quickly and the joint will separate. The chain separates+easily and does not recover. I used a bias factor of 0.2. A larger value lead to+the bridge collapsing when a heavy cube drops on it.++Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo+Velocities, but in other ways it is worse. The bridge and chain are much more+stable, but the simple pendulum goes unstable at high angular velocities.++Full NGS - stable in all tests. The joints display good stiffness. The bridge+still sags, but this is better than infinite forces.++Recommendations+Pseudo Velocities are not really worthwhile because the bridge and chain cannot+recover from joint separation. In other cases the benefit over Baumgarte is small.++Modified NGS is not a robust method for the revolute joint due to the violent+instability seen in the simple pendulum. Perhaps it is viable with other constraint+types, especially scalar constraints where the effective mass is a scalar.++This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities+and is very fast. I don't think we can escape Baumgarte, especially in highly+demanding cases where high constraint fidelity is not needed.++Full NGS is robust and easy on the eyes. I recommend this as an option for+higher fidelity simulation and certainly for suspension bridges and long chains.+Full NGS might be a good choice for ragdolls, especially motorized ragdolls where+joint separation can be problematic. The number of NGS iterations can be reduced+for better performance without harming robustness much.++Each joint in a can be handled differently in the position solver. So I recommend+a system where the user can select the algorithm on a per joint basis. I would+probably default to the slower Full NGS and let the user select the faster+Baumgarte method in performance critical scenarios.+*/++/*+Cache Performance++The Box2D solvers are dominated by cache misses. Data structures are designed+to increase the number of cache hits. Much of misses are due to random access+to body data. The constraint structures are iterated over linearly, which leads+to few cache misses.++The bodies are not accessed during iteration. Instead read only data, such as+the mass values are stored with the constraints. The mutable data are the constraint+impulses and the bodies velocities/positions. The impulses are held inside the+constraint structures. The body velocities/positions are held in compact, temporary+arrays to increase the number of cache hits. Linear and angular velocity are+stored in a single array since multiple arrays lead to multiple misses.+*/++/*+2D Rotation++R = [cos(theta) -sin(theta)]+ [sin(theta) cos(theta) ]++thetaDot = omega++Let q1 = cos(theta), q2 = sin(theta).+R = [q1 -q2]+ [q2 q1]++q1Dot = -thetaDot * q2+q2Dot = thetaDot * q1++q1_new = q1_old - dt * w * q2+q2_new = q2_old + dt * w * q1+then normalize.++This might be faster than computing sin+cos.+However, we can compute sin+cos of the same angle fast.+*/++b2Island::b2Island(+ int32 bodyCapacity,+ int32 contactCapacity,+ int32 jointCapacity,+ b2StackAllocator* allocator,+ b2ContactListener* listener)+{+ m_bodyCapacity = bodyCapacity;+ m_contactCapacity = contactCapacity;+ m_jointCapacity = jointCapacity;+ m_bodyCount = 0;+ m_contactCount = 0;+ m_jointCount = 0;++ m_allocator = allocator;+ m_listener = listener;++ m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));+ m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));+ m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));++ m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));+ m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));+}++b2Island::~b2Island()+{+ // Warning: the order should reverse the constructor order.+ m_allocator->Free(m_positions);+ m_allocator->Free(m_velocities);+ m_allocator->Free(m_joints);+ m_allocator->Free(m_contacts);+ m_allocator->Free(m_bodies);+}++void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)+{+ b2Timer timer;++ float32 h = step.dt;++ // Integrate velocities and apply damping. Initialize the body state.+ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Body* b = m_bodies[i];++ b2Vec2 c = b->m_sweep.c;+ float32 a = b->m_sweep.a;+ b2Vec2 v = b->m_linearVelocity;+ float32 w = b->m_angularVelocity;++ // Store positions for continuous collision.+ b->m_sweep.c0 = b->m_sweep.c;+ b->m_sweep.a0 = b->m_sweep.a;++ if (b->m_type == b2_dynamicBody)+ {+ // Integrate velocities.+ v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);+ w += h * b->m_invI * b->m_torque;++ // Apply damping.+ // ODE: dv/dt + c * v = 0+ // Solution: v(t) = v0 * exp(-c * t)+ // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)+ // v2 = exp(-c * dt) * v1+ // Pade approximation:+ // v2 = v1 * 1 / (1 + c * dt)+ v *= 1.0f / (1.0f + h * b->m_linearDamping);+ w *= 1.0f / (1.0f + h * b->m_angularDamping);+ }++ m_positions[i].c = c;+ m_positions[i].a = a;+ m_velocities[i].v = v;+ m_velocities[i].w = w;+ }++ timer.Reset();++ // Solver data+ b2SolverData solverData;+ solverData.step = step;+ solverData.positions = m_positions;+ solverData.velocities = m_velocities;++ // Initialize velocity constraints.+ b2ContactSolverDef contactSolverDef;+ contactSolverDef.step = step;+ contactSolverDef.contacts = m_contacts;+ contactSolverDef.count = m_contactCount;+ contactSolverDef.positions = m_positions;+ contactSolverDef.velocities = m_velocities;+ contactSolverDef.allocator = m_allocator;++ b2ContactSolver contactSolver(&contactSolverDef);+ contactSolver.InitializeVelocityConstraints();++ if (step.warmStarting)+ {+ contactSolver.WarmStart();+ }++ for (int32 i = 0; i < m_jointCount; ++i)+ {+ m_joints[i]->InitVelocityConstraints(solverData);+ }++ profile->solveInit = timer.GetMilliseconds();++ // Solve velocity constraints+ timer.Reset();+ for (int32 i = 0; i < step.velocityIterations; ++i)+ {+ for (int32 j = 0; j < m_jointCount; ++j)+ {+ m_joints[j]->SolveVelocityConstraints(solverData);+ }++ contactSolver.SolveVelocityConstraints();+ }++ // Store impulses for warm starting+ contactSolver.StoreImpulses();+ profile->solveVelocity = timer.GetMilliseconds();++ // Integrate positions+ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Vec2 c = m_positions[i].c;+ float32 a = m_positions[i].a;+ b2Vec2 v = m_velocities[i].v;+ float32 w = m_velocities[i].w;++ // Check for large velocities+ b2Vec2 translation = h * v;+ if (b2Dot(translation, translation) > b2_maxTranslationSquared)+ {+ float32 ratio = b2_maxTranslation / translation.Length();+ v *= ratio;+ }++ float32 rotation = h * w;+ if (rotation * rotation > b2_maxRotationSquared)+ {+ float32 ratio = b2_maxRotation / b2Abs(rotation);+ w *= ratio;+ }++ // Integrate+ c += h * v;+ a += h * w;++ m_positions[i].c = c;+ m_positions[i].a = a;+ m_velocities[i].v = v;+ m_velocities[i].w = w;+ }++ // Solve position constraints+ timer.Reset();+ bool positionSolved = false;+ for (int32 i = 0; i < step.positionIterations; ++i)+ {+ bool contactsOkay = contactSolver.SolvePositionConstraints();++ bool jointsOkay = true;+ for (int32 i = 0; i < m_jointCount; ++i)+ {+ bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);+ jointsOkay = jointsOkay && jointOkay;+ }++ if (contactsOkay && jointsOkay)+ {+ // Exit early if the position errors are small.+ positionSolved = true;+ break;+ }+ }++ // Copy state buffers back to the bodies+ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Body* body = m_bodies[i];+ body->m_sweep.c = m_positions[i].c;+ body->m_sweep.a = m_positions[i].a;+ body->m_linearVelocity = m_velocities[i].v;+ body->m_angularVelocity = m_velocities[i].w;+ body->SynchronizeTransform();+ }++ profile->solvePosition = timer.GetMilliseconds();++ Report(contactSolver.m_velocityConstraints);++ if (allowSleep)+ {+ float32 minSleepTime = b2_maxFloat;++ const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;+ const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;++ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Body* b = m_bodies[i];+ if (b->GetType() == b2_staticBody)+ {+ continue;+ }++ if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||+ b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||+ b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)+ {+ b->m_sleepTime = 0.0f;+ minSleepTime = 0.0f;+ }+ else+ {+ b->m_sleepTime += h;+ minSleepTime = b2Min(minSleepTime, b->m_sleepTime);+ }+ }++ if (minSleepTime >= b2_timeToSleep && positionSolved)+ {+ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Body* b = m_bodies[i];+ b->SetAwake(false);+ }+ }+ }+}++void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)+{+ b2Assert(toiIndexA < m_bodyCount);+ b2Assert(toiIndexB < m_bodyCount);++ // Initialize the body state.+ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Body* b = m_bodies[i];+ m_positions[i].c = b->m_sweep.c;+ m_positions[i].a = b->m_sweep.a;+ m_velocities[i].v = b->m_linearVelocity;+ m_velocities[i].w = b->m_angularVelocity;+ }++ b2ContactSolverDef contactSolverDef;+ contactSolverDef.contacts = m_contacts;+ contactSolverDef.count = m_contactCount;+ contactSolverDef.allocator = m_allocator;+ contactSolverDef.step = subStep;+ contactSolverDef.positions = m_positions;+ contactSolverDef.velocities = m_velocities;+ b2ContactSolver contactSolver(&contactSolverDef);++ // Solve position constraints.+ for (int32 i = 0; i < subStep.positionIterations; ++i)+ {+ bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);+ if (contactsOkay)+ {+ break;+ }+ }++#if 0+ // Is the new position really safe?+ for (int32 i = 0; i < m_contactCount; ++i)+ {+ b2Contact* c = m_contacts[i];+ b2Fixture* fA = c->GetFixtureA();+ b2Fixture* fB = c->GetFixtureB();++ b2Body* bA = fA->GetBody();+ b2Body* bB = fB->GetBody();++ int32 indexA = c->GetChildIndexA();+ int32 indexB = c->GetChildIndexB();++ b2DistanceInput input;+ input.proxyA.Set(fA->GetShape(), indexA);+ input.proxyB.Set(fB->GetShape(), indexB);+ input.transformA = bA->GetTransform();+ input.transformB = bB->GetTransform();+ input.useRadii = false;++ b2DistanceOutput output;+ b2SimplexCache cache;+ cache.count = 0;+ b2Distance(&output, &cache, &input);++ if (output.distance == 0 || cache.count == 3)+ {+ cache.count += 0;+ }+ }+#endif++ // Leap of faith to new safe state.+ m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;+ m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;+ m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;+ m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;++ // No warm starting is needed for TOI events because warm+ // starting impulses were applied in the discrete solver.+ contactSolver.InitializeVelocityConstraints();++ // Solve velocity constraints.+ for (int32 i = 0; i < subStep.velocityIterations; ++i)+ {+ contactSolver.SolveVelocityConstraints();+ }++ // Don't store the TOI contact forces for warm starting+ // because they can be quite large.++ float32 h = subStep.dt;++ // Integrate positions+ for (int32 i = 0; i < m_bodyCount; ++i)+ {+ b2Vec2 c = m_positions[i].c;+ float32 a = m_positions[i].a;+ b2Vec2 v = m_velocities[i].v;+ float32 w = m_velocities[i].w;++ // Check for large velocities+ b2Vec2 translation = h * v;+ if (b2Dot(translation, translation) > b2_maxTranslationSquared)+ {+ float32 ratio = b2_maxTranslation / translation.Length();+ v *= ratio;+ }++ float32 rotation = h * w;+ if (rotation * rotation > b2_maxRotationSquared)+ {+ float32 ratio = b2_maxRotation / b2Abs(rotation);+ w *= ratio;+ }++ // Integrate+ c += h * v;+ a += h * w;++ m_positions[i].c = c;+ m_positions[i].a = a;+ m_velocities[i].v = v;+ m_velocities[i].w = w;++ // Sync bodies+ b2Body* body = m_bodies[i];+ body->m_sweep.c = c;+ body->m_sweep.a = a;+ body->m_linearVelocity = v;+ body->m_angularVelocity = w;+ body->SynchronizeTransform();+ }++ Report(contactSolver.m_velocityConstraints);+}++void b2Island::Report(const b2ContactVelocityConstraint* constraints)+{+ if (m_listener == NULL)+ {+ return;+ }++ for (int32 i = 0; i < m_contactCount; ++i)+ {+ b2Contact* c = m_contacts[i];++ const b2ContactVelocityConstraint* vc = constraints + i;++ b2ContactImpulse impulse;+ impulse.count = vc->pointCount;+ for (int32 j = 0; j < vc->pointCount; ++j)+ {+ impulse.normalImpulses[j] = vc->points[j].normalImpulse;+ impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;+ }++ m_listener->PostSolve(c, &impulse);+ }+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.hnew file mode 100644index 00000000..ef3d9b1d--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.h@@ -0,0 +1,93 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_ISLAND_H+#define B2_ISLAND_H++#include <Box2D/Common/b2Math.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2TimeStep.h>++class b2Contact;+class b2Joint;+class b2StackAllocator;+class b2ContactListener;+struct b2ContactVelocityConstraint;+struct b2Profile;++/// This is an internal class.+class b2Island+{+public:+ b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity,+ b2StackAllocator* allocator, b2ContactListener* listener);+ ~b2Island();++ void Clear()+ {+ m_bodyCount = 0;+ m_contactCount = 0;+ m_jointCount = 0;+ }++ void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);++ void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB);++ void Add(b2Body* body)+ {+ b2Assert(m_bodyCount < m_bodyCapacity);+ body->m_islandIndex = m_bodyCount;+ m_bodies[m_bodyCount] = body;+ ++m_bodyCount;+ }++ void Add(b2Contact* contact)+ {+ b2Assert(m_contactCount < m_contactCapacity);+ m_contacts[m_contactCount++] = contact;+ }++ void Add(b2Joint* joint)+ {+ b2Assert(m_jointCount < m_jointCapacity);+ m_joints[m_jointCount++] = joint;+ }++ void Report(const b2ContactVelocityConstraint* constraints);++ b2StackAllocator* m_allocator;+ b2ContactListener* m_listener;++ b2Body** m_bodies;+ b2Contact** m_contacts;+ b2Joint** m_joints;++ b2Position* m_positions;+ b2Velocity* m_velocities;++ int32 m_bodyCount;+ int32 m_jointCount;+ int32 m_contactCount;++ int32 m_bodyCapacity;+ int32 m_contactCapacity;+ int32 m_jointCapacity;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2TimeStep.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2TimeStep.hnew file mode 100644index 00000000..abe6fb6e--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2TimeStep.h@@ -0,0 +1,70 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_TIME_STEP_H+#define B2_TIME_STEP_H++#include <Box2D/Common/b2Math.h>++/// Profiling data. Times are in milliseconds.+struct b2Profile+{+ float32 step;+ float32 collide;+ float32 solve;+ float32 solveInit;+ float32 solveVelocity;+ float32 solvePosition;+ float32 broadphase;+ float32 solveTOI;+};++/// This is an internal structure.+struct b2TimeStep+{+ float32 dt; // time step+ float32 inv_dt; // inverse time step (0 if dt == 0).+ float32 dtRatio; // dt * inv_dt0+ int32 velocityIterations;+ int32 positionIterations;+ bool warmStarting;+};++/// This is an internal structure.+struct b2Position+{+ b2Vec2 c;+ float32 a;+};++/// This is an internal structure.+struct b2Velocity+{+ b2Vec2 v;+ float32 w;+};++/// Solver Data+struct b2SolverData+{+ b2TimeStep step;+ b2Position* positions;+ b2Velocity* velocities;+};++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.cppnew file mode 100644index 00000000..d39b85b7--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.cpp@@ -0,0 +1,1339 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2Island.h>+#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>+#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>+#include <Box2D/Collision/b2Collision.h>+#include <Box2D/Collision/b2BroadPhase.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2EdgeShape.h>+#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>+#include <Box2D/Collision/b2TimeOfImpact.h>+#include <Box2D/Common/b2Draw.h>+#include <Box2D/Common/b2Timer.h>+#include <new>++b2World::b2World(const b2Vec2& gravity)+{+ m_destructionListener = NULL;+ g_debugDraw = NULL;++ m_bodyList = NULL;+ m_jointList = NULL;++ m_bodyCount = 0;+ m_jointCount = 0;++ m_warmStarting = true;+ m_continuousPhysics = true;+ m_subStepping = false;++ m_stepComplete = true;++ m_allowSleep = true;+ m_gravity = gravity;++ m_flags = e_clearForces;++ m_inv_dt0 = 0.0f;++ m_contactManager.m_allocator = &m_blockAllocator;++ memset(&m_profile, 0, sizeof(b2Profile));+}++b2World::~b2World()+{+ // Some shapes allocate using b2Alloc.+ b2Body* b = m_bodyList;+ while (b)+ {+ b2Body* bNext = b->m_next;++ b2Fixture* f = b->m_fixtureList;+ while (f)+ {+ b2Fixture* fNext = f->m_next;+ f->m_proxyCount = 0;+ f->Destroy(&m_blockAllocator);+ f = fNext;+ }++ b = bNext;+ }+}++void b2World::SetDestructionListener(b2DestructionListener* listener)+{+ m_destructionListener = listener;+}++void b2World::SetContactFilter(b2ContactFilter* filter)+{+ m_contactManager.m_contactFilter = filter;+}++void b2World::SetContactListener(b2ContactListener* listener)+{+ m_contactManager.m_contactListener = listener;+}++void b2World::SetDebugDraw(b2Draw* debugDraw)+{+ g_debugDraw = debugDraw;+}++b2Body* b2World::CreateBody(const b2BodyDef* def)+{+ b2Assert(IsLocked() == false);+ if (IsLocked())+ {+ return NULL;+ }++ void* mem = m_blockAllocator.Allocate(sizeof(b2Body));+ b2Body* b = new (mem) b2Body(def, this);++ // Add to world doubly linked list.+ b->m_prev = NULL;+ b->m_next = m_bodyList;+ if (m_bodyList)+ {+ m_bodyList->m_prev = b;+ }+ m_bodyList = b;+ ++m_bodyCount;++ return b;+}++void b2World::DestroyBody(b2Body* b)+{+ b2Assert(m_bodyCount > 0);+ b2Assert(IsLocked() == false);+ if (IsLocked())+ {+ return;+ }++ // Delete the attached joints.+ b2JointEdge* je = b->m_jointList;+ while (je)+ {+ b2JointEdge* je0 = je;+ je = je->next;++ if (m_destructionListener)+ {+ m_destructionListener->SayGoodbye(je0->joint);+ }++ DestroyJoint(je0->joint);++ b->m_jointList = je;+ }+ b->m_jointList = NULL;++ // Delete the attached contacts.+ b2ContactEdge* ce = b->m_contactList;+ while (ce)+ {+ b2ContactEdge* ce0 = ce;+ ce = ce->next;+ m_contactManager.Destroy(ce0->contact);+ }+ b->m_contactList = NULL;++ // Delete the attached fixtures. This destroys broad-phase proxies.+ b2Fixture* f = b->m_fixtureList;+ while (f)+ {+ b2Fixture* f0 = f;+ f = f->m_next;++ if (m_destructionListener)+ {+ m_destructionListener->SayGoodbye(f0);+ }++ f0->DestroyProxies(&m_contactManager.m_broadPhase);+ f0->Destroy(&m_blockAllocator);+ f0->~b2Fixture();+ m_blockAllocator.Free(f0, sizeof(b2Fixture));++ b->m_fixtureList = f;+ b->m_fixtureCount -= 1;+ }+ b->m_fixtureList = NULL;+ b->m_fixtureCount = 0;++ // Remove world body list.+ if (b->m_prev)+ {+ b->m_prev->m_next = b->m_next;+ }++ if (b->m_next)+ {+ b->m_next->m_prev = b->m_prev;+ }++ if (b == m_bodyList)+ {+ m_bodyList = b->m_next;+ }++ --m_bodyCount;+ b->~b2Body();+ m_blockAllocator.Free(b, sizeof(b2Body));+}++b2Joint* b2World::CreateJoint(const b2JointDef* def)+{+ b2Assert(IsLocked() == false);+ if (IsLocked())+ {+ return NULL;+ }++ b2Joint* j = b2Joint::Create(def, &m_blockAllocator);++ // Connect to the world list.+ j->m_prev = NULL;+ j->m_next = m_jointList;+ if (m_jointList)+ {+ m_jointList->m_prev = j;+ }+ m_jointList = j;+ ++m_jointCount;++ // Connect to the bodies' doubly linked lists.+ j->m_edgeA.joint = j;+ j->m_edgeA.other = j->m_bodyB;+ j->m_edgeA.prev = NULL;+ j->m_edgeA.next = j->m_bodyA->m_jointList;+ if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;+ j->m_bodyA->m_jointList = &j->m_edgeA;++ j->m_edgeB.joint = j;+ j->m_edgeB.other = j->m_bodyA;+ j->m_edgeB.prev = NULL;+ j->m_edgeB.next = j->m_bodyB->m_jointList;+ if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;+ j->m_bodyB->m_jointList = &j->m_edgeB;++ b2Body* bodyA = def->bodyA;+ b2Body* bodyB = def->bodyB;++ // If the joint prevents collisions, then flag any contacts for filtering.+ if (def->collideConnected == false)+ {+ b2ContactEdge* edge = bodyB->GetContactList();+ while (edge)+ {+ if (edge->other == bodyA)+ {+ // Flag the contact for filtering at the next time step (where either+ // body is awake).+ edge->contact->FlagForFiltering();+ }++ edge = edge->next;+ }+ }++ // Note: creating a joint doesn't wake the bodies.++ return j;+}++void b2World::DestroyJoint(b2Joint* j)+{+ b2Assert(IsLocked() == false);+ if (IsLocked())+ {+ return;+ }++ bool collideConnected = j->m_collideConnected;++ // Remove from the doubly linked list.+ if (j->m_prev)+ {+ j->m_prev->m_next = j->m_next;+ }++ if (j->m_next)+ {+ j->m_next->m_prev = j->m_prev;+ }++ if (j == m_jointList)+ {+ m_jointList = j->m_next;+ }++ // Disconnect from island graph.+ b2Body* bodyA = j->m_bodyA;+ b2Body* bodyB = j->m_bodyB;++ // Wake up connected bodies.+ bodyA->SetAwake(true);+ bodyB->SetAwake(true);++ // Remove from body 1.+ if (j->m_edgeA.prev)+ {+ j->m_edgeA.prev->next = j->m_edgeA.next;+ }++ if (j->m_edgeA.next)+ {+ j->m_edgeA.next->prev = j->m_edgeA.prev;+ }++ if (&j->m_edgeA == bodyA->m_jointList)+ {+ bodyA->m_jointList = j->m_edgeA.next;+ }++ j->m_edgeA.prev = NULL;+ j->m_edgeA.next = NULL;++ // Remove from body 2+ if (j->m_edgeB.prev)+ {+ j->m_edgeB.prev->next = j->m_edgeB.next;+ }++ if (j->m_edgeB.next)+ {+ j->m_edgeB.next->prev = j->m_edgeB.prev;+ }++ if (&j->m_edgeB == bodyB->m_jointList)+ {+ bodyB->m_jointList = j->m_edgeB.next;+ }++ j->m_edgeB.prev = NULL;+ j->m_edgeB.next = NULL;++ b2Joint::Destroy(j, &m_blockAllocator);++ b2Assert(m_jointCount > 0);+ --m_jointCount;++ // If the joint prevents collisions, then flag any contacts for filtering.+ if (collideConnected == false)+ {+ b2ContactEdge* edge = bodyB->GetContactList();+ while (edge)+ {+ if (edge->other == bodyA)+ {+ // Flag the contact for filtering at the next time step (where either+ // body is awake).+ edge->contact->FlagForFiltering();+ }++ edge = edge->next;+ }+ }+}++//+void b2World::SetAllowSleeping(bool flag)+{+ if (flag == m_allowSleep)+ {+ return;+ }++ m_allowSleep = flag;+ if (m_allowSleep == false)+ {+ for (b2Body* b = m_bodyList; b; b = b->m_next)+ {+ b->SetAwake(true);+ }+ }+}++// Find islands, integrate and solve constraints, solve position constraints+void b2World::Solve(const b2TimeStep& step)+{+ m_profile.solveInit = 0.0f;+ m_profile.solveVelocity = 0.0f;+ m_profile.solvePosition = 0.0f;++ // Size the island for the worst case.+ b2Island island(m_bodyCount,+ m_contactManager.m_contactCount,+ m_jointCount,+ &m_stackAllocator,+ m_contactManager.m_contactListener);++ // Clear all the island flags.+ for (b2Body* b = m_bodyList; b; b = b->m_next)+ {+ b->m_flags &= ~b2Body::e_islandFlag;+ }+ for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)+ {+ c->m_flags &= ~b2Contact::e_islandFlag;+ }+ for (b2Joint* j = m_jointList; j; j = j->m_next)+ {+ j->m_islandFlag = false;+ }++ // Build and simulate all awake islands.+ int32 stackSize = m_bodyCount;+ b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));+ for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)+ {+ if (seed->m_flags & b2Body::e_islandFlag)+ {+ continue;+ }++ if (seed->IsAwake() == false || seed->IsActive() == false)+ {+ continue;+ }++ // The seed can be dynamic or kinematic.+ if (seed->GetType() == b2_staticBody)+ {+ continue;+ }++ // Reset island and stack.+ island.Clear();+ int32 stackCount = 0;+ stack[stackCount++] = seed;+ seed->m_flags |= b2Body::e_islandFlag;++ // Perform a depth first search (DFS) on the constraint graph.+ while (stackCount > 0)+ {+ // Grab the next body off the stack and add it to the island.+ b2Body* b = stack[--stackCount];+ b2Assert(b->IsActive() == true);+ island.Add(b);++ // Make sure the body is awake.+ b->SetAwake(true);++ // To keep islands as small as possible, we don't+ // propagate islands across static bodies.+ if (b->GetType() == b2_staticBody)+ {+ continue;+ }++ // Search all contacts connected to this body.+ for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)+ {+ b2Contact* contact = ce->contact;++ // Has this contact already been added to an island?+ if (contact->m_flags & b2Contact::e_islandFlag)+ {+ continue;+ }++ // Is this contact solid and touching?+ if (contact->IsEnabled() == false ||+ contact->IsTouching() == false)+ {+ continue;+ }++ // Skip sensors.+ bool sensorA = contact->m_fixtureA->m_isSensor;+ bool sensorB = contact->m_fixtureB->m_isSensor;+ if (sensorA || sensorB)+ {+ continue;+ }++ island.Add(contact);+ contact->m_flags |= b2Contact::e_islandFlag;++ b2Body* other = ce->other;++ // Was the other body already added to this island?+ if (other->m_flags & b2Body::e_islandFlag)+ {+ continue;+ }++ b2Assert(stackCount < stackSize);+ stack[stackCount++] = other;+ other->m_flags |= b2Body::e_islandFlag;+ }++ // Search all joints connect to this body.+ for (b2JointEdge* je = b->m_jointList; je; je = je->next)+ {+ if (je->joint->m_islandFlag == true)+ {+ continue;+ }++ b2Body* other = je->other;++ // Don't simulate joints connected to inactive bodies.+ if (other->IsActive() == false)+ {+ continue;+ }++ island.Add(je->joint);+ je->joint->m_islandFlag = true;++ if (other->m_flags & b2Body::e_islandFlag)+ {+ continue;+ }++ b2Assert(stackCount < stackSize);+ stack[stackCount++] = other;+ other->m_flags |= b2Body::e_islandFlag;+ }+ }++ b2Profile profile;+ island.Solve(&profile, step, m_gravity, m_allowSleep);+ m_profile.solveInit += profile.solveInit;+ m_profile.solveVelocity += profile.solveVelocity;+ m_profile.solvePosition += profile.solvePosition;++ // Post solve cleanup.+ for (int32 i = 0; i < island.m_bodyCount; ++i)+ {+ // Allow static bodies to participate in other islands.+ b2Body* b = island.m_bodies[i];+ if (b->GetType() == b2_staticBody)+ {+ b->m_flags &= ~b2Body::e_islandFlag;+ }+ }+ }++ m_stackAllocator.Free(stack);++ {+ b2Timer timer;+ // Synchronize fixtures, check for out of range bodies.+ for (b2Body* b = m_bodyList; b; b = b->GetNext())+ {+ // If a body was not in an island then it did not move.+ if ((b->m_flags & b2Body::e_islandFlag) == 0)+ {+ continue;+ }++ if (b->GetType() == b2_staticBody)+ {+ continue;+ }++ // Update fixtures (for broad-phase).+ b->SynchronizeFixtures();+ }++ // Look for new contacts.+ m_contactManager.FindNewContacts();+ m_profile.broadphase = timer.GetMilliseconds();+ }+}++// Find TOI contacts and solve them.+void b2World::SolveTOI(const b2TimeStep& step)+{+ b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);++ if (m_stepComplete)+ {+ for (b2Body* b = m_bodyList; b; b = b->m_next)+ {+ b->m_flags &= ~b2Body::e_islandFlag;+ b->m_sweep.alpha0 = 0.0f;+ }++ for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)+ {+ // Invalidate TOI+ c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);+ c->m_toiCount = 0;+ c->m_toi = 1.0f;+ }+ }++ // Find TOI events and solve them.+ for (;;)+ {+ // Find the first TOI.+ b2Contact* minContact = NULL;+ float32 minAlpha = 1.0f;++ for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)+ {+ // Is this contact disabled?+ if (c->IsEnabled() == false)+ {+ continue;+ }++ // Prevent excessive sub-stepping.+ if (c->m_toiCount > b2_maxSubSteps)+ {+ continue;+ }++ float32 alpha = 1.0f;+ if (c->m_flags & b2Contact::e_toiFlag)+ {+ // This contact has a valid cached TOI.+ alpha = c->m_toi;+ }+ else+ {+ b2Fixture* fA = c->GetFixtureA();+ b2Fixture* fB = c->GetFixtureB();++ // Is there a sensor?+ if (fA->IsSensor() || fB->IsSensor())+ {+ continue;+ }++ b2Body* bA = fA->GetBody();+ b2Body* bB = fB->GetBody();++ b2BodyType typeA = bA->m_type;+ b2BodyType typeB = bB->m_type;+ b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);++ bool activeA = bA->IsAwake() && typeA != b2_staticBody;+ bool activeB = bB->IsAwake() && typeB != b2_staticBody;++ // Is at least one body active (awake and dynamic or kinematic)?+ if (activeA == false && activeB == false)+ {+ continue;+ }++ bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;+ bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;++ // Are these two non-bullet dynamic bodies?+ if (collideA == false && collideB == false)+ {+ continue;+ }++ // Compute the TOI for this contact.+ // Put the sweeps onto the same time interval.+ float32 alpha0 = bA->m_sweep.alpha0;++ if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)+ {+ alpha0 = bB->m_sweep.alpha0;+ bA->m_sweep.Advance(alpha0);+ }+ else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)+ {+ alpha0 = bA->m_sweep.alpha0;+ bB->m_sweep.Advance(alpha0);+ }++ b2Assert(alpha0 < 1.0f);++ int32 indexA = c->GetChildIndexA();+ int32 indexB = c->GetChildIndexB();++ // Compute the time of impact in interval [0, minTOI]+ b2TOIInput input;+ input.proxyA.Set(fA->GetShape(), indexA);+ input.proxyB.Set(fB->GetShape(), indexB);+ input.sweepA = bA->m_sweep;+ input.sweepB = bB->m_sweep;+ input.tMax = 1.0f;++ b2TOIOutput output;+ b2TimeOfImpact(&output, &input);++ // Beta is the fraction of the remaining portion of the .+ float32 beta = output.t;+ if (output.state == b2TOIOutput::e_touching)+ {+ alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);+ }+ else+ {+ alpha = 1.0f;+ }++ c->m_toi = alpha;+ c->m_flags |= b2Contact::e_toiFlag;+ }++ if (alpha < minAlpha)+ {+ // This is the minimum TOI found so far.+ minContact = c;+ minAlpha = alpha;+ }+ }++ if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)+ {+ // No more TOI events. Done!+ m_stepComplete = true;+ break;+ }++ // Advance the bodies to the TOI.+ b2Fixture* fA = minContact->GetFixtureA();+ b2Fixture* fB = minContact->GetFixtureB();+ b2Body* bA = fA->GetBody();+ b2Body* bB = fB->GetBody();++ b2Sweep backup1 = bA->m_sweep;+ b2Sweep backup2 = bB->m_sweep;++ bA->Advance(minAlpha);+ bB->Advance(minAlpha);++ // The TOI contact likely has some new contact points.+ minContact->Update(m_contactManager.m_contactListener);+ minContact->m_flags &= ~b2Contact::e_toiFlag;+ ++minContact->m_toiCount;++ // Is the contact solid?+ if (minContact->IsEnabled() == false || minContact->IsTouching() == false)+ {+ // Restore the sweeps.+ minContact->SetEnabled(false);+ bA->m_sweep = backup1;+ bB->m_sweep = backup2;+ bA->SynchronizeTransform();+ bB->SynchronizeTransform();+ continue;+ }++ bA->SetAwake(true);+ bB->SetAwake(true);++ // Build the island+ island.Clear();+ island.Add(bA);+ island.Add(bB);+ island.Add(minContact);++ bA->m_flags |= b2Body::e_islandFlag;+ bB->m_flags |= b2Body::e_islandFlag;+ minContact->m_flags |= b2Contact::e_islandFlag;++ // Get contacts on bodyA and bodyB.+ b2Body* bodies[2] = {bA, bB};+ for (int32 i = 0; i < 2; ++i)+ {+ b2Body* body = bodies[i];+ if (body->m_type == b2_dynamicBody)+ {+ for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)+ {+ if (island.m_bodyCount == island.m_bodyCapacity)+ {+ break;+ }++ if (island.m_contactCount == island.m_contactCapacity)+ {+ break;+ }++ b2Contact* contact = ce->contact;++ // Has this contact already been added to the island?+ if (contact->m_flags & b2Contact::e_islandFlag)+ {+ continue;+ }++ // Only add static, kinematic, or bullet bodies.+ b2Body* other = ce->other;+ if (other->m_type == b2_dynamicBody &&+ body->IsBullet() == false && other->IsBullet() == false)+ {+ continue;+ }++ // Skip sensors.+ bool sensorA = contact->m_fixtureA->m_isSensor;+ bool sensorB = contact->m_fixtureB->m_isSensor;+ if (sensorA || sensorB)+ {+ continue;+ }++ // Tentatively advance the body to the TOI.+ b2Sweep backup = other->m_sweep;+ if ((other->m_flags & b2Body::e_islandFlag) == 0)+ {+ other->Advance(minAlpha);+ }++ // Update the contact points+ contact->Update(m_contactManager.m_contactListener);++ // Was the contact disabled by the user?+ if (contact->IsEnabled() == false)+ {+ other->m_sweep = backup;+ other->SynchronizeTransform();+ continue;+ }++ // Are there contact points?+ if (contact->IsTouching() == false)+ {+ other->m_sweep = backup;+ other->SynchronizeTransform();+ continue;+ }++ // Add the contact to the island+ contact->m_flags |= b2Contact::e_islandFlag;+ island.Add(contact);++ // Has the other body already been added to the island?+ if (other->m_flags & b2Body::e_islandFlag)+ {+ continue;+ }++ // Add the other body to the island.+ other->m_flags |= b2Body::e_islandFlag;++ if (other->m_type != b2_staticBody)+ {+ other->SetAwake(true);+ }++ island.Add(other);+ }+ }+ }++ b2TimeStep subStep;+ subStep.dt = (1.0f - minAlpha) * step.dt;+ subStep.inv_dt = 1.0f / subStep.dt;+ subStep.dtRatio = 1.0f;+ subStep.positionIterations = 20;+ subStep.velocityIterations = step.velocityIterations;+ subStep.warmStarting = false;+ island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);++ // Reset island flags and synchronize broad-phase proxies.+ for (int32 i = 0; i < island.m_bodyCount; ++i)+ {+ b2Body* body = island.m_bodies[i];+ body->m_flags &= ~b2Body::e_islandFlag;++ if (body->m_type != b2_dynamicBody)+ {+ continue;+ }++ body->SynchronizeFixtures();++ // Invalidate all contact TOIs on this displaced body.+ for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)+ {+ ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);+ }+ }++ // Commit fixture proxy movements to the broad-phase so that new contacts are created.+ // Also, some contacts can be destroyed.+ m_contactManager.FindNewContacts();++ if (m_subStepping)+ {+ m_stepComplete = false;+ break;+ }+ }+}++void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)+{+ b2Timer stepTimer;++ // If new fixtures were added, we need to find the new contacts.+ if (m_flags & e_newFixture)+ {+ m_contactManager.FindNewContacts();+ m_flags &= ~e_newFixture;+ }++ m_flags |= e_locked;++ b2TimeStep step;+ step.dt = dt;+ step.velocityIterations = velocityIterations;+ step.positionIterations = positionIterations;+ if (dt > 0.0f)+ {+ step.inv_dt = 1.0f / dt;+ }+ else+ {+ step.inv_dt = 0.0f;+ }++ step.dtRatio = m_inv_dt0 * dt;++ step.warmStarting = m_warmStarting;++ // Update contacts. This is where some contacts are destroyed.+ {+ b2Timer timer;+ m_contactManager.Collide();+ m_profile.collide = timer.GetMilliseconds();+ }++ // Integrate velocities, solve velocity constraints, and integrate positions.+ if (m_stepComplete && step.dt > 0.0f)+ {+ b2Timer timer;+ Solve(step);+ m_profile.solve = timer.GetMilliseconds();+ }++ // Handle TOI events.+ if (m_continuousPhysics && step.dt > 0.0f)+ {+ b2Timer timer;+ SolveTOI(step);+ m_profile.solveTOI = timer.GetMilliseconds();+ }++ if (step.dt > 0.0f)+ {+ m_inv_dt0 = step.inv_dt;+ }++ if (m_flags & e_clearForces)+ {+ ClearForces();+ }++ m_flags &= ~e_locked;++ m_profile.step = stepTimer.GetMilliseconds();+}++void b2World::ClearForces()+{+ for (b2Body* body = m_bodyList; body; body = body->GetNext())+ {+ body->m_force.SetZero();+ body->m_torque = 0.0f;+ }+}++struct b2WorldQueryWrapper+{+ bool QueryCallback(int32 proxyId)+ {+ b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);+ return callback->ReportFixture(proxy->fixture);+ }++ const b2BroadPhase* broadPhase;+ b2QueryCallback* callback;+};++void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const+{+ b2WorldQueryWrapper wrapper;+ wrapper.broadPhase = &m_contactManager.m_broadPhase;+ wrapper.callback = callback;+ m_contactManager.m_broadPhase.Query(&wrapper, aabb);+}++struct b2WorldRayCastWrapper+{+ float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)+ {+ void* userData = broadPhase->GetUserData(proxyId);+ b2FixtureProxy* proxy = (b2FixtureProxy*)userData;+ b2Fixture* fixture = proxy->fixture;+ int32 index = proxy->childIndex;+ b2RayCastOutput output;+ bool hit = fixture->RayCast(&output, input, index);++ if (hit)+ {+ float32 fraction = output.fraction;+ b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;+ return callback->ReportFixture(fixture, point, output.normal, fraction);+ }++ return input.maxFraction;+ }++ const b2BroadPhase* broadPhase;+ b2RayCastCallback* callback;+};++void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const+{+ b2WorldRayCastWrapper wrapper;+ wrapper.broadPhase = &m_contactManager.m_broadPhase;+ wrapper.callback = callback;+ b2RayCastInput input;+ input.maxFraction = 1.0f;+ input.p1 = point1;+ input.p2 = point2;+ m_contactManager.m_broadPhase.RayCast(&wrapper, input);+}++void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color)+{+ switch (fixture->GetType())+ {+ case b2Shape::e_circle:+ {+ b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();++ b2Vec2 center = b2Mul(xf, circle->m_p);+ float32 radius = circle->m_radius;+ b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));++ g_debugDraw->DrawSolidCircle(center, radius, axis, color);+ }+ break;++ case b2Shape::e_edge:+ {+ b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();+ b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);+ b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);+ g_debugDraw->DrawSegment(v1, v2, color);+ }+ break;++ case b2Shape::e_chain:+ {+ b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();+ int32 count = chain->m_count;+ const b2Vec2* vertices = chain->m_vertices;++ b2Vec2 v1 = b2Mul(xf, vertices[0]);+ for (int32 i = 1; i < count; ++i)+ {+ b2Vec2 v2 = b2Mul(xf, vertices[i]);+ g_debugDraw->DrawSegment(v1, v2, color);+ g_debugDraw->DrawCircle(v1, 0.05f, color);+ v1 = v2;+ }+ }+ break;++ case b2Shape::e_polygon:+ {+ b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();+ int32 vertexCount = poly->m_count;+ b2Assert(vertexCount <= b2_maxPolygonVertices);+ b2Vec2 vertices[b2_maxPolygonVertices];++ for (int32 i = 0; i < vertexCount; ++i)+ {+ vertices[i] = b2Mul(xf, poly->m_vertices[i]);+ }++ g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);+ }+ break;++ default:+ break;+ }+}++void b2World::DrawJoint(b2Joint* joint)+{+ b2Body* bodyA = joint->GetBodyA();+ b2Body* bodyB = joint->GetBodyB();+ const b2Transform& xf1 = bodyA->GetTransform();+ const b2Transform& xf2 = bodyB->GetTransform();+ b2Vec2 x1 = xf1.p;+ b2Vec2 x2 = xf2.p;+ b2Vec2 p1 = joint->GetAnchorA();+ b2Vec2 p2 = joint->GetAnchorB();++ b2Color color(0.5f, 0.8f, 0.8f);++ switch (joint->GetType())+ {+ case e_distanceJoint:+ g_debugDraw->DrawSegment(p1, p2, color);+ break;++ case e_pulleyJoint:+ {+ b2PulleyJoint* pulley = (b2PulleyJoint*)joint;+ b2Vec2 s1 = pulley->GetGroundAnchorA();+ b2Vec2 s2 = pulley->GetGroundAnchorB();+ g_debugDraw->DrawSegment(s1, p1, color);+ g_debugDraw->DrawSegment(s2, p2, color);+ g_debugDraw->DrawSegment(s1, s2, color);+ }+ break;++ case e_mouseJoint:+ // don't draw this+ break;++ default:+ g_debugDraw->DrawSegment(x1, p1, color);+ g_debugDraw->DrawSegment(p1, p2, color);+ g_debugDraw->DrawSegment(x2, p2, color);+ }+}++void b2World::DrawDebugData()+{+ if (g_debugDraw == NULL)+ {+ return;+ }++ uint32 flags = g_debugDraw->GetFlags();++ if (flags & b2Draw::e_shapeBit)+ {+ for (b2Body* b = m_bodyList; b; b = b->GetNext())+ {+ const b2Transform& xf = b->GetTransform();+ for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())+ {+ if (b->IsActive() == false)+ {+ DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));+ }+ else if (b->GetType() == b2_staticBody)+ {+ DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));+ }+ else if (b->GetType() == b2_kinematicBody)+ {+ DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));+ }+ else if (b->IsAwake() == false)+ {+ DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f));+ }+ else+ {+ DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f));+ }+ }+ }+ }++ if (flags & b2Draw::e_jointBit)+ {+ for (b2Joint* j = m_jointList; j; j = j->GetNext())+ {+ DrawJoint(j);+ }+ }++ if (flags & b2Draw::e_pairBit)+ {+ b2Color color(0.3f, 0.9f, 0.9f);+ for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())+ {+ //b2Fixture* fixtureA = c->GetFixtureA();+ //b2Fixture* fixtureB = c->GetFixtureB();++ //b2Vec2 cA = fixtureA->GetAABB().GetCenter();+ //b2Vec2 cB = fixtureB->GetAABB().GetCenter();++ //g_debugDraw->DrawSegment(cA, cB, color);+ }+ }++ if (flags & b2Draw::e_aabbBit)+ {+ b2Color color(0.9f, 0.3f, 0.9f);+ b2BroadPhase* bp = &m_contactManager.m_broadPhase;++ for (b2Body* b = m_bodyList; b; b = b->GetNext())+ {+ if (b->IsActive() == false)+ {+ continue;+ }++ for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())+ {+ for (int32 i = 0; i < f->m_proxyCount; ++i)+ {+ b2FixtureProxy* proxy = f->m_proxies + i;+ b2AABB aabb = bp->GetFatAABB(proxy->proxyId);+ b2Vec2 vs[4];+ vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);+ vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);+ vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);+ vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);++ g_debugDraw->DrawPolygon(vs, 4, color);+ }+ }+ }+ }++ if (flags & b2Draw::e_centerOfMassBit)+ {+ for (b2Body* b = m_bodyList; b; b = b->GetNext())+ {+ b2Transform xf = b->GetTransform();+ xf.p = b->GetWorldCenter();+ g_debugDraw->DrawTransform(xf);+ }+ }+}++int32 b2World::GetProxyCount() const+{+ return m_contactManager.m_broadPhase.GetProxyCount();+}++int32 b2World::GetTreeHeight() const+{+ return m_contactManager.m_broadPhase.GetTreeHeight();+}++int32 b2World::GetTreeBalance() const+{+ return m_contactManager.m_broadPhase.GetTreeBalance();+}++float32 b2World::GetTreeQuality() const+{+ return m_contactManager.m_broadPhase.GetTreeQuality();+}++void b2World::ShiftOrigin(const b2Vec2& newOrigin)+{+ b2Assert((m_flags & e_locked) == 0);+ if ((m_flags & e_locked) == e_locked)+ {+ return;+ }++ for (b2Body* b = m_bodyList; b; b = b->m_next)+ {+ b->m_xf.p -= newOrigin;+ b->m_sweep.c0 -= newOrigin;+ b->m_sweep.c -= newOrigin;+ }++ for (b2Joint* j = m_jointList; j; j = j->m_next)+ {+ j->ShiftOrigin(newOrigin);+ }++ m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);+}++void b2World::Dump()+{+ if ((m_flags & e_locked) == e_locked)+ {+ return;+ }++ b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y);+ b2Log("m_world->SetGravity(g);\n");++ b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);+ b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);+ int32 i = 0;+ for (b2Body* b = m_bodyList; b; b = b->m_next)+ {+ b->m_islandIndex = i;+ b->Dump();+ ++i;+ }++ i = 0;+ for (b2Joint* j = m_jointList; j; j = j->m_next)+ {+ j->m_index = i;+ ++i;+ }++ // First pass on joints, skip gear joints.+ for (b2Joint* j = m_jointList; j; j = j->m_next)+ {+ if (j->m_type == e_gearJoint)+ {+ continue;+ }++ b2Log("{\n");+ j->Dump();+ b2Log("}\n");+ }++ // Second pass on joints, only gear joints.+ for (b2Joint* j = m_jointList; j; j = j->m_next)+ {+ if (j->m_type != e_gearJoint)+ {+ continue;+ }++ b2Log("{\n");+ j->Dump();+ b2Log("}\n");+ }++ b2Log("b2Free(joints);\n");+ b2Log("b2Free(bodies);\n");+ b2Log("joints = NULL;\n");+ b2Log("bodies = NULL;\n");+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.hnew file mode 100644index 00000000..3968cc78--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.h@@ -0,0 +1,354 @@+/*+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_WORLD_H+#define B2_WORLD_H++#include <Box2D/Common/b2Math.h>+#include <Box2D/Common/b2BlockAllocator.h>+#include <Box2D/Common/b2StackAllocator.h>+#include <Box2D/Dynamics/b2ContactManager.h>+#include <Box2D/Dynamics/b2WorldCallbacks.h>+#include <Box2D/Dynamics/b2TimeStep.h>++struct b2AABB;+struct b2BodyDef;+struct b2Color;+struct b2JointDef;+class b2Body;+class b2Draw;+class b2Fixture;+class b2Joint;++/// The world class manages all physics entities, dynamic simulation,+/// and asynchronous queries. The world also contains efficient memory+/// management facilities.+class b2World+{+public:+ /// Construct a world object.+ /// @param gravity the world gravity vector.+ b2World(const b2Vec2& gravity);++ /// Destruct the world. All physics entities are destroyed and all heap memory is released.+ ~b2World();++ /// Register a destruction listener. The listener is owned by you and must+ /// remain in scope.+ void SetDestructionListener(b2DestructionListener* listener);++ /// Register a contact filter to provide specific control over collision.+ /// Otherwise the default filter is used (b2_defaultFilter). The listener is+ /// owned by you and must remain in scope.+ void SetContactFilter(b2ContactFilter* filter);++ /// Register a contact event listener. The listener is owned by you and must+ /// remain in scope.+ void SetContactListener(b2ContactListener* listener);++ /// Register a routine for debug drawing. The debug draw functions are called+ /// inside with b2World::DrawDebugData method. The debug draw object is owned+ /// by you and must remain in scope.+ void SetDebugDraw(b2Draw* debugDraw);++ /// Create a rigid body given a definition. No reference to the definition+ /// is retained.+ /// @warning This function is locked during callbacks.+ b2Body* CreateBody(const b2BodyDef* def);++ /// Destroy a rigid body given a definition. No reference to the definition+ /// is retained. This function is locked during callbacks.+ /// @warning This automatically deletes all associated shapes and joints.+ /// @warning This function is locked during callbacks.+ void DestroyBody(b2Body* body);++ /// Create a joint to constrain bodies together. No reference to the definition+ /// is retained. This may cause the connected bodies to cease colliding.+ /// @warning This function is locked during callbacks.+ b2Joint* CreateJoint(const b2JointDef* def);++ /// Destroy a joint. This may cause the connected bodies to begin colliding.+ /// @warning This function is locked during callbacks.+ void DestroyJoint(b2Joint* joint);++ /// Take a time step. This performs collision detection, integration,+ /// and constraint solution.+ /// @param timeStep the amount of time to simulate, this should not vary.+ /// @param velocityIterations for the velocity constraint solver.+ /// @param positionIterations for the position constraint solver.+ void Step( float32 timeStep,+ int32 velocityIterations,+ int32 positionIterations);++ /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically+ /// after each call to Step. The default behavior is modified by calling SetAutoClearForces.+ /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain+ /// a fixed sized time step under a variable frame-rate.+ /// When you perform sub-stepping you will disable auto clearing of forces and instead call+ /// ClearForces after all sub-steps are complete in one pass of your game loop.+ /// @see SetAutoClearForces+ void ClearForces();++ /// Call this to draw shapes and other debug draw data. This is intentionally non-const.+ void DrawDebugData();++ /// Query the world for all fixtures that potentially overlap the+ /// provided AABB.+ /// @param callback a user implemented callback class.+ /// @param aabb the query box.+ void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const;++ /// Ray-cast the world for all fixtures in the path of the ray. Your callback+ /// controls whether you get the closest point, any point, or n-points.+ /// The ray-cast ignores shapes that contain the starting point.+ /// @param callback a user implemented callback class.+ /// @param point1 the ray starting point+ /// @param point2 the ray ending point+ void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const;++ /// Get the world body list. With the returned body, use b2Body::GetNext to get+ /// the next body in the world list. A NULL body indicates the end of the list.+ /// @return the head of the world body list.+ b2Body* GetBodyList();+ const b2Body* GetBodyList() const;++ /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get+ /// the next joint in the world list. A NULL joint indicates the end of the list.+ /// @return the head of the world joint list.+ b2Joint* GetJointList();+ const b2Joint* GetJointList() const;++ /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get+ /// the next contact in the world list. A NULL contact indicates the end of the list.+ /// @return the head of the world contact list.+ /// @warning contacts are created and destroyed in the middle of a time step.+ /// Use b2ContactListener to avoid missing contacts.+ b2Contact* GetContactList();+ const b2Contact* GetContactList() const;++ /// Enable/disable sleep.+ void SetAllowSleeping(bool flag);+ bool GetAllowSleeping() const { return m_allowSleep; }++ /// Enable/disable warm starting. For testing.+ void SetWarmStarting(bool flag) { m_warmStarting = flag; }+ bool GetWarmStarting() const { return m_warmStarting; }++ /// Enable/disable continuous physics. For testing.+ void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }+ bool GetContinuousPhysics() const { return m_continuousPhysics; }++ /// Enable/disable single stepped continuous physics. For testing.+ void SetSubStepping(bool flag) { m_subStepping = flag; }+ bool GetSubStepping() const { return m_subStepping; }++ /// Get the number of broad-phase proxies.+ int32 GetProxyCount() const;++ /// Get the number of bodies.+ int32 GetBodyCount() const;++ /// Get the number of joints.+ int32 GetJointCount() const;++ /// Get the number of contacts (each may have 0 or more contact points).+ int32 GetContactCount() const;++ /// Get the height of the dynamic tree.+ int32 GetTreeHeight() const;++ /// Get the balance of the dynamic tree.+ int32 GetTreeBalance() const;++ /// Get the quality metric of the dynamic tree. The smaller the better.+ /// The minimum is 1.+ float32 GetTreeQuality() const;++ /// Change the global gravity vector.+ void SetGravity(const b2Vec2& gravity);++ /// Get the global gravity vector.+ b2Vec2 GetGravity() const;++ /// Is the world locked (in the middle of a time step).+ bool IsLocked() const;++ /// Set flag to control automatic clearing of forces after each time step.+ void SetAutoClearForces(bool flag);++ /// Get the flag that controls automatic clearing of forces after each time step.+ bool GetAutoClearForces() const;++ /// Shift the world origin. Useful for large worlds.+ /// The body shift formula is: position -= newOrigin+ /// @param newOrigin the new origin with respect to the old origin+ void ShiftOrigin(const b2Vec2& newOrigin);++ /// Get the contact manager for testing.+ const b2ContactManager& GetContactManager() const;++ /// Get the current profile.+ const b2Profile& GetProfile() const;++ /// Dump the world into the log file.+ /// @warning this should be called outside of a time step.+ void Dump();++private:++ // m_flags+ enum+ {+ e_newFixture = 0x0001,+ e_locked = 0x0002,+ e_clearForces = 0x0004+ };++ friend class b2Body;+ friend class b2Fixture;+ friend class b2ContactManager;+ friend class b2Controller;++ void Solve(const b2TimeStep& step);+ void SolveTOI(const b2TimeStep& step);++ void DrawJoint(b2Joint* joint);+ void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color);++ b2BlockAllocator m_blockAllocator;+ b2StackAllocator m_stackAllocator;++ int32 m_flags;++ b2ContactManager m_contactManager;++ b2Body* m_bodyList;+ b2Joint* m_jointList;++ int32 m_bodyCount;+ int32 m_jointCount;++ b2Vec2 m_gravity;+ bool m_allowSleep;++ b2DestructionListener* m_destructionListener;+ b2Draw* g_debugDraw;++ // This is used to compute the time step ratio to+ // support a variable time step.+ float32 m_inv_dt0;++ // These are for debugging the solver.+ bool m_warmStarting;+ bool m_continuousPhysics;+ bool m_subStepping;++ bool m_stepComplete;++ b2Profile m_profile;+};++inline b2Body* b2World::GetBodyList()+{+ return m_bodyList;+}++inline const b2Body* b2World::GetBodyList() const+{+ return m_bodyList;+}++inline b2Joint* b2World::GetJointList()+{+ return m_jointList;+}++inline const b2Joint* b2World::GetJointList() const+{+ return m_jointList;+}++inline b2Contact* b2World::GetContactList()+{+ return m_contactManager.m_contactList;+}++inline const b2Contact* b2World::GetContactList() const+{+ return m_contactManager.m_contactList;+}++inline int32 b2World::GetBodyCount() const+{+ return m_bodyCount;+}++inline int32 b2World::GetJointCount() const+{+ return m_jointCount;+}++inline int32 b2World::GetContactCount() const+{+ return m_contactManager.m_contactCount;+}++inline void b2World::SetGravity(const b2Vec2& gravity)+{+ m_gravity = gravity;+}++inline b2Vec2 b2World::GetGravity() const+{+ return m_gravity;+}++inline bool b2World::IsLocked() const+{+ return (m_flags & e_locked) == e_locked;+}++inline void b2World::SetAutoClearForces(bool flag)+{+ if (flag)+ {+ m_flags |= e_clearForces;+ }+ else+ {+ m_flags &= ~e_clearForces;+ }+}++/// Get the flag that controls automatic clearing of forces after each time step.+inline bool b2World::GetAutoClearForces() const+{+ return (m_flags & e_clearForces) == e_clearForces;+}++inline const b2ContactManager& b2World::GetContactManager() const+{+ return m_contactManager;+}++inline const b2Profile& b2World::GetProfile() const+{+ return m_profile;+}++#endifdiff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.cppnew file mode 100644index 00000000..82b28cc0--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp@@ -0,0 +1,36 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#include <Box2D/Dynamics/b2WorldCallbacks.h>+#include <Box2D/Dynamics/b2Fixture.h>++// Return true if contact calculations should be performed between these two shapes.+// If you implement your own collision filter you may want to build from this implementation.+bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)+{+ const b2Filter& filterA = fixtureA->GetFilterData();+ const b2Filter& filterB = fixtureB->GetFilterData();++ if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)+ {+ return filterA.groupIndex > 0;+ }++ bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;+ return collide;+}diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.hnew file mode 100644index 00000000..9b937e81--- /dev/null+++ b/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.h@@ -0,0 +1,155 @@+/*+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org+*+* This software is provided 'as-is', without any express or implied+* warranty. In no event will the authors be held liable for any damages+* arising from the use of this software.+* Permission is granted to anyone to use this software for any purpose,+* including commercial applications, and to alter it and redistribute it+* freely, subject to the following restrictions:+* 1. The origin of this software must not be misrepresented; you must not+* claim that you wrote the original software. If you use this software+* in a product, an acknowledgment in the product documentation would be+* appreciated but is not required.+* 2. Altered source versions must be plainly marked as such, and must not be+* misrepresented as being the original software.+* 3. This notice may not be removed or altered from any source distribution.+*/++#ifndef B2_WORLD_CALLBACKS_H+#define B2_WORLD_CALLBACKS_H++#include <Box2D/Common/b2Settings.h>++struct b2Vec2;+struct b2Transform;+class b2Fixture;+class b2Body;+class b2Joint;+class b2Contact;+struct b2ContactResult;+struct b2Manifold;++/// Joints and fixtures are destroyed when their associated+/// body is destroyed. Implement this listener so that you+/// may nullify references to these joints and shapes.+class b2DestructionListener+{+public:+ virtual ~b2DestructionListener() {}++ /// Called when any joint is about to be destroyed due+ /// to the destruction of one of its attached bodies.+ virtual void SayGoodbye(b2Joint* joint) = 0;++ /// Called when any fixture is about to be destroyed due+ /// to the destruction of its parent body.+ virtual void SayGoodbye(b2Fixture* fixture) = 0;+};++/// Implement this class to provide collision filtering. In other words, you can implement+/// this class if you want finer control over contact creation.+class b2ContactFilter+{+public:+ virtual ~b2ContactFilter() {}++ /// Return true if contact calculations should be performed between these two shapes.+ /// @warning for performance reasons this is only called when the AABBs begin to overlap.+ virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB);+};++/// Contact impulses for reporting. Impulses are used instead of forces because+/// sub-step forces may approach infinity for rigid body collisions. These+/// match up one-to-one with the contact points in b2Manifold.+struct b2ContactImpulse+{+ float32 normalImpulses[b2_maxManifoldPoints];+ float32 tangentImpulses[b2_maxManifoldPoints];+ int32 count;+};++/// Implement this class to get contact information. You can use these results for+/// things like sounds and game logic. You can also get contact results by+/// traversing the contact lists after the time step. However, you might miss+/// some contacts because continuous physics leads to sub-stepping.+/// Additionally you may receive multiple callbacks for the same contact in a+/// single time step.+/// You should strive to make your callbacks efficient because there may be+/// many callbacks per time step.+/// @warning You cannot create/destroy Box2D entities inside these callbacks.+class b2ContactListener+{+public:+ virtual ~b2ContactListener() {}++ /// Called when two fixtures begin to touch.+ virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); }++ /// Called when two fixtures cease to touch.+ virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); }++ /// This is called after a contact is updated. This allows you to inspect a+ /// contact before it goes to the solver. If you are careful, you can modify the+ /// contact manifold (e.g. disable contact).+ /// A copy of the old manifold is provided so that you can detect changes.+ /// Note: this is called only for awake bodies.+ /// Note: this is called even when the number of contact points is zero.+ /// Note: this is not called for sensors.+ /// Note: if you set the number of contact points to zero, you will not+ /// get an EndContact callback. However, you may get a BeginContact callback+ /// the next step.+ virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold)+ {+ B2_NOT_USED(contact);+ B2_NOT_USED(oldManifold);+ }++ /// This lets you inspect a contact after the solver is finished. This is useful+ /// for inspecting impulses.+ /// Note: the contact manifold does not include time of impact impulses, which can be+ /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly+ /// in a separate data structure.+ /// Note: this is only called for contacts that are touching, solid, and awake.+ virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse)+ {+ B2_NOT_USED(contact);+ B2_NOT_USED(impulse);+ }+};++/// Callback class for AABB queries.+/// See b2World::Query+class b2QueryCallback+{+public:+ virtual ~b2QueryCallback() {}++ /// Called for each fixture found in the query AABB.+ /// @return false to terminate the query.+ virtual bool ReportFixture(b2Fixture* fixture) = 0;+};++/// Callback class for ray casts.+/// See b2World::RayCast+class b2RayCastCallback+{+public:+ virtual ~b2RayCastCallback() {}++ /// Called for each fixture found in the query. You control how the ray cast+ /// proceeds by returning a float:+ /// return -1: ignore this fixture and continue+ /// return 0: terminate the ray cast+ /// return fraction: clip the ray to this point+ /// return 1: don't clip the ray and continue+ /// @param fixture the fixture hit by the ray+ /// @param point the point of initial intersection+ /// @param normal the normal vector at the point of intersection+ /// @return -1 to filter, 0 to terminate, fraction to clip the ray for+ /// closest hit, 1 to continue+ virtual float32 ReportFixture( b2Fixture* fixture, const b2Vec2& point,+ const b2Vec2& normal, float32 fraction) = 0;+};++#endifdiff --git a/games/mrbullet/src/box2d/License.txt b/games/mrbullet/src/box2d/License.txtnew file mode 100644index 00000000..e99b7036--- /dev/null+++ b/games/mrbullet/src/box2d/License.txt@@ -0,0 +1,18 @@+Copyright (c) 2006-2013 Erin Catto http://www.gphysics.com++This software is provided 'as-is', without any express or implied+warranty. In no event will the authors be held liable for any damages+arising from the use of this software.++Permission is granted to anyone to use this software for any purpose,+including commercial applications, and to alter it and redistribute it+freely, subject to the following restrictions:++1. The origin of this software must not be misrepresented; you must not+claim that you wrote the original software. If you use this software+in a product, an acknowledgment in the product documentation would be+appreciated but is not required.+2. Altered source versions must be plainly marked as such, and must not be+misrepresented as being the original software.+3. This notice may not be removed or altered from any source distribution.+diff --git a/games/mrbullet/src/data.S b/games/mrbullet/src/data.Snew file mode 100644index 00000000..af6eaf62--- /dev/null+++ b/games/mrbullet/src/data.S@@ -0,0 +1,19 @@+/* data.bin (tools/pack.py) in the app's read-only data */+#ifdef __APPLE__+ .const_data+ .p2align 2+ .globl _mb_data+_mb_data:+ .incbin DATA_BIN+ .p2align 2+#else+ .section .rodata.mb_data,"a"+ .balign 4+ .global mb_data+mb_data:+ .incbin DATA_BIN+ .balign 4+#endif+#ifdef __ELF__+ .section .note.GNU-stack,"",%progbits+#endifdiff --git a/games/mrbullet/src/data.bin b/games/mrbullet/src/data.binnew file mode 100644index 00000000..2ff73631Binary files /dev/null and b/games/mrbullet/src/data.bin differdiff --git a/games/mrbullet/src/data.h b/games/mrbullet/src/data.hnew file mode 100644index 00000000..38bd0e09--- /dev/null+++ b/games/mrbullet/src/data.h@@ -0,0 +1,28 @@+/* Mr Bullet: what tools/pack.py put in data.bin (generated) */+#ifndef MB_DATA_H+#define MB_DATA_H+#define LEVEL_COUNT 596+#define LEVEL_MAX_BYTES 3104+#define MODE_COUNT 4+#define MODE_FIRST {0, 320, 404, 488, 596}+#define PIC_BG 0+#define PIC_HERO_TORSO 1+#define PIC_HERO_HEAD 2+#define PIC_HERO_DEAD 3+#define PIC_ENEMY_TORSO 4+#define PIC_ENEMY_HEAD 5+#define PIC_ENEMY_DEAD 6+#define PIC_HOSTAGE_TORSO 7+#define PIC_HOSTAGE_HEAD 8+#define PIC_HOSTAGE_DEAD 9+#define PIC_TITLE 10+#define PIC_BULLET 11+#define PIC_GRENADE 12+#define PIC_STAR 13+#define PIC_STAR_BIG 14+#define PIC_PADLOCK 15+#define PIC_COUNT 16+#define COLOURS_HERO {0x3AFA, 0x2237, 0xFE55, 0x21F4, 0x21F4, 0x0000}+#define COLOURS_ENEMY {0x39E7, 0x2104, 0xFE55, 0x2104, 0x2104, 0x0000}+#define COLOURS_HOSTAGE {0xFE54, 0xFE54, 0xFE54, 0x4228, 0x4228, 0x0000}+#endifdiff --git a/games/mrbullet/src/draw.c b/games/mrbullet/src/draw.cnew file mode 100644index 00000000..b997ccf2--- /dev/null+++ b/games/mrbullet/src/draw.c@@ -0,0 +1,488 @@+/* Drawing (draw.h): the level into the launcher's strip (gfx_buf, rows gfx_y0 to gfx_y1), clipped to the field. */+#include <math.h>+#include <string.h>+#include "draw.h"+#include "game.h"+#include "res.h"+#include "world.h"++static int clip_x0 = 0, clip_x1 = SCREEN_W;+static inline int clampi(int v, int lo, int hi) { return v < lo ? lo : v > hi ? hi : v; }++void draw_pic(int pic, int x, int y) {+ const Pic *p = res_pic(pic);+ int y0 = y > gfx_y0 ? y : gfx_y0, y1 = y + p->h < gfx_y1 ? y + p->h : gfx_y1;+ int x0 = x > clip_x0 ? x : clip_x0, x1 = x + p->w < clip_x1 ? x + p->w : clip_x1;+ if (y0 >= y1 || x0 >= x1) return;+ const uint16_t *pal = res_pic_palette(p);+ const uint8_t *px = res_pic_pixels(p);+ bool wide = p->flags & 2, opaque = p->flags & 1;+ for (int yy = y0; yy < y1; yy++) {+ uint32_t row = (uint32_t)(yy - y) * p->w;+ color_t *d = gfx_buf + (yy - gfx_y0) * SCREEN_W;+ for (int xx = x0; xx < x1; xx++) {+ uint32_t at = row + (uint32_t)(xx - x);+ unsigned k = wide ? px[at] : (px[at >> 1] >> ((at & 1) * 4)) & 15;+ if (k || opaque) d[xx] = pal[k];+ }+ }+}++void draw_pic_at(int pic, float sx, float sy, float scale, float sn, float cs, bool flip) {+ if (scale < 0.05f) return;+ const Pic *p = res_pic(pic);+ float px = p->px4 / 4.0f, py = p->py4 / 4.0f;+ float ex = fmaxf(px, p->w - px), ey = fmaxf(py, p->h - py);+ float r = sqrtf(ex * ex + ey * ey) * scale + 1;+ int y0 = (int)floorf(sy - r), y1 = (int)ceilf(sy + r), x0 = (int)floorf(sx - r), x1 = (int)ceilf(sx + r);+ if (y1 <= gfx_y0 || y0 >= gfx_y1 || x1 <= clip_x0 || x0 >= clip_x1) return;+ y0 = clampi(y0, gfx_y0, gfx_y1), y1 = clampi(y1, gfx_y0, gfx_y1);+ x0 = clampi(x0, clip_x0, clip_x1), x1 = clampi(x1, clip_x0, clip_x1);+ const uint16_t *pal = res_pic_palette(p);+ const uint8_t *pix = res_pic_pixels(p);+ float inv = 1 / scale, fl = flip ? -1.0f : 1.0f;+ int32_t dux = (int32_t)lroundf(cs * inv * fl * 65536), dvx = (int32_t)lroundf(-sn * inv * 65536);+ int32_t duy = (int32_t)lroundf(sn * inv * fl * 65536), dvy = (int32_t)lroundf(cs * inv * 65536);+ float fx = x0 + 0.5f - sx, fy = y0 + 0.5f - sy;+ int32_t u0 = (int32_t)lroundf(((fx * cs + fy * sn) * inv * fl + px) * 65536);+ int32_t v0 = (int32_t)lroundf(((-fx * sn + fy * cs) * inv + py) * 65536);+ uint32_t w = p->w, h = p->h;+ bool wide = p->flags & 2;+ for (int yy = y0; yy < y1; yy++, u0 += duy, v0 += dvy) {+ color_t *d = gfx_buf + (yy - gfx_y0) * SCREEN_W;+ int32_t u = u0, v = v0;+ for (int xx = x0; xx < x1; xx++, u += dux, v += dvx) {+ if (u < 0 || v < 0) continue;+ uint32_t iu = (uint32_t)u >> 16, iv = (uint32_t)v >> 16;+ if (iu >= w || iv >= h) continue;+ uint32_t at = iv * w + iu;+ unsigned k = wide ? pix[at] : (pix[at >> 1] >> ((at & 1) * 4)) & 15;+ if (k) d[xx] = pal[k];+ }+ }+}++static inline void plot(int x, int y, color_t c) {+ if (y >= gfx_y0 && y < gfx_y1 && x >= clip_x0 && x < clip_x1) gfx_buf[(y - gfx_y0) * SCREEN_W + x] = c;+}++void draw_line(float x0, float y0, float x1, float y1, color_t c) {+ float dx = x1 - x0, dy = y1 - y0;+ float n = fmaxf(fabsf(dx), fabsf(dy));+ if ((y0 < gfx_y0 - 1 && y1 < gfx_y0 - 1) || (y0 > gfx_y1 && y1 > gfx_y1)) return;+ int steps = (int)n + 1;+ if (steps > 2000) steps = 2000;+ float sx = n > 0 ? dx / n : 0, sy = n > 0 ? dy / n : 0;+ for (int i = 0; i < steps; i++) plot((int)floorf(x0 + sx * i), (int)floorf(y0 + sy * i), c);+}++void draw_disc(float cx, float cy, float r, color_t c) {+ int y0 = clampi((int)floorf(cy - r), gfx_y0, gfx_y1), y1 = clampi((int)ceilf(cy + r) + 1, gfx_y0, gfx_y1);+ for (int y = y0; y < y1; y++) {+ float dy = y + 0.5f - cy, w2 = r * r - dy * dy;+ if (w2 < 0) continue;+ float w = sqrtf(w2);+ int a = clampi((int)ceilf(cx - w - 0.5f), clip_x0, clip_x1), b = clampi((int)floorf(cx + w - 0.5f) + 1, clip_x0, clip_x1);+ color_t *d = gfx_buf + (y - gfx_y0) * SCREEN_W;+ for (int x = a; x < b; x++) d[x] = c;+ }+}++/* ------------------------------------------------------------------ polygons */+#define MAXP (SCRATCH_FLOATS / 2)+#define px_ mb_scratch+#define py_ (mb_scratch + MAXP)+static uint16_t cstart[17];+static int ncont;++enum { PAT_SOLID, PAT_BRICKS, PAT_GLASS };+typedef struct {+ color_t fill, alt, edge, edge2;+ uint8_t pattern;+} Style;++static void fill(const Style *st, float ymin, float ymax) {+ int ya = clampi((int)floorf(ymin), gfx_y0, gfx_y1), yb = clampi((int)ceilf(ymax) + 1, gfx_y0, gfx_y1);+ float xs[64];+ for (int y = ya; y < yb; y++) {+ float yc = y + 0.5f;+ int n = 0;+ for (int c = 0; c < ncont; c++) {+ int s = cstart[c], e = cstart[c + 1];+ for (int i = s; i < e; i++) {+ int j = i + 1 < e ? i + 1 : s;+ float ay = py_[i], by = py_[j];+ if ((ay <= yc) == (by <= yc)) continue;+ if (n < 64) xs[n++] = px_[i] + (yc - ay) * (px_[j] - px_[i]) / (by - ay);+ }+ }+ for (int i = 1; i < n; i++) {+ float v = xs[i];+ int k = i - 1;+ while (k >= 0 && xs[k] > v) xs[k + 1] = xs[k], k--;+ xs[k + 1] = v;+ }+ color_t *d = gfx_buf + (y - gfx_y0) * SCREEN_W;+ for (int i = 0; i + 1 < n; i += 2) {+ int a = clampi((int)ceilf(xs[i] - 0.5f), clip_x0, clip_x1);+ int b = clampi((int)ceilf(xs[i + 1] - 0.5f), clip_x0, clip_x1);+ if (st->pattern == PAT_BRICKS) {+ int row = y / 4, mortar_row = (y & 3) == 0;+ for (int x = a; x < b; x++) d[x] = (mortar_row || ((x + (row & 1) * 5) % 10) == 0) ? st->alt : st->fill;+ } else if (st->pattern == PAT_GLASS) {+ for (int x = a; x < b; x++) d[x] = ((x + y) % 9 < 2) ? st->alt : st->fill;+ } else {+ for (int x = a; x < b; x++) d[x] = st->fill;+ }+ }+ }+ /* the edge */+ for (int c = 0; c < ncont; c++) {+ int s = cstart[c], e = cstart[c + 1];+ for (int i = s; i < e; i++) {+ int j = i + 1 < e ? i + 1 : s;+ draw_line(px_[i], py_[i], px_[j], py_[j], st->edge);+ if (st->edge2 != st->edge) draw_line(px_[i], py_[i] + 1, px_[j], py_[j] + 1, st->edge2);+ }+ }+}++static const color_t SWITCH_COL[] = {HEX(0xE53935), HEX(0x1E88E5), HEX(0x43A047), HEX(0xFDD835),+ HEX(0x8E24AA), HEX(0xFB8C00)};++static void style_of(const Obj *o, Style *st) {+ uint32_t f = o->flags;+ st->pattern = PAT_SOLID;+ if (o->sw) {+ color_t c = SWITCH_COL[g.sw[o->sw - 1].colour % 6];+ st->fill = c, st->edge = st->edge2 = gfx_mix(c, 0, 12);+ } else if (f & F_BE) {+ st->fill = HEX(0xE0352B), st->edge = st->edge2 = HEX(0x7A1410);+ } else if (f & F_BR) {+ st->fill = HEX(0x4DBDAE), st->alt = HEX(0x8FDDD1), st->edge = st->edge2 = HEX(0x2A7C73);+ st->pattern = PAT_GLASS;+ } else if (f & F_BO) {+ st->fill = HEX(0xE9B872), st->edge = st->edge2 = HEX(0x9A6A2E);+ } else if (f & F_HV) {+ st->fill = HEX(0x8D99A6), st->edge = st->edge2 = HEX(0x3F4952);+ } else if (f & F_F) {+ st->fill = HEX(0x4B5459), st->alt = HEX(0x343C3F), st->edge = HEX(0xA5A6A8), st->edge2 = HEX(0x67767D);+ st->pattern = PAT_BRICKS;+ } else {+ st->fill = HEX(0xC98A45), st->edge = st->edge2 = HEX(0x7D5124);+ }+}++static float bb_y0[MAX_OBJ], bb_y1[MAX_OBJ];++static void draw_obj(int i) {+ const Obj *o = &g.obj[i];+ if (o->gone || o->kind == K_POINT) return;+ float bx = o->cx, by = o->cy, s = 0, c = 1;+ if (o->body >= 0 && world_alive(o->body)) world_xf(o->body, &bx, &by, &s, &c);+ Style st;+ style_of(o, &st);+ if (o->kind == K_SEG) { /* a ball */+ float x = SX(bx), y = SY(by), r = o->r * FIELD_S;+ if (y + r < gfx_y0 || y - r > gfx_y1) return;+ draw_disc(x, y, r + 0.6f, st.edge);+ draw_disc(x, y, r - 0.6f, st.fill);+ return;+ }+ if (gfx_y0 == 0) {+ /* the first strip: its rows on the screen */+ const uint8_t *q = o->outline;+ float lo = 1e9f, hi = -1e9f;+ for (int k = 0; k < o->nout; k++) {+ float lx = (int16_t)(q[0] | q[1] << 8) * 0.125f, ly = (int16_t)(q[2] | q[3] << 8) * 0.125f;+ float y = SY(by + s * lx + c * ly);+ if (y < lo) lo = y;+ if (y > hi) hi = y;+ q += 4;+ }+ bb_y0[i] = lo, bb_y1[i] = hi;+ }+ if (bb_y1[i] + 2 < gfx_y0 || bb_y0[i] - 2 > gfx_y1) return;+ const uint8_t *q = o->outline;+ int n = 0;+ ncont = 0;+ cstart[0] = 0;+ int total = o->nout;+ const uint8_t *hq = o->outline + 4 * o->nout;+ for (int part = 0; part <= o->nholes && ncont < 16; part++) {+ if (part) {+ total = *hq++;+ q = hq;+ hq += 4 * total;+ }+ for (int k = 0; k < total && n < MAXP; k++, q += 4) {+ float lx = (int16_t)(q[0] | q[1] << 8) * 0.125f, ly = (int16_t)(q[2] | q[3] << 8) * 0.125f;+ px_[n] = SX(bx + c * lx - s * ly), py_[n] = SY(by + s * lx + c * ly);+ n++;+ }+ cstart[++ncont] = (uint16_t)n;+ }+ fill(&st, bb_y0[i], bb_y1[i]);+}++/* ------------------------------------------------------------------ characters */+static const color_t COLS[3][6] = {COLOURS_HERO, COLOURS_ENEMY, COLOURS_HOSTAGE};++static void quad(float x, float y, float s, float c, float w, float h, float from, float to, color_t col) {+ /* the part of a piece from `from` to `to` of its length (0 top, 1 bottom) */+ float rx = c * w / 2, ry = s * w / 2, dx = -s * h, dy = c * h;+ float tx = x - dx / 2, ty = y - dy / 2;+ float ax = tx + dx * from, ay = ty + dy * from, bx = tx + dx * to, by = ty + dy * to;+ px_[0] = SX(ax - rx), py_[0] = SY(ay - ry);+ px_[1] = SX(ax + rx), py_[1] = SY(ay + ry);+ px_[2] = SX(bx + rx), py_[2] = SY(by + ry);+ px_[3] = SX(bx - rx), py_[3] = SY(by - ry);+ float lo = fminf(fminf(py_[0], py_[1]), fminf(py_[2], py_[3])), hi = fmaxf(fmaxf(py_[0], py_[1]), fmaxf(py_[2], py_[3]));+ if (hi + 1 < gfx_y0 || lo - 1 > gfx_y1) return;+ ncont = 1, cstart[0] = 0, cstart[1] = 4;+ Style st = {col, col, col, col, PAT_SOLID};+ /* (no edge: draw its fill only) */+ int ya = clampi((int)floorf(lo), gfx_y0, gfx_y1), yb = clampi((int)ceilf(hi) + 1, gfx_y0, gfx_y1);+ (void)st;+ for (int yy = ya; yy < yb; yy++) {+ float yc = yy + 0.5f, xs[4];+ int n = 0;+ for (int i = 0; i < 4; i++) {+ int j = (i + 1) & 3;+ if ((py_[i] <= yc) == (py_[j] <= yc)) continue;+ xs[n++] = px_[i] + (yc - py_[i]) * (px_[j] - px_[i]) / (py_[j] - py_[i]);+ }+ if (n < 2) continue;+ float a0 = fminf(xs[0], xs[1]), b0 = fmaxf(xs[0], xs[1]);+ int a = clampi((int)ceilf(a0 - 0.5f), clip_x0, clip_x1), b = clampi((int)ceilf(b0 - 0.5f), clip_x0, clip_x1);+ if (b == a && b0 - a0 > 0.2f && a < clip_x1) b = a + 1; /* (thin limbs keep a pixel) */+ color_t *d = gfx_buf + (yy - gfx_y0) * SCREEN_W;+ for (int x = a; x < b; x++) d[x] = col;+ }+}++static void draw_limb(const Char *ch, int p, color_t col, color_t end, float end_from) {+ float x, y, s, c, w, h;+ if (!game_part(ch, p, &x, &y, &s, &c)) return;+ game_part_size(ch, p, &w, &h);+ /* the sheet's limbs reach past their joints */+ h *= 1.2f;+ quad(x, y, s, c, w, h, 0, end_from, col);+ if (end_from < 1) quad(x, y, s, c, w * (p == P_SHIN_F || p == P_SHIN_B ? 1.0f : 0.95f), h, end_from, 1, end);+}++static void draw_shoe(const Char *ch, int p) {+ float x, y, s, c, w, h;+ if (!game_part(ch, p, &x, &y, &s, &c)) return;+ game_part_size(ch, p, &w, &h);+ float side = ch->left ? -1.0f : 1.0f;+ /* the foot: forward of the shin's bottom */+ float bx = x - s * h * 0.55f, by = y + c * h * 0.55f;+ float fx = bx + c * w * 0.45f * side, fy = by + s * w * 0.45f * side;+ quad(fx, fy, s, c, w * 1.9f, h * 0.22f, 0, 1, 0);+}++static void draw_char(const Char *ch) {+ int role = ch->role;+ const color_t *col = COLS[role];+ float k = ch->h / 80;+ int torso = role == R_HERO ? PIC_HERO_TORSO : role == R_ENEMY ? PIC_ENEMY_TORSO : PIC_HOSTAGE_TORSO;+ int head = role == R_HERO ? PIC_HERO_HEAD : role == R_ENEMY ? PIC_ENEMY_HEAD : PIC_HOSTAGE_HEAD;+ if (!ch->alive) head = role == R_HERO ? PIC_HERO_DEAD : role == R_ENEMY ? PIC_ENEMY_DEAD : PIC_HOSTAGE_DEAD;+ bool flip = ch->left != 0;+ draw_limb(ch, P_ARM_B, col[1], col[1], 1);+ draw_limb(ch, P_FORE_B, col[1], col[2], 0.72f);+ draw_limb(ch, P_THIGH_B, col[4], col[4], 1);+ draw_limb(ch, P_SHIN_B, col[4], col[4], 1);+ draw_shoe(ch, P_SHIN_B);+ draw_limb(ch, P_THIGH_F, col[3], col[3], 1);+ draw_limb(ch, P_SHIN_F, col[3], col[3], 1);+ draw_shoe(ch, P_SHIN_F);+ float x, y, s, c, w, h;+ if (game_part(ch, P_TORSO, &x, &y, &s, &c)) draw_pic_at(torso, SX(x), SY(y), k, s, c, flip);+ if (game_part(ch, P_HEAD, &x, &y, &s, &c)) {+ game_part_size(ch, P_HEAD, &w, &h);+ float nx = x - s * h / 2 * -1.0f, ny = y + c * h / 2; /* the neck: the head's bottom */+ nx = x + (-s) * (h / 2), ny = y + c * (h / 2);+ draw_pic_at(head, SX(nx), SY(ny), k, s, c, flip);+ }+ /* the shooter's gun, in his hand */+ if (ch == &g.ch[g.hero] && ch->alive) {+ float sx, sy, mx, my, dx = cosf(g.aim), dy = sinf(g.aim);+ game_gun(&sx, &sy, &mx, &my);+ float hx = sx + dx * 30 * k, hy = sy + dy * 30 * k;+ if (g.grenades) {+ draw_disc(SX(hx), SY(hy), 2.6f * k, HEX(0x3D4A23));+ } else {+ float ex = hx + dx * 15 * k, ey = hy + dy * 15 * k;+ draw_line(SX(hx), SY(hy), SX(ex), SY(ey), 0);+ draw_line(SX(hx) - dy, SY(hy) + dx, SX(ex) - dy, SY(ey) + dx, HEX(0x222222));+ }+ }+ draw_limb(ch, P_ARM_F, col[0], col[0], 1);+ draw_limb(ch, P_FORE_F, col[0], col[2], 0.72f);+}++/* ------------------------------------------------------------------ the world */+static void draw_pins(void) {+ for (int i = 0; i < g.npin; i++) {+ const Pin *p = &g.pin[i];+ if (p->t == 3) {+ if (p->joint < 0 || !world_joint_alive(p->joint)) continue;+ float ax, ay, bx, by;+ world_joint_anchors(p->joint, &ax, &ay, &bx, &by);+ draw_line(SX(ax), SY(ay), SX(bx), SY(by), HEX(0x6B4423));+ draw_line(SX(ax) + 1, SY(ay), SX(bx) + 1, SY(by), HEX(0x8E6236));+ } else if (p->t == 5) {+ draw_line(SX(p->x0), SY(p->y0), SX(p->x1), SY(p->y1), HEX(0x2B3238));+ }+ }+}++static void draw_pin_dots(void) {+ for (int i = 0; i < g.npin; i++) {+ const Pin *p = &g.pin[i];+ if (p->t != 2 || p->joint < 0 || !world_joint_alive(p->joint)) continue;+ float ax, ay, bx, by;+ world_joint_anchors(p->joint, &ax, &ay, &bx, &by);+ draw_disc(SX(ax), SY(ay), 2.2f, HEX(0x2B3238));+ draw_disc(SX(ax), SY(ay), 1.0f, HEX(0xB0B8C0));+ }+}++static void draw_switches(void) {+ for (int i = 0; i < g.nsw; i++) {+ const Switch *s = &g.sw[i];+ color_t c = s->on ? HEX(0x9AA3AA) : SWITCH_COL[s->colour % 6];+ float x0 = SX(s->x0), y0 = SY(s->y0), x1 = SX(s->x1), y1 = SY(s->y1);+ for (int k = -1; k <= 1; k++) {+ draw_line(x0 + k, y0, x1 + k, y1, c);+ draw_line(x0, y0 + k, x1, y1 + k, c);+ }+ }+}++static void draw_shots(void) {+ for (int i = 0; i < MAX_SHOT; i++) {+ const Shot *s = &g.shot[i];+ if (s->body < 0) continue;+ float x, y, sn, cs;+ world_xf(s->body, &x, &y, &sn, &cs);+ if (s->kind == 0) {+ for (int k = 0; k + 1 < s->tn; k++)+ draw_line(SX(s->tx[k]), SY(s->ty[k]), SX(s->tx[k + 1]), SY(s->ty[k + 1]),+ k < 4 ? 0xFFFF : HEX(0xD8DEE4));+ draw_disc(SX(x), SY(y), 1.6f, HEX(0xC99400));+ } else {+ draw_disc(SX(x), SY(y), 5 * FIELD_S + 0.8f, HEX(0x1F2612));+ draw_disc(SX(x), SY(y), 5 * FIELD_S, (s->life / 8) & 1 ? HEX(0x55692F) : HEX(0x76913F));+ }+ }+}++static void draw_booms(void) {+ for (int i = 0; i < MAX_BOOM; i++) {+ const Boom *b = &g.boom[i];+ if (!b->t) continue;+ float t = b->t / 24.0f, r = 160 * FIELD_S * (0.35f + 0.65f * fminf(1, t * 3));+ if (t < 0.7f) draw_disc(SX(b->x), SY(b->y), r, gfx_mix(HEX(0xFF8A1E), HEX(0xFFE08A), (int)(32 * t)));+ if (t < 0.45f) draw_disc(SX(b->x), SY(b->y), r * 0.55f, HEX(0xFFF4C2));+ }+}++static void draw_aim(void) {+ if (!game_can_fire()) return;+ float sx, sy, mx, my;+ game_gun(&sx, &sy, &mx, &my);+ if (g.grenades) {+ float xs[40], ys[40];+ int n = game_arc(xs, ys, 40);+ for (int i = 0; i < n; i++) {+ draw_disc(SX(xs[i]), SY(ys[i]), 1.6f, 0);+ draw_disc(SX(xs[i]), SY(ys[i]), 1.0f, 0xFFFF);+ }+ return;+ }+ float dx = cosf(g.aim), dy = sinf(g.aim);+ float ex = mx + dx * 1400, ey = my + dy * 1400;+ draw_line(SX(mx), SY(my), SX(ex), SY(ey), HEX(0xFF5A4E));+ float rx = SX(mx + dx * 150), ry = SY(my + dy * 150);+ for (int a = 0; a < 24; a++) {+ float t = a * 6.2831853f / 24;+ plot((int)floorf(rx + 4.5f * cosf(t)), (int)floorf(ry + 4.5f * sinf(t)), 0);+ }+ draw_line(rx - 6, ry, rx - 2, ry, 0), draw_line(rx + 2, ry, rx + 6, ry, 0);+ draw_line(rx, ry - 6, rx, ry - 2, 0), draw_line(rx, ry + 2, rx, ry + 6, 0);+}++void draw_world(bool aim) {+ clip_x0 = FIELD_X0, clip_x1 = FIELD_X0 + FIELD_W;+ draw_pic(PIC_BG, FIELD_X0, 0);+ draw_pins();+ for (int i = 0; i < g.nobj; i++) draw_obj(i);+ draw_switches();+ draw_pin_dots();+ for (int i = 0; i < g.nchar; i++)+ if (i != g.hero) draw_char(&g.ch[i]);+ if (g.hero >= 0) draw_char(&g.ch[g.hero]);+ draw_shots();+ draw_booms();+ if (aim) draw_aim();+ clip_x0 = 0, clip_x1 = SCREEN_W;+}++/* ------------------------------------------------------------------ the panels */+static void bullet_icon(int x, int y, int kind) {+ /* kind 0 black, 1 gold, 2 spent */+ color_t c = kind == 1 ? HEX(0xF2B807) : kind == 0 ? HEX(0x101418) : HEX(0x3A4756);+ color_t tip = kind == 1 ? HEX(0xFFE07A) : kind == 0 ? HEX(0x505A64) : HEX(0x3A4756);+ gfx_rrect(x, y + 4, 7, 12, 2, c, 256);+ gfx_rrect(x, y, 7, 8, 3, tip, 256);+ if (kind == 2) gfx_rrect(x + 1, y + 1, 5, 14, 2, COL_PANEL, 256);+}++void draw_hud(const char *title, const char *keys1, const char *keys2) {+ /* keys1, keys2: the left and right keys' name and what they do */+ gfx_fill(0, gfx_y0, FIELD_X0, gfx_y1 - gfx_y0, COL_PANEL);+ gfx_fill(FIELD_X0 + FIELD_W, gfx_y0, SCREEN_W - FIELD_X0 - FIELD_W, gfx_y1 - gfx_y0, COL_PANEL);+ gfx_text_center(&np_font_small, FIELD_X0 / 2, 16, title, COL_TEXT, 256);+ /* the bullets: the last one gold */+ int n = g.ammo, per = 4;+ for (int i = 0; i < n; i++) {+ int row = i / per, col = i % per;+ int x = 6 + col * 14, y = 28 + row * 22;+ int kind = i < g.shots ? 2 : (i == n - 1 ? 1 : 0);+ if (g.grenades) {+ if (y + 14 > gfx_y0 && y < gfx_y1) {+ if (kind == 2) gfx_circle(x + 4, y + 8, 5, HEX(0x3A4756), 256);+ else draw_pic(PIC_GRENADE, x - 2, y + 2);+ }+ } else if (y + 16 > gfx_y0 && y < gfx_y1) {+ bullet_icon(x, y, kind);+ }+ }+ /* the stars this would give */+ int st = game_stars(), sy = 30 + ((n + per - 1) / per) * 22;+ for (int i = 0; i < 3; i++) {+ int x = 6 + i * 19;+ if (sy + 20 > gfx_y0 && sy < gfx_y1) {+ if (i < st) draw_pic(PIC_STAR, x, sy);+ else gfx_circle(x + 9, sy + 9, 6, COL_PANEL_LIGHT, 256);+ }+ }+ if (g.grenades) {+ int x = FIELD_X0 + FIELD_W + 8, y = 40, h = 100;+ gfx_text(&np_font_small, x, y - 6, "Power", COL_MUTED, 256);+ gfx_rrect(x, y, 12, h, 3, COL_PANEL_LIGHT, 256);+ int f = (int)(g.power * (h - 4));+ gfx_rrect(x + 2, y + h - 2 - f, 8, f, 2, COL_FOCUS, 256);+ }+ /* the keys: each one's name, then what it does */+ int rx = FIELD_X0 + FIELD_W + 6;+ static const char *const lines[8] = {"Up Down", "aim", "", "", "OK", "fire", "Back", "pause"};+ for (int i = 0; i < 8; i++) {+ const char *t = i == 2 ? keys1 : i == 3 ? keys2 : lines[i];+ int y = 128 + i * 13 + (i / 2) * 3;+ if (y - 10 < gfx_y1 && y + 3 > gfx_y0) gfx_text(&np_font_small, rx, y, t, i & 1 ? COL_MUTED : COL_TEXT, 256);+ }+}diff --git a/games/mrbullet/src/draw.h b/games/mrbullet/src/draw.hnew file mode 100644index 00000000..df569082--- /dev/null+++ b/games/mrbullet/src/draw.h@@ -0,0 +1,32 @@+/* Mr Bullet: drawing (draw.c), a strip of 16 rows at a time with the launcher's gfx (its fonts and panels) and the+ * game's pictures. The play field (440 x 568 of the game's units) fills the screen's height in its middle. */+#ifndef MB_DRAW_H+#define MB_DRAW_H+#include <stdbool.h>+#include <stdint.h>+#include "gfx.h"++#define FIELD_X0 67+#define FIELD_W 186+#define FIELD_S (240.0f / 568.0f) /* pixels a unit */+#define SX(x) (160.0f + (x) * FIELD_S)+#define SY(y) (120.0f + (y) * FIELD_S)++#define COL_PANEL HEX(0x1B2633)+#define COL_PANEL_LIGHT HEX(0x2C3A4A)+#define COL_INK HEX(0x0E141B)+#define COL_TEXT 0xFFFF+#define COL_MUTED HEX(0x9AA8B6)+#define COL_FOCUS HEX(0xFFD23F)+#define COL_RED HEX(0xE8392F)++void draw_pic(int pic, int x, int y);+/* a picture with its pivot at screen (sx, sy), scaled and turned (sine and cosine), mirrored if flip */+void draw_pic_at(int pic, float sx, float sy, float scale, float sn, float cs, bool flip);+void draw_line(float x0, float y0, float x1, float y1, color_t c);+void draw_disc(float cx, float cy, float r, color_t c);+/* the level as it is now, in the field (and the aim when `aim`) */+void draw_world(bool aim);+/* the side panels: the bullets and the level */+void draw_hud(const char *title, const char *keys1, const char *keys2);+#endifdiff --git a/games/mrbullet/src/game.c b/games/mrbullet/src/game.cnew file mode 100644index 00000000..52490d47--- /dev/null+++ b/games/mrbullet/src/game.c@@ -0,0 +1,789 @@+/* A level being played (game.h): built from the level's bytes (tools/pack.py), stepped 60 times a second.+ *+ * As in the game (gamesrc spec): level parts of density 2.1 (heavy x3), restitution 0.35, friction 0.2; characters'+ * pieces 2.0, 0.35, 0.2; bullets circles of radius 2, density 0.1, restitution 1, no friction and no gravity, sent+ * by an impulse of 0.008 and kept 300 steps; grenades of density 0.8, restitution 0.4, friction 0.5, that go off+ * after 150 steps. Breakables (bo, br) break at the first bullet or a blow of 7.5 m/s; explosives (be) go off. */+#include <math.h>+#include <string.h>+#include "game.h"+#include "res.h"+#include "world.h"++Game g;+float mb_scratch[SCRATCH_FLOATS];+static uint8_t pending_blast[MAX_OBJ]; /* explosives set off, going off in a few steps */++#define PI 3.14159265f+#define T_LEVEL 1+#define T_CHAR 2+#define T_SHOT 3+#define T_SWITCH 4+#define CODE(t, i) ((t) << 8 | (i))+#define KIND(c) ((c) >> 8)+#define INDEX(c) ((c) & 255)++#define BULLET_R 2.0f+#define BULLET_SPEED (0.008f / (0.1f * PI * (BULLET_R / 60) * (BULLET_R / 60)) * 60) /* units/s */+#define GRENADE_R 5.0f+#define FUSE 150+#define BLAST_R 160.0f+#define KILL_SPEED 180.0f /* a blow that kills a standing character (units/s) */+#define BREAK_SPEED 450.0f /* a blow that breaks a breakable (7.5 m/s) */+#define RAGDOLL_BYTES 9000 /* about what a ragdoll takes of Box2D's memory */++static const WFix FIX_LEVEL = {2.1f, 0.2f, 0.35f, WC_LEVEL, 0};+static const WFix FIX_STATIC = {2.1f, 0.2f, 0.35f, WC_STATIC, 0};+static const WFix FIX_CHAR = {2.0f, 0.2f, 0.35f, WC_CHARACTER, 0};+static const WFix FIX_BULLET = {0.1f, 0.0f, 1.0f, WC_BULLET, 0};+static const WFix FIX_GRENADE = {0.8f, 0.5f, 0.4f, WC_GRENADE, 0};+static const WFix FIX_SENSOR = {0.0f, 0.2f, 0.0f, WC_SENSOR, 1};++/* ------------------------------------------------------------------ reading the level */+static const uint8_t *rp;+static int rd8(void) { return *rp++; }+static int rds8(void) { return (int8_t)*rp++; }+static int rd16(void) { int v = rp[0] | rp[1] << 8; rp += 2; return v; }+static float rdc(void) { int16_t v = (int16_t)(rp[0] | rp[1] << 8); rp += 2; return v * 0.125f; }+static uint32_t rd32(void) { uint32_t v = rp[0] | rp[1] << 8 | rp[2] << 16 | (uint32_t)rp[3] << 24; rp += 4; return v; }+static float rdf(void) { union { uint32_t u; float f; } x; x.u = rd32(); return x.f; }++static Obj *obj_by_idx(int idx) {+ for (int i = 0; i < g.nobj; i++)+ if (g.obj[i].idx == idx && g.obj[i].kind != 255) return &g.obj[i];+ return 0;+}++int game_mode_of(int level) {+ for (int m = MODE_COUNT - 1; m >= 0; m--)+ if (level >= res_mode_first(m)) return m;+ return 0;+}++/* ------------------------------------------------------------------ characters */+/* a piece: its centre (x right as the character faces, y up from the feet, in 80ths of its height) and size */+static const float PART[P_COUNT][4] = {+ {0, 43.7f, 28.8f, 31.4f}, /* torso */+ {0, 69.7f, 18.7f, 20.6f}, /* head */+ {17, 47.4f, 7.2f, 16}, /* front arm */+ {17, 31.4f, 7.2f, 16},+ {-17, 47.4f, 7.2f, 16}, /* back arm */+ {-17, 31.4f, 7.2f, 16},+ {9, 21, 7.2f, 14}, /* front leg */+ {9, 7, 7.2f, 14},+ {-9, 21, 7.2f, 14},+ {-9, 7, 7.2f, 14},+};+#define SHOULDER_Y 55.4f++void game_part_size(const Char *c, int part, float *w, float *h) {+ float k = c->h / 80;+ *w = PART[part][2] * k, *h = PART[part][3] * k;+}++/* the character's frame: feet, its right (as it faces) and up, from its standing body */+static void frame_of(const Char *c, float *fx, float *fy, float *rx, float *ry, float *ux, float *uy) {+ float s, cs;+ if (c->body >= 0 && world_alive(c->body)) world_xf(c->body, fx, fy, &s, &cs);+ else *fx = c->fx, *fy = c->fy, s = sinf(c->tilt), cs = cosf(c->tilt);+ float side = c->left ? -1.0f : 1.0f;+ *rx = cs * side, *ry = s * side;+ *ux = s, *uy = -cs;+}++/* a standing character's piece (and the shooter's arm along his aim) */+static void pose(const Char *c, int part, float *x, float *y, float *ang) {+ float fx, fy, rx, ry, ux, uy, k = c->h / 80;+ frame_of(c, &fx, &fy, &rx, &ry, &ux, &uy);+ float tilt = atan2f(ux, -uy); /* (the frame's turn) */+ if (c->role == R_HERO && (part == P_ARM_F || part == P_FORE_F) && &g.ch[g.hero] == c) {+ float shx = fx + rx * 17 * k + ux * SHOULDER_Y * k, shy = fy + ry * 17 * k + uy * SHOULDER_Y * k;+ float d = (part == P_ARM_F ? 8 : 24) * k;+ *x = shx + cosf(g.aim) * d, *y = shy + sinf(g.aim) * d;+ *ang = g.aim - PI / 2;+ return;+ }+ float lx = PART[part][0] * k, ly = PART[part][1] * k;+ *x = fx + rx * lx + ux * ly, *y = fy + ry * lx + uy * ly;+ *ang = tilt;+}++bool game_part(const Char *c, int part, float *x, float *y, float *s, float *cs) {+ if (!c->alive && c->part[part] >= 0 && world_alive(c->part[part])) {+ world_xf(c->part[part], x, y, s, cs);+ return true;+ }+ if (!c->alive && (c->body < 0 || !world_alive(c->body))) return false;+ float a;+ pose(c, part, x, y, &a);+ *s = sinf(a), *cs = cosf(a);+ return true;+}++static bool make_standing(Char *c, int ci) {+ float k = c->h / 80;+ c->body = (int16_t)world_body(WB_DYNAMIC, c->fx, c->fy, c->tilt, CODE(T_CHAR, ci), false, true, 1.0f);+ if (c->body < 0) return false;+ return world_box(c->body, 0, -c->h / 2, 15 * k, c->h / 2, &FIX_CHAR);+}++/* the ragdoll, from where the standing character is */+static void fall(Char *c, int ci) {+ float vx = 0, vy = 0, x[P_COUNT], y[P_COUNT], a[P_COUNT];+ if (c->body >= 0 && world_alive(c->body)) world_velocity(c->body, &vx, &vy);+ for (int p = 0; p < P_COUNT; p++) pose(c, p, &x[p], &y[p], &a[p]);+ float fx, fy, rx, ry, ux, uy, k = c->h / 80;+ frame_of(c, &fx, &fy, &rx, &ry, &ux, &uy);+ c->alive = 0;+ if (b2_heap_limit && b2_heap_now + RAGDOLL_BYTES > b2_heap_limit && c->body >= 0) {+ /* no room for its ragdoll: it falls stiff */+ world_set_fixed_rotation(c->body, false);+ world_wake(c->body);+ return;+ }+ if (c->body >= 0) world_destroy(c->body);+ c->body = -1;+ for (int p = 0; p < P_COUNT; p++) {+ int b = world_body(WB_DYNAMIC, x[p], y[p], a[p], CODE(T_CHAR, ci), false, false, 1.0f);+ c->part[p] = (int16_t)b;+ if (b < 0) continue;+ world_box(b, 0, 0, PART[p][2] * k / 2, PART[p][3] * k / 2, &FIX_CHAR);+ world_set_velocity(b, vx, vy);+ }+ /* the joints: neck and hips +-0.9425, knees 0 to 2.5133 and elbows 0 to 1.885 (by the way it faces), shoulders+ free */+ float side = c->left ? -1.0f : 1.0f;+#define AT(lx, ly) fx + rx * (lx) * k + ux * (ly) * k, fy + ry * (lx) * k + uy * (ly) * k+ world_revolute(c->part[P_TORSO], c->part[P_HEAD], AT(0, 59.4f), true, -0.9425f, 0.9425f, false, 0, 0);+ for (int s = 0; s < 2; s++) {+ int arm = s ? P_ARM_B : P_ARM_F, fore = arm + 1, thigh = s ? P_THIGH_B : P_THIGH_F, shin = thigh + 1;+ float ax = s ? -17.0f : 17.0f, lx = s ? -9.0f : 9.0f;+ if (s == 0 && &g.ch[g.hero] == c) {+ /* the aiming arm: its shoulder and elbow along the aim */+ float shx, shy, mx, my;+ game_gun(&shx, ­, &mx, &my);+ world_revolute(c->part[P_TORSO], c->part[arm], shx, shy, false, 0, 0, false, 0, 0);+ world_revolute(c->part[arm], c->part[fore], shx + cosf(g.aim) * 16 * k, shy + sinf(g.aim) * 16 * k, true,+ side > 0 ? 0 : -1.885f, side > 0 ? 1.885f : 0, false, 0, 0);+ } else {+ world_revolute(c->part[P_TORSO], c->part[arm], AT(ax, SHOULDER_Y), false, 0, 0, false, 0, 0);+ world_revolute(c->part[arm], c->part[fore], AT(ax, 39.4f), true, side > 0 ? 0 : -1.885f,+ side > 0 ? 1.885f : 0, false, 0, 0);+ }+ world_revolute(c->part[P_TORSO], c->part[thigh], AT(lx, 28), true, -0.9425f, 0.9425f, false, 0, 0);+ world_revolute(c->part[thigh], c->part[shin], AT(lx, 14), true, side > 0 ? -2.5133f : 0,+ side > 0 ? 0 : 2.5133f, false, 0, 0);+ }+#undef AT+}++static void kill(int ci, float ix, float iy, float px, float py) {+ Char *c = &g.ch[ci];+ if (!c->alive) return;+ if (c->role == R_HERO && !c->hittable) return;+ fall(c, ci);+ /* the blow goes to the piece nearest it */+ int best = -1;+ float bd = 1e9f;+ for (int p = 0; p < P_COUNT; p++) {+ if (c->part[p] < 0) continue;+ float x, y, s, cs;+ world_xf(c->part[p], &x, &y, &s, &cs);+ float d = (x - px) * (x - px) + (y - py) * (y - py);+ if (d < bd) bd = d, best = p;+ }+ if (best >= 0 && (ix != 0 || iy != 0)) world_impulse(c->part[best], ix, iy, px, py);+ if (c->role == R_ENEMY) g.enemies_left--;+ else if (c->role == R_HOSTAGE) { if (!g.lose) g.lose = LOSE_HOSTAGE; }+ else if (!g.lose) g.lose = LOSE_SELF;+}++/* ------------------------------------------------------------------ building */+static bool build_poly(Obj *o, int oi, const uint8_t *parts, int nparts) {+ bool fixed = (o->flags & F_F) != 0;+ o->body = (int16_t)world_body(fixed ? WB_STATIC : WB_DYNAMIC, o->cx, o->cy, 0, CODE(T_LEVEL, oi), false, false,+ 1.0f);+ if (o->body < 0) return false;+ WFix f = fixed ? FIX_STATIC : FIX_LEVEL;+ if (o->flags & F_HV) f.density *= 3;+ const uint8_t *save = rp;+ if (fixed) {+ /* a static one: its outline and holes as loops of edges (its convex parts' boxes overlap too much: thousands+ of contacts with what lies on it) */+ float *xy = mb_scratch;+ rp = o->outline;+ int n = o->nout;+ for (int h = 0; h <= o->nholes; h++) {+ if (h) n = rd8();+ int m = n < SCRATCH_FLOATS / 2 ? n : SCRATCH_FLOATS / 2;+ for (int i = 0; i < n; i++) {+ float x = rdc(), y = rdc();+ if (i < m) xy[2 * i] = x, xy[2 * i + 1] = y;+ }+ world_chain(o->body, xy, m, &f);+ }+ } else {+ rp = parts;+ float xy[32];+ for (int k = 0; k < nparts; k++) {+ int n = rd8();+ for (int i = 0; i < n; i++) xy[2 * i] = rdc(), xy[2 * i + 1] = rdc();+ world_poly(o->body, xy, n, &f);+ }+ }+ rp = save;+ return true;+}++static bool build_ball(Obj *o, int oi) {+ bool fixed = (o->flags & F_F) != 0;+ o->body = (int16_t)world_body(fixed ? WB_STATIC : WB_DYNAMIC, o->cx + o->bx, o->cy + o->by, 0, CODE(T_LEVEL, oi),+ false, false, 1.0f);+ if (o->body < 0) return false;+ WFix f = fixed ? FIX_STATIC : FIX_LEVEL;+ if (o->flags & F_HV) f.density *= 3;+ return world_circle(o->body, 0, 0, o->r, &f);+}++static const uint8_t *obj_parts[MAX_OBJ];+static uint8_t obj_nparts[MAX_OBJ];++static bool build_obj(int oi) {+ Obj *o = &g.obj[oi];+ if (o->kind == K_POLY) return build_poly(o, oi, obj_parts[oi], obj_nparts[oi]);+ if (o->kind == K_SEG && (o->flags & F_B)) return build_ball(o, oi);+ return true;+}++bool game_load(int level, uint8_t *buf) {+ game_end();+ memset(&g, 0, sizeof g);+ memset(pending_blast, 0, sizeof pending_blast);+ g.level = level;+ g.mode = game_mode_of(level);+ g.hero = -1;+ int size = res_level(level, buf);+ if (!size) return false;+ if (!world_begin()) return false;+ rp = buf;+ g.tr = rd8();+ int nobj = rd8(), npin = rd8(), nsw = rd8();+ g.ammo = g.tr + 3;+ for (int i = 0; i < MAX_SHOT; i++) g.shot[i].body = -1;+ for (int i = 0; i < nobj; i++) {+ int kind = rd8(), idx = rd8();+ uint32_t flags = rd32();+ float cx = rdc(), cy = rdc();+ if (kind == K_SEG && (flags & (F_SHOOTER | F_VI | F_HS)) && !(flags & F_B)) {+ float x0 = rdc() + cx, y0 = rdc() + cy, x1 = rdc() + cx, y1 = rdc() + cy;+ if (g.nchar >= MAX_CHAR) continue;+ Char *c = &g.ch[g.nchar];+ c->role = (flags & F_SHOOTER) ? R_HERO : (flags & F_HS) ? R_HOSTAGE : R_ENEMY;+ c->alive = 1;+ c->left = (flags & F_RH) != 0;+ c->hittable = (flags & (F_HKI | F_HGKI)) != 0;+ c->fx = x0, c->fy = y0;+ c->h = sqrtf((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0));+ if (c->h < 20) c->h = 80;+ c->tilt = atan2f(x1 - x0, -(y1 - y0));+ c->body = -1;+ for (int p = 0; p < P_COUNT; p++) c->part[p] = -1;+ if (c->role == R_HERO && g.hero < 0) {+ g.hero = g.nchar;+ g.grenades = (flags & (F_GKI | F_HGKI)) != 0;+ } else if (c->role == R_HERO) {+ c->role = R_ENEMY; /* (never: one shooter a level) */+ }+ if (c->role == R_ENEMY) g.enemies_left++;+ g.nchar++;+ continue;+ }+ if (g.nobj >= MAX_OBJ) return false;+ Obj *o = &g.obj[g.nobj];+ o->kind = (uint8_t)kind, o->idx = (uint8_t)idx, o->flags = flags, o->cx = cx, o->cy = cy, o->body = -1;+ if (kind == K_SEG) {+ float x0 = rdc(), y0 = rdc(), x1 = rdc(), y1 = rdc();+ o->bx = x0, o->by = y0;+ o->r = sqrtf((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0));+ if (!(flags & F_B)) o->kind = K_POINT; /* (a stray segment: nothing) */+ } else if (kind == K_POINT) {+ rp += 4;+ } else {+ int np = rd8();+ obj_parts[g.nobj] = rp;+ obj_nparts[g.nobj] = (uint8_t)np;+ for (int k = 0; k < np; k++) rp += 4 * rd8();+ o->nout = (uint16_t)rd16();+ o->outline = rp;+ rp += 4 * o->nout;+ o->nholes = (uint8_t)rd8();+ for (int k = 0; k < o->nholes; k++) rp += 4 * rd8();+ }+ g.nobj++;+ }+ /* pins */+ const uint8_t *pins = rp;+ for (int i = 0; i < npin; i++) {+ rp += 8;+ int nv = rd8();+ rp += 4 * nv + 8 + 4 + 8;+ }+ /* switches: the objects they hold */+ for (int i = 0; i < nsw && g.nsw < MAX_SW; i++) {+ Switch *s = &g.sw[g.nsw];+ s->id = (uint8_t)rd8(), s->colour = (uint8_t)rd8();+ int n = rd8();+ for (int k = 0; k < n; k++) {+ int l = rd8();+ if (s->nlinks < sizeof s->link) s->link[s->nlinks++] = (uint8_t)l;+ }+ float cx = rdc(), cy = rdc();+ s->x0 = rdc() + cx, s->y0 = rdc() + cy, s->x1 = rdc() + cx, s->y1 = rdc() + cy;+ for (int k = 0; k < s->nlinks; k++) {+ Obj *o = obj_by_idx(s->link[k]);+ if (o && !o->sw) o->sw = (uint8_t)(g.nsw + 1);+ }+ g.nsw++;+ }+ if ((uintptr_t)(rp - buf) != (uintptr_t)size) return false;+ /* bodies */+ for (int i = 0; i < g.nobj; i++) {+ Obj *o = &g.obj[i];+ if (o->sw && (o->flags & F_SI)) {+ o->gone = 1; /* it comes when its switch is hit */+ continue;+ }+ if (!build_obj(i)) return false;+ }+ for (int i = 0; i < g.nchar; i++)+ if (!make_standing(&g.ch[i], i)) return false;+ for (int i = 0; i < g.nsw; i++) {+ Switch *s = &g.sw[i];+ float mx = (s->x0 + s->x1) / 2, my = (s->y0 + s->y1) / 2, dx = s->x1 - s->x0, dy = s->y1 - s->y0;+ float len = sqrtf(dx * dx + dy * dy);+ s->body = (int16_t)world_body(WB_STATIC, mx, my, atan2f(dy, dx), CODE(T_SWITCH, i), false, false, 1);+ if (s->body < 0) return false;+ world_box(s->body, 0, 0, len / 2 + 2, 6, &FIX_SENSOR);+ }+ rp = pins;+ for (int i = 0; i < npin; i++) {+ int t = rd8(), a = rds8(), b = rds8(), motor = rd8();+ float cx = rdc(), cy = rdc();+ int nv = rd8();+ float vx[4] = {0}, vy[4] = {0};+ for (int k = 0; k < nv; k++) {+ float x = rdc() + cx, y = rdc() + cy;+ if (k < 4) vx[k] = x, vy[k] = y;+ }+ float lax = rdc() + cx, lay = rdc() + cy, lbx = rdc() + cx, lby = rdc() + cy;+ float rl = rdc(), as = rdc(), ms = rdf(), mt = rdf();+ if (g.npin >= MAX_PIN) continue;+ Obj *oa = a >= 0 ? obj_by_idx(a) : 0, *ob = b >= 0 ? obj_by_idx(b) : 0;+ int ba = oa && !oa->gone ? oa->body : -1, bb = ob && !ob->gone ? ob->body : -1;+ Pin *p = &g.pin[g.npin];+ p->t = (uint8_t)t, p->motor = (uint8_t)motor, p->a = (int8_t)a, p->b = (int8_t)b, p->joint = -1;+ if (t == 2) {+ if (ba < 0 && bb < 0) continue;+ p->joint = (int16_t)world_revolute(ba, bb, vx[0], vy[0], false, 0, 0, motor != 0, ms, mt);+ p->x0 = vx[0], p->y0 = vy[0];+ } else if (t == 3) {+ if (ba < 0 && bb < 0) continue;+ float len = rl > 0 ? rl : sqrtf((lbx - lax) * (lbx - lax) + (lby - lay) * (lby - lay));+ p->joint = (int16_t)world_rope(ba, bb, lax, lay, lbx, lby, len);+ p->x0 = lax, p->y0 = lay, p->x1 = lbx, p->y1 = lby;+ } else if (t == 5) {+ Obj *o = oa ? oa : ob;+ if (!o || o->body < 0 || nv < 2) continue;+ float dx = vx[1] - vx[0], dy = vy[1] - vy[0], len = sqrtf(dx * dx + dy * dy);+ if (len < 1) continue;+ o->kin = 1;+ o->kdx = dx / len, o->kdy = dy / len, o->klen = len, o->kspeed = as > 0 ? as : 60, o->kpos = 0;+ world_set_type(o->body, WB_KINEMATIC);+ p->x0 = vx[0], p->y0 = vy[0], p->x1 = vx[1], p->y1 = vy[1];+ } else {+ continue;+ }+ g.npin++;+ }+ /* the shooter aims straight ahead */+ if (g.hero >= 0) g.aim = g.ch[g.hero].left ? PI : 0;+ g.power = 0.55f;+ g.state = ST_PLAY;+ return g.hero >= 0;+}++void game_end(void) { world_end(); }++/* ------------------------------------------------------------------ the shooter */+void game_gun(float *sx, float *sy, float *mx, float *my) {+ const Char *c = &g.ch[g.hero];+ float fx, fy, rx, ry, ux, uy, k = c->h / 80;+ frame_of(c, &fx, &fy, &rx, &ry, &ux, &uy);+ *sx = fx + rx * 17 * k + ux * SHOULDER_Y * k;+ *sy = fy + ry * 17 * k + uy * SHOULDER_Y * k;+ float reach = (32 + 14) * k; /* the arm, then the gun */+ *mx = *sx + cosf(g.aim) * reach, *my = *sy + sinf(g.aim) * reach;+}++static float wrap(float a) {+ while (a > PI) a -= 2 * PI;+ while (a < -PI) a += 2 * PI;+ return a;+}++void game_aim_by(float d) {+ if (g.hero < 0) return;+ Char *c = &g.ch[g.hero];+ /* up turns the gun up on the side he faces; past straight up he turns round */+ g.aim = wrap(g.aim + (c->left ? d : -d));+ bool left = cosf(g.aim) < 0;+ if (fabsf(cosf(g.aim)) > 0.02f && left != (c->left != 0)) c->left = left;+}++void game_turn(bool left) {+ if (g.hero < 0) return;+ Char *c = &g.ch[g.hero];+ if ((c->left != 0) == left) return;+ c->left = left;+ g.aim = wrap(PI - g.aim);+}++bool game_can_fire(void) {+ return g.state == ST_PLAY && !g.end_wait && g.hero >= 0 && g.ch[g.hero].alive && g.shots < g.ammo;+}++static int free_shot(void) {+ for (int i = 0; i < MAX_SHOT; i++)+ if (g.shot[i].body < 0) return i;+ return -1;+}++void game_fire(void) {+ if (!game_can_fire()) return;+ int si = free_shot();+ if (si < 0) return;+ float sx, sy, mx, my;+ game_gun(&sx, &sy, &mx, &my);+ Shot *s = &g.shot[si];+ memset(s, 0, sizeof *s);+ float dx = cosf(g.aim), dy = sinf(g.aim);+ if (g.grenades) {+ float k = g.ch[g.hero].h / 80, hx = sx + dx * 32 * k, hy = sy + dy * 32 * k;+ s->body = (int16_t)world_body(WB_DYNAMIC, hx, hy, 0, CODE(T_SHOT, si), false, false, 1.0f);+ if (s->body < 0) return;+ world_circle(s->body, 0, 0, GRENADE_R, &FIX_GRENADE);+ float v = (3.0f + 13.0f * g.power) * 60;+ world_set_velocity(s->body, dx * v, dy * v);+ world_set_angular(s->body, g.ch[g.hero].left ? -6 : 6);+ s->kind = 1, s->life = FUSE;+ s->px = hx, s->py = hy;+ } else {+ s->body = (int16_t)world_body(WB_DYNAMIC, mx, my, 0, CODE(T_SHOT, si), true, false, 0.0f);+ if (s->body < 0) return;+ world_circle(s->body, 0, 0, BULLET_R, &FIX_BULLET);+ world_set_velocity(s->body, dx * BULLET_SPEED, dy * BULLET_SPEED);+ s->kind = 0, s->life = 300;+ s->px = mx, s->py = my;+ }+ s->tx[0] = s->px, s->ty[0] = s->py, s->tn = 1;+ g.shots++;+}++int game_arc(float *xs, float *ys, int n) {+ if (g.hero < 0 || !g.grenades) return 0;+ float sx, sy, mx, my, k = g.ch[g.hero].h / 80;+ game_gun(&sx, &sy, &mx, &my);+ float x = sx + cosf(g.aim) * 32 * k, y = sy + sinf(g.aim) * 32 * k;+ float v = (3.0f + 13.0f * g.power) * 60, vx = cosf(g.aim) * v, vy = sinf(g.aim) * v;+ int m = 0;+ for (int step = 1; step <= 120 && m < n; step++) {+ vy += 9.81f * 60 * W_STEP;+ vx /= 1 + W_STEP * 0.5f, vy /= 1 + W_STEP * 0.5f;+ float nx = x + vx * W_STEP, ny = y + vy * W_STEP, hx, hy, ax, ay;+ if (world_ray(x, y, nx, ny, 1u << WC_STATIC | 1u << WC_LEVEL, &hx, &hy, &ax, &ay) >= 0) {+ xs[m] = hx, ys[m] = hy, m++;+ break;+ }+ x = nx, y = ny;+ if (step % 4 == 0) xs[m] = x, ys[m] = y, m++;+ }+ return m;+}++/* ------------------------------------------------------------------ what happens */+typedef struct {+ int16_t a, b, ba, bb;+ float speed, x, y;+} Event;+#define MAX_EVENTS 48+static Event events[MAX_EVENTS];+static int nevents;++void game_contact(int a, int b, int ba, int bb, float speed, float x, float y) {+ if (nevents < MAX_EVENTS) events[nevents++] = (Event){(int16_t)a, (int16_t)b, (int16_t)ba, (int16_t)bb, speed, x, y};+}++static bool is_hero_code(int code) { return KIND(code) == T_CHAR && INDEX(code) == g.hero; }++bool game_contact_enabled(int a, int b) {+ /* his grenades don't touch the shooter */+ if (KIND(a) == T_SHOT && is_hero_code(b)) return false;+ if (KIND(b) == T_SHOT && is_hero_code(a)) return false;+ return true;+}++static void remove_obj(int oi) {+ Obj *o = &g.obj[oi];+ if (o->gone) return;+ o->gone = 1;+ if (o->body >= 0) world_destroy(o->body);+ o->body = -1;+ /* the pins on it go with it */+ for (int i = 0; i < g.npin; i++)+ if (g.pin[i].joint >= 0 && !world_joint_alive(g.pin[i].joint)) g.pin[i].joint = -1;+}++static void boom_at(float x, float y) {+ int k = 0;+ for (int i = 1; i < MAX_BOOM; i++)+ if (g.boom[i].t > g.boom[k].t) k = i;+ for (int i = 0; i < MAX_BOOM; i++)+ if (g.boom[i].t == 0) { k = i; break; }+ g.boom[k].x = x, g.boom[k].y = y, g.boom[k].t = 1;+}+++static void blast(float x, float y) {+ boom_at(x, y);+ for (int i = 0; i < g.nchar; i++) {+ Char *c = &g.ch[i];+ float cx, cy, s, cs;+ if (c->alive) {+ if (c->role == R_HERO && !c->hittable) continue;+ game_part(c, P_TORSO, &cx, &cy, &s, &cs);+ float dx = cx - x, dy = cy - y, d = sqrtf(dx * dx + dy * dy);+ if (d < BLAST_R) {+ float f = 1.2f * (1 - d / BLAST_R) / (d > 1 ? d : 1);+ kill(i, dx * f, dy * f, cx, cy);+ }+ continue;+ }+ for (int p = 0; p < P_COUNT; p++) {+ if (c->part[p] < 0 || !world_alive(c->part[p])) continue;+ world_xf(c->part[p], &cx, &cy, &s, &cs);+ float dx = cx - x, dy = cy - y, d = sqrtf(dx * dx + dy * dy);+ if (d < BLAST_R) {+ float f = 0.4f * (1 - d / BLAST_R) / (d > 1 ? d : 1);+ world_impulse(c->part[p], dx * f, dy * f, cx, cy);+ }+ }+ }+ for (int i = 0; i < g.nobj; i++) {+ Obj *o = &g.obj[i];+ if (o->gone || o->body < 0) continue;+ float cx, cy, s, cs;+ world_xf(o->body, &cx, &cy, &s, &cs);+ if (o->kind == K_SEG) cx += 0, cy += 0;+ /* the nearest point of its box: its centre is enough for these sizes */+ float dx = cx - x, dy = cy - y;+ if (o->kind == K_POLY && o->nout) {+ /* (polygons' origins can be far from them: use their outline's middle) */+ const uint8_t *q = o->outline;+ float mx = 0, my = 0;+ for (int k = 0; k < o->nout; k++) {+ mx += (int16_t)(q[4 * k] | q[4 * k + 1] << 8) * 0.125f;+ my += (int16_t)(q[4 * k + 2] | q[4 * k + 3] << 8) * 0.125f;+ }+ mx /= o->nout, my /= o->nout;+ dx = cx + cs * mx - s * my - x, dy = cy + s * mx + cs * my - y;+ }+ float d = sqrtf(dx * dx + dy * dy);+ if (d >= BLAST_R) continue;+ if (o->flags & F_BE) { if (!pending_blast[i]) pending_blast[i] = 6; continue; }+ if (o->flags & (F_BO | F_BR)) { remove_obj(i); continue; }+ if (!world_dynamic(o->body)) continue;+ float m = world_mass(o->body), f = 3.0f * (1 - d / BLAST_R) / (d > 1 ? d : 1);+ float cap = m * 12.0f / (f * (d > 1 ? d : 1) + 1e-6f);+ if (cap < 1) f *= cap;+ world_impulse(o->body, dx * f, dy * f, cx + dx * 0.5f, cy + dy * 0.5f);+ }+}++static void explode_obj(int oi) {+ Obj *o = &g.obj[oi];+ if (o->gone) return;+ float x = o->cx, y = o->cy, s, cs;+ if (o->body >= 0) world_xf(o->body, &x, &y, &s, &cs);+ if (o->kind == K_POLY && o->nout && o->body >= 0) {+ const uint8_t *q = o->outline;+ float mx = 0, my = 0;+ for (int k = 0; k < o->nout; k++) {+ mx += (int16_t)(q[4 * k] | q[4 * k + 1] << 8) * 0.125f;+ my += (int16_t)(q[4 * k + 2] | q[4 * k + 3] << 8) * 0.125f;+ }+ mx /= o->nout, my /= o->nout;+ float bx = x, by = y;+ x = bx + cs * mx - s * my, y = by + s * mx + cs * my;+ }+ remove_obj(oi);+ blast(x, y);+}++static void switch_on(int si) {+ Switch *s = &g.sw[si];+ if (s->on) return;+ s->on = 1;+ for (int k = 0; k < s->nlinks; k++)+ for (int i = 0; i < g.nobj; i++) {+ Obj *o = &g.obj[i];+ if (o->idx != s->link[k] || o->sw != si + 1) continue;+ if (o->flags & F_SI) {+ if (o->gone && o->body < 0) {+ o->gone = 0;+ build_obj(i);+ }+ } else {+ remove_obj(i);+ }+ }+}++static void hit_level(int oi, bool by_bullet, float speed) {+ Obj *o = &g.obj[oi];+ if (o->gone) return;+ if (o->flags & F_BE) {+ if ((by_bullet || speed > BREAK_SPEED) && !pending_blast[oi]) pending_blast[oi] = 1;+ } else if (o->flags & (F_BO | F_BR)) {+ if (by_bullet || speed > BREAK_SPEED) remove_obj(oi);+ }+}++static void handle(const Event *e) {+ int a = e->a, b = e->b;+ for (int pass = 0; pass < 2; pass++, a = e->b, b = e->a) {+ int ka = KIND(a), kb = KIND(b), ia = INDEX(a), ib = INDEX(b);+ if (ka == T_SWITCH && (kb == T_SHOT || kb == T_CHAR || (kb == T_LEVEL && g.obj[ib].body >= 0 &&+ world_dynamic(g.obj[ib].body)))) {+ switch_on(ia);+ continue;+ }+ if (ka == T_SHOT && g.shot[ia].body >= 0) {+ Shot *s = &g.shot[ia];+ if (s->kind == 0 && kb == T_LEVEL) hit_level(ib, true, e->speed);+ continue;+ }+ if (ka == T_CHAR && g.ch[ia].alive && kb != T_SHOT && kb != T_SWITCH && e->speed > KILL_SPEED) {+ bool other_still = kb == T_LEVEL && (g.obj[ib].body < 0 || !world_dynamic(g.obj[ib].body));+ (void)other_still;+ kill(ia, 0, 0, e->x, e->y);+ continue;+ }+ if (ka == T_LEVEL && kb != T_SHOT && kb != T_SWITCH) hit_level(ia, false, e->speed);+ }+}++static bool seg_cross(float ax, float ay, float bx, float by, float cx, float cy, float dx, float dy) {+ float r1 = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax), r2 = (bx - ax) * (dy - ay) - (by - ay) * (dx - ax);+ float r3 = (dx - cx) * (ay - cy) - (dy - cy) * (ax - cx), r4 = (dx - cx) * (by - cy) - (dy - cy) * (bx - cx);+ return r1 * r2 <= 0 && r3 * r4 <= 0 && (r1 != 0 || r2 != 0);+}++void game_step(void) {+ if (g.state != ST_PLAY) return;+ g.frame++;+ /* tracks */+ for (int i = 0; i < g.nobj; i++) {+ Obj *o = &g.obj[i];+ if (!o->kin || o->gone || o->body < 0) continue;+ o->kpos += o->kspeed * W_STEP;+ if (o->kpos > o->klen) o->kpos = o->klen, o->kspeed = -o->kspeed;+ if (o->kpos < 0) o->kpos = 0, o->kspeed = -o->kspeed;+ world_set_velocity(o->body, o->kdx * o->kspeed, o->kdy * o->kspeed);+ }+ nevents = 0;+ world_step();+ for (int i = 0; i < nevents; i++) handle(&events[i]);+ for (int i = 0; i < g.nobj; i++)+ if (pending_blast[i] && --pending_blast[i] == 0) explode_obj(i);+ /* shots */+ bool flying = false;+ for (int i = 0; i < MAX_SHOT; i++) {+ Shot *s = &g.shot[i];+ if (s->body < 0) continue;+ float x, y, sn, cs;+ world_xf(s->body, &x, &y, &sn, &cs);+ if (s->kind == 0) {+ /* a rope it crosses is cut */+ for (int p = 0; p < g.npin; p++) {+ Pin *pn = &g.pin[p];+ if (pn->t != 3 || pn->joint < 0 || !world_joint_alive(pn->joint)) continue;+ float ax, ay, bx, by;+ world_joint_anchors(pn->joint, &ax, &ay, &bx, &by);+ if (seg_cross(s->px, s->py, x, y, ax, ay, bx, by)) {+ world_destroy_joint(pn->joint);+ pn->joint = -1;+ }+ }+ /* characters along its path: shot through */+ int hit[8];+ float hx[8], hy[8];+ int nh = world_ray_all(s->px, s->py, x, y, 1u << WC_CHARACTER, hit, hx, hy, 8);+ float vx, vy;+ world_velocity(s->body, &vx, &vy);+ float v = sqrtf(vx * vx + vy * vy) + 1e-3f;+ for (int h = 0; h < nh; h++) {+ int code = world_obj(hit[h]);+ if (KIND(code) != T_CHAR) continue;+ Char *c = &g.ch[INDEX(code)];+ if (INDEX(code) == g.hero && !c->hittable) continue;+ if (c->alive) kill(INDEX(code), vx / v * 0.5f, vy / v * 0.5f, hx[h], hy[h]);+ else world_impulse(hit[h], vx / v * 0.15f, vy / v * 0.15f, hx[h], hy[h]);+ }+ for (int k = TRAIL - 1; k > 0; k--) s->tx[k] = s->tx[k - 1], s->ty[k] = s->ty[k - 1];+ s->tx[0] = x, s->ty[0] = y;+ if (s->tn < TRAIL) s->tn++;+ }+ s->px = x, s->py = y;+ if (--s->life <= 0 || fabsf(x) > 2500 || fabsf(y) > 2500) {+ world_destroy(s->body);+ s->body = -1;+ if (s->kind == 1) blast(x, y);+ continue;+ }+ flying = true;+ }+ /* characters that fell out of the world */+ for (int i = 0; i < g.nchar; i++) {+ Char *c = &g.ch[i];+ if (c->alive && c->body >= 0) {+ float x, y, s, cs;+ world_xf(c->body, &x, &y, &s, &cs);+ if (y > 1500) kill(i, 0, 0, x, y);+ }+ }+ for (int i = 0; i < MAX_BOOM; i++)+ if (g.boom[i].t && ++g.boom[i].t > 24) g.boom[i].t = 0;+ /* the end */+ if (g.end_wait) {+ if (--g.end_wait == 0) g.state = (g.lose || g.enemies_left > 0) ? ST_LOST : ST_WON;+ return;+ }+ if (g.lose) {+ g.end_wait = 50;+ } else if (g.enemies_left <= 0) {+ g.end_wait = 60;+ } else if (g.shots >= g.ammo && !flying) {+ if (++g.quiet > 150) {+ g.lose = LOSE_AMMO;+ g.end_wait = 1;+ }+ } else {+ g.quiet = 0;+ }+}++int game_stars(void) {+ int over = g.shots - g.tr;+ if (over <= 0) return 3;+ return over >= 3 ? 0 : 3 - over;+}diff --git a/games/mrbullet/src/game.h b/games/mrbullet/src/game.hnew file mode 100644index 00000000..beece0ae--- /dev/null+++ b/games/mrbullet/src/game.h@@ -0,0 +1,124 @@+/* Mr Bullet: a level being played (game.c): its objects, characters, shots and rules.+ *+ * Coordinates are the game's units (60 a metre), x right and y down, (0, 0) at the middle of the play field;+ * angles turn clockwise on the screen, 0 pointing right. */+#ifndef MB_GAME_H+#define MB_GAME_H+#include <stdbool.h>+#include <stdint.h>+#include "data.h"++#define MAX_OBJ 96+#define MAX_CHAR 24+#define MAX_PIN 48+#define MAX_SW 8+#define MAX_SHOT 16+#define MAX_BOOM 8+#define TRAIL 10++enum { MODE_CLASSIC, MODE_GRENADES, MODE_HOSTAGES, MODE_FRIENDLY };+enum { K_POLY, K_SEG, K_POINT };+enum { R_HERO, R_ENEMY, R_HOSTAGE };+enum { ST_PLAY, ST_WON, ST_LOST };+enum { LOSE_NONE, LOSE_AMMO, LOSE_HOSTAGE, LOSE_SELF };++/* the level's flags, in the order of tools/pack.py */+enum {+ F_KI = 1 << 0, F_GKI = 1 << 1, F_HKI = 1 << 2, F_HGKI = 1 << 3, F_VI = 1 << 4, F_HS = 1 << 5, F_RH = 1 << 6,+ F_SI = 1 << 7, F_B = 1 << 8, F_F = 1 << 9, F_CO = 1 << 10, F_BE = 1 << 11, F_HV = 1 << 12, F_BO = 1 << 13,+ F_BR = 1 << 14, F_BY = 1 << 15+};+#define F_SHOOTER (F_KI | F_GKI | F_HKI | F_HGKI)++/* a part of a character's ragdoll */+enum { P_TORSO, P_HEAD, P_ARM_F, P_FORE_F, P_ARM_B, P_FORE_B, P_THIGH_F, P_SHIN_F, P_THIGH_B, P_SHIN_B, P_COUNT };++typedef struct {+ uint8_t kind, idx, gone, sw; /* gone: broken, switched away or not yet there; sw: its switch + 1 */+ uint32_t flags;+ int16_t body; /* -1: none */+ float cx, cy; /* the body's origin */+ float bx, by, r; /* a ball's centre (from the origin) and radius */+ const uint8_t *outline; /* points (x 8 units, from the origin), then holes */+ uint16_t nout;+ uint8_t nholes;+ uint8_t kin; /* moved along a track (pin t 5) */+ float kx0, ky0, kdx, kdy, klen, kspeed, kpos;+} Obj;++typedef struct {+ uint8_t role, alive, left, hittable, obj;+ float h; /* height */+ float fx, fy, tilt; /* where it stood: feet and lean (at the start) */+ int16_t body; /* standing: one body */+ int16_t part[P_COUNT]; /* fallen: the ragdoll */+} Char;++typedef struct {+ uint8_t kind, life_on; /* kind 0 bullet, 1 grenade */+ int16_t body;+ int16_t life; /* steps left */+ float px, py; /* where it was the step before */+ float tx[TRAIL], ty[TRAIL];+ uint8_t tn;+} Shot;++typedef struct {+ uint8_t t, motor;+ int8_t a, b; /* objects (-1: the world) */+ int16_t joint;+ float x0, y0, x1, y1; /* a track's ends */+} Pin;++typedef struct {+ uint8_t id, colour, on, nlinks;+ uint8_t link[12];+ int16_t body;+ float x0, y0, x1, y1;+} Switch;++typedef struct {+ float x, y;+ int16_t t; /* frames since */+} Boom;++typedef struct {+ int level, mode, state, lose;+ int tr, ammo, shots; /* three stars at tr shots; tr + 3 bullets */+ int nobj, nchar, npin, nsw;+ Obj obj[MAX_OBJ];+ Char ch[MAX_CHAR];+ Pin pin[MAX_PIN];+ Switch sw[MAX_SW];+ Shot shot[MAX_SHOT];+ Boom boom[MAX_BOOM];+ int hero; /* the shooter's character */+ bool grenades; /* he throws grenades */+ float aim; /* screen angle */+ float power; /* grenades: 0 to 1 */+ int frame, quiet, end_wait; /* steps played, steps since anything moved, steps to the result */+ int enemies_left;+} Game;++extern Game g;+/* memory for points, shared by loading and drawing */+#define SCRATCH_FLOATS 1200+extern float mb_scratch[SCRATCH_FLOATS];++int game_mode_of(int level);+bool game_load(int level, uint8_t *buffer); /* buffer: LEVEL_MAX_BYTES, kept while it is played */+void game_end(void);+void game_step(void);+bool game_can_fire(void);+void game_fire(void);+void game_aim_by(float d); /* raise (d > 0) or lower the aim */+void game_turn(bool left);+int game_stars(void); /* for the shots so far */+/* the shooter's shoulder, hand and muzzle */+void game_gun(float *sx, float *sy, float *mx, float *my);+/* the grenade's path for the preview: up to n points */+int game_arc(float *xs, float *ys, int n);+/* where a character's part is: centre, sine and cosine of its turn (false: not there) */+bool game_part(const Char *c, int part, float *x, float *y, float *s, float *cs);+void game_part_size(const Char *c, int part, float *w, float *h);+#endifdiff --git a/games/mrbullet/src/icon.png b/games/mrbullet/src/icon.pngnew file mode 100644index 00000000..201cf9d5Binary files /dev/null and b/games/mrbullet/src/icon.png differdiff --git a/games/mrbullet/src/inflate.c b/games/mrbullet/src/inflate.cnew file mode 100644index 00000000..ae3f4283--- /dev/null+++ b/games/mrbullet/src/inflate.c@@ -0,0 +1,46 @@+/* Bounded RFC 1951 decoder. A level's output is its own history window. */+#include "inflate.h"+#include <string.h>+typedef struct {const uint8_t *p,*end; uint32_t bits; unsigned count; bool bad;} Bits;+typedef struct {uint16_t counts[16],symbols[288];} Tree;+static unsigned bits(Bits *b,unsigned n){+ while(b->count<n){if(b->p==b->end){b->bad=true;return 0;}b->bits|=(uint32_t)*b->p++<<b->count;b->count+=8;}+ unsigned v=b->bits&((1u<<n)-1);b->bits>>=n;b->count-=n;return v;+}+static bool tree(Tree *t,const uint8_t *lengths,unsigned n){+ memset(t->counts,0,sizeof(t->counts));+ for(unsigned i=0;i<n;i++){if(lengths[i]>15)return false;t->counts[lengths[i]]++;}+ int left=1;for(unsigned i=1;i<=15;i++){left=(left<<1)-t->counts[i];if(left<0)return false;}+ uint16_t offsets[16]={0};for(unsigned i=1;i<15;i++)offsets[i+1]=offsets[i]+t->counts[i];+ for(unsigned i=0;i<n;i++)if(lengths[i])t->symbols[offsets[lengths[i]]++]=(uint16_t)i;+ return t->counts[0]!=n;+}+static int symbol(Bits *b,const Tree *t){+ unsigned code=0,first=0,index=0;+ for(unsigned n=1;n<=15;n++){code|=bits(b,1);unsigned count=t->counts[n];if(code>=first&&code-first<count)return t->symbols[index+code-first];index+=count;first=(first+count)<<1;code<<=1;}+ b->bad=true;return -1;+}+bool inflate_raw(const uint8_t *src,size_t length,uint8_t *out,size_t capacity){+ static const uint16_t lb[]={3,4,5,6,7,8,9,10,11,13,15,17,19,23,27,31,35,43,51,59,67,83,99,115,131,163,195,227,258};+ static const uint8_t le[]={0,0,0,0,0,0,0,0,1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4,5,5,5,5,0};+ static const uint16_t db[]={1,2,3,4,5,7,9,13,17,25,33,49,65,97,129,193,257,385,513,769,1025,1537,2049,3073,4097,6145,8193,12289,16385,24577};+ static const uint8_t de[]={0,0,0,0,1,1,2,2,3,3,4,4,5,5,6,6,7,7,8,8,9,9,10,10,11,11,12,12,13,13};+ Bits b={src,src+length,0,0,false};size_t used=0;bool final=false;Tree lit,dist;+ while(!final&&!b.bad){+ final=bits(&b,1);unsigned type=bits(&b,2);+ if(type==0){b.bits=0;b.count=0;if((size_t)(b.end-b.p)<4)return false;unsigned n=b.p[0]|b.p[1]<<8,inv=b.p[2]|b.p[3]<<8;b.p+=4;if((n^65535)!=inv||n>capacity-used||(size_t)(b.end-b.p)<n)return false;memcpy(out+used,b.p,n);b.p+=n;used+=n;continue;}+ uint8_t lens[320]={0};unsigned nl=288,nd=32;+ if(type==1){for(unsigned i=0;i<288;i++)lens[i]=i<144?8:i<256?9:i<280?7:8;for(unsigned i=288;i<320;i++)lens[i]=5;}+ else if(type==2){+ static const uint8_t order[]={16,17,18,0,8,7,9,6,10,5,11,4,12,3,13,2,14,1,15};+ nl=257+bits(&b,5);nd=1+bits(&b,5);unsigned nc=4+bits(&b,4);uint8_t cl[19]={0};Tree codes;+ if(nl>286||nd>32)return false;+ for(unsigned i=0;i<nc;i++)cl[order[i]]=bits(&b,3);+ if(!tree(&codes,cl,19))return false;+ unsigned n=0;while(n<nl+nd&&!b.bad){int s=symbol(&b,&codes);if(s<0)return false;if(s<16)lens[n++]=(uint8_t)s;else{unsigned repeat=s==16?3+bits(&b,2):s==17?3+bits(&b,3):11+bits(&b,7);if(s==16&&!n)return false;uint8_t value=s==16?lens[n-1]:0;if(n+repeat>nl+nd)return false;while(repeat--)lens[n++]=value;}}+ }else return false;+ if(!lens[256]||!tree(&lit,lens,nl)||!tree(&dist,lens+nl,nd))return false;+ for(;;){int s=symbol(&b,&lit);if(b.bad)return false;if(s<256){if(used==capacity)return false;out[used++]=(uint8_t)s;}else if(s==256)break;else{if(s>285)return false;unsigned n=lb[s-257]+bits(&b,le[s-257]);int d=symbol(&b,&dist);if(d<0||d>=30)return false;unsigned back=db[d]+bits(&b,de[d]);if(b.bad||back>used||n>capacity-used)return false;while(n--){out[used]=out[used-back];used++;}}}+ }+ return !b.bad&&used==capacity;+}diff --git a/games/mrbullet/src/inflate.h b/games/mrbullet/src/inflate.hnew file mode 100644index 00000000..30beba24--- /dev/null+++ b/games/mrbullet/src/inflate.h@@ -0,0 +1,7 @@+#ifndef MB_INFLATE_H+#define MB_INFLATE_H+#include <stdbool.h>+#include <stddef.h>+#include <stdint.h>+bool inflate_raw(const uint8_t *src,size_t length,uint8_t *out,size_t capacity);+#endifdiff --git a/games/mrbullet/src/main.c b/games/mrbullet/src/main.cnew file mode 100644index 00000000..09a26968--- /dev/null+++ b/games/mrbullet/src/main.c@@ -0,0 +1,48 @@+/* Mr Bullet for the NumWorks calculator.+ *+ * Inspired by Mr Bullet - Spy Puzzles by Lion Studios, not affiliated with them. Made by Mason Chen as part of+ * NumPlay; the physics is Box2D by Erin Catto. */+#include "data.h"+#include "draw.h"+#include "plat.h"+#include "save.h"+#include "ui.h"+#include "world.h"++#ifndef HOST+/* (read in tools/emu.py: frames drawn, the slowest drawing, frames slower than 33 ms) */+uint32_t perf_frames __attribute__((used));+uint32_t perf_max __attribute__((used));+uint32_t perf_slow __attribute__((used));+uint32_t perf_step_max __attribute__((used));++void np_push(int x, int y, int w, int h, const color_t *px) { plat_push(x, y, w, h, px); }+extern char _heap_start[], _heap_end[];++int main(void) {+ /* on main's stack: the level's bytes, the screen's strip and the save's scratch memory; the heap is Box2D's */+ uint64_t level[LEVEL_MAX_BYTES / 8 + 1];+ uint64_t strip[SCREEN_W * STRIP_H * 2 / 8];+ uint64_t scratch[SAVE_SCRATCH / 8];+ plat_begin();+ /* Box2D makes no new contact past the heap less 16 KB (for a step's islands and the broad phase growing) */+ b2_heap_limit = (long)(_heap_end - _heap_start) - 16384;+ ui_init((uint8_t *)level, (uint16_t *)strip, scratch);+ for (;;) {+ uint32_t now = plat_millis();+ if (!ui_frame(plat_keys(), now)) break;+ if (ui_dirty()) {+ uint32_t t0 = plat_millis();+ ui_draw();+ uint32_t dt = plat_millis() - t0;+ if (dt > perf_max && perf_frames > 30) perf_max = dt;+ if (t0 - now + dt > 33) perf_slow++;+ if (t0 - now + dt > perf_step_max && perf_frames > 30) perf_step_max = t0 - now + dt;+ perf_frames++;+ } else {+ plat_sleep(5);+ }+ }+ return plat_end();+}+#endifdiff --git a/games/mrbullet/src/plat.h b/games/mrbullet/src/plat.hnew file mode 100644index 00000000..c35276be--- /dev/null+++ b/games/mrbullet/src/plat.h@@ -0,0 +1,16 @@+/* Mr Bullet: what the game needs from the calculator (plat_eadk.c), or from a computer for the tests. */+#ifndef MB_PLAT_H+#define MB_PLAT_H+#include <stdbool.h>+#include <stdint.h>++enum { KB_OK = 1, KB_UP = 2, KB_DOWN = 4, KB_LEFT = 8, KB_RIGHT = 16, KB_BACK = 32, KB_HOME = 64 };+uint32_t plat_keys(void);+uint32_t plat_millis(void);+void plat_sleep(uint32_t ms);+void plat_push(int x, int y, int w, int h, const uint16_t *px);+bool plat_save(const char *name, const void *data, uint32_t len);+const uint8_t *plat_load(const char *name, uint32_t *len);+void plat_begin(void);+int plat_end(void);+#endifdiff --git a/games/mrbullet/src/plat_eadk.c b/games/mrbullet/src/plat_eadk.cnew file mode 100644index 00000000..bae01e83--- /dev/null+++ b/games/mrbullet/src/plat_eadk.c@@ -0,0 +1,32 @@+/* The calculator: keys, time, the screen and files (EADK, and NumPlay's careful file helpers). */+#include <eadk.h>+#include "../../common/epsilon_app.h"+#include "../../common/epsilon_files.h"+#include "plat.h"++const char eadk_app_name[] __attribute__((section(".rodata.eadk_app_name"))) = "Mr Bullet";+const uint32_t eadk_api_level __attribute__((section(".rodata.eadk_api_level"))) = 0;++#define KEY(k) ((uint64_t)1 << (k))++uint32_t plat_keys(void) {+ uint64_t k = eadk_keyboard_scan();+ uint32_t r = 0;+ if (k & (KEY(eadk_key_ok) | KEY(eadk_key_exe))) r |= KB_OK;+ if (k & (KEY(eadk_key_up) | KEY(eadk_key_eight))) r |= KB_UP;+ if (k & (KEY(eadk_key_down) | KEY(eadk_key_two))) r |= KB_DOWN;+ if (k & (KEY(eadk_key_left) | KEY(eadk_key_four))) r |= KB_LEFT;+ if (k & (KEY(eadk_key_right) | KEY(eadk_key_six))) r |= KB_RIGHT;+ if (k & (KEY(eadk_key_back) | KEY(eadk_key_backspace))) r |= KB_BACK;+ if (k & (KEY(eadk_key_home) | KEY(eadk_key_on_off))) r |= KB_HOME;+ return r;+}+uint32_t plat_millis(void) { return (uint32_t)eadk_timing_millis(); }+void plat_sleep(uint32_t ms) { eadk_timing_msleep(ms); }+void plat_push(int x, int y, int w, int h, const uint16_t *px) {+ eadk_display_push_rect((eadk_rect_t){(uint16_t)x, (uint16_t)y, (uint16_t)w, (uint16_t)h}, (const eadk_color_t *)px);+}+bool plat_save(const char *name, const void *data, uint32_t len) { return ef_write(name, data, len); }+const uint8_t *plat_load(const char *name, uint32_t *len) { return ef_read(name, len); }+void plat_begin(void) { np_app_begin(); }+int plat_end(void) { return np_app_end(); }diff --git a/games/mrbullet/src/res.c b/games/mrbullet/src/res.cnew file mode 100644index 00000000..7f4d8f27--- /dev/null+++ b/games/mrbullet/src/res.c@@ -0,0 +1,29 @@+/* data.bin (res.h): a header, the levels' index, the pictures' index, then their bytes. */+#include <string.h>+#include "inflate.h"+#include "res.h"++static uint32_t u32(const uint8_t *p) { return p[0] | p[1] << 8 | p[2] << 16 | (uint32_t)p[3] << 24; }+static uint16_t u16(const uint8_t *p) { return (uint16_t)(p[0] | p[1] << 8); }++static const uint8_t *pic_at(int i) {+ uint32_t levels = u32(mb_data);+ return mb_data + u32(mb_data + 16 + 8 * levels + 4 * i);+}+const Pic *res_pic(int i) { return (const Pic *)pic_at(i); }+const uint16_t *res_pic_palette(const Pic *p) { return (const uint16_t *)((const uint8_t *)p + sizeof(Pic)); }+const uint8_t *res_pic_pixels(const Pic *p) { return (const uint8_t *)(res_pic_palette(p) + p->ncol); }++int res_level(int i, uint8_t *out) {+ if (i < 0 || i >= LEVEL_COUNT) return 0;+ const uint8_t *e = mb_data + 16 + 8 * i;+ uint32_t off = u32(e);+ uint16_t packed = u16(e + 4), size = u16(e + 6);+ if (size > LEVEL_MAX_BYTES) return 0;+ if (!inflate_raw(mb_data + off, packed, out, size)) return 0;+ return size;+}++static const int first[] = MODE_FIRST;+int res_mode_first(int mode) { return first[mode]; }+int res_mode_count(int mode) { return first[mode + 1] - first[mode]; }diff --git a/games/mrbullet/src/res.h b/games/mrbullet/src/res.hnew file mode 100644index 00000000..da16363d--- /dev/null+++ b/games/mrbullet/src/res.h@@ -0,0 +1,22 @@+/* Mr Bullet: the contents of data.bin (tools/pack.py): the levels, packed, and the pictures. */+#ifndef MB_RES_H+#define MB_RES_H+#include <stdbool.h>+#include <stdint.h>+#include "data.h"++typedef struct {+ uint16_t w, h;+ int16_t px4, py4; /* the pivot x 4 */+ uint8_t ncol, flags; /* flags 1: opaque, 2: a byte a pixel (else 4 bits, colour 0 clear) */+} Pic;++extern const uint8_t mb_data[];+const Pic *res_pic(int i);+const uint16_t *res_pic_palette(const Pic *p);+const uint8_t *res_pic_pixels(const Pic *p);+/* a level unpacked into out (LEVEL_MAX_BYTES): its size, or 0 */+int res_level(int i, uint8_t *out);+int res_mode_first(int mode);+int res_mode_count(int mode);+#endifdiff --git a/games/mrbullet/src/save.c b/games/mrbullet/src/save.cnew file mode 100644index 00000000..23857369--- /dev/null+++ b/games/mrbullet/src/save.c@@ -0,0 +1,122 @@+/* The save (save.h). */+#include <stddef.h>+#include <string.h>+#include "plat.h"+#include "save.h"++static uint8_t *scratch; /* a SaveFile, then the copy's text */+void save_scratch(void *mem) { scratch = mem; }++void save_fresh(Progress *p) {+ memset(p, 0, sizeof *p);+}++uint32_t save_sum(const void *data, uint32_t n) {+ const uint8_t *p = data;+ uint32_t h = 2166136261u;+ for (uint32_t i = 0; i < n; i++) h = (h ^ p[i]) * 16777619u;+ return h;+}++bool save_ok(const SaveFile *f) {+ if (f->magic != MB_MAGIC || f->version != MB_VERSION || f->size != sizeof *f) return false;+ if (f->sum != save_sum(f, offsetof(SaveFile, sum))) return false;+ const Progress *p = &f->p;+ for (int i = 0; i < LEVEL_COUNT; i++)+ if (p->stars[i] > 3 || p->flags[i] > 1) return false;+ for (int m = 0; m < MODE_COUNT; m++)+ if (p->last[m] >= LEVEL_COUNT) return false;+ return p->guide <= 1 && p->mode < MODE_COUNT;+}++static const char head[] = "# Mr Bullet's saves: keep this file\n";+static const char b64[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";+#define COPY_BYTES (1 + sizeof head + 2 + sizeof MB_SAVE_NAME + 1 + (sizeof(SaveFile) + 2) / 3 * 4 + 2)++/* the copy: the file in base 64, in a comment line of a script (made in the scratch memory, after the file) */+_Static_assert(sizeof(SaveFile) + COPY_BYTES <= SAVE_SCRATCH, "the scratch memory holds the file and its copy");+static bool copy_write(const SaveFile *f) {+ uint8_t *copy = scratch + sizeof(SaveFile);+ const uint8_t *bytes = (const uint8_t *)f;+ uint32_t len = sizeof *f, nl = sizeof MB_SAVE_NAME - 1;+ uint8_t *p = copy;+ *p++ = 0; /* (a script's first byte: not imported) */+ memcpy(p, head, sizeof head - 1), p += sizeof head - 1;+ *p++ = '#', *p++ = '>';+ memcpy(p, MB_SAVE_NAME, nl), p += nl;+ *p++ = ':';+ for (uint32_t i = 0; i < len; i += 3, p += 4) {+ uint32_t left = len - i;+ uint32_t v = (uint32_t)bytes[i] << 16 | (left > 1 ? (uint32_t)bytes[i + 1] << 8 : 0) | (left > 2 ? bytes[i + 2] : 0);+ p[0] = (uint8_t)b64[v >> 18 & 63];+ p[1] = (uint8_t)b64[v >> 12 & 63];+ p[2] = left > 1 ? (uint8_t)b64[v >> 6 & 63] : '=';+ p[3] = left > 2 ? (uint8_t)b64[v & 63] : '=';+ }+ *p++ = '\n';+ return plat_save(MB_COPY_NAME, copy, (uint32_t)(p - copy));+}++static int b64v(uint8_t c) {+ for (int i = 0; i < 64; i++)+ if ((uint8_t)b64[i] == c) return i;+ return -1;+}++static bool copy_read(SaveFile *f) {+ uint32_t n, nl = sizeof MB_SAVE_NAME - 1;+ const uint8_t *c = plat_load(MB_COPY_NAME, &n);+ if (!c) return false;+ for (uint32_t i = 0; i + nl + 3 <= n; i++) {+ if (c[i] != '#' || c[i + 1] != '>' || memcmp(c + i + 2, MB_SAVE_NAME, nl) || c[i + 2 + nl] != ':') continue;+ const uint8_t *l = c + i + 3 + nl;+ uint32_t m = 0;+ while (l + m < c + n && l[m] != '\n' && l[m]) m++;+ if (m != (sizeof *f + 2) / 3 * 4) return false;+ uint8_t *bytes = (uint8_t *)f;+ uint32_t o = 0;+ for (uint32_t k = 0; k < m; k += 4) {+ int v0 = b64v(l[k]), v1 = b64v(l[k + 1]), v2 = l[k + 2] == '=' ? 0 : b64v(l[k + 2]),+ v3 = l[k + 3] == '=' ? 0 : b64v(l[k + 3]);+ if (v0 < 0 || v1 < 0 || v2 < 0 || v3 < 0) return false;+ uint32_t w = (uint32_t)(v0 << 18 | v1 << 12 | v2 << 6 | v3);+ if (o < sizeof *f) bytes[o++] = (uint8_t)(w >> 16);+ if (o < sizeof *f) bytes[o++] = (uint8_t)(w >> 8);+ if (o < sizeof *f) bytes[o++] = (uint8_t)w;+ }+ return true;+ }+ return false;+}++bool save_load(Progress *p) {+ SaveFile *file = (SaveFile *)scratch;+ uint32_t n;+ const uint8_t *d = plat_load(MB_SAVE_NAME, &n);+ bool ok = false;+ if (d && n == sizeof *file) {+ memcpy(file, d, sizeof *file);+ ok = save_ok(file);+ }+ if (!ok && copy_read(file) && save_ok(file)) {+ ok = true;+ plat_save(MB_SAVE_NAME, file, sizeof *file); /* back where it belongs */+ }+ if (!ok) {+ save_fresh(p);+ return false;+ }+ *p = file->p;+ return true;+}++bool save_write(const Progress *p) {+ SaveFile *file = (SaveFile *)scratch;+ memset(file, 0, sizeof *file);+ file->magic = MB_MAGIC, file->version = MB_VERSION, file->size = sizeof *file;+ file->p = *p;+ file->sum = save_sum(file, offsetof(SaveFile, sum));+ bool ok = plat_save(MB_SAVE_NAME, file, sizeof *file);+ copy_write(file);+ return ok;+}diff --git a/games/mrbullet/src/save.h b/games/mrbullet/src/save.hnew file mode 100644index 00000000..38e3d09d--- /dev/null+++ b/games/mrbullet/src/save.h@@ -0,0 +1,42 @@+/* Mr Bullet: what is kept, in mrbullet.sav, and a copy of it in a Python script, mrbullet_saves.py (installing+ * apps from the NumWorks website keeps only scripts, so the save comes back from it).+ *+ * One format: a magic, a version, its size, then the progress, then a checksum of all before it. A file that is+ * damaged, cut short or from elsewhere is not read: the copy is tried, then a fresh start. */+#ifndef MB_SAVE_H+#define MB_SAVE_H+#include <stdbool.h>+#include <stdint.h>+#include "data.h"++#define MB_SAVE_NAME "mrbullet.sav"+#define MB_COPY_NAME "mrbullet_saves.py"+#define MB_MAGIC 0x3142524Du /* "MRB1" */+#define MB_VERSION 1++enum { LV_CLEARED = 1 };++typedef struct {+ uint8_t stars[LEVEL_COUNT]; /* best stars, 0 to 3 */+ uint8_t flags[LEVEL_COUNT]; /* LV_* */+ uint16_t last[MODE_COUNT]; /* the last level played in each mode */+ uint8_t guide; /* the first-run guide was shown */+ uint8_t mode; /* the last mode */+} Progress;++typedef struct {+ uint32_t magic;+ uint16_t version, size;+ Progress p;+ uint32_t sum; /* FNV-1a of everything before it */+} SaveFile;++/* the memory saving and loading work in: at least SAVE_SCRATCH bytes */+#define SAVE_SCRATCH 4096+void save_scratch(void *mem);+void save_fresh(Progress *p);+uint32_t save_sum(const void *data, uint32_t n);+bool save_ok(const SaveFile *f);+bool save_load(Progress *p); /* false: a fresh start */+bool save_write(const Progress *p);+#endifdiff --git a/games/mrbullet/src/ui.c b/games/mrbullet/src/ui.cnew file mode 100644index 00000000..3f98353a--- /dev/null+++ b/games/mrbullet/src/ui.c@@ -0,0 +1,418 @@+/* The screens (ui.h): the title, the modes, the levels, a level with its key lines, the pause menu, the result,+ * how to play and quitting. Every screen says in its bottom line what the keys do. */+#include <math.h>+#include <string.h>+#include "draw.h"+#include "game.h"+#include "plat.h"+#include "res.h"+#include "save.h"+#include "ui.h"++enum { S_TITLE, S_MODES, S_LEVELS, S_PLAY, S_PAUSE, S_RESULT, S_HOWTO, S_QUIT, S_COUNT };++static Progress P;+static int screen = S_TITLE, howto_back = S_TITLE, quit_back = S_TITLE;+static int sel, mode, level = -1;+static bool dirty = true;+static uint8_t *level_mem;+static uint32_t held, edge, last_ms, acc_ms, hold_ms;+static int result_stars, result_new;++static const char *const MODE_NAME[MODE_COUNT] = {"Classic", "Grenades", "Hostages", "Friendly Fire"};+static const int MODE_STARS[MODE_COUNT] = {0, 50, 120, 200}; /* stars that open it */+static const char *const PRAISE[4] = {"Good!", "Well done!", "Awesome!", "Fantastic!"};++/* ------------------------------------------------------------------ text */+static char *put(char *p, const char *s) {+ while (*s) *p++ = *s++;+ *p = 0;+ return p;+}+static char *num(char *p, uint32_t v) {+ char t[12];+ int n = 0;+ do t[n++] = (char)('0' + v % 10); while ((v /= 10) && n < 11);+ while (n) *p++ = t[--n];+ *p = 0;+ return p;+}++/* ------------------------------------------------------------------ progress */+static int total_stars(void) {+ int s = 0;+ for (int i = 0; i < LEVEL_COUNT; i++) s += P.stars[i];+ return s;+}+static int mode_stars(int m) {+ int s = 0, f = res_mode_first(m), n = res_mode_count(m);+ for (int i = f; i < f + n; i++) s += P.stars[i];+ return s;+}+static bool mode_open(int m) { return total_stars() >= MODE_STARS[m]; }+static bool level_open(int l) {+ int m = game_mode_of(l), f = res_mode_first(m);+ return l == f || (P.flags[l - 1] & LV_CLEARED) || (P.flags[l] & LV_CLEARED);+}++/* ------------------------------------------------------------------ playing */+static void start(int l) {+ level = l;+ mode = game_mode_of(l);+ P.last[mode] = (uint16_t)l;+ P.mode = (uint8_t)mode;+ if (!game_load(l, level_mem)) {+ game_end();+ screen = S_LEVELS;+ return;+ }+ acc_ms = 0;+ screen = S_PLAY;+}++static void finish(void) {+ if (g.state == ST_WON) {+ int st = game_stars();+ result_stars = st;+ result_new = st > P.stars[level];+ if (st > P.stars[level]) P.stars[level] = (uint8_t)st;+ P.flags[level] |= LV_CLEARED;+ save_write(&P);+ } else {+ result_stars = -1;+ }+ sel = 0;+ screen = S_RESULT;+}++void ui_quit(void) {+ game_end();+ save_write(&P);+}++/* ------------------------------------------------------------------ keys */+static bool pressed(uint32_t k) { return (edge & k) != 0; }++static void menu_keys(int count) {+ if (pressed(KB_UP)) sel = (sel + count - 1) % count, dirty = true;+ if (pressed(KB_DOWN)) sel = (sel + 1) % count, dirty = true;+}++static void play_keys(uint32_t dt) {+ uint32_t aim = held & (KB_UP | KB_DOWN);+ if (aim) hold_ms += dt;+ else hold_ms = 0;+ float rate = hold_ms < 350 ? 0.35f : hold_ms < 900 ? 1.0f : 2.0f; /* radians a second */+ if (pressed(KB_UP)) game_aim_by(0.0175f), dirty = true;+ else if (held & KB_UP) game_aim_by(rate * dt / 1000.0f), dirty = true;+ if (pressed(KB_DOWN)) game_aim_by(-0.0175f), dirty = true;+ else if (held & KB_DOWN) game_aim_by(-rate * dt / 1000.0f), dirty = true;+ if (g.grenades) {+ if (held & KB_RIGHT) g.power = fminf(1, g.power + 0.6f * dt / 1000.0f), dirty = true;+ if (held & KB_LEFT) g.power = fmaxf(0, g.power - 0.6f * dt / 1000.0f), dirty = true;+ } else {+ if (pressed(KB_LEFT)) game_turn(true), dirty = true;+ if (pressed(KB_RIGHT)) game_turn(false), dirty = true;+ }+ if (pressed(KB_OK) && game_can_fire()) game_fire(), dirty = true;+}++bool ui_frame(uint32_t keys, uint32_t now) {+ edge = keys & ~held;+ held = keys;+ uint32_t dt = last_ms ? now - last_ms : 0;+ if (dt > 100) dt = 100;+ last_ms = now;+ if (pressed(KB_HOME)) {+ ui_quit();+ return false;+ }+ switch (screen) {+ case S_TITLE:+ menu_keys(3);+ if (pressed(KB_OK)) {+ if (sel == 0) screen = S_MODES, sel = P.mode;+ else if (sel == 1) howto_back = S_TITLE, screen = S_HOWTO;+ else quit_back = S_TITLE, screen = S_QUIT, sel = 1;+ dirty = true;+ } else if (pressed(KB_BACK)) {+ quit_back = S_TITLE, screen = S_QUIT, sel = 1, dirty = true;+ }+ break;+ case S_MODES:+ menu_keys(MODE_COUNT);+ if (pressed(KB_OK) && mode_open(sel)) {+ mode = sel;+ int l = P.last[mode];+ if (game_mode_of(l) != mode || !level_open(l)) l = res_mode_first(mode);+ sel = l - res_mode_first(mode);+ screen = S_LEVELS, dirty = true;+ } else if (pressed(KB_BACK)) {+ screen = S_TITLE, sel = 0, dirty = true;+ }+ break;+ case S_LEVELS: {+ int n = res_mode_count(mode);+ int old = sel;+ if (pressed(KB_LEFT)) sel--;+ if (pressed(KB_RIGHT)) sel++;+ if (pressed(KB_UP)) sel -= 4;+ if (pressed(KB_DOWN)) sel += 4;+ if (sel < 0) sel = 0;+ if (sel >= n) sel = n - 1;+ if (sel != old) dirty = true;+ if (pressed(KB_OK) && level_open(res_mode_first(mode) + sel)) {+ start(res_mode_first(mode) + sel);+ if (!P.guide) P.guide = 1, howto_back = S_PLAY, screen = S_HOWTO;+ dirty = true;+ } else if (pressed(KB_BACK)) {+ screen = S_MODES, sel = mode, dirty = true;+ }+ break;+ }+ case S_PLAY:+ if (pressed(KB_BACK)) {+ screen = S_PAUSE, sel = 0, dirty = true;+ break;+ }+ play_keys(dt);+ /* 60 steps a second, at most 3 a frame (a slow frame: the game slows down rather than skip) */+ acc_ms += dt * 60;+ for (int k = 0; acc_ms >= 1000 && k < 3; k++) {+ acc_ms -= 1000;+ game_step();+ dirty = true;+ }+ if (acc_ms >= 1000) acc_ms = 0;+ if (g.state != ST_PLAY) finish();+ break;+ case S_PAUSE:+ menu_keys(5);+ if (pressed(KB_BACK)) screen = S_PLAY, dirty = true;+ else if (pressed(KB_OK)) {+ if (sel == 0) screen = S_PLAY;+ else if (sel == 1) start(level);+ else if (sel == 2) game_end(), screen = S_LEVELS, sel = level - res_mode_first(mode);+ else if (sel == 3) howto_back = S_PAUSE, screen = S_HOWTO;+ else quit_back = S_PAUSE, screen = S_QUIT, sel = 1;+ dirty = true;+ }+ break;+ case S_RESULT: {+ bool won = result_stars >= 0;+ bool more = won && level + 1 < res_mode_first(mode) + res_mode_count(mode);+ int count = more ? 3 : 2;+ menu_keys(count);+ if (pressed(KB_OK)) {+ int pick = more ? sel : sel + 1;+ if (pick == 0) start(level + 1);+ else if (pick == 1) start(level);+ else game_end(), screen = S_LEVELS, sel = level - res_mode_first(mode);+ dirty = true;+ } else if (pressed(KB_BACK)) {+ game_end(), screen = S_LEVELS, sel = level - res_mode_first(mode), dirty = true;+ }+ break;+ }+ case S_HOWTO:+ if (pressed(KB_OK) || pressed(KB_BACK)) {+ screen = howto_back, sel = 0, dirty = true;+ if (screen == S_PLAY) save_write(&P);+ }+ break;+ case S_QUIT:+ if (pressed(KB_UP) || pressed(KB_DOWN)) sel ^= 1, dirty = true;+ if (pressed(KB_BACK)) screen = quit_back, sel = 0, dirty = true;+ else if (pressed(KB_OK)) {+ if (sel == 0) {+ ui_quit();+ return false;+ }+ screen = quit_back, sel = 0, dirty = true;+ }+ break;+ }+ return true;+}++bool ui_dirty(void) { return dirty; }++/* ------------------------------------------------------------------ drawing */+static void key_line(const char *s) {+ if (gfx_y1 <= 222) return;+ gfx_fill(0, 222, SCREEN_W, 18, COL_INK);+ gfx_text_center(&np_font_small, SCREEN_W / 2, 235, s, COL_MUTED, 256);+}++static void backdrop(void) {+ gfx_fill(0, gfx_y0, SCREEN_W, gfx_y1 - gfx_y0, COL_PANEL);+ draw_pic(PIC_BG, FIELD_X0, 0);+ gfx_fill_alpha(FIELD_X0, gfx_y0, FIELD_W, gfx_y1 - gfx_y0, COL_INK, 110);+}++static void button(int x, int y, int w, int h, const char *s, bool on, bool locked) {+ if (y + h < gfx_y0 || y > gfx_y1) return;+ gfx_rrect(x, y, w, h, 7, on ? COL_RED : COL_PANEL_LIGHT, 256);+ if (on) gfx_rrect_outline(x - 2, y - 2, w + 4, h + 4, 9, 2, COL_FOCUS, 256);+ gfx_text_center(&np_font_body, x + w / 2, y + h / 2 + 5, s, locked ? COL_MUTED : COL_TEXT, 256);+}++static void scene_title(void *ctx) {+ (void)ctx;+ backdrop();+ draw_pic(PIC_TITLE, (SCREEN_W - res_pic(PIC_TITLE)->w) / 2, 22);+ static const char *const items[3] = {"Play", "How to play", "Quit"};+ for (int i = 0; i < 3; i++) button(100, 104 + i * 36, 120, 28, items[i], sel == i, false);+ key_line("Up Down choose - OK select - Home quit");+}++static void scene_modes(void *ctx) {+ (void)ctx;+ backdrop();+ gfx_text_center(&np_font_title, SCREEN_W / 2, 30, "Modes", COL_TEXT, 256);+ for (int m = 0; m < MODE_COUNT; m++) {+ int y = 44 + m * 42;+ bool open = mode_open(m);+ button(40, y, 240, 34, "", sel == m, !open);+ if (y + 34 < gfx_y0 || y > gfx_y1) continue;+ gfx_text(&np_font_body, 54, y + 22, MODE_NAME[m], open ? COL_TEXT : COL_MUTED, 256);+ char t[24], *p = t;+ if (open) {+ p = num(p, (uint32_t)mode_stars(m));+ p = put(p, " / ");+ num(p, (uint32_t)res_mode_count(m) * 3);+ } else {+ p = num(p, (uint32_t)total_stars());+ p = put(p, " / ");+ num(p, (uint32_t)MODE_STARS[m]);+ }+ gfx_text_right(&np_font_small, 248, y + 21, t, COL_TEXT, 256);+ if (open) draw_pic(PIC_STAR, 252, y + 7);+ else draw_pic(PIC_PADLOCK, 256, y + 10);+ }+ key_line("Up Down choose - OK open - Back title");+}++static void scene_levels(void *ctx) {+ (void)ctx;+ backdrop();+ int first = res_mode_first(mode), n = res_mode_count(mode);+ int page = sel / 16, pages = (n + 15) / 16;+ char t[40], *p = t;+ p = put(p, MODE_NAME[mode]);+ p = put(p, " - Chapter ");+ p = num(p, (uint32_t)page + 1);+ p = put(p, " of ");+ num(p, (uint32_t)pages);+ gfx_text_center(&np_font_body, SCREEN_W / 2, 20, t, COL_TEXT, 256);+ for (int k = 0; k < 16; k++) {+ int i = page * 16 + k;+ if (i >= n) break;+ int x = 52 + (k % 4) * 56, y = 30 + (k / 4) * 47;+ if (y + 44 < gfx_y0 || y > gfx_y1) continue;+ bool open = level_open(first + i), on = sel == i;+ gfx_rrect(x, y, 48, 42, 8, on ? COL_RED : open ? COL_PANEL_LIGHT : COL_INK, 256);+ if (on) gfx_rrect_outline(x - 2, y - 2, 52, 46, 10, 2, COL_FOCUS, 256);+ if (open) {+ char s[8];+ num(s, (uint32_t)i + 1);+ gfx_text_center(&np_font_body, x + 24, y + 20, s, COL_TEXT, 256);+ for (int st = 0; st < 3; st++) {+ int sx = x + 7 + st * 13, sy = y + 28;+ if (st < P.stars[first + i]) gfx_circle(sx + 4, sy + 4, 4, COL_FOCUS, 256);+ else gfx_circle(sx + 4, sy + 4, 4, COL_INK, 200);+ }+ } else {+ draw_pic(PIC_PADLOCK, x + 24 - res_pic(PIC_PADLOCK)->w / 2, y + 14);+ }+ }+ key_line("Arrows choose - OK play - Back modes");+}++static void scene_play(void *ctx) {+ (void)ctx;+ draw_world(screen == S_PLAY);+ char t[24], *p = t;+ p = put(p, "Level ");+ num(p, (uint32_t)(level - res_mode_first(mode) + 1));+ draw_hud(t, "Left Right", g.grenades ? "power" : "turn");+}++static void scene_pause(void *ctx) {+ scene_play(ctx);+ gfx_fill_alpha(FIELD_X0, gfx_y0, FIELD_W, gfx_y1 - gfx_y0, COL_INK, 150);+ static const char *const items[5] = {"Resume", "Restart", "Levels", "How to play", "Quit game"};+ for (int i = 0; i < 5; i++) button(90, 34 + i * 36, 140, 28, items[i], sel == i, false);+}++static void scene_result(void *ctx) {+ scene_play(ctx);+ gfx_fill_alpha(FIELD_X0, gfx_y0, FIELD_W, gfx_y1 - gfx_y0, COL_INK, 150);+ bool won = result_stars >= 0;+ if (won) {+ gfx_text_center(&np_font_title, SCREEN_W / 2, 40, PRAISE[result_stars], COL_TEXT, 256);+ for (int i = 0; i < 3; i++) {+ int x = 160 - 54 + i * 38, y = 52 + (i == 1 ? -4 : 0);+ if (i < result_stars) draw_pic(PIC_STAR_BIG, x, y);+ else if (y + 34 > gfx_y0 && y < gfx_y1) gfx_circle(x + 17, y + 17, 13, COL_PANEL_LIGHT, 256);+ }+ } else {+ const char *why = g.lose == LOSE_HOSTAGE ? "Don't kill the hostages!" : g.lose == LOSE_SELF ? "Don't shoot yourself!"+ : "No bullets!";+ gfx_text_center(&np_font_body, SCREEN_W / 2, 60, why, COL_TEXT, 256);+ }+ bool more = won && level + 1 < res_mode_first(mode) + res_mode_count(mode);+ static const char *const items[3] = {"Next level", "Replay", "Levels"};+ int k = 0;+ for (int i = more ? 0 : 1; i < 3; i++, k++) button(90, 104 + k * 36, 140, 28, items[i], sel == k, false);+}++static void scene_howto(void *ctx) {+ (void)ctx;+ backdrop();+ gfx_text_center(&np_font_title, SCREEN_W / 2, 26, "How to play", COL_TEXT, 256);+ static const char *const lines[] = {+ "Shoot every enemy in the level.",+ "Bullets bounce off walls: aim at them",+ "to reach enemies you can't see.",+ "Fewer bullets, more stars.",+ "Glass breaks, red boxes explode,",+ "a coloured switch moves its blocks.",+ "Grenades go off after 2.5 seconds.",+ "Don't hit hostages, or yourself in",+ "Friendly Fire.",+ "",+ "Up Down aim, Left Right turn",+ "(grenades: Left Right power), OK fire.",+ };+ for (unsigned i = 0; i < sizeof lines / sizeof *lines; i++)+ gfx_text_center(&np_font_small, SCREEN_W / 2, 48 + 14 * (int)i, lines[i], COL_TEXT, 256);+ key_line("OK or Back continue");+}++static void scene_quit(void *ctx) {+ if (quit_back == S_PAUSE) scene_play(ctx);+ else backdrop();+ gfx_fill_alpha(0, gfx_y0, SCREEN_W, gfx_y1 - gfx_y0, COL_INK, 150);+ gfx_text_center(&np_font_body, SCREEN_W / 2, 80, "Quit Mr Bullet?", COL_TEXT, 256);+ button(110, 100, 100, 28, "Quit", sel == 0, false);+ button(110, 136, 100, 28, "Stay", sel == 1, false);+}++static void (*const scenes[S_COUNT])(void *) = {scene_title, scene_modes, scene_levels, scene_play,+ scene_pause, scene_result, scene_howto, scene_quit};++void ui_draw(void) {+ gfx_render(scenes[screen], NULL, 0, SCREEN_H);+ dirty = false;+}++void ui_init(uint8_t *mem, uint16_t *strip, void *scratch) {+ level_mem = mem;+ gfx_buf = strip;+ save_scratch(scratch);+ save_load(&P);+ sel = 0;+ screen = S_TITLE;+ dirty = true;+}diff --git a/games/mrbullet/src/ui.h b/games/mrbullet/src/ui.hnew file mode 100644index 00000000..083c09f3--- /dev/null+++ b/games/mrbullet/src/ui.h@@ -0,0 +1,13 @@+/* Mr Bullet: the screens (ui.c). */+#ifndef MB_UI_H+#define MB_UI_H+#include <stdbool.h>+#include <stdint.h>++/* mem: the level's bytes are kept there (LEVEL_MAX_BYTES); strip: the screen's strip */+void ui_init(uint8_t *level_mem, uint16_t *strip, void *scratch);+bool ui_frame(uint32_t keys, uint32_t now); /* false: quit */+bool ui_dirty(void);+void ui_draw(void);+void ui_quit(void); /* saves */+#endifdiff --git a/games/mrbullet/src/world.cpp b/games/mrbullet/src/world.cppnew file mode 100644index 00000000..5343236c--- /dev/null+++ b/games/mrbullet/src/world.cpp@@ -0,0 +1,377 @@+/* The physics (world.h): Box2D 2.3.1 as Mr Bullet sets it up.+ *+ * As in the game: gravity 9.81 m/s2, steps of 1/60 s with 18 velocity and 14 position iterations, sleeping,+ * warm starting and continuous collision, 60 of the game's units a metre. Collision categories as its matrix:+ * category 0 meets nothing, characters' pieces never meet each other, nor bullets each other. */+#include <new>+#include <math.h>+#include <Box2D/Dynamics/b2World.h>+#include <Box2D/Dynamics/b2Body.h>+#include <Box2D/Dynamics/b2Fixture.h>+#include <Box2D/Dynamics/b2WorldCallbacks.h>+#include <Box2D/Dynamics/Contacts/b2Contact.h>+#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>+#include <Box2D/Dynamics/Joints/b2RopeJoint.h>+#include <Box2D/Collision/Shapes/b2ChainShape.h>+#include <Box2D/Collision/Shapes/b2CircleShape.h>+#include <Box2D/Collision/Shapes/b2PolygonShape.h>+#include "world.h"++#define U (1.0f / 60.0f) /* metres a unit */++/* (Box2D's classes have virtual destructors, which name delete: the world itself is placed in static memory) */+void operator delete(void *p) noexcept { free(p); }+void operator delete(void *p, unsigned int) noexcept { free(p); }+void operator delete(void *p, unsigned long) noexcept { free(p); }+#ifndef HOST+extern "C" void __cxa_pure_virtual(void) {+ for (;;) {}+}+#endif++static union { char mem[sizeof(b2World)]; double align; } world_mem;+static b2World *world;+static b2Body *bodies[W_BODIES];+static int body_obj[W_BODIES];+static b2Joint *joints[W_JOINTS];+static b2Body *ground;++static inline b2Vec2 M(float x, float y) { return b2Vec2(x * U, -y * U); }++class Listener : public b2ContactListener {+ public:+ void BeginContact(b2Contact *c) {+ b2Body *a = c->GetFixtureA()->GetBody(), *b = c->GetFixtureB()->GetBody();+ int ia = (int)(intptr_t)a->GetUserData() - 1, ib = (int)(intptr_t)b->GetUserData() - 1;+ if (ia < 0 || ib < 0) return;+ b2WorldManifold m;+ c->GetWorldManifold(&m);+ b2Vec2 p = c->GetManifold()->pointCount ? m.points[0] : 0.5f * (a->GetWorldCenter() + b->GetWorldCenter());+ b2Vec2 dv = b->GetLinearVelocityFromWorldPoint(p) - a->GetLinearVelocityFromWorldPoint(p);+ float speed = c->GetManifold()->pointCount ? -b2Dot(dv, m.normal) : dv.Length();+ if (speed < 0) speed = -speed;+ game_contact(body_obj[ia], body_obj[ib], ia, ib, speed / U, p.x / U, -p.y / U);+ }+ void PreSolve(b2Contact *c, const b2Manifold *old) {+ (void)old;+ int ia = (int)(intptr_t)c->GetFixtureA()->GetBody()->GetUserData() - 1;+ int ib = (int)(intptr_t)c->GetFixtureB()->GetBody()->GetUserData() - 1;+ if (ia >= 0 && ib >= 0 && !game_contact_enabled(body_obj[ia], body_obj[ib])) c->SetEnabled(false);+ }+};+static union { char mem[sizeof(Listener)]; void *align; } listener_mem;+static Listener *listener;++extern "C" bool world_begin(void) {+ world_end();+ world = new (world_mem.mem) b2World(b2Vec2(0.0f, -9.81f));+ world->SetAllowSleeping(true);+ world->SetWarmStarting(true);+ world->SetContinuousPhysics(true);+ if (!listener) listener = new (listener_mem.mem) Listener;+ world->SetContactListener(listener);+ return true;+}++extern "C" void world_end(void) {+ if (world) world->~b2World(); /* (gives back all its blocks) */+ world = 0;+ ground = 0;+ for (int i = 0; i < W_BODIES; i++) bodies[i] = 0;+ for (int i = 0; i < W_JOINTS; i++) joints[i] = 0;+}++extern "C" int world_body(int type, float x, float y, float angle, int obj, bool bullet, bool fixed_rotation,+ float gravity) {+ if (!world) return -1;+ int i = 0;+ while (i < W_BODIES && bodies[i]) i++;+ if (i == W_BODIES) return -1;+ b2BodyDef bd;+ bd.type = type == WB_STATIC ? b2_staticBody : type == WB_KINEMATIC ? b2_kinematicBody : b2_dynamicBody;+ bd.position = M(x, y);+ bd.angle = -angle;+ bd.linearDamping = type == WB_DYNAMIC ? 0.5f : 0.0f; /* every dynamic body: (0.5, 0) */+ bd.bullet = bullet;+ bd.fixedRotation = fixed_rotation;+ bd.gravityScale = gravity;+ bd.userData = (void *)(intptr_t)(i + 1);+ b2Body *b = world->CreateBody(&bd);+ if (!b) return -1;+ bodies[i] = b;+ body_obj[i] = obj;+ return i;+}++static void filter_of(b2FixtureDef *fd, const WFix *f) {+ fd->density = f->density;+ fd->friction = f->friction;+ fd->restitution = f->restitution;+ fd->isSensor = f->sensor != 0;+ int c = f->category;+ fd->filter.categoryBits = (uint16)(1u << c);+ uint16 mask = 0xFFFF;+ if (c == WC_NONE) mask = 0;+ else mask &= (uint16)~1u; /* (category 0 meets nothing) */+ if (c == WC_CHARACTER) mask &= (uint16)~(1u << WC_CHARACTER);+ if (c == WC_BULLET) mask &= (uint16)~(1u << WC_BULLET | 1u << WC_CHARACTER); /* (bullets go through+ characters: game.c finds them along their path) */+ if (c == WC_CHARACTER) mask &= (uint16)~(1u << WC_BULLET);+ fd->filter.maskBits = mask;+}++static bool add_shape(int b, const b2Shape *s, const WFix *f) {+ if (b < 0 || b >= W_BODIES || !bodies[b]) return false;+ b2FixtureDef fd;+ fd.shape = s;+ filter_of(&fd, f);+ return bodies[b]->CreateFixture(&fd) != 0;+}++extern "C" bool world_poly(int b, const float *xy, int n, const WFix *f) {+ if (n < 3 || n > b2_maxPolygonVertices) return false;+ b2Vec2 v[b2_maxPolygonVertices];+ float area = 0;+ for (int i = 0; i < n; i++) {+ v[i] = b2Vec2(xy[2 * i] * U, -xy[2 * i + 1] * U);+ int j = i ? i - 1 : n - 1;+ area += xy[2 * j] * xy[2 * i + 1] - xy[2 * i] * xy[2 * j + 1];+ }+ if (fabsf(area) < 2.0f) return false; /* (a sliver Box2D would turn into its 2 m box) */+ b2PolygonShape p;+ p.Set(v, n);+ if (p.m_count < 3) return false;+ return add_shape(b, &p, f);+}++extern "C" bool world_chain(int b, float *xy, int n, const WFix *f) {+ /* the points become Box2D's in place (b2Vec2: two floats) */+ b2Vec2 *v = (b2Vec2 *)(void *)xy;+ int m = 0;+ for (int i = 0; i < n; i++) {+ b2Vec2 p(xy[2 * i] * U, -xy[2 * i + 1] * U);+ /* (the rounded corners' points 3 units apart at most: fewer edges, each a box in the broad phase) */+ if (m && b2DistanceSquared(p, v[m - 1]) < 9.0f * U * U) continue;+ v[m++] = p;+ }+ while (m > 3 && b2DistanceSquared(v[0], v[m - 1]) < 0.25f * U * U) m--;+ if (m < 3) return false;+ b2ChainShape c;+ c.CreateLoop(v, m);+ return add_shape(b, &c, f); /* (the fixture keeps its own copy of the points) */+}++extern "C" bool world_box(int b, float cx, float cy, float hw, float hh, const WFix *f) {+ b2PolygonShape p;+ p.SetAsBox(hw * U, hh * U, b2Vec2(cx * U, -cy * U), 0);+ return add_shape(b, &p, f);+}++extern "C" bool world_circle(int b, float cx, float cy, float r, const WFix *f) {+ b2CircleShape c;+ c.m_p.Set(cx * U, -cy * U);+ c.m_radius = r * U;+ return add_shape(b, &c, f);+}++extern "C" void world_destroy(int b) {+ if (!world || b < 0 || b >= W_BODIES || !bodies[b]) return;+ /* its joints go with it */+ for (b2JointEdge *e = bodies[b]->GetJointList(); e; e = e->next)+ for (int j = 0; j < W_JOINTS; j++)+ if (joints[j] == e->joint) joints[j] = 0;+ world->DestroyBody(bodies[b]);+ bodies[b] = 0;+}++extern "C" bool world_alive(int b) { return b >= 0 && b < W_BODIES && bodies[b]; }+extern "C" int world_obj(int b) { return world_alive(b) ? body_obj[b] : -1; }++static b2Body *ground_body(void) {+ if (!ground) {+ b2BodyDef bd;+ ground = world->CreateBody(&bd);+ }+ return ground;+}++static int add_joint(b2Joint *j) {+ if (!j) return -1;+ for (int i = 0; i < W_JOINTS; i++)+ if (!joints[i]) {+ joints[i] = j;+ return i;+ }+ world->DestroyJoint(j);+ return -1;+}++static b2Body *body_or_ground(int b) { return world_alive(b) ? bodies[b] : ground_body(); }++extern "C" int world_revolute(int a, int b, float x, float y, bool limit, float lo, float hi, bool motor, float speed,+ float torque) {+ if (!world) return -1;+ b2Body *ba = body_or_ground(a), *bb = body_or_ground(b);+ if (ba == bb) return -1;+ b2RevoluteJointDef jd;+ jd.Initialize(ba, bb, M(x, y));+ jd.enableLimit = limit;+ jd.lowerAngle = lo;+ jd.upperAngle = hi;+ jd.enableMotor = motor;+ jd.motorSpeed = speed;+ jd.maxMotorTorque = torque;+ return add_joint(world->CreateJoint(&jd));+}++extern "C" int world_rope(int a, int b, float ax, float ay, float bx, float by, float length) {+ if (!world) return -1;+ b2Body *ba = body_or_ground(a), *bb = body_or_ground(b);+ if (ba == bb) return -1;+ b2RopeJointDef jd;+ jd.bodyA = ba;+ jd.bodyB = bb;+ jd.localAnchorA = ba->GetLocalPoint(M(ax, ay));+ jd.localAnchorB = bb->GetLocalPoint(M(bx, by));+ jd.maxLength = length * U;+ jd.collideConnected = true;+ return add_joint(world->CreateJoint(&jd));+}++extern "C" void world_destroy_joint(int j) {+ if (!world || j < 0 || j >= W_JOINTS || !joints[j]) return;+ world->DestroyJoint(joints[j]);+ joints[j] = 0;+}+extern "C" bool world_joint_alive(int j) { return j >= 0 && j < W_JOINTS && joints[j]; }+extern "C" void world_joint_anchors(int j, float *ax, float *ay, float *bx, float *by) {+ b2Vec2 a = joints[j]->GetAnchorA(), b = joints[j]->GetAnchorB();+ *ax = a.x / U, *ay = -a.y / U, *bx = b.x / U, *by = -b.y / U;+}+extern "C" void world_set_limits(int j, bool on, float lo, float hi) {+ if (!world_joint_alive(j) || joints[j]->GetType() != e_revoluteJoint) return;+ b2RevoluteJoint *r = (b2RevoluteJoint *)joints[j];+ r->EnableLimit(on);+ if (on) r->SetLimits(lo, hi);+}++extern "C" void world_step(void) {+ if (world) world->Step(W_STEP, 18, 14);+}++extern "C" void world_xf(int b, float *x, float *y, float *s, float *c) {+ const b2Transform &t = bodies[b]->GetTransform();+ *x = t.p.x / U, *y = -t.p.y / U, *s = -t.q.s, *c = t.q.c;+}+extern "C" float world_angle(int b) { return -bodies[b]->GetAngle(); }+extern "C" void world_velocity(int b, float *vx, float *vy) {+ b2Vec2 v = bodies[b]->GetLinearVelocity();+ *vx = v.x / U, *vy = -v.y / U;+}+extern "C" void world_set_velocity(int b, float vx, float vy) { bodies[b]->SetLinearVelocity(b2Vec2(vx * U, -vy * U)); }+extern "C" void world_set_angular(int b, float w) { bodies[b]->SetAngularVelocity(-w); }+extern "C" float world_angular(int b) { return -bodies[b]->GetAngularVelocity(); }+extern "C" void world_set_transform(int b, float x, float y, float angle) { bodies[b]->SetTransform(M(x, y), -angle); }+extern "C" void world_impulse(int b, float ix, float iy, float px, float py) {+ bodies[b]->ApplyLinearImpulse(b2Vec2(ix, -iy), M(px, py), true);+}+extern "C" void world_angular_impulse(int b, float i) { bodies[b]->ApplyAngularImpulse(-i, true); }+extern "C" float world_mass(int b) { return bodies[b]->GetMass(); }+extern "C" bool world_awake(int b) { return bodies[b]->IsAwake(); }+extern "C" bool world_dynamic(int b) { return bodies[b]->GetType() == b2_dynamicBody; }+extern "C" void world_set_type(int b, int type) {+ bodies[b]->SetType(type == WB_STATIC ? b2_staticBody : type == WB_KINEMATIC ? b2_kinematicBody : b2_dynamicBody);+}+extern "C" void world_wake(int b) { bodies[b]->SetAwake(true); }+extern "C" void world_set_fixed_rotation(int b, bool on) { bodies[b]->SetFixedRotation(on); }++class Ray : public b2RayCastCallback {+ public:+ unsigned mask;+ b2Fixture *hit;+ b2Vec2 p, n;+ float32 ReportFixture(b2Fixture *f, const b2Vec2 &point, const b2Vec2 &normal, float32 fraction) {+ if (f->IsSensor() || !(mask & f->GetFilterData().categoryBits)) return -1;+ hit = f, p = point, n = normal;+ return fraction;+ }+};+extern "C" int world_ray(float x0, float y0, float x1, float y1, unsigned mask, float *hx, float *hy, float *nx,+ float *ny) {+ if (!world) return -1;+ Ray r;+ r.mask = mask, r.hit = 0;+ b2Vec2 a = M(x0, y0), b = M(x1, y1);+ if ((b - a).LengthSquared() < 1e-12f) return -1;+ world->RayCast(&r, a, b);+ if (!r.hit) return -1;+ *hx = r.p.x / U, *hy = -r.p.y / U, *nx = r.n.x, *ny = -r.n.y;+ return (int)(intptr_t)r.hit->GetBody()->GetUserData() - 1;+}++class RayAll : public b2RayCastCallback {+ public:+ unsigned mask;+ int *out, max, n;+ float *xs, *ys;+ float32 ReportFixture(b2Fixture *f, const b2Vec2 &point, const b2Vec2 &normal, float32 fraction) {+ (void)normal, (void)fraction;+ if (f->IsSensor() || !(mask & f->GetFilterData().categoryBits)) return -1;+ int b = (int)(intptr_t)f->GetBody()->GetUserData() - 1;+ if (b < 0) return 1;+ for (int i = 0; i < n; i++)+ if (out[i] == b) return 1;+ if (n < max) out[n] = b, xs[n] = point.x / U, ys[n] = -point.y / U, n++;+ return n < max ? 1 : 0;+ }+};+extern "C" int world_ray_all(float x0, float y0, float x1, float y1, unsigned mask, int *out, float *xs, float *ys,+ int max) {+ if (!world) return 0;+ RayAll r;+ r.mask = mask, r.out = out, r.max = max, r.n = 0, r.xs = xs, r.ys = ys;+ b2Vec2 a = M(x0, y0), b = M(x1, y1);+ if ((b - a).LengthSquared() < 1e-12f) return 0;+ world->RayCast(&r, a, b);+ return r.n;+}++class Query : public b2QueryCallback {+ public:+ int *out, max, n;+ bool ReportFixture(b2Fixture *f) {+ int b = (int)(intptr_t)f->GetBody()->GetUserData() - 1;+ if (b < 0) return true;+ for (int i = 0; i < n; i++)+ if (out[i] == b) return true;+ if (n < max) out[n++] = b;+ return n < max;+ }+};+extern "C" int world_query(float x0, float y0, float x1, float y1, int *out, int max) {+ if (!world) return 0;+ Query q;+ q.out = out, q.max = max, q.n = 0;+ b2AABB box;+ box.lowerBound = b2Vec2(fminf(x0, x1) * U, -fmaxf(y0, y1) * U);+ box.upperBound = b2Vec2(fmaxf(x0, x1) * U, -fminf(y0, y1) * U);+ world->QueryAABB(&q, box);+ return q.n;+}++extern "C" int world_bodies(void) { return world ? world->GetBodyCount() : 0; }+extern "C" int world_contacts(void) { return world ? world->GetContactCount() : 0; }+#ifdef HOST+#include <stdio.h>+extern "C" void world_debug(void) {+ int n = 0, touching = 0;+ for (b2Contact *c = world->GetContactList(); c; c = c->GetNext()) {+ n++;+ if (c->IsTouching()) touching++;+ if (n < 40 && c->IsTouching()) printf("contact %d-%d types %d %d touching %d\n", (int)(intptr_t)c->GetFixtureA()->GetBody()->GetUserData(),+ (int)(intptr_t)c->GetFixtureB()->GetBody()->GetUserData(), c->GetFixtureA()->GetType(),+ c->GetFixtureB()->GetType(), c->IsTouching());+ }+ printf("contacts %d touching %d bodies %d proxies %d\n", n, touching, world->GetBodyCount(), world->GetProxyCount());+}+#endifdiff --git a/games/mrbullet/src/world.h b/games/mrbullet/src/world.hnew file mode 100644index 00000000..d7f92803--- /dev/null+++ b/games/mrbullet/src/world.h@@ -0,0 +1,85 @@+/* Mr Bullet: the physics, Box2D 2.3.1 set up as the game sets it up (world.cpp).+ *+ * Everything here is in the game's units (60 a metre) with y pointing down, as its levels are; world.cpp turns+ * them into Box2D's metres with y up. Bodies and joints are numbered; a body carries the game's object number. */+#ifndef MB_WORLD_H+#define MB_WORLD_H+#include <stdbool.h>+#include <stdint.h>++#ifdef __cplusplus+extern "C" {+#endif++#define W_BODIES 200+#define W_JOINTS 160+#define W_STEP (1.0f / 60.0f)++enum { WB_STATIC, WB_DYNAMIC, WB_KINEMATIC };+/* collision categories, as the game's collision matrix: 3 with 3 and 4 with 4 never touch */+enum { WC_NONE, WC_STATIC, WC_LEVEL, WC_CHARACTER, WC_BULLET, WC_GRENADE, WC_SENSOR = 7 };++typedef struct {+ float density, friction, restitution;+ uint8_t category, sensor;+} WFix;++bool world_begin(void); /* a fresh world: gravity 9.81 m/s2 down */+void world_end(void);+/* a body (-1 when the world is full): its object number `obj` comes back in the contacts */+int world_body(int type, float x, float y, float angle, int obj, bool bullet, bool fixed_rotation, float gravity);+bool world_poly(int b, const float *xy, int n, const WFix *f); /* xy: local, n points */+/* a static outline: a loop of edges (its points: local, n of them, used up) */+bool world_chain(int b, float *xy, int n, const WFix *f);+bool world_box(int b, float cx, float cy, float hw, float hh, const WFix *f);+bool world_circle(int b, float cx, float cy, float r, const WFix *f);+void world_destroy(int b);+bool world_alive(int b);+int world_obj(int b);+/* joints (-1 when full): anchors in world units */+int world_revolute(int a, int b, float x, float y, bool limit, float lo, float hi, bool motor, float speed,+ float torque);+int world_rope(int a, int b, float ax, float ay, float bx, float by, float length);+void world_destroy_joint(int j);+bool world_joint_alive(int j);+void world_joint_anchors(int j, float *ax, float *ay, float *bx, float *by);+void world_set_limits(int j, bool on, float lo, float hi);++void world_step(void);+/* where it is: position and the sine and cosine of its turn (screen sense) */+void world_xf(int b, float *x, float *y, float *s, float *c);+float world_angle(int b);+void world_velocity(int b, float *vx, float *vy);+void world_set_velocity(int b, float vx, float vy);+void world_set_angular(int b, float w);+float world_angular(int b);+void world_set_transform(int b, float x, float y, float angle);+void world_impulse(int b, float ix, float iy, float px, float py); /* units: kg m/s, at a world point */+void world_angular_impulse(int b, float i);+float world_mass(int b);+bool world_awake(int b);+bool world_dynamic(int b);+void world_set_type(int b, int type);+void world_wake(int b);+void world_set_fixed_rotation(int b, bool on);+/* Box2D's memory: in use, and the limit past which it makes no new contact (0: none) */+extern long b2_heap_now, b2_heap_peak, b2_heap_limit;+/* the first fixture a ray from (x0, y0) to (x1, y1) meets, of the categories in `mask` (1 << category):+ its body, the point and the normal; -1 for none */+int world_ray(float x0, float y0, float x1, float y1, unsigned mask, float *hx, float *hy, float *nx, float *ny);+/* every body the ray meets (once each), of the categories in `mask`, with where */+int world_ray_all(float x0, float y0, float x1, float y1, unsigned mask, int *out, float *xs, float *ys, int max);+/* the bodies whose boxes meet a box */+int world_query(float x0, float y0, float x1, float y1, int *out, int max);++/* the game's side (game.c): a contact begins (with how fast they meet, units/s), whether one holds, and the+ impulse it took */+void game_contact(int obj_a, int obj_b, int body_a, int body_b, float speed, float px, float py);+bool game_contact_enabled(int obj_a, int obj_b);++int world_bodies(void);+int world_contacts(void);+#ifdef __cplusplus+}+#endif+#endifdiff --git a/games/mrbullet/tests/play.c b/games/mrbullet/tests/play.cnew file mode 100644index 00000000..ad89d268--- /dev/null+++ b/games/mrbullet/tests/play.c@@ -0,0 +1,299 @@+/* Mr Bullet on a computer: the tests (make test) and pictures.+ *+ * build/play test DIR physics checks, a bot through the first levels of every mode, every level played+ * with random keys, saves; pictures of a few levels into DIR+ * build/play shot LEVEL OUT a picture of a level (PPM), after FRAMES steps with the aim at ANGLE (env) */+#include <math.h>+#include <stdio.h>+#include <stdlib.h>+#include <string.h>+#include "draw.h"+#include "game.h"+#include "plat.h"+#include "res.h"+#include "save.h"+#include "ui.h"+#include "world.h"++extern long b2_heap_now, b2_heap_peak;++/* ------------------------------------------------------------------ the platform */+static uint16_t screen[SCREEN_W * SCREEN_H];+static uint32_t clock_ms;+static char dir[512] = "build";+uint32_t plat_keys(void) { return 0; }+uint32_t plat_millis(void) { return clock_ms; }+void plat_sleep(uint32_t ms) { clock_ms += ms; }+void plat_push(int x, int y, int w, int h, const uint16_t *px) {+ for (int r = 0; r < h; r++) memcpy(&screen[(y + r) * SCREEN_W + x], &px[r * w], (size_t)w * 2);+}+void np_push(int x, int y, int w, int h, const color_t *px) { plat_push(x, y, w, h, px); }+void *np_alloc(uint32_t size) { (void)size; return NULL; } /* (gfx_init: not used) */+static uint8_t files[2][8192];+static uint32_t file_len[2];+static int file_of(const char *name) { return strcmp(name, MB_SAVE_NAME) ? 1 : 0; }+bool plat_save(const char *name, const void *data, uint32_t len) {+ int f = file_of(name);+ if (len > sizeof files[f]) return false;+ memcpy(files[f], data, len);+ file_len[f] = len;+ return true;+}+const uint8_t *plat_load(const char *name, uint32_t *len) {+ int f = file_of(name);+ if (!file_len[f]) return NULL;+ *len = file_len[f];+ return files[f];+}+void plat_begin(void) {}+int plat_end(void) { return 0; }++static uint64_t level_mem[LEVEL_MAX_BYTES / 8 + 1];+static uint64_t strip[SCREEN_W * STRIP_H * 2 / 8];+static uint64_t scratch[SAVE_SCRATCH / 8];++static void save_ppm(const char *path) {+ FILE *f = fopen(path, "wb");+ if (!f) return;+ fprintf(f, "P6\n%d %d\n255\n", SCREEN_W, SCREEN_H);+ for (int i = 0; i < SCREEN_W * SCREEN_H; i++) {+ uint16_t c = screen[i];+ uint8_t rgb[3] = {(uint8_t)((c >> 11) << 3 | (c >> 13)), (uint8_t)(((c >> 5) & 63) << 2 | ((c >> 9) & 3)),+ (uint8_t)((c & 31) << 3 | ((c >> 2) & 7))};+ fwrite(rgb, 1, 3, f);+ }+ fclose(f);+}++static void scene(void *ctx) {+ (void)ctx;+ draw_world(true);+ draw_hud("Level 1", "Left Right", g.grenades ? "power" : "turn");+}+static void picture(const char *name) {+ gfx_buf = (uint16_t *)strip;+ gfx_render(scene, NULL, 0, SCREEN_H);+ char path[700];+ snprintf(path, sizeof path, "%s/%s.ppm", dir, name);+ save_ppm(path);+}++/* ------------------------------------------------------------------ checks */+static int failures;+#define CHECK(c, ...) do { if (!(c)) { failures++; printf("FAIL " __VA_ARGS__); printf("\n"); } } while (0)++static bool finite_world(void) {+ for (int i = 0; i < g.nchar; i++) {+ float x, y, s, c;+ if (game_part(&g.ch[i], P_TORSO, &x, &y, &s, &c) && !(isfinite(x) && isfinite(y))) return false;+ }+ return true;+}++/* one shot at an angle (and power), then steps until the level ends */+static int try_shot(int level, float aim, float power, int *stars) {+ if (!game_load(level, (uint8_t *)level_mem)) return -1;+ g.aim = aim;+ g.ch[g.hero].left = cosf(aim) < 0;+ g.power = power;+ game_fire();+ for (int s = 0; s < 900 && g.state == ST_PLAY; s++) game_step();+ *stars = game_stars();+ return g.state;+}++static void physics(void) {+ /* the first level, untouched: everyone stays standing where they are */+ CHECK(game_load(0, (uint8_t *)level_mem), "level 1 loads");+ float x0[MAX_CHAR], y0[MAX_CHAR];+ for (int i = 0; i < g.nchar; i++) {+ float s, c;+ game_part(&g.ch[i], P_TORSO, &x0[i], &y0[i], &s, &c);+ }+ for (int s = 0; s < 600; s++) game_step();+ for (int i = 0; i < g.nchar; i++) {+ float x, y, s, c;+ game_part(&g.ch[i], P_TORSO, &x, &y, &s, &c);+ CHECK(g.ch[i].alive, "character %d stands", i);+ CHECK(fabsf(x - x0[i]) < 2 && fabsf(y - y0[i]) < 2, "character %d stays (%.1f %.1f)", i, x - x0[i], y - y0[i]);+ }+ CHECK(g.state == ST_PLAY, "nothing happens on its own");+ /* a bullet straight at the enemy: about 23 m/s, the level won with three stars */+ int stars = 0;+ int st = try_shot(0, g.ch[g.hero].left ? 3.14159f : 0, 0, &stars);+ CHECK(st == ST_WON && stars == 3, "level 1: one bullet ahead wins (state %d, %d stars)", st, stars);+ /* a bullet straight up comes back down? no: no gravity, it bounces off the ceiling or leaves */+ game_load(0, (uint8_t *)level_mem);+ g.aim = -1.5707963f;+ game_fire();+ float vx = 0, vy = 0;+ for (int i = 0; i < MAX_SHOT; i++)+ if (g.shot[i].body >= 0) world_velocity(g.shot[i].body, &vx, &vy);+ float v = sqrtf(vx * vx + vy * vy) / 60;+ CHECK(v > 22 && v < 24, "a bullet leaves at 23 m/s (%.2f)", v);+ game_step();+ CHECK(finite_world(), "the world stays finite");+ game_end();+}++/* the bot: every angle by half a degree (and a few powers for grenades) until one shot clears the level */+static int solve(int level, int *stars_out) {+ int best = -1, best_stars = -1;+ bool gren = game_load(level, (uint8_t *)level_mem) && g.grenades;+ int powers = gren ? 6 : 1;+ for (int p = 0; p < powers && best_stars < 3; p++)+ for (int a = 0; a < 720 && best_stars < 3; a++) {+ int stars;+ float aim = (a * 0.5f - 180) * 3.14159265f / 180;+ if (try_shot(level, aim, 0.25f + 0.15f * p, &stars) == ST_WON && stars > best_stars) best = a, best_stars = stars;+ }+ *stars_out = best_stars;+ return best;+}++static uint32_t rng = 12345;+static uint32_t rnd(void) { rng ^= rng << 13, rng ^= rng >> 17, rng ^= rng << 5; return rng; }++static int max_contacts, max_bodies;+void world_debug(void);+static void fuzz_level(int level) {+ if (!game_load(level, (uint8_t *)level_mem)) {+ failures++;+ printf("FAIL level %d loads\n", level + 1);+ return;+ }+ for (int s = 0; s < 700 && g.state == ST_PLAY; s++) {+ uint32_t r = rnd() % 16;+ if (r == 0) game_fire();+ else if (r < 4) game_aim_by(0.05f);+ else if (r < 7) game_aim_by(-0.05f);+ else if (r == 7) game_turn(rnd() & 1);+ else if (r == 8) g.power = (rnd() % 100) / 100.0f;+ game_step();+ if (world_contacts() > max_contacts) {+ max_contacts = world_contacts();+ if (getenv("DEBUGC") && max_contacts > atoi(getenv("DEBUGC"))) { world_debug(); exit(0); }+ }+ if (world_bodies() > max_bodies) max_bodies = world_bodies();+ if (!finite_world()) {+ failures++;+ printf("FAIL level %d: a character went to infinity\n", level + 1);+ break;+ }+ }+ if ((level % 7) == 0) {+ gfx_buf = (uint16_t *)strip;+ gfx_render(scene, NULL, 0, SCREEN_H);+ }+ game_end();+}++static void ui_fuzz(void) {+ ui_init((uint8_t *)level_mem, (uint16_t *)strip, scratch);+ static const uint32_t keys[] = {0, KB_OK, KB_UP, KB_DOWN, KB_LEFT, KB_RIGHT, KB_BACK, KB_OK | KB_UP};+ for (int f = 0; f < 40000; f++) {+ uint32_t k = keys[rnd() % 8];+ if (rnd() % 3) k = 0;+ clock_ms += 16;+ if (!ui_frame(k, clock_ms)) ui_init((uint8_t *)level_mem, (uint16_t *)strip, scratch);+ if (ui_dirty() && (f % 5) == 0) ui_draw();+ }+ ui_quit();+}++static void saves(void) {+ Progress p, q;+ save_scratch(scratch);+ memset(files, 0, sizeof files), file_len[0] = file_len[1] = 0;+ CHECK(!save_load(&p), "no save: a fresh start");+ p.stars[3] = 2, p.flags[3] = LV_CLEARED, p.last[1] = 330, p.guide = 1, p.mode = 1;+ CHECK(save_write(&p), "the save is written");+ CHECK(save_load(&q) && !memcmp(&p, &q, sizeof p), "the save comes back");+ files[0][20] ^= 0x55;+ CHECK(save_load(&q) && !memcmp(&p, &q, sizeof p), "a damaged save comes back from its copy");+ file_len[0] = 0, file_len[1] = 0;+ CHECK(!save_load(&q), "no files: a fresh start");+}++int main(int argc, char **argv) {+ if (argc >= 3 && !strcmp(argv[1], "shot")) {+ int level = atoi(argv[2]) - 1;+ const char *fr = getenv("FRAMES"), *an = getenv("ANGLE");+ if (argc >= 4) snprintf(dir, sizeof dir, "%s", argv[3]);+ if (!game_load(level, (uint8_t *)level_mem)) return 1;+ if (an) {+ game_aim_by(0);+ g.aim = (float)atof(an) * 3.14159265f / 180;+ g.ch[g.hero].left = cosf(g.aim) < 0;+ }+ int frames = fr ? atoi(fr) : 0;+ if (frames > 0) game_fire();+ for (int i = 0; i < frames; i++) game_step();+ char name[32];+ snprintf(name, sizeof name, "level%d", level + 1);+ picture(name);+ return 0;+ }+ if (argc >= 4 && !strcmp(argv[1], "bot")) {+ for (int l = atoi(argv[2]) - 1; l < atoi(argv[3]); l++) {+ int stars, a = solve(l, &stars);+ printf("level %d: %s", l + 1, a >= 0 ? "solved" : "not solved in one shot");+ if (a >= 0) printf(" at %.1f deg, %d stars", a * 0.5f - 180, stars);+ printf("\n");+ }+ return 0;+ }+ if (argc >= 2 && !strcmp(argv[1], "mem")) {+ /* Box2D's memory: loaded, then the most while played with random keys */+ for (int l = 0; l < LEVEL_COUNT; l++) {+ b2_heap_peak = b2_heap_now = 0;+ game_load(l, (uint8_t *)level_mem);+ long loaded = b2_heap_now;+ game_end();+ b2_heap_peak = b2_heap_now = 0;+ max_contacts = max_bodies = 0;+ fuzz_level(l);+ printf("%d %ld %ld %d %d\n", l + 1, loaded, b2_heap_peak, max_contacts, max_bodies);+ }+ return 0;+ }+ if (argc >= 3) snprintf(dir, sizeof dir, "%s", argv[2]);+ physics();+ printf("physics: done (%d failures)\n", failures);+ saves();+ /* the bot through the first levels of each mode */+ int solved = 0, tried = 0;+ for (int m = 0; m < MODE_COUNT; m++) {+ int first = res_mode_first(m), n = m == 0 ? 16 : 4;+ for (int l = first; l < first + n; l++) {+ int stars, a = solve(l, &stars);+ tried++;+ if (a >= 0) solved++;+ printf("level %d (mode %d #%d): %s", l + 1, m, l - first + 1, a >= 0 ? "solved" : "not solved in one shot");+ if (a >= 0) printf(" at %.1f deg, %d stars", a * 0.5f - 180, stars);+ printf("\n");+ }+ }+ printf("bot: %d of %d levels cleared with one shot\n", solved, tried);+ CHECK(solved * 10 >= tried * 8, "the bot clears most of the first levels with one shot");+ /* every level, random keys; Box2D's memory held as on the calculator (its blocks are bigger here: 64 bits) */+ b2_heap_limit = 120000;+ long peak = 0;+ for (int l = 0; l < LEVEL_COUNT; l++) {+ b2_heap_peak = b2_heap_now;+ fuzz_level(l);+ if (b2_heap_peak > peak) peak = b2_heap_peak;+ }+ printf("fuzz: %d levels, Box2D's memory at most %ld bytes\n", LEVEL_COUNT, peak);+ ui_fuzz();+ printf("ui fuzz: done\n");+ for (int l = 0; l < 3; l++) {+ game_load(l, (uint8_t *)level_mem);+ char name[32];+ snprintf(name, sizeof name, "level%d", l + 1);+ picture(name);+ game_end();+ }+ printf(failures ? "%d FAILURES\n" : "all passed\n", failures);+ return failures ? 1 : 0;+}diff --git a/games/mrbullet/tools/pack.py b/games/mrbullet/tools/pack.pynew file mode 100644index 00000000..e8e8cadd--- /dev/null+++ b/games/mrbullet/tools/pack.py@@ -0,0 +1,306 @@+"""Mr Bullet: src/data.bin and src/data.h from the game's own files.++python3 -I tools/pack.py REF (REF: the APK's assets folder: Levels/*.jg, Images/...)++Levels: the Classic, Grenades, Hostages and Friendly Fire worlds (Levels/world0, 1, 3, 4), each level its own raw+deflate stream of the format game.c reads (see level_bytes). Pictures: the city background cut to the play field,+the characters' torsos and heads, the interface's pictures; 16 colours each (the background 64)."""+import json, os, struct, sys, zlib+from PIL import Image++REF = sys.argv[1]+HERE = os.path.dirname(os.path.abspath(__file__))+SRC = os.path.join(HERE, '..', 'src')++# the play field: 440 x 568 of the game's units on 240 rows+SCALE = 240 / 568.0+SHEET = 7.5 # the body sheets' pixels for one unit (a torso: 216 pixels, 0.36 x 80 units)+MODES = [('world0', 20), ('world1', 7), ('world3', 7), ('world4', 9)]++FLAGS = ['ki', 'gki', 'hki', 'hgki', 'vi', 'hs', 'rh', 'si', 'b', 'f', 'co', 'be', 'hv', 'bo', 'br', 'by', 'sb',+ 'ig', 'lg', 'rg', 'c', 'fa', 'm', 'mcw']+++def q(v):+ r = int(round(v * 8))+ assert -32768 <= r < 32768, v+ return r+++def pts(out, flat, cx=0, cy=0):+ for i in range(0, len(flat), 2):+ out += struct.pack('<hh', q(flat[i]), q(flat[i + 1]))+++def area(pts):+ a = 0+ for i in range(len(pts)):+ x0, y0 = pts[i - 1]; x1, y1 = pts[i]+ a += x0 * y1 - x1 * y0+ return a / 2+++def hull(pts):+ pts = sorted(set(pts))+ if len(pts) < 3: return pts+ def cross(o, a, b): return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])+ lo, hi = [], []+ for p in pts:+ while len(lo) >= 2 and cross(lo[-2], lo[-1], p) <= 0: lo.pop()+ lo.append(p)+ for p in reversed(pts):+ while len(hi) >= 2 and cross(hi[-2], hi[-1], p) <= 0: hi.pop()+ hi.append(p)+ return lo[:-1] + hi[:-1]+++def simplify(pts, n):+ # Visvalingam: drop the point making the smallest triangle until n are left+ pts = list(pts)+ while len(pts) > n:+ best, bi = None, 0+ for i in range(len(pts)):+ a, b, c = pts[i - 1], pts[i], pts[(i + 1) % len(pts)]+ t = abs((b[0] - a[0]) * (c[1] - a[1]) - (c[0] - a[0]) * (b[1] - a[1]))+ if best is None or t < best: best, bi = t, i+ pts.pop(bi)+ return pts+++def convex_parts(p):+ """A dynamic shape that is convex (most: rounded boxes) as one polygon of at most 16 points, rather than the+ editor's fan of overlapping parts (their boxes overlap their neighbours' in a pile: hundreds of contacts)."""+ tp = p.get('tp')+ if not tp or tp.get('h'): return p['p']+ o = [(tp['p'][i], tp['p'][i + 1]) for i in range(0, len(tp['p']), 2)]+ h = hull(o)+ if len(o) < 3 or abs(area(h)) - abs(area(o)) > 0.002 * abs(area(h)): return p['p']+ s = simplify(h, 8)+ return [[v for xy in s for v in xy]]+++def split8(part):+ """a convex part of more than 8 points as fans of at most 8 (Box2D's polygons hold 8)"""+ pts = [(part[i], part[i + 1]) for i in range(0, len(part), 2)]+ if len(pts) <= 8: return [part]+ out, k = [], 1+ while k < len(pts) - 1:+ piece = [pts[0]] + pts[k:k + 7]+ out.append([v for xy in piece for v in xy])+ k += 6+ return out+++def level_bytes(c):+ j = c['j']+ polys = j['poly']+ pins = j.get('pins', [])+ sws = j.get('switches', [])+ out = bytearray(struct.pack('<BBBB', c['tr'], len(polys), len(pins), len(sws)))+ for p in polys:+ bits = 0+ for k, name in enumerate(FLAGS):+ if p.get(name): bits |= 1 << k+ parts = p['p']+ is_char = any(p.get(k) for k in ('ki', 'gki', 'hki', 'hgki', 'vi', 'hs'))+ if is_char and len(parts) == 1 and len(parts[0]) > 4:+ parts = [parts[0][:4]] # (a few have the editor's stray points after feet and head)+ if not is_char and not p.get('f') and not p.get('b') and len(parts) > 1:+ parts = convex_parts(p)+ if not is_char and not p.get('b') and (len(parts) > 1 or len(parts[0]) > 4):+ if p.get('f'):+ parts = [] # static: its outline is its shape (a loop of edges, game.c)+ else:+ parts = [q for part in parts for q in split8(part)]+ if not parts: kind = 0+ elif len(parts) == 1 and len(parts[0]) == 4: kind = 1+ elif len(parts) == 1 and len(parts[0]) == 2: kind = 2+ else: kind = 0+ cx, cy = p['ce']+ out += struct.pack('<BBI', kind, p['i'], bits)+ out += struct.pack('<hh', q(cx), q(cy))+ if kind == 1:+ pts(out, parts[0])+ elif kind == 2:+ pts(out, parts[0])+ else:+ out += struct.pack('<B', len(parts))+ for part in parts:+ assert 3 <= len(part) // 2 <= 8, len(part)+ out += struct.pack('<B', len(part) // 2)+ pts(out, part)+ tp = p.get('tp')+ outline = tp['p'] if tp else (parts[0] if len(parts) == 1 else p['v'][2:])+ holes = tp.get('h', []) if tp else []+ out += struct.pack('<H', len(outline) // 2)+ pts(out, outline)+ out += struct.pack('<B', len(holes))+ for h in holes:+ out += struct.pack('<B', len(h) // 2)+ pts(out, h)+ for p in pins:+ cx, cy = p['ce']+ v = p['v'][2:]+ out += struct.pack('<BbbB', p['t'], p.get('pia', -1), p.get('pib', -1), 1 if p.get('m') else 0)+ out += struct.pack('<hh', q(cx), q(cy))+ out += struct.pack('<B', len(v) // 2)+ pts(out, v)+ pts(out, p['la'] + p['lb'])+ out += struct.pack('<hhff', q(p.get('rl', 0)), q(p.get('as', 0)), p.get('ms', 0), p.get('mt', 0))+ for s in sws:+ cx, cy = s['ce']+ v = s['v'][2:]+ links = s.get('swi', [])+ out += struct.pack('<BBB', s['id'] & 255, s.get('swci', 0) & 255, len(links))+ out += bytes(links)+ out += struct.pack('<hh', q(cx), q(cy))+ pts(out, v[:4])+ return bytes(out)+++def deflate(b):+ z = zlib.compressobj(9, zlib.DEFLATED, -15, 9)+ return z.compress(b) + z.flush()+++# ---- pictures: 16 colours (index 0 clear) and 4 bits a pixel, or 64 colours a byte a pixel+def rgb565(r, g, b): return ((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3)+++def pic_bytes(im, colours=15, pivot=(0, 0), opaque=False):+ im = im.convert('RGBA')+ w, h = im.size+ alpha = im.getchannel('A')+ rgb = Image.new('RGB', im.size, (0, 0, 0))+ rgb.paste(im, mask=alpha)+ pq = rgb.quantize(colors=colours, method=Image.Quantize.MEDIANCUT, dither=Image.Dither.NONE)+ pal = pq.getpalette()[:colours * 3]+ idx = list(pq.tobytes()); a = list(alpha.tobytes())+ wide = colours > 15+ px = [0 if (a[i] < 128 and not opaque) else idx[i] + (0 if wide else 1) for i in range(w * h)]+ ncol = colours if wide else colours + 1+ palette = [0] * ncol+ for k in range(colours):+ palette[k + (0 if wide else 1)] = rgb565(*pal[3 * k:3 * k + 3])+ head = struct.pack('<HHhhBB', w, h, int(round(pivot[0] * 4)), int(round(pivot[1] * 4)), ncol,+ (1 if opaque else 0) | (2 if wide else 0))+ body = struct.pack('<%dH' % ncol, *palette)+ if wide:+ data = bytes(px)+ else:+ data = bytearray((w * h + 1) // 2)+ for i, k in enumerate(px):+ data[i >> 1] |= k << ((i & 1) * 4)+ return head + body + bytes(data)+++def img(path): return Image.open(os.path.join(REF, 'Images', path)).convert('RGBA')+++def scaled(im, f):+ w, h = im.size+ return im.resize((max(1, round(w * f)), max(1, round(h * f))), Image.LANCZOS)+++pics = [] # (name, bytes)+def add(name, b):+ pics.append((name, b)); return len(pics) - 1+++# the background: the phone shows it 834 units tall, its bottom 97 units under the field (field +-284 units)+bg = img('Backgrounds/background_city.png')+k = 834 / 900.0 # units a background pixel+top = (-284 + 834 - 97 - 284) / k # field top in background rows: the picture's top is at -453+x0 = 250 - 220 / k; x1 = 250 + 220 / k+crop = bg.crop((round(x0), round(top), round(x1), round(top + 568 / k)))+add('BG', pic_bytes(crop.resize((186, 240), Image.LANCZOS), colours=64, opaque=True))++# character pieces: torso and heads as pictures (the limbs are drawn as the sheet's colours)+F = SCALE / SHEET+SKINS = [('HERO', 'blue_suit_body.png', 'Killer/killer_head.png', 'Killer/killer_head.png'),+ ('ENEMY', 'black_suit_body.png', 'gangster_head.png', 'gangster_head_dead.png'),+ ('HOSTAGE', 'hostage_body.png', 'hostage_head.png', 'hostage_head_dead.png')]+colours = {}+for name, body, head, dead in SKINS:+ sheet = img('Characters/' + body)+ torso = sheet.crop((204, 50, 420, 286))+ t = scaled(torso, F)+ add(name + '_TORSO', pic_bytes(t, pivot=(t.size[0] / 2, t.size[1] / 2)))+ for tag, hp in (('HEAD', head), ('DEAD', dead)):+ hi = img('Characters/' + hp).crop((34, 44, 212, 208))+ s = scaled(hi, F)+ add(name + '_' + tag, pic_bytes(s, pivot=(s.size[0] / 2, s.size[1]))) # pivot: the neck++ def avg(box):+ reg = sheet.crop(box).convert('RGBA')+ raw = reg.tobytes()+ px = [tuple(raw[i:i + 4]) for i in range(0, len(raw), 4) if raw[i + 3] > 200]+ px.sort(key=lambda p: p[0] + p[1] + p[2])+ p = px[len(px) // 2]+ return rgb565(*p[:3])+ colours[name] = [avg((50, 60, 104, 180)), avg((520, 60, 574, 180)), avg((60, 360, 100, 385)),+ avg((204, 380, 258, 500)), avg((366, 380, 420, 500)), avg((210, 670, 280, 690))]+ # front sleeve, back sleeve, hand, front leg, back leg, shoe++# the interface+add('TITLE', pic_bytes(scaled(img('Misc/title.png'), 230 / 800.0)))+bullet = img('Misc/bullet.png')+add('BULLET', pic_bytes(scaled(bullet, 14 / 310.0), colours=7))+add('GRENADE', pic_bytes(scaled(img('Misc/grenade.png'), 12 / 140.0), colours=7))+star = img('Misc/star-md.png')+add('STAR', pic_bytes(scaled(star, 20 / 162.0), colours=7))+add('STAR_BIG', pic_bytes(scaled(star, 34 / 162.0), colours=11))+add('PADLOCK', pic_bytes(scaled(img('Misc/padlock.png'), 14 / img('Misc/padlock.png').size[1]), colours=7))++# the levels+levels = []+first = []+for world, chapters in MODES:+ first.append(len(levels))+ for ch in range(chapters):+ d = json.loads(zlib.decompress(open(os.path.join(REF, 'Levels', '%s-%d.jg' % (world, ch)), 'rb').read()))+ for c in d['cs']:+ levels.append(level_bytes(c))+first.append(len(levels))++blob = bytearray()+lvl_index = []+biggest = 0+for b in levels:+ z = deflate(b)+ lvl_index.append((len(blob), len(z), len(b)))+ biggest = max(biggest, len(b))+ blob += z+ while len(blob) % 4: blob += b'\0'+pic_index = []+pblob = bytearray()+for name, b in pics:+ pic_index.append(len(pblob))+ pblob += b+ while len(pblob) % 4: pblob += b'\0'++data = bytearray()+data += struct.pack('<IIII', len(levels), len(pics), 0, 0)+lo = 16+po = lo + 8 * len(levels)+bo = po + 4 * len(pics)+for off, n, u in lvl_index:+ data += struct.pack('<IHH', bo + off, n, u)+pb = bo + len(blob)+for off in pic_index:+ data += struct.pack('<I', pb + off)+data += blob + pblob+open(os.path.join(SRC, 'data.bin'), 'wb').write(data)++with open(os.path.join(SRC, 'data.h'), 'w') as h:+ h.write('/* Mr Bullet: what tools/pack.py put in data.bin (generated) */\n#ifndef MB_DATA_H\n#define MB_DATA_H\n')+ h.write('#define LEVEL_COUNT %d\n#define LEVEL_MAX_BYTES %d\n' % (len(levels), (biggest + 7) & ~7))+ h.write('#define MODE_COUNT %d\n' % len(MODES))+ h.write('#define MODE_FIRST {%s}\n' % ', '.join(str(x) for x in first))+ for k, (name, b) in enumerate(pics):+ h.write('#define PIC_%s %d\n' % (name, k))+ h.write('#define PIC_COUNT %d\n' % len(pics))+ for name, c in colours.items():+ h.write('#define COLOURS_%s {%s}\n' % (name, ', '.join('0x%04X' % x for x in c)))+ h.write('#endif\n')+print('levels', len(levels), 'packed', len(blob), 'biggest', biggest, 'pictures', len(pblob), 'data.bin', len(data))