/*
 * Copyright (c) 1992, The Geometry Center
 *                     University of Minnesota 
 *                     1300 South Second Street
 *                     Minneapolis, MN  55454
 *
 * email address: software@geom.umn.edu
 *
 * This software is copyrighted as noted above.  It may be freely copied,
 * modified, and redistributed, provided that the copyright notice is
 * preserved on all copies.
 *
 * There is no warranty or other guarantee of fitness for this software,
 * it is provided solely "as is".  Bug reports or fixes may be sent
 * to the authors, who may or may not act on them as they desire.
 *
 * You may not include this software in a program or other software product
 * without supplying the source, or without informing the end-user that the
 * source is available for no extra charge.
 *
 * If you modify this software, you should include a notice giving the
 * name of the person performing the modification, the date of modification,
 * and the reason for such modification.
 *
 *     The National Science and Technology Research Center for
 *      Computation and Visualization of Geometric Structures
 */

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

typedef struct {
  double dx,dy,dz;
 } Vector;

void CrossProduct(vec1,vec2,out)

Vector *vec1,*vec2,*out;

{

  out->dx = vec1->dy*vec2->dz - vec1->dz*vec2->dy;
  out->dy = vec1->dz*vec2->dx - vec1->dx*vec2->dz;
  out->dz = vec1->dx*vec2->dy - vec1->dy*vec2->dx;
}

double Angle(pnt1,pnt2)

double pnt1[3],pnt2[3];
{
  return(acos(DotProduct(pnt1,pnt2)/(Norm(pnt1) * Norm(pnt2))));
}

double DotProduct(vec1,vec2)

double vec1[3],vec2[3];
{
  return(vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]);
}

double Norm(vec)

double vec[3];
{
  return(sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]));
}


void Normalize(vec)
double vec[3];
{
  double norm;
  norm = Norm(vec);
  if(norm < EPSILON)
    fprintf(stderr,"Warning: tried to normalize zero length vector.\n");
  else
    vec[0] /= norm; vec[1] /= norm; vec[2] /= norm;
}

void TransformPoint(xfm,in,out)

double xfm[4][4],in[3],out[3];

{
  int i;
  double x[4];

  for(i=0;i<4;++i)
    x[i] = xfm[i][0]*in[0] + xfm[i][1]*in[1] + xfm[i][2]*in[2] + xfm[i][3];

  if(x[3] == 1.0) 
     for(i=0;i<3;++i)
        out[i] = x[i];
  else
     for(i=0;i<3;++i)
        out[i] = x[i]/x[3];
}

double TwoByTwoDeterminant(array)

double array[2][2];
{
  return(array[0][0]*array[1][1] - array[0][1]*array[1][0]);
}


int SolveTwoByTwo(coeff,x,y)

double coeff[2][3],*x,*y;

{
  double det,array[2][2];
  int i,j;

  for(i=0;i<2;++i)
     for(j=0;j<2;++j) array[i][j] = coeff[i][j];

  det = TwoByTwoDeterminant(array);
  if(det < EPSILON && det > -EPSILON) return(0);

  array[0][0] = coeff[0][2]; array[1][0] = coeff[1][2];
  *x = TwoByTwoDeterminant(array)/det;

  for(i=0;i<2;++i)
     for(j=0;j<2;++j) array[i][j] = coeff[i][j];
  array[0][1] = coeff[0][2]; array[1][1] = coeff[1][2];
  *y = TwoByTwoDeterminant(array)/det;
  return(1);

}


