Skip to navigation

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 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 }