diff --git a/src/pathfinding.rs b/src/pathfinding.rs
index 129742b..0f9bf7a 100644
--- a/src/pathfinding.rs
+++ b/src/pathfinding.rs
@@ -1,7 +1,7 @@
 use std::collections::HashMap;
 
 use bevy::{prelude::*, tasks::prelude::*};
-use bevy_rapier2d::prelude::*;
+use bevy_rapier2d::{na::UnitComplex, prelude::*};
 use crossbeam_channel::{unbounded, Receiver};
 use derive_more::{Deref, DerefMut};
 use pathfinding::prelude::*;
@@ -41,43 +41,6 @@ pub fn find_path(
     )
 }
 
-fn nearest_extreme(from: f32, to: i32) -> f32 {
-    let to = to as f32;
-    let range = to..=(to + 0.999);
-    if from <= *range.start() {
-        *range.start()
-    } else {
-        *range.end()
-    }
-}
-
-fn cheat_assign(
-    start: (f32, f32),
-    end: (i32, i32),
-    map_width: usize,
-    motion_blocked: Vec<bool>,
-) -> Option<(f32, f32)> {
-    let x;
-    let y;
-    if start.0 as i32 == end.0 {
-        x = start.0;
-    } else {
-        x = nearest_extreme(start.0, end.0);
-    }
-    if start.1 as i32 == end.1 {
-        y = start.1;
-    } else {
-        y = nearest_extreme(start.1, end.1);
-    }
-    let point = (x, y);
-    let index = point.to_index(map_width);
-    if motion_blocked[index] {
-        None
-    } else {
-        Some(point)
-    }
-}
-
 fn calculate_path(
     mut commands: Commands,
     pool: Res<AsyncComputeTaskPool>,
@@ -117,93 +80,62 @@ fn calculate_path(
     }
 }
 
-/*fn negotiate_path(
+fn negotiate_path(
     mut commands: Commands,
-    time: Res<Time>,
     mut query: Query<(
         Entity,
         &mut Path,
-        &mut Coordinates,
+        &mut RigidBodyPosition,
         &mut RigidBodyVelocity,
         &Speed,
         Option<&RotationSpeed>,
-        &mut Transform,
     )>,
-    map: Query<&Map>,
 ) {
-    for (entity, mut path, mut coordinates, mut velocity, speed, rotation_speed, mut transform) in
-        query.iter_mut()
-    {
-        for map in map.iter() {
-            let mut new_path = path.0.clone();
-            let start_i32 = coordinates.i32();
-            let new_path_clone = new_path.clone();
-            let mut iter = new_path_clone.split(|p| *p == start_i32);
-            if iter.next().is_some() {
-                if let Some(upcoming) = iter.next() {
-                    new_path = vec![start_i32];
-                    new_path.append(&mut upcoming.to_vec());
-                }
-            }
-            **path = new_path;
-            if path.len() >= 2 {
-                let start = **coordinates;
-                let start_index = start.to_index(map.width());
-                let start = Vec2::new(start.0, start.1);
-                let next = path[1];
-                if motion_blocked[next.to_index(map.width())] {
-                    // TODO: Should probably handle.
-                }
-                let next = Vec2::new(next.0 as f32, next.1 as f32);
-                if rotation_speed.is_some() {
-                    let start = start.floor();
-                    let v = next - start;
-                    let angle = v.y.atan2(v.x);
-                    transform.rotation = Quat::from_rotation_z(angle);
-                }
-                let mut direction = next - start;
-                direction = direction.normalize();
-                direction *= speed.0;
-                let displacement = direction * time.delta_seconds();
-                let dest = start + displacement;
-                let dest = (dest.x, dest.y);
-                let index = dest.to_index(map.width());
-                if start_index != index && motion_blocked[index] {
-                    let (normal_x, normal_y) = **coordinates;
-                    let next = path[1];
-                    if let Some((cheat_x, cheat_y)) =
-                        cheat_assign(**coordinates, next, map.width(), motion_blocked.0.clone())
-                    {
-                        let index = (normal_x, cheat_y).to_index(map.width());
-                        if !motion_blocked.0[index] {
-                            **coordinates = (normal_x, cheat_y);
-                            return;
-                        }
-                        let index = (cheat_x, normal_y).to_index(map.width());
-                        if !motion_blocked.0[index] {
-                            **coordinates = (cheat_x, normal_y);
-                            return;
-                        }
-                        **coordinates = (cheat_x, cheat_y);
-                    }
-                    velocity.linvel = Vec2::ZERO.into();
-                } else {
-                    velocity.linvel = direction.into();
-                }
-            } else {
-                commands.entity(entity).remove::<Path>();
-                commands.entity(entity).remove::<Destination>();
-                velocity.linvel = Vec2::ZERO.into();
+    for (entity, mut path, mut position, mut velocity, speed, rotation_speed) in query.iter_mut() {
+        let mut new_path = path.0.clone();
+        let start_i32 = (
+            position.position.translation.x,
+            position.position.translation.y,
+        )
+            .i32();
+        let new_path_clone = new_path.clone();
+        let mut iter = new_path_clone.split(|p| *p == start_i32);
+        if iter.next().is_some() {
+            if let Some(upcoming) = iter.next() {
+                new_path = vec![start_i32];
+                new_path.append(&mut upcoming.to_vec());
             }
         }
+        **path = new_path;
+        if path.len() >= 2 {
+            let start = Vec2::new(
+                position.position.translation.x,
+                position.position.translation.y,
+            );
+            let next = path[1];
+            let next = Vec2::new(next.0 as f32, next.1 as f32);
+            if rotation_speed.is_some() {
+                let v = next - start;
+                let angle = v.y.atan2(v.x);
+                position.position.rotation = UnitComplex::new(angle);
+            }
+            let mut direction = next - start;
+            direction = direction.normalize();
+            direction *= speed.0;
+            velocity.linvel = direction.into();
+        } else {
+            commands.entity(entity).remove::<Path>();
+            commands.entity(entity).remove::<Destination>();
+            velocity.linvel = Vec2::ZERO.into();
+        }
     }
-}*/
+}
 
 pub struct PathfindingPlugin;
 
 impl Plugin for PathfindingPlugin {
     fn build(&self, app: &mut AppBuilder) {
-        app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system());
-        //.add_system(negotiate_path.system());
+        app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system())
+            .add_system(negotiate_path.system());
     }
 }