Subversion Repositories Kolibri OS

Rev

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