.include "lynx.inc" .include "../starfly/3d.h" .include "../starfly/starfly.inc" .import SinLow .import SinHigh .import CosLow .import CosHigh .import OneMinusCosLow .import OneMinusCosHigh .global _BSeed .global _BMove .global _BRotateX .global _BRotateZ .segment "DATA" vresult: .word $0000, $0000, $0000 .segment "CODE" .macro CLEAR_ACCUMULATOR stz MATHM stz MATHK .endmacro .macro WAITSUZY .local notready notready: bit SPRSYS bmi notready .endmacro .macro ABSOLUTE_TO_BA lda (_MatrixAbsolute), y sta MATHB iny lda (_MatrixAbsolute), y sta MATHA iny .endmacro .macro VAL_TO_BA val lda val sta MATHB lda val+1 sta MATHA .endmacro .macro MULTIPLY_TO place lda (_MatrixAbsolute), y sta MATHB iny lda (_MatrixAbsolute), y sta MATHA iny WAITSUZY lda MATHE sta place+1 lda MATHF sta place lda MATHG asl rol place rol place+1 asl rol place rol place+1 .endmacro .macro ABSOLUTE_TO_DC val ldy #(val) lda (_MatrixAbsolute), y sta MATHD iny lda (_MatrixAbsolute), y sta MATHC .endmacro .macro VAL_TO_DC_ABSOLUTE_TO_BA val lda val sta MATHD lda val+1 sta MATHC lda (_MatrixAbsolute), y sta MATHB iny lda (_MatrixAbsolute), y sta MATHA iny .endmacro .macro ADD_AND_STORE from, place clc lda from ;wait for end of multiply WAITSUZY ;add adc MATHF sta place lda from+1 adc MATHE sta place+1 .endmacro .macro SIN_TO_DC tax lda SinLow, x sta MATHD lda SinHigh, x sta MATHC .endmacro .macro COS_TO_PLACE place lda CosLow, x sta place lda CosHigh, x sta place+1 .endmacro .macro ONE_MINUS_COS_TO_DC lda OneMinusCosLow, x sta MATHD lda OneMinusCosHigh, x sta MATHC .endmacro ; VBasisOffset ; ; A,X is the new start of array ; Y is index to use ; _MatrixAbsolute contains base VBasisOffset: sta _MatrixAbsolute stx _MatrixAbsolute+1 lda #0 cpy #0 beq @L2 @L1: clc adc #VBasisSize dey bne @L1 @L2: clc adc _MatrixAbsolute sta _MatrixAbsolute bcc @L3 inc _MatrixAbsolute+1 @L3: ldx _MatrixAbsolute+1 rts ; BSeed - initialises (_MatrixAbsolute) to the identity _BSeed: sta _MatrixAbsolute stx _MatrixAbsolute+1 lda #0 ldy #0 ldx #VBasisSize @L1: sta (_MatrixAbsolute), y iny dex bne @L1 lda #64 ;64*256 = 16384 ldy #MATRIX_RIGHT_X+1 sta (_MatrixAbsolute), y ldy #MATRIX_UP_Y+1 sta (_MatrixAbsolute), y ldy #MATRIX_FRONT_Z+1 sta (_MatrixAbsolute), y rts ; BRotateX - rotates (_MatrixAbsolute) around its right axis by the number of ; units in the accumulator _BRotateX: ldx #MATRIX_RIGHT_X stx VBRotationVector ldx #MATRIX_UP_X stx VBRotationAxis1 ldx #MATRIX_FRONT_X stx VBRotationAxis2 jmp BRotate ; BRotateZ - rotates (_MatrixAbsolute) around its front axis by the number of ; units in the accumulator _BRotateZ: ldx #MATRIX_FRONT_X stx VBRotationVector ldx #MATRIX_UP_X stx VBRotationAxis1 ldx #MATRIX_RIGHT_X stx VBRotationAxis2 ; BRotate - intended for use by BRotateX and BRotateZ only. Rotates by the ; number of units in the accumulator around VBRotationVector, affecting ; VBRotationAxis1 and VBRotationAxis2 BRotate: ;s[8:8] = sintable[accumulator] SIN_TO_DC ldy VBRotationVector ;VBSinX[2:14] = s*VBRotationVector.x MULTIPLY_TO VBSinX ;VBSinY[2:14] = s*VBRotationVector.y MULTIPLY_TO VBSinY ;VBSinZ[2:14] = s*VBRotationVector.z MULTIPLY_TO VBSinZ ;c[2:14] = costable[accumulator] COS_TO_PLACE _MatrixWorkArea+MATRIX_FRONT_Z ;cc[2:14] = 1-c ONE_MINUS_COS_TO_DC ldy VBRotationVector ;VBSubCosX[2:14] = cc*VBRotationVector.x MULTIPLY_TO VBSubCosX ;VBSubCosY[2:14] = cc*VBRotationVector.y MULTIPLY_TO VBSubCosY ;VBSubCosZ[2:14] = cc*VBRotationVector.z MULTIPLY_TO VBSubCosZ ;_MatrixWorkArea.right.x[2:14] = VBSubCosX*VBRotationVector.x + c ldy VBRotationVector VAL_TO_DC_ABSOLUTE_TO_BA VBSubCosX ADD_AND_STORE _MatrixWorkArea+MATRIX_FRONT_Z, _MatrixWorkArea+MATRIX_RIGHT_X ;_MatrixWorkArea.up.y[2:14] = VBSubCosY*VBRotationVector.y + c VAL_TO_DC_ABSOLUTE_TO_BA VBSubCosY ADD_AND_STORE _MatrixWorkArea+MATRIX_FRONT_Z, _MatrixWorkArea+MATRIX_UP_Y ;_MatrixWorkArea.front.z[2:14] = VBSubCosZ*VBRotationVector.z + c; VAL_TO_DC_ABSOLUTE_TO_BA VBSubCosZ ADD_AND_STORE _MatrixWorkArea+MATRIX_FRONT_Z, _MatrixWorkArea+MATRIX_FRONT_Z ;VBXY[2:14] = VBSubCosX*VBRotationVector.y ldy VBRotationVector iny iny VAL_TO_DC_ABSOLUTE_TO_BA VBSubCosX ;wait for end of multiply WAITSUZY lda MATHG sta VBXY lda MATHF sta VBXY+1 ;VBXZ[2:14] = VBSubCosX*VBRotationVector.z ABSOLUTE_TO_BA dey dey ;wait for end of multiply WAITSUZY lda MATHG sta VBXZ lda MATHF sta VBXZ+1 ;VBYZ[2:14] = VBSubCosY*VBRotationVector.z VAL_TO_DC_ABSOLUTE_TO_BA VBSubCosY ;wait for end of multiply WAITSUZY lda MATHG sta VBYZ lda MATHF sta VBYZ+1 ;_MatrixWorkArea.front.x[2:14] = VBXZ + VBSinY clc lda VBXZ adc VBSinY sta _MatrixWorkArea+MATRIX_FRONT_X lda VBXZ+1 adc VBSinY+1 sta _MatrixWorkArea+MATRIX_FRONT_X+1 ;_MatrixWorkArea.right.z[2:14] = VBXZ - VBSinY sec lda VBXZ sbc VBSinY sta _MatrixWorkArea+MATRIX_RIGHT_Z lda VBXZ+1 sbc VBSinY+1 sta _MatrixWorkArea+MATRIX_RIGHT_Z+1 ;_MatrixWorkArea.right.y[2:14] = VBXY + VBSinZ clc lda VBXY adc VBSinZ sta _MatrixWorkArea+MATRIX_RIGHT_Y lda VBXY+1 adc VBSinZ+1 sta _MatrixWorkArea+MATRIX_RIGHT_Y+1 ;_MatrixWorkArea.up.x[2:14] = VBXY - VBSinZ sec lda VBXY sbc VBSinZ sta _MatrixWorkArea+MATRIX_UP_X lda VBXY+1 sbc VBSinZ+1 sta _MatrixWorkArea+MATRIX_UP_X+1 ;_MatrixWorkArea.front.y[2:14] = VBYZ - VBSinX sec lda VBYZ sbc VBSinX sta _MatrixWorkArea+MATRIX_FRONT_Y lda VBYZ+1 sbc VBSinX+1 sta _MatrixWorkArea+MATRIX_FRONT_Y+1 ;_MatrixWorkArea.up.z[2:14] = VBYZ + VBSinX clc lda VBYZ adc VBSinX sta _MatrixWorkArea+MATRIX_UP_Z lda VBYZ+1 adc VBSinX+1 sta _MatrixWorkArea+MATRIX_UP_Z+1 ;apply _MatrixWorkArea to the two rotational axis ldy VBRotationAxis1 jsr BMatApply ldy VBRotationAxis2 jsr BMatApply ;check various things are still orthogonal jsr BOrthoCheck lda VBRotationAxis2 ldx VBRotationAxis1 stx VBRotationAxis2 sta VBRotationAxis1 jsr BOrthoCheck ldx VBRotationAxis1 ldy VBRotationAxis2 jsr BOrthoCheck ;make sure various vectors are still unit length jsr BLengthCheck lda VBRotationAxis2 sta VBRotationAxis1 jsr BLengthCheck rts ; BMatApply - applies the matrix _MatrixWorkArea to (_MatrixAbsolute),y BMatApply: CLEAR_ACCUMULATOR ; ;dest.x[2:14] = matrix.right.x * src.x + matrix.up.x * src.y + matrix.front.x * src.z ; ;matrix.right.x * src.x sty Temp1 VAL_TO_DC_ABSOLUTE_TO_BA _MatrixWorkArea+MATRIX_RIGHT_X ; + matrix.up.x * src.y lda (_MatrixAbsolute), y tax iny lda (_MatrixAbsolute), y iny WAITSUZY stx MATHD sta MATHC lda _MatrixWorkArea+MATRIX_UP_X sta MATHB lda _MatrixWorkArea+MATRIX_UP_X+1 sta MATHA ; + matrix.front.x * src.z lda (_MatrixAbsolute), y tax iny lda (_MatrixAbsolute), y WAITSUZY stx MATHD sta MATHC lda _MatrixWorkArea+MATRIX_FRONT_X sta MATHB lda _MatrixWorkArea+MATRIX_FRONT_X+1 sta MATHA ; wait for total result WAITSUZY ; shift + store lda MATHL sta vresult+1 lda MATHK sta vresult lda MATHJ asl vresult+1 rol vresult rol asl vresult+1 rol vresult rol sta vresult+1 ; ;dest.y[2:14] = matrix.right.y * src.x + matrix.up.y * src.y + matrix.front.y * src.z ; CLEAR_ACCUMULATOR ;matrix.right.y * src.x ldy Temp1 VAL_TO_DC_ABSOLUTE_TO_BA _MatrixWorkArea+MATRIX_RIGHT_Y ; + matrix.up.y * src.y lda (_MatrixAbsolute), y tax iny lda (_MatrixAbsolute), y iny WAITSUZY stx MATHD sta MATHC lda _MatrixWorkArea+MATRIX_UP_Y sta MATHB lda _MatrixWorkArea+MATRIX_UP_Y+1 sta MATHA ; + matrix.front.y * src.z lda (_MatrixAbsolute), y tax iny lda (_MatrixAbsolute), y WAITSUZY stx MATHD sta MATHC lda _MatrixWorkArea+MATRIX_FRONT_Y sta MATHB lda _MatrixWorkArea+MATRIX_FRONT_Y+1 sta MATHA ; wait for total result WAITSUZY ; shift + store lda MATHL sta vresult+3 lda MATHK sta vresult+2 lda MATHJ asl vresult+3 rol vresult+2 rol asl vresult+3 rol vresult+2 rol sta vresult+3 ; ;dest.z[2:14] = matrix.right.y * src.x + matrix.up.y * src.y + matrix.front.y * src.z + matrix.pos.y ; CLEAR_ACCUMULATOR ;matrix.right.z * src.x ldy Temp1 VAL_TO_DC_ABSOLUTE_TO_BA _MatrixWorkArea+MATRIX_RIGHT_Z ; + matrix.up.z * src.y lda (_MatrixAbsolute), y tax iny lda (_MatrixAbsolute), y iny WAITSUZY stx MATHD sta MATHC lda _MatrixWorkArea+MATRIX_UP_Z sta MATHB lda _MatrixWorkArea+MATRIX_UP_Z+1 sta MATHA ; + matrix.front.z * src.z lda (_MatrixAbsolute), y tax iny lda (_MatrixAbsolute), y WAITSUZY stx MATHD sta MATHC lda _MatrixWorkArea+MATRIX_FRONT_Z sta MATHB lda _MatrixWorkArea+MATRIX_FRONT_Z+1 sta MATHA ; wait for total result WAITSUZY ; shift + store lda MATHL sta vresult+5 lda MATHK sta vresult+4 lda MATHJ asl vresult+5 rol vresult+4 rol asl vresult+5 rol vresult+4 rol sta vresult+5 ;replace original vector ldy Temp1 lda vresult sta (_MatrixAbsolute), y iny lda vresult+1 sta (_MatrixAbsolute), y iny lda vresult+2 sta (_MatrixAbsolute), y iny lda vresult+3 sta (_MatrixAbsolute), y iny lda vresult+4 sta (_MatrixAbsolute), y iny lda vresult+5 sta (_MatrixAbsolute), y rts ; BOrthoCheck - removes any non-orthogonal part of (_MatrixAbsolute),y when ; compared to (_MatrixAbsolute),x. i.e. x is not adjusted BOrthoCheck: CLEAR_ACCUMULATOR ;(y).x*VBRotationVector.x ldy VBRotationAxis1 lda (_MatrixAbsolute), y iny sta MATHD lda (_MatrixAbsolute), y iny sta MATHC sty Temp1 ldy VBRotationVector ABSOLUTE_TO_BA sty Temp2 ;get VBRotationAxis1.y into registers ldy Temp1 lda (_MatrixAbsolute), y iny tax lda (_MatrixAbsolute), y iny sty Temp1 ldy Temp2 ;wait for end of multiply WAITSUZY ;store VBRotationAxis1.y stx MATHD sta MATHC ;get VBRotationVector.y to start multiply ABSOLUTE_TO_BA sty Temp2 ;get VBRotationAxis1.z into registers ldy Temp1 lda (_MatrixAbsolute), y iny tax lda (_MatrixAbsolute), y ldy Temp2 ;wait for end of multiply WAITSUZY ;store VBRotationAxis1.y stx MATHD sta MATHC ;get VBRotationVector.y to start multiply ABSOLUTE_TO_BA ;wait for end of multiply WAITSUZY ; shift result - 4:28 to 8:8 avoid using registers, stupid lda MATHJ sta Temp4 bmi OrthoShiftNeg lda MATHK lsr Temp4 ror lsr Temp4 ror lsr Temp4 ror lsr Temp4 ror sta MATHD lda Temp4 sta MATHC bra OrthoMult OrthoShiftNeg: lda MATHK lsr Temp4 ror lsr Temp4 ror lsr Temp4 ror lsr Temp4 ror sta MATHD lda Temp4 ora #$f0 sta MATHC ; store for multiplication OrthoMult: ; fix x ldy VBRotationVector ABSOLUTE_TO_BA sty Temp1 ;wait for end of multiply WAITSUZY ; do subtraction & store sec ldy VBRotationAxis1 lda (_MatrixAbsolute), y sbc MATHG sta (_MatrixAbsolute), y iny lda (_MatrixAbsolute), y sbc MATHE sta (_MatrixAbsolute), y iny sty Temp2 ; fix y ldy Temp1 ABSOLUTE_TO_BA sty Temp1 ;wait for end of multiply WAITSUZY ; do subtraction & store sec ldy Temp2 lda (_MatrixAbsolute), y sbc MATHG sta (_MatrixAbsolute), y iny lda (_MatrixAbsolute), y sbc MATHE sta (_MatrixAbsolute), y iny sty Temp2 ; fix z ldy Temp1 ABSOLUTE_TO_BA ;wait for end of multiply WAITSUZY ; do subtraction & store sec ldy Temp2 lda (_MatrixAbsolute), y sbc MATHG sta (_MatrixAbsolute), y iny lda (_MatrixAbsolute), y sbc MATHE sta (_MatrixAbsolute), y iny sty Temp2 rts ; BLengthCheck - tweaks VBRotationAxis1 from not quite unit length to being ; genuinely so BLengthCheck: CLEAR_ACCUMULATOR ;VBRotationAxis1.x*VBRotationAxis1.x ldy VBRotationAxis1 lda (_MatrixAbsolute), y iny sta MATHD sta MATHB lda (_MatrixAbsolute), y iny sta MATHC sta MATHA ;get VBRotationAxis1.y into registers lda (_MatrixAbsolute), y iny tax lda (_MatrixAbsolute), y iny ;wait for end of multiply WAITSUZY ;start VBRotationAxis1.y*VBRotationAxis1.y stx MATHD stx MATHB sta MATHC sta MATHA ;get VBRotationAxis1.z into registers lda (_MatrixAbsolute), y iny tax lda (_MatrixAbsolute), y ;wait for end of multiply WAITSUZY ;start VBRotationAxis1.z*VBRotationAxis1.z stx MATHD stx MATHB sta MATHC sta MATHA ;wait for end of multiply WAITSUZY ;2 bit shift - cannot leave in register since any write to _K zeros _J lda MATHJ sta Temp1 lda MATHK asl MATHL rol rol Temp1 asl MATHL rol rol Temp1 ;get square root of result jsr BGetSqRoot ;get one over square root - notice that 1 = 256 and is preshifted ;left 7 positions here before the divide in order to get the correct ;result sta MATHP stz MATHN stz MATHH lda #128 sta MATHG stz MATHF stz MATHE ;wait for end of divide WAITSUZY ;result is already in MATHC:MATHD ;fix for Handy, maybe for real Lynx lda MATHD ldx MATHC sta MATHD stx MATHC ;multiply VBRotationAxis1.x by result of division ldy VBRotationAxis1 lda (_MatrixAbsolute), y iny sta MATHB lda (_MatrixAbsolute), y dey sta MATHA ;wait for end of multiply WAITSUZY ;store result lda MATHG sta (_MatrixAbsolute), y iny lda MATHF sta (_MatrixAbsolute), y iny ;multiply VBRotationAxis1.y by result of division lda (_MatrixAbsolute), y iny sta MATHB lda (_MatrixAbsolute), y dey sta MATHA ;wait for end of multiply WAITSUZY ;store result lda MATHG sta (_MatrixAbsolute), y iny lda MATHF sta (_MatrixAbsolute), y iny ;multiply VBRotationAxis1.z by result of division lda (_MatrixAbsolute), y iny sta MATHB lda (_MatrixAbsolute), y dey sta MATHA ;wait for end of multiply WAITSUZY ;store result lda MATHG sta (_MatrixAbsolute), y iny lda MATHF sta (_MatrixAbsolute), y rts BGetSqRoot: bit Temp1 bvs GetSqRootGt asl rol Temp1 asl rol Temp1 lda Temp1 tax lda SQTable, x lsr rts GetSqRootGt: ldx Temp1 lda SQTable, x rts ; BMove - moves (_MatrixAbsolute) along its front axis by the number of units ; in MovementFactor divided by 4 .macro ADDFINISH .local local1 .local local2 lda MATHE bmi local1 adc (_MatrixAbsolute), y sta (_MatrixAbsolute), y iny lda (_MatrixAbsolute), y adc #0 sta (_MatrixAbsolute), y bra local2 local1: adc (_MatrixAbsolute), y sta (_MatrixAbsolute), y iny lda (_MatrixAbsolute), y adc #$ff sta (_MatrixAbsolute), y local2: .endmacro _BMove: sta MovementFactor stx MovementFactor+1 ;multiply x component by MovementFactor ABSOLUTE_TO_DC VBASIS_FRONT_X lda MovementFactor sta MATHB lda MovementFactor+1 sta MATHA ;do a little preparation for addition about to come ldy #VBASIS_POS_X clc lda (_MatrixAbsolute), y ;wait for end of multiply WAITSUZY ;complete addition adc MATHF sta (_MatrixAbsolute), y iny ADDFINISH ;multiply y component by MovementFactor ABSOLUTE_TO_DC VBASIS_FRONT_Y lda MovementFactor sta MATHB lda MovementFactor+1 sta MATHA ;do a little preparation for addition about to come ldy #VBASIS_POS_Y clc lda (_MatrixAbsolute), y ;wait for end of multiply WAITSUZY ;complete addition adc MATHF sta (_MatrixAbsolute), y iny ADDFINISH ;multiply z component by MovementFactor ABSOLUTE_TO_DC VBASIS_FRONT_Z lda MovementFactor sta MATHB lda MovementFactor+1 sta MATHA ;do a little preparation for addition about to come ldy #VBASIS_POS_Z clc lda (_MatrixAbsolute), y ;wait for end of multiply WAITSUZY ;complete addition adc MATHF sta (_MatrixAbsolute), y iny ADDFINISH rts .segment "RODATA" ; GetSqRoot - returns the square root of Temp1:a in the accumulator SQTable: .byte 0, 16, 22, 27, 32, 35, 39, 42, 45, 48, 50, 53, 55, 57 .byte 59, 61, 64, 65, 67, 69, 71, 73, 75, 76, 78, 80, 81, 83 .byte 84, 86, 87, 89, 90, 91, 93, 94, 96, 97, 98, 99, 101, 102 .byte 103, 104, 106, 107, 108, 109, 110, 112, 113, 114, 115, 116, 117, 118 .byte 119, 120, 121, 122, 123, 124, 125, 126, 128, 128, 129, 130, 131, 132 .byte 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 144, 145 .byte 146, 147, 148, 149, 150, 150, 151, 152, 153, 154, 155, 155, 156, 157 .byte 158, 159, 160, 160, 161, 162, 163, 163, 164, 165, 166, 167, 167, 168 .byte 169, 170, 170, 171, 172, 173, 173, 174, 175, 176, 176, 177, 178, 178 .byte 179, 180, 181, 181, 182, 183, 183, 184, 185, 185, 186, 187, 187, 188 .byte 189, 189, 190, 191, 192, 192, 193, 193, 194, 195, 195, 196, 197, 197 .byte 198, 199, 199, 200, 201, 201, 202, 203, 203, 204, 204, 205, 206, 206 .byte 207, 208, 208, 209, 209, 210, 211, 211, 212, 212, 213, 214, 214, 215 .byte 215, 216, 217, 217, 218, 218, 219, 219, 220, 221, 221, 222, 222, 223 .byte 224, 224, 225, 225, 226, 226, 227, 227, 228, 229, 229, 230, 230, 231 .byte 231, 232, 232, 233, 234, 234, 235, 235, 236, 236, 237, 237, 238, 238 .byte 239, 240, 240, 241, 241, 242, 242, 243, 243, 244, 244, 245, 245, 246 .byte 246, 247, 247, 248, 248, 249, 249, 250, 250, 251, 251, 252, 252, 253 .byte 253, 254, 254, 255