Skip to navigation

⌂ ∞ ▽ The Sentinel on the BBC Micro

Maths (Geometry): GetAngleFromCoords (Part 3 of 3)

       Name: GetAngleFromCoords (Part 3 of 3)                        [Show more]
       Type: Subroutine
   Category: Maths (Geometry)
    Summary: Calculate the arctangent to get the angle

    Context: See this subroutine in context in the source code
 References: No direct references to this subroutine in this source file




.gang11

                        \ By this point we have calculated the following:
                        \
                        \   A = 256 * (A T) / (V W)
                        \
                        \ so now we calculate arctan(A)

 TAY                    \ Set Y to the value of A so we can use it as an index
                        \ into the arctan tables

 STA angleTangent       \ Set angleTangent = 256 * (A T) / (V W)
                        \
                        \ So we return the tangent from the routine in
                        \ angleTangent

 LDA arctanLo,Y         \ Set angle(Hi Lo) = arctan(Y)
 STA angleLo
 LDA arctanHi,Y
 STA angleHi

                        \ We now improve the accuracy of this result by applying
                        \ interpolation and rounding, but only if one of bit 6
                        \ or bit 7 is set in G

 BIT G                  \ If bit 7 of G is set, jump to gang12 to calculate
 BMI gang12             \ a value in (U T) that we can then use for rounding up
                        \ the result of the interpolation

 BVS gang14             \ If bit 7 of G is clear and bit 6 of G is set, jump to
                        \ gang14 to interpolate the result with the next entry
                        \ in the arctan table, without using rounding

 JMP gang16             \ Otherwise both bit 7 and bit 6 of G must be clear, so
                        \ jump to gang16 to return from the subroutine, as the
                        \ result in angle(Hi Lo) is already accurate and doesn't
                        \ need interpolating

.gang12

                        \ If we get here then we need to apply rounding to the
                        \ result as well as interpolating the result between
                        \ arctan(Y) and arctan(Y + 1)

 LDA angleLo            \ Set (A T) = angle(Hi Lo) - arctan(Y + 1)
 SEC                    \           = arctan(Y) - arctan(Y + 1)
 SBC arctanLo+1,Y       \
 STA T                  \ So (A T) contains the amount of rounding we need to
 LDA angleHi            \ add to the result (the result is divided by two below)
 SBC arctanHi+1,Y

 BIT G                  \ If bit 6 of G is set, negate (A T) to give the correct
 BVC gang13             \ sign to the amount of rounding
 JSR Negate16Bit

.gang13

 STA U                  \ Set (U T) = (A T)
                        \           = arctan(Y) - arctan(Y + 1)

 ROL A                  \ Set the C flag to bit 7 of A, which is the top bit of
                        \ (U T), so the next instruction rotates the correct
                        \ bit into bit 7 of U to retain the sign of the result

 ROR U                  \ Set (U T) = (U T) / 2
 ROR T                  \           = arctan(Y) - arctan(Y + 1)
                        \
                        \ So (U T) contains half the difference between
                        \ arctan(Y) and arctan(Y + 1)
                        \
                        \ This is effectively half the difference between the
                        \ two values, so this is the equivalent of the 0.5 in
                        \ INT(x + 0.5) when rounding x to the nearest integer,
                        \ just with the arctan results in our calculation

.gang14

                        \ If we get here then we need to interpolate the result
                        \ between arctan(Y) and arctan(Y + 1)

 LDA angleLo            \ Set angle(Hi Lo) = angle(Hi Lo) + arctan(Y + 1)
 CLC                    \                  = arctan(Y) + arctan(Y + 1)
 ADC arctanLo+1,Y       \
 STA angleLo            \ We will divide this value by two to get the average
 LDA angleHi            \ of arctan(Y) and arctan(Y + 1), but first we need to
 ADC arctanHi+1,Y       \ add the rounding in (U T), if applicable
 STA angleHi

 BIT G                  \ If bit 7 of G is clear, jump to gang15 to skip the
 BPL gang15             \ following, as the rounding in (U T) is only applied
                        \ when bit 7 of G is set

                        \ If we get here then bit 7 of G is set and we need to
                        \ add on the rounding in (U T)

 LDA angleLo            \ Set angle(Hi Lo) = angle(Hi Lo) + (U T)
 CLC
 ADC T
 STA angleLo
 LDA angleHi
 ADC U
 STA angleHi

.gang15

 LSR angleHi            \ Set angle(Hi Lo) = angle(Hi Lo) / 2
 ROR angleLo            \
                        \ So we now have the average value of arctan(Y) and
                        \ arctan(Y + 1), including rounding if applicable, so
                        \ the result has now been interpolated for more accuracy

.gang16

 RTS                    \ Return from the subroutine