diff --git a/.github/workflows/macos_builds.yml b/.github/workflows/macos_builds.yml
index bbdd809..0960316 100644
--- a/.github/workflows/macos_builds.yml
+++ b/.github/workflows/macos_builds.yml
@@ -8,7 +8,7 @@ env:
   SCONSFLAGS: verbose=yes warnings=extra werror=yes debug_symbols=no
 
 jobs:
-  ios:
+  macos:
     runs-on: "macos-latest"
     name: macOS Build ${{ matrix.target }} ${{ matrix.arch }}
     strategy:
diff --git a/.github/workflows/runner.yml b/.github/workflows/runner.yml
index 8655499..7000b86 100644
--- a/.github/workflows/runner.yml
+++ b/.github/workflows/runner.yml
@@ -1,5 +1,8 @@
 name: 🔗 Builds
-on: [push, pull_request]
+on: 
+  push:
+  pull_request:
+  workflow_dispatch:
 
 jobs:
   static-checks:
@@ -25,6 +28,7 @@ jobs:
     name: 🍎 macOS
     needs: static-checks
     uses: ./.github/workflows/macos_builds.yml
+    secrets: inherit
 
   windows-build:
     name: 🏁 Windows
diff --git a/.gitmodules b/.gitmodules
index 4e6d582..d18ec66 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -6,3 +6,6 @@
 	path = box2d
 	url = https://github.com/godot-box2d/box2d
 	branch = common-assert-noop
