NAM    MATH ROUTINES - 7/19/76
       OPT O
       OPT S
             ORG    $0600
****
*
*       PROGRAM NAME:    FIX
*
*
*       FUNCTION:        TO CHANGE A FLOATING POINT
*                        VALUE TO INTEGER.
*
*       CALLING SEQUENCE:
*
*           JSR    FIX
*           FDB    ARG1     ADDRESS OF ARG1
*           FDB    ARG2     ADDRESS OF ARG2
*           ---             RETURN LOCATION
*
*       WHERE:
*           ARG1 = ADDRESS OF FLOATING POINT VALUE
*                  BYTE 0   BIT 7   = SIGN OF FRACTION
*                           BIT 6-0 = EXPONENT (2'S COMP)
*                  BYTE 1-3         = FRACTION
*
*           ARG2 = ADDRESS OF RETURNED INTEGER VALUE
*                  BYTE 0  =  MSB
*                  BYTE 1  =  LSB
*            IF FLOATING POINT VALUE GREATER THAN 32767,
*             RESULTS UNPREDICTABLE.
*
*       PROGRAMMER:      D. W. CAWELTI
*
*       STACK SIZE:      9 BYTES
*
*       STACK DESCRIPTION:
*           BYTE 0  = MULTIPLER SIGN (BIT 7)
*                1  = MULTIPLER  (FLOATING POINT)
*                2  = INTEGER MSB
*                3  = INTEGER LSB
*                4  = *
*                5  = FRACTION (FLOATING POINT)
*                6  = *
*                7  = RETURN LOCATION MSB
*                8  =    "       "    LSB
*
*       RV    DATE    BY   REASON FOR CHANGE
*       --  --------  ---  ------------------
*       00  06/08/76  DWC  INITIAL RELEASE
*
****
*
FIX    TSX  GET LOC OF STACK IN X
       LDX 0,X  GET ADDR. OF CALLING SEQ.
       LDX 0,X  GET ADDR. OF ARG1
       LDA A 3,X  GET LSB OF FRACTION
       PSH A
       LDA A 2,X
       PSH A
       LDA A  1,X  GET MSB OF FRACTION
       PSH A
       CLR A  CLEAR 2 BYTE INTEGER VALUE
       PSH A
       PSH A
       LDA A 0,X  GET EXPONENT
       AND A #$7F  AND OUT EXPONENT SIGN
       PSH A
       LDA A 0,X  GET EXPONENT AGAIN AND SAVE
       PSH A    FOR SIGN.
       TSX  GET NEW LOC. OF STACK
*
FIX1   NOP
       LDA A 1,X  CHECK IF MULTIPLER LT 1
       AND A #$40
       BNE   FIX6
       LDA A 1,X  GET MULTIPLER AND CHECK FOR
       BEQ  FIX4  ZERO.
       LDA A #4  SHIFT LEFT 4 BITS
FIX2   ASL 6,X  FLOATING POINT FRACTION
       ROL 5,X  INTO INTEGER VALUE.
       ROL 4,X
       ROL 3,X
       ROL 2,X
       DEC A
       BNE FIX2
       DEC 1,X
       BRA FIX1
FIX4   NOP
       LDA A 4,X GET FRACTION - CHECK IF GT
       BPL FIX5  .5 (ROUND OFF)
       INC 3,X  GREATER THAN .5 - ADD ONE
       BNE FIX5  TO INTEGER
       INC 2,X
FIX5   NOP
       LDA A 0,X GET MULTIPLER SIGN
       BPL FIX6  CHECK IF NEG.
       LDA A 3,X  YES - COMPLIMENT INTEGER
       COM A
       LDA B 2,X
       COM B
       STA A 3,X
       STA B 2,X
       INC 3,X
       BNE FIX6
       INC 2,X
FIX6   NOP
       LDX 7,X  USE STACK POINTER TO GET TO
       LDX 2,X  ADDR OF RESULT.
       PUL A
       PUL A
       PUL A
       STA A 0,X  STORE MS BYTE OF INTEGER
       PUL A
       STA A 1,X
       PUL A  CLEAN STACK
       PUL A
       PUL A
       TSX  GET LOC OF STACK IN X
       LDX 0,X  GET RETURN LOCATION
       INS
       INS
       JMP 4,X  RETURN TO CALLER.
       PAGE
****
*
*       PROGRAM NAME:    FLOAT
*
*       FUNCTION:        TO CHANGE A INTEGER VALUE TO
*                        FLOATING POINT.
*
*       CALLING SEQUENCE:
*
*           JSR    FLOAT
*           FDB    ARG1     ADDRESS OF ARG1
*           FDB    ARG2     ADDRESS OF ARG2
*           ---             RETURN LOCATION
*
*       WHERE:
*
*          ARG1 = ADDRESS OF INTEGER VALUE(16 BIT 2'S COMP)
*                 BYTE 0  =  MS BYTE
*                 BYTE 1  =  LS BYTE
*
*          ARG2 = ADDRESS OF RETURNED FLOATING POINT NO
*                 BYTE 0   BIT 7  = SIGN OF FRACTION
*                          BIT 6-0 = EXPONENT (2'S COMP)
*                 BYTE 1-3         = FRACTION
*
*       PROGRAMMER:      D. W. CAWELTI
*
*       STACK SIZE:      9 BYTES
*
*       STACK DESCRIPTION:
*           BYTE 0  = INTEGER SIGN (BIT 7)
*                1  = MULTIPLER (FLOATING POINT)
*                2  = INTEGER MS BYTE
*                3  = INTEGER LS BYTE
*                4  = *
*                5  = FRACTION (FLOATING POINT)
*                6  = *
*                7  = RETURN LOCATION  MS BYTE
*                8  = RETURN LOCATION  LS BYTE
*
*       RV    DATE    BY   REASON FOR CHANGE
*       --  --------  ---  -------------------------------
*       00  06/08/76  DWC  INITIAL RELEASE
*
***
*
FLOAT  TSX  GET LOC OF STACK IN X
       LDX 0,X  GET ADDR. OF CALLING SEQ.
       LDX 0,X  GET ADDR OF ARG1
       CLR A
       PSH A  ZERO 3 BYTES IN STACK FOR
       PSH A   FRACTION
       PSH A
       LDA A 1,X  GET INTEGER VALUE
       PSH A   BYTE 1 (LS BYTE)
       LDA A 0,X  GET INTEGER VALUE
       PSH A  
       CLR B
       PSH B  ZERO FLOATING POINT MULTIPLER
       PSH A  SAVE INTEGER VALUE SIGN BIT 7
       TSX  GET NEW LOC. OF STACK
*
       LDA A 0,X  CHECK IF SIGN NEG.
       BPL FLOAT1
       LDA A 3,X  YES - COMPLEMENT AND ADD 1 TO
       COM A  MAKE 2' COMPLEMENT POSITIVE NUMBER
       STA A 3,X
       LDA A 2,X
       COM A
       STA A 2,X
       INC 3,X
       BNE  FLOAT1
       INC 2,X
FLOAT1 NOP
       LDA A 2,X  TEST IF INTEGER VALUE ZERO
       EOR A 3,X
       BEQ  FLOAT3
       LDA A #4
FLOAT2 LSR 2,X  MOVE LS BYTE OF INTEGER
       ROR 3,X  TO MOST LEFT BYTE OF
       ROR 4,X  FRACTION.
       ROR 5,X
       ROR 6,X
       DEC A  CHECK IF 4 BITS MOVED
       BNE FLOAT2
       INC 1,X  INCREMENT MULTIPLER
       BRA FLOAT1
FLOAT3 NOP
       LDA A 0,X  GET INTEGER SIGN
       BPL FLOAT4  CHECK IF NEG.
       LDA A 1,X  YES - SET BIT 7 TO A 1
       ORA A #$80
       STA A 1,X
FLOAT4 NOP
       LDX 7,X  USE STACK POINTER TO GET TO
       LDX 2,X  ADDR OF RESULT
       PUL A  CLEAR STACK IF SIGN
       PUL A  MULTIPLER VALUE
       STA A 0,X
       PUL A  CLEAR STACK INTEGER VALUE
       PUL A
       PUL A  GET FLOATING POINT FRACTION
       STA A 1,X
       PUL A
       STA A 2,X
       PUL A
       STA A 3,X
       TSX  GET LOC OF STACK IN X
       LDX 0,X  GET RETURN LOCATION
       INS  CLEAN STACK
       INS
       JMP 4,X RETURN TO CALLER
       SPC 1
       PAGE
*
****
*
*       PROGRAM NAME:    SQRT
*
*       FUNCTION:        TO CALCULATE SQUARE ROOT VALUE  
*                        OF A FLOATING POINT VALUE.
*
*       CALLING SEQUENCE:
*
*           JSR    SQRT
*           FDB    ARG1     ADDR OF VALUE TO BE SQRT ROOTED
*           FDB    ARG2     ADDR OF ANSWER
*           ---             RETURN LOCATION
*
*                           NEG.VALUE CAUSES UNPREDICTABLE
*                           RESULTS.  ZERO VALUE ALSO CAUSES
*                           UNPREDICTABLE RESULTS.
*
*       PROGRAMMER:      D. W. CAWELTI
*
*       STACK SIZE:      3 BYTES
*
*       STACK DESCRIPTION:
*
*         BYTE 0  = COUNTER
*              1  = RETURN LOCATION MS BYTE
*              2  = RETURN LOCATION LS BYTE
*
*       RV    DATE    BY   REASON FOR CHANGE
*       --  --------  ---  -----------------------------------
*       00  06/24/76  DWC  INITIAL RELEASE
*
****
*
       PAGE
FPADD   EQU $0B00
FPSUB   EQU $0B02
FPMULT EQU $0B04
FPDIV   EQU $0B06
SQR       EQU $05F9  SQRT TEMP STORAGE 
SQRTP   EQU $05FC  SQRT TEMP STORAGE 
       SPC 4
SQRT   TSX  GET LOC OF STACK IN X
       CLR A  CLEAR COUNTER
       PSH A
       LDX  0,X  GET ADDR OF CALLING SEQUENCE
       LDX  0,X  GET ADDR OF VALUE
       LDA A 0,X  GET VALUE TO BE
       LDA B 1,X  SQRT ROOTED AND STORE
       STA A SQR  IN LOW CORE SQR ADDRESS.
       STA B SQR+1
       LDA A 2,X
       LDA B 3,X
       STA A SQR+2
       STA B SQR+3
       TSX    GET NEW LOC. OF STACK
SQRT1   NOP
       JSR FPSUB  CHECK IF SQR LT 1.0
       FDB SQR
       FDB ONE
       FDB SQRTP
       SPC 1
       LDA A SQRTP  CHECK IF BIT 7 SET
       BMI SQRT6
       JSR FPDIV  SQR = SQR/2.0
       FDB TWO
       FDB SQR
       FDB SQR
       SPC 1
       TSX
       INC 0,X  INCR COUNTER
       BRA SQRT1
       SPC 1
SQRT6  NOP
       JSR FPADD  SQRTP = SQR + 2.1372553
       FDB SQR
       FDB V1
       FDB SQRTP
       SPC 1
       JSR FPDIV  SQRTP = 4.8375280 / SQRTP
       FDB SQRTP
       FDB V2
       FDB SQRTP
       SPC 1
       JSR FPSUB  SQRTP = 2.5416391 - SQRTP
       FDB V3
       FDB SQRTP
       FDB SQRTP
       SPC 1
       JSR FPDIV  SQR = SQR/SQRTP
       FDB SQRTP
       FDB SQR
       FDB SQR
       SPC 1
       JSR FPADD  SQRTP = SQR + SQRTP
       FDB SQR
       FDB SQRTP
       FDB SQRTP
       SPC 1
       JSR FPDIV  SQR = SQRTP/2.0
       FDB TWO
       FDB SQRTP
       FDB SQR
       SPC 1
SQRT7  TSX  GET STACK ADDRESS
       LDA A 0,X  GET COUNTER
       LSR A    CHECK IF COUNTER EVEN
       BCC  SQRT8
       JSR FPMULT  NO - SQR = SQR*1.41421356
       FDB SQR
       FDB V4
       FDB SQR
       SPC 1
       TSX
       DEC 0,X  DECR COUNTER
       SPC 1
SQRT8  LDA A 0,X  GET COUNTER
       BEQ SQRT10
       ASR 0,X  COUNTER = COUNTER/2
SQRT9  DEC 0,X  DECR COUNTER
       JSR FPMULT  SQR = SQR*2.0
       FDB SQR
       FDB TWO
       FDB SQR
       SPC 1
       TSX
       TST  0,X
       BNE SQRT9
       SPC 1
SQRT10 NOP
       LDX 1,X  GET ADDR OF CALLING SEQ.
       LDX 2,X  GET ADDR OF ANSWER
       LDA A SQR  GET SQRT ROOT VALUE
       LDA B SQR+1
       STA A 0,X  STORE IN ANS ADDRESS
       STA B 1,X
       LDA A SQR+2
       LDA B SQR+3
       STA A 2,X
       STA B 3,X
       SPC 1
RET    NOP
       PUL A   CLEAN STACK
       TSX
       LDX 0,X  GET RETURN LOC
       INS  DECR STACK POINTER
       INS
       JMP 4,X  RETURN TO CALLER
       SPC 4
ONE    FDB $0110   FLOATING POINT 1.0
          FDB $0000
TWO    FDB $0120   FLOATING POINT 2.0
          FDB $0000
V1      FDB $0122   FLOATING POINT 2.1372553
          FDB $3232
V2      FDB $014D   FLOATING POINT 4.8375280
          FDB $6684
V3      FDB $0128   FLOATING POINT 2.5416391
          FDB $AA8D
V4      FDB $0116   FLOATING POINT 1.41421356
          FDB $A09E
       SPC 1
        END
        END
        END
        END