diff --git a/hough_machine.cpp b/hough_machine.cpp
index 0d3330f..4c6a252 100644
--- a/hough_machine.cpp
+++ b/hough_machine.cpp
@@ -140,22 +140,44 @@ class HoughMachine {
 	double theta = lines[i].theta;
 	int r = lines[i].r;
 	int quadrant = floor(theta / pi_half);
-	theta = theta - quadrant * pi_half;
 	std::cout << "Top " << i << ": " << lines[i].value << ", r: " << r << ", theta: " << theta << " quadrand: " << quadrant;
-	int x_inter = r / cos(theta);
-	int y_inter = r / cos(pi_half - theta);
-	//if(quadrant % 2 == 1) x_inter *= -1;
-	//if(quadrant > 2) y_inter *= -1;
-	//double x_factor = (this->x_center + x_inter) / x_inter;
-	//double y_factor = (this->y_center + y_inter) / y_inter;
-	//x_inter *= x_factor;
-	//y_inter *= y_factor;
-	int x1 = 0; int x2 = 0; int y1 = 0; int y2 = 0;
-	std::cout << " x: " << x_inter << ", y: " << y_inter << std::endl;
-	x1 = 0; y1 = y_inter;
-	x2 = x_inter; y2 = 0;
-	this->otherBresenham(x1, y1, x2, y2);
 
+	// cos(pi/8) -sin(pi/8)
+        // sin(pi/8) cos(pi/8)
+        // Do matrix multiplication
+	// Turn r into a x/y vector
+        double vec_x = r * cos(theta) + 0 * -sin(theta);
+        double vec_y = r * sin(theta) + 0 * cos(theta);
+	// find point in image
+	int x = vec_x + this->x_center;
+	int y = vec_y + this->y_center;
+	std::cout << " vec x: " << x << ", vec y: " << y << std::endl;
+	// create vector for line
+	double vec_target_x = vec_y;
+	double vec_target_y = -vec_x;
+	// normalize it
+	double vec_target_len = sqrt(pow(vec_target_x, 2) + pow(vec_target_y, 2));
+	vec_target_x = vec_target_x / vec_target_len;
+	vec_target_y = vec_target_y / vec_target_len;
+	// find first out of bounds condition
+	double i = x;
+	double j = y;
+	while(i < this->working_copy->width() && i > 0 && j < this->working_copy->height() && j > 0) {
+	  i += vec_target_x;
+	  j += vec_target_y;
+	}
+	int x1 = i;
+	int y1 = j;
+	i = x;
+	j = y;
+	while(i < this->working_copy->width() && i > 0 && j < this->working_copy->height() && j > 0) {
+	  i -= vec_target_x;
+	  j -= vec_target_y;
+	}
+	int x2 = i;
+	int y2 = j;
+	// TODO: Draw the lines
+	this->otherBresenham(x1, y1, x2, y2);
       }
       
     };
@@ -174,6 +196,7 @@ class HoughMachine {
       int our_color = QColor::fromRgb(0, 255, 0).rgb();
       if(color == 0) color = our_color;
       //std::cout << " --> SET (" << x << ", " << y << ")" << std::endl;
+      if(x < 0 || y < 0 || x > this->working_copy->width() || y > this->working_copy->height()) return;
       this->working_copy->getImage()->setPixel(x, y, color);
     };
     
diff --git a/imageviewer-qt4.cpp b/imageviewer-qt4.cpp
index 9781119..186bb86 100644
--- a/imageviewer-qt4.cpp
+++ b/imageviewer-qt4.cpp
@@ -792,7 +792,7 @@ void ImageViewer::runHoughTransformation(void) {
   hm->setMatchingThreshold(threshold);
   //hm->run(256, 256);
   hm->run(300, 300);
-  hm->drawLines(40);  
+  hm->drawLines(70);  
   delete hm;
   updateImageDisplay();  
 }