# 3D geometry: SetMatrices

```       Name: SetMatrices                                             [Show more]
Type: Subroutine
Category: 3D geometry
Summary: Set up matrices 1 to 4
Deep dive: Rotation matrices
Context: See this subroutine in context in the source code
References: This subroutine is called as follows:
* UpdateFlightModel (Part 4 of 4) calls SetMatrices

This routine populates the four matrix tables with 16-bit values.

Matrix 1 is the most complex matrix, and contains the rotation matrix for
rotating a point around the origin by the plane's roll, then pitch, then yaw.
It is constructed from the coordinates produced by the ProjectAxisAngle
routine as follows:

[  (mx2 * my2 * mz2)     -mx1 * mz2      (mx2 * my1 * mz2) ]
[    + (my1 * mz1)                         - (my2 * mz1)   ]
[                                                          ]
[ -(mx2 * my2 * mz1)      mx1 * mz1     -(mx2 * my1 * mz1) ]
[    + (my1 * mz2)                         - (my2 * mz2)   ]
[                                                          ]
[     mx1 * my2              mx2            mx1 * my1      ]

Matrix 2 is the transpose of matrix 1:

[ m0 m3 m6 ]
[ m1 m4 m7 ]
[ m2 m5 m8 ]

and therefore represents the inverse transformation of matrix 1.

Matrix 3 is as follows:

[ mz1  -mz2   0 ]
[ mz2   mz1   0 ]
[  0     0    1 ]

This is a rotation matrix that rotates through the plane's roll angle. The
zeroes and 1 are hardcoded in memory, with the latter being stored as &FFFE =

Matrix 4 contains an interim step from the calculation of matrix 1, and
contains the rotation matrix for the roll-and-pitch rotation (i.e. without any
yaw). It looks like this:

[ mz1     -mx1 * mz2      mx2 * mz2 ]
[ mz2      mx1 * mz1     -mx2 * mz1 ]
[  0          mx2            mx1    ]

The m6 entry is not set by any code, but it contains a zero in the source code
and stays that way.

Arguments:

matrixNumber         The routine is only ever called with matrixNumber = 0,
so it only ever writes to matrices 1 to 4 - presumably
the need for this argument was dropped at some point

matrixAxis           The routine is only ever called with matrixAxis = 0,
so it only ever reads from mx1 and mx2 - presumably
the need for this argument was dropped at some point

.SetMatrices

LDX matrixAxis         \ Set X to the matrix axis (this routine is only ever
\ called with a matrixAxis of 0, so this sets X = 0

\ We first set up a load of temporary variables that we
\ can use to populate the various matrices, storing them
\ in the matrix 1 variables (we will populate matrix 1
\ properly after we have populated matrices 2, 3 and 4)

LDA mx1Lo,X            \ Set (J I) = -(mx1Hi mx1Lo)
EOR #1                 \           = -mx1
STA I                  \
STA J

LDX #5                 \ Set m1 = (J I) * mz2 / 256
LDY #1                 \        = -mx1 * mz2 / 256
JSR SetMatrixEntry     \
\ Also set (S R) = mz2

LDX #1                 \ Set m3 = (S R) * my1 / 256
LDY #3                 \        = mz2 * my1 / 256
JSR SetMatrixEntry2

LDX #3                 \ Set m2 = (S R) * mx2 / 256
LDY #2                 \        = mz2 * mx2 / 256
JSR SetMatrixEntry2

LDX #4                 \ Set m0 = (S R) * my2 / 256
LDY #0                 \        = mz2 * my2 / 256
JSR SetMatrixEntry2    \
\ Also set (J I) = my2

LDX #0                 \ Set m6 = (J I) * mx1 / 256
LDY #6                 \        = my2 * mx1 / 256
JSR SetMatrixEntry     \
\ Also set (S R) = mx1

LDX #1                 \ Set m8 = (S R) * my1 / 256
LDY #8                 \        = mx1 * my1 / 256
JSR SetMatrixEntry2

LDX #2                 \ Set m4 = (S R) * mz1 / 256
LDY #4                 \        = mx1 * mz1 / 256
JSR SetMatrixEntry2    \
\ Also set (J I) = mz1

LDX #3                 \ Set m5 = (J I) * -mx2 / 256
LDY #5                 \        = mz1 * -mx2 / 256
JSR SetMatrixEntry3    \
\ Also set (H G) = m5

\ So we now have the following set of values (stripping
\ out the division by 256 and reordering the
\ multiplications for clarity):
\
\   m0 =  my2 * mz2
\   m1 = -mx1 * mz2
\   m2 =  mx2 * mz2
\   m3 =  my1 * mz2
\   m4 =  mx1 * mz1
\   m5 = -mx2 * mz1
\   m6 =  mx1 * my2
\   m8 =  mx1 * my1
\
\ Note that we didn't set m7 in the above, and also note
\ that in the following, we update some of these values,
\ so, for example, m0 and m2 change part way through the
\ calculation process (which I have pointed out)
\
\ We now work our way through the matrices as follows:
\
\   * Populate matrix 3 (which is hardcoded apart from
\     the 2x2 matrix in the top-left corner)
\
\   * Populate matrix 4 (except m6, which is hardcoded)
\
\   * Populate matrix 1 (by overwriting some of the
\     m0-m8 variables we just set, but keeping others)
\
\   * Set matrix 2 to the transpose of matrix 1

LDX matrixAxis         \ Set X = 0

LDY matrixNumber       \ Set Y = 0

LDA mz1Lo,X            \ Set m0 in matrix 3 = mz1
STA matrix3Lo,Y        \     m4 in matrix 3 = mz1
STA matrix3Lo+4,Y      \     m0 in matrix 4 = mz1
STA matrix4Lo,Y
LDA mz1Hi,X
STA matrix3Hi,Y
STA matrix3Hi+4,Y
STA matrix4Hi,Y

LDA mz2Lo,X            \ Set m3 in matrix 3 = mz2
STA matrix3Lo+3,Y      \     m3 in matrix 4 = mz2
STA matrix4Lo+3,Y      \     m1 in matrix 3 = -mz2
EOR #1
STA matrix3Lo+1,Y
LDA mz2Hi,X
STA matrix3Hi+1,Y
STA matrix3Hi+3,Y
STA matrix4Hi+3,Y

LDA mx1Lo,X            \ Set m8 in matrix 4 = mx1
STA matrix4Lo+8,Y
LDA mx1Hi,X
STA matrix4Hi+8,Y

LDA mx2Lo,X            \ Set m7 in matrix 4 = mx2
STA matrix4Lo+7,Y
LDA mx2Hi,X
STA matrix4Hi+7,Y

\ The following loop does the following:
\
\   * Set m1 in matrix 4 = m1 in matrix 1 = -mx1 * mz2
\   * Set m2 in matrix 4 = m2 in matrix 1 =  mx2 * mz2
\   * Set m4 in matrix 4 = m4 in matrix 1 =  mx1 * mz1
\   * Set m5 in matrix 4 = m5 in matrix 1 = -mx2 * mz1

LDY #5                 \ Set a loop counter in Y to count through 5, 4, 2, 1

.smat1

CPY #3                 \ If Y = 3 then jump to smat2 to skip this iteration
BEQ smat2

LDA matrix1Lo,Y        \ Set mY in matrix 4 = mY in matrix 1
STA matrix4Lo,Y
LDA matrix1Hi,Y
STA matrix4Hi,Y

.smat2

DEY                    \ Decrement the loop counter

BNE smat1              \ Loop back until we have copied all four entries from
\ matrix 1 to matrix 4

LDA G                  \ Set (J I) = (H G)
STA I                  \           = m5
LDA H                  \           = -mx2 * mz1
STA J

LDX matrixAxis         \ Set X = 0

LDA my2Lo,X            \ Set (S R) = my2
STA R
LDA my2Hi,X
STA S

JSR Multiply16x16Bit0  \ Set (H G) = (J I) * (S R) >> 16
\           = -(mx2 * mz1 * my2) >> 16

LDY matrixNumber       \ Set Y = 0

LDA matrix1Lo+3,Y      \ Set (J I) = m3
STA I                  \           = my1 * mz2
LDA matrix1Hi+3,Y
STA J

JSR Add16x16Bit0       \ Set (S R) = (H G) + (J I)
\           = -(mx2 * mz1 * my2) + (my1 * mz2)

LDA R                  \ Set m3 = (S R)
STA matrix1Lo+3,Y
LDA S
STA matrix1Hi+3,Y

LDA matrix1Lo+5,Y      \ Set (J I) = m5
STA I                  \           = -mx2 * mz1
LDA matrix1Hi+5,Y
STA J

LDX matrixAxis         \ Set X = 0

LDA my1Lo,X            \ Set (S R) = my1
STA R
LDA my1Hi,X
STA S

JSR Multiply16x16Bit0  \ Set (H G) = (J I) * (S R) >> 16
\           = -(mx2 * mz1 * my1) >> 16

LDY matrixNumber       \ Set Y = 0

LDA matrix1Lo,Y        \ Set (J I) = -m0
EOR #1                 \           = -my2 * mz2
STA I
LDA matrix1Hi,Y
STA J

JSR Add16x16Bit0       \ Set (S R) = (H G) + (J I)
\           = -(mx2 * mz1 * my1) - (my2 * mz2)

LDA R                  \ Set m5 = (S R)
STA matrix1Lo+5,Y
LDA S
STA matrix1Hi+5,Y

LDX matrixAxis         \ Set X = 0

LDA my1Lo,X            \ Set (S R) = my1
STA R
LDA my1Hi,X
STA S

LDX #2                 \ Set m0 = (S R) * mz1 / 256
LDY #0                 \        = my1 * mz1 / 256
JSR SetMatrixEntry2    \
\ Also set (J I) = mz1

LDX matrixAxis         \ Set X = 0

LDA my2Lo,X            \ Set (S R) = -my2
EOR #1
STA R
LDA my2Hi,X
STA S

JSR Multiply16x16Bit0  \ Set (H G) = (J I) * (S R) >> 16
\           = mz1 * -my2 >> 16

LDY matrixNumber       \ Set Y = 0

LDA matrix1Lo+2,Y      \ Set (J I) = m2
STA I                  \           = mx2 * mz2
STA xTemp1Lo           \
LDA matrix1Hi+2,Y      \ Also set xTemp1 = m2
STA J                  \                = mx2 * mz2
STA xTemp1Hi

LDA G                  \ Set m2 = (H G)
STA matrix1Lo+2,Y      \        = -my2 * mz1
LDA H
STA matrix1Hi+2,Y

LDA R                  \ Set (S R) = -(S R)
EOR #1                 \           = my2
STA R

JSR Multiply16x16Bit0  \ Set (H G) = (J I) * (S R) >> 16
\           = mx2 * mz2 * my2 >> 16

LDY matrixNumber       \ Set Y = 0

LDA matrix1Lo,Y        \ Set (J I) = m0
STA I                  \           = my1 * mz1
LDA matrix1Hi,Y        \
STA J                  \ (note that we updated m0 above, so this is different
\ to the first value we gave m0)

JSR Add16x16Bit0       \ Set (S R) = (H G) + (J I)
\           = (mx2 * mz2 * my2) + (my1 * mz1)

LDA R                  \ Set m0 = (S R)
STA matrix1Lo,Y
LDA S
STA matrix1Hi,Y

LDA xTemp1Lo           \ Set (S R) = xTemp1
STA R                  \           = mx2 * mz2
LDA xTemp1Hi
STA S

LDX matrixAxis         \ Set X = 0

LDA my1Lo,X            \ Set (J I) = my1
STA I
LDA my1Hi,X
STA J

JSR Multiply16x16Bit0  \ Set (H G) = (J I) * (S R) >> 16
\           = my1 * mx2 * mz2 >> 16

LDY matrixNumber       \ Set Y = 0

LDA matrix1Lo+2,Y      \ Set (J I) = m2
STA I                  \           = -my2 * mz1
LDA matrix1Hi+2,Y      \
STA J                  \ (note that we updated m2 above, so this is different
\ to the first value we gave m2)

JSR Add16x16Bit0       \ Set (S R) = (H G) + (J I)
\           = (my1 * mx2 * mz2) - (my2 * mz1)

LDA R                  \ Set m2 = (S R)
STA matrix1Lo+2,Y      \        = (my1 * mx2 * mz2) - (my2 * mz1)
LDA S
STA matrix1Hi+2,Y

LDX matrixAxis         \ Set X = 0

LDA mx2Lo,X            \ Set m7 = mx2
STA matrix1Lo+7,Y
LDA mx2Hi,X
STA matrix1Hi+7,X

\ We now set matrix 2 to the transpose of matrix 1, so
\ if matrix 1 looks like this:
\
\   [ m0 m1 m2 ]
\   [ m3 m4 m5 ]
\   [ m6 m7 m8 ]
\
\ then matrix 2 looks like this:
\
\   [ m0 m3 m6 ]
\   [ m1 m4 m7 ]
\   [ m2 m5 m8 ]
\
\ This sets matrix 2 to the inverse of matrix 1

LDA #2                 \ We transpose the matrix by looping through each column
STA T                  \ in matrix 1 and copy it into the corresponding column
\ in matrix 2, so set a loop counter in T for each row

LDY matrixNumber       \ Set Y = 0 to loop through the start of each row in
\ matrix 1, so it iterates through 0, 3 and 6

LDX matrixNumber       \ Set X = 0 to loop through the top of each column in
\ matrix 2, so it iterates through 0, 1 and 2

.smat3

\ The following blocks iterate through the nine copies
\ (three in each loop) as follows:

LDA matrix1Lo,Y        \ Set m0 in matrix 2 = m0 in matrix 1
STA matrix2Lo,X        \     m1             = m3
LDA matrix1Hi,Y        \     m2             = m6
STA matrix2Hi,X

LDA matrix1Lo+1,Y      \ Set m3 in matrix 2 = m1 in matrix 1
STA matrix2Lo+3,X      \     m4             = m4
LDA matrix1Hi+1,Y      \     m5             = m7
STA matrix2Hi+3,X

LDA matrix1Lo+2,Y      \ Set m6 in matrix 2 = m2 in matrix 1
STA matrix2Lo+6,X      \     m7             = m5
LDA matrix1Hi+2,Y      \     m8             = m8
STA matrix2Hi+6,X

INY                    \ Set Y = Y + 3
INY                    \
INY                    \ to point to the start of the next row in matrix 1

INX                    \ Set X = X + 1
\
\ to point to the top of the next column in matrix 2

DEC T                  \ Decrement the loop counter

BPL smat3              \ Loop back until we have copied all nine entries from
\ matrix 1 to matrix 2

RTS                    \ Return from the subroutine
```