x3d equ 0
y3d equ 2
z3d equ 4
vec_x equ 0
vec_y equ 4
vec_z equ 8
; 3d point - triple integer word coordinate
; vector   - triple float dword coordinate
;----------------------in: --------------------------------
;------------------------ esi - pointer to 1st 3d point ---
;------------------------ edi - pointer to 2nd 3d point ---
;------------------------ ebx - pointer to result vector --
;---------------------- out : none ------------------------
reverse_mx_3x3:
; esi - source matrix
; edi - desired reversed matrix

  push  ebp
  mov   ebp,esp
  sub   esp,4
  .det  equ  ebp-4

  fninit
  fld  dword[esi]
  fmul dword[esi+16]
  fmul dword[esi+32]
  fld  dword[esi+12]
  fmul dword[esi+28]
  fmul dword[esi+8]
  faddp
  fld  dword[esi+24]
  fmul dword[esi+4]
  fmul dword[esi+20]
  faddp
  fld  dword[esi]
  fmul dword[esi+28]
  fmul dword[esi+20]
  fchs
  faddp
  fld  dword[esi+24]
  fmul dword[esi+16]
  fmul dword[esi+8]
  fchs
  faddp
  fld  dword[esi+12]
  fmul dword[esi+4]
  fmul dword[esi+32]
  fchs
  faddp
  fstp dword[.det]
  cmp  dword[.det],0
  jne  @f
  int3
 @@:
 ; fld1
 ; fdiv dword[.det]
 ; fstp dword[.det]

  fld  dword[esi+16]
  fmul dword[esi+32]
  fld  dword[esi+20]
  fmul dword[esi+28]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi]

  fld  dword[esi+8]
  fmul dword[esi+28]
  fld  dword[esi+4]
  fmul dword[esi+32]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+4]

  fld  dword[esi+4]
  fmul dword[esi+20]
  fld  dword[esi+8]
  fmul dword[esi+16]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+8]

  fld  dword[esi+20]
  fmul dword[esi+24]
  fld  dword[esi+12]
  fmul dword[esi+32]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+12]

  fld  dword[esi]
  fmul dword[esi+32]
  fld  dword[esi+8]
  fmul dword[esi+24]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+16]

  fld  dword[esi+8]
  fmul dword[esi+12]
  fld  dword[esi]
  fmul dword[esi+20]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+20]

  fld  dword[esi+12]
  fmul dword[esi+28]
  fld  dword[esi+16]
  fmul dword[esi+24]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+24]

  fld  dword[esi+4]
  fmul dword[esi+24]
  fld  dword[esi]
  fmul dword[esi+28]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+28]

  fld  dword[esi]
  fmul dword[esi+16]
  fld  dword[esi+4]
  fmul dword[esi+12]
  fchs
  faddp
  fdiv dword[.det]
  fstp dword[edi+32]


  mov  esp,ebp
  pop  ebp
ret

make_vector_r:
   if Ext < SSE2
        fninit
        fld     dword[edi]                ;edi+x3d
        fsub    dword[esi]                ;esi+x3d
        fstp    dword[ebx+vec_x]

        fld     dword[edi+4]
        fsub    dword[esi+4]
        fstp    dword[ebx+vec_y]

        fld     dword[edi+8]
        fsub    dword[esi+8]
        fstp    dword[ebx+vec_z]
    else
        movups  xmm0,[esi]
        movups  xmm1,[edi]
        subps   xmm1,xmm0
        movlps  [ebx],xmm1
        movhlps  xmm1,xmm1
        movss   [ebx+8],xmm1
     end if

ret
;---------------------- in: -------------------------------
;--------------------------- esi - pointer to 1st vector --
;--------------------------- edi - pointer to 2nd vector --
;--------------------------- ebx - pointer to result vector
;---------------------- out : none
cross_product:
        fninit
        fld     dword [esi+vec_y]
        fmul    dword [edi+vec_z]
        fld     dword [esi+vec_z]
        fmul    dword [edi+vec_y]
        fsubp   ;st1 ,st
        fstp    dword [ebx+vec_x]

        fld     dword [esi+vec_z]
        fmul    dword [edi+vec_x]
        fld     dword [esi+vec_x]
        fmul    dword [edi+vec_z]
        fsubp   ;st1 ,st
        fstp    dword [ebx+vec_y]

        fld     dword [esi+vec_x]
        fmul    dword [edi+vec_y]
        fld     dword [esi+vec_y]
        fmul    dword [edi+vec_x]
        fsubp   ;st1 ,st
        fstp    dword [ebx+vec_z]
ret
;----------------------- in: ------------------------------
;---------------------------- edi - pointer to vector -----
;----------------------- out : none
normalize_vector:
if Ext >= SSE3
        movups  xmm0,[edi]
        andps   xmm0,[zero_hgst_dd]
        movups  xmm1,xmm0
        mulps   xmm0,xmm0
        haddps  xmm0,xmm0
        haddps  xmm0,xmm0
        rsqrtps xmm0,xmm0
        mulps   xmm0,xmm1
        movlps  [edi],xmm0
        movhlps xmm0,xmm0
        movss   [edi+8],xmm0
