Skip to navigation

⌂ ∞ ▽ Revs on the BBC Micro

Maths (Geometry): GetRotationMatrix (Part 2 of 5)

       Name: GetRotationMatrix (Part 2 of 5)                         [Show more]
       Type: Subroutine
   Category: Maths (Geometry)
    Summary: Calculate sin(H G) for smaller angles
  Deep dive: The core driving model
             Trigonometry

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




.rotm2

                        \ If we get here then (U G) = yawRadians / 2 and U < 122

 LDA #171               \ Set A = 171

 JSR Multiply8x8        \ Set (A T) = (A * U) * U
 JSR Multiply8x8        \           = A * U^2
                        \           = 171 * (yawRadians / 2)^2

 STA V                  \ Set (V T) = (A T)
                        \           = 171 * (yawRadians / 2)^2

 JSR Multiply8x16       \ Set (U T) = U * (V T) / 256
                        \           = (171 / 256) * (yawRadians / 2)^3
                        \           = 2/3 * (yawRadians / 2)^3

 LDA G                  \ Set (A T) = (H G) - (U T)
 SEC                    \           = yawRadians / 2 - 2/3 * (yawRadians / 2)^3
 SBC T                  \
 STA T                  \ starting with the low bytes

 LDA H                  \ And then the high bytes
 SBC U

 ASL T                  \ Set (A T) = (A T) * 2
 ROL A

                        \ So we now have the following in (A T):
                        \
                        \     (yawRadians / 2 - 2/3 * (yawRadians / 2)^3) * 2
                        \
                        \   = yawRadians - 4/3 * (yawRadians / 2)^3
                        \
                        \   = yawRadians - 4/3 * yawRadians^3 / 2^3
                        \
                        \   = yawRadians - 8/6 * yawRadians^3 * 1/8
                        \
                        \   = yawRadians - 1/6 * yawRadians^3
                        \
                        \   = yawRadians - yawRadians^3 / 3!
                        \
                        \ The Taylor series expansion of sin(x) starts like
                        \ this:
                        \
                        \   sin(x) = x - (x^3 / 3!) + (x^5 / 5!) - ...
                        \
                        \ If we take the first two parts of the series and
                        \ apply them to yawRadians, we get:
                        \
                        \   sin(yawRadians) = yawRadians - (yawRadians^3 / 3!)
                        \
                        \ which is the same as our value in (A T)
                        \
                        \ So the value in (A T) is equal to the first two parts
                        \ of the Taylor series, and we have effectively just
                        \ calculated an approximation of this:
                        \
                        \   (A T) = sin(yawRadians)

 STA sinYawAngleHi,X    \ Set (sinYawAngleHi sinYawAngleLo) = (A T)
 LDA T                  \
 AND #%11111110         \ with the sign bit cleared in bit 0 of sinYawAngleLo to
 STA sinYawAngleLo,X    \ denote a positive result

 JMP rotm5              \ Jump to rotm5 to move on to the next axis