Totally revamped the program, and made it faster.

8 files changed, 798 insertions, 315 deletions

diff --git a/Makefile b/Makefile
index 09649e1..cdc6662 100644
--- a/Makefile
+++ b/Makefile
@@ -10,11 +10,11 @@ endif
 CFLAGS = $(PCC_CFLAGS) $(CFLAGS_EXTRA)
 OBJS = clld.c
 OBJ_NAME = clld
-all : $(OBJS)
+all : clean $(OBJS)
 	$(CC) $(OBJS) $(CFLAGS) -o $(OBJ_NAME)
 test-mvmt :
 	$(CC) test-mvmt.c $(CFLAGS) -o test-mvmt
-benchmark:
+benchmark: clean
 	$(CC) clld-bench.c $(CFLAGS) -o clld-bench
 clean :
 	rm -f $(OBJ_NAME) test-mvmt clld-bench
diff --git a/README b/README
index 94c2e3e..65aa297 100644
--- a/README
+++ b/README
@@ -9,9 +9,9 @@ Contents/Index:
 		2.2. Installation	47
 		2.3. Running		59
 	3. Input Format			65
-	4. Output Format		91
-	5. License			117
-	6. Authors			125
+	4. Output Format		104
+	5. License			135
+	6. Authors			143
 
 1. Introduction
 
diff --git a/clld-bench.c b/clld-bench.c
index 85d7f22..84bc768 100644
--- a/clld-bench.c
+++ b/clld-bench.c
@@ -78,136 +78,335 @@ struct colis {
 	unsigned int flp : 1;
 };
 
