.ProjectAxisAngle \ The routine processes the axis defined in Y, but the \ comments below are for the x-axis, where we set the \ values of mx1 and mx2 depending on the value of \ xRotation LDA xRotationHi,Y \ Set (G A) = (xRotationHi xRotationLo) for axis Y STA G \ \ starting with the high byte STA K \ Set K = xRotationHi, so we can access the high byte \ when writing the results below LDA xRotationLo,Y \ Set the low byte of (G A) \ \ so (G A) = xRotation ASL A \ Set (G W) = (G A) << 2 ROL G \ = xRotation << 2 ASL A \ ROL G \ so (G W) is the angle, reduced into a quarter circle STA W \ by removing the top four bits \ \ The original angle is stored as the portion of a whole \ circle, where xRotation is in the range 0 to 65535 and \ 65535 represents a full circle, so shifting it to the \ left and dropping the top two bits out reduces the \ range of the angle to a quarter circle, while leaving \ the range of the top byte as 0 to 255, which is what \ we need for looking up the reduced angle in the sine \ table LDX G \ Set (X W) = (G W) to pass the rotation angle to \ Sine16Bit JSR Sine16Bit \ Set (A Y) = sin(X W) \ = sin(xRotation) STA Q \ Set (Q P) = (A Y) STY P LDA G \ Set (X W) = ~(G W) EOR #&FF \ = ~xRotation << 2 TAX \ LDA W \ This sets (X W) to the original xRotation, reduced to EOR #&FF \ the quarter circle as before, and then inverted within STA W \ that quarter - or, to put it another way, if (G W) is \ the angle within the quarter (0 to 90 degrees), then \ (X W) is now 90 - (G W) \ \ Because sin(90 - X) = -cos(X), this means the call to \ Sine16Bit returns the cosine instead of the sine JSR Sine16Bit \ Set (A Y) = sin(X W) \ = sin(~xRotation) \ = -cos(xRotation) STA S \ Set (S R) = (A Y) STY R \ We now copy the results into mx1 and mx2, setting the \ sign in bit 0 as we go (the sign is in bit 0 as this \ is a matrix number) \ \ We do this based on the value in K, which contains the \ high byte of (xRotationHi xRotationLo) for this axis \ \ * 0 to 63 (bit 6 clear, bit 7 clear) \ * 64 to 127 (bit 6 set, bit 7 clear) \ * 128 to 191 (bit 6 clear, bit 7 set) \ * 192 to 255 (bit 6 set, bit 7 set) \ \ These four ranges correspond to the four quadrants of \ the rotation angle. For example, for the z-axis, which \ points into the screen, we can visualise the quadrants \ by imagining the plane doing a full 360-degree roll to \ the right; for the first quarter the plane is still \ upright, for the second and third quarters it is \ upside down, and then for the final quarter it is \ upright again \ \ The following sets the m1 and m2 values according to \ which quadrant this particular axis is rotated into LDY matrixAxis \ Set Y to the axis once again BIT K \ If bit 6 of K is set, jump to axis2 BVS axis2 BMI axis1 \ If bit 7 of K is set, jump to axis1 \ If we get here then K has: \ \ * Bit 6 clear \ * Bit 7 clear \ \ so this is the first quadrant of the axis' rotation LDA Q \ Set (mx2Hi mx2Lo) = (Q P) STA mx2Hi,Y \ = sin(xRotation) LDA P \ AND #%11111110 \ with bit 0 clear (positive) STA mx2Lo,Y LDA S \ Set (mx1Hi mx1Lo) = (S R) STA mx1Hi,Y \ = -cos(xRotation) LDA R \ AND #%11111110 \ with bit 0 clear (positive) STA mx1Lo,Y JMP axis4 \ Jump to axis4 to return from the subroutine .axis1 \ If we get here then K has: \ \ * Bit 6 clear \ * Bit 7 set \ \ so this is the third quadrant of the axis' rotation LDA Q \ Set (mx2Hi mx2Lo) = -(Q P) STA mx2Hi,Y \ LDA P \ with bit 0 set (negative) ORA #1 STA mx2Lo,Y LDA S \ Set (mx1Hi mx1Lo) = -(S R) STA mx1Hi,Y \ LDA R \ ORA #1 \ with bit 0 set (negative) STA mx1Lo,Y BNE axis4 \ Jump to axis4 to return from the subroutine (this BNE \ is effectively a JMP as A is never zero) .axis2 \ If we get here then K has bit 6 set BMI axis3 \ If bit 7 of K is set, jump to axis3 \ If we get here then K has: \ \ * Bit 6 set \ * Bit 7 clear \ \ so this is the second quadrant of the axis' rotation LDA S \ Set (mx2Hi mx2Lo) = (S R) STA mx2Hi,Y \ LDA R \ with bit 0 clear (positive) AND #%11111110 STA mx2Lo,Y LDA Q \ Set (mx1Hi mx1Lo) = -(Q P) STA mx1Hi,Y \ LDA P \ with bit 0 set (negative) ORA #1 STA mx1Lo,Y BNE axis4 \ Jump to axis4 to return from the subroutine (this BNE \ is effectively a JMP as A is never zero) .axis3 \ If we get here then K has: \ \ * Bit 6 set \ * Bit 7 set \ \ so this is the fourth quadrant of the axis' rotation LDA S \ Set (mx2Hi mx2Lo) = -(S R) STA mx2Hi,Y \ LDA R \ with bit 0 set (negative) ORA #1 STA mx2Lo,Y LDA Q \ Set (mx1Hi mx1Lo) = (Q P) STA mx1Hi,Y \ LDA P \ with bit 0 clear (positive) AND #%11111110 STA mx1Lo,Y .axis4 RTS \ Return from the subroutineName: ProjectAxisAngle [Show more] Type: Subroutine Category: 3D geometry Summary: Convert the rotation angles of the plane into coordinates 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 ProjectAxisAngle
This routine converts the plane's rotation angles into a set of coordinate values that we can use in the SetMatrices routine to populate the four rotation matrices. There are six of these values, each of which is a 16-bit number with the sign in bit 0: * (mx1Hi mx1Lo), which we can refer to as mx1 * (mx2Hi mx2Lo), which we can refer to as mx2 * (my1Hi my1Lo), which we can refer to as my1 * (my2Hi my2Lo), which we can refer to as my2 * (mz1Hi mz1Lo), which we can refer to as mz1 * (mz2Hi mz2Lo), which we can refer to as mz2 The calculation is done for one axis at a time, so the same routine not only sets mx1 and mx2, but also my1 and my2, and mz1 and mz2. If we take the x-axis as an example, and assume for simplicity that the angle in xRotation is less than 90 degrees, the routine will calculate the following: * mx1 = -cos(xRotation) * mx2 = sin(xRotation) Considering the x-axis calculation, the routine sets mx1 and mx2 according to the current rotation of the plane around the x-axis, which is stored in (xRotationHi xRotationLo), which we can refer to as xRotation. The routine converts the angle in xRotation into a pair of Cartesian coordinates, by projecting a line rotated by that amount onto the Cartesian axes; in a sense, we are converting from Polar coordinates into Cartesian, so we can use those coordinates to populate the rotation matrices in SetMatrices.
Arguments: matrixAxis The axis to be processed: * 0 = set mx1 and mx2 from xRotation * 1 = set my1 and my2 from yRotation * 2 = set mz1 and mz2 from zRotation Y Same value as matrixAxis
[X]
Subroutine Sine16Bit (category: Maths)
Calculate the sine of a 16-bit number
[X]
Label axis1 is local to this routine
[X]
Label axis2 is local to this routine
[X]
Label axis3 is local to this routine
[X]
Label axis4 is local to this routine
[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 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 xRotationHi in workspace Main variable workspace
Plane rotation angle around the x-axis (high byte)
[X]
Variable xRotationLo in workspace Main variable workspace
Plane rotation angle around the x-axis (low byte)