Maths (Geometry): AddVectorToCoord

       Name: AddVectorToCoord                                        [Show more]
       Type: Subroutine
   Category: Maths (Geometry)
    Summary: Add a vector to a coordinate
    Context: See this subroutine in context in the source code
 References: This subroutine is called as follows:
             * FollowGazeVector (Part 1 of 5) calls AddVectorToCoord


 This routine adds a vector to a coordinate:

   [ xCoord ]   [ xCoord ]   [ xVector ]
   [ yCoord ] = [ yCoord ] + [ yVector ]
   [ zCoord ]   [ zCoord ]   [ zVector ]

 where the coordinate consists of 24-bit signed numbers:

   [ xCoord ]   [ xCoord(Hi Lo Bot) ]
   [ yCoord ] = [ yCoord(Hi Lo Bot) ]
   [ zCoord ]   [ zCoord(Hi Lo Bot) ]

 and the vector consists of 16-bit signed numbers:

   [ xVector ]   [ xVector(Lo Bot) ]
   [ yVector ] = [ yVector(Lo Bot) ]
   [ zVector ]   [ zVector(Lo Bot) ]


.AddVectorToCoord

 LDX #2                 \ We now work through all three axes in turn, so set an
                        \ axis counter in X to iterate through 2, 1 and 0 (for
                        \ the z-axis, y-axis and x-axis respectively)
                        \
                        \ The comments in the following loop will concentrate on
                        \ the x-axis to keep things simple

.addv1

                        \ We now perform the following addition of a 24-bit
                        \ coordinate and a 16-bit vector:
                        \
                        \   xCoord = xCoord + xVector
                        \
                        \ where:
                        \
                        \   * xCoord is xCoord(Hi Lo Bot)
                        \
                        \   * xVector is xVector(Lo Bot)
                        \
                        \ We do this for each axis in turn, but let's talk about
                        \ the x-axis

 LDA #0                 \ Set T = 0
 STA T                  \
                        \ We use T as the high byte of xVector, which we either
                        \ set to 0 (if xVector is positive) or &FF (if xVector
                        \ is negative)
                        \
                        \ We set T for positive numbers here, and change it to
                        \ &FF during the addition if xVector turns out to be
                        \ negative
                        \
                        \ The addition therefore supports signed numbers

 LDA xCoordBot,X        \ Add the bottom bytes of the calculation
 CLC
 ADC xVectorBot,X
 STA xCoordBot,X

 LDA xVectorLo,X        \ Set A to xVectorLo so we can check its sign

 BPL addv2              \ If xVectorLo is negative, set T = &FF so we can use it
 DEC T                  \ as the high byte in the negative 24-bit number, like
                        \ this:
                        \
                        \   (&FF xVectorLo xVectorBot)

.addv2

 ADC xCoordLo,X         \ Now add the low bytes of the calculation
 STA xCoordLo,X

 LDA xCoordHi,X         \ And then add the high bytes, incorporating the high
 ADC T                  \ byte of xVector that we set in T
 STA xCoordHi,X

 DEX                    \ Decrement the axis counter in X to move on to the next
                        \ axis

 BPL addv1              \ Loop back until we have processed all three axes

 RTS                    \ Return from the subroutine

 EQUB &00               \ This byte appears to be unused