Re: LOGO-L> Substitute for "setorientation towardsxyz"

[George Mills <mills@softronix.com>]

Tom Lynn wrote:
> 
> ; setorientation towardsxyz does not completely define the new orientation
> ; of the turtle. settowardsxyz defines the new orientation of the turtle by
> ; these rules:
>

setorientation towardsxyz most certainly does completely define a new orientation.
I do agree the documentation could use better explanation of what it does and
in fact I had to experiment with it to remind myself exactly what it did :-)
The difference between settowardsxyz and setorientation towardsxyz is that
settowardsxyz is relative and setorientation towardsxyz is absolute. What does
setorientation towardsxyz do? It is really quit simple and easy to explain.
First it orients the turtle to the "HOME" orientation, then it rolls until the
turtle is in the same plane as the desired destination point and then turns (RT)
towards it (no pitch is used). This is much like longitude and latitude and the
turtle will have a consistent explainable roll. But it is not based on the
current orientation.

I liked Tom's solutions and was ready to *replace* towardsxyz with his method
until I realized they are both useful and I don't think the current towardsxyz
should be eliminated. The thing that convinced me is this.

To be at an arbitrary orientation and then "minimally" and "predictably"
turn and pitch towards a new destination point sometimes requires a very high
level understanding to understand what your roll will be. But it does
extend TOWARDSXYZ applications to "relative" maneuvers. What is attractive
about it is that if it's already pointing at the destination, nothing changes.
Or if it requires just a turn or a pitch that's all that's applied. But if it
requires both maneuvers it gets more complex to comprehend or explain your new
roll at the new orientation.

When I did SETORIENTATION, TOWARDSXYZ etc. I expected it to be used in
an absolute context. Much like SETHEADING, SETPOS is used in 2D. TOWARDS (2D)
did not have this problem but TOWARDSXYZ could be "correct" as either
an absolute function or as a relative one. SETORIENTATION TOWARDSXYZ
is actually much like Tom's SETTOWARDSXYZ and is actually one of
the other combinations Tom spoke of but is simply based on the HOME
orientation rather than your current one. It's really a matter of taste
and the problem at hand. So lets just have both.

To avoid confusion I took the liberty of renaming Tom's SETTOWARDSXYZ
to TURNTOWARDSXYZ to remind you that it's relative. I also cleaned things
up a bit (partially for my own taste) but also to make it ready for
your LOGOLIB insertion and added some performance improvements (by hiding the
turtle while it gains some information about the turtles orientation).
That is, it redraws the turtle once rather than 7 times if it's currently
shown.  I will likely add this to the MSWLogo logolib.

Sorry for using third person Tom, I never know whether to write to the group
or the person posting. I sort of went back and forth.

Those not using the Win9x/NT kit should save this LOGOLIB file as TURNTOWA.
Those using the Win9x/NT kit should save this LOGOLIB file as TURNTOWARDSXYZ.

to turntowardsxyz :F
;
; Written By Tom Lynn <thl22@cam.ac.uk>
;
; turntowardsxyz 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).
;
; 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.
;
  localmake "InitialShownState shownp
  localmake "InitialDownState pendownp
  localmake "T posxyz

  localmake "fvec vecsub :F :T
  ht
  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]
    ] ~
; else ~
    [
    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]
      ]
    ]

  if :InitialDownState [pd]
  if :InitialShownState [st]
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

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

bury "vecadd
bury "vecsub
bury "vecscale
bury "veclen
bury "vecdiv
bury "settowardsxyz
bury "dotprod


Tom Lynn wrote:
> 
> 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.

--
===============================================================
George Mills (mills@softronix.com)
http://www.softronix.com/
The www page contains some very powerful educational software.
Our single most important investment is our kids.
---------------------------------------------------------------
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.