Maths (Geometry): RotateVector

       Name: RotateVector                                            [Show more]
       Type: Subroutine
   Category: Maths (Geometry)
    Summary: Rotate a vector by a rotation matrix
    Context: See this subroutine in context in the source code
 References: This subroutine is called as follows:
             * RotateCoordToCar calls RotateVector


 If bit 7 of X is clear, this routine calculates:

   [  variableA  ]   [ cosYawAngle   0   -sinYawAngle ]   [  variableY  ]
   [      -      ] = [      0        1         0      ] . [      -      ]
   [ variableA+1 ]   [ sinYawAngle   0    cosYawAngle ]   [ variableY+1 ]

 by doing these individual calculations:

   variableA = variableY * cosYawAngle + variableY+1 * sinYawAngle

   variableA+1 = variableY+1 * cosYawAngle - variableY * sinYawAngle

 If bit 7 of X is set, this routine calculates:

   [  variableA  ]   [  cosYawAngle   0   sinYawAngle ]   [  variableY  ]
   [      -      ] = [       0        1        0      ] . [      -      ]
   [ variableA+1 ]   [ -sinYawAngle   0   cosYawAngle ]   [ variableY+1 ]

 by doing these individual calculations:

   variableA = variableY * cosYawAngle - variableY+1 * sinYawAngle

   variableA+1 = variableY+1 * cosYawAngle + variableY * sinYawAngle

 For it to work, the routine must be called with bit 6 of X set.


 Arguments:

   Y                    Offset of the 16-bit signed number to multiply:

                          * 0 = xPlayerSpeed and zPlayerSpeed

                          * 6 = xPlayerAccel and zPlayerAccel

   A                    Offset of the variable to store the result in:

                          * 3 = xAcceleration and zAcceleration

                          * 8 = xVelocity and zVelocity

   X                    Details of the operation to perform on the second and
                        fourth multiplications:

                          * Bit 6 needs to be set

                          * Bit 7 defines the sign to apply to the result:

                            * 0 = do not negate the result

                            * 1 = negate the result


.RotateVector

 STY N                  \ Set N to the offset of the 16-bit signed number, and
                        \ let's call this number variableY (as it is specified
                        \ by parameter Y)

 STA K                  \ Set K to the offset of the variable to store the
                        \ result in, and let's call this number variableA (as it
                        \ is specified by parameter A)

 STX GG                 \ Store the details of the operation to perform in GG

 LDX #1                 \ Set X = 1, so in the call to MultiplyCoords we use
                        \ cosYawAngle as the 16-bit sign-magnitude value to
                        \ multiply

 LDA #%00000000         \ Set A = %00000000, so in the call to MultiplyCoords we
                        \ overwrite the result rather than adding, and do not
                        \ negate the multiplication

 JSR MultiplyCoords     \ Set variableA = variableY * variableX
                        \               = variableY * cosYawAngle

 DEX                    \ Set X = 0, so in the call to MultiplyCoords we use
                        \ sinYawAngle as the 16-bit sign-magnitude value to
                        \ multiply

 INC N                  \ Point to the next variable after the 16-bit signed
                        \ number we just used, so in the call to MultiplyCoords
                        \ we use variableY+1 as the 16-bit signed number

 LDA GG                 \ Set A to the details of the operation to perform in
                        \ GG, as specified by parameter X
                        \
                        \ Bit 6 of parameter X is always set in calls to this
                        \ routine, so we add the result to variableA

 JSR MultiplyCoords     \ Set:
                        \
                        \   variableA = variableA + variableY+1 * variableX
                        \             = variableA + variableY+1 * sinYawAngle
                        \
                        \ if bit 7 of parameter X is clear, or:
                        \
                        \   variableA = variableA - variableY+1 * sinYawAngle
                        \
                        \ if bit 7 of parameter X is set

 INX                    \ Set X = 1, so in the call to MultiplyCoords we use
                        \ cosYawAngle as the 16-bit sign-magnitude value to
                        \ multiply

 INC K                  \ Point to the next variable after the one we just
                        \ stored the result in, so in the call to MultiplyCoords
                        \ we use variableA+1 to store the result

 LDA #%00000000         \ Set A = %00000000, so in the call to MultiplyCoords we
                        \ overwrite the result rather than adding, and do not
                        \ negate the multiplication

 JSR MultiplyCoords     \ Set variableA+1 = variableY+1 * variableX
                        \                 = variableY+1 * cosYawAngle

 DEX                    \ Set X = 0, so in the call to MultiplyCoords we use
                        \ sinYawAngle as the 16-bit sign-magnitude value to
                        \ multiply

 DEC N                  \ Point back to the original variable for the 16-bit
                        \ signed number we just used, so in the call to
                        \ MultiplyCoords we use variableY again as the 16-bit
                        \ signed number

 LDA GG                 \ Set A to the details of the operation to perform in
 EOR #%10000000         \ GG with bit 7 flipped, which is as specified by
                        \ parameter X, but with a flipped sign
                        \
                        \ Bit 6 of parameter X is always set in calls to this
                        \ routine, so we add the result to variableA+1

 JSR MultiplyCoords     \ Set:
                        \
                        \   variableA+1 = variableA+1 - variableY * variableX
                        \               = variableA+1 - variableY * sinYawAngle
                        \
                        \ if bit 7 of parameter X is clear, or:
                        \
                        \   variableA+1 = variableA+1 + variableY * sinYawAngle
                        \
                        \ if bit 7 of parameter X is set

 RTS                    \ Return from the subroutine