Skip to content

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.

Mr Bullet: first playable build

Mason36398ddcc85a5eb
diff --git a/games/mrbullet/.gitignore b/games/mrbullet/.gitignore
new file mode 100644
index 00000000..e48c9cd3
--- /dev/null
+++ b/games/mrbullet/.gitignore
@@ -0,0 +1,4 @@
+output/
+build/
+__pycache__/
+*.pyc
diff --git a/games/mrbullet/Makefile b/games/mrbullet/Makefile
new file mode 100644
index 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) build
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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];
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2Shape.h b/games/mrbullet/src/box2d/Box2D/Collision/Shapes/b2Shape.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2BroadPhase.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2CollideCircle.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2CollideCircle.cpp
new file mode 100644
index 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.cpp
new file mode 100644
index 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.cpp
new file mode 100644
index 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.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2Distance.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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];
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2DynamicTree.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+ }
+ }
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.cpp b/games/mrbullet/src/box2d/Box2D/Collision/b2TimeOfImpact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2BlockAllocator.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Draw.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Draw.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2GrowableStack.h b/games/mrbullet/src/box2d/Box2D/Common/b2GrowableStack.h
new file mode 100644
index 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;
+};
+
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Math.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Math.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Settings.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Settings.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2StackAllocator.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Timer.cpp b/games/mrbullet/src/box2d/Box2D/Common/b2Timer.cpp
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Common/b2Timer.h b/games/mrbullet/src/box2d/Box2D/Common/b2Timer.h
new file mode 100644
index 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
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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);
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2Joint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MotorJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Body.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2ContactManager.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Fixture.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2Island.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2TimeStep.h b/games/mrbullet/src/box2d/Box2D/Dynamics/b2TimeStep.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2World.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+}
+
+#endif
diff --git a/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp b/games/mrbullet/src/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp
new file mode 100644
index 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.h
new file mode 100644
index 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;
+};
+
+#endif
diff --git a/games/mrbullet/src/box2d/License.txt b/games/mrbullet/src/box2d/License.txt
new file mode 100644
index 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.S
new file mode 100644
index 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
+#endif
diff --git a/games/mrbullet/src/data.bin b/games/mrbullet/src/data.bin
new file mode 100644
index 00000000..2ff73631
Binary files /dev/null and b/games/mrbullet/src/data.bin differ
diff --git a/games/mrbullet/src/data.h b/games/mrbullet/src/data.h
new file mode 100644
index 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}
+#endif
diff --git a/games/mrbullet/src/draw.c b/games/mrbullet/src/draw.c
new file mode 100644
index 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.h
new file mode 100644
index 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);
+#endif
diff --git a/games/mrbullet/src/game.c b/games/mrbullet/src/game.c
new file mode 100644
index 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, &shy, &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.h
new file mode 100644
index 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);
+#endif
diff --git a/games/mrbullet/src/icon.png b/games/mrbullet/src/icon.png
new file mode 100644
index 00000000..201cf9d5
Binary files /dev/null and b/games/mrbullet/src/icon.png differ
diff --git a/games/mrbullet/src/inflate.c b/games/mrbullet/src/inflate.c
new file mode 100644
index 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.h
new file mode 100644
index 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);
+#endif
diff --git a/games/mrbullet/src/main.c b/games/mrbullet/src/main.c
new file mode 100644
index 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();
+}
+#endif
diff --git a/games/mrbullet/src/plat.h b/games/mrbullet/src/plat.h
new file mode 100644
index 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);
+#endif
diff --git a/games/mrbullet/src/plat_eadk.c b/games/mrbullet/src/plat_eadk.c
new file mode 100644
index 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.c
new file mode 100644
index 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.h
new file mode 100644
index 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);
+#endif
diff --git a/games/mrbullet/src/save.c b/games/mrbullet/src/save.c
new file mode 100644
index 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.h
new file mode 100644
index 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);
+#endif
diff --git a/games/mrbullet/src/ui.c b/games/mrbullet/src/ui.c
new file mode 100644
index 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.h
new file mode 100644
index 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 */
+#endif
diff --git a/games/mrbullet/src/world.cpp b/games/mrbullet/src/world.cpp
new file mode 100644
index 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());
+}
+#endif
diff --git a/games/mrbullet/src/world.h b/games/mrbullet/src/world.h
new file mode 100644
index 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
+#endif
diff --git a/games/mrbullet/tests/play.c b/games/mrbullet/tests/play.c
new file mode 100644
index 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.py
new file mode 100644
index 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))