Maths (Geometry): TIDY

       Name: TIDY                                                    [Show more]
       Type: Subroutine
   Category: Maths (Geometry)
    Summary: Orthonormalise the orientation vectors for a ship
  Deep dive: Tidying orthonormal vectors
             Orientation vectors
    Context: See this subroutine in context in the source code
 References: This subroutine is called as follows:
             * MVEIT (Part 1 of 9) calls TIDY


 This routine orthonormalises the orientation vectors for a ship. This means
 making the three orientation vectors orthogonal (perpendicular to each other),
 and normal (so each of the vectors has length 1).

 We do this because we use the small angle approximation to rotate these
 vectors in space. It is not completely accurate, so the three vectors tend
 to get stretched over time, so periodically we tidy the vectors with this
 routine to ensure they remain as orthonormal as possible.


.TI2

                        ; Called from below with A = 0, X = 0, Y = 4 when
                        ; nosev_x and nosev_y are small, so we assume that
                        ; nosev_z is big

 TYA                    ; A = Y = 4
 LDY #2
 JSR TIS3               ; Call TIS3 with X = 0, Y = 2, A = 4, to set roofv_z =
 STA INWK+20            ; -(nosev_x * roofv_x + nosev_y * roofv_y) / nosev_z

 JMP TI3                ; Jump to TI3 to keep tidying

.TI1

                        ; Called from below with A = 0, Y = 4 when nosev_x is
                        ; small

 TAX                    ; Set X = A = 0

 LDA XX15+1             ; Set A = nosev_y, and if the top two magnitude bits
 AND #%01100000         ; are both clear, jump to TI2 with A = 0, X = 0, Y = 4
 BEQ TI2

 LDA #2                 ; Otherwise nosev_y is big, so set up the index values
                        ; to pass to TIS3

 JSR TIS3               ; Call TIS3 with X = 0, Y = 4, A = 2, to set roofv_y =
 STA INWK+18            ; -(nosev_x * roofv_x + nosev_z * roofv_z) / nosev_y

 JMP TI3                ; Jump to TI3 to keep tidying

.TIDY

 LDA INWK+10            ; Set (XX15, XX15+1, XX15+2) = nosev
 STA XX15
 LDA INWK+12
 STA XX15+1
 LDA INWK+14
 STA XX15+2

 JSR NORM               ; Call NORM to normalise the vector in XX15, i.e. nosev

 LDA XX15               ; Set nosev = (XX15, XX15+1, XX15+2)
 STA INWK+10
 LDA XX15+1
 STA INWK+12
 LDA XX15+2
 STA INWK+14

 LDY #4                 ; Set Y = 4

 LDA XX15               ; Set A = nosev_x, and if the top two magnitude bits
 AND #%01100000         ; are both clear, jump to TI1 with A = 0, Y = 4
 BEQ TI1

 LDX #2                 ; Otherwise nosev_x is big, so set up the index values
 LDA #0                 ; to pass to TIS3

 JSR TIS3               ; Call TIS3 with X = 2, Y = 4, A = 0, to set roofv_x =
 STA INWK+16            ; -(nosev_y * roofv_y + nosev_z * roofv_z) / nosev_x

.TI3

 LDA INWK+16            ; Set (XX15, XX15+1, XX15+2) = roofv
 STA XX15
 LDA INWK+18
 STA XX15+1
 LDA INWK+20
 STA XX15+2

 JSR NORM               ; Call NORM to normalise the vector in XX15, i.e. roofv

 LDA XX15               ; Set roofv = (XX15, XX15+1, XX15+2)
 STA INWK+16
 LDA XX15+1
 STA INWK+18
 LDA XX15+2
 STA INWK+20

 LDA INWK+12            ; Set Q = nosev_y
 STA Q

 LDA INWK+20            ; Set A = roofv_z

 JSR MULT12             ; Set (S R) = Q * A = nosev_y * roofv_z

 LDX INWK+14            ; Set X = nosev_z

 LDA INWK+18            ; Set A = roofv_y

 JSR TIS1               ; Set (A ?) = (-X * A + (S R)) / 96
                        ;        = (-nosev_z * roofv_y + nosev_y * roofv_z) / 96
                        ;
                        ; This also sets Q = nosev_z

 EOR #%10000000         ; Set sidev_x = -A
 STA INWK+22            ;        = (nosev_z * roofv_y - nosev_y * roofv_z) / 96

 LDA INWK+16            ; Set A = roofv_x

 JSR MULT12             ; Set (S R) = Q * A = nosev_z * roofv_x

 LDX INWK+10            ; Set X = nosev_x

 LDA INWK+20            ; Set A = roofv_z

 JSR TIS1               ; Set (A ?) = (-X * A + (S R)) / 96
                        ;        = (-nosev_x * roofv_z + nosev_z * roofv_x) / 96
                        ;
                        ; This also sets Q = nosev_x

 EOR #%10000000         ; Set sidev_y = -A
 STA INWK+24            ;        = (nosev_x * roofv_z - nosev_z * roofv_x) / 96

 LDA INWK+18            ; Set A = roofv_y

 JSR MULT12             ; Set (S R) = Q * A = nosev_x * roofv_y

 LDX INWK+12            ; Set X = nosev_y

 LDA INWK+16            ; Set A = roofv_x

 JSR TIS1               ; Set (A ?) = (-X * A + (S R)) / 96
                        ;        = (-nosev_y * roofv_x + nosev_x * roofv_y) / 96

 EOR #%10000000         ; Set sidev_z = -A
 STA INWK+26            ;        = (nosev_y * roofv_x - nosev_x * roofv_y) / 96

 LDA #0                 ; Set A = 0 so we can clear the low bytes of the
                        ; orientation vectors

 LDX #14                ; We want to clear the low bytes, so start from sidev_y
                        ; at byte #9+14 (we clear all except sidev_z_lo, though
                        ; I suspect this is in error and that X should be 16)

.TIL1

 STA INWK+9,X           ; Set the low byte in byte #9+X to zero

 DEX                    ; Set X = X - 2 to jump down to the next low byte
 DEX

 BPL TIL1               ; Loop back until we have zeroed all the low bytes

 RTS                    ; Return from the subroutine