API tools faq
paste
Advertisement
Guest User

routine calcul matrice

a guest
Mar 9th, 2015
290
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Z80 Assembler 2.99 KB | None | 0 0
raw download clone embed print report
  1. ;calcul sur cette matrice:
  2. ;X_1 Z_2 Y_3 =  \c_2 C_3                - S_2                c_2 S_3 \
  3. ;           \ s_1 S_3 + c_1 C_3 S_2      c_1 c_2         c_1 S_2 S_3 - C_3 s_1 \
  4. ;           \ C_3 s_1 S_2 - c_1 S_3      c_2 s_1         c_1 C_3 + s_1 S_2 S_3 \
  5.  
  6. ;calcule la matris de rotation pour les angles x, y et z
  7. ;entrées:  GRotateAngle_surX
  8. ;       GRotateAngle_surY
  9. ;       GRotateAngle_surZ
  10. ;       SinTable : ta table de sinus
  11. gMatrix:   
  12.     .db 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0     ;matrice 4*4
  13. gMatrixRotationCalcul:
  14.     ;recherche des sinus et cosinus dans les tables
  15.     ld d,0
  16.     ld a,(GRotateAngle_surX)
  17.     ld ixh,a
  18.     ld a,(GRotateAngle_surY)
  19.     ld ixl,a
  20.     ld a,(GRotateAngle_surZ)
  21.     ld c,a
  22.     ld hl, SinTable
  23.     ;sin x
  24.     ld e, ixh
  25.     add hl, de
  26.     ld a, (hl)
  27.     ld (gSin_X),a
  28.     ;sin y
  29.     ld hl, SinTable
  30.     ld e, ixl
  31.     add hl, de
  32.     ld a, (hl)
  33.     ld (gSin_Y),a
  34.     ;sin z                                              ; a revoir pour optimiser
  35.     ld hl, SinTable
  36.     ld e, c
  37.     add hl, de
  38.     ld a, (hl)
  39.     ld (gSin_Z),a
  40.     ;cos x
  41.     ld hl, SinTable+128
  42.     ld e, ixh
  43.     sbc hl,de
  44.     ld a, (hl)
  45.     ld (gCos_X),a
  46.     ;cos y
  47.     ld hl, SinTable+128
  48.     ld e, ixl
  49.     sbc hl,de
  50.     ld a, (hl)
  51.     ld (gCos_Y),a
  52.     ;cos z
  53.     ld hl, SinTable+128
  54.     ld e, c
  55.     sbc hl,de
  56.     ld a, (hl)
  57.     ld (gCos_Z),a
  58.    
  59.     ;calculs
  60.  
  61.     ld hl, gCos_Y
  62.     ld e , (hl)
  63.     ld hl, gCos_Z
  64.     ld h, (hl)
  65.     call    mult_h_e
  66.     ld (gMatrix),a
  67.    
  68.     ld a,(gSin_Y)
  69.     neg
  70.     ld (gMatrix+1),a
  71.    
  72.     ld hl, gCos_Y
  73.     ld e, (hl)
  74.     inc hl
  75.     ld h, (hl)
  76.     call    mult_h_e
  77.     ld (gMatrix+2),a
  78.    
  79.     ld hl, gSin_X
  80.     ld e, (hl)
  81.     ld hl, gSin_Z
  82.     ld h,(hl)
  83.     call    mult_h_e
  84.     ld a,c
  85.     ld hl,gCos_X
  86.     ld e, (hl)
  87.     inc hl
  88.     ld h, (hl)
  89.     call    mult_h_e
  90.     ld e,a
  91.     ld hl,gCos_Z
  92.     ld h, (hl)
  93.     call mult_h_e
  94.     add a,c
  95.     ld (gMatrix+4),a
  96.    
  97.     ld hl,gCos_X
  98.     ld e,(hl)
  99.     ld hl,gCos_Y
  100.     ld h, (hl)
  101.     call mult_h_e
  102.     ld (gMatrix+5),a
  103.    
  104.     ld hl, gCos_X
  105.     ld e,(hl)
  106.     inc hl ;gSin_Y
  107.     ld h, (hl)
  108.     call mult_h_e
  109.     ld e,a
  110.     ld hl, gSin_Z
  111.     ld h, (hl)
  112.     call mult_h_e
  113.     ld c, a
  114.     ld hl, gSin_X
  115.     ld e, (hl)
  116.     ld hl, gCos_Z
  117.     ld h,(hl)
  118.     call mult_h_e
  119.     sub c
  120.     ld (gMatrix+6),a
  121.    
  122.     ld hl,gSin_X
  123.     ld e, (hl)
  124.     ld hl,gSin_Y
  125.     ld h, (hl)
  126.     call mult_h_e
  127.     ld e,a
  128.     ld hl, gCos_Z
  129.     ld h, (hl)
  130.     call mult_h_e
  131.     ld c, a
  132.     ld hl, gCos_X
  133.     ld e, (hl)
  134.     ld hl, gSin_Z
  135.     ld h, (hl)
  136.     call mult_h_e
  137.     sub c
  138.     ld (gMatrix+8),a
  139.    
  140.     ld hl, gSin_X
  141.     ld e, (hl)
  142.     ld hl, gCos_Y
  143.     ld h, (hl)
  144.     call mult_h_e
  145.     ld (gMatrix+9),a
  146.    
  147.     ld hl,gCos_X
  148.     ld e,(hl)
  149.     ld hl,gCos_Z
  150.     ld h,(hl)
  151.     call mult_h_e
  152.     ld c,a
  153.     ld hl, gSin_X
  154.     ld e,(hl)
  155.     ld hl, gSin_Y
  156.     ld h, (hl)
  157.     call mult_h_e
  158.     ld e,a
  159.     ld hl, gSin_Z
  160.     ld h, (hl)
  161.     call mult_h_e
  162.     add a,c
  163.     ld (gMatrix+10),a
  164.    
  165. gSin_X:
  166.     .db 0  
  167. gCos_X:
  168.     .db 0
  169. gSin_Y:
  170.     .db 0  
  171. gCos_Y:
  172.     .db 0
  173. gSin_Z:
  174.     .db 0
  175. gCos_Z:
  176.     .db 0
  177.  
  178.  
  179. ;multiplie fixed point à 64 et retourne dans a
  180. mult_h_e
  181.     ld  l, 0
  182.     ld  d, l
  183.  
  184.     sla h       ; optimised 1st iteration
  185.     jr  nc, $+3
  186.     ld  l, e
  187.    
  188.     ld b, 7
  189. _loop:
  190.     add hl, hl          
  191.     jr  nc, $+3
  192.     add hl, de
  193.    
  194.     djnz    _loop
  195.     ;pour remettre au bon format fixed point
  196.     ld a,l 
  197.     rla        
  198.     rla        
  199.     rla        
  200.     and 3      
  201.     ld l,a
  202.     ld a,h
  203.     rla        
  204.     rla        
  205.     or l           
  206.     ret
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!