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

       Name: GetRotationMatrix (Part 4 of 5)                         [Show more]
       Type: Subroutine
   Category: Maths (Geometry)
    Summary: Loop back to calculate cos instead of sin
  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


.rotm5

 CPX secondAxis         \ If we just processed the second axis, then we have
 BEQ rotm6              \ now set both sinYawAngle and cosYawAngle, so jump to
                        \ rotm6 to set their signs

 LDX secondAxis         \ Otherwise set X = secondAxis so the next time we reach
                        \ the end of the loop, we take the BEQ branch we just
                        \ passed through

 LDA #0                 \ Set (H G) = (201 0) - (H G)
 SEC                    \
 SBC G                  \ starting with the low bytes
 STA G

 LDA #201               \ And then the high bytes
 SBC H
 STA H

 STA U                  \ Set (U G) = (H G)
                        \
                        \ (U G) and (H G) were set to yawRadians / 2 for the
                        \ first pass through the loop above, so we now have the
                        \ following:
                        \
                        \   201 - yawRadians / 2
                        \
                        \ PI is represented by 804, as 804 / 256 = 3.14, so 201
                        \ represents PI/4, so this the same as:
                        \
                        \   PI/4 - yawRadians / 2
                        \
                        \ Given that we expect (U G) to contain half the angle
                        \ we are projecting, this means we are going to find the
                        \ sine of this angle when we jump back to rotm1:
                        \
                        \   PI/2 - yawRadians
                        \
                        \ It's a trigonometric identity that:
                        \
                        \   sin(PI/2 - x) = cos(x)
                        \
                        \ so jumping back will, in fact, find the cosine of the
                        \ angle

 JMP rotm1              \ Loop back to set the other variable of sinYawAngle and
                        \ cosYawAngle to the cosine of the angle