//*****************************************************************************************
// Truevision - a 3d modeler for gnome and povray
//
// utils3d.cc
//
// Vincent LE PRINCE <vincentleprince@users.sourceforge.net>
// Copyright (C) 2000-2005 Vincent LE PRINCE
// This file is part of the TRUEVISION Package

//   This program is free software; you can redistribute it and/or modify
//   it under the terms of the GNU General Public License as published by
//   the Free Software Foundation; either version 2 of the License, or
//   (at your option) any later version.
//
//   This program is distributed in the hope that it will be useful,
//   but WITHOUT ANY WARRANTY; without even the implied warranty of
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//   GNU General Public License for more details.
//
//   You should have received a copy of the GNU General Public License
//   along with this program; if not, write to the Free Software
//   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.  */ 
//*******************************************************************************************
#include "include/utils3d.h"
#include <GL/gl.h>
#include "math.h"


void normalize( tv_point & pt )
{
float norm = sqrt( pt[0]*pt[0] + pt[1]*pt[1] + pt[2]*pt[2] );
if ( norm == 0 ) return;
pt[0] /= norm;
pt[1] /= norm;
pt[2] /= norm;
}

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


void prod_matrix_matrix( tv_matrix & m1, tv_matrix & m2, tv_matrix & dest )
{
for ( int i = 0 ; i < 4 ; i++ )
	for ( int j = 0 ; j < 4 ; j++ )
		{
		dest[i][j] = 0;
		for ( int k = 0 ; k < 4 ; k++ )
			dest[i][j] += m1[i][k] * m2[k][j];
		}		
}

void prod_matrix_vector( tv_matrix & m1, tv_vector & v1,  tv_vector & dest )
{
for ( int i = 0 ; i < 4 ; i++ )
	{
	dest[i] = 0;
	for ( int k = 0 ; k < 4 ; k++ )
		dest[i] += m1[i][k] * v1[k];
	}
}


void prod_vector_matrix( tv_vector & v1, tv_matrix & m1,  tv_vector & dest )
{
for ( int i = 0 ; i < 4 ; i++ )
	{
	dest[i] = 0;
	for ( int k = 0 ; k < 4 ; k++ )
		dest[i] += m1[k][i] * v1[k];
	}
}

float prod_vector_vector( tv_vector & v1, tv_vector & v2 )
{
float res = 0;
for ( int i = 0 ; i < 4 ; i++ )
	res += v1[i] * v2[i];
return res;
}

void prod_vector( tv_point & v1, tv_point & v2,  tv_point & dest )
{
dest[0] = v1[1] * v2[2] - v1[2]*v2[1];
dest[1] = v1[2] * v2[0] - v1[0]*v2[2];
dest[2] = v1[0] * v2[1] - v1[1]*v2[0];	
}


float square( float a )  { return a*a; }


void get_normal( float ax, float ay, float az, float bx, float by, float bz, float cx, float cy, float cz, bool invert )
{
float T1[3], T2[3];
T1[0] = ax-bx;  T1[1] = ay-by;  T1[2] = az-bz;
T2[0] = bx-cx;  T2[1] = by-cy;  T2[2] = bz-cz;
if ( !invert ) glNormal3f( T1[1]*T2[2] - T1[2]*T2[1], T1[2]*T2[0] - T1[0]*T2[2], T1[0]*T2[1] - T1[1]*T2[0] );
else glNormal3f(  -T1[1]*T2[2] + T1[2]*T2[1], -T1[2]*T2[0] + T1[0]*T2[2], -T1[0]*T2[1] + T1[1]*T2[0] );
}

void get_normal( float *a, float *b, float *c ) 
{ 
get_normal( a[0], a[1], a[2], b[0], b[1], b[2], c[0], c[1], c[2] ); 
}

void get_normal( float *a, float *b, float *c, bool invert ) 
{ 
float T1[3], T2[3], N[3];
for ( int i = 0 ; i < 3 ; i++ ) { T1[i] = a[i] - b[i]; T2[i] = b[i] - c[i]; }

N[0] = T1[1]*T2[2] - T1[2]*T2[1];
N[1] = T1[2]*T2[0] - T1[0]*T2[2];
N[2] = T1[0]*T2[1] - T1[1]*T2[0];
float coef = sqrt( N[0]*N[0] +  N[1]*N[1] +  N[2]*N[2] );
for ( int i = 0 ; i < 3 ; i++ ) N[i] /= coef;

if ( !invert ) glNormal3f( N[0],  N[1], N[2] );
else glNormal3f( -N[0],  -N[1], -N[2] );
}


void get_normal( float *a, float *b, float *c, float *d, float *N, bool invert )
{
float T1[3], T2[3];
for ( int i = 0 ; i < 3 ; i++ ) { T1[i] = a[i] - b[i]; T2[i] = b[i] - c[i]; }

N[0] = T1[1]*T2[2] - T1[2]*T2[1];
N[1] = T1[2]*T2[0] - T1[0]*T2[2];
N[2] = T1[0]*T2[1] - T1[1]*T2[0];

float coef = sqrt( N[0]*N[0] +  N[1]*N[1] +  N[2]*N[2] );
for ( int i = 0 ; i < 3 ; i++ ) N[i] /= coef;

float d1 = 0, d2 = 0;
for ( int i = 0 ; i < 3 ; i++ )
	{
	T1[i] = a[i] + N[i];
	T2[i] = a[i] - N[i];
	d1 += ( T1[i] - d[i] ) * ( T1[i] - d[i] );
	d2 += ( T2[i] - d[i] ) * ( T2[i] - d[i] );
	}

if ( d1 <= d2 ) for ( int i = 0 ; i < 3 ; i++ ) N[i] = -N[i];
}