/*
 * Copyright 1991, 1992, 1993, 1994, 1995, Silicon Graphics, Inc.
 * All Rights Reserved.
 *
 * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
 * the contents of this file may not be disclosed to third parties, copied or
 * duplicated in any form, in whole or in part, without the prior written
 * permission of Silicon Graphics, Inc.
 *
 * RESTRICTED RIGHTS LEGEND:
 * Use, duplication or disclosure by the Government is subject to restrictions
 * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
 * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
 * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
 * rights reserved under the Copyright Laws of the United States.
 */
/*
 * 	vect -
 *		Various functions to support operations on vectors.
 *
 *	David M. Ciemiewicz, Mark Grossman, Henry Moreton, and Paul Haeberli 
 *
 *	Converted to U-64 by Rob Moore.
 *
 */
#include <ultratypes.h>
#include <rmon.h>
#include "vect.h"
#include "em.h"
#include "gu.h"
#include "stress.h"

void
  vcopy(vect *v1, vect *v2)
{
    *v2 = *v1;
}

void
  vprint( vect *v )
{
    PRINTF("x: %f y: %f z: %f\n",v->x,v->y,v->z);
}

void
  dvprint(dvect *v)
{
    PRINTF("x: %f y: %f z: %f\n",v->x,v->y,v->z);
}

void
  vprint4(vect *v)
{
    PRINTF("x: %f y: %f z: %f w: %f\n",v->x,v->y,v->z,v->w);
}

void
  vset(vect *v, float x, float y, float z)
{
    v->x = x;
    v->y = y;
    v->z = z;
}

void
  vzero(vect *v)
{
    v->x = 0.0;
    v->y = 0.0;
    v->z = 0.0;
    v->w = 0.0;
}

void
  vone(vect *v)
{
    v->x = 1.0;
    v->y = 1.0;
    v->z = 1.0;
    v->w = 1.0;
}

void
  vset4(vect *v, float x, float y, float z, float w)
{
    v->x = x;
    v->y = y;
    v->z = z;
    v->w = w;
}

void
  vnormal(void *v)
{
    float len;

    len = vlength(v);
    if(len>0.0)
	vscale(v,1.0/len);
    else
	vzero(v);
}

float 
  vlength(vect *v) 
{
    return sqrtf(v->x*v->x + v->y*v->y + v->z*v->z);

}

void
  vscale(vect *v, float mul)
{
    v->x *= mul;
    v->y *= mul;
    v->z *= mul;
}

void
  vmult(vect *src1, vect *src2, vect *dst)
{
    dst->x = src1->x * src2->x;
    dst->y = src1->y * src2->y;
    dst->z = src1->z * src2->z;
}

void
  vadd(vect *src1, vect *src2, vect *dst)
{
    dst->x = src1->x + src2->x;
    dst->y = src1->y + src2->y;
    dst->z = src1->z + src2->z;
}

void
  vsub(vect *src1, vect *src2, vect *dst)
{
    dst->x = src1->x - src2->x;
    dst->y = src1->y - src2->y;
    dst->z = src1->z - src2->z;
}

void
  vhalf(vect *v1, vect *v2, vect *half)
{
    float len;

    vadd(v2,v1,half);
    len = vlength(half);
    if(len>0.0001)
	vscale(half,1.0/len);
    else
	*half = *v1;
}

float 
  vdot(vect *v1, vect *v2) 
{
    return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z;
}

double 
  dvdot(dvect *v1, dvect *v2) 
{
    return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z;
}

void
  vcross(vect *v1, vect *v2, vect *cross)
{
    vect temp;

    temp.x = (v1->y * v2->z) - (v1->z * v2->y);
    temp.y = (v1->z * v2->x) - (v1->x * v2->z);
    temp.z = (v1->x * v2->y) - (v1->y * v2->x);
    *cross = temp;
}

void
  vplane(vect *normal, vect *point, vect *plane)
{
    plane->x = normal->x;
    plane->y = normal->y;
    plane->z = normal->z;
    plane->w = - vdot(normal, point);
}

void
  dvplane( dvect *normal,  dvect *point,  dvect *plane)
{
    plane->x = normal->x;
    plane->y = normal->y;
    plane->z = normal->z;
    plane->w = - dvdot(normal, point);
}

void
  vdirection(vect *v1, vect *dir)
{
	*dir = *v1;
	vnormal(dir);
}

void
  makeplane(vect *p1, vect *p2, vect *p3, vect *v)
{
    vect a, b;

    vsub(p1,p2,&a);
    vsub(p3,p2,&b);
    vcross(&a,&b,&b);
    vnormal(&b);
    vplane(&b,p1,v);
}

void
  vreflect(vect *in, vect *mirror, vect *out)
{
    vect temp;

    temp = *mirror;
    vscale(&temp,vdot(mirror,in));
    vsub(&temp,in,out);
    vadd(&temp,out,out);
}


void
  vmatrixcopy( float m1[4][4], float m2[4][4] )
{
    int i,j;
    for(i = 0; i < 4; i++)
      for(j = 0; j < 4; j++)
        m2[i][j] = m1[i][j];
}


void
  vmultmatrix(float m1[4][4], float m2[4][4], float prod[4][4])
{
    int row, col;
    float temp[4][4];

    for(row=0 ; row<4 ; row++) 
        for(col=0 ; col<4 ; col++)
            temp[row][col] = m1[row][0] * m2[0][col]
                           + m1[row][1] * m2[1][col]
                           + m1[row][2] * m2[2][col]
                           + m1[row][3] * m2[3][col];
    for(row=0 ; row<4 ; row++) 
        for(col=0 ; col<4 ; col++)
	    prod[row][col] = temp[row][col];
}

