Subversion Repositories Kolibri OS

Rev

Rev 8666 | 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 >= SSE3
  202.         movups  xmm0,[edi]
  203.         andps   xmm0,[zero_hgst_dd]
  204.         movups  xmm1,xmm0
  205.         mulps   xmm0,xmm0
  206.         haddps  xmm0,xmm0
  207.         haddps  xmm0,xmm0
  208.         rsqrtps xmm0,xmm0
  209.         mulps   xmm0,xmm1
  210.         movlps  [edi],xmm0
  211.         movhlps xmm0,xmm0
  212.         movss   [edi+8],xmm0
  213. else
  214.  
  215.         fninit
  216.         fld     dword [edi+vec_x]
  217.         fmul    st, st
  218.         fld     dword [edi+vec_y]
  219.         fmul    st, st
  220.         fld     dword [edi+vec_z]
  221.         fmul    st, st
  222.         faddp   st1, st
  223.         faddp   st1, st
  224.         fsqrt
  225.  
  226.         ftst
  227.         fstsw ax
  228.         sahf
  229.         jnz     @f
  230.  
  231.         fst     dword [edi+vec_x]
  232.         fst     dword [edi+vec_y]
  233.         fstp    dword [edi+vec_z]
  234.         ret
  235.       @@:
  236.         fld st
  237.         fld st
  238.         fdivr dword [edi+vec_x]
  239.         fstp  dword [edi+vec_x]
  240.         fdivr dword [edi+vec_y]
  241.         fstp  dword [edi+vec_y]
  242.         fdivr dword [edi+vec_z]
  243.         fstp  dword [edi+vec_z]
  244. end if
  245. ret
  246. ;------------------in: -------------------------
  247. ;------------------ esi - pointer to 1st vector
  248. ;------------------ edi - pointer to 2nd vector
  249. ;------------------out: ------------------------
  250. ;------------------ st0 - dot-product
  251. dot_product:
  252.         fninit
  253. ;if Ext >=SSE3
  254. ;        movups  xmm0,[esi]
  255. ;        movups  xmm1,[edi]
  256. ;        andps   xmm0,[zero_hgst_dd]
  257. ;        mulps   xmm0,xmm1
  258. ;        haddps  xmm0,xmm0
  259. ;        haddps  xmm0,xmm0
  260. ;        movss   [esp-4],xmm0
  261. ;        fld     dword[esp-4]
  262. ;else
  263.         fld     dword [esi+vec_x]
  264.         fmul    dword [edi+vec_x]
  265.         fld     dword [esi+vec_y]
  266.         fmul    dword [edi+vec_y]
  267.         fld     dword [esi+vec_z]
  268.         fmul    dword [edi+vec_z]
  269.         faddp
  270.         faddp
  271. ;end if
  272. ret
  273.  
  274. ; DOS version Coded by Mikolaj Felix aka Majuma
  275. ; mfelix@polbox.com
  276. ; www.majuma.xt.pl
  277. ; into FASM translation by Macgub
  278. init_sincos_tab:
  279. .counter   equ  dword [ebp-4]  ; cur angle
  280.  
  281.      push       ebp
  282.      mov        ebp,esp
  283.  
  284.      xor        eax,eax
  285.      push       eax            ; init .counter
  286.      mov        edi,cos_tab
  287.      mov        esi,sin_tab
  288.      mov        ecx,256
  289.      fninit
  290.  
  291.      fld        .counter
  292.   @@:
  293.      fld        st
  294.      fsincos
  295.      fstp       dword [edi]
  296.      fstp       dword [esi]
  297. ;     fadd       [piD180]
  298.      fadd       [piD128]
  299.      add        esi,4
  300.      add        edi,4
  301.      loop       @b
  302.      ffree      st
  303.  
  304.      mov        esp,ebp
  305.      pop        ebp
  306. ret
  307. ;------
  308. ; esi - offset (pointer) to angles, edi offset to 3x3 matrix
  309. make_rotation_matrix:
  310.    .sinx   equ dword[ebp-4]
  311.    .cosx   equ dword[ebp-8]
  312.    .siny   equ dword[ebp-12]
  313.    .cosy   equ dword[ebp-16]
  314.    .sinz   equ dword[ebp-20]
  315.    .cosz   equ dword[ebp-24]
  316.      push      ebp
  317.      mov       ebp,esp
  318.      sub       esp,24
  319.  
  320.      movzx     ebx,word[esi]
  321.      shl       ebx,2
  322.      mov       eax,dword[sin_tab+ebx]
  323.      mov       .sinx,eax
  324.      mov       edx,dword[cos_tab+ebx]
  325.      mov       .cosx,edx
  326.  
  327.      movzx     ebx,word[esi+2]
  328.      shl       ebx,2
  329.      mov       eax,dword[sin_tab+ebx]
  330.      mov       .siny,eax
  331.      mov       edx,dword[cos_tab+ebx]
  332.      mov       .cosy,edx
  333.  
  334.      movzx     ebx,word[esi+4]
  335.      shl       ebx,2
  336.      mov       eax,dword[sin_tab+ebx]
  337.      mov       .sinz,eax
  338.      mov       edx,dword[cos_tab+ebx]
  339.      mov       .cosz,edx
  340.  
  341.      fninit
  342.      fld       .cosy
  343.      fmul      .cosz
  344.      fstp      dword[edi]
  345.  
  346.      fld       .sinx
  347.      fmul      .siny
  348.      fmul      .cosz
  349.      fld       .cosx
  350.      fmul      .sinz
  351.      fchs
  352.      faddp
  353.      fstp      dword[edi+12]
  354.  
  355.      fld       .cosx
  356.      fmul      .siny
  357.      fmul      .cosz
  358.      fld       .sinx
  359.      fmul      .sinz
  360.      faddp
  361.      fstp      dword[edi+24]
  362.  
  363.      fld       .cosy
  364.      fmul      .sinz
  365.      fstp      dword[edi+4]
  366.  
  367.      fld       .sinx
  368.      fmul      .siny
  369.      fmul      .sinz
  370.      fld       .cosx
  371.      fmul      .cosz
  372.      faddp
  373.      fstp      dword[edi+16]
  374.  
  375.      fld       .cosx
  376.      fmul      .siny
  377.      fmul      .sinz
  378.      fld       .sinx
  379.      fchs
  380.      fmul      .cosz
  381.      faddp
  382.      fstp      dword[edi+28]
  383.  
  384.      fld       .siny
  385.      fchs
  386.      fstp      dword[edi+8]
  387.  
  388.      fld       .cosy
  389.      fmul      .sinx
  390.      fstp      dword[edi+20]
  391.  
  392.      fld       .cosx
  393.      fmul      .cosy
  394.      fstp      dword[edi+32]
  395.  
  396.      mov       esp,ebp
  397.      pop       ebp
  398. ret
  399. ;---------------------
  400. ;  in:  esi - ptr to points(normals], each point(normal) coeficient as dword
  401. ;       edi - ptr to rotated points(normals)
  402. ;       ebx - ptr to 3x3 (9 dwords, 36 bytes) rotation matrix
  403. ;       ecx - number of points(normals)
  404. rotary:
  405. if Ext<SSE
  406.     fninit
  407.  .again:
  408.  
  409.     fld     dword[esi]
  410.     fmul    dword[ebx]
  411.     fld     dword[esi+4]
  412.     fmul    dword[ebx+12]
  413.     faddp
  414.     fld     dword[esi+8]
  415.     fmul    dword[ebx+24]
  416.     faddp
  417.     fstp    dword[edi]
  418.  
  419.  
  420.     fld     dword[esi+4]
  421.     fmul    dword[ebx+16]
  422.     fld     dword[esi]
  423.     fmul    dword[ebx+4]
  424.     faddp
  425.     fld     dword[esi+8]
  426.     fmul    dword[ebx+28]
  427.     faddp
  428.     fstp    dword[edi+4]
  429.  
  430.  
  431.     fld     dword[esi+8]
  432.     fmul    dword[ebx+32]
  433.     fld     dword[esi]
  434.     fmul    dword[ebx+8]
  435.     fld     dword[esi+4]
  436.     fmul    dword[ebx+20]
  437.     faddp
  438.     faddp
  439.     fstp    dword[edi+8]
  440.  
  441.  
  442.     add     esi,12
  443.     add     edi,12
  444.     loop    .again
  445.     mov     [edi],dword -1
  446. else
  447. ;   Copyright (C) 1999-2001  Brian Paul
  448. ;   Copyright (C)            Maciej Guba
  449. ;---------------------
  450. ;  in:  esi - ptr to points(normals], each point(normal) coeficient as dword
  451. ;       edi - ptr to rotated points(normals)
  452. ;       ebx - ptr to 3x3 (9 dwords, 36 bytes) rotation matrix
  453. ;       ecx - number of points(normals)
  454. ;align 32
  455.     movups   xmm4,[ebx]
  456.  ;   lddqu    xmm4,[ebx]   ; I tried sse3 :D
  457.     movups   xmm5,[ebx+12]
  458.     movups   xmm6,[ebx+24]
  459. ;align 32
  460.   .again:
  461.     movss    xmm0,dword[esi]
  462.     shufps   xmm0,xmm0,0
  463.     mulps    xmm0,xmm4
  464.  
  465.     movss    xmm1,dword[esi+4]
  466.     shufps   xmm1,xmm1,0
  467.     mulps    xmm1,xmm5
  468.  
  469.     movss    xmm2,dword[esi+8]
  470.     shufps   xmm2,xmm2,0
  471.     mulps    xmm2,xmm6
  472.  
  473.     addps    xmm0,xmm1
  474.     addps    xmm0,xmm2
  475.  
  476.     movups   [edi],xmm0
  477.  
  478.     add      esi,12
  479.     add      edi,12
  480.     dec      ecx
  481.     jne      .again
  482.     mov      [edi],dword -1
  483. end if
  484. ret
  485. ;----------------------------------------------
  486. ;  esi - pointer to 3x3 matrix
  487. add_scale_to_matrix:
  488.      fninit
  489.      fld     [rsscale]
  490.      fld     dword[esi]            ;-----
  491.      fmul    st,st1
  492.      fstp    dword[esi]
  493.      fld     dword[esi+12]           ; x scale
  494.      fmul    st,st1
  495.      fstp    dword[esi+12]
  496.      fld     dword[esi+24]
  497.      fmul    st,st1
  498.      fstp    dword[esi+24]         ;------
  499.  
  500.      fld     dword[esi+4]          ;-----
  501.      fmul    st,st1
  502.      fstp    dword[esi+4]
  503.      fld     dword[esi+16]            ; y scale
  504.      fmul    st,st1
  505.      fstp    dword[esi+16]
  506.      fld     dword[esi+28]
  507.      fmul    st,st1
  508.      fstp    dword[esi+28]         ;------
  509.  
  510.  
  511.      fld     dword[esi+8]          ;-----
  512.      fmul    st,st1
  513.      fstp    dword[esi+8]
  514.      fld     dword[esi+20]              ; z scale
  515.      fmul    st,st1
  516.      fstp    dword[esi+20]
  517.      fld     dword[esi+32]
  518.      fmulp    st1,st
  519.      fstp    dword[esi+32]         ;------
  520.  
  521. ret
  522.  
  523. ;in   esi - offset to 3d points  (point as 3 dwords float)
  524. ;     edi - offset to 2d points  ( as 3 words integer)
  525. ;     ecx - number of points
  526. translate_points:  ; just convert into integer; z coord still needed
  527.     fninit
  528.   .again:
  529.   if 0
  530.     fld    dword[esi+8]
  531.  ;   fmul   [rsscale]
  532.     fist   word[edi+4]
  533.  
  534.     fisub  [zobs]
  535.     fchs
  536.  
  537.     fld    dword[esi]
  538. ;    fmul   [rsscale]
  539.     fisub  [xobs]
  540.     fimul  [zobs]
  541.     fdiv   st0,st1
  542.  
  543.     fiadd  [xobs]
  544.     fiadd  [vect_x]
  545.     fistp  word[edi]
  546.  
  547.     fld    dword[esi+4]
  548. ;    fmul   [rsscale]
  549.     fisub  [yobs]
  550.     fimul  [zobs]
  551.     fdivrp  ;   st0,st1
  552.  
  553.     fiadd  [yobs]
  554.     fiadd  [vect_y]
  555.     fistp  word[edi+2]
  556.    end if
  557.    ; movups   xmm0,[esi]
  558.    ; cvtps2dq xmm0,xmm0
  559.    ; packsdw xmm0,xmm0
  560.    ; movq     [edi]
  561.     fld    dword[esi]
  562.     fiadd  [vect_x]
  563.     fistp  word[edi]
  564.     fld    dword[esi+4]
  565.     fiadd  [vect_y]
  566.     fistp  word[edi+2]
  567.     fld    dword[esi+8]
  568.     fistp  word[edi+4]
  569.     add    esi,12
  570.     add    edi,6
  571.     dec    ecx
  572.     jnz    .again
  573.  
  574. ret
  575.