else

        fninit
        fld     dword [edi+vec_x]
        fmul    st, st
        fld     dword [edi+vec_y]
        fmul    st, st
        fld     dword [edi+vec_z]
        fmul    st, st
        faddp   st1, st
        faddp   st1, st
        fsqrt

        ftst
        fstsw ax
        sahf
        jnz     @f

        fst     dword [edi+vec_x]
        fst     dword [edi+vec_y]
        fstp    dword [edi+vec_z]
        ret
      @@:
        fld st
        fld st
        fdivr dword [edi+vec_x]
        fstp  dword [edi+vec_x]
        fdivr dword [edi+vec_y]
        fstp  dword [edi+vec_y]
        fdivr dword [edi+vec_z]
        fstp  dword [edi+vec_z]
end if
ret
;------------------in: -------------------------
;------------------ esi - pointer to 1st vector
;------------------ edi - pointer to 2nd vector
;------------------out: ------------------------
;------------------ st0 - dot-product
dot_product:
        fninit
;if Ext >=SSE3
;        movups  xmm0,[esi]
;        movups  xmm1,[edi]
;        andps   xmm0,[zero_hgst_dd]
;        mulps   xmm0,xmm1
;        haddps  xmm0,xmm0
;        haddps  xmm0,xmm0
;        movss   [esp-4],xmm0
;        fld     dword[esp-4]
;else
        fld     dword [esi+vec_x]
        fmul    dword [edi+vec_x]
        fld     dword [esi+vec_y]
        fmul    dword [edi+vec_y]
        fld     dword [esi+vec_z]
        fmul    dword [edi+vec_z]
        faddp
        faddp
;end if
ret

; DOS version Coded by Mikolaj Felix aka Majuma
; mfelix@polbox.com
; www.majuma.xt.pl
; into FASM translation by Macgub
init_sincos_tab:
.counter   equ  dword [ebp-4]  ; cur angle

     push       ebp
     mov        ebp,esp

     xor        eax,eax
     push       eax            ; init .counter
     mov        edi,cos_tab
     mov        esi,sin_tab
     mov        ecx,256
     fninit

     fld        .counter
  @@:
     fld        st
     fsincos
     fstp       dword [edi]
     fstp       dword [esi]
;     fadd       [piD180]
     fadd       [piD128]
     add        esi,4
     add        edi,4
     loop       @b
     ffree      st

     mov        esp,ebp
     pop        ebp
ret
;------
; esi - offset (pointer) to angles, edi offset to 3x3 matrix
make_rotation_matrix:
   .sinx   equ dword[ebp-4]
   .cosx   equ dword[ebp-8]
   .siny   equ dword[ebp-12]
   .cosy   equ dword[ebp-16]
   .sinz   equ dword[ebp-20]
   .cosz   equ dword[ebp-24]
     push      ebp
     mov       ebp,esp
     sub       esp,24

     movzx     ebx,word[esi]
     shl       ebx,2
     mov       eax,dword[sin_tab+ebx]
     mov       .sinx,eax
     mov       edx,dword[cos_tab+ebx]
     mov       .cosx,edx

     movzx     ebx,word[esi+2]
     shl       ebx,2
     mov       eax,dword[sin_tab+ebx]
     mov       .siny,eax
     mov       edx,dword[cos_tab+ebx]
     mov       .cosy,edx

     movzx     ebx,word[esi+4]
     shl       ebx,2
     mov       eax,dword[sin_tab+ebx]
     mov       .sinz,eax
     mov       edx,dword[cos_tab+ebx]
     mov       .cosz,edx

     fninit
     fld       .cosy
     fmul      .cosz
     fstp      dword[edi]

     fld       .sinx
     fmul      .siny
     fmul      .cosz
     fld       .cosx
     fmul      .sinz
     fchs
     faddp
     fstp      dword[edi+12]

     fld       .cosx
     fmul      .siny
     fmul      .cosz
     fld       .sinx
     fmul      .sinz
     faddp
     fstp      dword[edi+24]

     fld       .cosy
     fmul      .sinz
     fstp      dword[edi+4]

     fld       .sinx
     fmul      .siny
     fmul      .sinz
     fld       .cosx
     fmul      .cosz
     faddp
     fstp      dword[edi+16]

     fld       .cosx
     fmul      .siny
     fmul      .sinz
     fld       .sinx
     fchs
     fmul      .cosz
     faddp
     fstp      dword[edi+28]

     fld       .siny
     fchs
     fstp      dword[edi+8]

     fld       .cosy
     fmul      .sinx
     fstp      dword[edi+20]

     fld       .cosx
     fmul      .cosy
     fstp      dword[edi+32]

     mov       esp,ebp
     pop       ebp
