Add coordinate system

1 files changed, 39 insertions, 21 deletions

diff --git a/src/turtle-system.lisp b/src/turtle-system.lisp
index 5e2bd3d..807986b 100644
--- a/src/turtle-system.lisp
+++ b/src/turtle-system.lisp
@@ -10,7 +10,9 @@
 
 (defun turtle-system (fn list radians)
   (iter (with pos = (sb-cga:vec 0.0 0.0 0.0))
-	(with vec = (sb-cga:vec 0.0 1.0 0.0))
+	(with vec-x = (sb-cga:vec 1.0 0.0 0.0))
+	(with vec-y = (sb-cga:vec 0.0 1.0 0.0))
+	(with vec-z = (sb-cga:vec 0.0 0.0 1.0))
 	(with pile = '())
 	(with angle = radians)
 	(for item in list)
@@ -18,49 +20,65 @@
 	  ;;Move forward one unit,adding data to mesh.
 	  ((f)
 	   (let ((new-pos
-		  (vec+ pos vec)))
-	     (appending (funcall fn pos new-pos vec))
+		  (vec+ pos vec-y)))
+	     (appending (funcall fn pos new-pos vec-x vec-y vec-z))
 	     (setf pos new-pos)))
 	  ;;Move forward one unit,without adding data to mesh.
 	  ((j)
 	   (setf pos
 		 (vec+ pos
-		       vec)))
+		       vec-y)))
 	  ;;Rotate left on axis z
 	  ((+)
-	   (setf vec
-		 (vec-rotate-around vec (vec 0.0 0.0 1.0) angle)))
+	   (setf vec-x
+		 (vec-rotate-around vec-x vec-z angle))
+	   (setf vec-y
+		 (vec-rotate-around vec-y vec-z angle)))
 	  ;;Rotate right on axis z
 	  ((-)
-	   (setf vec
-		 (vec-rotate-around vec (vec 0.0 0.0 -1.0) angle)))
+	   (setf vec-x
+		 (vec-rotate-around vec-x vec-z (- angle)))
+	   (setf vec-y
+		 (vec-rotate-around vec-y vec-z (- angle))))
 	  ;;Rotate left on axis y
 	  ((&)
-	   (setf vec
-		 (vec-rotate-around vec (vec 0.0 1.0 0.0) angle)))
+	   (setf vec-x
+		 (vec-rotate-around vec-x vec-y angle))
+	   (setf vec-z
+		 (vec-rotate-around vec-z vec-y angle)))
 	  ;;Rotate right on axis y
 	  ((^)
-	   (setf vec
-		 (vec-rotate-around vec (vec 0.0 -1.0 0.0) angle)))
+	   (setf vec-x
+		 (vec-rotate-around vec-x vec-y (- angle)))
+	   (setf vec-z
+		 (vec-rotate-around vec-z vec-y (- angle))))
 	  ;;Rotate left on axis x
 	  ((\ )
-	   (setf vec
-		 (vec-rotate-around vec (vec 1.0 0.0 0.0) angle)))
+	   (setf vec-z
+		 (vec-rotate-around vec-z vec-x angle))
+	   (setf vec-y
+		 (vec-rotate-around vec-y vec-x angle)))
 	  ;;Rotate right on axis x
 	  ((/)
-	   (setf vec
-		 (vec-rotate-around vec (vec -1.0 0.0 0.0) angle)))
+	   (setf vec-z
+		 (vec-rotate-around vec-z vec-x (- angle)))
+	   (setf vec-y
+		 (vec-rotate-around vec-y vec-x (- angle))))
 	  ;;Push the current turtle state onto a stack
 	  (([)
-	   (push (list pos vec)
+	   (push (list pos vec-x vec-y vec-z)
 		 pile))
 	  ;;Pop the turtle stack, restoring an earlier state
 	  ((])
 	   (let* ((asd (pop pile))
-		  (pos0 (first asd))
-		  (vec0 (second asd)))
-	     (setf pos pos0)
-	     (setf vec vec0))))))
+		  (pos0 (pop asd))
+		  (vec-x0 (pop asd))
+		  (vec-y0 (pop asd))
+		  (vec-z0 (pop asd)))
+	     (setf pos pos0
+		   vec-x vec-x0
+		   vec-y vec-y0
+		   vec-z vec-z0))))))
 
 ;;; Turgle utils