.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 \ LDA mx1Hi,X \ by flipping the sign in bit 0 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 subroutineName: SetMatrices [Show more] Type: Subroutine Category: 3D geometry Summary: Set up matrices 1 to 4 Deep dive: Rotation matricesContext: 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 = %11111111 11111110, with the sign in bit 0. 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
[X]
Subroutine Add16x16Bit0 (category: Maths)
Add two 16-bit numbers that have their signs in bit 0
[X]
Subroutine Multiply16x16Bit0 (category: Maths)
Multiply two 16-bit numbers that have their signs in bit 0
[X]
Subroutine SetMatrixEntry (category: 3D geometry)
Calculate a matrix entry
[X]
Entry point SetMatrixEntry2 in subroutine SetMatrixEntry (category: 3D geometry)
Set mWrite = (S R) * mRead >> 16 and (J I) = mRead SetMatrixEntry3 Set mWrite = (J I) * -mRead >> 16 and (S R) = -mRead
[X]
Entry point SetMatrixEntry3 in subroutine SetMatrixEntry (category: 3D geometry)
Set mWrite = (J I) * -mRead >> 16 and (S R) = -mRead
[X]
Variable matrix1Hi (category: Maths)
The high bytes of matrix 1
[X]
Variable matrix1Lo (category: Maths)
The low bytes of matrix 1
[X]
Variable matrix2Hi (category: Maths)
The high bytes of matrix 2
[X]
Variable matrix2Lo (category: Maths)
The low bytes of matrix 2
[X]
Variable matrix3Hi (category: Maths)
The high bytes of matrix 3
[X]
Variable matrix3Lo (category: Maths)
The low bytes of matrix 3
[X]
Variable matrix4Hi (category: Maths)
The high bytes of matrix 4
[X]
Variable matrix4Lo (category: Maths)
The low bytes of matrix 4
[X]
Variable matrixAxis in workspace Main variable workspace
The axis to be processed by the matrix routines, specifically those that populate the matrices
[X]
Variable matrixNumber in workspace Main variable workspace
The matrix used in matrix operations
[X]
Variable mx1Hi in workspace Stack variables
The value of mx1, used when constructing the rotation matrices (high byte)
[X]
Variable mx1Lo in workspace Stack variables
The value of mx1, used when constructing the rotation matrices (low byte)
[X]
Variable mx2Hi in workspace Stack variables
The value of mx2, used when constructing the rotation matrices (high byte)
[X]
Variable mx2Lo in workspace Stack variables
The value of mx2, used when constructing the rotation matrices (low byte)
[X]
Variable my1Hi in workspace Stack variables
The value of my1, used when constructing the rotation matrices (high byte)
[X]
Variable my1Lo in workspace Stack variables
The value of my1, used when constructing the rotation matrices (low byte)
[X]
Variable my2Hi in workspace Stack variables
The value of my2, used when constructing the rotation matrices (high byte)
[X]
Variable my2Lo in workspace Stack variables
The value of my2, used when constructing the rotation matrices (low byte)
[X]
Variable mz1Hi in workspace Stack variables
The value of mz1, used when constructing the rotation matrices (high byte)
[X]
Variable mz1Lo in workspace Stack variables
The value of mz1, used when constructing the rotation matrices (low byte)
[X]
Variable mz2Hi in workspace Stack variables
The value of mz2, used when constructing the rotation matrices (high byte)
[X]
Variable mz2Lo in workspace Stack variables
The value of mz2, used when constructing the rotation matrices (low byte)
[X]
Label smat1 is local to this routine
[X]
Label smat2 is local to this routine
[X]
Label smat3 is local to this routine
[X]
Variable xTemp1Hi in workspace Main variable workspace
The high byte of the xTemp1 temporary variable
[X]
Variable xTemp1Lo in workspace Main variable workspace
The low byte of the xTemp1 temporary variable