/**************************************************************************/
/******************  vector calculation aids  *****************************/
/**************************************************************************/

#include <math.h>
#include "steraid.h"
#include "vectors.h"

/**************************************************************************/

Vector Vsum(Vector u, Vector v)
{
  Vector s;
  s.x=u.x+v.x;
  s.y=u.y+v.y;
  s.z=u.z+v.z;
  return(s);
}

/**************************************************************************/

Vector Vdif(Vector u, Vector v)
{
  Vector s;
  s.x=u.x-v.x;
  s.y=u.y-v.y;
  s.z=u.z-v.z;
  return(s);
}

/**************************************************************************/

double Vmag(Vector v)
{ return(sqrt(v.x*v.x+v.y*v.y+v.z*v.z)); }

/**************************************************************************/

double dot_product(Vector u, Vector v)
{
  return(u.x*v.x+u.y*v.y+u.z*v.z);
}

/**************************************************************************/

Vector cross_product(Vector u, Vector v)
{
  Vector p;
  p.x=u.y*v.z-u.z*v.y;
  p.y=u.z*v.x-u.x*v.z;
  p.z=u.x*v.y-u.y*v.x;
  return(p);
}

/**************************************************************************/

Vector unit_vector(Vector v)
{
  double magv;
  if((magv=Vmag(v))==0.0) return(v);
  v.x/=magv;
  v.y/=magv;
  v.z/=magv;
  return(v);
}

/**************************************************************************/

double VangleV(Vector u, Vector v)
{
  double d=dot_product(unit_vector(v),unit_vector(u));
  if(AlmostZero(1.0-d)) return(0.0);
  if(AlmostZero(1.0+d)) return(PI);
  return(acos(d));
}

/**************************************************************************/

Vector SxV(double s, Vector v)
{
  v.x*=s;
  v.y*=s;
  v.z*=s;
  return(v);
}

/**************************************************************************/

Matrix SxM(double s, Matrix M)
{
  M.x=SxV(s,M.x);
  M.y=SxV(s,M.y);
  M.z=SxV(s,M.z);
  return(M);
}

/**************************************************************************/

Vector VequalV(Vector v) { return(v); }

/**************************************************************************/

Vector Vequal(double x, double y, double z)
{
  Vector v;
  v.x=x;
  v.y=y;
  v.z=z;
  return(v);
}

/**************************************************************************/

int Vcmp(Vector u, Vector v)
{
  double d=0.0;
  d=Vmag(Vdif(u,v));
  return((int)(1000*d));
}

/**************************************************************************/

Matrix Mtranspose(Matrix A)
{
  Matrix T;
  T.x.x=A.x.x; T.x.y=A.y.x; T.x.z=A.z.x;
  T.y.x=A.x.y; T.y.y=A.y.y; T.y.z=A.z.y;
  T.z.x=A.x.z; T.z.y=A.y.z; T.z.z=A.z.z;
  return(T);
}

/**************************************************************************/

Vector Mtransform(Matrix R, Vector u)
{
  Vector v;
  v.x=dot_product(R.x,u);
  v.y=dot_product(R.y,u);
  v.z=dot_product(R.z,u);
  return(v);
}

/**************************************************************************/

double SDLP(Vector line, Vector v)
{
  if (Vmag(v)==0.0) return(10e10);
  return(Vmag(v)*sin(VangleV(line,v)));
}

/**************************************************************************/

Vector average_direction(int n, Vector v[])
{
  int i;
  Vector u;
  Vector vo;
  u=Vequal(0,0,0);
  for(i=0;i<n;i++)
  {
    vo=unit_vector(v[i]);
    u=Vsum(vo,u);
  }
  return(unit_vector(u));
}

/**************************************************************************/

double Get_Vector_Theta(Vector v)
{
  v=unit_vector(v);
  return(acos(v.z));
}

/**************************************************************************/

double Get_Vector_Phi(Vector v)
{
  double phi=0.0;
  v=unit_vector(v);
  if (AlmostZero(v.x))
  {
    if (v.y >= 0) return(PI/2);
    else return(PI+PI/2);
  }
  else
  {
    phi=atan(v.y/v.x);
    if((v.x<0.0)&&(v.y>=0.0)) phi+=PI;
    if((v.x<0.0)&&(v.y<0.0)) phi-=PI;
  }
  return(phi);
}

/**************************************************************************/

Vector Rotate_About_X(Vector u, double angle)
{
  Vector v;
  v.x=u.x;
  v.y=u.y*cos(angle)-u.z*sin(angle);
  v.z=u.z*cos(angle)+u.y*sin(angle);
  return(v);
}

/**************************************************************************/

Vector Rotate_About_Y(Vector u, double angle)
{
  Vector v;
  v.x=u.x*cos(angle)+u.z*sin(angle);
  v.y=u.y;
  v.z=u.z*cos(angle)-u.x*sin(angle);
  return(v);
}

/**************************************************************************/

Vector Rotate_About_Z(Vector u, double angle)
{
  Vector v;
  v.x=u.x*cos(angle)-u.y*sin(angle);
  v.y=u.y*cos(angle)+u.x*sin(angle);
  v.z=u.z;
  return(v);
}

/**************************************************************************/

void Rotate_Indices_Right(Vector *v)
{
  Vector u;
  u.x=v->z;
  u.y=v->x;
  u.z=v->y;
  *v=VequalV(u);
}

/**************************************************************************/

void Rotate_Indices_Left(Vector *v)
{
  Vector u;
  u.x=v->y;
  u.y=v->z;
  u.z=v->x;
  *v=VequalV(u);
}

/**************************************************************************/

double seperate(struct vector A, struct vector B)
{
  double xsq, ysq, zsq;

  xsq=(A.x-B.x)*(A.x-B.x);
  ysq=(A.y-B.y)*(A.y-B.y);
  zsq=(A.z-B.z)*(A.z-B.z);
  return(msqrt(xsq+ysq+zsq,"seperate"));
}

/**************************************************************************/

Vector Vwrap(struct vector v, struct vector lo, struct vector hi)
{
  int suc;
  do
  {
    suc=0;
    if (v.x<lo.x) { v.x+=hi.x-lo.x; suc++; }
    if (v.y<lo.y) { v.y+=hi.y-lo.y; suc++; }
    if (v.z<lo.z) { v.z+=hi.z-lo.z; suc++; }
    if (v.x>hi.x) { v.x-=hi.x-lo.x; suc++; }
    if (v.y>hi.y) { v.y-=hi.y-lo.y; suc++; }
    if (v.z>hi.z) { v.z-=hi.z-lo.z; suc++; }
  } while(suc);
  return(v);
}

/**************************************************************************/

Vector *Reciprocal_Vector(Vector *V)
{
  if(!AlmostZero(V->x)) V->x=1.0/V->x;
  if(!AlmostZero(V->y)) V->x=1.0/V->y;
  if(!AlmostZero(V->z)) V->x=1.0/V->z;
  return(V);
}

/**************************************************************************/

Matrix *Reciprocal_Matrix(Matrix *M)
{
  Reciprocal_Vector(&M->x);
  Reciprocal_Vector(&M->y);
  Reciprocal_Vector(&M->z);
  return(M);
}

/**************************************************************************/