diff --git a/demo/ChipmunkDemo.c b/demo/ChipmunkDemo.c
index 7f144e50..65d29297 100644
--- a/demo/ChipmunkDemo.c
+++ b/demo/ChipmunkDemo.c
@@ -240,7 +240,7 @@ DrawInfo()
 	cpFloat ke = 0.0f;
 	for(int i=0; i<bodies->num; i++){
 		cpBody *body = (cpBody *)bodies->arr[i];
-		if(body->m == INFINITY || body->i == INFINITY) continue;
+		if(isinf(body->m) || isinf(body->i)) continue;
 		
 		ke += body->m*cpvdot(body->v, body->v) + body->i*body->w*body->w;
 	}
@@ -475,7 +475,7 @@ Click(const sapp_event *event)
 			cpPointQueryInfo info = {0};
 			cpShape *shape = cpSpacePointQueryNearest(space, mouse_pos, radius, GRAB_FILTER, &info);
 			
-			if(shape && cpBodyGetMass(cpShapeGetBody(shape)) < INFINITY){
+			if(shape && isfinite(cpBodyGetMass(cpShapeGetBody(shape)))){
 				// Use the closest point on the surface if the click is outside of the shape.
 				cpVect nearest = (info.distance > 0.0f ? info.point : mouse_pos);
 				
diff --git a/include/chipmunk/chipmunk_private.h b/include/chipmunk/chipmunk_private.h
index e606ba16..9d510a81 100644
--- a/include/chipmunk/chipmunk_private.h
+++ b/include/chipmunk/chipmunk_private.h
@@ -329,7 +329,16 @@ cpConstraintNext(cpConstraint *node, cpBody *body)
 static inline cpArbiter *
 cpArbiterNext(cpArbiter *node, cpBody *body)
 {
-	return (node->body_a == body ? node->thread_a.next : node->thread_b.next);
+	// Check both body_a and body_b to handle cases where they may have been swapped
+	// after the arbiter was threaded into the body's list
+	if(node->body_a == body){
+		return node->thread_a.next;
+	} else if(node->body_b == body){
+		return node->thread_b.next;
+	} else {
+		// This should never happen in normal operation, but return NULL to prevent infinite loops
+		return NULL;
+	}
 }
 
 #define CP_BODY_FOREACH_ARBITER(bdy, var)\
diff --git a/include/chipmunk/cpBB.h b/include/chipmunk/cpBB.h
index 8fc87049..a13c7a8a 100644
--- a/include/chipmunk/cpBB.h
+++ b/include/chipmunk/cpBB.h
@@ -145,7 +145,8 @@ static inline cpFloat cpBBSegmentQuery(cpBB bb, cpVect a, cpVect b)
 /// Return true if the bounding box intersects the line segment with ends @c a and @c b.
 static inline cpBool cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b)
 {
-	return (cpBBSegmentQuery(bb, a, b) != INFINITY);
+	cpFloat result = cpBBSegmentQuery(bb, a, b);
+	return isfinite(result);
 }
 
 /// Clamp a vector to a bounding box.
diff --git a/src/cpBody.c b/src/cpBody.c
index 4ba4b494..d53785aa 100644
--- a/src/cpBody.c
+++ b/src/cpBody.c
@@ -20,6 +20,7 @@
  */
 
 #include <float.h>
+#include <math.h>
 #include <stdarg.h>
 
 #include "chipmunk/chipmunk_private.h"
@@ -104,7 +105,7 @@ cpBodyFree(cpBody *body)
 	#define	cpAssertSaneBody(body)
 #else
 	static void cpv_assert_nan(cpVect v, const char *message){cpAssertHard(v.x == v.x && v.y == v.y, message);}
-	static void cpv_assert_infinite(cpVect v, const char *message){cpAssertHard(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);}
+	static void cpv_assert_infinite(cpVect v, const char *message){cpAssertHard(!isinf(cpfabs(v.x)) && !isinf(cpfabs(v.y)), message);}
 	static void cpv_assert_sane(cpVect v, const char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);}
 	
 	static void
@@ -119,9 +120,9 @@ cpBodyFree(cpBody *body)
 		cpv_assert_sane(body->v, "Body's velocity is invalid.");
 		cpv_assert_sane(body->f, "Body's force is invalid.");
 
-		cpAssertHard(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid.");
-		cpAssertHard(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid.");
-		cpAssertHard(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid.");
+		cpAssertHard(body->a == body->a && !isinf(cpfabs(body->a)), "Body's angle is invalid.");
+		cpAssertHard(body->w == body->w && !isinf(cpfabs(body->w)), "Body's angular velocity is invalid.");
+		cpAssertHard(body->t == body->t && !isinf(cpfabs(body->t)), "Body's torque is invalid.");
 	}
 	
 	#define	cpAssertSaneBody(body) cpBodySanityCheck(body)
@@ -136,9 +137,9 @@ cpBodyIsSleeping(const cpBody *body)
 cpBodyType
 cpBodyGetType(cpBody *body)
 {
-	if(body->sleeping.idleTime == INFINITY){
+	if(isinf(body->sleeping.idleTime)){
 		return CP_BODY_TYPE_STATIC;
-	} else if(body->m == INFINITY){
+	} else if(isinf(body->m)){
 		return CP_BODY_TYPE_KINEMATIC;
 	} else {
 		return CP_BODY_TYPE_DYNAMIC;
@@ -254,7 +255,7 @@ void
 cpBodySetMass(cpBody *body, cpFloat mass)
 {
 	cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "You cannot set the mass of kinematic or static bodies.");
-	cpAssertHard(0.0f <= mass && mass < INFINITY, "Mass must be positive and finite.");
+	cpAssertHard(0.0f <= mass && isfinite(mass), "Mass must be positive and finite.");
 	
 	cpBodyActivate(body);
 	body->m = mass;
diff --git a/src/cpSpaceComponent.c b/src/cpSpaceComponent.c
index 7b2d6069..e718132a 100644
--- a/src/cpSpaceComponent.c
+++ b/src/cpSpaceComponent.c
@@ -20,6 +20,7 @@
  */
  
 #include <string.h>
+#include <math.h>
 
 #include "chipmunk/chipmunk_private.h"
 
@@ -220,7 +221,7 @@ ComponentActive(cpBody *root, cpFloat threshold)
 void
 cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
 {
-	cpBool sleep = (space->sleepTimeThreshold != INFINITY);
+	cpBool sleep = isfinite(space->sleepTimeThreshold);
 	cpArray *bodies = space->dynamicBodies;
 	
 #ifndef NDEBUG
diff --git a/src/cpSpaceStep.c b/src/cpSpaceStep.c
index 85cbb3d0..ec323fe4 100644
--- a/src/cpSpaceStep.c
+++ b/src/cpSpaceStep.c
@@ -19,6 +19,8 @@
  * SOFTWARE.
  */
 
+#include <math.h>
+
 #include "chipmunk/chipmunk_private.h"
 
 //MARK: Post Step Callback Functions
@@ -269,7 +271,7 @@ cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space)
 		!(a->sensor || b->sensor) &&
 		// Don't process collisions between two infinite mass bodies.
 		// This includes collisions between two kinematic bodies, or a kinematic body and a static body.
-		!(a->body->m == INFINITY && b->body->m == INFINITY)
+		!(isinf(a->body->m) && isinf(b->body->m))
 	){
 		cpArrayPush(space->arbiters, arb);
 	} else {