1  use <../../matrix/m_rotation.scad>
  2  
  3  function _create(pt, unit_vts) = [pt, unit_vts];
  4  function _create_default() = _create(
  5      [0, 0, 0], 
  6      // unit vectors from the turtle's viewpoint
  7      [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 
  8  );
  9  
 10  function _pt(turtle) = turtle[0];
 11  function _unit_vts(turtle) = turtle[1];
 12  
 13  // forward the turtle in the x' direction
 14  function _xu_move(turtle, leng) = _create(
 15      _pt(turtle) + _unit_vts(turtle).x * leng,
 16      _unit_vts(turtle)
 17  );
 18  
 19  // forward the turtle in the y' direction
 20  function _yu_move(turtle, leng) = _create(
 21      _pt(turtle) + _unit_vts(turtle).y * leng,
 22      _unit_vts(turtle)
 23  );
 24  
 25  // forward the turtle in the z' direction
 26  function _zu_move(turtle, leng) = _create(
 27      _pt(turtle) + _unit_vts(turtle).z * leng,
 28      _unit_vts(turtle)
 29  );
 30  
 31  function _q_rotation(a, v) = 
 32      let(
 33          uv = v / norm(v),
 34          s = sin(a / 2) * uv,
 35          w = sin(a) * uv,
 36  
 37          xx = 2 * s.x ^ 2,
 38          yy = 2 * s.y ^ 2,
 39          zz = 2 * s.z ^ 2,
 40  
 41          xy = 2 * s.x * s.y,
 42          xz = 2 * s.x * s.z,
 43          yz = 2 * s.y * s.z
 44      )
 45      [
 46          [1 - yy - zz, xy - w.z, xz + w.y],
 47          [xy + w.z, 1 - xx - zz, yz - w.x],
 48          [xz - w.y, yz + w.x, 1 - xx - yy]
 49      ];
 50      
 51  // turn the turtle around the x'-axis
 52  // return a new unit vector
 53  function _xu_turn(turtle, a) = 
 54      let(
 55          unit_vts = _unit_vts(turtle),
 56          xu = unit_vts.x,
 57          m = _q_rotation(a, xu)
 58      )
 59      _create(
 60          _pt(turtle),
 61          [
 62              xu, 
 63              m * unit_vts.y, 
 64              m * unit_vts.z
 65          ]
 66      );
 67  
 68  // turn the turtle around the y'-axis
 69  // return a new unit vector
 70  function _yu_turn(turtle, a) = 
 71      let(
 72          unit_vts = _unit_vts(turtle),
 73          yu = unit_vts.y,
 74          m = _q_rotation(a, yu)
 75      )
 76      _create(
 77          _pt(turtle),
 78          [
 79              m * unit_vts.x, 
 80              yu, 
 81              m * unit_vts.z
 82          ]
 83      );
 84  
 85  // turn the turtle around the z'-axis
 86  // return a new unit vector
 87  function _zu_turn(turtle, a) = 
 88      let(
 89          unit_vts = _unit_vts(turtle),
 90          zu = unit_vts.z,
 91          m = _q_rotation(a, zu)
 92      )
 93      _create(
 94          _pt(turtle),
 95          [
 96              m * unit_vts.x, 
 97              m * unit_vts.y,
 98              zu
 99          ]
100      );
101  
102  function _create_cmd(arg1, arg2) = 
103      is_undef(arg1) && is_undef(arg2) ? _create_default() : 
104      !is_undef(arg1) && !is_undef(arg2) ? _create(arg1, arg2) : undef;
105      
106  function _chain_move(cmd, arg1, arg2) =
107      cmd == "forward" || cmd == "xu_move" ? _xu_move(arg1, arg2) : 
108      cmd == "yu_move" ? _yu_move(arg1, arg2) : 
109      cmd == "zu_move" ? _zu_move(arg1, arg2) : _chain_turn(cmd, arg1, arg2);
110      
111  function _chain_turn(cmd, arg1, arg2) = 
112      cmd == "roll" ? _xu_turn(arg1, -arg2) :
113      cmd == "pitch" ? _yu_turn(arg1, -arg2) : 
114      cmd == "turn" || cmd == "zu_turn" ? _zu_turn(arg1, arg2) :
115      cmd == "xu_turn" ? _xu_turn(arg1, arg2) : 
116      cmd == "yu_turn" ? _yu_turn(arg1, arg2) : _chain_one_arg(cmd, arg1);    
117  
118  function _chain_one_arg(cmd, arg) = 
119      cmd == "pt" ? _pt(arg) : 
120      cmd == "unit_vts" ? _unit_vts(arg) : undef;
121      
122  function _turtle3d_impl(cmd, arg1, arg2) =
123      cmd == "create" ? 
124          _create_cmd(arg1, arg2) : 
125          _chain_move(cmd, arg1, arg2);