home *** CD-ROM | disk | FTP | other *** search
/ Enigma Amiga Life 113 / EnigmaAmiga113CD.iso / software / sviluppo / quakeworld_src / client / mathlib68k.s < prev    next >
Encoding:
Text File  |  2000-06-17  |  21.6 KB  |  1,105 lines
  1. * 
  2. * Copyright (C) 1996-1997 Id Software, Inc. 
  3. * 
  4. * This program is free software; you can redistribute it and/or 
  5. * modify it under the terms of the GNU General Public License 
  6. * as published by the Free Software Foundation; either version 2 
  7. * of the License, or (at your option) any later version. 
  8. * 
  9. * This program is distributed in the hope that it will be useful, 
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of 
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.   
  12. * 
  13. * See the GNU General Public License for more details. 
  14. * 
  15. * You should have received a copy of the GNU General Public License 
  16. * along with this program; if not, write to the Free Software 
  17. * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA. 
  18. * 
  19.  
  20. **
  21. ** mathlib68k.asm
  22. ** math primitives, M68040/68060 or 68881/68882 (define M881)
  23. ** written by Frank Wille <frank@phoenix.owl.de>
  24. **
  25.  
  26. ; structure mplane_t
  27.         rsreset
  28. normal          rs.l    3
  29. dist            rs.s    1
  30. type            rs.b    1
  31. signbits        rs.b    1
  32.  
  33.  
  34.     code
  35.  
  36.     xref    _Sys_Error
  37.     IFD    STORMC
  38.     xref    _sin__r
  39.     xref    _cos__r
  40.     ELSE
  41.     xref    _sin
  42.     xref    _cos
  43.  
  44.     xdef    _anglemod
  45.     xdef    _anglemod__r
  46.     cnop    0,4
  47. _anglemod:
  48. _anglemod__r:
  49.     setso   4
  50. .a      so.s    1
  51.  
  52. ; a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
  53.     fmove.s .a(sp),fp0
  54.     fmul.d    #$4066c16c16c16c17,fp0 ;(182.04444)double precision required!
  55.     fmove.l fp0,d0
  56.     swap    d0
  57.     clr.w   d0
  58.     swap    d0
  59.     fmove.l d0,fp0
  60.     fmul.s  #0.0054931641,fp0
  61.     rts
  62.  
  63.  
  64.     xdef    _BoxOnPlaneSide
  65.     xdef    _BoxOnPlaneSide__r
  66.     cnop    0,4
  67. _BoxOnPlaneSide:
  68. _BoxOnPlaneSide__r:
  69.     move.l  a2,d1
  70.     fmovem.x fp2-fp6,-(sp)
  71.     setso   4+5*12
  72. .emins  so.l    1
  73. .emaxs  so.l    1
  74. .p      so.l    1
  75.  
  76.     move.l  .p(sp),a2
  77.     move.l  a2,a0                   ; preload normal[0..2]
  78.     fmove.s (a0)+,fp1
  79.     fmove.s (a0)+,fp2
  80.     move.l  .emins(sp),a1
  81.     fmove.s (a0),fp3
  82.     move.l  .emaxs(sp),a0
  83.  
  84. ; switch (p->signbits)
  85.     moveq   #7,d0
  86.     and.b   signbits(a2),d0
  87.     jmp     ([.jtab,pc,d0.w*4])
  88.  
  89. .jtab:  dc.l    .c0,.c1,.c2,.c3,.c4,.c5,.c6,.c7
  90.  
  91. ; dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
  92. ; dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
  93. .c0:    fmove.s (a0)+,fp5
  94.     fmul    fp1,fp5
  95.     fmove.s (a1)+,fp6
  96.     fmul    fp1,fp6
  97.     fmove.s (a1)+,fp0
  98.     fmul    fp2,fp0
  99.     fmove.s (a0)+,fp4
  100.     fadd    fp0,fp5
  101.     fmul    fp2,fp4
  102.     fmove.s (a1)+,fp0
  103.     fadd    fp4,fp6
  104.     fmul    fp3,fp0
  105.     fmove.s (a0)+,fp4
  106.     fadd    fp0,fp5
  107.     fmul    fp3,fp4
  108.     fadd    fp4,fp6
  109.     bra     .1
  110.  
  111. ; dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
  112. ; dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
  113. .c1:    fmove.s (a1)+,fp5
  114.     fmul    fp1,fp5
  115.     fmove.s (a0)+,fp6
  116.     fmul    fp1,fp6
  117.     fmove.s (a0)+,fp0
  118.     fmul    fp2,fp0
  119.     fmove.s (a1)+,fp4
  120.     fadd    fp0,fp5
  121.     fmul    fp2,fp4
  122.     fmove.s (a0)+,fp0
  123.     fadd    fp4,fp6
  124.     fmul    fp3,fp0
  125.     fmove.s (a1)+,fp4
  126.     fadd    fp0,fp5
  127.     fmul    fp3,fp4
  128.     fadd    fp4,fp6
  129.     bra     .1
  130.  
  131. ; dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
  132. ; dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
  133. .c2:    fmove.s (a0)+,fp5
  134.     fmul    fp1,fp5
  135.     fmove.s (a1)+,fp6
  136.     fmul    fp1,fp6
  137.     fmove.s (a1)+,fp0
  138.     fmul    fp2,fp0
  139.     fmove.s (a0)+,fp4
  140.     fadd    fp0,fp5
  141.     fmul    fp2,fp4
  142.     fmove.s (a0)+,fp0
  143.     fadd    fp4,fp6
  144.     fmul    fp3,fp0
  145.     fmove.s (a1)+,fp4
  146.     fadd    fp0,fp5
  147.     fmul    fp3,fp4
  148.     fadd    fp4,fp6
  149.     bra     .1
  150.  
  151. ; dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
  152. ; dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
  153. .c3:    fmove.s (a1)+,fp5
  154.     fmul    fp1,fp5
  155.     fmove.s (a0)+,fp6
  156.     fmul    fp1,fp6
  157.     fmove.s (a1)+,fp0
  158.     fmul    fp2,fp0
  159.     fmove.s (a0)+,fp4
  160.     fadd    fp0,fp5
  161.     fmul    fp2,fp4
  162.     fmove.s (a0)+,fp0
  163.     fadd    fp4,fp6
  164.     fmul    fp3,fp0
  165.     fmove.s (a1)+,fp4
  166.     fadd    fp0,fp5
  167.     fmul    fp3,fp4
  168.     fadd    fp4,fp6
  169.     bra     .1
  170.  
  171. ; dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
  172. ; dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
  173. .c4:    fmove.s (a0)+,fp5
  174.     fmul    fp1,fp5
  175.     fmove.s (a1)+,fp6
  176.     fmul    fp1,fp6
  177.     fmove.s (a0)+,fp0
  178.     fmul    fp2,fp0
  179.     fmove.s (a1)+,fp4
  180.     fadd    fp0,fp5
  181.     fmul    fp2,fp4
  182.     fmove.s (a1)+,fp0
  183.     fadd    fp4,fp6
  184.     fmul    fp3,fp0
  185.     fmove.s (a0)+,fp4
  186.     fadd    fp0,fp5
  187.     fmul    fp3,fp4
  188.     fadd    fp4,fp6
  189.     bra     .1
  190.  
  191. ; dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
  192. ; dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
  193. .c5:    fmove.s (a1)+,fp5
  194.     fmul    fp1,fp5
  195.     fmove.s (a0)+,fp6
  196.     fmul    fp1,fp6
  197.     fmove.s (a0)+,fp0
  198.     fmul    fp2,fp0
  199.     fmove.s (a1)+,fp4
  200.     fadd    fp0,fp5
  201.     fmul    fp2,fp4
  202.     fmove.s (a1)+,fp0
  203.     fadd    fp4,fp6
  204.     fmul    fp3,fp0
  205.     fmove.s (a0)+,fp4
  206.     fadd    fp0,fp5
  207.     fmul    fp3,fp4
  208.     fadd    fp4,fp6
  209.     bra     .1
  210.  
  211. ; dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
  212. ; dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
  213. .c6:    fmove.s (a0)+,fp5
  214.     fmul    fp1,fp5
  215.     fmove.s (a1)+,fp6
  216.     fmul    fp1,fp6
  217.     fmove.s (a1)+,fp0
  218.     fmul    fp2,fp0
  219.     fmove.s (a0)+,fp4
  220.     fadd    fp0,fp5
  221.     fmul    fp2,fp4
  222.     fmove.s (a1)+,fp0
  223.     fadd    fp4,fp6
  224.     fmul    fp3,fp0
  225.     fmove.s (a0)+,fp4
  226.     fadd    fp0,fp5
  227.     fmul    fp3,fp4
  228.     fadd    fp4,fp6
  229.     bra     .1
  230.  
  231. ; dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
  232. ; dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
  233. .c7:    fmove.s (a1)+,fp5
  234.     fmul    fp1,fp5
  235.     fmove.s (a0)+,fp6
  236.     fmul    fp1,fp6
  237.     fmove.s (a1)+,fp0
  238.     fmul    fp2,fp0
  239.     fmove.s (a0)+,fp4
  240.     fadd    fp0,fp5
  241.     fmul    fp2,fp4
  242.     fmove.s (a1)+,fp0
  243.     fadd    fp4,fp6
  244.     fmul    fp3,fp0
  245.     fmove.s (a0)+,fp4
  246.     fadd    fp0,fp5
  247.     fmul    fp3,fp4
  248.     fadd    fp4,fp6
  249.  
  250. .1:
  251. ; if (dist1 >= p->dist) sides = 1;
  252.     fmove.s dist(a2),fp0
  253.     moveq   #0,d0                   ; sides=0
  254.     fcmp    fp0,fp5
  255.     fblt    .2
  256.     moveq   #1,d0                   ; sides=1
  257. .2:
  258. ; if (dist2 < p->dist)  sides |= 2;
  259.     fcmp    fp0,fp6
  260.     fbge    .3
  261.     addq.b  #2,d0                   ; sides|=2
  262.  
  263. .3:     fmovem.x (sp)+,fp2-fp6
  264.     move.l  d1,a2
  265.     rts
  266.  
  267.  
  268.     xdef    _AngleVectors
  269.     xdef    _AngleVectors__r
  270.     cnop    0,4
  271. _AngleVectors:
  272. _AngleVectors__r:
  273.     fmovem.x fp2-fp7,-(sp)
  274.     move.l  a2,-(sp)
  275.     setso   4+6*12+1*4
  276. .angles so.l    1
  277. .forward so.l   1
  278. .right  so.l    1
  279. .up     so.l    1
  280.  
  281.     move.l  .angles(sp),a2
  282.     fmove.s #0.01745329252,fp5      ; M_PI*2 / 360
  283.  
  284. ; angle = angles[PITCH] * (M_PI*2 / 360);
  285. ; sp = sin(angle);
  286. ; cp = cos(angle);
  287.     fmove   fp5,fp2
  288.     fmul.s  (a2)+,fp2
  289.     IFD     M881
  290.     fsincos fp2,fp6:fp3
  291.     ELSE
  292.     fmove.d fp2,-(sp)
  293.     IFD    STORMC
  294.     jsr     _sin__r
  295.     ELSE
  296.     jsr    _sin
  297.     ENDC
  298.     fmove   fp0,fp3                 ; sp
  299.     IFD    STORMC
  300.     jsr     _cos__r
  301.     ELSE
  302.     jsr    _cos
  303.     ENDC
  304.     fmove   fp0,fp6                 ; cp
  305.     ENDC
  306.  
  307. ; angle = angles[YAW] * (M_PI*2 / 360);
  308. ; sy = sin(angle);
  309. ; cy = cos(angle);
  310.     fmove   fp5,fp2
  311.     fmul.s  (a2)+,fp2
  312.     IFD     M881
  313.     fsincos fp2,fp7:fp4
  314.     ELSE
  315.     fmove.d fp2,(sp)
  316.     IFD    STORMC
  317.     jsr     _sin__r
  318.     ELSE
  319.     jsr    _sin
  320.     ENDC
  321.     fmove   fp0,fp4                 ; sy
  322.     IFD    STORMC
  323.     jsr     _cos__r
  324.     ELSE
  325.     jsr    _cos
  326.     ENDC
  327.     fmove   fp0,fp7                 ; cy
  328.     ENDC
  329.  
  330. ; angle = angles[ROLL] * (M_PI*2 / 360);
  331. ; sr = sin(angle);
  332. ; cr = cos(angle);
  333.     fmove   fp5,fp2
  334.     fmul.s  (a2)+,fp2
  335.     IFD     M881
  336.     fsincos fp2,fp5:fp2
  337.     ELSE
  338.     fmove.d fp2,(sp)
  339.     IFD    STORMC
  340.     jsr     _sin__r
  341.     ELSE
  342.     jsr    _sin
  343.     ENDC
  344.     fmove   fp0,fp2                 ; sr
  345.     IFD    STORMC
  346.     jsr     _cos__r
  347.     ELSE
  348.     jsr    _cos
  349.     ENDC
  350.     fmove   fp0,fp5                 ; cr
  351.     addq.w  #8,sp
  352.     ENDC
  353.  
  354. ; forward[0] = cp*cy;
  355. ; forward[1] = cp*sy;
  356. ; forward[2] = -sp;
  357.     fmove   fp6,fp0
  358.     fmul    fp7,fp0
  359.     move.l  .forward(sp),a0
  360.     fmove.s fp0,(a0)+
  361.     fmove   fp6,fp0
  362.     fmul    fp4,fp0
  363.     fneg    fp3,fp1
  364.     fmove.s fp0,(a0)+
  365.     fmove.s fp1,(a0)+
  366.  
  367. ; right[0] = (-1*sr*sp*cy+-1*cr*-sy);
  368. ; right[1] = (-1*sr*sp*sy+-1*cr*cy);
  369. ; right[2] = -1*sr*cp;
  370.     fmove   fp4,fp0
  371.     fmul    fp5,fp0
  372.     fmove   fp2,fp1
  373.     fmul    fp3,fp1
  374.     fmove.d fp7,-(sp)
  375.     fmul    fp1,fp7
  376.     move.l  .right+8(sp),a0
  377.     fsub    fp7,fp0
  378.     fmove.s fp0,(a0)+
  379.     fmul    fp4,fp1
  380.     fmove.d (sp)+,fp7
  381.     fneg    fp5,fp0
  382.     fmul    fp7,fp0
  383.     fsub    fp1,fp0
  384.     fmove.s fp0,(a0)+
  385.     fneg    fp2,fp1
  386.     fmul    fp6,fp1
  387.     fmove.s fp1,(a0)+
  388.  
  389. ; up[0] = (cr*sp*cy+-sr*-sy);
  390. ; up[1] = (cr*sp*sy+-sr*cy);
  391. ; up[2] = cr*cp;
  392.     fmul    fp5,fp3
  393.     fneg    fp4,fp1
  394.     fmove   fp7,fp0
  395.     fmul    fp2,fp1
  396.     fmul    fp3,fp0
  397.     move.l  .up(sp),a0
  398.     fsub    fp1,fp0
  399.     fmul    fp7,fp2
  400.     fmove.s fp0,(a0)+
  401.     fmul    fp4,fp3
  402.     move.l  (sp)+,a2
  403.     fsub    fp2,fp3
  404.     fmove.s fp3,(a0)+
  405.     fmul    fp6,fp5
  406.     fmove.s fp5,(a0)+
  407.     fmovem.x (sp)+,fp2-fp7
  408.     rts
  409.  
  410.  
  411.     xdef    _VectorCompare
  412.     xdef    _VectorCompare__r
  413.     cnop    0,4
  414. _VectorCompare:
  415. _VectorCompare__r:
  416.     setso   4
  417. .v1     so.l    1
  418. .v2     so.l    1
  419.  
  420.     move.l  .v1(sp),a0
  421.     fmove.s (a0)+,fp0
  422.     move.l  .v2(sp),a1
  423.     fcmp.s  (a1)+,fp0
  424.     fbne    .1
  425.     fmove.s (a0)+,fp0
  426.     fcmp.s  (a1)+,fp0
  427.     fbne    .1
  428.     fmove.s (a0),fp0
  429.     fcmp.s  (a1),fp0
  430.     fbne    .1
  431.     moveq   #1,d0
  432.     rts
  433. .1:     moveq   #0,d0
  434.     rts
  435.  
  436.     xdef    _VectorMA
  437.     xdef    _VectorMA__r
  438.     cnop    0,4
  439. _VectorMA:
  440. _VectorMA__r:
  441.     setso   4
  442. .veca   so.l    1
  443. .scale  so.s    1
  444. .vecb   so.l    1
  445. .vecc   so.l    1
  446.  
  447.     move.l  a2,d1
  448.     move.l  .vecb(sp),a1
  449.     fmove.s (a1)+,fp0
  450.     fmove.s .scale(sp),fp1
  451.     fmul    fp1,fp0
  452.     move.l  .veca(sp),a0
  453.     fadd.s  (a0)+,fp0
  454.     move.l  .vecc(sp),a2
  455.     fmove.s fp0,(a2)+
  456.     fmove.s (a1)+,fp0
  457.     fmul    fp1,fp0
  458.     fadd.s  (a0)+,fp0
  459.     fmul.s  (a1),fp1
  460.     fmove.s fp0,(a2)+
  461.     fadd.s  (a0),fp1
  462.     fmove.s fp1,(a2)
  463.     move.l  d1,a2
  464.     rts
  465.  
  466.     xdef    __DotProduct
  467.     xdef    __DotProduct__r
  468.     cnop    0,4
  469. __DotProduct:
  470. __DotProduct__r:
  471.     setso   4
  472. .v1     so.l    1
  473. .v2     so.l    1
  474.  
  475.     move.l  .v1(sp),a0
  476.     fmove.s (a0)+,fp0
  477.     move.l  .v2(sp),a1
  478.     fmul.s  (a1)+,fp0
  479.     fmove.s (a0)+,fp1
  480.     fmul.s  (a1)+,fp1
  481.     fadd    fp1,fp0
  482.     fmove.s (a0),fp1
  483.     fmul.s  (a1)+,fp1
  484.     fadd    fp1,fp0
  485.     rts
  486.  
  487.     xdef    __VectorSubtract
  488.     xdef    __VectorSubtract__r
  489.     cnop    0,4
  490. __VectorSubtract:
  491. __VectorSubtract__r:
  492.     setso   4
  493. .veca   so.l    1
  494. .vecb   so.l    1
  495. .vecc   so.l    1
  496.  
  497.     move.l  .veca(sp),a0
  498.     fmove.s (a0)+,fp0
  499.     move.l  .vecb(sp),a1
  500.     fsub.s  (a1)+,fp0
  501.     move.l  a2,d1
  502.     fmove.s (a0)+,fp1
  503.     move.l  .vecc(sp),a2
  504.     fmove.s fp0,(a2)+
  505.     fsub.s  (a1)+,fp1
  506.     fmove.s (a0),fp0
  507.     fmove.s fp1,(a2)+
  508.     fsub.s  (a1),fp0
  509.     fmove.s fp0,(a2)
  510.     move.l  d1,a2
  511.     rts
  512.  
  513.  
  514.     xdef    __VectorAdd
  515.     xdef    __VectorAdd__r
  516.     cnop    0,4
  517. __VectorAdd:
  518. __VectorAdd__r:
  519.     setso   4
  520. .veca   so.l    1
  521. .vecb   so.l    1
  522. .vecc   so.l    1
  523.  
  524.     move.l  .veca(sp),a0
  525.     fmove.s (a0)+,fp0
  526.     move.l  .vecb(sp),a1
  527.     fadd.s  (a1)+,fp0
  528.     move.l  a2,d1
  529.     fmove.s (a0)+,fp1
  530.     move.l  .vecc(sp),a2
  531.     fmove.s fp0,(a2)+
  532.     fadd.s  (a1)+,fp1
  533.     fmove.s (a0),fp0
  534.     fmove.s fp1,(a2)+
  535.     fadd.s  (a1),fp0
  536.     fmove.s fp0,(a2)
  537.     move.l  d1,a2
  538.     rts
  539.  
  540.  
  541.     xdef    __VectorCopy
  542.     xdef    __VectorCopy__r
  543.     cnop    0,4
  544. __VectorCopy:
  545. __VectorCopy__r:
  546.     setso   4
  547. .in     so.l    1
  548. .out    so.l    1
  549.  
  550.     move.l  .in(sp),a0
  551.     move.l  .out(sp),a1
  552.     move.l  (a0)+,(a1)+
  553.     move.l  (a0)+,(a1)+
  554.     move.l  (a0),(a1)
  555.     rts
  556.  
  557.  
  558.     xdef    _CrossProduct
  559.     xdef    _CrossProduct__r
  560.     cnop    0,4
  561. _CrossProduct:
  562. _CrossProduct__r:
  563.     fmovem.x fp2-fp5,-(sp)
  564.     setso   4+4*12
  565. .v1     so.l    1
  566. .v2     so.l    1
  567. .cross  so.l    1
  568.  
  569. ; cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
  570. ; cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
  571. ; cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
  572.     move.l  a2,d1
  573.     move.l  .v1(sp),a1
  574.     fmove.s (a1)+,fp0
  575.     move.l  .v2(sp),a2
  576.     fmove.s (a2)+,fp1
  577.     fmove.s (a1)+,fp2
  578.     fmove   fp1,fp4
  579.     fmove.s (a2)+,fp3
  580.     fmul    fp2,fp4
  581.     fmove   fp0,fp5
  582.     move.l  .cross(sp),a0
  583.     fmul    fp3,fp5
  584.     add.w   #12,a0
  585.     fsub    fp4,fp5
  586.     fmove.s (a1),fp4
  587.     fmove.s fp5,-(a0)
  588.     fmul    fp4,fp1
  589.     fmove.s (a2),fp5
  590.     fmul    fp5,fp0
  591.     move.l  d1,a2
  592.     fmul    fp4,fp3
  593.     fsub    fp0,fp1
  594.     fmul    fp5,fp2
  595.     fmove.s fp1,-(a0)
  596.     fsub    fp3,fp2
  597.     fmove.s fp2,-(a0)
  598.     fmovem.x (sp)+,fp2-fp5
  599.     rts
  600.  
  601.  
  602.     xdef    _Length
  603.     xdef    _Length__r
  604.     cnop    0,4
  605. _Length:
  606. _Length__r:
  607.     setso   4
  608. .v      so.l    1
  609.  
  610. ; length = 0;
  611. ; for (i=0 ; i< 3 ; i++)
  612. ;   length += v[i]*v[i];
  613. ; length = sqrt (length);
  614.     move.l  .v(sp),a0
  615.     fmove.s (a0)+,fp0
  616.     fmul    fp0,fp0
  617.     fmove.s (a0)+,fp1
  618.     fmul    fp1,fp1
  619.     fadd    fp1,fp0
  620.     fmove.s (a0),fp1
  621.     fmul    fp1,fp1
  622.     fadd    fp1,fp0
  623.     fsqrt   fp0
  624.     rts
  625.  
  626.  
  627.     xdef    _VectorNormalize
  628.     xdef    _VectorNormalize__r
  629.     cnop    0,4
  630. _VectorNormalize:
  631. _VectorNormalize__r:
  632.     fmovem.x fp2-fp4,-(sp)
  633.     setso   4+3*12
  634. .v      so.l    1
  635.  
  636. ; length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
  637. ; length = sqrt (length);
  638.     move.l  .v(sp),a0
  639.     fmove.s (a0)+,fp2
  640.     fmove   fp2,fp0
  641.     fmul    fp2,fp0
  642.     fmove.s (a0)+,fp3
  643.     fmove   fp3,fp1
  644.     fmul    fp3,fp1
  645.     fmove.s (a0),fp4
  646.     fadd    fp1,fp0
  647.     fmove   fp4,fp1
  648.     fmul    fp4,fp1
  649.     fadd    fp1,fp0
  650.     fsqrt   fp0
  651.  
  652. ; if (length)
  653. ; {
  654. ;   ilength = 1/length;
  655. ;   v[0] *= ilength;
  656. ;   v[1] *= ilength;
  657. ;   v[2] *= ilength;
  658. ; }
  659.     ftst    fp0
  660.     fbeq    .1
  661.     fmove.s #1.0,fp1
  662.     fdiv    fp0,fp1
  663.     subq.w  #8,a0
  664.     fmul    fp1,fp2
  665.     fmove.s fp2,(a0)+
  666.     fmul    fp1,fp3
  667.     fmove.s fp3,(a0)+
  668.     fmul    fp1,fp4
  669.     fmove.s fp4,(a0)
  670. .1:     fmovem.x (sp)+,fp2-fp4
  671.     rts
  672.  
  673.  
  674.     xdef    _VectorInverse
  675.     xdef    _VectorInverse__r
  676.     cnop    0,4
  677. _VectorInverse:
  678. _VectorInverse__r:
  679.     setso   4
  680. .v      so.l    1
  681.  
  682. ; v[0] = -v[0];
  683. ; v[1] = -v[1];
  684. ; v[2] = -v[2];
  685.     move.l  .v(sp),a0
  686.     fneg.s  (a0),fp0
  687.     fmove.s fp0,(a0)+
  688.     fneg.s  (a0),fp0
  689.     fmove.s fp0,(a0)+
  690.     fneg.s  (a0),fp0
  691.     fmove.s fp0,(a0)
  692.     rts
  693.  
  694.  
  695.     xdef    _VectorScale
  696.     xdef    _VectorScale__r
  697.     cnop    0,4
  698. _VectorScale:
  699. _VectorScale__r:
  700.     setso   4
  701. .in     so.l    1
  702. .scale  so.l    1
  703. .out    so.l    1
  704.  
  705. ; out[0] = in[0]*scale;
  706. ; out[1] = in[1]*scale;
  707. ; out[2] = in[2]*scale;
  708.     fmove.s .scale(sp),fp1
  709.     move.l  .in(sp),a0
  710.     fmove.s (a0)+,fp0
  711.     fmul    fp1,fp0
  712.     move.l  .out(sp),a1
  713.     fmove.s fp0,(a1)+
  714.     fmove.s (a0)+,fp0
  715.     fmul    fp1,fp0
  716.     fmove.s fp0,(a1)+
  717.     fmul.s  (a0),fp1
  718.     fmove.s fp1,(a1)
  719.     rts
  720.  
  721.  
  722.     xdef    _Q_log2
  723.     xdef    _Q_log2__r
  724.     cnop    0,4
  725. _Q_log2:
  726. _Q_log2__r:
  727.     setso   4
  728. .val    so.l    1
  729.  
  730. ; int answer=0;
  731. ; while (val>>=1) answer++;
  732.     moveq   #-1,d0
  733. .1:     addq.l  #1,d0
  734.     asr.l   #1,d0
  735.     bne.b   .1
  736.     rts
  737.  
  738.     xdef    _R_ConcatRotations
  739.     xdef    _R_ConcatRotations__r
  740.     cnop    0,4
  741. _R_ConcatRotations:
  742. _R_ConcatRotations__r:
  743.     fmovem.x fp2/fp4/fp5/fp6,-(sp)
  744.     move.l  a2,d1
  745.     setso   4+4*12
  746. .in1    so.l    1
  747. .in2    so.l    1
  748. .out    so.l    1
  749.  
  750.     move.l  .in1(sp),a0
  751.     move.l  .in2(sp),a1
  752.     move.l  .out(sp),a2
  753.  
  754. ; out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
  755. ; out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
  756. ; out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
  757.     fmove.s (a0)+,fp4
  758.     fmove   fp4,fp0
  759.     fmul.s  (0*12+0*4,a1),fp0
  760.     fmove.s (a0)+,fp5
  761.     fmove   fp5,fp1
  762.     fmul.s  (1*12+0*4,a1),fp1
  763.     fmove.s (a0)+,fp6
  764.     fmove   fp6,fp2
  765.     fmul.s  (2*12+0*4,a1),fp2
  766.     fadd    fp0,fp1
  767.     fmove   fp4,fp0
  768.     fmul.s  (0*12+1*4,a1),fp0
  769.     fadd    fp1,fp2
  770.     fmove   fp5,fp1
  771.     fmul.s  (1*12+1*4,a1),fp1
  772.     fmove.s fp2,(a2)+
  773.     fmove   fp6,fp2
  774.     fmul.s  (2*12+1*4,a1),fp2
  775.     fadd    fp0,fp1
  776.     fmul.s  (0*12+2*4,a1),fp4
  777.     fadd    fp1,fp2
  778.     fmul.s  (1*12+2*4,a1),fp5
  779.     fmove.s fp2,(a2)+
  780.     fadd    fp4,fp5
  781.     fmul.s  (2*12+2*4,a1),fp6
  782.     fadd    fp5,fp6
  783.     fmove.s fp6,(a2)+
  784.  
  785. ; out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
  786. ; out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
  787. ; out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
  788.     fmove.s (a0)+,fp4
  789.     fmove   fp4,fp0
  790.     fmul.s  (0*12+0*4,a1),fp0
  791.     fmove.s (a0)+,fp5
  792.     fmove   fp5,fp1
  793.     fmul.s  (1*12+0*4,a1),fp1
  794.     fmove.s (a0)+,fp6
  795.     fmove   fp6,fp2
  796.     fmul.s  (2*12+0*4,a1),fp2
  797.     fadd    fp0,fp1
  798.     fmove   fp4,fp0
  799.     fmul.s  (0*12+1*4,a1),fp0
  800.     fadd    fp1,fp2
  801.     fmove   fp5,fp1
  802.     fmul.s  (1*12+1*4,a1),fp1
  803.     fmove.s fp2,(a2)+
  804.     fmove   fp6,fp2
  805.     fmul.s  (2*12+1*4,a1),fp2
  806.     fadd    fp0,fp1
  807.     fmul.s  (0*12+2*4,a1),fp4
  808.     fadd    fp1,fp2
  809.     fmul.s  (1*12+2*4,a1),fp5
  810.     fmove.s fp2,(a2)+
  811.     fadd    fp4,fp5
  812.     fmul.s  (2*12+2*4,a1),fp6
  813.     fadd    fp5,fp6
  814.     fmove.s fp6,(a2)+
  815.  
  816. ; out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
  817. ; out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
  818. ; out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
  819.     fmove.s (a0)+,fp4
  820.     fmove   fp4,fp0
  821.     fmul.s  (0*12+0*4,a1),fp0
  822.     fmove.s (a0)+,fp5
  823.     fmove   fp5,fp1
  824.     fmul.s  (1*12+0*4,a1),fp1
  825.     fmove.s (a0)+,fp6
  826.     fmove   fp6,fp2
  827.     fmul.s  (2*12+0*4,a1),fp2
  828.     fadd    fp0,fp1
  829.     fmove   fp4,fp0
  830.     fmul.s  (0*12+1*4,a1),fp0
  831.     fadd    fp1,fp2
  832.     fmove   fp5,fp1
  833.     fmul.s  (1*12+1*4,a1),fp1
  834.     fmove.s fp2,(a2)+
  835.     fmove   fp6,fp2
  836.     fmul.s  (2*12+1*4,a1),fp2
  837.     fadd    fp0,fp1
  838.     fmul.s  (0*12+2*4,a1),fp4
  839.     fadd    fp1,fp2
  840.     fmul.s  (1*12+2*4,a1),fp5
  841.     fmove.s fp2,(a2)+
  842.     fadd    fp4,fp5
  843.     fmul.s  (2*12+2*4,a1),fp6
  844.     fadd    fp5,fp6
  845.     fmove.s fp6,(a2)+
  846.  
  847.     move.l  d1,a2
  848.     fmovem.x (sp)+,fp2/fp4/fp5/fp6
  849.     rts
  850.  
  851.  
  852.     xdef    _R_ConcatTransforms
  853.     xdef    _R_ConcatTransforms__r
  854.     cnop    0,4
  855. _R_ConcatTransforms:
  856. _R_ConcatTransforms__r:
  857.     fmovem.x fp2/fp4/fp5/fp6,-(sp)
  858.     move.l  a2,d1
  859.     setso   4+4*12
  860. .in1    so.l    1
  861. .in2    so.l    1
  862. .out    so.l    1
  863.  
  864.     move.l  .in1(sp),a0
  865.     move.l  .in2(sp),a1
  866.     move.l  .out(sp),a2
  867.  
  868. ; out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
  869. ; out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
  870. ; out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
  871. ; out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] + in1[0][2] * in2[2][3] + in1[0][3];
  872.     fmove.s (a0)+,fp4
  873.     fmove   fp4,fp0
  874.     fmul.s  (0*16+0*4,a1),fp0
  875.     fmove.s (a0)+,fp5
  876.     fmove   fp5,fp1
  877.     fmul.s  (1*16+0*4,a1),fp1
  878.     fmove.s (a0)+,fp6
  879.     fmove   fp6,fp2
  880.     fmul.s  (2*16+0*4,a1),fp2
  881.     fadd    fp0,fp1
  882.     fmove   fp4,fp0
  883.     fmul.s  (0*16+1*4,a1),fp0
  884.     fadd    fp1,fp2
  885.     fmove   fp5,fp1
  886.     fmul.s  (1*16+1*4,a1),fp1
  887.     fmove.s fp2,(a2)+
  888.     fmove   fp6,fp2
  889.     fmul.s  (2*16+1*4,a1),fp2
  890.     fadd    fp0,fp1
  891.     fmove   fp4,fp0
  892.     fmul.s  (0*16+2*4,a1),fp0
  893.     fadd    fp1,fp2
  894.     fmove   fp5,fp1
  895.     fmul.s  (1*16+2*4,a1),fp1
  896.     fmove.s fp2,(a2)+
  897.     fmove   fp6,fp2
  898.     fmul.s  (2*16+2*4,a1),fp2
  899.     fadd    fp0,fp1
  900.     fmul.s  (0*16+3*4,a1),fp4
  901.     fadd    fp1,fp2
  902.     fmul.s  (1*16+3*4,a1),fp5
  903.     fmove.s fp2,(a2)+
  904.     fadd    fp4,fp5
  905.     fmul.s  (2*16+3*4,a1),fp6
  906.     fadd.s  (a0)+,fp5
  907.     fadd    fp5,fp6
  908.     fmove.s fp6,(a2)+
  909.  
  910. ; out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
  911. ; out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
  912. ; out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
  913. ; out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] + in1[1][2] * in2[2][3] + in1[1][3];
  914.     fmove.s (a0)+,fp4
  915.     fmove   fp4,fp0
  916.     fmul.s  (0*16+0*4,a1),fp0
  917.     fmove.s (a0)+,fp5
  918.     fmove   fp5,fp1
  919.     fmul.s  (1*16+0*4,a1),fp1
  920.     fmove.s (a0)+,fp6
  921.     fmove   fp6,fp2
  922.     fmul.s  (2*16+0*4,a1),fp2
  923.     fadd    fp0,fp1
  924.     fmove   fp4,fp0
  925.     fmul.s  (0*16+1*4,a1),fp0
  926.     fadd    fp1,fp2
  927.     fmove   fp5,fp1
  928.     fmul.s  (1*16+1*4,a1),fp1
  929.     fmove.s fp2,(a2)+
  930.     fmove   fp6,fp2
  931.     fmul.s  (2*16+1*4,a1),fp2
  932.     fadd    fp0,fp1
  933.     fmove   fp4,fp0
  934.     fmul.s  (0*16+2*4,a1),fp0
  935.     fadd    fp1,fp2
  936.     fmove   fp5,fp1
  937.     fmul.s  (1*16+2*4,a1),fp1
  938.     fmove.s fp2,(a2)+
  939.     fmove   fp6,fp2
  940.     fmul.s  (2*16+2*4,a1),fp2
  941.     fadd    fp0,fp1
  942.     fmul.s  (0*16+3*4,a1),fp4
  943.     fadd    fp1,fp2
  944.     fmul.s  (1*16+3*4,a1),fp5
  945.     fmove.s fp2,(a2)+
  946.     fadd    fp4,fp5
  947.     fmul.s  (2*16+3*4,a1),fp6
  948.     fadd.s  (a0)+,fp5
  949.     fadd    fp5,fp6
  950.     fmove.s fp6,(a2)+
  951.  
  952. ; out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
  953. ; out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
  954. ; out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
  955. ; out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + in1[2][2] * in2[2][3] + in1[2][3];
  956.     fmove.s (a0)+,fp4
  957.     fmove   fp4,fp0
  958.     fmul.s  (0*16+0*4,a1),fp0
  959.     fmove.s (a0)+,fp5
  960.     fmove   fp5,fp1
  961.     fmul.s  (1*16+0*4,a1),fp1
  962.     fmove.s (a0)+,fp6
  963.     fmove   fp6,fp2
  964.     fmul.s  (2*16+0*4,a1),fp2
  965.     fadd    fp0,fp1
  966.     fmove   fp4,fp0
  967.     fmul.s  (0*16+1*4,a1),fp0
  968.     fadd    fp1,fp2
  969.     fmove   fp5,fp1
  970.     fmul.s  (1*16+1*4,a1),fp1
  971.     fmove.s fp2,(a2)+
  972.     fmove   fp6,fp2
  973.     fmul.s  (2*16+1*4,a1),fp2
  974.     fadd    fp0,fp1
  975.     fmove   fp4,fp0
  976.     fmul.s  (0*16+2*4,a1),fp0
  977.     fadd    fp1,fp2
  978.     fmove   fp5,fp1
  979.     fmul.s  (1*16+2*4,a1),fp1
  980.     fmove.s fp2,(a2)+
  981.     fmove   fp6,fp2
  982.     fmul.s  (2*16+2*4,a1),fp2
  983.     fadd    fp0,fp1
  984.     fmul.s  (0*16+3*4,a1),fp4
  985.     fadd    fp1,fp2
  986.     fmul.s  (1*16+3*4,a1),fp5
  987.     fmove.s fp2,(a2)+
  988.     fadd    fp4,fp5
  989.     fmul.s  (2*16+3*4,a1),fp6
  990.     fadd.s  (a0)+,fp5
  991.     fadd    fp5,fp6
  992.     fmove.s fp6,(a2)+
  993.  
  994.     move.l  d1,a2
  995.     fmovem.x (sp)+,fp2/fp4/fp5/fp6
  996.     rts
  997.  
  998.  
  999.     xdef    _FloorDivMod
  1000.     xdef    _FloorDivMod__r
  1001.     cnop    0,4
  1002. _FloorDivMod:
  1003. _FloorDivMod__r:
  1004.     setso   4+4
  1005. .numer  so.d    1
  1006. .denom  so.d    1
  1007. .quot   so.l    1
  1008. .rem    so.l    1
  1009.  
  1010. ; set rounding mode towards minus infinity
  1011.     fmove.l fpcr,d0
  1012.     move.l  d0,-(sp)
  1013.     moveq   #%10,d1
  1014.     bfins   d1,d0{26:2}
  1015.     fmove.l d0,fpcr
  1016.  
  1017. ; if (numer >= 0.0)
  1018.     fmove.d .numer(sp),fp0
  1019.     fmove.d .denom(sp),fp1
  1020.     ftst    fp0
  1021.     fblt    .1
  1022.  
  1023. ; x = floor(numer / denom);
  1024. ; q = (int)x;
  1025. ; r = (int)floor(numer - (x * denom));
  1026.     fdiv    fp1,fp0
  1027.     move.l  .quot(sp),a0
  1028.     move.l  .rem(sp),a1
  1029.     fmove.l fp0,(a0)
  1030.     fmul.l  (a0),fp1
  1031.     fmove.d .numer(sp),fp0
  1032.     fsub    fp1,fp0
  1033.     fmove.l fp0,(a1)
  1034.     fmove.l (sp)+,fpcr
  1035.     rts
  1036.  
  1037. ; else /* numer < 0.0) */
  1038. .1:
  1039. ; x = floor(-numer / denom);
  1040. ; q = -(int)x;
  1041. ; r = (int)floor(-numer - (x * denom));
  1042.     fmovem.x fp2/fp3,-(sp)
  1043.     fneg    fp0
  1044.     fmove   fp0,fp2
  1045.     fdiv    fp1,fp0
  1046.     move.l  .quot+2*12(sp),a0
  1047.     move.l  .rem+2*12(sp),a1
  1048.     fmove.l fp0,d0
  1049.     fmove   fp1,fp3
  1050.     fmul.l  d0,fp1
  1051.     neg.l   d0
  1052.     fsub    fp1,fp2
  1053.     move.l  d0,(a0)
  1054.     fmove.l fp2,d0
  1055.     move.l  d0,d1
  1056.     beq.b   .2
  1057.     fmove.l fp3,d1
  1058.     subq.l  #1,(a0)
  1059.     sub.l   d0,d1
  1060. .2:     move.l  d1,(a1)
  1061.     fmovem.x (sp)+,fp2/fp3
  1062.     fmove.l (sp)+,fpcr
  1063.     rts
  1064.     
  1065.  
  1066.     xdef    _GreatestCommonDivisor
  1067.     xdef    _GreatestCommonDivisor__r
  1068.     cnop    0,4
  1069. _GreatestCommonDivisor:
  1070. _GreatestCommonDivisor__r:
  1071.     setso   4
  1072. .i1     so.l    1
  1073. .i2     so.l    1
  1074.  
  1075.     move.l  .i1(sp),d1
  1076.     move.l  d2,a0
  1077.     move.l  .i2(sp),d0
  1078.     bra.b   .2
  1079. .1:     move.l  d0,d2
  1080.     divsl.l d1,d0:d2
  1081. .2:     cmp.l   d1,d0
  1082.     bge.b   .3
  1083.     exg     d0,d1
  1084. .3:     tst.l   d1
  1085.     bne.b   .1
  1086.     move.l  a0,d2
  1087.     rts
  1088.  
  1089.  
  1090.     xdef    _Invert24To16
  1091.     xdef    _Invert24To16__r
  1092.     cnop    0,4
  1093. _Invert24To16:
  1094. _Invert24To16__r:
  1095.     move.l  4(sp),d0
  1096.     cmp.l   #256,d0
  1097.     blt.b   .1
  1098.     fmove.d #$4270000000000000,fp0
  1099.     fdiv.l  d0,fp0
  1100.     fadd.d  #0.5,fp0
  1101.     fmove.l fp0,d0
  1102.     rts
  1103. .1:     moveq   #-1,d0
  1104.     rts
  1105.