;             3D ROTATION AND MISCELLANEOUS 3D MATHS ROUTINES



; This is the 3x3 transformation matrix needed:

; / cos(z)*cos(y)+sin(z)*sin(x)*sin(y)  -sin(z)*cos(x)  -cos(z)*sin(y)+sin(z)*sin(x)*cos(y) \
;|  sin(z)*cos(y)-cos(z)*sin(x)*sin(y)   cos(z)*cos(x)  -sin(z)*sin(y)-cos(z)*sin(x)*cos(y)  |
; \            cos(x)*sin(y)                sin(x)                 cos(x)*cos(y)            /

; So let's get busy!!!

CalcTransformationMatrix:       ;Rotation angles in A, B and C
        ld de,64
        ld h,SinCosTable/256
        ld l,a
        ld a,(hl)
        ld (SinX),a
        add hl,de
        ld a,(hl)
        ld (CosX),a
        ld h,SinCosTable/256
        ld l,b
        ld a,(hl)
        ld (SinY),a
        add hl,de
        ld a,(hl)
        ld (CosY),a
        ld h,SinCosTable/256
        ld l,c
        ld a,(hl)
        ld (SinZ),a
        add hl,de
        ld a,(hl)
        ld (CosZ),a

        ld hl,(SinY-1)
        ld a,(SinX)
        call MultAxH
        ld b,h          ;b=sx.sy
        ld a,(SinZ)
        call MultAxH
        ld c,h          ;c=sx.sy.sz
        ld hl,(CosZ-1)
        ld a,(CosY)
        call MultAxH
        ld a,h
        add a,c         
        ld (RotXX),a    ;xx=sx.sy.sz+cy.cz

        ld a,(CosZ)
        ld h,b
        call MultAxH
        ld c,h          ;c=sx.sy.cz
        ld a,(SinZ)
        ld hl,(CosY-1)
        call MultAxH
        ld a,h
        sub c
        ld (RotXY),a    ;xy=sz.cy-sx.sy.cz

        ld hl,(SinY-1)
        ld a,(CosX)
        call MultAxH
        ld a,h
        ld (RotXZ),a    ;xz=cx.sy

        ld a,(SinZ)
        ld hl,(CosX-1)
        call MultAxH
        xor a
        sub h
        ld (RotYX),a    ;yx=-sz.cx

        ld a,(CosZ)
        ld hl,(CosX-1)
        call MultAxH
        ld a,h
        ld (RotYY),a    ;yy=cz.cx

        ld a,(SinX)
        ld hl,(CosY-1)
        call MultAxH
        ld c,h          ;c=sx.cy
        ld a,(SinZ)
        call MultAxH
        ld b,h          ;b=sx.cy.sz
        ld a,(SinY)
        ld hl,(CosZ-1)
        call MultAxH
        ld a,b
        sub h
        ld (RotZX),a    ;zx=sx.cy.sz-sy.cz

        ld h,c
        ld a,(CosZ)
        call MultAxH
        ld b,h          ;b=sx.cy.cz
        ld a,(SinZ)
        ld hl,(SinY-1)
        call MultAxH
        xor a
        sub b
        sub h
        ld (RotZY),a    ;zy=-sz.sy-sx.cy.cz

        ld a,(CosX)
        ld hl,(CosY-1)
        call MultAxH
        ld a,h
        ld (RotZZ),a    ;zz=cx.cy
        ret

; MUAH! :)

RotatePoint: ;coords in a,b,c
        ld d,3
        ld e,a
        ld ix,RotXX
DoApplyMatrix:
        push de
        push bc
        push bc
        ld a,e
        ld d,(ix)
        ld e,0
        inc ix
        call MultAxDE
        pop af
        push hl
        ld d,(ix)
        ld e,0
        inc ix
        call MultAxDE
        push hl
        ld a,c
        ld d,(ix)
        ld e,0
        inc ix
        call MultAxDE
        pop de
        pop bc
        add hl,de
        add hl,bc
        pop bc
        pop de
        push hl
        dec d
        jr nz,DoApplyMatrix
        pop hl
        pop de
        pop bc
        ret

BackRotatePoint: ;coords in a,b,c
        ld d,3
        ld e,a
        ld ix,RotXX
DoUnApplyMatrix:
        push de
        push bc
        ld h,(ix)
        call MultExH
        ld e,b
        ld b,h
        ld h,(ix+3)
        call MultExH
        ld e,c
        ld c,h
        ld h,(ix+6)
        inc ix
        call MultExH
        ld a,h
        add a,b
        add a,c
        pop bc
        pop de
        push af
        dec d
        jr nz,DoUnApplyMatrix
        pop de
        pop bc
        ld c,d
        pop af
        ret

DotProduct:     ;(a,b,c)=vector 1   hl -> 16 bit coords of vector 2
        ld e,(hl)
        inc hl
        ld d,(hl)
        inc hl
        push hl
        push bc
        call MultAxDE
        pop af
        ex (sp),hl
        ld e,(hl)
        inc hl
        ld d,(hl)
        inc hl
        push hl
        call MultAxDE
        ex (sp),hl
        ld e,(hl)
        inc hl
        ld d,(hl)
        inc hl
        ld a,c
        call MultAxDE
        pop de
        pop bc
        add hl,de
        add hl,bc
        ret

RotXX =RotWS
RotXY =RotWS+1
RotXZ =RotWS+2
RotYX =RotWS+3
RotYY =RotWS+4
RotYZ =RotWS+5
RotZX =RotWS+6
RotZY =RotWS+7
RotZZ =RotWS+8
SinX =RotYZ
SinY =RotWS+9
SinZ =RotWS+10
CosX =RotWS+11
CosY =RotWS+12
CosZ =RotWS+13