-static int usage() {
-	printf(
-		"clld Version MFIYA.\n"
-		"Usage: clld [options]\n"
-		"Options:\n"
-		"	-c[type]	Force the collision type to be\n"
-		"			the specified type (THM=0, AABB=1).\n"
-		"	-v[level]	Sets the verbosity level (0-3).\n"
-		"	-h		What do you fucking think it does.\n"
-		"			Anyways, it brings up this cool help\n"
-		"			message.\n"
-	      );
-	return 0;
+static void usage() {
+	puts("clld Version MFIYA.");
+	puts("Usage: clld [options]");
+	puts("Options:");
+	puts("	-c[type]	Force the collision type to be");
+	puts("			the specified type (THM=0, AABB=1).");
+	puts("	-v[level]	Sets the verbosity level (0-3).");
+	puts("	-h		What do you fucking think it does.");
+	puts("			Anyways, it brings up this cool help");
+	puts("			message.");
+}
+
+/* Get the direction of a collision. */
+uint8_t isleft = 0;
+uint8_t isright = 0;
+uint8_t istop = 0;
+uint8_t isbottom = 0;
+uint8_t isinfloor_x = 0;
+uint8_t isinfloor_y = 0;
+
+uint8_t get_coldir(uint8_t flt, uint8_t col_type, int hmpeak, struct colis *cursor, struct colis *floor) {
+	uint8_t coldir = 0;
+	uint8_t isinfloor_left   = 0;
+	uint8_t isinfloor_right  = 0;
+	uint8_t isinfloor_top    = 0;
+	uint8_t isinfloor_bottom = 0;
+	switch (col_type) {
+		case AABB:
+			if (flt) {
+				isleft   = cursor->c2 > floor->c1-cursor->xvel-1;
+				isright  = cursor->c1 < floor->c2-cursor->xvel+1;
+				istop    = cursor->d2 > floor->d1-cursor->yvel-1;
+				isbottom = cursor->d1 < floor->d2-cursor->yvel+1;
+
+				isinfloor_left   = cursor->d2 > floor->d1;
+				isinfloor_right  = cursor->d1 < floor->d2;
+				isinfloor_top    = cursor->c2 > floor->c1;
+				isinfloor_bottom = cursor->c1 < floor->c2;
+			} else {
+				isleft   = cursor->a2 > floor->a1-cursor->xvel-1;
+				isright  = cursor->a1 < floor->a2-cursor->xvel+1;
+				istop    = cursor->b2 > floor->b1-cursor->yvel-1;
+				isbottom = cursor->b1 < floor->b2-cursor->yvel+1;
+
+				isinfloor_left   = cursor->b2 > floor->b1;
+				isinfloor_right  = cursor->b1 < floor->b2;
+				isinfloor_top    = cursor->a2 > floor->a1;
+				isinfloor_bottom = cursor->a1 < floor->a2;
+			}
+			isinfloor_x = isinfloor_left && isinfloor_right ;
+			isinfloor_y = isinfloor_top  && isinfloor_bottom;
+			break;
+		case THM:
+			if (flt) {
+				/* Did the cursor collide with the floor from the left? */
+				isleft   = cursor->c2 > floor->c1-cursor->xvel-1;
+				/* Did the cursor collide with the floor from the right? */
+				isright  = cursor->c1 < (floor->c2-cursor->w)-cursor->xvel+1;
+
+				/* Is the floor's heightmap vertically flipped? */
+				if (!floor->flp) {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->d2 > (floor->d2-hmpeak)-cursor->yvel;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->d1 < floor->d2-cursor->yvel+1;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left   = floor->d2-cursor->d2 < floor->heightmap[0] && cursor->xvel > 0;
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right  = floor->d2-cursor->d2 < floor->heightmap[floor->w-1] && cursor->xvel < 0;
+					/* Is the bottom of the cursor at, or above the floor's ground? */
+					isinfloor_top    = floor->d2-cursor->d2 >= 0;
+					/* Is the top of the cursor at, or below the floor's ground? */
+					isinfloor_bottom = floor->d2-cursor->d1 <= 0;
+				} else {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->d2 > floor->d1-cursor->yvel-1;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->d1 < (floor->d1+hmpeak)-cursor->yvel;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left   = floor->d1-cursor->d1 > -floor->heightmap[0] && cursor->xvel > 0;
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right  = floor->d1-cursor->d1 > -floor->heightmap[floor->w-1] && cursor->xvel < 0;
+					/* Is the bottom of the cursor at, or above the floor's height? */
+					isinfloor_top    = floor->d1-cursor->d2 >= 0 && cursor->yvel > 0;
+					/* Is the top of the cursor at, or above the floor's height? */
+					isinfloor_bottom = cursor->d1-floor->d1 >= 0;
+				}
+			} else {
+				/* Did the cursor collide with the floor from the left? */
+				isleft   = cursor->a2 > floor->a1-cursor->xvel-1;
+				/* Did the cursor collide with the floor from the right? */
+				isright  = cursor->a1 < (floor->a2-cursor->w)-cursor->xvel+1;
+
+				/* Is the floor's heightmap vertically flipped? */
+				if (!floor->flp) {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->b2 > (floor->b2-hmpeak)-cursor->yvel;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->b1 < floor->b2-cursor->yvel+1;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left  = floor->b2-cursor->b2 < floor->heightmap[0];
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right = floor->b2-cursor->b2 < floor->heightmap[floor->w-1];
+					/* Is the bottom of the cursor at, or above the floor's ground? */
+					isinfloor_top = floor->b2-cursor->b2 >= 0;
+					/* Is the top of the cursor at, or below the floor's ground? */
+					isinfloor_bottom = floor->b2-cursor->b1 <= 0;
+				} else {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->b2 > floor->b1-cursor->yvel-1;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->b1 < (floor->b1+hmpeak)-cursor->yvel;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left   = floor->b1-cursor->b1 > -floor->heightmap[0];
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right  = floor->b1-cursor->b1 > -floor->heightmap[floor->w-1];
+					/* Is the bottom of the cursor at, or above the floor's ground? */
+					isinfloor_top    = floor->b1-cursor->b2 >= 0 && cursor->yvel > 0;
+					/* Is the top of the cursor at, or below the floor's ground? */
+					isinfloor_bottom = cursor->b1-floor->b1 >= 0;
+				}
+			}
+			isinfloor_x = isinfloor_left ^ isinfloor_right ;
+			isinfloor_y = isinfloor_top  ^ isinfloor_bottom;
+			break;
+	}
+	if (isinfloor_x) {
+		coldir |= isleft   << 0;
+		coldir |= isright  << 1;
+	}
+	if (isinfloor_y) {
+		coldir |= istop    << 2;
+		coldir |= isbottom << 3;
+	}
+	return coldir;
 }
 
 /* Tile Heightmap Collision. */
-int thm(uint8_t flt, int hmpeak, struct colis cursor, struct colis floor) {
+int thm(int hmpeak, struct colis *cursor, struct colis *floor) {
 	int newy = 0;
-	if (!flt) {
-		cursor.clx = (cursor.a2 > floor.a1-(int)cursor.xvel-1 && cursor.b2 > floor.b1) && (cursor.a1 < (floor.a2-cursor.w)-(int)cursor.xvel+1 && cursor.b1 < floor.b2);
-		cursor.cly = (cursor.b2 >= floor.b1-(int)cursor.yvel && cursor.a2 >= floor.a1) && (cursor.b1 <= floor.b2-(int)cursor.yvel && cursor.a1 <= floor.a2);
-		uint8_t l;
-		uint8_t r;
-		uint8_t t;
-		uint8_t b;
-		if (!floor.flp) {
-			l = (cursor.a2 > floor.a1-(int)cursor.xvel-1 && floor.b2-cursor.b2 < (int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.a1 < (floor.a2-cursor.w)-(int)cursor.xvel+1 && floor.b2-cursor.b2 < (int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.b2 > (floor.b2-hmpeak)-(int)cursor.yvel && floor.b2-cursor.b2 >= 0);
-			b = (cursor.b1 < floor.b2-(int)cursor.yvel+1 && floor.b2-cursor.b1 <= 0);
-		} else {
-			l = (cursor.a2 > floor.a1-(int)cursor.xvel-1 && floor.b1-cursor.b1 > -(int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.a1 < (floor.a2-cursor.w)-(int)cursor.xvel+1 && floor.b1-cursor.b1 > -(int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.b2 > floor.b1-(int)cursor.yvel-1 && floor.b1-cursor.b2 >= 0 && cursor.yvel > 0);
-			b = (cursor.b1 < (floor.b1+hmpeak)-(int)cursor.yvel && cursor.b1-floor.b1 >= 0);
+	cursor->clx = (cursor->a2 > floor->a1-cursor->xvel-1 && cursor->b2 > floor->b1) && (cursor->a1 < (floor->a2-cursor->w)-cursor->xvel+1 && cursor->b1 < floor->b2);
+	cursor->cly = (cursor->b2 >= floor->b1-cursor->yvel && cursor->a2 >= floor->a1) && (cursor->b1 <= floor->b2-cursor->yvel && cursor->a1 <= floor->a2);
+	uint8_t coldir = get_coldir(0, THM, hmpeak, cursor, floor);
+
+	if (cursor->clx && (cursor->a2-floor->a1 < 0 || cursor->a2-floor->a1 > floor->w)) {
+		switch (coldir & 3) {
+			case 1: cursor->pos.x = (floor->pos.x-cursor->xvel)-cursor->w-1; break;
+			case 2: cursor->pos.x = (floor->a2-cursor->w)-cursor->xvel+1; break;
 		}
+		cursor->xvel = 0;
+		floor->xvel = 0;
+	}
+	if (cursor->cly && cursor->a2-floor->a1 < floor->w) {
+		newy = floor->heightmap[cursor->a2-floor->a1];
+		switch (coldir & 12) {
+			case 4:
+				if (!floor->flp) {
+					cursor->pos.y = ((floor->b2-cursor->h)-newy)-cursor->yvel;
+				} else {
+					cursor->pos.y = (floor->pos.y-cursor->h)-cursor->yvel;
+				}
+				break;
+			case 8:
+				if (!floor->flp) {
+					cursor->pos.y = (floor->pos.y+floor->h)-cursor->yvel+1;
+				} else {
+					cursor->pos.y = (floor->pos.y+newy)-cursor->yvel;
+				}
+				break;
+		}
+		cursor->yvel = 0;
+		floor->yvel = 0;
+	}
+	cursor->gnded = (cursor->pos.y == ((floor->b2-cursor->h)-newy)-cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%i, %i\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, newy, cursor->pos.x, cursor->pos.y, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%i, %i\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, newy, floor->pos.x, floor->pos.y, floor->xvel, floor->yvel);
+	return 0;
+}
 
-		if (cursor.clx) {
-			if (cursor.a2-floor.a1 < 0 || cursor.a2-floor.a1 > floor.w) {
-				if (l)
-					cursor.pos.x = (floor.pos.x-(int)cursor.xvel)-cursor.w-1;
-				else if (r)
-					cursor.pos.x = (floor.a2-cursor.w)-(int)cursor.xvel+1;
-				cursor.xvel = 0;
-			}
+/* Floating point Tile Heightmap Collision. */
+int fthm(int hmpeak, struct colis *cursor, struct colis *floor) {
+	int newy = 0;
+	cursor->clx = (cursor->c2 > floor->c1-cursor->xvel-1 && cursor->d2 > floor->d1) && (cursor->c1 < (floor->c2-cursor->w)-cursor->xvel+1 && cursor->d1 < floor->d2);
+	cursor->cly = (cursor->d2 >= floor->d1-cursor->yvel && cursor->c2 >= floor->c1) && (cursor->d1 <= floor->d2-cursor->yvel && cursor->c1 <= floor->c2);
+	uint8_t coldir = get_coldir(1, THM, hmpeak, cursor, floor);
+
+	if (cursor->clx && cursor->c2-floor->c1 < -1 || cursor->c2-floor->c1 > floor->w) {
+		switch (coldir & 3) {
+			case 1: cursor->pos.fx = (floor->pos.fx-cursor->xvel)-cursor->w-1; break;
+			case 2: cursor->pos.fx = (floor->c2-cursor->w)-cursor->xvel+1; break;
 		}
-		if (cursor.cly) {
-			if (cursor.a2-floor.a1 < floor.w) {
-				newy = (int)floor.heightmap[cursor.a2-floor.a1];
-				if (t)
-					(!floor.flp) ? (cursor.pos.y = ((floor.b2-cursor.h)-newy)-(int)cursor.yvel) : (cursor.pos.y = (floor.pos.y-cursor.h)-(int)cursor.yvel);
-				else if (b)
-					(!floor.flp) ? (cursor.pos.y = (floor.pos.y+floor.h)-(int)cursor.yvel+1) : (cursor.pos.y = (floor.pos.y+newy)-(int)cursor.yvel);
-				cursor.yvel = 0;
+		cursor->xvel = 0;
+		cursor->xvel = 0;
+	}
+	if (cursor->cly && cursor->c2-floor->c1 < floor->w) {
+		newy = floor->heightmap[(int)cursor->c2-(int)floor->c1];
+		switch (coldir & 12) {
+			case 4:
+				if (!floor->flp) {
+					cursor->pos.fy = ((floor->d2-cursor->h)-newy)-cursor->yvel;
+				} else {
+					cursor->pos.fy = (floor->pos.fy-cursor->h)-cursor->yvel;
+				}
+				break;
+			case 8:
+				if (!floor->flp) {
+					cursor->pos.fy = (floor->pos.fy+floor->h)-cursor->yvel;
+				} else {
+					cursor->pos.fy = (floor->pos.fy+newy)-cursor->yvel;
+				}
+				break;
+		}
+		cursor->yvel = 0;
+		floor->yvel = 0;
+	}
+	cursor->gnded = (cursor->pos.fy == ((floor->d2-cursor->h)-newy)-cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, newy, cursor->pos.fx, cursor->pos.fy, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%07.06f, %07.06f\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, newy, floor->pos.fx, floor->pos.fy, floor->xvel, floor->yvel);
+	return 0;
+}
+
+
+/* Axis Aligned Bounding Box Collision. */
+int aabb(struct colis *cursor, struct colis *floor) {
+	uint8_t coldir = get_coldir(0, AABB, -1, cursor, floor);
+	uint8_t left   = (cursor->xvel > 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t right  = (cursor->xvel < 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t top    = (cursor->yvel > 0) && (!floor->yvel || floor->yvel != 0);
+	uint8_t bottom = (cursor->yvel < 0) && (!floor->yvel || floor->yvel != 0);
+
+	cursor->clx = (coldir & 0x03) == 0x03;
+	cursor->cly = (coldir & 0x0C) == 0x0C;
+	/* Is the Cursor is on the ground? */
+	cursor->gnded = (cursor->b2 == floor->b1-cursor->yvel-1);
+	if (cursor->clx) {
+		if (left) {
+			cursor->pos.x = (floor->pos.x-cursor->xvel)-cursor->w-1;
+		} else if (!left && floor->xvel != 0) {
+			if (!right && floor->xvel < 0) {
+				cursor->pos.x = (floor->pos.x-cursor->xvel)-cursor->w-1;
 			}
 		}
-		cursor.gnded = (cursor.pos.y == ((floor.b2-cursor.h)-newy)-(int)cursor.yvel) ? 1 : 0;
-		printf("%u, %u\n%u\n%i\n%i, %i\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, newy, cursor.pos.x, cursor.pos.y, cursor.xvel, cursor.yvel);
-	} else {
-		cursor.clx = (cursor.c2 > floor.c1-cursor.xvel-1 && cursor.d2 > floor.d1) && (cursor.c1 < (floor.c2-cursor.w)-cursor.xvel+1 && cursor.d1 < floor.d2);
-		cursor.cly = (cursor.d2 >= floor.d1-cursor.yvel && cursor.c2 >= floor.c1) && (cursor.d1 <= floor.d2-cursor.yvel && cursor.c1 <= floor.c2);
-		uint8_t l;
-		uint8_t r;
-		uint8_t t;
-		uint8_t b;
-		if (!floor.flp) {
-			l = (cursor.c2 > floor.c1-cursor.xvel-1 && floor.d2-cursor.d2 < (int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.c1 < (floor.c2-cursor.w)-cursor.xvel+1 && floor.d2-cursor.d2 < (int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.d2 > (floor.d2-hmpeak)-cursor.yvel && floor.d2-cursor.d2 >= 0);
-			b = (cursor.d1 < floor.d2-cursor.yvel+1 && floor.d2-cursor.d1 <= 0);
-		} else {
-			l = (cursor.c2 > floor.c1-cursor.xvel-1 && floor.d1-cursor.d1 > -(int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.c1 < (floor.c2-cursor.w)-cursor.xvel+1 && floor.d1-cursor.d1 > -(int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.d2 > floor.d1-cursor.yvel-1 && floor.d1-cursor.d2 >= 0 && cursor.yvel > 0);
-			b = (cursor.c1 < (floor.d1+hmpeak)-cursor.yvel && cursor.d1-floor.d1 >= 0);
+		if (right) {
+			cursor->pos.x = (floor->pos.x+floor->w)-cursor->xvel+1;
+		} else if (!right && floor->xvel != 0) {
+			if (!left && floor->xvel > 0) {
+				cursor->pos.x = (floor->pos.x+floor->w)-cursor->xvel+1;
+			}
 		}
-
-		if (cursor.clx) {
-			if (cursor.c2-floor.c1 < -1 || cursor.c2-floor.c1 > floor.w) {
-				if (l)
-					cursor.pos.fx = (floor.pos.fx-cursor.xvel)-cursor.w-1;
-				else if (r)
-					cursor.pos.fx = (floor.c2-cursor.w)-cursor.xvel+1;
-				cursor.xvel = 0;
+		cursor->xvel = 0;
+		floor->xvel = 0;
+	}
+	if (cursor->cly) {
+		if (top) {
+			cursor->pos.y = (floor->pos.y-cursor->yvel)-cursor->h-1;
+		} else if (!top && floor->yvel != 0) {
+			if (!bottom && floor->yvel < 0) {
+				cursor->pos.y = (floor->pos.y-cursor->yvel)-cursor->h-1;
 			}
 		}
-		if (cursor.cly) {
-			if (cursor.c2-floor.c1 < floor.w) {
-				newy = (int)floor.heightmap[(int)cursor.c2-(int)floor.c1];
-				if (t)
-					(!floor.flp) ? (cursor.pos.fy = ((floor.d2-cursor.h)-newy)-cursor.yvel) : (cursor.pos.fy = (floor.pos.fy-cursor.h)-cursor.yvel);
-				else if (b)
-					(!floor.flp) ? (cursor.pos.fy = (floor.pos.fy+floor.h)-cursor.yvel) : (cursor.pos.fy = (floor.pos.fy+newy)-cursor.yvel);
-				cursor.yvel = 0;
+		if (bottom) {
+			cursor->pos.y = (floor->pos.y+floor->h)-cursor->yvel+1;
+		} else if (!bottom && floor->yvel != 0) {
+			if (!top && floor->yvel > 0) {
+				cursor->pos.y = (floor->pos.y+floor->h)-cursor->yvel+1;
 			}
 		}
-		cursor.gnded = (cursor.pos.fy == ((floor.d2-cursor.h)-newy)-cursor.yvel) ? 1 : 0;
-		printf("%u, %u\n%u\n%i\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, newy, cursor.pos.fx, cursor.pos.fy, cursor.xvel, cursor.yvel);
+		cursor->yvel = 0;
+		floor->yvel = 0;
 	}
+	printf("%u, %u\n%u\n%i, %i\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, cursor->pos.x, cursor->pos.y, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%i, %i\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, floor->pos.x, floor->pos.y, floor->xvel, floor->yvel);
 	return 0;
 }
 
-/* Axis Aligned Bounding Box Collision. */
-int aabb(uint8_t flt, struct colis cursor, struct colis floor) {
-	uint8_t l = (!flt) ? (cursor.a2 > floor.a1-(int)cursor.xvel-1 && cursor.b2 > floor.b1) : (cursor.c2 > floor.c1-cursor.xvel-1 && cursor.d2 > floor.d1);
-	uint8_t r = (!flt) ? (cursor.a1 < floor.a2-(int)cursor.xvel+1 && cursor.b1 < floor.b2) : (cursor.c1 < floor.c2-cursor.xvel+1 && cursor.d1 < floor.d2);
-	uint8_t t = (!flt) ? (cursor.b2 > floor.b1-(int)cursor.yvel-1 && cursor.a2 > floor.a1) : (cursor.d2 > floor.d1-cursor.yvel-1 && cursor.c2 > floor.c1);
-	uint8_t b = (!flt) ? (cursor.b1 < floor.b2-(int)cursor.yvel+1 && cursor.a1 < floor.a2) : (cursor.d1 < floor.d2-cursor.yvel+1 && cursor.c1 < floor.c2);
-	cursor.clx = l && r;
-	cursor.cly = t && b;
+/* Floating point Axis Aligned Bounding Box Collision. */
+int faabb(struct colis *cursor, struct colis *floor) {
+	uint8_t coldir = get_coldir(1, AABB, -1, cursor, floor);
+	uint8_t left   = (cursor->xvel > 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t right  = (cursor->xvel < 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t top    = (cursor->yvel > 0) && (!floor->yvel || floor->yvel != 0);
+	uint8_t bottom = (cursor->yvel < 0) && (!floor->yvel || floor->yvel != 0);
+
+	cursor->clx = (coldir & 0x03) == 0x03;
+	cursor->cly = (coldir & 0x0C) == 0x0C;
 	/* Is the Cursor is on the ground? */
-	cursor.gnded = (cursor.b2 == floor.b1-(int)cursor.yvel-1) ? 1 : 0;
-	/* Is the Cursor coming from the left? */
-	if (l && cursor.xvel > 0)
-		(!flt) ? (cursor.pos.x = (floor.pos.x-(int)cursor.xvel)-cursor.w-1) : (cursor.pos.fx = (floor.pos.fx-cursor.xvel)-(double)cursor.w-1);
-	/* Is the Cursor coming from the right? */
-	else if (r && cursor.xvel < 0)
-		(!flt) ? (cursor.pos.x = (floor.pos.x+floor.w)-(int)cursor.xvel+1) : (cursor.pos.fx = (floor.pos.fx+(double)floor.w)-cursor.xvel+1);
-	cursor.xvel = 0;
-	/* Is the Cursor coming from the top? */
-	if (t && cursor.yvel > 0)
-		(!flt) ? (cursor.pos.y = (floor.pos.y-(int)cursor.yvel)-cursor.h-1) : (cursor.pos.fy = (floor.pos.fy-cursor.yvel)-(double)cursor.h-1);
-	/* Is the Cursor coming from the bottom? */
-	else if (b && cursor.yvel < 0)
-		(!flt) ? (cursor.pos.y = (floor.pos.y+floor.h)-(int)cursor.yvel+1) : (cursor.pos.fy = (floor.pos.fy+(double)floor.h)-cursor.yvel+1);
-	cursor.yvel = 0;
-	if (!flt)
-		printf("%u, %u\n%u\n%i, %i\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, cursor.pos.x, cursor.pos.y, cursor.xvel, cursor.yvel);
-	else
-		printf("%u, %u\n%u\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, cursor.pos.fx, cursor.pos.fy, cursor.xvel, cursor.yvel);
+	cursor->gnded = (cursor->d2 == floor->d1-cursor->yvel-1);
+	if (cursor->clx) {
+		if (left) {
+			cursor->pos.fx = (floor->pos.fx-cursor->xvel)-cursor->w-1;
+		} else if (!left && floor->xvel != 0) {
+			if (!right && floor->xvel < 0) {
+				cursor->pos.fx = (floor->pos.fx-cursor->xvel)-cursor->w-1;
+			}
+		}
+		if (right) {
+			cursor->pos.fx = (floor->pos.fx+floor->w)-cursor->xvel+1;
+		} else if (!right && floor->xvel != 0) {
+			if (!left && floor->xvel > 0) {
+				cursor->pos.fx = (floor->pos.fx+floor->w)-cursor->xvel+1;
+			}
+		}
+		cursor->xvel = 0;
+		floor->xvel = 0;
+	}
+	if (cursor->cly) {
+		if (top) {
+			cursor->pos.fy = (floor->pos.fy-cursor->yvel)-cursor->h-1;
+		} else if (!top && floor->yvel != 0) {
+			if (!bottom && floor->yvel < 0) {
+				cursor->pos.fy = (floor->pos.fy-cursor->yvel)-cursor->h-1;
+			}
+		}
+		if (bottom) {
+			cursor->pos.fy = (floor->pos.fy+floor->h)-cursor->yvel+1;
+		} else if (!bottom && floor->yvel != 0) {
+			if (!top && floor->yvel > 0) {
+				cursor->pos.fy = (floor->pos.fy+floor->h)-cursor->yvel+1;
+			}
+		}
+		cursor->yvel = 0;
+		floor->yvel = 0;
+	}
+	printf("%u, %u\n%u\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, cursor->pos.fx, cursor->pos.fy, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%07.06f, %07.06f\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, floor->pos.fx, floor->pos.fy, floor->xvel, floor->yvel);
 	return 0;
 }
 
@@ -239,17 +438,14 @@ int main(int argc, char **argv) {
 						verbose = 1;
 						break;
 					} else {
-						fprintf(stderr
-							, "The option that you have typed in, which in this case\n"
-							  "is -%c, does not exist.\n"
-							  "Try looking at the help message, or RTFM, next time.\n"
-							, optopt);
+						fprintf(stderr, "The option that you have typed in, which in this case\n");
+						fprintf(stderr, "is -%c, does not exist.\n", optopt);
+						fprintf(stderr, "Try looking at the help message, or RTFM, next time.\n");
 					}
 				} else {
-					fprintf(stderr
-						, "What the fuck are you trying to type in, a binary blob?\n"
-						  "Because of this, clld will exit, so, stop acting like a\n"
-						  "jackass.\n");
+					fprintf(stderr, "What the fuck are you trying to type in, a binary blob?\n");
+					fprintf(stderr, "Because of this, clld will exit, so, stop acting like a\n");
+					fprintf(stderr, "jackass.\n");
 				}
 				return -1;
 			default: ;
@@ -259,7 +455,7 @@ int main(int argc, char **argv) {
 	/* Create the Cursor, and the Floor */
 	struct colis cursor;
 	struct colis floor;
-	colltype = AABB;
+	colltype = THM;
 	cursor.h = 16;
 	cursor.w = 16;
 	cursor.pos.x = 17;
@@ -272,31 +468,42 @@ int main(int argc, char **argv) {
 	int hmpeak;
 	if (colltype == THM) {
 		floor.heightmap = malloc(sizeof(uint8_t *)*floor.w);
-		for (int i = 0; i <floor.w; i++)
-			floor.heightmap[i] = i+1;
+		int i = 0;
+		for (; i < floor.w; i++) floor.heightmap[i] = i+1;
 		hmpeak = floor.heightmap[floor.w-1];
 	}
 	uint8_t flt = 0;
 	floor.pos.x = 32;
 	floor.pos.y = 32;
-	(!flt) ? (cursor.a1 = cursor.pos.x) : (cursor.c1 = cursor.pos.fx);
-	(!flt) ? (cursor.a2 = cursor.pos.x+cursor.w) : (cursor.c2 = cursor.pos.fx+cursor.w);
-	(!flt) ? (cursor.b1 = cursor.pos.y) : (cursor.d1 = cursor.pos.fy);
-	(!flt) ? (cursor.b2 = cursor.pos.y+cursor.h) : (cursor.d2 = cursor.pos.fy+cursor.h);
-	(!flt) ? (floor.a1 = floor.pos.x) : (floor.c1 = floor.pos.fx);
-	(!flt) ? (floor.a2 = floor.pos.x+floor.w) : (floor.c2 = floor.pos.fx+floor.w);
-	(!flt) ? (floor.b1 = floor.pos.y) : (floor.d1 = floor.pos.fy);
-	(!flt) ? (floor.b2 = floor.pos.y+floor.h) : (floor.d2 = floor.pos.fy+floor.h);
-
-	if (colltype == THM)
-		thm(flt, hmpeak, cursor, floor);
-	/* Axis Aligned Bounding Box Collision. */
-	if (colltype == AABB)
-		aabb(flt, cursor, floor);
+	floor.xvel = 0;
+	floor.yvel = 0;
+	if (!flt) {
+		cursor.a1 = cursor.pos.x;
+		cursor.a2 = cursor.pos.x+cursor.w;
+		cursor.b1 = cursor.pos.y;
+		cursor.b2 = cursor.pos.y+cursor.h;
+		floor.a1  = floor.pos.x;
+		floor.a2  = floor.pos.x+floor.w;
+		floor.b1  = floor.pos.y;
+		floor.b2  = floor.pos.y+floor.h;
+	} else {
+		cursor.c1 = cursor.pos.fx;
+		cursor.c2 = cursor.pos.fx+cursor.w;
+		cursor.d1 = cursor.pos.fy;
+		cursor.d2 = cursor.pos.fy+cursor.h;
+		floor.c1  = floor.pos.fx;
+		floor.c2  = floor.pos.fx+floor.w;
+		floor.d1  = floor.pos.fy;
+		floor.d2  = floor.pos.fy+floor.h;
+	}
+	switch (colltype) {
+		case THM : (!flt) ? thm(hmpeak, &cursor, &floor) : fthm(hmpeak, &cursor, &floor); break; /* Tile Heightmap Collision. */
+		case AABB: (!flt) ? aabb(&cursor, &floor       ) : faabb(&cursor, &floor       ); break; /* Axis Aligned Bounding Box Collision. */
+	}
 	/*uint8_t a = 0;
 	unsigned int i = 0;
 	free(buf);*/
-	if(colltype == THM) {
+	if (colltype == THM) {
 		free(floor.heightmap);
 	}
 	fflush(stdout);
diff --git a/clld.c b/clld.c
index b08e0de..ca28220 100644
--- a/clld.c
+++ b/clld.c
@@ -93,165 +93,381 @@ static int usage() {
 	return 0;
 }
 
+/* Get the direction of a collision. */
+uint8_t isleft = 0;
+uint8_t isright = 0;
+uint8_t istop = 0;
+uint8_t isbottom = 0;
+uint8_t isinfloor_x = 0;
+uint8_t isinfloor_y = 0;
+
+uint8_t get_coldir(uint8_t flt, uint8_t col_type, int hmpeak, struct colis *cursor, struct colis *floor) {
+	uint8_t coldir = 0;
+	uint8_t isinfloor_left   = 0;
+	uint8_t isinfloor_right  = 0;
+	uint8_t isinfloor_top    = 0;
+	uint8_t isinfloor_bottom = 0;
+	switch (col_type) {
+		case AABB:
+			if (flt) {
+				isleft   = cursor->c2 > floor->c1-cursor->xvel-1;
+				isright  = cursor->c1 < floor->c2-cursor->xvel+1;
+				istop    = cursor->d2 > floor->d1-cursor->yvel-1;
+				isbottom = cursor->d1 < floor->d2-cursor->yvel+1;
+
+				isinfloor_left   = cursor->d2 > floor->d1;
+				isinfloor_right  = cursor->d1 < floor->d2;
+				isinfloor_top    = cursor->c2 > floor->c1;
+				isinfloor_bottom = cursor->c1 < floor->c2;
+			} else {
+				isleft   = cursor->a2 > floor->a1-cursor->xvel-1;
+				isright  = cursor->a1 < floor->a2-cursor->xvel+1;
+				istop    = cursor->b2 > floor->b1-cursor->yvel-1;
+				isbottom = cursor->b1 < floor->b2-cursor->yvel+1;
+
+				isinfloor_left   = cursor->b2 > floor->b1;
+				isinfloor_right  = cursor->b1 < floor->b2;
+				isinfloor_top    = cursor->a2 > floor->a1;
+				isinfloor_bottom = cursor->a1 < floor->a2;
+			}
+			isinfloor_x = isinfloor_left && isinfloor_right ;
+			isinfloor_y = isinfloor_top  && isinfloor_bottom;
+			break;
+		case THM:
+			if (flt) {
+				/* Did the cursor collide with the floor from the left? */
+				isleft   = cursor->c2 > floor->c1-cursor->xvel-1;
+				/* Did the cursor collide with the floor from the right? */
+				isright  = cursor->c1 < (floor->c2-cursor->w)-cursor->xvel+1;
+
+				/* Is the floor's heightmap vertically flipped? */
+				if (!floor->flp) {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->d2 > (floor->d2-hmpeak)-cursor->yvel;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->d1 < floor->d2-cursor->yvel+1;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left   = floor->d2-cursor->d2 < floor->heightmap[0] && cursor->xvel > 0;
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right  = floor->d2-cursor->d2 < floor->heightmap[floor->w-1] && cursor->xvel < 0;
+					/* Is the bottom of the cursor at, or above the floor's ground? */
+					isinfloor_top    = floor->d2-cursor->d2 >= 0;
+					/* Is the top of the cursor at, or below the floor's ground? */
+					isinfloor_bottom = floor->d2-cursor->d1 <= 0;
+				} else {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->d2 > floor->d1-cursor->yvel-1;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->d1 < (floor->d1+hmpeak)-cursor->yvel;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left   = floor->d1-cursor->d1 > -floor->heightmap[0] && cursor->xvel > 0;
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right  = floor->d1-cursor->d1 > -floor->heightmap[floor->w-1] && cursor->xvel < 0;
+					/* Is the bottom of the cursor at, or above the floor's height? */
+					isinfloor_top    = floor->d1-cursor->d2 >= 0 && cursor->yvel > 0;
+					/* Is the top of the cursor at, or above the floor's height? */
+					isinfloor_bottom = cursor->d1-floor->d1 >= 0;
+				}
+			} else {
+				/* Did the cursor collide with the floor from the left? */
+				isleft   = cursor->a2 > floor->a1-cursor->xvel-1;
+				/* Did the cursor collide with the floor from the right? */
+				isright  = cursor->a1 < (floor->a2-cursor->w)-cursor->xvel+1;
+
+				/* Is the floor's heightmap vertically flipped? */
+				if (!floor->flp) {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->b2 > (floor->b2-hmpeak)-cursor->yvel;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->b1 < floor->b2-cursor->yvel+1;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left  = floor->b2-cursor->b2 < floor->heightmap[0];
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right = floor->b2-cursor->b2 < floor->heightmap[floor->w-1];
+					/* Is the bottom of the cursor at, or above the floor's ground? */
+					isinfloor_top = floor->b2-cursor->b2 >= 0;
+					/* Is the top of the cursor at, or below the floor's ground? */
+					isinfloor_bottom = floor->b2-cursor->b1 <= 0;
+				} else {
+					/* Did the cursor collide with the floor from the top? */
+					istop    = cursor->b2 > floor->b1-cursor->yvel-1;
+					/* Did the cursor collide with the floor from the bottom? */
+					isbottom = cursor->b1 < (floor->b1+hmpeak)-cursor->yvel;
+
+					/* Is the bottom of the cursor below the floor's leftmost height? */
+					isinfloor_left   = floor->b1-cursor->b1 > -floor->heightmap[0];
+					/* Is the bottom of the cursor below the floor's rightmost height? */
+					isinfloor_right  = floor->b1-cursor->b1 > -floor->heightmap[floor->w-1];
+					/* Is the bottom of the cursor at, or above the floor's ground? */
+					isinfloor_top    = floor->b1-cursor->b2 >= 0 && cursor->yvel > 0;
+					/* Is the top of the cursor at, or below the floor's ground? */
+					isinfloor_bottom = cursor->b1-floor->b1 >= 0;
+				}
+			}
+			isinfloor_x = isinfloor_left ^ isinfloor_right ;
+			isinfloor_y = isinfloor_top  ^ isinfloor_bottom;
+			break;
+	}
+	if (isinfloor_x) {
+		coldir |= isleft   << 0;
+		coldir |= isright  << 1;
+	}
+	if (isinfloor_y) {
+		coldir |= istop    << 2;
+		coldir |= isbottom << 3;
+	}
+	return coldir;
+}
+
 /* Tile Heightmap Collision. */
-int thm(uint8_t flt, int hmpeak, struct colis cursor, struct colis floor) {
+int thm(int hmpeak, struct colis *cursor, struct colis *floor) {
 	int newy = 0;
-	if (!flt) {
-		cursor.clx = (cursor.a2 > floor.a1-(int)cursor.xvel-1 && cursor.b2 > floor.b1) && (cursor.a1 < (floor.a2-cursor.w)-(int)cursor.xvel+1 && cursor.b1 < floor.b2);
-		cursor.cly = (cursor.b2 >= floor.b1-(int)cursor.yvel && cursor.a2 >= floor.a1) && (cursor.b1 <= floor.b2-(int)cursor.yvel && cursor.a1 <= floor.a2);
-		uint8_t l;
-		uint8_t r;
-		uint8_t t;
-		uint8_t b;
-		if (!floor.flp) {
-			l = (cursor.a2 > floor.a1-(int)cursor.xvel-1 && floor.b2-cursor.b2 < (int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.a1 < (floor.a2-cursor.w)-(int)cursor.xvel+1 && floor.b2-cursor.b2 < (int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.b2 > (floor.b2-hmpeak)-(int)cursor.yvel && floor.b2-cursor.b2 >= 0);
-			b = (cursor.b1 < floor.b2-(int)cursor.yvel+1 && floor.b2-cursor.b1 <= 0);
-		} else {
-			l = (cursor.a2 > floor.a1-(int)cursor.xvel-1 && floor.b1-cursor.b1 > -(int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.a1 < (floor.a2-cursor.w)-(int)cursor.xvel+1 && floor.b1-cursor.b1 > -(int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.b2 > floor.b1-(int)cursor.yvel-1 && floor.b1-cursor.b2 >= 0 && cursor.yvel > 0);
-			b = (cursor.b1 < (floor.b1+hmpeak)-(int)cursor.yvel && cursor.b1-floor.b1 >= 0);
+	cursor->clx = (cursor->a2 > floor->a1-cursor->xvel-1 && cursor->b2 > floor->b1) && (cursor->a1 < (floor->a2-cursor->w)-cursor->xvel+1 && cursor->b1 < floor->b2);
+	cursor->cly = (cursor->b2 >= floor->b1-cursor->yvel && cursor->a2 >= floor->a1) && (cursor->b1 <= floor->b2-cursor->yvel && cursor->a1 <= floor->a2);
+	uint8_t coldir = get_coldir(0, THM, hmpeak, cursor, floor);
+
+	if (cursor->clx && (cursor->a2-floor->a1 < 0 || cursor->a2-floor->a1 > floor->w)) {
+		switch (coldir & 3) {
+			case 1: cursor->pos.x = (floor->pos.x-cursor->xvel)-cursor->w-1; break;
+			case 2: cursor->pos.x = (floor->a2-cursor->w)-cursor->xvel+1; break;
+		}
+		cursor->xvel = 0;
+		floor->xvel = 0;
+	}
+	if (cursor->cly && cursor->a2-floor->a1 < floor->w) {
+		newy = floor->heightmap[cursor->a2-floor->a1];
+		switch (coldir & 12) {
+			case 4:
+				if (!floor->flp) {
+					cursor->pos.y = ((floor->b2-cursor->h)-newy)-cursor->yvel;
+				} else {
+					cursor->pos.y = (floor->pos.y-cursor->h)-cursor->yvel;
+				}
+				break;
+			case 8:
+				if (!floor->flp) {
+					cursor->pos.y = (floor->pos.y+floor->h)-cursor->yvel+1;
+				} else {
+					cursor->pos.y = (floor->pos.y+newy)-cursor->yvel;
+				}
+				break;
 		}
+		cursor->yvel = 0;
+		floor->yvel = 0;
+	}
+	cursor->gnded = (cursor->pos.y == ((floor->b2-cursor->h)-newy)-cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%i, %i\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, newy, cursor->pos.x, cursor->pos.y, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%i, %i\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, newy, floor->pos.x, floor->pos.y, floor->xvel, floor->yvel);
+	return 0;
+}
 
-		if (cursor.clx) {
-			if (cursor.a2-floor.a1 < 0 || cursor.a2-floor.a1 > floor.w) {
-				if (l)
-					cursor.pos.x = (floor.pos.x-(int)cursor.xvel)-cursor.w-1;
-				else if (r)
-					cursor.pos.x = (floor.a2-cursor.w)-(int)cursor.xvel+1;
-				cursor.xvel = 0;
-			}
+/* Floating point Tile Heightmap Collision. */
+int fthm(int hmpeak, struct colis *cursor, struct colis *floor) {
+	int newy = 0;
+	cursor->clx = (cursor->c2 > floor->c1-cursor->xvel-1 && cursor->d2 > floor->d1) && (cursor->c1 < (floor->c2-cursor->w)-cursor->xvel+1 && cursor->d1 < floor->d2);
+	cursor->cly = (cursor->d2 >= floor->d1-cursor->yvel && cursor->c2 >= floor->c1) && (cursor->d1 <= floor->d2-cursor->yvel && cursor->c1 <= floor->c2);
+	uint8_t coldir = get_coldir(1, THM, hmpeak, cursor, floor);
+
+	if (cursor->clx && cursor->c2-floor->c1 < -1 || cursor->c2-floor->c1 > floor->w) {
+		switch (coldir & 3) {
+			case 1: cursor->pos.fx = (floor->pos.fx-cursor->xvel)-cursor->w-1; break;
+			case 2: cursor->pos.fx = (floor->c2-cursor->w)-cursor->xvel+1; break;
+		}
+		cursor->xvel = 0;
+		cursor->xvel = 0;
+	}
+	if (cursor->cly && cursor->c2-floor->c1 < floor->w) {
+		newy = floor->heightmap[(int)cursor->c2-(int)floor->c1];
+		switch (coldir & 12) {
+			case 4:
+				if (!floor->flp) {
+					cursor->pos.fy = ((floor->d2-cursor->h)-newy)-cursor->yvel;
+				} else {
+					cursor->pos.fy = (floor->pos.fy-cursor->h)-cursor->yvel;
+				}
+				break;
+			case 8:
+				if (!floor->flp) {
+					cursor->pos.fy = (floor->pos.fy+floor->h)-cursor->yvel;
+				} else {
+					cursor->pos.fy = (floor->pos.fy+newy)-cursor->yvel;
+				}
+				break;
 		}
-		if (cursor.cly) {
-			if (cursor.a2-floor.a1 < floor.w) {
-				newy = (int)floor.heightmap[cursor.a2-floor.a1];
-				if (t)
-					(!floor.flp) ? (cursor.pos.y = ((floor.b2-cursor.h)-newy)-(int)cursor.yvel) : (cursor.pos.y = (floor.pos.y-cursor.h)-(int)cursor.yvel);
-				else if (b)
-					(!floor.flp) ? (cursor.pos.y = (floor.pos.y+floor.h)-(int)cursor.yvel+1) : (cursor.pos.y = (floor.pos.y+newy)-(int)cursor.yvel);
-				cursor.yvel = 0;
+		cursor->yvel = 0;
+		floor->yvel = 0;
+	}
+	cursor->gnded = (cursor->pos.fy == ((floor->d2-cursor->h)-newy)-cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, newy, cursor->pos.fx, cursor->pos.fy, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%i\n%07.06f, %07.06f\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, newy, floor->pos.fx, floor->pos.fy, floor->xvel, floor->yvel);
+	return 0;
+}
+
+
+/* Axis Aligned Bounding Box Collision. */
+int aabb(struct colis *cursor, struct colis *floor) {
+	uint8_t coldir = get_coldir(0, AABB, -1, cursor, floor);
+	uint8_t left   = (cursor->xvel > 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t right  = (cursor->xvel < 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t top    = (cursor->yvel > 0) && (!floor->yvel || floor->yvel != 0);
+	uint8_t bottom = (cursor->yvel < 0) && (!floor->yvel || floor->yvel != 0);
+
+	cursor->clx = (coldir & 0x03) == 0x03;
+	cursor->cly = (coldir & 0x0C) == 0x0C;
+	/* Is the Cursor is on the ground? */
+	cursor->gnded = (cursor->b2 == floor->b1-cursor->yvel-1);
+	if (cursor->clx) {
+		if (left) {
+			cursor->pos.x = (floor->pos.x-cursor->xvel)-cursor->w-1;
+		} else if (!left && floor->xvel != 0) {
+			if (!right && floor->xvel < 0) {
+				cursor->pos.x = (floor->pos.x-cursor->xvel)-cursor->w-1;
 			}
 		}
-		cursor.gnded = (cursor.pos.y == ((floor.b2-cursor.h)-newy)-(int)cursor.yvel) ? 1 : 0;
-		printf("%u, %u\n%u\n%i\n%i, %i\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, newy, cursor.pos.x, cursor.pos.y, cursor.xvel, cursor.yvel);
-	} else {
-		cursor.clx = (cursor.c2 > floor.c1-cursor.xvel-1 && cursor.d2 > floor.d1) && (cursor.c1 < (floor.c2-cursor.w)-cursor.xvel+1 && cursor.d1 < floor.d2);
-		cursor.cly = (cursor.d2 >= floor.d1-cursor.yvel && cursor.c2 >= floor.c1) && (cursor.d1 <= floor.d2-cursor.yvel && cursor.c1 <= floor.c2);
-		uint8_t l;
-		uint8_t r;
-		uint8_t t;
-		uint8_t b;
-		if (!floor.flp) {
-			l = (cursor.c2 > floor.c1-cursor.xvel-1 && floor.d2-cursor.d2 < (int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.c1 < (floor.c2-cursor.w)-cursor.xvel+1 && floor.d2-cursor.d2 < (int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.d2 > (floor.d2-hmpeak)-cursor.yvel && floor.d2-cursor.d2 >= 0);
-			b = (cursor.d1 < floor.d2-cursor.yvel+1 && floor.d2-cursor.d1 <= 0);
-		} else {
-			l = (cursor.c2 > floor.c1-cursor.xvel-1 && floor.d1-cursor.d1 > -(int)floor.heightmap[0] && cursor.xvel > 0);
-			r = (cursor.c1 < (floor.c2-cursor.w)-cursor.xvel+1 && floor.d1-cursor.d1 > -(int)floor.heightmap[floor.w-1] && cursor.xvel < 0);
-			t = (cursor.d2 > floor.d1-cursor.yvel-1 && floor.d1-cursor.d2 >= 0 && cursor.yvel > 0);
-			b = (cursor.c1 < (floor.d1+hmpeak)-cursor.yvel && cursor.d1-floor.d1 >= 0);
+		if (right) {
+			cursor->pos.x = (floor->pos.x+floor->w)-cursor->xvel+1;
+		} else if (!right && floor->xvel != 0) {
+			if (!left && floor->xvel > 0) {
+				cursor->pos.x = (floor->pos.x+floor->w)-cursor->xvel+1;
+			}
 		}
-
-		if (cursor.clx) {
-			if (cursor.c2-floor.c1 < -1 || cursor.c2-floor.c1 > floor.w) {
-				if (l)
-					cursor.pos.fx = (floor.pos.fx-cursor.xvel)-cursor.w-1;
-				else if (r)
-					cursor.pos.fx = (floor.c2-cursor.w)-cursor.xvel+1;
-				cursor.xvel = 0;
+		cursor->xvel = 0;
+		floor->xvel = 0;
+	}
+	if (cursor->cly) {
+		if (top) {
+			cursor->pos.y = (floor->pos.y-cursor->yvel)-cursor->h-1;
+		} else if (!top && floor->yvel != 0) {
+			if (!bottom && floor->yvel < 0) {
+				cursor->pos.y = (floor->pos.y-cursor->yvel)-cursor->h-1;
 			}
 		}
-		if (cursor.cly) {
-			if (cursor.c2-floor.c1 < floor.w) {
-				newy = (int)floor.heightmap[(int)cursor.c2-(int)floor.c1];
-				if (t)
-					(!floor.flp) ? (cursor.pos.fy = ((floor.d2-cursor.h)-newy)-cursor.yvel) : (cursor.pos.fy = (floor.pos.fy-cursor.h)-cursor.yvel);
-				else if (b)
-					(!floor.flp) ? (cursor.pos.fy = (floor.pos.fy+floor.h)-cursor.yvel) : (cursor.pos.fy = (floor.pos.fy+newy)-cursor.yvel);
-				cursor.yvel = 0;
+		if (bottom) {
+			cursor->pos.y = (floor->pos.y+floor->h)-cursor->yvel+1;
+		} else if (!bottom && floor->yvel != 0) {
+			if (!top && floor->yvel > 0) {
+				cursor->pos.y = (floor->pos.y+floor->h)-cursor->yvel+1;
 			}
 		}
-		cursor.gnded = (cursor.pos.fy == ((floor.d2-cursor.h)-newy)-cursor.yvel) ? 1 : 0;
-		printf("%u, %u\n%u\n%i\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, newy, cursor.pos.fx, cursor.pos.fy, cursor.xvel, cursor.yvel);
+		cursor->yvel = 0;
+		floor->yvel = 0;
 	}
+	printf("%u, %u\n%u\n%i, %i\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, cursor->pos.x, cursor->pos.y, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%i, %i\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, floor->pos.x, floor->pos.y, floor->xvel, floor->yvel);
 	return 0;
 }
 
-/* Axis Aligned Bounding Box Collision. */
-int aabb(uint8_t flt, struct colis cursor, struct colis floor) {
-	uint8_t l = (!flt) ? (cursor.a2 > floor.a1-(int)cursor.xvel-1 && cursor.b2 > floor.b1) : (cursor.c2 > floor.c1-cursor.xvel-1 && cursor.d2 > floor.d1);
-	uint8_t r = (!flt) ? (cursor.a1 < floor.a2-(int)cursor.xvel+1 && cursor.b1 < floor.b2) : (cursor.c1 < floor.c2-cursor.xvel+1 && cursor.d1 < floor.d2);
-	uint8_t t = (!flt) ? (cursor.b2 > floor.b1-(int)cursor.yvel-1 && cursor.a2 > floor.a1) : (cursor.d2 > floor.d1-cursor.yvel-1 && cursor.c2 > floor.c1);
-	uint8_t b = (!flt) ? (cursor.b1 < floor.b2-(int)cursor.yvel+1 && cursor.a1 < floor.a2) : (cursor.d1 < floor.d2-cursor.yvel+1 && cursor.c1 < floor.c2);
-	cursor.clx = l && r;
-	cursor.cly = t && b;
+/* Floating point Axis Aligned Bounding Box Collision. */
+int faabb(struct colis *cursor, struct colis *floor) {
+	uint8_t coldir = get_coldir(1, AABB, -1, cursor, floor);
+	uint8_t left   = (cursor->xvel > 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t right  = (cursor->xvel < 0) && (!floor->xvel || floor->xvel != 0);
+	uint8_t top    = (cursor->yvel > 0) && (!floor->yvel || floor->yvel != 0);
+	uint8_t bottom = (cursor->yvel < 0) && (!floor->yvel || floor->yvel != 0);
+
+	cursor->clx = (coldir & 0x03) == 0x03;
+	cursor->cly = (coldir & 0x0C) == 0x0C;
 	/* Is the Cursor is on the ground? */
-	cursor.gnded = (cursor.b2 == floor.b1-(int)cursor.yvel-1) ? 1 : 0;
-	/* Is the Cursor coming from the left? */
-	if (l && cursor.xvel > 0)
-		(!flt) ? (cursor.pos.x = (floor.pos.x-(int)cursor.xvel)-cursor.w-1) : (cursor.pos.fx = (floor.pos.fx-cursor.xvel)-(double)cursor.w-1);
-	/* Is the Cursor coming from the right? */
-	else if (r && cursor.xvel < 0)
-		(!flt) ? (cursor.pos.x = (floor.pos.x+floor.w)-(int)cursor.xvel+1) : (cursor.pos.fx = (floor.pos.fx+(double)floor.w)-cursor.xvel+1);
-	cursor.xvel = 0;
-	/* Is the Cursor coming from the top? */
-	if (t && cursor.yvel > 0)
-		(!flt) ? (cursor.pos.y = (floor.pos.y-(int)cursor.yvel)-cursor.h-1) : (cursor.pos.fy = (floor.pos.fy-cursor.yvel)-(double)cursor.h-1);
-	/* Is the Cursor coming from the bottom? */
-	else if (b && cursor.yvel < 0)
-		(!flt) ? (cursor.pos.y = (floor.pos.y+floor.h)-(int)cursor.yvel+1) : (cursor.pos.fy = (floor.pos.fy+(double)floor.h)-cursor.yvel+1);
-	cursor.yvel = 0;
-	if (!flt)
-		printf("%u, %u\n%u\n%i, %i\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, cursor.pos.x, cursor.pos.y, cursor.xvel, cursor.yvel);
-	else
-		printf("%u, %u\n%u\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor.clx, cursor.cly, cursor.gnded, cursor.pos.fx, cursor.pos.fy, cursor.xvel, cursor.yvel);
+	cursor->gnded = (cursor->d2 == floor->d1-cursor->yvel-1);
+	if (cursor->clx) {
+		if (left) {
+			cursor->pos.fx = (floor->pos.fx-cursor->xvel)-cursor->w-1;
+		} else if (!left && floor->xvel != 0) {
+			if (!right && floor->xvel < 0) {
+				cursor->pos.fx = (floor->pos.fx-cursor->xvel)-cursor->w-1;
+			}
+		}
+		if (right) {
+			cursor->pos.fx = (floor->pos.fx+floor->w)-cursor->xvel+1;
+		} else if (!right && floor->xvel != 0) {
+			if (!left && floor->xvel > 0) {
+				cursor->pos.fx = (floor->pos.fx+floor->w)-cursor->xvel+1;
+			}
+		}
+		cursor->xvel = 0;
+		floor->xvel = 0;
+	}
+	if (cursor->cly) {
+		if (top) {
+			cursor->pos.fy = (floor->pos.fy-cursor->yvel)-cursor->h-1;
+		} else if (!top && floor->yvel != 0) {
+			if (!bottom && floor->yvel < 0) {
+				cursor->pos.fy = (floor->pos.fy-cursor->yvel)-cursor->h-1;
+			}
+		}
+		if (bottom) {
+			cursor->pos.fy = (floor->pos.fy+floor->h)-cursor->yvel+1;
+		} else if (!bottom && floor->yvel != 0) {
+			if (!top && floor->yvel > 0) {
+				cursor->pos.fy = (floor->pos.fy+floor->h)-cursor->yvel+1;
+			}
+		}
+		cursor->yvel = 0;
+		floor->yvel = 0;
+	}
+	printf("%u, %u\n%u\n%07.06f, %07.06f\n%07.06f, %07.06f\n", cursor->clx, cursor->cly, cursor->gnded, cursor->pos.fx, cursor->pos.fy, cursor->xvel, cursor->yvel);
+	printf("%u, %u\n%u\n%07.06f, %07.06f\n%07.06f, %07.06f\n", floor->clx, floor->cly, floor->gnded, floor->pos.fx, floor->pos.fy, floor->xvel, floor->yvel);
 	return 0;
 }
 
 int main(int argc, char **argv) {
 	int c;
+	FILE *fp = NULL;
+	uint8_t isfile = 0;
 	/* Enable custom error messages, and optional arguments/ */
 	opterr = 0;
 	/* Get options. */
-	while ((c = getopt(argc, argv, opts)) != -1) {
-		switch(c) {
-			case 'c':
-				colltype = atoi(optarg);
-				break;
-			case 'v':
-				verbose = atoi(optarg);
-				break;
-			case 'h':
-				usage();
-				return 2;
-			case '?':
-				if (isprint(optopt)) {
-					if (optopt == 'c') {
-						fprintf(stderr, "Setting colltype to default.\n");
-						colltype = -1;
-						break;
-					} else if (optopt == 'v') {
-						fprintf(stderr, "Enabling single level verbosity.\n");
-						verbose = 1;
-						break;
+	while (optind < argc) {
+		if ((c = getopt(argc, argv, opts)) != -1) {
+			switch(c) {
+				case 'c':
+					colltype = atoi(optarg);
+					break;
+				case 'v':
+					verbose = atoi(optarg);
+					break;
+				case 'h':
+					usage();
+					return 2;
+				case '?':
+					if (isprint(optopt)) {
+						if (optopt == 'c') {
+							fprintf(stderr, "Setting colltype to default.\n");
+							colltype = -1;
+							break;
+						} else if (optopt == 'v') {
+							fprintf(stderr, "Enabling single level verbosity.\n");
+							verbose = 1;
+							break;
+						} else {
+							fprintf(stderr
+								, "The option that you have typed in, which in this case\n"
+								  "is -%c, does not exist.\n"
+								  "Try looking at the help message, or RTFM, next time.\n"
+								, optopt);
+						}
 					} else {
 						fprintf(stderr
-							, "The option that you have typed in, which in this case\n"
-							  "is -%c, does not exist.\n"
-							  "Try looking at the help message, or RTFM, next time.\n"
-							, optopt);
+							, "What the fuck are you trying to type in, a binary blob?\n"
+							  "Because of this, clld will exit, so, stop acting like a\n"
+							  "jackass.\n");
 					}
-				} else {
-					fprintf(stderr
-						, "What the fuck are you trying to type in, a binary blob?\n"
-						  "Because of this, clld will exit, so, stop acting like a\n"
-						  "jackass.\n");
+					return -1;
+				default: ;
+			}
+		} else {
+			if (!isfile) {
+				fp = fopen(argv[optind], "r");
+				if (fp == NULL) {
+					fprintf(stderr, "Failed to open file %s\n", optarg);
+					return -1;
 				}
-				return -1;
-			default: ;
+			}
+			isfile = (fp != NULL);
+			optind++;
 		}
 	}
 
@@ -264,19 +480,18 @@ int main(int argc, char **argv) {
 	 * the process of getting collision data, to one for loop.
 	 */
 	struct colis tmp;
-	int hmpeak;
+	int hmpeak = 0;
 	/* Get collision type. */
 	char *buf = NULL;
 	size_t size;
-
-	getline(&buf, &size, stdin);
+	getline(&buf, &size, (isfile) ? fp : stdin);
 
 	if (colltype == -1)
 		colltype = atoi(strtok(buf, "\n"));
 
 	for (int coll = 0; coll <= 1; coll++) {
 		buf = NULL;
-		getline(&buf, &size, stdin);
+		getline(&buf, &size, (isfile) ? fp : stdin);
 		/*
 		 * Format is:
 		 *
@@ -293,11 +508,12 @@ int main(int argc, char **argv) {
 		if (colltype == THM && coll) {
 			/* Get vertical flip flag */
 			buf = NULL;
-			getline(&buf, &size, stdin);
+			getline(&buf, &size, (isfile) ? fp : stdin);
 			floor.flp = buf[0] - '0';
 			/* Get Heightmap */
 			buf = NULL;
-                        size_t newidth = getline(&buf, &size, stdin);
+                        size_t newidth =  getline(&buf, &size, (isfile) ? fp : stdin);
+
 			/*
 			 * Format is:
 			 *
@@ -332,7 +548,7 @@ int main(int argc, char **argv) {
 		uint8_t delms = 0;
 		char *tmp2;
 		buf = NULL;
-		getline(&buf, &size, stdin);
+		getline(&buf, &size, (isfile) ? fp : stdin);
 		for (int j = 0; buf[j] != '\0'; j++)
 			if (buf[j] == ',')
 				delms++;
@@ -367,11 +583,14 @@ int main(int argc, char **argv) {
 			}
 		}
 		/* Get X, and Y velocity. */
-		if(!coll) {
-			buf = NULL;
-			getline(&buf, &size, stdin);
+		buf = NULL;
+		getline(&buf, &size, (isfile) ? fp : stdin);
+		if (!coll) {
 			cursor.xvel = atof(strtok(buf, ","));
 			cursor.yvel = atof(strtok(NULL, "\n"));
+		} else {
+			floor.xvel = atof(strtok(buf, ","));
+			floor.yvel = atof(strtok(NULL, "\n"));
 		}
 
 		(!flt) ? (cursor.a1 = cursor.pos.x) : (cursor.c1 = cursor.pos.fx);
@@ -383,11 +602,12 @@ int main(int argc, char **argv) {
 		(!flt) ? (floor.b1 = floor.pos.y) : (floor.d1 = floor.pos.fy);
 		(!flt) ? (floor.b2 = floor.pos.y+floor.h) : (floor.d2 = floor.pos.fy+floor.h);
 
-		if (colltype == THM && coll)
-			thm(flt, hmpeak, cursor, floor);
-
-		if (colltype == AABB && coll)
-			aabb(flt, cursor, floor);
+		if (coll) {
+			switch (colltype) {
+				case THM : (!flt) ? thm(hmpeak, &cursor, &floor) : fthm(hmpeak, &cursor, &floor); break;
+				case AABB: (!flt) ? aabb(&cursor, &floor       ) : faabb(&cursor, &floor       ); break;
+			}
+		}
 	}
 	uint8_t a = 0;
 	unsigned int i = 0;
@@ -396,6 +616,5 @@ int main(int argc, char **argv) {
 		free(floor.heightmap);
 	}
 	fflush(stdout);
-
 	return 0;
 }
diff --git a/test-aabb b/test-aabb
index 899c7ad..3fc243a 100644
--- a/test-aabb
+++ b/test-aabb
@@ -4,3 +4,4 @@
 1,1
 16,16
 32,32
+0,0
diff --git a/test-mvmt.c b/test-mvmt.c
index 27f0aa3..e3bdb06 100644
--- a/test-mvmt.c
+++ b/test-mvmt.c
@@ -71,13 +71,13 @@ int startpipe(char *path, char *input , char *output, ssize_t *size) {
 				exit(EXIT_FAILURE);
 			}
 			if (WIFEXITED(status)) {
-				printf("\033[10;1H\033[2Kchild exited, status=%d", WEXITSTATUS(status));
+				/*printf("\033[24;1H\033[2Kchild exited, status=%d", WEXITSTATUS(status));*/
 			} else if (WIFSIGNALED(status)) {
-				printf("\033[10;1H\033[2Kchild killed (signal %d)", WTERMSIG(status));
+				printf("\033[24;1H\033[2Kchild killed (signal %d)", WTERMSIG(status));
 			} else if (WIFSTOPPED(status)) {
-				printf("\033[10;1H\033[2Kchild stopped (signal %d)", WSTOPSIG(status));
+				printf("\033[24;1H\033[2Kchild stopped (signal %d)", WSTOPSIG(status));
 			} else {
-				printf("\033[10;1H\033[2KUnexpected status (0x%x)", status);
+				printf("\033[24;1H\033[2KUnexpected status (0x%x)", status);
 			}
 		}
 		close(fd0[1]);
@@ -88,7 +88,7 @@ int startpipe(char *path, char *input , char *output, ssize_t *size) {
 	return 0;
 }
 
-void split(char *str, char **splitstr) {
+int split(char *str, char **splitstr) {
 	char *p;
 	int i=0;
 	p = strtok(str,"\n");
@@ -99,22 +99,26 @@ void split(char *str, char **splitstr) {
 		i++;
 		p = strtok (NULL, "\n");
 	}
+	return i;
 }
 
 int main (int argc, char **argv) {
-	uint8_t u = 0, d = 0, l = 0;
-	uint8_t r = 0;
+	uint8_t u = 0, u2 = 0;
+	uint8_t d = 0, d2 = 0;
+	uint8_t l = 0, l2 = 0;
+	uint8_t r = 0, r2 = 0;
 	int frame = 0;
         double x1 = 224, y1 = 128;
         int w1 = 16, h1 = 16;
 	double xvel = 0, yvel = 0;
         int x2 = 256, y2 = 256;
         int w2 = 64, h2 = 64;
+	double xvel2 = 0, yvel2 = 0;
 	int stat;
 	char *keys;
 	char *buf = malloc(8193);
 	FILE *fp;
-	fp = popen("actkbd -Ps", "r");
+	fp = popen("actkbd -PsN -d /dev/input/by-path/platform-i8042-serio-0-event-kbd", "r");
 	printf("\033[2J");
 	if (fp == NULL) {
 		fprintf(stderr, "Error occured when trying to run actkbd.\n");
@@ -126,44 +130,73 @@ int main (int argc, char **argv) {
 		char *tmp = malloc(4096);
 		char *input = malloc(2049);
 		char *path = malloc(4102);
-		char ln[129];
+		char ln[2049];
+		int lines = 0;
 		frame = atoi(strtok(buf, " "));
 		keys = strtok(NULL, " ");
 
 		for(int ch = 0; ch<sizeof(ln); ch++)
 			ln[ch] = '\0';
 		sprintf(path, "%s/clld", getcwd(tmp, 4096));
-		sprintf(input, "1\n%i,%i\n%i,%i\n%07.06f,%07.06f\n%i,%i\n%i,%i\n", w1, h1, (int)x1, (int)y1, xvel, yvel, w2, h2, x2, y2);
+		sprintf(input, "1\n%i,%i\n%i,%i\n%07.06f,%07.06f\n%i,%i\n%i,%i\n%07.06f,%07.06f\n", w1, h1, (int)x1, (int)y1, xvel, yvel, w2, h2, x2, y2, xvel2, yvel2);
 		startpipe(path, input, &ln, &size);
 		char *line[65] = {NULL};
-		split(ln, line);
+		lines = split(ln, line);
 		x1 = atof(strtok(line[2], ","));
 		y1 = atof(strtok(NULL, "\0"));
 		xvel = atof(strtok(line[3], ","));
 		yvel = atof(strtok(NULL, "\0"));
 
-		u = match(keys, "KEY_W");
-		d = (match(keys, "KEY_S") && !match(keys, "KEY_(SHIFT|SLASH|SPACE|SEMICOLON)"));
-		l = match(keys, "KEY_A") && !match(keys,"KEY_APOSTROPHE");
-		r = (match(keys, "KEY_D") && !match(keys, "KEY_(DOWN|DELETE|DOT)"));
+		x2 = atof(strtok(line[6], ","));
+		y2 = atof(strtok(NULL, "\0"));
+		/*xvel2 = atof(strtok(line[7], ","));
+		yvel2 = atof(strtok(NULL, "\0"));*/
+
+		u = match(keys, "17");
+		d = match(keys, "31");
+		l = match(keys, "30");
+		r = match(keys, "32");
+
+		u2 = match(keys, "72");
+		d2 = match(keys, "80");
+		l2 = match(keys, "75");
+		r2 = match(keys, "77");
 
 		(u) ? (yvel-=0.25) : ((yvel < 0) ? (d ? (yvel+=0.5) : (yvel+=0.25)) : (yvel=yvel));
 		(d) ? (yvel+=0.25) : ((yvel > 0) ? (u ? (yvel-=0.5) : (yvel-=0.25)) : (yvel=yvel));
 		(l) ? (xvel-=0.25) : ((xvel < 0) ? (r ? (xvel+=0.5) : (xvel+=0.25)) : (xvel=xvel));
 		(r) ? (xvel+=0.25) : ((xvel > 0) ? (l ? (xvel-=0.5) : (xvel-=0.25)) : (xvel=xvel));
+
 		yvel = (yvel > 5) ? 5 : (yvel < -5 ? -5 : yvel);
 		xvel = (xvel > 5) ? 5 : (xvel < -5 ? -5 : xvel);
+
 		x1+=xvel;
 		y1+=yvel;
 
+		(u2) ? (yvel2-=0.25) : ((yvel2 < 0) ? (d2 ? (yvel2+=0.5) : (yvel2+=0.25)) : (yvel2=yvel2));
+		(d2) ? (yvel2+=0.25) : ((yvel2 > 0) ? (u2 ? (yvel2-=0.5) : (yvel2-=0.25)) : (yvel2=yvel2));
+		(l2) ? (xvel2-=0.25) : ((xvel2 < 0) ? (r2 ? (xvel2+=0.5) : (xvel2+=0.25)) : (xvel2=xvel2));
+		(r2) ? (xvel2+=0.25) : ((xvel2 > 0) ? (l2 ? (xvel2-=0.5) : (xvel2-=0.25)) : (xvel2=xvel2));
+
+		yvel2 = (yvel2 > 5) ? 5 : (yvel2 < -5 ? -5 : yvel2);
+		xvel2 = (xvel2 > 5) ? 5 : (xvel2 < -5 ? -5 : xvel2);
+
+		x2+=xvel2;
+		y2+=yvel2;
+
 		printf("\033[1;1H\033[2K%s", keys);
 		printf(
 			"\033[E\033[2Kframe: %i, u: %u, d: %u, l: %u, r: %u"
 			"\033[E\033[2Kx1: %i, y1: %i"
 			"\033[E\033[2Kxvel: %07.06f, yvel: %07.06f"
 			"\n\033[2K", frame, u, d, l, r, (int)x1, (int)y1, xvel, yvel);
+		printf(
+			"\033[E\033[2Ku2: %u, d2: %u, l2: %u, r2: %u"
+			"\033[E\033[2Kx2: %i, y2: %i"
+			"\033[E\033[2Kxvel2: %07.06f, yvel2: %07.06f"
+			"\n\033[2K", u2, d2, l2, r2, (int)x2, (int)y2, xvel2, yvel2);
 
-		if (match(keys, "KEY_Q")) {
+		if (match(keys, "16")) {
 			printf("\033[2J\033[1;1H");
 			free(buf);
 			free(tmp);
diff --git a/test-thm-mvmt.sh b/test-thm-mvmt.sh
index be6a8c3..b8ef9b8 100755
--- a/test-thm-mvmt.sh
+++ b/test-thm-mvmt.sh
@@ -5,6 +5,8 @@ xvel=1
 xspd=$xvel
 yvel=1
 yspd=$yvel
+xspd2=$xvel
+yspd2=$yvel
 x2=256
 y2=256
 flip=0
@@ -21,18 +23,38 @@ d=0
 l=0
 r=0
 clear
-actkbd -Ps | \
+actkbd -Ps -d /dev/input/by-path/platform-i8042-serio-0-event-kbd | \
 while read frame key held; do
 	yspd=0 && xspd=0 && negx="" && negy="";
-        [ "${key#*KEY_W}" != "$key" -o "${key#*KEY_S}" != "$key" ] && yspd=$yvel
-        [ "${key#*KEY_A}" != "$key" -o "${key#*KEY_D}" != "$key" ] && xspd=$xvel
-        [ "${key#*KEY_W}" != "$key" ] && negy="-" && y1=$(echo "$y1-$yspd" | bc)
-        [ "${key#*KEY_S}" != "$key" ] && negy=""  && y1=$(echo "$y1+$yspd" | bc)
-        [ "${key#*KEY_A}" != "$key" ] && negx="-" && x1=$(echo "$x1-$xspd" | bc)
-        [ "${key#*KEY_D}" != "$key" ] && negx=""  && x1=$(echo "$x1+$xspd" | bc)
-	#printf "0\n$w1,$h1\n$x1,$y1\n$negx$xspd,$negy$yspd\n$w2,$h2\n$flip\n$heightmap\n$x2,$y2"  | ./clld
-	xy=$(printf "0\n$w1,$h1\n$x1,$y1\n$negx$xspd,$negy$yspd\n$w2,$h2\n$flip\n$heightmap\n$x2,$y2"  | ./clld | tail -n2 | tr -d ',' | head -n1)
+	yspd2=0 && xspd2=0 && negx2="" && negy2="";
+	if [ "$held" = "1" ]; then
+        	[ "${key#*KEY_W}" != "$key" -o "${key#*KEY_S}" != "$key" ] && yspd=$yvel
+        	[ "${key#*KEY_A}" != "$key" -o "${key#*KEY_D}" != "$key" ] && xspd=$xvel
+        	[ "${key#*KEY_W}" != "$key" ] && negy="-" && y1=$(echo "$y1-$yspd" | bc)
+        	[ "${key#*KEY_S}" != "$key" ] && negy=""  && y1=$(echo "$y1+$yspd" | bc)
+        	[ "${key#*KEY_A}" != "$key" ] && negx="-" && x1=$(echo "$x1-$xspd" | bc)
+        	[ "${key#*KEY_D}" != "$key" ] && negx=""  && x1=$(echo "$x1+$xspd" | bc)
+	fi
+
+	str=$(printf "0\n$w1,$h1\n$x1,$y1\n$negx$xspd,$negy$yspd\n$w2,$h2\n$flip\n$heightmap\n$x2,$y2"| ./clld)
+	cursor=$(printf "$str" | head -n5)
+	floor=$(printf "$str" | tail -n5)
+	xy=$(printf "$cursor" | tail -n2 | tr -d ',' | head -n1)
+	xy2=$(printf "$floor" | tail -n2 | tr -d ',' | head -n1)
 	x1=$(echo "$xy" | awk '{print $1}')
 	y1=$(echo "$xy" | awk '{print $2}')
-	printf "\33[1;1H\33[2K$x1 $y1\33[E\33[2K$negx$xspd $negy$yspd\n\33[2K"
+	x2=$(echo "$xy2" | awk '{print $1}')
+	y2=$(echo "$xy2" | awk '{print $2}')
+	printf "$cursor\n$floor"
+	#printf "\33[1;1H\33[2K$x1 $y1\33[E\33[2K$negx$xspd $negy$yspd\n\33[2K\n"
+	#printf "\33[2K$x2 $y2\33[E\33[2K$negx2$xspd2 $negy2$yspd2\n\33[2K\n"
+	if [ "$held" = "1" -a "${key#*KEY_KP}" != "$key" ]; then
+        	[ "${key#*KEY_KP8}" != "$key" -o "${key#*KEY_KP2}" != "$key" ] && yspd2=$yvel
+        	[ "${key#*KEY_KP4}" != "$key" -o "${key#*KEY_KP6}" != "$key" ] && xspd2=$xvel
+        	[ "${key#*KEY_KP8}" != "$key" ] && negy2="-" && y2=$(echo "$y2-$yspd2" | bc)
+        	[ "${key#*KEY_KP6}" != "$key" ] && negy2=""  && y2=$(echo "$y2+$yspd2" | bc)
+        	[ "${key#*KEY_KP4}" != "$key" ] && negx2="-" && x2=$(echo "$x2-$xspd2" | bc)
+        	[ "${key#*KEY_KP6}" != "$key" ] && negx2=""  && x2=$(echo "$x2+$xspd2" | bc)
+	fi
+
 done
diff --git a/test-tile b/test-tile
index 998ccb5..4934bf9 100644
--- a/test-tile
+++ b/test-tile
@@ -6,3 +6,4 @@
 0
 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16
 32,32
+0,0`