ret
;---------------------
;  in:  esi - ptr to points(normals], each point(normal) coeficient as dword
;       edi - ptr to rotated points(normals)
;       ebx - ptr to 3x3 (9 dwords, 36 bytes) rotation matrix
;       ecx - number of points(normals)
rotary:
if Ext<SSE
    fninit
 .again:

    fld     dword[esi]
    fmul    dword[ebx]
    fld     dword[esi+4]
    fmul    dword[ebx+12]
    faddp
    fld     dword[esi+8]
    fmul    dword[ebx+24]
    faddp
    fstp    dword[edi]


    fld     dword[esi+4]
    fmul    dword[ebx+16]
    fld     dword[esi]
    fmul    dword[ebx+4]
    faddp
    fld     dword[esi+8]
    fmul    dword[ebx+28]
    faddp
    fstp    dword[edi+4]


    fld     dword[esi+8]
    fmul    dword[ebx+32]
    fld     dword[esi]
    fmul    dword[ebx+8]
    fld     dword[esi+4]
    fmul    dword[ebx+20]
    faddp
    faddp
    fstp    dword[edi+8]


    add     esi,12
    add     edi,12
    loop    .again
    mov     [edi],dword -1
else
;   Copyright (C) 1999-2001  Brian Paul
;   Copyright (C)            Maciej Guba
;---------------------
;  in:  esi - ptr to points(normals], each point(normal) coeficient as dword
;       edi - ptr to rotated points(normals)
;       ebx - ptr to 3x3 (9 dwords, 36 bytes) rotation matrix
;       ecx - number of points(normals)
;align 32
    movups   xmm4,[ebx]
 ;   lddqu    xmm4,[ebx]   ; I tried sse3 :D
    movups   xmm5,[ebx+12]
    movups   xmm6,[ebx+24]
;align 32
  .again:
    movss    xmm0,dword[esi]
    shufps   xmm0,xmm0,0
    mulps    xmm0,xmm4

    movss    xmm1,dword[esi+4]
    shufps   xmm1,xmm1,0
    mulps    xmm1,xmm5

    movss    xmm2,dword[esi+8]
    shufps   xmm2,xmm2,0
    mulps    xmm2,xmm6

    addps    xmm0,xmm1
    addps    xmm0,xmm2

    movups   [edi],xmm0

    add      esi,12
    add      edi,12
    dec      ecx
    jne      .again
    mov      [edi],dword -1
end if
ret
;----------------------------------------------
;  esi - pointer to 3x3 matrix
add_scale_to_matrix:
     fninit
     fld     [rsscale]
     fld     dword[esi]            ;-----
     fmul    st,st1
     fstp    dword[esi]
     fld     dword[esi+12]           ; x scale
     fmul    st,st1
     fstp    dword[esi+12]
     fld     dword[esi+24]
     fmul    st,st1
     fstp    dword[esi+24]         ;------

     fld     dword[esi+4]          ;-----
     fmul    st,st1
     fstp    dword[esi+4]
     fld     dword[esi+16]            ; y scale
     fmul    st,st1
     fstp    dword[esi+16]
     fld     dword[esi+28]
     fmul    st,st1
     fstp    dword[esi+28]         ;------


     fld     dword[esi+8]          ;-----
     fmul    st,st1
     fstp    dword[esi+8]
     fld     dword[esi+20]              ; z scale
     fmul    st,st1
     fstp    dword[esi+20]
     fld     dword[esi+32]
     fmulp    st1,st
     fstp    dword[esi+32]         ;------

ret

;in   esi - offset to 3d points  (point as 3 dwords float)
;     edi - offset to 2d points  ( as 3 words integer)
;     ecx - number of points
translate_points:  ; just convert into integer; z coord still needed
    fninit
  .again:
  if 0
    fld    dword[esi+8]
 ;   fmul   [rsscale]
    fist   word[edi+4]

    fisub  [zobs]
    fchs

    fld    dword[esi]
;    fmul   [rsscale]
    fisub  [xobs]
    fimul  [zobs]
    fdiv   st0,st1

    fiadd  [xobs]
    fiadd  [vect_x]
    fistp  word[edi]

    fld    dword[esi+4]
;    fmul   [rsscale]
    fisub  [yobs]
    fimul  [zobs]
    fdivrp  ;   st0,st1

    fiadd  [yobs]
    fiadd  [vect_y]
    fistp  word[edi+2]
   end if
   ; movups   xmm0,[esi]
   ; cvtps2dq xmm0,xmm0
   ; packsdw xmm0,xmm0
   ; movq     [edi]
    fld    dword[esi]
    fiadd  [vect_x]
    fistp  word[edi]
    fld    dword[esi+4]
    fiadd  [vect_y]
    fistp  word[edi+2]
    fld    dword[esi+8]
    fistp  word[edi+4]
    add    esi,12
    add    edi,6
    dec    ecx
    jnz    .again

ret