/*
* File: points.c
* Project: DXF to RIB
* (c) SPDsoft Tuesday, June 21, 1994, GTIC
*/
#include <math.h>
#include "points.h"
Point3D ElEjeDeGiro;
/*_______________________________________________*/
/* Devuelve el modulo del argumento */
Float ModV(Point3D vect)
{
return((Float)pow((double)
( vect[0]*vect[0]+
vect[1]*vect[1]+
vect[2]*vect[2] ),0.5));
}
/*_______________________________________________*/
/* Deja sobre el segundo argumento el primero
normalizado */
void NormV(Point3D vect_i, Point3D vect_o)
{
Float aux = ModV(vect_i);
vect_o[0] = vect_i[0] / aux;
vect_o[1] = vect_i[1] / aux;
vect_o[2] = vect_i[2] / aux;
}
/*_______________________________________________*/
/* Deja sobre el segundo argumento el primero */
void CopV(Point3D vect_i, Point3D vect_o)
{
vect_o[0] = vect_i[0];
vect_o[1] = vect_i[1];
vect_o[2] = vect_i[2];
}
/*_______________________________________________*/
/* Deja sobre el tercer argumento la suma
de los dos primeros */
void SumV(Point3D vect_1,Point3D vect_2,Point3D vect_o)
{
vect_o[0] = vect_1[0] + vect_2[0];
vect_o[1] = vect_1[1] + vect_2[1];
vect_o[2] = vect_1[2] + vect_2[2];
}
/*_______________________________________________*/
/* Deja sobre el tercer argumento la resta
de los dos primeros */
void ResV(Point3D vect_1,Point3D vect_2,Point3D vect_o)
{
vect_o[0] = vect_1[0] - vect_2[0];
vect_o[1] = vect_1[1] - vect_2[1];
vect_o[2] = vect_1[2] - vect_2[2];
}
/*_______________________________________________*/
/* Deja sobre el tercer argumento el producto
vectorial de los dos primeros */
void ProdvV(Point3D vect_1,Point3D vect_2,Point3D vect_o)
{
vect_o[0] = vect_1[1] * vect_2[2] -
vect_1[2] * vect_2[1] ;
vect_o[1] = vect_1[2] * vect_2[0] -
vect_1[0] * vect_2[2] ;
vect_o[2] = vect_1[0] * vect_2[1] -
vect_1[1] * vect_2[0] ;
}
/*_______________________________________________*/
/* Devuelve el producto escalar de los dos
argumentos */
Float ProdeV(Point3D vect_1,Point3D vect_2)
{
return((Float)( vect_1[0] * vect_2[0] +
vect_1[1] * vect_2[1] +
vect_1[2] * vect_2[2] ));
}
/*_______________________________________________*/
/* Deja sobre el tercer argumento el producto de
el primer argumento ( escalar ) por el segundo
(vector) */
void ConsV(Float k,Point3D vect_i,Point3D vect_o)
{
vect_o[0] = vect_i[0] * k;
vect_o[1] = vect_i[1] * k;
vect_o[2] = vect_i[2] * k;
}
/*_______________________________________________*/
/* Devuelve el angulo ( en radianes ) entre
los dos argumentos */
Float AngrV(Point3D vect_1,Point3D vect_2)
{
Float s2,c2;
ProdvV(vect_1,vect_2,ElEjeDeGiro);
/* ElEjeDeGiro <- v1 * v2 */
s2 = ModV(ElEjeDeGiro)/ModV(vect_1)/ModV(vect_2);
/* seno */
c2 = ProdeV(vect_1,vect_2)/ModV(vect_1)/ModV(vect_2);
/* coseno */
return((Float)atan2(s2,c2)); /* 0->M_PI */
}
/*_______________________________________________*/
/* Devuelve el angulo ( en grados ) entre
los dos argumentos */
Float AnggV(Point3D vect_1,Point3D vect_2)
{
return((Float)180.0/M_PI*AngrV(vect_1,vect_2));
}
/*_______________________________________________*/
/* Devuelve la distancia entre 2 puntos */
Float DistV(Point3D vect_1,Point3D vect_2)
{
Point3D tmp;
ResV( vect_1, vect_2, tmp );
return( ModV(tmp));
}
/*_______________________________________________*/
void printfV(int id,Point3D vect)
{
printf("\n\t %d: x= %lf : y = %lf : z = %lf \n"
, id , vect[0] , vect[1] , vect[2] );
}
/*_______________________________________________*/
void PEsfAV(PEsf *pesfi,Point3D vecto)
{
vecto[0] = pesfi->Radio
* SIN( pesfi->Long )
* COS( pesfi->Lat );
vecto[1] = pesfi->Radio
* SIN( pesfi->Long )
* SIN( pesfi->Lat );
vecto[2] = pesfi->Radio
* COS( pesfi->Long );
}
/*_______________________________________________*/
void VAPEsf( Point3D vecti, PEsf *pesfo )
{
pesfo->Radio=(Float)pow((double)
( vecti[0]*vecti[0]+
vecti[1]*vecti[1]+
vecti[2]*vecti[2] ),0.5);
pesfo->Lat=(Float)atan2( (double) vecti[1],
(double) vecti[0] );
if ( pesfo->Lat < 0 ) pesfo->Lat += _2_M_PI;
pesfo->Long = (Float) acos ( (double)(vecti[2]/pesfo->Radio));
if ( pesfo->Long < 0 ) pesfo->Long = M_PI_2 - pesfo->Long;
}
/******************************************************************************/
void Identity2D(Matrix2D M) /* return matrix */
{
int I, J; /* loop control variables */
for (I = 0; I < 3; I++)
for (J = 0; J < 2; J++ )
M[I][J] = (Float) I == J;
}
/******************************************************************************/
void Scale2D(Matrix2D M, Float Sx, Float Sy)
{
Identity2D(M);
M[0][0] = Sx;
M[1][1] = Sy;
}
/******************************************************************************/
void Translate2D(Matrix2D M, Float Tx, Float Ty)
{
Identity2D(M);
M[2][0] = Tx;
M[2][1] = Ty;
}
/******************************************************************************/
void Rotate2D(Matrix2D M, Float Theta)
{
Identity2D(M);
M[0][0] = COS(Theta);
M[0][1] = -SIN(Theta);
M[1][0] = -M[0][1];
M[1][1] = M[0][0];
}
/******************************************************************************/
void Shear2D(Matrix2D M, Float Sx, Float Sy)
{
Identity2D(M);
M[1][0] = Sx;
M[0][1] = Sy;
}
/******************************************************************************/
void Identity3D(Matrix M)
{
int i, j;
for (i = 0; i <= 3; i++)
for (j = 0; j <= 3; j++)
M[i][j] = (Float) i == j;
}
/******************************************************************************/
void Scale3D(Float Sx, Float Sy, Float Sz, Matrix M)
{
Identity3D(M);
M[0][0] = Sx;
M[1][1] = Sy;
M[2][2] = Sz;
}
/******************************************************************************/
void Translate3D(Float Tx, Float Ty, Float Tz, Matrix M)
{
Identity3D(M);
M[3][0] = Tx;
M[3][1] = Ty;
M[3][2] = Tz;
}
/******************************************************************************/
void Rotate3D(
char Axis, /* axis about which to rotate */
Float Angle, /* how much to rotate */
Matrix M /* return matrix */
)
{
int top, bottom, left, right;
Identity3D(M);
if (Axis >= 'a' && Axis <= 'z')
Axis -= 32; /* convert to uppercase */
left = (Axis == 'X' ? 1 : 0);
right = (Axis == 'Z' ? 1 : 2);
top = (Axis == 'X' ? 1 : 0);
bottom = (Axis == 'Z' ? 1 : 2);
M[top ][left ] = COS(Angle);
M[top ][right] = SIN(Angle);
if (Axis != 'Y')
M[top ][right] = -M[top][right];
M[bottom][left ] = -M[top][right];
M[bottom][right] = M[top][left ];
}
/******************************************************************************/
void Shear3D(
Float Sxy,
Float Sxz,
Float Syx,
Float Syz,
Float Szx,
Float Szy,
Matrix M /* return matrix */
)
{
Identity3D(M);
M[1][0] = Sxy;
M[2][0] = Sxz;
M[0][1] = Syx;
M[2][1] = Syz;
M[0][2] = Szx;
M[1][2] = Szy;
}
/******************************************************************************/
void Perspective3D(
Float VPx, Float VPy, Float VPz, /* vanishing points */
Float OverallScaling, /* overall scaling */
Matrix M /* return matrix */
)
{
Identity3D(M);
M[0][3] = 1 / VPx;
M[1][3] = 1 / VPy;
M[2][3] = 1 / VPz;
M[2][2] = 0.0; /* project onto Z=0 plane */
M[3][3] = OverallScaling;
}
/*===========================================================================*/
void ProdM3DV( Matrix M, Point3D V, Float D4th, Point3D Result )
{
int i,j;
for( i=0; i<3 ; i++ )
{
for( j=0,Result[i]=0.0; j<3 ; j++ )
Result[i] += M[j][i]*V[j];
Result[i] += M[3][i]*D4th;
}
}
/*===========================================================================*/
void ProdM4DV( Matrix M, Point4D V, Point4D Result )
{
int i,j;
for( i=0; i<4 ; i++ )
for( j=0,Result[i]=0.0; j<4 ; j++ )
Result[i] += M[j][i]*V[j];
}
/*===========================================================================*/
void ProdM3DM( Matrix Mio, Matrix M )
{
int k,i,j;
Matrix Mtmp;
Float *pfio, *pftmp;
for( k=0; k<4 ; k++ )
for( i=0; i<4 ; i++ )
for( j=0,Mtmp[k][i]=0.0; j<4 ; j++ )
Mtmp[k][i] += Mio[j][i]*M[k][j];
for( pfio=(Float*)Mio ,pftmp=(Float*)Mtmp ; pfio<((Float*)Mio+16);*pfio++ = *pftmp++)
;
}