#pragma ident "@(#)linalg.c 1.1 97/10/31 SMI"

/*
 * linalg.c
 *
 *	@(#)linalg.c 1.8 96/11/04 
 *
 * Copyright (c) 1997 by Mike M. Chow - All Rights Reserved 
 *
 *
 *      Author: Mike M. Chow 
 *
 *      This module contains linear algebra routines.
 * 
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "zdebug.h"
#include "utils.h"
#include "types.h"
#include "linalg.h"

double IdentityMat[16] = {1.0, 0.0, 0.0, 0.0,
			  0.0, 1.0, 0.0, 0.0,
			  0.0, 0.0, 1.0, 0.0,
			  0.0, 0.0, 0.0, 1.0};
/* More Point Methods */

 
float 
sqrtDist(Point a, Point b)
{
  return ((float) sqrt((double)( (a[0] - b[0]) * (a[0] - b[0]) +
				 (a[1] - b[1]) * (a[1] - b[1]) +
				  (a[2] - b[2]) * (a[2] - b[2]))));
}



void
pointSet(Point p, float x, float y, float z)
{
  p[0] = x;
  p[1] = y;
  p[2] = z;
}



/* The next few implements typical vector operations: add, sub,
 *  dot, cross... 
 */

float *
pointAdd(Point result, Point a, Point b)
{
  int i;

  for (i = 0; i < 3; i++)
    result[i] = a[i] + b[i];
  return result;
}



float *
pointSub(Point result, Point a, Point b)
{
  int i;

  for (i = 0; i < 3; i++)
    result[i] = a[i] - b[i];
  return result;
}



float *
pointMulScalar(Point result, Point a, float b)
{
  int i;

  for (i = 0; i < 3; i++)
    result[i] = a[i] * b;
  return result;
}


float *
pointDivScalar(Point result, Point a, float b)
{
  int i;

  for (i = 0; i < 3; i++)
    result[i] = a[i]/ b;
  return result;
}


/*
 * pointDot
 *
 *	Compute the dot product of the two specified normals.
 *	The dot product is the cosine of the angle between the vectors.
 */

float
pointDot(Point v0,   Point v1)
{
    float dp;				/* The resulting dot product */
    dp = v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2];
    return (dp);
} /* End of pointDot */



/*
 * pointCross
 *
 *	Compute the cross product of the three specified vectors.
 *	The cross product produces a vector normal to two input vectors.
 */

void
pointCross(Point cp, Point v0, Point v1, Point v2)
{
    
    register float x01, y01, z01;	/* v0[0] - v1[0], etc. */
    register float x21, y21, z21;	/* v2[0] - v1[0], etc. */

    x01 = v0[0] - v1[0];
    y01 = v0[1] - v1[1];
    z01 = v0[2] - v1[2];
    x21 = v2[0] - v1[0];
    y21 = v2[1] - v1[1];
    z21 = v2[2] - v1[2];

    cp[0] = (y01 * z21) - (z01 * y21);
    cp[1] = (z01 * x21) - (x01 * z21);
    cp[2] = (x01 * y21) - (y01 * x21);

} /* End of pointCross */



float 
pointLength(Point p)
{
  return ((float) sqrt((double) (p[0]*p[0] + p[1]*p[1] + p[2]*p[2])));
}




void
pointNormalize(Point p)
{
  float len;

  len = pointLength(p);
  if (len != 0.0)
    pointDivScalar(p, p, len);
}


int 
ptInFacet(Point testPt, Facet *fptr)
{

  return (ptInBbox(testPt, fptr->bbox));
}

int 
ptInFacetOrig(Point testPt, Facet *fptr)
{
  int i;
  int prevSign = -1;

  for (i = 0; i < fptr->numVerts; i++){
    Point *p1, *p2, *inPt;
    int xtest, ytest, ztest;
    Vertex *varray;
    Point vec12, vec1h, cross;
    int currSign;

    varray = fptr->owner->obj->varray;
    p1 = &varray[fptr->vindex[i]].p;
    p2 = &varray[fptr->vindex[(i + 1) % fptr->numVerts]].p;
    inPt = &varray[fptr->vindex[(i + 2) % fptr->numVerts]].p;

    zdo6 printf("checking line ");
    zdo6 printPoint(*p1);
    zdo6 printPoint(*p2);
    zdo6 printf("in pt ");
    zdo6 printPoint(*inPt);
    
    pointSub(vec12, *p1, *p2);
    pointSub(vec1h, *p1, testPt);
    CROSS(cross, vec12, vec1h);
    zdo6 printf("cross ");
    zdo6 printPoint(cross);
    currSign = SIGN3(cross);
    zdo6 printf("sign %d \n", currSign);
    if (prevSign != -1)
      if (prevSign != currSign)
	return 0;
    prevSign = currSign;
  }
  return 1;
}



