/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	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:	vectorF6.c
 *
 * DESCRIPTION:	A library of vector support routines for floats.
 *
 *		The object is to use these to replace the myriad of different
 *		routines that exist for manipulating identical data 
 *		structures.
 * 
 * AUTHOR/DATE:	John Lloyd, Spring 87
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <math.h>

/* Assignment routines */

float *copyF6 (r, v)
register float *r, *v;
{
	r[0] = v[0];
	r[1] = v[1];
	r[2] = v[2];

	r[3] = v[3];
	r[4] = v[4];
	r[5] = v[5];

	return (r);
}

float *copyDF6 (r, v)
register float *r;
register double *v;
{
	r[0] = v[0];
	r[1] = v[1];
	r[2] = v[2];

	r[3] = v[3];
	r[4] = v[4];
	r[5] = v[5];

	return (r);
}

/* Arithmetic routines */

float *subtractF6 (r, v1, v2)
register float *r, *v1, *v2;
{
	r[0] = v1[0] - v2[0];
	r[1] = v1[1] - v2[1];
	r[2] = v1[2] - v2[2];

	r[3] = v1[3] - v2[3];
	r[4] = v1[4] - v2[4];
	r[5] = v1[5] - v2[5];

	return (r);
}

float *addF6 (r, v1, v2)
register float *r, *v1, *v2;
{
	r[0] = v1[0] + v2[0];
	r[1] = v1[1] + v2[1];
	r[2] = v1[2] + v2[2];

	r[3] = v1[3] + v2[3];
	r[4] = v1[4] + v2[4];
	r[5] = v1[5] + v2[5];

	return (r);
}

float *scaleF6 (r, a, v)
register float *r, *v;
register float a;
{
	r[0] = a * v[0];
	r[1] = a * v[1];
	r[2] = a * v[2];

	r[3] = a * v[3];
	r[4] = a * v[4];
	r[5] = a * v[5];

	return (r);
}

float *combineF6 (r, a, v1, b, v2)
register float *r, *v1, *v2;
register float a, b;
{
	r[0] = a * v1[0] + b * v2[0];
	r[1] = a * v1[1] + b * v2[1];
	r[2] = a * v1[2] + b * v2[2];

	r[3] = a * v1[3] + b * v2[3];
	r[4] = a * v1[4] + b * v2[4];
	r[5] = a * v1[5] + b * v2[5];

	return (r);
}

float normF6 (v)
register float *v;
{
	return (sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2] +
		      v[3]*v[3] + v[4]*v[4] + v[5]*v[5]));
}

float dotF6 (v1, v2)
register float *v1, *v2;
{
	return (v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] +
		v1[3]*v2[3] + v1[4]*v2[4] + v1[5]*v2[5]);
}
 
/* IO routines */

fscanF6 (fp, v)
FILE *fp;
float *v;
{
	int i;

	for (i=0; i<6; i++)
	 { if (scanf ("%f", &v[i]) != 1)
	    { break;
	    }
	 }
	while (fgetc(fp) != '\n')
	   ;
	return (i == 6 ? 1 : 0 );
}

scanF6 (v)
float *v;
{
	return (fscanF6 (stdin, v));
}

fprintF6 (fp, s, v)
FILE *fp;
char *s;
float *v;
{
#define MAXF	256
	char buf[MAXF+2];
	char *format;
	register char *cp;
	register int c, i;

	strncpy (buf, s, MAXF);
	buf[MAXF] = '\0';

	cp = format = buf;		/* locate format specifier */
	while (*cp != '\0')
	 { if (*cp == '%')
	    { if (*(cp+1) == '%')
	       { cp++;
	       }
	      else
	       { break;
	       }
	    }
	   cp++;
	 }
	
	if (*cp == '\0')
	 { fprintf (fp, format);
	 }
	else
	 { *cp = '\0';
	   fprintf (fp, format);

	   *cp = '%';
	   format = cp;
	   while (*cp != '\0')
	    { c = *cp;
	      if (c == 'd' || c == 'o' || c == 'x' ||
		  c == 'f' || c == 'g' || c == 'e' ||
		  c == 'F' || c == 'G' ||
		  c == 'c' || c == 's' || c == 'u' )
	       { break;
	       }
	      cp++;
	    }
	   if (*cp != '\0')
 	    { char c1, c2;

	      c1 = cp[1];		/* massage format string to add */
	      c2 = cp[2];		/* extra <space> after number */
	      cp[1] = ' ';
	      cp[2] = '\0';
	      for (i=0; i<6; i++)
	       { fprintf (fp, format, v[i]);
	       }
	      cp[1] = c1;		/* restore format string */
	      cp[2] = c2;

	      format = cp+1;
	      fprintf (fp, format);
	    }
	 }
}
	
printF6 (s, v)
char *s;
float *v;
{
	fprintF6 (stdout, s, v);
}
