LOGO-L> Substitute for "setorientation towardsxyz"

["Tom Lynn" <thl22@cam.ac.uk>]

Hi.

After a few hours of pushing vectors around a piece of paper, moving 
protractors around at funny angles in the air and finally actually doing 
some coding, I've come up with this substitute for "setorientation 
towardsxyz".

The reason I think it's necessary is that after doing "setorientation 
towardsxyz [100 100 100]", the turtle ends up pointing towards [100 100 
100], but if you follow this with "rt 90", you can't be sure where it's 
pointing.  So we need some rule which will fix the turtle's final position.

With the new procedure "settowardsxyz" given here, the new orientation of 
the turtle depends on the initial position.  To orientate itself towards a 
point, the turtle turns left or right until it is "above" or "below" the 
point and then pitches its nose up or down accordingly.  I find this rule 
makes visualising the final orientation of the turtle relatively easy.

The other procedures given here are just to test settowardsxyz to see that 
it works.  Totally off-subject: A strange problem I had was that when I was 
flooding the commander with output (uncomment the "print ..." lines) it 
occasionally clicked and output blank lines on one computer I tried it on.

George did say he was going to look at this problem too, so he may well 
have come up with another solution.

Tom

---
Begin "settowardsxyz.lgo"
I don't trust my email program with attachments any more. :(
---
to _test :p :q
; Test that settowardsxyz works between points :p and :q
  pu setposxyz :p pd settowardsxyz :q fd distancexyz :q
; (print "Result: posxyz)
end

to aaa
; Draws some axes (pretty pointless :) and then plots points at random
; directions and checks that the turtle has actually reached the correct
; point.

  cs perspective ht
  ask -1 [setposxyz [1000 1000 1000]]
  drawaxes

  ; Comment out the next line to keep record of maxerror between tests
  localmake "maxerror 0

  repeat 1000 [
    localmake "x 100 * (3 - random 6)
    localmake "y 100 * (3 - random 6)
    localmake "z 100 * (3 - random 6)

    localmake "px 300 - random 600
    localmake "py 300 - random 600
    localmake "pz 300 - random 600

    ; print (combine "From: (list :px :py :pz))
    ; print (combine "To: (list :x :y :z))
    _test (list :px :py :pz) (list :x :y :z)
    
    if :maxerror < abs ((first posxyz) - :x) [
      make "maxerror abs ((first posxyz) - :x)
    ]

    if :maxerror < abs ((first butfirst posxyz) - :y) [
      make "maxerror abs ((first butfirst posxyz) - :y)
    ]

    if :maxerror < abs ((first butfirst butfirst posxyz) - :z) [
      make "maxerror abs ((first butfirst butfirst posxyz) - :z)
    ]
  ]
  print (list "Maximum "error: :maxerror)
end

to dotprod :a :b
; Returns the dot product of vectors :a and :b.
  output apply "sum (map [?1 * ?2] :a :b)
end

to drawaxes
; Draws the positive parts of the grid axes 
  pu home pd settowardsxyz [1000 0 0] fd distancexyz [1000 0 0]
  pu home pd settowardsxyz [0 1000 0] fd distancexyz [0 1000 0]
  pu home pd settowardsxyz [0 0 1000] fd distancexyz [0 0 1000]
end

to settowardsxyz :F
; setorientation towardsxyz does not completely define the new orientation
; of the turtle. settowardsxyz defines the new orientation of the turtle by
; these rules:
; 
; 1. Turns right until the target position is in the same pitch plane (sets
; yaw).
;
; 2. Pitch up or down until headed towards the target (sets pitch).
;
; I haven't found a situation where this gives an inappropriate
; orientation, but there probably is one, seeing as this is a hard problem.
; There are another three complementary functions which could be defined,
; which set pitch then yaw, roll then pitch and roll then yaw.  It might be
; useful to have these as well, but someone else can do them :)

  localmake "InitialPenState pendownp
  localmake "T posxyz
  localmake "fvec vecsub :F :T
  pu fd 1 localmake "cvec vecsub posxyz :T bk 1
  rt 90 fd 1 localmake "rvec vecsub posxyz :T bk 1 lt 90
  down 90 fd 1 localmake "nvec vecsub posxyz :T bk 1 up 90
  localmake "avec vecscale (dotprod :fvec :cvec) :cvec
  localmake "bvec vecscale (dotprod :fvec :rvec) :rvec
  localmake "svec vecadd :avec :bvec

  ifelse (veclen :svec) = 0 [
    ifelse (dotprod :fvec :nvec) < 0 [up 90] [down 90]
  ] [
    localmake "costheta (dotprod :svec :cvec) / veclen :svec
    if (abs :costheta) > 1 [  ; May happen due to numerical errors
      ifelse :costheta < 0 [localmake "costheta -1][localmake "costheta 1]
    ]
    localmake "theta arccos :costheta
    ifelse (dotprod :fvec :rvec) < 0 [lt :theta] [rt :theta]

    if not ((veclen :fvec) = 0) [
      localmake "cosalpha ((dotprod :svec :fvec) / ((veclen :svec) * ~
        (veclen :fvec)))
      if (abs :cosalpha) > 1 [  ; May happen due to numerical errors
        ifelse :cosalpha < 0 [localmake "cosalpha -1][ ~
          localmake "cosalpha 1]
      ]

      localmake "alpha arccos :cosalpha
      ifelse (dotprod :fvec :nvec) < 0 [up :alpha] [down :alpha]
    ]
  ]

  ifelse :InitialPenState [pd][pu]
end

to vecadd :a :b
; Adds vectors :a and :b
  output (map [?1 + ?2] :a :b)
end

to vecdiv :v :s
; Divides vector :v by scalar :s
  output map (list "? "/ :s) :v
end

to veclen :v
; Returns the length of vector :v
  localmake "a first :v
  localmake "b first butfirst :v
  localmake "c first butfirst butfirst :v
  output sqrt(:a*:a + :b*:b + :c*:c)
end

to vecscale :s :v
; Multiplies vector :v by scalar :s
  output map (list "? "* :s) :v
end

to vecsub :a :b
; Subtracts 3d vector :b from 3d vector :a
  output (map [?1 - ?2] :a :b)
end
---------------------------------------------------------------
Please post messages to the Logo forum to logo-l@gsn.org.  Mail
questions about the list administration to logofdn@gsn.org.  To
unsubscribe send    unsubscribe logo-l    to majordomo@gsn.org.