BBC Micro Elite

# Maths (Geometry): TIDY

```       Name: TIDY                                              [View in context]
Type: Subroutine
Category: Maths (Geometry)
Summary: Orthonormalise the orientation vectors for a ship

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

.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

.^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
}
```