Subversion Repositories Kolibri OS

Rev

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