/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Michael C. Wrinn.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	sin.c
 *
 * DESCRIPTION:	Contains code for the table driven sin_T() routines.
 * 
 * AUTHOR/DATE:	John Lloyd and Mike Wrinn, Fall 1986
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>
#include "SIN_Cdefs.h"
#define SIN_IVSIZE (SIN_TBSIZE-2)

#ifdef vax
asm ("#include <SIN_Adefs.h>");	/* asm doesn't allow nested quotes */
asm (".set SIN_IVSIZE,SIN_TBSIZE-2");	/* and as dosn't honor parans! */
#endif

float SIN (theta)
double theta;
{
#ifdef vax
 	asm ("emodf	$SIN_TSCALE_F,$0,4(ap),r4,r0");
	asm ("blss	1f");

	asm ("bicl2	$-SIN_IVSIZE,r4");	/* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O)[r4],r2");
	asm ("mulf2	r3,r0");		/* interpolate sine */
	asm ("addf2	r2,r0");
	asm ("ret");

	asm ("1:");
	asm ("bicl2	$-SIN_IVSIZE,r4");	/* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O-SIN_TESIZE/2)[r4],r2");
	asm ("mulf2	r2,r0");		/* interpolate sine */
	asm ("addf2	r3,r0");
#else
	register float *stp;
	register int index;
	double x;

	index = (x = SIN_TSCALE * theta);
	x -= index;
	stp = (float *) &mt_sin [ index & (SIN_IVSIZE-1) ];

	if (x >= 0.)
	 { return ( stp[3] * x + stp[2] );
	 }
	else
	 { return ( stp[1] * x + stp[2] );
	 }
#endif
}

float COS (theta)
float theta;
{
#ifdef vax
	asm ("emodf	$SIN_TSCALE_F,$0,4(ap),r4,r0");
	asm ("blss	1f");

	asm ("addl2	$(SIN_IVSIZE/4),r4");/* bump index for cosine   */
	asm ("bicl2	$-SIN_IVSIZE,r4");	 /* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O)[r4],r2");
	asm ("mulf2	r3,r0");		 /* interpolate sine */
	asm ("addf2	r2,r0");
	asm ("ret");

	asm ("1:");
	asm ("addl2	$(SIN_IVSIZE/4),r4");/* bump index for cosine   */
	asm ("bicl2	$-SIN_IVSIZE,r4");	 /* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O-SIN_TESIZE/2)[r4],r2");
	asm ("mulf2	r2,r0");		 /* interpolate cosine */
	asm ("addf2	r3,r0");
#else
	register float *stp;
	register int index;
	double x;

	index = (x = SIN_TSCALE * theta);
	x -= index;
	stp = (float *) &mt_sin [ (index+SIN_IVSIZE/4) & (SIN_IVSIZE-1) ];

	if (x >= 0.)
	 { return ( stp[3] * x + stp[2] );
	 }
	else
	 { return ( stp[1] * x + stp[2] );
	 }
#endif
}

SINCOS (theta, sin, cos)
float theta;
float *sin, *cos;
{
#ifdef vax
	asm ("emodf	$SIN_TSCALE_F,$0,4(ap),r4,r1");
	asm ("blss	1f");

	asm ("bicl2	$-SIN_IVSIZE,r4");	/* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O)[r4],r2");
	asm ("mulf3	r3,r1,r0");		/* interpolate sine */
	asm ("addf3	r2,r0,*12(ap)");

	asm ("addl2	$(SIN_IVSIZE/4),r4");	/* bump index for cosine   */
	asm ("bicl2	$-SIN_IVSIZE,r4");	/* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O)[r4],r2");
	asm ("mulf3	r3,r1,r0");		/* interpolate cosine */
	asm ("addf3	r2,r0,*16(ap)");

	asm ("ret");

	asm ("1:");
	asm ("bicl2	$-SIN_IVSIZE,r4");	/* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O-SIN_TESIZE/2)[r4],r2");
	asm ("mulf3	r2,r1,r0");		/* interpolate sine */
	asm ("addf3	r3,r0,*12(ap)");

	asm ("addl2	$(SIN_IVSIZE/4),r4");	/* bump index for cosine   */
	asm ("bicl2	$-SIN_IVSIZE,r4");	/* range reduce and scale  */
	asm ("movq	(_mt_sin+SIN_TINDEX1_O-SIN_TESIZE/2)[r4],r2");
	asm ("mulf3	r2,r1,r0");		/* interpolate cosine */
	asm ("addf3	r3,r0,*16(ap)");
#else
	register float *stp;
	register int index;
	double x;

	index = (x = SIN_TSCALE * theta);
	x -= index;
	stp = (float *) &mt_sin [ index & (SIN_IVSIZE-1) ];

	if (x >= 0.)
	 { *sin = stp[3] * x + stp[2];
	   stp = (float *) &mt_sin [ (index+SIN_IVSIZE/4) & (SIN_IVSIZE-1) ];
	   *cos = stp[3] * x + stp[2];
	 }
	else
	 { *sin = stp[1] * x + stp[2];
	   stp = (float *) &mt_sin [ (index+SIN_IVSIZE/4) & (SIN_IVSIZE-1) ];
	   *cos = stp[1] * x + stp[2];
	 }
#endif
}
