/*-=-=-=-=-=-=-=-=-=-=-=- 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:	vector.c
 *
 * DESCRIPTION:	A library of vector support routines for floats and doubles.
 *
 *		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>

#include "../h/cdefs.h"
#include "../h/vector.h"

static fprintV();

/* Assignment routines */

float *copyVf (r, v, size)
register float *r, *v;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v[j];
	 }
	return (r);
}

double *copyVd (r, v, size)
register double *r, *v;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v[j];
	 }
	return (r);
}

float *copyVdf (r, v, size)
register float *r;
register double *v;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v[j];
	 }
	return (r);
}

double *copyVfd (r, v, size)
register double *r;
register float *v;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v[j];
	 }
	return (r);
}

/* routines for zeroing a vector */

float *zeroVf (r, size)
register float *r;
int size;
{
	register int j;
	for (j=0; j<size; j++)
	 { r[j] = 0.0;
	 }
	return (r);	
}

double *zeroVd (r, size)
register double *r;
int size;
{
	register int j;
	for (j=0; j<size; j++)
	 { r[j] = 0.0;
	 }
	return (r);	
}

/* single precision arithmetic routines */

float *subtractVf (r, v1, v2, size)
register float *r, *v1, *v2;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v1[j] - v2[j];
	 }
	return (r);
}

float *addVf (r, v1, v2, size)
register float *r, *v1, *v2;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v1[j] + v2[j];
	 }
	return (r);
}

float *scaleVf (r, a, v, size)
register float *r, *v;
register double a;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = a * v[j];
	 }
	return (r);
}

float *combineVf (r, a, v1, b, v2, size)
register float *r, *v1, *v2;
register double a, b;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = a * v1[j] + b * v2[j];
	 }
	return (r);
}

float normVf (v, size)
register float *v;
int size;
{
	register int j;
	register float x = 0.0;

	for (j=0; j<size; j++)
	 { x += v[j]*v[j];
	 }
	return (sqrt(x));
}

float dotVf (v1, v2, size)
register float *v1, *v2;
int size;
{
	register int j;
	register float x = 0.0;

	for (j=0; j<size; j++)
	 { x += v1[j]*v2[j];
	 }
	return (x);
}

/* double precision arithmetic routines */

double *subtractVd (r, v1, v2, size)
register double *r, *v1, *v2;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v1[j] - v2[j];
	 }
	return (r);
}

double *addVd (r, v1, v2, size)
register double *r, *v1, *v2;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = v1[j] + v2[j];
	 }
	return (r);
}

double *scaleVd (r, a, v, size)
register double *r, *v;
register double a;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = a * v[j];
	 }
	return (r);
}

double *combineVd (r, a, v1, b, v2, size)
register double *r, *v1, *v2;
register double a, b;
int size;
{
	register int j;

	for (j=0; j<size; j++)
	 { r[j] = a * v1[j] + b * v2[j];
	 }
	return (r);
}

double normVd (v, size)
register double *v;
int size;
{
	register int j;
	register double x = 0.0;

	for (j=0; j<size; j++)
	 { x += v[j]*v[j];
	 }
	return (sqrt(x));
}

double dotVd (v1, v2, size)
register double *v1, *v2;
int size;
{
	register int j;
	register double x = 0.0;

	for (j=0; j<size; j++)
	 { x += v1[j]*v2[j];
	 }
	return (x);
}
 
/* IO routines */

fscanVd (fp, v, size)
FILE *fp;
double *v;
int size;
{
	register int j;
	int status;

	for (j=0; j<size; j++)
	 { if ((status = fscanf (fp, "%lf", &v[j])) != 1)
	    { break;
	    }
	 }
	return (j == size ? 1 : status );
}

fscanVf (fp, v, size)
FILE *fp;
float *v;
int size;
{
	register int j;
	int status;

	for (j=0; j<size; j++)
	 { if ((status = fscanf (fp, "%f", &v[j])) != 1)
	    { break;
	    }
	 }
	return (j == size ? 1 : status );
}

scanVf (v, size)
float *v;
int size;
{
	return (fscanVf (stdin, v, size));
}

scanVd (v, size)
double *v;
int size;
{
	return (fscanVd (stdin, v, size));
}

fprintVd (fp, s, v, size)
FILE *fp;
const char *s;
double *v;
{
	fprintV (fp, s, (void*)v, size, 1 /* double */);
}

fprintVf (fp, s, v, size)
FILE *fp;
const char *s;
float *v;
{
	fprintV (fp, s, (void*)v, size, 0 /* float */);
}

static fprintV (fp, s, v, size, dble)
FILE *fp;
const char *s;
void *v;
int size;
int dble;
{
#define MAXF	256
	char buf[MAXF+2];
	char *format;
	register char *cp;
	register int c, i;
	
	float *vf;
	double *vd;

	if (dble)
	 { vd = (double*)v;
	 }
	else
	 { vf = (float*)v;
	 }

	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';
	      if (dble)
	       { for (i=0; i<size; i++)
		  { fprintf (fp, format, vd[i]);
		  }
	       }
	      else
	       { for (i=0; i<size; i++)
		  { fprintf (fp, format, vf[i]);
		  }
	       }
	      cp[1] = c1;		/* restore format string */
	      cp[2] = c2;

	      format = cp+1;
	      fprintf (fp, format);
	    }
	 }
}
	
printVd (s, v, size)
const char *s;
double *v;
int size;
{
	fprintVd (stdout, s, v, size);
}

printVf (s, v, size)
const char *s;
float *v;
int size;
{
	fprintVf (stdout, s, v, size);
}