int 
ptInFacet2(Point testPt, Facet *fptr)
{
  int i;

  for (i = 0; i < fptr->numVerts; i++){
    Point *p1, *p2, *inPt;
    int xtest, ytest, ztest;
    Vertex *varray;

    varray = fptr->owner->obj->varray;
    p1 = &varray[fptr->vindex[i]].p;
    p2 = &varray[fptr->vindex[(i + 1) % fptr->numVerts]].p;
    inPt = &varray[fptr->vindex[(i + 2) % fptr->numVerts]].p;

    zdo6 printf("checking line ");
    zdo6 printPoint(*p1);
    zdo6 printPoint(*p2);
    zdo6 printf("in pt ");
    zdo6 printPoint(*inPt);
    
    xtest = ptSameSide(*p1, *p2, *inPt, testPt, 0);
    ytest = ptSameSide(*p1, *p2, *inPt, testPt, 1);
    ztest = ptSameSide(*p1, *p2, *inPt, testPt, 2);
    zdo6 printf("xtest %d ytest %d ztest %d \n", 
	       xtest, ytest, ztest);
    if (!(xtest || ytest || ztest))
      return 0;
  }
  return 1;
}

void
matInit(float a[16])
{
  int i;
  
  for (i = 0; i < 16; i++)
    a[i] = 0.0;
}


void
matMakeIdentity(float m[16])
{
    m[0+4*0] = 1; m[0+4*1] = 0; m[0+4*2] = 0; m[0+4*3] = 0;
    m[1+4*0] = 0; m[1+4*1] = 1; m[1+4*2] = 0; m[1+4*3] = 0;
    m[2+4*0] = 0; m[2+4*1] = 0; m[2+4*2] = 1; m[2+4*3] = 0;
    m[3+4*0] = 0; m[3+4*1] = 0; m[3+4*2] = 0; m[3+4*3] = 1;
}

void
matPrint(float a[16])
{
  int i, j;

  for (i = 0; i < 4; i++){
    for (j = 0; j < 4; j++)
      printf("%f ", a[i*4 + j]);
    printf("\n");
  }
}



void
matCopy(float r[16], float a[16])
{
  int i, j;
  
  for (i = 0; i < 16; i++)
    r[i] = a[i];
}

void
matTranspose(float r[16], float a[16])
{
  float temp[16];
  int i, j;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      temp[i*4+j] = a[j*4+i];

  matCopy(r, temp);
}



void
matMult(float r[16], float a[16], float b[16])
{
  int i, j;
  
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      r[i*4+j] = 
	a[i*4+0]*b[0*4+j] +
	a[i*4+1]*b[1*4+j] +
	a[i*4+2]*b[2*4+j] +
	a[i*4+3]*b[3*4+j];
    }
  }
}

void
matMultVec(float out[3],  float matrix[16], float in[3])
{
    int i;

    for (i=0; i<3; i++) {
	out[i] = 
	    in[0] * matrix[0*4+i] +
	    in[1] * matrix[1*4+i] +
	    in[2] * matrix[2*4+i];
    }
}



