Subversion Repositories Kolibri OS

Rev

Rev 6769 | Go to most recent revision | Blame | Last modification | View Log | Download | RSS feed

  1. x3d equ 0
  2. y3d equ 2
  3. z3d equ 4
  4. vec_x equ 0
  5. vec_y equ 4
  6. vec_z equ 8
  7. ; 3d point - triple integer word coordinate
  8. ; vector   - triple float dword coordinate
  9. ;----------------------in: --------------------------------
  10. ;------------------------ esi - pointer to 1st 3d point ---
  11. ;------------------------ edi - pointer to 2nd 3d point ---
  12. ;------------------------ ebx - pointer to result vector --
  13. ;---------------------- out : none ------------------------
  14. if 0
  15. make_vector:
  16.         fninit
  17.         fild    word[edi+x3d]                ;edi+x3d
  18.         fisub   word[esi+x3d]                ;esi+x3d
  19.         fstp    dword[ebx+vec_x]
  20.  
  21.         fild    word[edi+y3d]
  22.         fisub   word[esi+y3d]
  23.         fstp    dword[ebx+vec_y]
  24.  
  25.         fild    word[edi+z3d]
  26.         fisub   word[esi+z3d]
  27.         fstp    dword[ebx+vec_z]
  28.  
  29. ret
  30. end if
  31. reverse_mx_3x3:
  32. ; esi - source matrix
  33. ; edi - desired reversed matrix
  34.  
  35.   push  ebp
  36.   mov   ebp,esp
  37.   sub   esp,4
  38.   .det  equ  ebp-4
  39.  
  40.   fninit
  41.   fld  dword[esi]
  42.   fmul dword[esi+16]
  43.   fmul dword[esi+32]
  44.   fld  dword[esi+12]
  45.   fmul dword[esi+28]
  46.   fmul dword[esi+8]
  47.   faddp
  48.   fld  dword[esi+24]
  49.   fmul dword[esi+4]
  50.   fmul dword[esi+20]
  51.   faddp
  52.   fld  dword[esi]
  53.   fmul dword[esi+28]
  54.   fmul dword[esi+20]
  55.   fchs
  56.   faddp
  57.   fld  dword[esi+24]
  58.   fmul dword[esi+16]
  59.   fmul dword[esi+8]
  60.   fchs
  61.   faddp
  62.   fld  dword[esi+12]
  63.   fmul dword[esi+4]
  64.   fmul dword[esi+32]
  65.   fchs
  66.   faddp
  67.   fstp dword[.det]
  68.   cmp  dword[.det],0
  69.   jne  @f
  70.   int3
  71.  @@:
  72.  ; fld1
  73.  ; fdiv dword[.det]
  74.  ; fstp dword[.det]
  75.  
  76.   fld  dword[esi+16]
  77.   fmul dword[esi+32]
  78.   fld  dword[esi+20]
  79.   fmul dword[esi+28]
  80.   fchs
  81.   faddp
  82.   fdiv dword[.det]
  83.   fstp dword[edi]
  84.  
  85.   fld  dword[esi+8]
  86.   fmul dword[esi+28]
  87.   fld  dword[esi+4]
  88.   fmul dword[esi+32]
  89.   fchs
  90.   faddp
  91.   fdiv dword[.det]
  92.   fstp dword[edi+4]
  93.  
  94.   fld  dword[esi+4]
  95.   fmul dword[esi+20]
  96.   fld  dword[esi+8]
  97.   fmul dword[esi+16]
  98.   fchs
  99.   faddp
  100.   fdiv dword[.det]
  101.   fstp dword[edi+8]
  102.  
  103.   fld  dword[esi+20]
  104.   fmul dword[esi+24]
  105.   fld  dword[esi+12]
  106.   fmul dword[esi+32]
  107.   fchs
  108.   faddp
  109.   fdiv dword[.det]
  110.   fstp dword[edi+12]
  111.  
  112.   fld  dword[esi]
  113.   fmul dword[esi+32]
  114.   fld  dword[esi+8]
  115.   fmul dword[esi+24]
  116.   fchs
  117.   faddp
  118.   fdiv dword[.det]
  119.   fstp dword[edi+16]
  120.  
  121.   fld  dword[esi+8]
  122.   fmul dword[esi+12]
  123.   fld  dword[esi]
  124.   fmul dword[esi+20]
  125.   fchs
  126.   faddp
  127.   fdiv dword[.det]
  128.   fstp dword[edi+20]
  129.  
  130.   fld  dword[esi+12]
  131.   fmul dword[esi+28]
  132.   fld  dword[esi+16]
  133.   fmul dword[esi+24]
  134.   fchs
  135.   faddp
  136.   fdiv dword[.det]
  137.   fstp dword[edi+24]
  138.  
  139.   fld  dword[esi+4]
  140.   fmul dword[esi+24]
  141.   fld  dword[esi]
  142.   fmul dword[esi+28]
  143.   fchs
  144.   faddp
  145.   fdiv dword[.det]
  146.   fstp dword[edi+28]
  147.  
  148.   fld  dword[esi]
  149.   fmul dword[esi+16]
  150.   fld  dword[esi+4]
  151.   fmul dword[esi+12]
  152.   fchs
  153.   faddp
  154.   fdiv dword[.det]
  155.   fstp dword[edi+32]
  156.  
  157.  
  158.   mov  esp,ebp
  159.   pop  ebp
  160. ret
  161.  
  162. make_vector_r:
  163.    if Ext < SSE2
  164.         fninit
  165.         fld     dword[edi]                ;edi+x3d
  166.         fsub    dword[esi]                ;esi+x3d
  167.         fstp    dword[ebx+vec_x]
  168.  
  169.         fld     dword[edi+4]
  170.         fsub    dword[esi+4]
  171.         fstp    dword[ebx+vec_y]
  172.  
  173.         fld     dword[edi+8]
  174.         fsub    dword[esi+8]
  175.         fstp    dword[ebx+vec_z]
  176.     else
  177.         movups  xmm0,[esi]
  178.         movups  xmm1,[edi]
  179.         subps   xmm1,xmm0
  180.         movlps  [ebx],xmm1
  181.         movhlps  xmm1,xmm1
  182.         movss   [ebx+8],xmm1
  183.      end if
  184.  
  185. ret
  186. ;---------------------- in: -------------------------------
  187. ;--------------------------- esi - pointer to 1st vector --
  188. ;--------------------------- edi - pointer to 2nd vector --
  189. ;--------------------------- ebx - pointer to result vector
  190. ;---------------------- out : none
  191. cross_product:
  192.         fninit
  193.         fld     dword [esi+vec_y]
  194.         fmul    dword [edi+vec_z]
  195.         fld     dword [esi+vec_z]
  196.         fmul    dword [edi+vec_y]
  197.         fsubp   ;st1 ,st
  198.         fstp    dword [ebx+vec_x]
  199.  
  200.         fld     dword [esi+vec_z]
  201.         fmul    dword [edi+vec_x]
  202.         fld     dword [esi+vec_x]
  203.         fmul    dword [edi+vec_z]
  204.         fsubp   ;st1 ,st
  205.         fstp    dword [ebx+vec_y]
  206.  
  207.         fld     dword [esi+vec_x]
  208.         fmul    dword [edi+vec_y]
  209.         fld     dword [esi+vec_y]
  210.         fmul    dword [edi+vec_x]
  211.         fsubp   ;st1 ,st
  212.         fstp    dword [ebx+vec_z]
  213. ret
  214. ;----------------------- in: ------------------------------
  215. ;---------------------------- edi - pointer to vector -----
  216. ;----------------------- out : none
  217. normalize_vector:
  218.         fninit
  219.         fld     dword [edi+vec_x]
  220.         fmul    st, st
  221.         fld     dword [edi+vec_y]
  222.         fmul    st, st
  223.         fld     dword [edi+vec_z]
  224.         fmul    st, st
  225.         faddp   st1, st
  226.         faddp   st1, st
  227.         fsqrt
  228.  
  229.         ftst
  230.         fstsw ax
  231.         sahf
  232.         jnz     @f
  233.  
  234.         fst     dword [edi+vec_x]
  235.         fst     dword [edi+vec_y]
  236.         fstp    dword [edi+vec_z]
  237.         ret
  238.       @@:
  239.         fld st
  240.         fld st
  241.         fdivr dword [edi+vec_x]
  242.         fstp  dword [edi+vec_x]
  243.         fdivr dword [edi+vec_y]
  244.         fstp  dword [edi+vec_y]
  245.         fdivr dword [edi+vec_z]
  246.         fstp  dword [edi+vec_z]
  247. ret
  248. ;------------------in: -------------------------
  249. ;------------------ esi - pointer to 1st vector
  250. ;------------------ edi - pointer to 2nd vector
  251. ;------------------out: ------------------------
  252. ;------------------ st0 - dot-product
  253. dot_product:
  254.         fninit
  255.         fld     dword [esi+vec_x]
  256.         fmul    dword [edi+vec_x]
  257.         fld     dword [esi+vec_y]
  258.         fmul    dword [edi+vec_y]
  259.         fld     dword [esi+vec_z]
  260.         fmul    dword [edi+vec_z]
  261.         faddp
  262.         faddp
  263. ret
  264.  
  265. ; DOS version Coded by Mikolaj Felix aka Majuma
  266. ; mfelix@polbox.com
  267. ; www.majuma.xt.pl
  268. ; into FASM translation by Macgub
  269. init_sincos_tab:
  270. .counter   equ  dword [ebp-4]  ; cur angle
  271.  
  272.      push       ebp
  273.      mov        ebp,esp
  274.  
  275.      xor        eax,eax
  276.      push       eax            ; init .counter
  277.      mov        edi,cos_tab
  278.      mov        esi,sin_tab
  279.      mov        ecx,256
  280.      fninit
  281.  
  282.      fld        .counter
  283.   @@:
  284.      fld        st
  285.      fsincos
  286.      fstp       dword [edi]
  287.      fstp       dword [esi]
  288. ;     fadd       [piD180]
  289.      fadd       [piD128]
  290.      add        esi,4
  291.      add        edi,4
  292.      loop       @b
  293.      ffree      st
  294.  
  295.      mov        esp,ebp
  296.      pop        ebp
  297. ret
  298. ;------
  299. ; esi - offset (pointer) to angles, edi offset to 3x3 matrix
  300. make_rotation_matrix:
  301.    .sinx   equ dword[ebp-4]
  302.    .cosx   equ dword[ebp-8]
  303.    .siny   equ dword[ebp-12]
  304.    .cosy   equ dword[ebp-16]
  305.    .sinz   equ dword[ebp-20]
  306.    .cosz   equ dword[ebp-24]
  307.      push      ebp
  308.      mov       ebp,esp
  309.      sub       esp,24
  310.  
  311.      movzx     ebx,word[esi]
  312.      shl       ebx,2
  313.      mov       eax,dword[sin_tab+ebx]
  314.      mov       .sinx,eax
  315.      mov       edx,dword[cos_tab+ebx]
  316.      mov       .cosx,edx
  317.  
  318.      movzx     ebx,word[esi+2]
  319.      shl       ebx,2
  320.      mov       eax,dword[sin_tab+ebx]
  321.      mov       .siny,eax
  322.      mov       edx,dword[cos_tab+ebx]
  323.      mov       .cosy,edx
  324.  
  325.      movzx     ebx,word[esi+4]
  326.      shl       ebx,2
  327.      mov       eax,dword[sin_tab+ebx]
  328.      mov       .sinz,eax
  329.      mov       edx,dword[cos_tab+ebx]
  330.      mov       .cosz,edx
  331.  
  332.      fninit
  333.      fld       .cosy
  334.      fmul      .cosz
  335.      fstp      dword[edi]
  336.  
  337.      fld       .sinx
  338.      fmul      .siny
  339.      fmul      .cosz
  340.      fld       .cosx
  341.      fmul      .sinz
  342.      fchs
  343.      faddp
  344.      fstp      dword[edi+12]
  345.  
  346.      fld       .cosx
  347.      fmul      .siny
  348.      fmul      .cosz
  349.      fld       .sinx
  350.      fmul      .sinz
  351.      faddp
  352.      fstp      dword[edi+24]
  353.  
  354.      fld       .cosy
  355.      fmul      .sinz
  356.      fstp      dword[edi+4]
  357.  
  358.      fld       .sinx
  359.      fmul      .siny
  360.      fmul      .sinz
  361.      fld       .cosx
  362.      fmul      .cosz
  363.      faddp
  364.      fstp      dword[edi+16]
  365.  
  366.      fld       .cosx
  367.      fmul      .siny
  368.      fmul      .sinz
  369.      fld       .sinx
  370.      fchs
  371.      fmul      .cosz
  372.      faddp
  373.      fstp      dword[edi+28]
  374.  
  375.      fld       .siny
  376.      fchs
  377.      fstp      dword[edi+8]
  378.  
  379.      fld       .cosy
  380.      fmul      .sinx
  381.      fstp      dword[edi+20]
  382.  
  383.      fld       .cosx
  384.      fmul      .cosy
  385.      fstp      dword[edi+32]
  386.  
  387.      mov       esp,ebp
  388.      pop       ebp
  389. ret
  390. ;---------------------
  391. ;  in:  esi - ptr to points(normals], each point(normal) coeficient as dword
  392. ;       edi - ptr to rotated points(normals)
  393. ;       ebx - ptr to 3x3 (9 dwords, 36 bytes) rotation matrix
  394. ;       ecx - number of points(normals)
  395. rotary:
  396. if Ext<SSE
  397.     fninit
  398.  .again:
  399.  
  400.     fld     dword[esi]
  401.     fmul    dword[ebx]
  402.     fld     dword[esi+4]
  403.     fmul    dword[ebx+12]
  404.     faddp
  405.     fld     dword[esi+8]
  406.     fmul    dword[ebx+24]
  407.     faddp
  408.     fstp    dword[edi]
  409.  
  410.  
  411.     fld     dword[esi+4]
  412.     fmul    dword[ebx+16]
  413.     fld     dword[esi]
  414.     fmul    dword[ebx+4]
  415.     faddp
  416.     fld     dword[esi+8]
  417.     fmul    dword[ebx+28]
  418.     faddp
  419.     fstp    dword[edi+4]
  420.  
  421.  
  422.     fld     dword[esi+8]
  423.     fmul    dword[ebx+32]
  424.     fld     dword[esi]
  425.     fmul    dword[ebx+8]
  426.     fld     dword[esi+4]
  427.     fmul    dword[ebx+20]
  428.     faddp
  429.     faddp
  430.     fstp    dword[edi+8]
  431.  
  432.  
  433.     add     esi,12
  434.     add     edi,12
  435.     loop    .again
  436.     mov     [edi],dword -1
  437. else
  438. ;   Copyright (C) 1999-2001  Brian Paul
  439. ;   Copyright (C)            Maciej Guba
  440. ;---------------------
  441. ;  in:  esi - ptr to points(normals], each point(normal) coeficient as dword
  442. ;       edi - ptr to rotated points(normals)
  443. ;       ebx - ptr to 3x3 (9 dwords, 36 bytes) rotation matrix
  444. ;       ecx - number of points(normals)
  445. ;align 32
  446.     movups   xmm4,[ebx]
  447.  ;   lddqu    xmm4,[ebx]   ; I tried sse3 :D
  448.     movups   xmm5,[ebx+12]
  449.     movups   xmm6,[ebx+24]
  450. ;align 32
  451.   .again:
  452.     movss    xmm0,dword[esi]
  453.     shufps   xmm0,xmm0,0
  454.     mulps    xmm0,xmm4
  455.  
  456.     movss    xmm1,dword[esi+4]
  457.     shufps   xmm1,xmm1,0
  458.     mulps    xmm1,xmm5
  459.  
  460.     movss    xmm2,dword[esi+8]
  461.     shufps   xmm2,xmm2,0
  462.     mulps    xmm2,xmm6
  463.  
  464.     addps    xmm0,xmm1
  465.     addps    xmm0,xmm2
  466.  
  467.     movups   [edi],xmm0
  468.  
  469.     add      esi,12
  470.     add      edi,12
  471.     dec      ecx
  472.     jne      .again
  473.     mov      [edi],dword -1
  474. end if
  475. ret
  476. ;----------------------------------------------
  477. ;  esi - pointer to 3x3 matrix
  478. add_scale_to_matrix:
  479.      fninit
  480.      fld     [rsscale]
  481.      fld     dword[esi]            ;-----
  482.      fmul    st,st1
  483.      fstp    dword[esi]
  484.      fld     dword[esi+12]           ; x scale
  485.      fmul    st,st1
  486.      fstp    dword[esi+12]
  487.      fld     dword[esi+24]
  488.      fmul    st,st1
  489.      fstp    dword[esi+24]         ;------
  490.  
  491.      fld     dword[esi+4]          ;-----
  492.      fmul    st,st1
  493.      fstp    dword[esi+4]
  494.      fld     dword[esi+16]            ; y scale
  495.      fmul    st,st1
  496.      fstp    dword[esi+16]
  497.      fld     dword[esi+28]
  498.      fmul    st,st1
  499.      fstp    dword[esi+28]         ;------
  500.  
  501.  
  502.      fld     dword[esi+8]          ;-----
  503.      fmul    st,st1
  504.      fstp    dword[esi+8]
  505.      fld     dword[esi+20]              ; z scale
  506.      fmul    st,st1
  507.      fstp    dword[esi+20]
  508.      fld     dword[esi+32]
  509.      fmulp    st1,st
  510.      fstp    dword[esi+32]         ;------
  511.  
  512. ret
  513.  
  514. ;in   esi - offset to 3d points  (point as 3 dwords float)
  515. ;     edi - offset to 2d points  ( as 3 words integer)
  516. ;     ecx - number of points
  517. translate_points:  ; just convert into integer; z coord still needed
  518.     fninit
  519.   .again:
  520.   if 0
  521.     fld    dword[esi+8]
  522.  ;   fmul   [rsscale]
  523.     fist   word[edi+4]
  524.  
  525.     fisub  [zobs]
  526.     fchs
  527.  
  528.     fld    dword[esi]
  529. ;    fmul   [rsscale]
  530.     fisub  [xobs]
  531.     fimul  [zobs]
  532.     fdiv   st0,st1
  533.  
  534.     fiadd  [xobs]
  535.     fiadd  [vect_x]
  536.     fistp  word[edi]
  537.  
  538.     fld    dword[esi+4]
  539. ;    fmul   [rsscale]
  540.     fisub  [yobs]
  541.     fimul  [zobs]
  542.     fdivrp  ;   st0,st1
  543.  
  544.     fiadd  [yobs]
  545.     fiadd  [vect_y]
  546.     fistp  word[edi+2]
  547.    end if
  548.    ; movups   xmm0,[esi]
  549.    ; cvtps2dq xmm0,xmm0
  550.    ; packsdw xmm0,xmm0
  551.    ; movq     [edi]
  552.     fld    dword[esi]
  553.     fiadd  [vect_x]
  554.     fistp  word[edi]
  555.     fld    dword[esi+4]
  556.     fiadd  [vect_y]
  557.     fistp  word[edi+2]
  558.     fld    dword[esi+8]
  559.     fistp  word[edi+4]
  560.     add    esi,12
  561.     add    edi,6
  562.     dec    ecx
  563.     jnz    .again
  564.  
  565. ret
  566.