Revs on the BBC Micro

# Maths (Geometry): GetRotationMatrix (Part 3 of 5)

```       Name: GetRotationMatrix (Part 3 of 5)                         [Show more]
Type: Subroutine
Category: Maths (Geometry)
Summary: Calculate sin(H G) for bigger 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

.rotm3

\ If we get here then (H G) = yawRadians / 2 and
\ H >= 122

\ PI is represented by 804, as 804 / 256 = 3.14, so 201
\ represents PI/4, which we use in the following
\ subtraction

LDA #0                 \ Set (U T) = (201 0) - (H G)
SEC                    \           = PI/4 - yawRadians / 2
SBC G                  \
STA T                  \ starting with the low bytes

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

STA V                  \ Set (V T) = (U T)
\           = PI/4 - yawRadians / 2

JSR Multiply8x16       \ Set (U T) = U * (V T) / 256
\           = U * (PI/4 - yawRadians / 2)
\
\ U is the high byte of (U T), which also contains
\ PI/4 - yawRadians / 2, so this approximation holds
\ true:
\
\   (U T) = U * (PI/4 - yawRadians / 2)
\         =~ (PI/4 - yawRadians / 2) ^ 2

ASL T                  \ Set (U T) = (U T) * 2
ROL U                  \           = (PI/4 - yawRadians / 2) ^ 2 * 2

\ By this point we have the following:
\
\   (U T) = (PI/4 - yawRadians / 2) ^ 2 * 2
\         = ((PI/2 - yawRadians) / 2) ^ 2 * 2
\
\ If we define x = PI/2 - yawRadians, then we have:
\
\   (U T) = (x / 2) ^ 2 * 2
\         = ((x ^ 2) / (2 ^ 2)) * 2
\         = (x ^ 2) / 2
\
\ The small angle approximation states that for small
\ values of x, the following approximation holds true:
\
\   cos(x) =~ 1 - (x ^ 2) / 2!
\
\ As yawRadians is large, this means x is small, so we
\ can use this approximation
\
\ We are storing the cosine, which is in the range 0 to
\ 1, in the 16-bit variable (U T), so in terms of 16-bit
\ arithmetic, the 1 in the above equation is (1 0 0)
\
\ So this is the same as:
\
\   cos(x) =~ (1 0 0) - (x ^ 2) / 2!
\          =  (1 0 0) - (U T)
\
\ It's a trigonometric identity that:
\
\   cos(PI/2 - x) = sin(x)
\
\ so we have:
\
\   cos(x) = cos(PI/2 - yawRadians)
\
\ and we already calculated that:
\
\   cos(x) =~ (1 0 0) - (U T)
\
\ so that means that:
\
\                   =~ (1 0 0) - (U T)
\
\ So we just need to calculate (1 0 0) - (U T) to get
\ our result

LDA #0                 \ Set A = (1 0 0) - (U T)
SEC                    \
SBC T                  \ starting with the low bytes

AND #%11111110         \ Which we store in sinYawAngleLo, with bit 0 cleared to
STA sinYawAngleLo,X    \ denote a positive result (as it's a sign-magnitude
\ number we want to store)

LDA #0                 \ And then the high bytes
SBC U

BCC rotm4              \ We now need to subtract the top bytes, i.e. the 1 in
\ (1 0 0) and the 0 in (0 U T), while including the
\ carry from the high byte subtraction
\
\ So the top byte should be:
\
\   A = 1 - 0 - (1 - C)
\     = 1 - (1 - C)
\     = C
\
\ If the C flag is clear, then that means the top byte
\ is zero, so we already have a valid result from the
\ high and low bytes, so we jump to rotm4 to store the
\ high byte of the result in sinYawAngleHi
\
\ If the C flag is set, then the result is (1 A T), but
\ the highest possible value for sin or cos is 1, so
\ that's what we return
\
\ Because sinYawAngle is a sign-magnitude number with
\ the sign bit in bit 0, we return the following value
\ to represent the closest value to 1 that we can fit
\ into 16 bits:
\
\   (11111111 11111110)

LDA #%11111110         \ Set sinYawAngleLo to the highest possible positive
STA sinYawAngleLo,X    \ value (i.e. all ones except for the sign in bit 0)

LDA #%11111111         \ Set A to the highest possible value of sinYawAngleHi,
\ so we can store it in the next instruction

.rotm4

STA sinYawAngleHi,X    \ Store A in the high byte in sinYawAngleHi
```