Moving: MVS4

       Name: MVS4                                                    [Show more]
       Type: Subroutine
   Category: Moving
    Summary: Apply pitch and roll to an orientation vector
  Deep dive: Orientation vectors
             Pitching and rolling
    Context: See this subroutine in context in the source code
 References: This subroutine is called as follows:
             * MVEIT (Part 7 of 9) calls MVS4


 Apply pitch and roll angles alpha and beta to the orientation vector in Y.

 Specifically, this routine rotates a point (x, y, z) around the origin by
 pitch alpha and roll beta, using the small angle approximation to make the
 maths easier, and incorporating the Minsky circle algorithm to make the
 rotation more stable (though more elliptic).

 If that paragraph makes sense to you, then you should probably be writing
 this commentary! For the rest of us, there's a detailed explanation of all
 this in the deep dive on "Pitching and rolling".


 Arguments:

   Y                    Determines which of the INWK orientation vectors to
                        transform:

                          * Y = 9 rotates nosev: (nosev_x, nosev_y, nosev_z)

                          * Y = 15 rotates roofv: (roofv_x, roofv_y, roofv_z)

                          * Y = 21 rotates sidev: (sidev_x, sidev_y, sidev_z)


.MVS4

 LDA ALPHA              ; Set Q = alpha (the roll angle to rotate through)
 STA Q

 LDX INWK+2,Y           ; Set (S R) = nosev_y
 STX R
 LDX INWK+3,Y
 STX S

 LDX INWK,Y             ; These instructions have no effect as MAD overwrites
 STX P                  ; X and P when called, but they set X = P = nosev_x_lo

 LDA INWK+1,Y           ; Set A = -nosev_x_hi
 EOR #%10000000

 JSR MAD                ; Set (A X) = Q * A + (S R)
 STA INWK+3,Y           ;           = alpha * -nosev_x_hi + nosev_y
 STX INWK+2,Y           ;
                        ; and store (A X) in nosev_y, so this does:
                        ;
                        ; nosev_y = nosev_y - alpha * nosev_x_hi

 STX P                  ; This instruction has no effect as MAD overwrites P,
                        ; but it sets P = nosev_y_lo

 LDX INWK,Y             ; Set (S R) = nosev_x
 STX R
 LDX INWK+1,Y
 STX S

 LDA INWK+3,Y           ; Set A = nosev_y_hi

 JSR MAD                ; Set (A X) = Q * A + (S R)
 STA INWK+1,Y           ;           = alpha * nosev_y_hi + nosev_x
 STX INWK,Y             ;
                        ; and store (A X) in nosev_x, so this does:
                        ;
                        ; nosev_x = nosev_x + alpha * nosev_y_hi

 STX P                  ; This instruction has no effect as MAD overwrites P,
                        ; but it sets P = nosev_x_lo

 LDA BETA               ; Set Q = beta (the pitch angle to rotate through)
 STA Q

 LDX INWK+2,Y           ; Set (S R) = nosev_y
 STX R
 LDX INWK+3,Y
 STX S
 LDX INWK+4,Y

 STX P                  ; This instruction has no effect as MAD overwrites P,
                        ; but it sets P = nosev_y

 LDA INWK+5,Y           ; Set A = -nosev_z_hi
 EOR #%10000000

 JSR MAD                ; Set (A X) = Q * A + (S R)
 STA INWK+3,Y           ;           = beta * -nosev_z_hi + nosev_y
 STX INWK+2,Y           ;
                        ; and store (A X) in nosev_y, so this does:
                        ;
                        ; nosev_y = nosev_y - beta * nosev_z_hi

 STX P                  ; This instruction has no effect as MAD overwrites P,
                        ; but it sets P = nosev_y_lo

 LDX INWK+4,Y           ; Set (S R) = nosev_z
 STX R
 LDX INWK+5,Y
 STX S

 LDA INWK+3,Y           ; Set A = nosev_y_hi

 JSR MAD                ; Set (A X) = Q * A + (S R)
 STA INWK+5,Y           ;           = beta * nosev_y_hi + nosev_z
 STX INWK+4,Y           ;
                        ; and store (A X) in nosev_z, so this does:
                        ;
                        ; nosev_z = nosev_z + beta * nosev_y_hi

 RTS                    ; Return from the subroutine