/*------------------------------------------------------------------------
 * filename - coord.cas         Version 1.0 /DEE 2-92
 *
 * function(s)
 *        rpc - Converts Rectangular to Polar Coordinates
 *        prc - Converts Polar to Rectangular Coordinates
 *        matan2 - Modified Arctangent routine
 *-----------------------------------------------------------------------*/

#pragma inline
#include <asmrules.h>

#include <math.h>
#include <_math.h>

/*--------------------------------------------------------------------------*

Name            matan2 - trigonometric function

Usage           void matan2(double y, double x);  ST(0) = y, ST(1) = x

Prototype in    None

Description
                matan2  returns ST(0), the  arc tangent   of y/x  and will
                produce correct results even  when resulting is near pi/2
                or -pi/2 (x near 0).

Return value    atan2 returns a value in the range -pi to pi on ST(0).
                If a ratio of 0/0  is supplied then zero is returned

*---------------------------------------------------------------------------*/
static  unsigned short  piBy2 [] = {0xC235, 0x2168, 0xDAA2, 0xC90F, 0x3FFF};

#pragma warn -rvl
#pragma warn -use
void matan2 (double y, double x)
{
register        SI, DI;         /* prevent the compiler making its own usage */

asm     mov     ax, x [6]       /* select MSW of x ..   */
asm     and     al, 0F0h        /* ignore fraction      */
asm     mov     bx, y [6]       /* .. and of y          */
asm     and     bl, 0F0h
asm     shl     bx, 1           /* discard sign         */
asm     jz      at2_yIsZero
asm     shl     ax, 1           /* discard sign         */
asm     jz      at2_xIsZero
asm     FDIVRP  ST(1), ST(0)
asm     _FAST_  (_FATAN_)

/* convert the simple answer to a four quadrant answer. */

at2_setQuad:
asm     test    BY0 (x [7]), 80h                /* the sign bit */
asm     jz      at2_end

asm     FLDPI

asm     test    BY0 (y [7]), 80h
asm     jz      at2_2ndQuad

at2_3rdQuad:
asm     FSUBP   ST(1), ST
asm     jmp     short   at2_end

at2_2ndQuad:
asm     FADDP   ST(1), ST

at2_end:
        return;


/*      Special cases.
*/
at2_yIsZero:
asm     shl     ax, 1
asm     jc      at2_retPi               /* x<0, return PI */
asm     FSTP    ST(1)                   /* else y is result     */
asm     jmp     short   at2_end

at2_retPi:                              /* y = 0, x < 0 */
asm     FCOMPP                          /* discard x and y */
asm     FLDPI                           /* and return PI */
asm     jmp     short  at2_end

at2_xIsZero:                            /* and y is not zero    */
                                        /*      or              */
at2_yIsInf:                             /* and x is finite      */
asm     FCOMPP                          /* discard x and y      */
asm     FLD     tbyte ptr (piBy2)
asm     test    BY0 (y [7]), 80h        /* check sign of Y */
asm     jz      at2_HPi                 /* positive - return PI/2 */
asm     FCHS                            /* negative - return -PI/2 */
at2_HPi:
asm     jmp     short   at2_setQuad

at2_xIsInf:
asm     FCOMPP                          /* discard x and y      */
asm     FLDZ
asm     jmp     short   at2_setQuad

}
#pragma warn .rvl
#pragma warn .use



/*--------------------------------------------------------------------------*

Name            rpc - Convert from Rectangular to Polar Coordinates

Usage           void rpc(double x, double y, double *r, double *theta);

Prototype in    math.h

Description     The magnitude is the same result as "hypot" except the input
                is not checked for the INF value.

                        r = (x^2 + y^2)^.5

                The angle is in the range of -180 < theta <= 180.

Return value    The function fills the variables pointed by "r" and "theta".
                "theta" is in radians.
                The function provides no error or overflow checking.
*---------------------------------------------------------------------------*/
void rpc (double x, double y, double *r, double *theta)
{
#if LDATA
asm     push    ES
#endif

asm     FLD     DOUBLE (x)              /* x                            */
asm     FLD     st(0)                   /* st(1) = x                    */
asm     FMUL    st,st(0)                /* x^2                          */
asm     FLD     DOUBLE (y)              /* y                            */
asm     FLD     st(0)                   /* 0=y,1=y,2=x^2,3=x            */
asm     FMUL    st,st(0)                /* y^2                          */
asm     FADDP   st(2),st                /* x^2 + y^2                    */
asm     FXCH                            /* Switch ST and ST(1)          */
asm     FSQRT                           /* sqrt (               )       */

asm     LES_    bx,r
asm     FSTP    DOUBLE (ES_ [bx])       /* *r = (x^2 + y^2)^.5          */

        matan2(y,x);
asm     LES_    bx,theta
asm     FSTP    DOUBLE (ES_ [bx])       /* *theta = matan2(y/x)         */
asm     FWAIT                           /* Will bomb out w/o this       */

#if LDATA
asm     pop     ES
#endif

}


/*--------------------------------------------------------------------------*

Name            prc - Convert from Polar to Rectangular Coordinates

Usage           void rpc(double r, double theta, double *x, double *y);

Prototype in    math.h

Description     Given the magnitude r, and the angle from the positive x-axis,
                convert to x and y coordinates.

                        x=r*cos(theta)
                        y=r*sin(theta)

                The angle is in the range of -180 < theta <= 180.

Return value    The function fills the variables pointed by "r" and "theta".
                The function provides no error or overflow checking.
*---------------------------------------------------------------------------*/
void prc (double r, double theta, double *x, double *y)
{

#if LDATA
asm     push    ES
#endif

asm     FLD     DOUBLE (r)
asm     FLD     DOUBLE (theta)
asm     FLD     st(0)
asm     _FAST_  (_FCOS_)                /* cos(theta)           */
asm     FMUL    st,st(2)                /* cos(theta)*r         */
asm     LES_    bx,x
asm     FSTP    DOUBLE (ES_ [bx])       /* x                    */
asm     _FAST_  (_FSIN_)
asm     FMULP                           /* y = r*sin(theta)     */
asm     LES_    bx,y
asm     FSTP    DOUBLE (ES_ [bx])       /* y                    */
asm     FWAIT

/* Needs quadrant check for y=sqrt(r^2-x^2)
asm     FLD     DOUBLE (r)
asm     FLD     st(0)                   /* r for later          */
asm     FMUL    st,st(0)                /* r^2                  */
asm     FLD     DOUBLE (theta)
asm     _FAST_  (_FCOS_)                /* cos(theta)           */
asm     FMULP   st(2),st                /* x=r*cos(theta)       */
asm     FXCH
asm     FMUL    st,st(0)                /* x^2                  */
asm     FSUBP                           /* r^2 - x^2            */
asm     FSQRT                           /* y                    */
asm     LES_    bx,y
asm     FSTP    DOUBLE (ES_ [bx])       /* y                    */
asm     FWAIT                           /* Must have            */
*/

#if LDATA
asm     pop     ES
#endif

}
