Skip to navigation

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) \ = sin(yawRadians) \ \ and we already calculated that: \ \ cos(x) =~ (1 0 0) - (U T) \ \ so that means that: \ \ sin(yawRadians) = cos(x) \ =~ (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