/*  Project CADshell

	 Mechanical Design Research Lab
	 Mechanical Engineering - Engineering Mechanics Dept.
	 Michigan Technological University
	 Copyright (C) 2001. All Rights Reserved.

	 FILE:			vmath.c++
	 MODULE:		CADshell
	 DEPENDS ON MODULE(S):	none
	 AUTHOR(S):       	Bernie Bettig


    OVERVIEW
    ========
	 Source file for vector algebra objects: sh_Point (or sh_Vector) and sh_Transform.
*/

#include "vmath.h"
#include <math.h>


// sh_Transform

sh_Transform::sh_Transform()
{
  int i,j;
  for (j=0;j<3;j++)
	 for (i=0;i<4;i++)
		if (i==j)
		  mat[i][j] = 1.;
		else
		  mat[i][j] = 0.;
}

sh_Transform::sh_Transform(sh_Trans Trans, const double Val)
{
  int i,j;
  for (j=0;j<3;j++)
	 for (i=0;i<4;i++)
		if (i==j)
		  mat[i][j] = 1.;
		else
		  mat[i][j] = 0.;
  Concat(Trans, Val);
}

sh_Transform::sh_Transform(const sh_Transform &M)
{
  *this = M;
}

sh_Transform::sh_Transform(sh_Point &zdir, sh_Point &xdir, sh_Point &trans)
{
  sh_Point ydir = zdir.Cross(xdir); 
  mat[0][0] = xdir.x;
  mat[0][1] = xdir.y;
  mat[0][2] = xdir.z;
  mat[1][0] = ydir.x;
  mat[1][1] = ydir.y;
  mat[1][2] = ydir.z;
  mat[2][0] = zdir.x;
  mat[2][1] = zdir.y;
  mat[2][2] = zdir.z;
  mat[3][0] = trans.x;
  mat[3][1] = trans.y;
  mat[3][2] = trans.z;
}

void sh_Transform::Concat(sh_Trans T, const double V)
{
  switch (T) {
	 case RotX:
	 {
		int i;
		sh_Transform M(*this);
		double Cos = cos(V);
		double Sin = sin(V);
		for (i=0;i<4;i++) {
		  mat[i][1] = M.mat[i][1]*Cos - M.mat[i][2]*Sin;
		  mat[i][2] = M.mat[i][2]*Cos + M.mat[i][1]*Sin;
		}
	 }
	 break;
	 case RotY:
	 {
		int i;
		sh_Transform M(*this);
		double Cos = cos(V);
		double Sin = sin(V);
		for (i=0;i<4;i++) {
		  mat[i][0] = M.mat[i][0]*Cos + M.mat[i][2]*Sin;
		  mat[i][2] = M.mat[i][2]*Cos - M.mat[i][0]*Sin;
		}
	 }
	 break;
	 case RotZ:
	 {
		int i;
		sh_Transform M(*this);
		double Cos = cos(V);
		double Sin = sin(V);
		for (i=0;i<4;i++) {
		  mat[i][0] = M.mat[i][0]*Cos - M.mat[i][1]*Sin;
		  mat[i][1] = M.mat[i][1]*Cos + M.mat[i][0]*Sin;
		}
	 }
	 break;
	 case Scale:
	 {
		for (int j=0; j<3; j++)
		  for (int i=0; i<4; i++)
			 mat[i][j] *= V;
	 }
	 break;
	 case ScaleX:
	 {
		for (int i=0; i<4; i++)
		  mat[i][0] *= V;
	 }
	 break;
	 case ScaleY:
	 {
		for (int i=0; i<4; i++)
		  mat[i][1] *= V;
	 }
	 break;
	 case ScaleZ:
	 {
		for (int i=0; i<4; i++)
		  mat[i][2] *= V;
    }
	 break;
	 case TranslateX:
		mat[3][0] += V;
	 break;
	 case TranslateY:
		mat[3][1] += V;
	 break;
	 case TranslateZ:
		mat[3][2] += V;
	 break;
  }
}

void sh_Transform::Translate(const double dx, const double dy, const double dz)
{
  mat[3][0] += dx;
  mat[3][1] += dy;
  mat[3][2] += dz;
}