+[submodule "Godot-Physics-Tests"]
+	path = Godot-Physics-Tests
+	url = https://github.com/godot-box2d/Godot-Physics-Tests
diff --git a/CHANGELOG.md b/CHANGELOG.md
new file mode 100644
index 0000000..ac6de82
--- /dev/null
+++ b/CHANGELOG.md
@@ -0,0 +1,13 @@
+# Changelog
+
+## [v0.2](https://github.com/godot-box2d/godot-box2d/releases/tag/v0.2)
+
+- Fix CharacterController2D to correctly work(passing 19/20 tests)
+- Fix normal issue on test_motion function
+
+## [v0.1](https://github.com/godot-box2d/godot-box2d/releases/tag/v0.1)
+
+- Add all api's from physics server
+
+Known issues:
+- KinematicBody2D has a lot of issues still.
diff --git a/Godot-Physics-Tests b/Godot-Physics-Tests
new file mode 160000
index 0000000..0d6ea45
--- /dev/null
+++ b/Godot-Physics-Tests
@@ -0,0 +1 @@
+Subproject commit 0d6ea450c3a28bd0533d62cd159efd0296f688f2
diff --git a/README.md b/README.md
index 6edab42..29af823 100644
--- a/README.md
+++ b/README.md
@@ -7,33 +7,14 @@ A [box2D](https://github.com/erincatto/box2d) physics server for [Godot Engine](
 
 Based of [rburing/physics_server_box2d](https://github.com/rburing/physics_server_box2d).
 
-## Features
-
-Bodies:
-- [x] Rigid Body
-- [] Kinematic Body
-- [x] Static Body
-- [x] Area
-
-Joints:
-- [x] Pin Joint
-- [x] Damped Spring Joint
-- [x] Groove Joint
-
-Shapes:
-- [x] Capsule Shape
-- [x] Circle Shape
-- [x] Concave Polygon Shape
-- [x] Convex Polygon Shape
-- [x] Rectangle Shape
-- [x] Segment Shape
-- [x] Separation Ray Shape
-- [x] World Boundary Shape
-
-Direct State:
-- [x] Direct Body State
-- [x] Direct Space State
+## Missing/Not implemented
 
+- Skewed shapes
+- Scaled shapes
+- Constant speed on static bodies
+- Collision layers and masks don't work exactly the same (having non symetric layer/mask)
+- Body pickable
+- Torque uses wrong values
 
 ## Install from binaries
 
@@ -45,9 +26,6 @@ Currently it's built automatically for:
 - iOS (arm64)
 - Android (arm64 + x86_64)
 
-NOTE: the builds are not signed right now, so you might get a warning if you download for mac for eg.
-
-
 Go to any action workflow on this project: [Actions List](https://github.com/rburing/physics_server_box2d/actions)
 
 1. [Download latest release](https://github.com/godot-box2d/godot-box2d/releases/latest) from github job
@@ -69,21 +47,7 @@ Go to any action workflow on this project: [Actions List](https://github.com/rbu
        cd godot-cpp
        scons target=template_debug generate_bindings=yes
 
-4. Hack to disable b2Assert. Run:
-
-On linux:
-
-```
-sed -i 's/#define b2Assert(A) assert(A)/#define b2Assert(A) ((void)(A))/g' ./box2d/include/box2d/b2_common.h
-```
-
-On macos:
-
-```
-sed -i '' 's/#define b2Assert(A) assert(A)/#define b2Assert(A) ((void)(A))/g' ./box2d/include/box2d/b2_common.h
-```
-
-5. Compile the GDExtension for the same `target` as above:
+4. Compile the GDExtension for the same `target` as above:
 
        cd ..
        scons target=template_debug generate_bindings=no
diff --git a/build.sh b/build.sh
new file mode 100755
index 0000000..8ba8726
--- /dev/null
+++ b/build.sh
@@ -0,0 +1,3 @@
+scons target=template_debug generate_bindings=no
+rm -rf ./Godot-Physics-Tests/addons
+cp -r dist/addons ./Godot-Physics-Tests/addons
diff --git a/scripts/ci-sign-macos.ps1 b/scripts/ci-sign-macos.ps1
index bf26ecf..6b6592e 100644
--- a/scripts/ci-sign-macos.ps1
+++ b/scripts/ci-sign-macos.ps1
@@ -1,4 +1,5 @@
 #!/usr/bin/env pwsh
+# Taken from https://github.com/godot-jolt/godot-jolt/blob/master/scripts/ci_sign_macos.ps1
 
 #Requires -PSEdition Core
 #Requires -Version 7.2
diff --git a/src/servers/physics_server_box2d.cpp b/src/servers/physics_server_box2d.cpp
index 51c58e2..4ae0ce4 100644
--- a/src/servers/physics_server_box2d.cpp
+++ b/src/servers/physics_server_box2d.cpp
@@ -146,6 +146,7 @@ bool PhysicsServerBox2D::_shape_collide(const RID &p_shape_A, const Transform2D
 			if (output.collision) {
 				sweep_results.append(output);
 			}
+			memfree(b2_shape_B);
 		}
 	}
 	auto *results = static_cast<Vector2 *>(p_results);
@@ -995,17 +996,17 @@ bool PhysicsServerBox2D::_body_test_motion(const RID &p_body, const Transform2D
 	if (!sweep_test_result.collision) {
 		current_result.travel = p_motion;
 		current_result.remainder = Vector2();
-		current_result.collision_safe_fraction = 1;
-		current_result.collision_unsafe_fraction = 1;
+		current_result.collision_safe_fraction = 0;
+		current_result.collision_unsafe_fraction = 0;
 		return false;
 	}
 	current_result.collider = sweep_test_result.sweep_shape_B.fixture->GetUserData().shape->get_self();
-	current_result.collider_id = sweep_test_result.sweep_shape_B.fixture->GetBody()->GetUserData().collision_object->get_b2Body()->GetUserData().collision_object->get_object_instance_id();
+	current_result.collider_id = sweep_test_result.sweep_shape_B.fixture->GetBody()->GetUserData().collision_object->get_object_instance_id();
 	current_result.collision_point = box2d_to_godot(sweep_test_result.manifold.points[0]);
-	current_result.collision_normal = Vector2(sweep_test_result.manifold.normal.x, sweep_test_result.manifold.normal.y);
+	current_result.collision_normal = -Vector2(sweep_test_result.manifold.normal.x, sweep_test_result.manifold.normal.y);
 	current_result.collider_velocity = box2d_to_godot(sweep_test_result.sweep_shape_B.fixture->GetBody()->GetUserData().collision_object->get_b2Body()->GetLinearVelocity());
 	current_result.collision_safe_fraction = sweep_test_result.safe_fraction();
-	current_result.collision_unsafe_fraction = sweep_test_result.unsafe_fraction();
+	current_result.collision_unsafe_fraction = sweep_test_result.unsafe_fraction(current_result.collision_safe_fraction);
 	current_result.travel = p_motion * current_result.collision_safe_fraction;
 	current_result.remainder = p_motion - current_result.travel;
 	int shape_A_index = 0;
diff --git a/src/shapes/box2d_shape_convex_polygon.cpp b/src/shapes/box2d_shape_convex_polygon.cpp
index 10ea031..88b2d43 100644
--- a/src/shapes/box2d_shape_convex_polygon.cpp
+++ b/src/shapes/box2d_shape_convex_polygon.cpp
@@ -109,9 +109,9 @@ int Box2DShapeConvexPolygon::remove_bad_points(b2Vec2 *vertices, int32 count) {
 
 	// Find the right most point on the hull
 	int32 i0 = 0;
-	double x0 = ps[0].x;
+	float x0 = ps[0].x;
 	for (int32 i = 1; i < n; ++i) {
-		double x = ps[i].x;
+		float x = ps[i].x;
 		if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) {
 			i0 = i;
 			x0 = x;
@@ -139,7 +139,7 @@ int Box2DShapeConvexPolygon::remove_bad_points(b2Vec2 *vertices, int32 count) {
 
 			b2Vec2 r = ps[ie] - ps[hull[m]];
 			b2Vec2 v = ps[j] - ps[hull[m]];
-			double c = b2Cross(r, v);
+			float c = b2Cross(r, v);
 			if (c < 0.0f) {
 				ie = j;
 			}
diff --git a/src/spaces/box2d_direct_space_state.cpp b/src/spaces/box2d_direct_space_state.cpp
index 17edb52..abf8b32 100644
--- a/src/spaces/box2d_direct_space_state.cpp
+++ b/src/spaces/box2d_direct_space_state.cpp
@@ -125,8 +125,8 @@ bool Box2DDirectSpaceState::_cast_motion(const RID &shape_rid, const Transform2D
 	if (!sweep_test_result.collision) {
 		return true;
 	}
-	*closest_unsafe = sweep_test_result.unsafe_fraction();
 	*closest_safe = sweep_test_result.safe_fraction();
+	*closest_unsafe = sweep_test_result.unsafe_fraction(*closest_safe);
 	return true;
 }
 bool Box2DDirectSpaceState::_collide_shape(const RID &shape_rid, const Transform2D &transform, const Vector2 &motion, double margin, uint32_t collision_mask, bool collide_with_bodies, bool collide_with_areas, void *results, int32_t max_results, int32_t *result_count) {
@@ -162,7 +162,7 @@ bool Box2DDirectSpaceState::_rest_info(const RID &shape_rid, const Transform2D &
 	result_instance.rid = sweep_test_result.sweep_shape_B.fixture->GetUserData().shape->get_self();
 	result_instance.collider_id = sweep_test_result.sweep_shape_B.fixture->GetBody()->GetUserData().collision_object->get_object_instance_id();
 	result_instance.point = transform.get_origin() + box2d_to_godot(sweep_test_result.manifold.points[0]);
-	result_instance.normal = box2d_to_godot(sweep_test_result.manifold.normal);
+	result_instance.normal = -Vector2(sweep_test_result.manifold.normal.x, sweep_test_result.manifold.normal.y);
 	result_instance.linear_velocity = box2d_to_godot(sweep_test_result.sweep_shape_B.fixture->GetBody()->GetLinearVelocity());
 	return true;
 }
diff --git a/src/spaces/box2d_sweep_test.cpp b/src/spaces/box2d_sweep_test.cpp
index c26cedf..e0b926a 100644
--- a/src/spaces/box2d_sweep_test.cpp
+++ b/src/spaces/box2d_sweep_test.cpp
@@ -19,14 +19,27 @@
 #include <box2d/b2_time_of_impact.h>
 
 real_t SweepTestResult::safe_fraction() {
-	float motion_length = (sweep_shape_A.sweep.c - sweep_shape_A.sweep.c0).Length();
-	float unsafe_length = (sweep_shape_A.transform.p - sweep_shape_A.sweep.c0).Length();
-	float separation_distance = (distance_output.pointA - sweep_shape_A.transform.p).Length();
-	float safe_length = unsafe_length - separation_distance;
-	return safe_length / motion_length;
+	b2Vec2 motion_normal = sweep_shape_A.sweep.c - sweep_shape_A.sweep.c0;
+	float motion_length = motion_normal.Normalize();
+	float unsafe_length = motion_length * toi_output.t;
+	b2Vec2 separation = distance_output.pointA - sweep_shape_A.transform.p;
+	float separation_distance = separation.Length();
+	// Vector projection https://math.stackexchange.com/questions/108980/projecting-a-point-onto-a-vector-2d
+	b2Vec2 projection = (b2Cross(separation, motion_normal)) * motion_normal;
+
+	float safe_length = unsafe_length - projection.Length();
+	float safe_fraction = safe_length / motion_length;
+	if (safe_fraction <= b2_epsilon) {
+		return 0;
+	}
+	return safe_fraction;
 }
-real_t SweepTestResult::unsafe_fraction() {
-	return toi_output.t;
+real_t SweepTestResult::unsafe_fraction(float safe_fraction) {
+	if (safe_fraction <= b2_epsilon) {
+		return 0;
+	}
+	float unsafe = safe_fraction + b2_linearSlop;
+	return unsafe;
 }
 
 b2Sweep Box2DSweepTest::create_b2_sweep(b2Transform p_transform, b2Vec2 p_center, b2Vec2 p_motion) {
@@ -130,14 +143,18 @@ b2DistanceOutput _call_b2_distance(b2Transform p_transformA, b2Shape *shapeA, b2
 	b2DistanceOutput output;
 	b2DistanceInput input;
 	b2SimplexCache cache;
+	cache.count = 0;
+	float margin = 0.01f;
 	input.proxyA.Set(shapeA, 0);
+	//input.proxyA.m_radius = margin;
 	input.proxyB.Set(shapeB, 0);
-	// TODO use margin
-	// input.useRadii = true;
+	//input.proxyB.m_radius = margin;
 	input.transformA = p_transformA;
-	input.transformA = p_transformB;
-	cache.count = 0;
+	input.transformB = p_transformB;
+	input.useRadii = true;
 	b2Distance(&output, &cache, &input);
+	b2PolygonShape *polyA = (b2PolygonShape *)shapeA;
+	b2PolygonShape *polyB = (b2PolygonShape *)shapeB;
 	return output;
 }
 
@@ -145,6 +162,9 @@ b2AABB get_shape_aabb(Box2DShape *shape, const b2Transform &shape_transform) {
 	b2AABB aabb;
 	b2AABB aabb_total;
 	bool first_time = true;
+	if (shape->get_b2Shape_count(false) == 0) {
+		ERR_FAIL_V_MSG(aabb_total, "Cannot get aabb of empty shape.");
+	}
 	Transform2D identity;
 	for (int i = 0; i < shape->get_b2Shape_count(false); i++) {
 		b2Shape *b2_shape = (shape->get_transformed_b2Shape(i, identity, false, false));
@@ -158,7 +178,7 @@ b2AABB get_shape_aabb(Box2DShape *shape, const b2Transform &shape_transform) {
 		memdelete(b2_shape);
 	}
 	if (!aabb.IsValid()) {
-		ERR_PRINT("aabb of shape is not valid.");
+		ERR_FAIL_V_MSG(aabb_total, "aabb of shape is not valid.");
 	}
 	return aabb_total;
 }
@@ -185,11 +205,13 @@ SweepTestResult Box2DSweepTest::shape_cast(SweepShape p_sweep_shape_A, b2Shape *
 			sweep_A.GetTransform(&p_sweep_shape_A.transform, toi_output.t);
 			sweep_B.GetTransform(&p_sweep_shape_B.transform, toi_output.t);
 			b2DistanceOutput distance_output = _call_b2_distance(p_sweep_shape_A.transform, shape_A, p_sweep_shape_B.transform, shape_B);
+			if (distance_output.distance > b2_epsilon) {
+				break;
+			}
 			IntersectionManifoldResult intersection = _evaluate_intersection_manifold(shape_A, 0, p_sweep_shape_A.transform, shape_B, 0, p_sweep_shape_B.transform);
 			b2Manifold local_manifold = intersection.manifold;
 
 			if (!intersection.intersecting()) {
-				WARN_PRINT("`test_motion_toi` failed intersection! Report this!");
 				break;
 			}
 			manifold.Initialize(&local_manifold, p_sweep_shape_A.transform, shape_A->m_radius, p_sweep_shape_B.transform, shape_B->m_radius);
@@ -199,7 +221,6 @@ SweepTestResult Box2DSweepTest::shape_cast(SweepShape p_sweep_shape_A, b2Shape *
 
 			const b2Vec2 normal = manifold.normal;
 			if (b2Dot(normal, motion) <= FLT_EPSILON) {
-				WARN_PRINT("`shape_cast` failed normal! Report this!");
 				break;
 			}
 
diff --git a/src/spaces/box2d_sweep_test.h b/src/spaces/box2d_sweep_test.h
index 49a788b..86fe3da 100644
--- a/src/spaces/box2d_sweep_test.h
+++ b/src/spaces/box2d_sweep_test.h
@@ -35,7 +35,7 @@ struct SweepTestResult {
 	b2WorldManifold manifold;
 	bool collision = false;
 	real_t safe_fraction();
-	real_t unsafe_fraction();
+	real_t unsafe_fraction(float safe_fraction);
 };
 
 class Box2DSweepTest {