diff --git a/src/engine/surface_collision.c b/src/engine/surface_collision.c
index 749b99bc..6fcf28df 100644
--- a/src/engine/surface_collision.c
+++ b/src/engine/surface_collision.c
@@ -44,9 +44,8 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode, struc
     register f32 invDenom;
     register f32 v, w;
     register f32 margin_radius = (radius - 1.0f);
-
+    register TerrainData type = SURFACE_DEFAULT;
     s32 numCols = 0;
-
 // #if EXTENDED_BOUNDS_MODE
 //     const float down_scale = (1.0f / gWorldScale);
 //     radius *= down_scale;
@@ -55,73 +54,56 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode, struc
 //     z *= down_scale;
 //     margin_radius *= down_scale;
 // #endif
-
     // Max collision radius = 200
     if (radius > 200.0f) {
         radius = 200.0f;
     }
-
     // Stay in this loop until out of walls.
     while (surfaceNode != NULL) {
-        surf = surfaceNode->surface;
+        surf        = surfaceNode->surface;
         surfaceNode = surfaceNode->next;
-
+        type        = surf->type;
         // Exclude a large number of walls immediately to optimize.
-        if (pos[1] < surf->lowerY || pos[1] > surf->upperY) {
-            continue;
-        }
-
-        offset = (surf->normal.x * pos[0]) + (surf->normal.y * pos[1]) + (surf->normal.z * pos[2]) + surf->originOffset;
-
-        if (offset < -radius || offset > radius) {
-            continue;
-        }
-
+        if ((type == SURFACE_NEW_WATER) || (type == SURFACE_NEW_WATER_BOTTOM)) continue;
         // Determine if checking for the camera or not.
         if (gCheckingSurfaceCollisionsForCamera) {
-            if (surf->flags & SURFACE_FLAG_NO_CAM_COLLISION || surf->type == SURFACE_NEW_WATER || surf->type == SURFACE_NEW_WATER_BOTTOM) {
+            if (surf->flags & SURFACE_FLAG_NO_CAM_COLLISION) {
                 continue;
             }
         } else {
             // Ignore camera only surfaces.
-            if (surf->type == SURFACE_CAMERA_BOUNDARY || surf->type == SURFACE_NEW_WATER || surf->type == SURFACE_NEW_WATER_BOTTOM) {
+            if (type == SURFACE_CAMERA_BOUNDARY) {
                 continue;
             }
-
             // If an object can pass through a vanish cap wall, pass through.
-            if (surf->type == SURFACE_VANISH_CAP_WALLS) {
+            if (type == SURFACE_VANISH_CAP_WALLS && gCurrentObject != NULL) {
                 // If an object can pass through a vanish cap wall, pass through.
-                if (gCurrentObject != NULL
-                    && (gCurrentObject->activeFlags & ACTIVE_FLAG_MOVE_THROUGH_GRATE)) {
+                if (gCurrentObject->activeFlags & ACTIVE_FLAG_MOVE_THROUGH_GRATE) {
                     continue;
                 }
-
                 // If Mario has a vanish cap, pass through the vanish cap wall.
-                if (gCurrentObject != NULL && gCurrentObject == gMarioObject
-                    && (gMarioState->flags & MARIO_VANISH_CAP)) {
+                if (gCurrentObject == gMarioObject && (gMarioState->flags & MARIO_VANISH_CAP)) {
                     continue;
                 }
             }
         }
-
-        v0[0] = (f32)(surf->vertex2[0] - surf->vertex1[0]);
-        v0[1] = (f32)(surf->vertex2[1] - surf->vertex1[1]);
-        v0[2] = (f32)(surf->vertex2[2] - surf->vertex1[2]);
-
-        v1[0] = (f32)(surf->vertex3[0] - surf->vertex1[0]);
-        v1[1] = (f32)(surf->vertex3[1] - surf->vertex1[1]);
-        v1[2] = (f32)(surf->vertex3[2] - surf->vertex1[2]);
-
-        v2[0] = pos[0] - (f32)surf->vertex1[0];
-        v2[1] = pos[1] - (f32)surf->vertex1[1];
-        v2[2] = pos[2] - (f32)surf->vertex1[2];
-
+        if (pos[1] < surf->lowerY || pos[1] > surf->upperY) {
+            continue;
+        }
+        // Dot of normal and pos, + origin offset
+        offset = (surf->normal.x * pos[0]) + (surf->normal.y * pos[1]) + (surf->normal.z * pos[2]) + surf->originOffset;
+        if (offset < -radius || offset > radius) {
+            continue;
+        }
+        vec3_diff(v0, surf->vertex2, surf->vertex1);
+        vec3_diff(v1, surf->vertex3, surf->vertex1);
+        vec3_diff(v2, pos,           surf->vertex1);
         // Face
-        d00 = (v0[0] * v0[0]) + (v0[1] * v0[1]) + (v0[2] * v0[2]);
-        d01 = (v0[0] * v1[0]) + (v0[1] * v1[1]) + (v0[2] * v1[2]);
-        d11 = (v1[0] * v1[0]) + (v1[1] * v1[1]) + (v1[2] * v1[2]);
-        d20 = (v2[0] * v0[0]) + (v2[1] * v0[1]) + (v2[2] * v0[2]);
-        d21 = (v2[0] * v1[0]) + (v2[1] * v1[1]) + (v2[2] * v1[2]);
+        d00 = vec3_dot(v0, v0);
+        d01 = vec3_dot(v0, v1);
+        d11 = vec3_dot(v1, v1);
+        d20 = vec3_dot(v2, v0);
+        d21 = vec3_dot(v2, v1);
         invDenom = 1.0f / ((d00 * d11) - (d01 * d01));
         v = ((d11 * d20) - (d01 * d21)) * invDenom;
         if (v < 0.0f || v > 1.0f) {
@@ -134,22 +116,14 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode, struc
         pos[0] += surf->normal.x * (radius - offset);
         pos[2] += surf->normal.z * (radius - offset);
         goto hasCollision;
-
     edge_1_2:
         if (offset < 0) continue;
         CALC_OFFSET(v0, goto edge_1_3);
     edge_1_3:
         CALC_OFFSET(v1, goto edge_2_3);
     edge_2_3:
-        // Edge 2-3
-        v1[0] = (f32)(surf->vertex3[0] - surf->vertex2[0]);
-        v1[1] = (f32)(surf->vertex3[1] - surf->vertex2[1]);
-        v1[2] = (f32)(surf->vertex3[2] - surf->vertex2[2]);
-
-        v2[0] = pos[0] - (f32)surf->vertex2[0];
-        v2[1] = pos[1] - (f32)surf->vertex2[1];
-        v2[2] = pos[2] - (f32)surf->vertex2[2];
-
+        vec3_diff(v1, surf->vertex3, surf->vertex2);
+        vec3_diff(v2, pos, surf->vertex2);
         CALC_OFFSET(v1, continue);
     check_collision:
         invDenom = offset / invDenom;
@@ -166,19 +140,15 @@ static s32 find_wall_collisions_from_list(struct SurfaceNode *surfaceNode, struc
         if (data->numWalls < MAX_REFEREMCED_WALLS) {
             data->walls[data->numWalls++] = surf;
         }
-
         numCols++;
     }
-
 // #if EXTENDED_BOUNDS_MODE
 //     x *= gWorldScale;
 //     y *= gWorldScale;
 //     z *= gWorldScale;
 // #endif
-
     data->x = pos[0];
     data->z = pos[2];
-
     return numCols;
 }
 
@@ -222,10 +192,7 @@ s32 find_wall_collisions(struct WallCollisionData *colData) {
 
     colData->numWalls = 0;
 
-    if (x <= -LEVEL_BOUNDARY_MAX || x >= LEVEL_BOUNDARY_MAX) {
-        return numCollisions;
-    }
-    if (z <= -LEVEL_BOUNDARY_MAX || z >= LEVEL_BOUNDARY_MAX) {
+    if (is_outside_level_bounds(x, z)) {
         return numCollisions;
     }
 
@@ -243,7 +210,7 @@ s32 find_wall_collisions(struct WallCollisionData *colData) {
     numCollisions += find_wall_collisions_from_list(node, colData);
 
     // Increment the debug tracker.
-    gNumCalls.wall += 1;
+    gNumCalls.wall++;
 
     #if PUPPYPRINT_DEBUG
     collisionTime[perfIteration] += osGetTime()-first;
@@ -286,64 +253,59 @@ void add_ceil_margin(s32 *x, s32 *z, Vec3s target1, Vec3s target2, f32 margin) {
  * Iterate through the list of ceilings and find the first ceiling over a given point.
  */
 static struct Surface *find_ceil_from_list(struct SurfaceNode *surfaceNode, s32 x, s32 y, s32 z, f32 *pheight) {
-    register struct Surface *surf;
-    s32 x1, z1, x2, z2, x3, z3;
-    f32 nx, ny, nz, oo, height;
     const f32 margin = 1.5f;
+    register struct Surface *surf;
+    Vec3i vx, vz;
+    f32 height;
     struct Surface *ceil = NULL;
     *pheight = CELL_HEIGHT_LIMIT;
     // Stay in this loop until out of ceilings.
     while (surfaceNode != NULL) {
         surf = surfaceNode->surface;
         surfaceNode = surfaceNode->next;
-        if (y > surf->upperY)
+        if (y > surf->upperY) {
             continue;
-        x1 = surf->vertex1[0];
-        z1 = surf->vertex1[2];
-		if (surf->type != SURFACE_HANGABLE) {
-			add_ceil_margin(&x1, &z1, surf->vertex2, surf->vertex3, margin);
         }
-        z2 = surf->vertex2[2];
-        x2 = surf->vertex2[0];
+        vx[0] = surf->vertex1[0];
+        vz[0] = surf->vertex1[2];
+		if (surf->type != SURFACE_HANGABLE) {
+			add_ceil_margin(&vx[0], &vz[0], surf->vertex2, surf->vertex3, margin);
+        }
+        vx[1] = surf->vertex2[0];
+        vz[1] = surf->vertex2[2];
         if (surf->type != SURFACE_HANGABLE) {
-			add_ceil_margin(&x2, &z2, surf->vertex3, surf->vertex1, margin);
+			add_ceil_margin(&vx[1], &vz[1], surf->vertex3, surf->vertex1, margin);
         }
         // Checking if point is in bounds of the triangle laterally.
-        if ((z1 - z) * (x2 - x1) - (x1 - x) * (z2 - z1) > 0) {
+        if ((vz[0] - z) * (vx[1] - vx[0]) - (vx[0] - x) * (vz[1] - vz[0]) > 0) {
             continue;
         }
         // Slight optimization by checking these later.
-        x3 = surf->vertex3[0];
-        z3 = surf->vertex3[2];
+        vx[2] = surf->vertex3[0];
+        vz[2] = surf->vertex3[2];
 		if (surf->type != SURFACE_HANGABLE) {
-			add_ceil_margin(&x3, &z3, surf->vertex1, surf->vertex2, margin);
+			add_ceil_margin(&vx[2], &vz[2], surf->vertex1, surf->vertex2, margin);
         }
-        if ((z2 - z) * (x3 - x2) - (x2 - x) * (z3 - z2) > 0) {
+        if ((vz[1] - z) * (vx[2] - vx[1]) - (vx[1] - x) * (vz[2] - vz[1]) > 0) {
             continue;
         }
-        if ((z3 - z) * (x1 - x3) - (x3 - x) * (z1 - z3) > 0) {
+        if ((vz[2] - z) * (vx[0] - vx[2]) - (vx[2] - x) * (vz[0] - vz[2]) > 0) {
             continue;
         }
         // Determine if checking for the camera or not.
-        if (gCheckingSurfaceCollisionsForCamera != 0) {
-            if (surf->flags & SURFACE_FLAG_NO_CAM_COLLISION || surf->type == SURFACE_NEW_WATER || surf->type == SURFACE_NEW_WATER_BOTTOM) {
-                continue;
-            }
-        }
-        // Ignore camera only surfaces.
-        else if (surf->type == SURFACE_CAMERA_BOUNDARY || surf->type == SURFACE_NEW_WATER || surf->type == SURFACE_NEW_WATER_BOTTOM) {
+        if (surf->type == SURFACE_NEW_WATER || surf->type == SURFACE_NEW_WATER_BOTTOM) {
             continue;
         }
-        nx = surf->normal.x;
-        ny = surf->normal.y;
-        nz = surf->normal.z;
-        oo = surf->originOffset;
-        // If a wall, ignore it. Likely a remnant, should never occur.
-        if (ny == 0.0f) {
+        if (gCheckingSurfaceCollisionsForCamera) {
+            if (surf->flags & SURFACE_FLAG_NO_CAM_COLLISION) {
+                continue;
+            }
+        } else if (surf->type == SURFACE_CAMERA_BOUNDARY) {
+            // Ignore camera only surfaces.
             continue;
         }
         // Find the ceil height at the specific point.
-        height = -(x * nx + nz * z + oo) / ny;
+        height = get_surface_height_at_location(x, z, surf);
         if (height > *pheight) {
             continue;
         }
@@ -382,10 +344,7 @@ f32 find_ceil(f32 posX, f32 posY, f32 posZ, struct Surface **pceil) {
     z = posZ;
     *pceil = NULL;
 
-    if (x <= -LEVEL_BOUNDARY_MAX || x >= LEVEL_BOUNDARY_MAX) {
-        return height;
-    }
-    if (z <= -LEVEL_BOUNDARY_MAX || z >= LEVEL_BOUNDARY_MAX) {
+    if (is_outside_level_bounds(x, z)) {
         return height;
     }
 
@@ -409,7 +368,7 @@ f32 find_ceil(f32 posX, f32 posY, f32 posZ, struct Surface **pceil) {
     *pceil = ceil;
 
     // Increment the debug tracker.
-    gNumCalls.ceil += 1;
+    gNumCalls.ceil++;
 
     #if PUPPYPRINT_DEBUG
     collisionTime[perfIteration] += osGetTime()-first;
@@ -436,31 +395,32 @@ f32 unused_obj_find_floor_height(struct Object *obj) {
  */
 static struct Surface *find_floor_from_list(struct SurfaceNode *surfaceNode, s32 x, s32 y, s32 z, f32 *pheight) {
     register struct Surface *surf;
-    register s32 x1, z1, x2, z2, x3, z3;
-    f32 nx, ny, nz, oo, height;
+    register Vec3i vx, vz;
+    f32 height;
     struct Surface *floor = NULL;
     *pheight = FLOOR_LOWER_LIMIT;
     // Iterate through the list of floors until there are no more floors.
     while (surfaceNode != NULL) {
         surf = surfaceNode->surface;
         surfaceNode = surfaceNode->next;
-        if (y < surf->lowerY - 30)
+        if (y < surf->lowerY - 30) {
             continue;
-        x1 = surf->vertex1[0];
-        z1 = surf->vertex1[2];
-        x2 = surf->vertex2[0];
-        z2 = surf->vertex2[2];
+        }
+        vx[0] = surf->vertex1[0];
+        vz[0] = surf->vertex1[2];
+        vx[1] = surf->vertex2[0];
+        vz[1] = surf->vertex2[2];
         // Check that the point is within the triangle bounds.
-        if ((z1 - z) * (x2 - x1) - (x1 - x) * (z2 - z1) < 0) {
+        if ((vz[0] - z) * (vx[1] - vx[0]) - (vx[0] - x) * (vz[1] - vz[0]) < 0) {
             continue;
         }
         // To slightly save on computation time, set this later.
-        x3 = surf->vertex3[0];
-        z3 = surf->vertex3[2];
-        if ((z2 - z) * (x3 - x2) - (x2 - x) * (z3 - z2) < 0) {
+        vx[2] = surf->vertex3[0];
+        vz[2] = surf->vertex3[2];
+        if ((vz[1] - z) * (vx[2] - vx[1]) - (vx[1] - x) * (vz[2] - vz[1]) < 0) {
             continue;
         }
-        if ((z3 - z) * (x1 - x3) - (x3 - x) * (z1 - z3) < 0) {
+        if ((vz[2] - z) * (vx[0] - vx[2]) - (vx[2] - x) * (vz[0] - vz[2]) < 0) {
             continue;
         }
         // Determine if we are checking for the camera or not.
@@ -468,21 +428,12 @@ static struct Surface *find_floor_from_list(struct SurfaceNode *surfaceNode, s32
             if (surf->flags & SURFACE_FLAG_NO_CAM_COLLISION || surf->type == SURFACE_NEW_WATER || surf->type == SURFACE_NEW_WATER_BOTTOM) {
                 continue;
             }
-        }
-        // If we are not checking for the camera, ignore camera only floors.
-        else if (surf->type == SURFACE_CAMERA_BOUNDARY) {
+        } else if (surf->type == SURFACE_CAMERA_BOUNDARY) {
+            // If we are not checking for the camera, ignore camera only floors.
             continue;
         }
-        nx = surf->normal.x;
-        ny = surf->normal.y;
-        nz = surf->normal.z;
-        oo = surf->originOffset;
-		// If a wall, ignore it. Likely a remnant, should never occur.
-		if (ny == 0.0f) {
-			continue;
-		}
         // Find the height of the floor at a given location.
-        height = -(x * nx + nz * z + oo) / ny;
+        height = get_surface_height_at_location(x, z, surf);
         if (height < *pheight) {
             continue;
         }
@@ -501,33 +452,23 @@ static struct Surface *find_floor_from_list(struct SurfaceNode *surfaceNode, s32
 
 
 static s16 check_within_triangle_bounds(s32 x, s32 z, struct Surface *surf) {
-    register s32 x1, z1, x2, z2, x3, z3;
-    x1 = surf->vertex1[0];
-    z1 = surf->vertex1[2];
-    x2 = surf->vertex2[0];
-    z2 = surf->vertex2[2];
-
-    if ((z1 - z) * (x2 - x1) - (x1 - x) * (z2 - z1) < 0) return FALSE;
-
-    x3 = surf->vertex3[0];
-    z3 = surf->vertex3[2];
-
-    if ((z2 - z) * (x3 - x2) - (x2 - x) * (z3 - z2) < 0) return FALSE;
-    if ((z3 - z) * (x1 - x3) - (x3 - x) * (z1 - z3) < 0) return FALSE;
-
+    register Vec3i vx, vz;
+    vx[0] = surf->vertex1[0];
+    vz[0] = surf->vertex1[2];
+    vx[1] = surf->vertex2[0];
+    vz[1] = surf->vertex2[2];
+    if ((vz[0] - z) * (vx[1] - vx[0]) - (vx[0] - x) * (vz[1] - vz[0]) < 0) return FALSE;
+    vx[2] = surf->vertex3[0];
+    vz[2] = surf->vertex3[2];
+    if ((vz[1] - z) * (vx[2] - vx[1]) - (vx[1] - x) * (vz[2] - vz[1]) < 0) return FALSE;
+    if ((vz[2] - z) * (vx[0] - vx[2]) - (vx[2] - x) * (vz[0] - vz[2]) < 0) return FALSE;
     return TRUE;
 }
 
-// Find the height of the floor at a given location
-static f32 get_floor_height_at_location(s32 x, s32 z, struct Surface *surf) {
-    return -(x * surf->normal.x + surf->normal.z * z + surf->originOffset) / surf->normal.y;
-}
-
 /**
  * Iterate through the list of water floors and find the first water floor under a given point.
  */
-struct Surface *find_water_floor_from_list(struct SurfaceNode *surfaceNode, s32 x, s32 y, s32 z,
-                                            s32 *pheight) {
+struct Surface *find_water_floor_from_list(struct SurfaceNode *surfaceNode, s32 x, s32 y, s32 z, s32 *pheight) {
     register struct Surface *surf;
     struct Surface *floor = NULL;
     struct SurfaceNode *topSurfaceNode = surfaceNode;
@@ -543,7 +484,7 @@ struct Surface *find_water_floor_from_list(struct SurfaceNode *surfaceNode, s32
 
         if (surf->type != SURFACE_NEW_WATER_BOTTOM || !check_within_triangle_bounds(x, z, surf)) continue;
 
-        curBottomHeight = get_floor_height_at_location(x, z, surf);
+        curBottomHeight = get_surface_height_at_location(x, z, surf);
 
         if (curBottomHeight < y - 78.0f) continue;
         if (curBottomHeight >= y - 78.0f) bottomHeight = curBottomHeight;
@@ -557,7 +498,7 @@ struct Surface *find_water_floor_from_list(struct SurfaceNode *surfaceNode, s32
 
         if (surf->type == SURFACE_NEW_WATER_BOTTOM || !check_within_triangle_bounds(x, z, surf)) continue;
 
-        curHeight = get_floor_height_at_location(x, z, surf);
+        curHeight = get_surface_height_at_location(x, z, surf);
 
         if (bottomHeight != FLOOR_LOWER_LIMIT && curHeight > bottomHeight) continue;
 
@@ -632,13 +573,7 @@ f32 find_floor(f32 xPos, f32 yPos, f32 zPos, struct Surface **pfloor) {
 
     *pfloor = NULL;
 
-    if (x <= -LEVEL_BOUNDARY_MAX || x >= LEVEL_BOUNDARY_MAX) {
-        #if PUPPYPRINT_DEBUG
-        collisionTime[perfIteration] += osGetTime()-first;
-        #endif
-        return height;
-    }
-    if (z <= -LEVEL_BOUNDARY_MAX || z >= LEVEL_BOUNDARY_MAX) {
+    if (is_outside_level_bounds(x, z)) {
         #if PUPPYPRINT_DEBUG
         collisionTime[perfIteration] += osGetTime()-first;
         #endif
@@ -675,7 +610,7 @@ f32 find_floor(f32 xPos, f32 yPos, f32 zPos, struct Surface **pfloor) {
 
     // If a floor was missed, increment the debug counter.
     if (floor == NULL) {
-        gNumFindFloorMisses += 1;
+        gNumFindFloorMisses++;
     }
 
     if (dynamicHeight > height) {
@@ -686,7 +621,7 @@ f32 find_floor(f32 xPos, f32 yPos, f32 zPos, struct Surface **pfloor) {
     *pfloor = floor;
 
     // Increment the debug tracker.
-    gNumCalls.floor += 1;
+    gNumCalls.floor++;
 
     #if PUPPYPRINT_DEBUG
     collisionTime[perfIteration] += osGetTime()-first;
@@ -710,10 +645,7 @@ s32 find_water_floor(s32 xPos, s32 yPos, s32 zPos, struct Surface **pfloor) {
     s32 y = yPos;
     s32 z = zPos;
 
-    if (x <= -LEVEL_BOUNDARY_MAX || x >= LEVEL_BOUNDARY_MAX) {
-        return height;
-    }
-    if (z <= -LEVEL_BOUNDARY_MAX || z >= LEVEL_BOUNDARY_MAX) {
+    if (is_outside_level_bounds(x, z)) {
         return height;
     }
 
diff --git a/src/engine/surface_collision.h b/src/engine/surface_collision.h
index 56f5abdf..cfa87987 100644
--- a/src/engine/surface_collision.h
+++ b/src/engine/surface_collision.h
@@ -11,8 +11,13 @@
 #define FLOOR_LOWER_LIMIT           -11000
 #define FLOOR_LOWER_LIMIT_MISC      (FLOOR_LOWER_LIMIT + 1000)
 // same as FLOOR_LOWER_LIMIT_MISC, explicitly for shadow.c
-// It doesn't match if ".0" is removed or ".f" is added
-#define FLOOR_LOWER_LIMIT_SHADOW    (FLOOR_LOWER_LIMIT + 1000.0)
+#define FLOOR_LOWER_LIMIT_SHADOW    (FLOOR_LOWER_LIMIT + 1000)
+
+#define is_outside_level_bounds(x, z) (((x) <= -LEVEL_BOUNDARY_MAX) || ((x) >=  LEVEL_BOUNDARY_MAX) || ((z) <= -LEVEL_BOUNDARY_MAX) || ((z) >=  LEVEL_BOUNDARY_MAX))
+
+#define get_surface_height_at_location(x, z, surf) (-((x) * (surf)->normal.x + (surf)->normal.z * (z) + (surf)->originOffset) / (surf)->normal.y);
+
+#define SURFACE_YAW(s) (atan2s(((s)->normal.z), ((s)->normal.x)))
 
 struct WallCollisionData
 {