sh_Transform sh_Transform::operator *(const sh_Transform& T) const {
  sh_Transform N(*this);
  int i;
  for (i=0;i<3;i++) {
	 N.mat[0][i] = mat[0][0]*T.mat[0][i] + mat[0][1]*T.mat[1][i] + mat[0][2]*T.mat[2][i];
	 N.mat[1][i] = mat[1][0]*T.mat[0][i] + mat[1][1]*T.mat[1][i] + mat[1][2]*T.mat[2][i];
	 N.mat[2][i] = mat[2][0]*T.mat[0][i] + mat[2][1]*T.mat[1][i] + mat[2][2]*T.mat[2][i];
	 N.mat[3][i] = mat[3][0]*T.mat[0][i] + mat[3][1]*T.mat[1][i] + mat[3][2]*T.mat[2][i] + T.mat[3][i];
  }
  return N;
}


sh_Transform sh_Transform::Inverse()
{
  sh_Transform M;
  double a, b, c, D;
  a = mat[1][1]*mat[2][2]-mat[2][1]*mat[1][2];
  b = mat[0][1]*mat[2][2]-mat[2][1]*mat[0][2];
  c = mat[0][1]*mat[1][2]-mat[1][1]*mat[0][2];
  D = mat[0][0]*a - mat[1][0]*b + mat[2][0]*c;
  M.mat[0][0] = a/D;
  M.mat[0][1] = -b/D;
  M.mat[0][2] = c/D;
  M.mat[1][0] = -(mat[1][0]*mat[2][2]-mat[2][0]*mat[1][2])/D;
  M.mat[1][1] = (mat[0][0]*mat[2][2]-mat[2][0]*mat[0][2])/D;
  M.mat[1][2] = -(mat[0][0]*mat[1][2]-mat[1][0]*mat[0][2])/D;
  M.mat[2][0] = (mat[1][0]*mat[2][1]-mat[2][0]*mat[1][1])/D;
  M.mat[2][1] = -(mat[0][0]*mat[2][1]-mat[2][0]*mat[0][1])/D;
  M.mat[2][2] = (mat[0][0]*mat[1][1]-mat[1][0]*mat[0][1])/D;
  M.mat[3][0] = -(mat[1][0]*(mat[2][1]*mat[3][2]-mat[3][1]*mat[2][2])
                -mat[2][0]*(mat[1][1]*mat[3][2]-mat[3][1]*mat[1][2])
                +mat[3][0]*a)/D;
  M.mat[3][1] = (mat[0][0]*(mat[2][1]*mat[3][2]-mat[3][1]*mat[2][2])
                 -mat[2][0]*(mat[0][1]*mat[3][2]-mat[3][1]*mat[0][2])
                 +mat[3][0]*b)/D;
  M.mat[3][2] = -(mat[0][0]*(mat[1][1]*mat[3][2]-mat[3][1]*mat[1][2])
                -mat[1][0]*(mat[0][1]*mat[3][2]-mat[3][1]*mat[0][2])
                +mat[3][0]*c)/D;
  return M;
}

// sh_Point

double sh_Point::Magnitude() 
{
  return sqrt(x*x + y*y + z*z);
}

sh_Point operator *(const double& r, const sh_Point& p)
{
  return sh_Point(r*p.x, r*p.y, r*p.z);
}

sh_Point sh_Point::Perpendicular(const sh_Point &ref)
{
  sh_Point perp = this->Cross(ref);
  if (perp.Magnitude() < .001)
  {
    sh_Point ref2(ref.z, ref.x, ref.y);
    perp = this->Cross(ref2);
  }
  perp.UnitNormalize();
  return perp;
}


void StretchBounds(sh_Point& Min, sh_Point& Max, const sh_Point& point)
{
  if (point.x <  Min.x) Min.x = point.x;
  if (point.y <  Min.y) Min.y = point.y;
  if (point.z <  Min.z) Min.z = point.z;
  if (point.x >  Max.x) Max.x = point.x;
  if (point.y >  Max.y) Max.y = point.y;
  if (point.z >  Max.z) Max.z = point.z;
}