Subversion Repositories Kolibri OS

Rev

Rev 5256 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. ; Some simple mathematical functions. Don't look for some logic in
  2. ; the function names :-)
  3.  
  4. ; ******* Gestion des matrices 4x4 ******
  5.  
  6. align 4
  7. proc gl_M4_Id uses eax ecx edi, a:dword
  8.         mov edi,[a]
  9.         add edi,4
  10.         mov ecx,14
  11.         mov eax,0.0
  12.         rep stosd
  13.         mov eax,1.0
  14.         stosd
  15.         mov edi,[a]
  16.         stosd
  17.         add edi,16
  18.         stosd
  19.         add edi,16
  20.         stosd
  21.         ret
  22. endp
  23.  
  24. align 4
  25. proc gl_M4_IsId uses ebx ecx, a:dword
  26.         mov eax,[a]
  27.         xor ebx,ebx
  28.         xor ecx,ecx
  29.         .cycle_01:
  30.                 fld dword[eax]
  31.                 cmp ecx,ebx
  32.                 je .once
  33.                         ftst ;ñðàâíåíèå ñ 0.0
  34.                         fstsw ax
  35.                         sahf
  36.                         je @f
  37.                         jmp .not_1 ;åñëè äèàãîíàëüíûå ÷èñëà íå ðàâíû 0.0 ìàòðèöà íå åäèíè÷íàÿ
  38.                 .once:
  39.                         fld1
  40.                         fcomp st1 ;ñðàâíåíèå ñ 1.0
  41.                         fstsw ax
  42.                         test ah,0x40
  43.                         je .not_1 ;åñëè íå ðàâíî 1.0 ìàòðèöà íå åäèíè÷íàÿ
  44.                 @@:
  45.                 add eax,4
  46.                 inc ebx
  47.                 btr ebx,2
  48.                 jnc .cycle_01
  49.         inc ecx
  50.         bt ecx,2 ;ïðîâåðÿåì ðàâåíñòâî ecx==4
  51.         jnc .cycle_01
  52.  
  53.         mov eax,1
  54.         jmp @f
  55.         .not_1:
  56.                 xor eax,eax
  57.         @@:
  58.         ret
  59. endp
  60.  
  61. align 4
  62. proc gl_M4_Mul, c:dword,a:dword,b:dword
  63. pushad
  64.         mov edx,[c]
  65.         xor eax,eax
  66.         .cycle_0: ;i
  67.                 xor ebx,ebx
  68.                 .cycle_1: ;j
  69.                         finit
  70.                         fldz ;sum=0
  71.                         xor ecx,ecx
  72.                         M4_reg edi,[a],eax,0
  73.                         .cycle_2: ;k
  74.                                 fld dword[edi]
  75.                                 add edi,4
  76.                                 M4_reg esi,[b],ecx,ebx
  77.                                 fmul dword[esi]
  78.                                 fadd st0,st1 ;sum += a[i][k] * b[k][j]
  79.                                 inc ecx
  80.                                 cmp ecx,4
  81.                                 jl .cycle_2
  82.                         fstp dword[edx] ;c[i][j] = sum
  83.                         add edx,4
  84.                         inc ebx
  85.                         cmp ebx,4
  86.                         jl .cycle_1
  87.                 inc eax
  88.                 cmp eax,4
  89.                 jl .cycle_0
  90.         finit
  91. if DEBUG ;gl_M4_Mul
  92.         stdcall dbg_print,f_m4m,txt_nl
  93.         stdcall gl_print_matrix,[c],4
  94.         stdcall dbg_print,txt_sp,txt_nl
  95. end if
  96. popad
  97.         ret
  98. endp
  99.  
  100. ; c=c*a
  101. align 4
  102. proc gl_M4_MulLeft, c:dword,b:dword
  103. locals
  104.         i dd ?
  105.         a M4
  106. endl
  107. pushad
  108.         mov ecx,16
  109.         mov esi,[c]
  110.         mov edi,ebp
  111.         sub edi,sizeof.M4
  112.         rep movsd ;êîïèðîâàíèå ìàòðèö [a]=[c]
  113.  
  114.         mov edx,[c]
  115.         mov dword[i],0
  116.         .cycle_0: ;i
  117.                 xor ebx,ebx
  118.                 .cycle_1: ;j
  119.                         finit
  120.                         fldz ;sum=0
  121.                         xor ecx,ecx
  122.                         mov eax,ebp
  123.                         sub eax,sizeof.M4
  124.                         M4_reg edi,eax,dword[i],0
  125.                         .cycle_2: ;k
  126.                                 fld dword[edi]
  127.                                 add edi,4
  128.                                 M4_reg esi,[b],ecx,ebx
  129.                                 fmul dword[esi]
  130.                                 fadd st0,st1 ;sum += a[i][k] * b[k][j]
  131.                                 inc ecx
  132.                                 add eax,4
  133.                                 cmp ecx,4
  134.                                 jl .cycle_2
  135.                         fstp dword[edx] ;c[i][j] = sum
  136.                         add edx,4
  137.                         inc ebx
  138.                         cmp ebx,4
  139.                         jl .cycle_1
  140.                 inc dword[i]
  141.                 cmp dword[i],4
  142.                 jl .cycle_0
  143.         finit
  144. if DEBUG ;gl_M4_MulLeft
  145.         stdcall dbg_print,f_m4ml,txt_nl
  146.         stdcall gl_print_matrix,[c],4
  147.         stdcall dbg_print,txt_sp,txt_nl
  148. end if
  149. popad
  150.         ret
  151. endp
  152.  
  153. align 4
  154. proc gl_M4_Move uses ecx edi esi, a:dword,b:dword
  155.         mov edi,[a]
  156.         mov esi,[b]
  157.         mov ecx,sizeof.M4/4
  158.         rep movsd
  159.         ret
  160. endp
  161.  
  162. align 4
  163. proc gl_MoveV3 uses edi esi, a:dword,b:dword
  164.         mov edi,[a]
  165.         mov esi,[b]
  166.         movsd
  167.         movsd
  168.         movsd
  169.         ret
  170. endp
  171.  
  172. ;void gl_MulM4V3(V3 *a,M4 *b,V3 *c)
  173. ;{
  174. ;        a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3];
  175. ;        a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3];
  176. ;        a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3];
  177. ;}
  178.  
  179. ;void gl_MulM3V3(V3 *a,M4 *b,V3 *c)
  180. ;{
  181. ;        a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z;
  182. ;        a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z;
  183. ;        a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z;
  184. ;}
  185.  
  186. ;void gl_M4_MulV4(V4 *a,M4 *b,V4 *c)
  187. ;{
  188. ;        a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3]*c->W;
  189. ;        a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3]*c->W;
  190. ;        a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3]*c->W;
  191. ;        a->W=b->m[3][0]*c->X+b->m[3][1]*c->Y+b->m[3][2]*c->Z+b->m[3][3]*c->W;
  192. ;}
  193.  
  194. ; transposition of a 4x4 matrix
  195. align 4
  196. proc gl_M4_Transpose uses eax ecx edx, a:dword, b:dword
  197.         mov eax,[a]
  198.         mov ecx,[b]
  199.  
  200.         mov edx,[ecx]
  201.         mov [eax],edx
  202.         mov edx,[ecx+0x10]
  203.         mov [eax+0x4],edx
  204.         mov edx,[ecx+0x20]
  205.         mov [eax+0x8],edx
  206.         mov edx,[ecx+0x30]
  207.         mov [eax+0x0c],edx
  208.  
  209.         mov edx,[ecx+0x4]
  210.         mov [eax+0x10],edx
  211.         mov edx,[ecx+0x14]
  212.         mov [eax+0x14],edx
  213.         mov edx,[ecx+0x24]
  214.         mov [eax+0x18],edx
  215.         mov edx,[ecx+0x34]
  216.         mov [eax+0x1c],edx
  217.  
  218.         mov edx,[ecx+0x8]
  219.         mov [eax+0x20],edx
  220.         mov edx,[ecx+0x18]
  221.         mov [eax+0x24],edx
  222.         mov edx,[ecx+0x28]
  223.         mov [eax+0x28],edx
  224.         mov edx,[ecx+0x38]
  225.         mov [eax+0x2c],edx
  226.  
  227.         mov edx,[ecx+0x0c]
  228.         mov [eax+0x30],edx
  229.         mov edx,[ecx+0x1c]
  230.         mov [eax+0x34],edx
  231.         mov edx,[ecx+0x2c]
  232.         mov [eax+0x38],edx
  233.         mov edx,[ecx+0x3c]
  234.         mov [eax+0x3c],edx
  235.         ret
  236. endp
  237.  
  238. ;/* inversion of an orthogonal matrix of type Y=M.X+P */
  239. ;void gl_M4_InvOrtho(M4 *a,M4 b)
  240. ;{
  241. ;       int i,j;
  242. ;       float s;
  243. ;       for(i=0;i<3;i++)
  244. ;       for(j=0;j<3;j++) a->m[i][j]=b.m[j][i];
  245. ;       a->m[3][0]=0.0; a->m[3][1]=0.0; a->m[3][2]=0.0; a->m[3][3]=1.0;
  246. ;       for(i=0;i<3;i++) {
  247. ;               s=0;
  248. ;               for(j=0;j<3;j++) s-=b.m[j][i]*b.m[j][3];
  249. ;               a->m[i][3]=s;
  250. ;       }
  251. ;}
  252.  
  253. ;/* Inversion of a general nxn matrix.
  254. ;   Note : m is destroyed */
  255.  
  256. align 4
  257. proc Matrix_Inv uses ecx, r:dword, m:dword, n:dword ;(float *r,float *m,int n)
  258. ;        int i,j,k,l;
  259. ;        float max,tmp,t;
  260.  
  261. ;        /* identitée dans r */
  262. ;        for(i=0;i<n*n;i++) r[i]=0;
  263. ;        for(i=0;i<n;i++) r[i*n+i]=1;
  264.          
  265. ;        for(j=0;j<n;j++) {
  266.                        
  267. ;                       /* recherche du nombre de plus grand module sur la colonne j */
  268. ;                       max=m[j*n+j];
  269. ;                       k=j;
  270. ;                       for(i=j+1;i<n;i++)
  271. ;                               if (fabs(m[i*n+j])>fabs(max)) {
  272. ;                                        k=i;
  273. ;                                        max=m[i*n+j];
  274. ;                               }
  275.  
  276. ;      /* non intersible matrix */
  277. ;      if (max==0) return 1;
  278.  
  279. ;                       /* permutation des lignes j et k */
  280. ;                       if (k!=j) {
  281. ;                                for(i=0;i<n;i++) {
  282. ;                                               tmp=m[j*n+i];
  283. ;                                               m[j*n+i]=m[k*n+i];
  284. ;                                               m[k*n+i]=tmp;
  285. ;                                              
  286. ;                                               tmp=r[j*n+i];
  287. ;                                               r[j*n+i]=r[k*n+i];
  288. ;                                               r[k*n+i]=tmp;
  289. ;                                }
  290. ;                       }
  291.  
  292. ;                       /* multiplication de la ligne j par 1/max */
  293. ;                       max=1/max;
  294. ;                       for(i=0;i<n;i++) {
  295. ;                                m[j*n+i]*=max;
  296. ;                                r[j*n+i]*=max;
  297. ;                       }
  298.  
  299. ;                       for(l=0;l<n;l++) if (l!=j) {
  300. ;                                t=m[l*n+j];
  301. ;                                for(i=0;i<n;i++) {
  302. ;                                               m[l*n+i]-=m[j*n+i]*t;
  303. ;                                               r[l*n+i]-=r[j*n+i]*t;
  304. ;                                }
  305. ;                       }
  306. ;        }
  307.  
  308. ;        return 0;
  309.         ret
  310. endp
  311.  
  312. ; inversion of a 4x4 matrix
  313.  
  314. align 4
  315. proc gl_M4_Inv uses eax ecx edi esi, a:dword, b:dword
  316. locals
  317.         tmp M4
  318. endl
  319.         mov esi,[b]
  320.         mov edi,ebp
  321.         sub edi,sizeof.M4 ;edi = &tmp
  322.         mov ecx,16
  323.         rep movsd
  324.         sub edi,sizeof.M4 ;edi = &tmp
  325.         stdcall Matrix_Inv,[a],edi,4 ;ïîðòèò eax ïîòîìó â uses åñòü eax
  326.         ret
  327. endp
  328.  
  329. align 4
  330. proc gl_M4_Rotate uses eax ecx, a:dword,t:dword,u:dword
  331. locals
  332.         s dd ? ;float
  333.         c dd ? ;float
  334.         v dd ? ;int
  335.         w dd ? ;int
  336. endl
  337.         mov eax,[u]
  338.         inc eax
  339.         mov dword [v],eax
  340.         cmp dword [v],2
  341.         jle @f
  342.                 mov dword [v],0
  343.         @@:
  344.         mov eax,[v]
  345.         inc eax
  346.         mov dword [w],eax
  347.         cmp dword [w],2
  348.         jle @f
  349.                 mov dword [w],0
  350.         @@:
  351.         fld dword [t]
  352.         fsin
  353.         fstp dword [s]
  354.         fld dword [t]
  355.         fcos
  356.         fstp dword [c]
  357.  
  358.         stdcall gl_M4_Id,[a]
  359.  
  360.         M4_reg ecx,[a],[v],[v]
  361.         mov eax,[c]
  362.         mov [ecx],eax
  363.  
  364.         M4_reg ecx,[a],[v],[w]
  365.         fld dword [s]
  366.         fchs
  367.         fstp dword [ecx]
  368.  
  369.         M4_reg ecx,[a],[w],[v]
  370.         mov eax,[s]
  371.         mov [ecx],eax
  372.  
  373.         M4_reg ecx,[a],[w],[w]
  374.         mov eax,[c]
  375.         mov [ecx],eax
  376.  
  377.         ret
  378. endp
  379.  
  380. ; inverse of a 3x3 matrix
  381. ;void gl_M3_Inv(M3 *a,M3 *m)
  382. ;{
  383. ;        float det;
  384.  
  385. ;        det = m->m[0][0]*m->m[1][1]*m->m[2][2]-m->m[0][0]*m->m[1][2]*m->m[2][1]-
  386. ;                m->m[1][0]*m->m[0][1]*m->m[2][2]+m->m[1][0]*m->m[0][2]*m->m[2][1]+
  387. ;                m->m[2][0]*m->m[0][1]*m->m[1][2]-m->m[2][0]*m->m[0][2]*m->m[1][1];
  388.  
  389. ;        a->m[0][0] = (m->m[1][1]*m->m[2][2]-m->m[1][2]*m->m[2][1])/det;
  390. ;        a->m[0][1] = -(m->m[0][1]*m->m[2][2]-m->m[0][2]*m->m[2][1])/det;
  391. ;        a->m[0][2] = -(-m->m[0][1]*m->m[1][2]+m->m[0][2]*m->m[1][1])/det;
  392.  
  393. ;        a->m[1][0] = -(m->m[1][0]*m->m[2][2]-m->m[1][2]*m->m[2][0])/det;
  394. ;        a->m[1][1] = (m->m[0][0]*m->m[2][2]-m->m[0][2]*m->m[2][0])/det;
  395. ;        a->m[1][2] = -(m->m[0][0]*m->m[1][2]-m->m[0][2]*m->m[1][0])/det;
  396.  
  397. ;        a->m[2][0] = (m->m[1][0]*m->m[2][1]-m->m[1][1]*m->m[2][0])/det;
  398. ;        a->m[2][1] = -(m->m[0][0]*m->m[2][1]-m->m[0][1]*m->m[2][0])/det;
  399. ;        a->m[2][2] = (m->m[0][0]*m->m[1][1]-m->m[0][1]*m->m[1][0])/det;
  400. ;}
  401.  
  402. ; vector arithmetic
  403.  
  404. ;int gl_V3_Norm(V3 *a)
  405. ;{
  406. ;       float n;
  407. ;       n=sqrt(a->X*a->X+a->Y*a->Y+a->Z*a->Z);
  408. ;       if (n==0) return 1;
  409. ;       a->X/=n;
  410. ;       a->Y/=n;
  411. ;       a->Z/=n;
  412. ;       return 0;
  413. ;}
  414.  
  415. macro gl_V3_New p_mem, x, y, z
  416. {
  417.         mov dword[p_mem],x
  418.         mov dword[p_mem+4],y
  419.         mov dword[p_mem+8],z
  420. }
  421.  
  422. macro gl_V4_New p_mem, x, y, z, w
  423. {
  424.         mov dword[p_mem],x
  425.         mov dword[p_mem+4],y
  426.         mov dword[p_mem+8],z
  427.         mov dword[p_mem+12],w
  428. }
  429.