int 
matInverse(float inverse[16], float src[16])
{
    int i, j, k, swap;
    float t;
    float temp[4][4];

    for (i=0; i<4; i++) {
	for (j=0; j<4; j++) {
	    temp[i][j] = src[i*4+j];
	}
    }
    matMakeIdentity(inverse);

    for (i = 0; i < 4; i++) {
	/*
	** Look for largest element in column
	*/
	swap = i;
	for (j = i + 1; j < 4; j++) {
	    if (fabs(temp[j][i]) > fabs(temp[i][i])) {
		swap = j;
	    }
	}

	if (swap != i) {
	    /*
	    ** Swap rows.
	    */
	    for (k = 0; k < 4; k++) {
		t = temp[i][k];
		temp[i][k] = temp[swap][k];
		temp[swap][k] = t;

		t = inverse[i*4+k];
		inverse[i*4+k] = inverse[swap*4+k];
		inverse[swap*4+k] = t;
	    }
	}


	if (temp[i][i] == 0) {
	    /*
	    ** No non-zero pivot.  The matrix is singular, which shouldn't
	    ** happen.  This means the user gave us a bad matrix.
	    */
	    return 0;
	}

	t = temp[i][i];
	for (k = 0; k < 4; k++) {
	    temp[i][k] /= t;
	    inverse[i*4+k] /= t;
	}
	for (j = 0; j < 4; j++) {
	    if (j != i) {
		t = temp[j][i];
		for (k = 0; k < 4; k++) {
		    temp[j][k] -= temp[i][k]*t;
		    inverse[j*4+k] -= inverse[i*4+k]*t;
		}
	    }
	}
    }
    return 1;
}


Plane 
*planeCreate(Point normal, Point p1)
{
  Plane *plane;

  plane = (Plane *) malloc(sizeof(Plane));

  pointCopy(plane->normal, normal);
  plane->d = -1.0 * pointDot(normal, p1);
  zdo6 printf("pointDot(normal, p1) %f d %f\n", 
	 pointDot(normal, p1),
	 plane->d);
  pointCopy(plane->p, p1);
  return plane;
}


Plane
*planeCreate2(Point p1, Point p2, Point p3)
{
  printf("Warning planeCreate2 not done.\n");
  return NULL;
}
  


void
planeProject(Plane *plane, Point pt, Point proj,
	     float *dist, Point error)
{
  /* proj + N*dist = pt */
  /* dist = dot(N,pt) + d */
  Point temp;

  *dist = pointDot(plane->normal, pt) + plane->d;
  pointMulScalar(temp, plane->normal, *dist);
  pointSub(proj, pt, temp);
  pointSub(error, pt, proj);
}

void
planeProjectOld(Plane *plane, Point in, Point out,
	     Point error)
{
  float a[16], atrans[16], ata[16], invata[16], 
    temp[16], proj[16];
  Point tempVec;

  matInit(a);
  if (fabs(plane->normal[1]) > 0.00001) {
    /* A = [ b 0]
           [-a c]
	   [ 0 -b]
	   */
    a[0] = plane->normal[1];
    a[1*4+0] = -1.0 * plane->normal[0];
    a[1*4+1] = plane->normal[2];
    a[2*4+1] = -1.0 * plane->normal[1];
  }
  else {
    /* A = [-c 0]
           [ 0 1]
	   [ a 0]
	   */
    a[0] = -1.0 * plane->normal[2];
    a[2*4+0] = plane->normal[0];
    a[1*4+1] = 1.0;
  }
    

  zdo6 printf("A = \n");
  zdo6 matPrint(a);
  matTranspose(atrans, a);

  zdo6 printf("A' = \n");
  zdo6 matPrint(atrans);

  matMult(ata, atrans, a);

  zdo6 printf("\nA'*A = \n");
  zdo6 matPrint(ata);
  
  zdo6 printf("\ninv(A'*A) = \n");

  matInverse(invata, ata);
  zdo6 matPrint(invata);

  matMult(temp, invata, atrans);
  matMult(proj, a, temp);
  zdo6 printf("\n a*inv(ata)*atrans = \n");
  zdo6 matPrint(proj);

  /* b' = b - plane->p (coordinate change) */
  zdo6 printf("in ");
  zdo6 printPoint(in);
  zdo6 printf("plane->p");
  zdo6 printPoint(plane->p);
  pointSub(tempVec, in, plane->p);
  zdo6 printf("vec");
  zdo6 printPoint(tempVec);

  /* out is p = Ax */ 
  matMultVec(out, proj, tempVec);
  zdo6 printf("projection is ");
  zdo6 printPoint(out);
    
  /* error = p - b' */
  pointSub(error, tempVec, out);

  pointAdd(out, out, plane->p);
  zdo6 printf("\n Ax = %f %f %f", out[0], 
	 out[1], out[2]);
  zdo6 printf("error :");
  zdo6 printPoint(error);
}
  

