#include <math.h>
#include "Matrix.H"
#include "MatrixMisc.H"

Matrix CrossProd(const Matrix& A, const Matrix& B)
   // Returns the cross product AxB, or NAM if A and B are not both
   // 1x3 matrices (vectors).
{
     if (A.Rows() != 1 || B.Rows() != 1 || A.Cols() != 3 || B.Cols()
	 != 3) return Matrix::NAM();

     Matrix C(1, 3,
	      A(1,2)*B(1,3)-A(1,3)*B(1,2),
	      A(1,3)*B(1,1)-A(1,1)*B(1,3),
	      A(1,1)*B(1,2)-A(1,2)*B(1,1));
     return C;
}

double MatrixMag(const Matrix& m, int i)
   // Returns the magnitude of the ith row of m or -1.0.
{
     if (i > m.Rows())
	  return (-1.0);

     int j;
     double tot = 0.0;

     for (j = 1; j <= m.Cols(); j++)
	  tot += m(i, j)*m(i, j);

     return sqrt(tot);
}

Matrix& UnitizeInPlace(Matrix& m, int i)
   // Makes the ith row on m a unit vector, if that row exists.
{
     if (i > m.Rows())
	  return(m);

     double mag = MatrixMag(m, i);
     int j;

     if (mag <= 0.0) {
	  fprintf(stderr, "Magnitude is zero. Lose!\n");
	  exit(1);
     }	  

     for (j = 1; j <= m.Cols(); j++)
	  m(i, j) /= mag;

     return(m);
}

Matrix Unitize(const Matrix& m, int i)
   // Returns a unit vector (1xN matrix) of the ith row of m, or NAM.
{
   Matrix n(m, i, 1, 1, m.Cols());
   UnitizeInPlace(n,1);

   return(n);
}

double DotProd(const Matrix& m1, const Matrix& m2)
   // Returns m1.m2 or aborts.
{
     if (m1.Rows() != 1 || m2.Rows() != 1 || m1.Cols() != m2.Cols())
	  abort();

     double d = 0;

     for (int i = 1; i <= m1.Cols(); i++)
	  d += m1(1, i)*m2(1, i);

     return d;
}