void
  vtransform(vect *v, float mat[4][4], vect *vt)
{
    vect t;

    t.x = v->x*mat[0][0] + v->y*mat[1][0] + v->z*mat[2][0] + v->w*mat[3][0];
    t.y = v->x*mat[0][1] + v->y*mat[1][1] + v->z*mat[2][1] + v->w*mat[3][1];
    t.z = v->x*mat[0][2] + v->y*mat[1][2] + v->z*mat[2][2] + v->w*mat[3][2];
    t.w = v->x*mat[0][3] + v->y*mat[1][3] + v->z*mat[2][3] + v->w*mat[3][3];
    *vt = t;
}

void
  vlerp(vect *v0, vect *v1, vect *v, float p)
{
    v->x = flerp(v0->x,v1->x,p);
    v->y = flerp(v0->y,v1->y,p);
    v->z = flerp(v0->z,v1->z,p);
    v->w = flerp(v0->w,v1->w,p);
}

void
  dvlerp(dvect *v0, dvect *v1, dvect *v, float p)
{
    v->x = dlerp(v0->x,v1->x,p);
    v->y = dlerp(v0->y,v1->y,p);
    v->z = dlerp(v0->z,v1->z,p);
    v->w = dlerp(v0->w,v1->w,p);
}

float 
  flerp(float f0, float f1, float p)
{
    return ((f0*(1.0-p))+(f1*p));
}

double 
  dlerp(double f0, double f1, double p)
{
    return ((f0*(1.0-p))+(f1*p));
}

int
  lerp(int i0, int i1, float p)
{
    return (int)((i0*(1.0-p))+(i1*p));
}

void
  vclamp(vect *v)
{
    if(v->x<0.0)
	v->x = 0.0;
    else if(v->x>1.0)
	v->x = 1.0;
    if(v->y<0.0)
	v->y = 0.0;
    else if(v->y>1.0)
	v->y = 1.0;
    if(v->z<0.0)
	v->z = 0.0;
    else if(v->z>1.0)
	v->z = 1.0;
}

int 
  trinormal(vect *p00, vect *p01, vect *p10, vect *n, float tol )
{
    double tx, ty, tz;
    double Xj, Yj, Zj;
    double Xi, Yi, Zi;
    double mag;

    Xj = p10->x;
    Yj = p10->y;
    Zj = p10->z;
    tx = ty = tz = 0.0;

    Xi = Xj; Xj = p00->x;
    Yi = Yj; Yj = p00->y;
    Zi = Zj; Zj = p00->z;
    tx += (Yi - Yj) * (Zi + Zj);
    ty += (Zi - Zj) * (Xi + Xj);
    tz += (Xi - Xj) * (Yi + Yj);
	
    Xi = Xj; Xj = p01->x;
    Yi = Yj; Yj = p01->y;
    Zi = Zj; Zj = p01->z;
    tx += (Yi - Yj) * (Zi + Zj);
    ty += (Zi - Zj) * (Xi + Xj);
    tz += (Xi - Xj) * (Yi + Yj);
	
    Xi = Xj; Xj = p10->x;
    Yi = Yj; Yj = p10->y;
    Zi = Zj; Zj = p10->z;
    tx += (Yi - Yj) * (Zi + Zj);
    ty += (Zi - Zj) * (Xi + Xj);
    tz += (Xi - Xj) * (Yi + Yj);

    mag = sqrtf(tx*tx + ty*ty + tz*tz);
    if( mag < tol) {
	n->x = 0.0;
	n->y = 0.0;
	n->z = 0.0;
	return 0;
    } else {
	n->x = tx/mag;
	n->y = ty/mag;
	n->z = tz/mag;
	return 1;
    }
}

int 
  dtrinormal( dvect *p00, dvect *p01, dvect *p10, dvect *n, double tol )
{
    double tx, ty, tz;
    double Xj, Yj, Zj;
    double Xi, Yi, Zi;
    double mag;

    Xj = p10->x;
    Yj = p10->y;
    Zj = p10->z;
    tx = ty = tz = 0.0;

    Xi = Xj; Xj = p00->x;
    Yi = Yj; Yj = p00->y;
    Zi = Zj; Zj = p00->z;
    tx += (Yi - Yj) * (Zi + Zj);
    ty += (Zi - Zj) * (Xi + Xj);
    tz += (Xi - Xj) * (Yi + Yj);
	
    Xi = Xj; Xj = p01->x;
    Yi = Yj; Yj = p01->y;
    Zi = Zj; Zj = p01->z;
    tx += (Yi - Yj) * (Zi + Zj);
    ty += (Zi - Zj) * (Xi + Xj);
    tz += (Xi - Xj) * (Yi + Yj);
	
    Xi = Xj; Xj = p10->x;
    Yi = Yj; Yj = p10->y;
    Zi = Zj; Zj = p10->z;
    tx += (Yi - Yj) * (Zi + Zj);
    ty += (Zi - Zj) * (Xi + Xj);
    tz += (Xi - Xj) * (Yi + Yj);

    mag = sqrtf(tx*tx + ty*ty + tz*tz);
    if( mag < tol) {
	n->x = 0.0;
	n->y = 0.0;
	n->z = 0.0;
	return 0;
    } else {
	n->x = tx/mag;
	n->y = ty/mag;
	n->z = tz/mag;
	return 1;
    }
}

static float   idmat[4][4] = {
    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
};

void
  myidentity( float mat[4][4] )
{
  vmatrixcopy(idmat, mat);
}