.MVS5 LDA INWK+1,X \ Fetch roofv_x_hi, clear the sign bit, divide by 2 and AND #%01111111 \ store in T, so: LSR A \ STA T \ T = |roofv_x_hi| / 2 \ = |roofv_x| / 512 \ \ The above is true because: \ \ |roofv_x| = |roofv_x_hi| * 256 + roofv_x_lo \ \ so: \ \ |roofv_x| / 512 = |roofv_x_hi| * 256 / 512 \ + roofv_x_lo / 512 \ = |roofv_x_hi| / 2 LDA INWK,X \ Now we do the following subtraction: SEC \ SBC T \ (S R) = (roofv_x_hi roofv_x_lo) - |roofv_x| / 512 STA R \ = (1 - 1/512) * roofv_x \ \ by doing the low bytes first LDA INWK+1,X \ And then the high bytes (the high byte of the right SBC #0 \ side of the subtraction being 0) STA S LDA INWK,Y \ Set P = nosev_x_lo STA P LDA INWK+1,Y \ Fetch the sign of nosev_x_hi (bit 7) and store in T AND #%10000000 STA T LDA INWK+1,Y \ Fetch nosev_x_hi into A and clear the sign bit, so AND #%01111111 \ A = |nosev_x_hi| LSR A \ Set (A P) = (A P) / 16 ROR P \ = |nosev_x_hi nosev_x_lo| / 16 LSR A \ = |nosev_x| / 16 ROR P LSR A ROR P LSR A ROR P ORA T \ Set the sign of A to the sign in T (i.e. the sign of \ the original nosev_x), so now: \ \ (A P) = nosev_x / 16 EOR RAT2 \ Give it the sign as if we multiplied by the direction \ by the pitch or roll direction STX Q \ Store the value of X so it can be restored after the \ call to ADD JSR ADD \ (A X) = (A P) + (S R) \ = +/-nosev_x / 16 + (1 - 1/512) * roofv_x STA K+1 \ Set K(1 0) = (1 - 1/512) * roofv_x +/- nosev_x / 16 STX K LDX Q \ Restore the value of X from before the call to ADD LDA INWK+1,Y \ Fetch nosev_x_hi, clear the sign bit, divide by 2 and AND #%01111111 \ store in T, so: LSR A \ STA T \ T = |nosev_x_hi| / 2 \ = |nosev_x| / 512 LDA INWK,Y \ Now we do the following subtraction: SEC \ SBC T \ (S R) = (nosev_x_hi nosev_x_lo) - |nosev_x| / 512 STA R \ = (1 - 1/512) * nosev_x \ \ by doing the low bytes first LDA INWK+1,Y \ And then the high bytes (the high byte of the right SBC #0 \ side of the subtraction being 0) STA S LDA INWK,X \ Set P = roofv_x_lo STA P LDA INWK+1,X \ Fetch the sign of roofv_x_hi (bit 7) and store in T AND #%10000000 STA T LDA INWK+1,X \ Fetch roofv_x_hi into A and clear the sign bit, so AND #%01111111 \ A = |roofv_x_hi| LSR A \ Set (A P) = (A P) / 16 ROR P \ = |roofv_x_hi roofv_x_lo| / 16 LSR A \ = |roofv_x| / 16 ROR P LSR A ROR P LSR A ROR P ORA T \ Set the sign of A to the opposite sign to T (i.e. the EOR #%10000000 \ sign of the original -roofv_x), so now: \ \ (A P) = -roofv_x / 16 EOR RAT2 \ Give it the sign as if we multiplied by the direction \ by the pitch or roll direction STX Q \ Store the value of X so it can be restored after the \ call to ADD JSR ADD \ (A X) = (A P) + (S R) \ = -/+roofv_x / 16 + (1 - 1/512) * nosev_x STA INWK+1,Y \ Set nosev_x = (1-1/512) * nosev_x -/+ roofv_x / 16 STX INWK,Y LDX Q \ Restore the value of X from before the call to ADD LDA K \ Set roofv_x = K(1 0) STA INWK,X \ = (1-1/512) * roofv_x +/- nosev_x / 16 LDA K+1 STA INWK+1,X RTS \ Return from the subroutineName: MVS5 [Show more] Type: Subroutine Category: Moving Summary: Apply a 3.6 degree pitch or roll to an orientation vector Deep dive: Orientation vectors Pitching and rolling by a fixed angleContext: See this subroutine in context in the source code References: This subroutine is called as follows: * MVEIT (Part 8 of 9) calls MVS5
Pitch or roll a ship by a small, fixed amount (1/16 radians, or 3.6 degrees), in a specified direction, by rotating the orientation vectors. The vectors to rotate are given in X and Y, and the direction of the rotation is given in RAT2. The calculation is as follows: * If the direction is positive: X = X * (1 - 1/512) + Y / 16 Y = Y * (1 - 1/512) - X / 16 * If the direction is negative: X = X * (1 - 1/512) - Y / 16 Y = Y * (1 - 1/512) + X / 16 So if X = 15 (roofv_x), Y = 21 (sidev_x) and RAT2 is positive, it does this: roofv_x = roofv_x * (1 - 1/512) + sidev_x / 16 sidev_x = sidev_x * (1 - 1/512) - roofv_x / 16
Arguments: X The first vector to rotate: * If X = 15, rotate roofv_x * If X = 17, rotate roofv_y * If X = 19, rotate roofv_z * If X = 21, rotate sidev_x * If X = 23, rotate sidev_y * If X = 25, rotate sidev_z Y The second vector to rotate: * If Y = 9, rotate nosev_x * If Y = 11, rotate nosev_y * If Y = 13, rotate nosev_z * If Y = 21, rotate sidev_x * If Y = 23, rotate sidev_y * If Y = 25, rotate sidev_z RAT2 The direction of the pitch or roll to perform, positive or negative (i.e. the sign of the roll or pitch counter in bit 7)
[X]
Subroutine ADD (category: Maths (Arithmetic))
Calculate (A X) = (A P) + (S R)