From e7223a520b12663127b21afe8a68965898f34ded Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 25 Nov 2022 21:41:31 +0200
Subject: [PATCH] Fix various cases of bad math

---
 examples/bassteroids/source/physics.cpp       | 26 ++++++++++++++++---
 .../bassteroids/source/playercontroller.cpp   |  6 +++--
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 612a3ec..67cdf6f 100644
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -186,7 +186,9 @@ void Physics::apply_impulses()
 		LinAl::Vector<float, 2> v_p1 = entity1.velocity+LinAl::Vector<float, 2>(-r1.y, r1.x)*entity1.angular_velocity.radians();
 		LinAl::Vector<float, 2> v_p2 = entity2.velocity+LinAl::Vector<float, 2>(-r2.y, r2.x)*entity2.angular_velocity.radians();
 		LinAl::Vector<float, 2> v_rel = v_p2-v_p1;
-		LinAl::Vector<float, 2> tangent = normalize(v_rel-c.normal*inner_product(v_rel, c.normal));
+		LinAl::Vector<float, 2> tangent = v_rel-c.normal*inner_product(v_rel, c.normal);
+		float v_tan = tangent.norm();
+		tangent = (v_tan>1e-5 ? normalize(tangent) : LinAl::Vector<float, 2>(-c.normal.y, c.normal.x));
 		float restitution = 1.0f;
 		float friction_coeff = 0.1f;
 		float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
@@ -249,11 +251,29 @@ void Physics::collide_circle_box(unsigned i, unsigned j)
 	LinAl::Vector<float, 2> local_closest(clamp(local_delta.x, -half_size.x, half_size.x), clamp(local_delta.y, -half_size.y, half_size.y));
 	LinAl::Vector<float, 2> local_cdelta = local_delta-local_closest;
 	float d_sq = inner_product(local_cdelta, local_cdelta);
+
 	if(d_sq<radius*radius)
 	{
 		Collision &collision = get_collision(i, j);
-		collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
-		collision.depth = radius-sqrt(d_sq);
+		if(d_sq>1e-10)
+		{
+			collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
+			collision.depth = radius-sqrt(d_sq);
+		}
+		else
+		{
+			LinAl::Vector<float, 2> inside_dist(half_size.x-abs(local_delta.x), half_size.y-abs(local_delta.y));
+			if(inside_dist.x<inside_dist.y)
+			{
+				collision.normal = LinAl::Vector<float, 2>(c, s) * (local_delta.x<0 ? -1.0f : 1.0f);
+				collision.depth = radius+inside_dist.x;
+			}
+			else
+			{
+				collision.normal = LinAl::Vector<float, 2>(-s, c) * (local_delta.y<0 ? -1.0f : 1.0f);
+				collision.depth = radius+inside_dist.y;
+			}
+		}
 		collision.point = pos1-collision.normal*(radius-collision.depth/2);
 		if(collision.body1!=i)
 			collision.normal = -collision.normal;
diff --git a/examples/bassteroids/source/playercontroller.cpp b/examples/bassteroids/source/playercontroller.cpp
index 606f34e..cd7e72a 100644
--- a/examples/bassteroids/source/playercontroller.cpp
+++ b/examples/bassteroids/source/playercontroller.cpp
@@ -38,10 +38,12 @@ void PlayerController::tick(Time::TimeDelta dt)
 	body->add_force(fwd_dir*(controls->forward.get_value()*thrust));
 	const LinAl::Vector<float, 2> &velocity = body->get_velocity();
 	float speed = velocity.norm();
-	body->add_force(velocity*-min(speed+setup.speed/speed, 1.0f/dt_secs));
+	if(speed>1e-5)
+		body->add_force(velocity*-min(speed+setup.speed/speed, 1.0f/dt_secs));
 
 	body->add_torque(controls->turn_left.get_value()*torque);
 	Geometry::Angle<float> angular_vel = body->get_angular_velocity();
 	Geometry::Angle<float> angular_speed = abs(angular_vel);
-	body->add_torque(angular_vel.radians()*-min(angular_speed.radians()+setup.turn_rate/angular_speed.radians(), 1.0f/dt_secs));
+	if(abs(angular_speed.radians())>1e-5)
+		body->add_torque(angular_vel.radians()*-min(angular_speed.radians()+setup.turn_rate/angular_speed.radians(), 1.0f/dt_secs));
 }
-- 
2.43.0