int 
ptSameSide(Point p1, Point p2, Point inPt, 
	   Point testPt, int axis)
{
  float a, b, c;
  float x1, y1, x2, y2, inPtx, inPty, testPtx, testPty;
  float test1, test2;

  if (axis == 0) {
    x1 = p1[1];
    x2 = p2[1];
    y1 = p1[2];
    y2 = p2[2];
    inPtx = inPt[1];
    inPty = inPt[2];
    testPtx = testPt[1];
    testPty = testPt[2];
  }
  else 
    if (axis == 1) {
      x1 = p1[0];
      x2 = p2[0];
      y1 = p1[2];
      y2 = p2[2];
      inPtx = inPt[0];
      inPty = inPt[2];
      testPtx = testPt[0];
      testPty = testPt[2];
    }
    else {
      x1 = p1[0];
      x2 = p2[0];
      y1 = p1[1];
      y2 = p2[1];
      inPtx = inPt[0];
      inPty = inPt[1];
      testPtx = testPt[0];
      testPty = testPt[1];
    }

    
  b = (x2 - x1);
  a = -(y2 - y1);
  c = y1*(x2 - x1) - x1 * (y2 - y1);
  test1 = (a * inPtx + b * inPty);
  test2 = (a * testPtx + b * testPty);

  zdo6 printf("axis %d %f x1 %f y1 %f x2 %f y2 %f inPtx %f  inPty %f \n",
	      axis, x1, y1, x2, y2, inPtx, inPty );
  zdo6 printf("a %f b %f c %f test1 %f test2 %f t1 %d t2 %d\n",
	      a, b, c, test1, test2, 
	      (test1 < c), (test2 < c));

  return ((test1 == c) || (test2 == c) || ((test1 < c) == (test2 < c)));

}


int
ptInPolygon(Point pts[], int numPts, Point testPt)
{
  int i;

  for (i = 0; i < numPts; i++){
    Point *p1, *p2, *inPt;
    int xtest, ytest, ztest;

    p1 = &(pts[i]);
    p2 = &pts[(i + 1) % numPts];
    inPt = &pts[(i + 2) % numPts];

    zdo6 printf("checking line ");
    zdo6 printPoint(*p1);
    zdo6 printPoint(*p2);
    zdo6 printf("in pt ");
    zdo6 printPoint(*inPt);
    
    xtest = ptSameSide(*p1, *p2, *inPt, testPt, 0);
    ytest = ptSameSide(*p1, *p2, *inPt, testPt, 1);
    ztest = ptSameSide(*p1, *p2, *inPt, testPt, 2);
    zdo6 printf("xtest %d ytest %d ztest %d \n", 
	       xtest, ytest, ztest);
    if (!(xtest && ytest && ztest))
      return 0;
  }
  return 1;
}

void
ptInPolyTest()
{

  Point tri[3] = {{1.0, 0, 0},
		  {0, 1.0, 0},
		  {0, 0, 1.0}};
  Point testPt1 = {0.25, 0.5, 0.25};
  Point testPt2 = {0, 0.0, 0.5};

  printf("See if poly contains pt ");
  printPoint(testPt2);
  printf("result %d \n", ptInPolygon(tri, 3, testPt2));
}
  

#if 0 

float a[16] = {1, 1, 0, 0, 
	   1, 2, 0, 0,
	   2, 3, 0, 0,
	   0, 0, 0, 0};

float in[3] = {1,1,1};

Plane plane = {{1.0, 1.0, 1.0}, {0.0, 0.0, 0.0}};
Point vec = {2.0, 2.0, 2.0};


void planeTest()
{
  Point out, error;

  planeProject(&plane, vec, out, error);
}

  

void 
allTest()
{
  float atrans[16], ata[16], invata[16], 
    temp[16], all[16];
  float out[3];

  printf("A = \n");
  matPrint(a);
  matTranspose(atrans, a);
  printf("A' = \n");
  matPrint(atrans);

  matMult(ata, atrans, a);
  printf("\nA'*A = \n");
  matPrint(ata);
  
  printf("\ninv(A'*A) = \n");
  matInverse(invata, ata);
  matPrint(invata);

  matMult(temp, invata, atrans);
  matMult(all, a, temp);
  printf("\n a*inv(ata)*atrans = \n");
  matPrint(all);

  matMultVec(out, all, in);
  printf("\n p*vec = %f %f %f", out[0], 
	 out[1], out[2]);
  
}


void
main()
{
  planeTest();
}


#endif
