diff --git a/head_pose.py b/head_pose.py
index 85c3c69..6184bd1 100644
--- a/head_pose.py
+++ b/head_pose.py
@@ -88,20 +88,6 @@ while True:
             p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
             cv2.line(im, p1, p2, (255,0,0), 2)
 
-            # Translation vector gives position in space:
-            # x, y z: 0,0,0 is center of camera
-            # line: (x,y,z) = f(a) = (t1 + r1*a, t2+r2*a, t3 + r3*a)
-            # Screen: (x,y,z) = (x,y,0)
-            # Interesection:
-            #   x = t1 + r1 * a
-            #   y = t2 + r2 * a
-            #   z = t3 * r3 * a = 0
-            #       => a = -t3 / r3
-            #   substitute found a in x,y
-            
-            a = - translation_vector[2] / rotation_vector[2]
-            x = translation_vector[0] + rotation_vector[0] * a
-            y = translation_vector[1] + rotation_vector[1] * a
 
             rotMatrix = np.zeros([3,3])
             cv2.Rodrigues(rotation_vector, rotMatrix, jacobian=0)
@@ -135,9 +121,36 @@ while True:
             cv2.circle(im, (mapPosZ + 60, mapPosY + 10), 2, (0,0,255), -1)
 
             
-            cv2.line(im, (mapPosZ + 10, mapPosX + 10), (mapPosZ + 10 + int(rotation_vector[2]*5), mapPosX + 10  + int(rotation_vector[0]*5)), (255,255,0), 1)
-            cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60 + int(rotation_vector[2]*5), mapPosY + 10  + int(rotation_vector[1]*50)), (255,255,0), 1)
-            # print (a, x, y, rotMatrix)
+            # cv2.line(im, (mapPosZ + 10, mapPosX + 10), (mapPosZ + 10 + int(rotation_vector[2]*5), mapPosX + 10  + int(rotation_vector[0]*5)), (255,255,0), 1)
+            # cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60 + int(rotation_vector[2]*5), mapPosY + 10  + int(rotation_vector[1]*200)), (255,255,0), 1)
+
+            # print(rotMatrix)
+            viewDirectionVector = np.dot(np.array([0.0, 0.0, 1000.0]), rotMatrix)
+            cv2.line(im, (mapPosZ + 10, mapPosX + 10), (mapPosZ + 10 + int(viewDirectionVector[2] * 100), mapPosX + 10  + int(viewDirectionVector[0] * 100)), (255,255,0), 1)
+            cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60 + int(viewDirectionVector[2] * 100), mapPosY + 10  - int(viewDirectionVector[1] * 100)), (255,0,255), 1)
+
+
+            # Translation vector gives position in space:
+            # x, y z: 0,0,0 is center of camera
+            # line: (x,y,z) = f(a) = (t1 + r1*a, t2+r2*a, t3 + r3*a)
+            # Screen: (x,y,z) = (x,y,0)
+            # Interesection:
+            #   x = t1 + r1 * a
+            #   y = t2 + r2 * a
+            #   z = t3 * r3 * a = 0
+            #       => a = -t3 / r3
+            #   substitute found a in x,y
+            
+            a = - translation_vector[2] / rotation_vector[2]
+            x = translation_vector[0] + rotation_vector[0] * a
+            y = translation_vector[1] + rotation_vector[1] * a
+
+            a = - translation_vector[2] / viewDirectionVector[2]
+            x = translation_vector[0] + viewDirectionVector[0] * a
+            y = translation_vector[1] + viewDirectionVector[1] * a
+
+
+            print (a, x, y)
          
     # draw little floorplan for 10 -> 50, sideplan 60 -> 100 (40x40 px)
     cv2.rectangle(im, (9, 9), (51, 51), (255,255,255), 1)
@@ -147,7 +160,10 @@ while True:
 
     # Display image
     cv2.imshow("Output", im)
-    if cv2.waitKey(5)==27:
+    keyPress = cv2.waitKey(5)
+
+    if keyPress==27:
         break
 
+
 cv2.destroyAllWindows()