/*


Linear algebra.

*/

/****************************************************************************
    This file is part of the FreeWRL/FreeX3D Distribution.

    Copyright 2009 CRC Canada. (http://www.crc.gc.ca)

    FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    FreeWRL/FreeX3D 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 FreeWRL/FreeX3D.  If not, see <http://www.gnu.org/licenses/>.
****************************************************************************/


#ifndef __FREEWRL_LINEAR_ALGEBRA_H__
#define __FREEWRL_LINEAR_ALGEBRA_H__


#define VECSQ(a) VECPT(a,a)
#define VECPT(a,b) ((a).x*(b).x + (a).y*(b).y + (a).z*(b).z)
#define VECDIFF(a,b,c) {(c).x = (a).x-(b).x;(c).y = (a).y-(b).y;(c).z = (a).z-(b).z;}
#define VECADD(a,b) {(a).x += (b).x; (a).y += (b).y; (a).z += (b).z;}
#define VEC_FROM_CDIFF(a,b,r) {(r).x = (a).c[0]-(b).c[0];(r).y = (a).c[1]-(b).c[1];(r).z = (a).c[2]-(b).c[2];}
#define VECCP(a,b,c) {(c).x = (a).y*(b).z-(b).y*(a).z; (c).y = -((a).x*(b).z-(b).x*(a).z); (c).z = (a).x*(b).y-(b).x*(a).y;}
#define VECSCALE(a,c) {(a).x *= c; (a).y *= c; (a).z *= c;}
#define VECCOPY(a,b) {(a).x = (b).x; (a).y = (b).y; (a).z = (b).z;}

/*special case ; used in Extrusion.GenPolyRep and ElevationGrid.GenPolyRep:
 *	Calc diff vec from 2 coordvecs which must be in the same field 	*/
#define VEC_FROM_COORDDIFF(f,a,g,b,v) {\
	(v).x= (f)[(a)*3]-(g)[(b)*3];	\
	(v).y= (f)[(a)*3+1]-(g)[(b)*3+1];	\
	(v).z= (f)[(a)*3+2]-(g)[(b)*3+2];	\
}

/* rotate a vector along one axis				*/
#define VECROTATE_X(c,angle) { \
	/*(c).x =  (c).x	*/ \
	  (c).y = 		  cos(angle) * (c).y 	- sin(angle) * (c).z; \
	  (c).z = 		  sin(angle) * (c).y 	+ cos(angle) * (c).z; \
	}
#define VECROTATE_Y(c,angle) { \
	  (c).x = cos(angle)*(c).x +			+ sin(angle) * (c).z; \
	/*(c).y = 				(c).y 	*/ \
	  (c).z = -sin(angle)*(c).x 			+ cos(angle) * (c).z; \
	}
#define VECROTATE_Z(c,angle) { \
	  (c).x = cos(angle)*(c).x - sin(angle) * (c).y;	\
	  (c).y = sin(angle)*(c).x + cos(angle) * (c).y; 	\
	/*(c).z = s						 (c).z; */ \
	}

#define MATRIX_ROTATION_X(angle,m) {\
	m[0][0]=1; m[0][1]=0; m[0][2]=0; \
	m[1][0]=0; m[1][1]=cos(angle); m[1][2]=- sin(angle); \
	m[2][0]=0; m[2][1]=sin(angle); m[2][2]=cos(angle); \
}
#define MATRIX_ROTATION_Y(angle,m) {\
	m[0][0]=cos(angle); m[0][1]=0; m[0][2]=sin(angle); \
	m[1][0]=0; m[1][1]=1; m[1][2]=0; \
	m[2][0]=-sin(angle); m[2][1]=0; m[2][2]=cos(angle); \
}
#define MATRIX_ROTATION_Z(angle,m) {\
	m[0][0]=cos(angle); m[0][1]=- sin(angle); m[0][2]=0; \
	m[1][0]=sin(angle); m[1][1]=cos(angle); m[1][2]=0; \
	m[2][0]=0; m[2][1]=0; m[2][2]=1; \
}

/* next matrix calculation comes from comp.graphics.algorithms FAQ	*/
/* the axis vector has to be normalized					*/
#define MATRIX_FROM_ROTATION(ro,m) { \
	struct { double x,y,z,w ; } __q; \
        double sinHalfTheta = sin(0.5*(ro.c[3]));\
        double xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;\
        __q.x = (ro.c[0])*sinHalfTheta;\
        __q.y = (ro.c[1])*sinHalfTheta;\
        __q.z = (ro.c[2])*sinHalfTheta;\
        __q.w = cos(0.5*(ro.c[3]));\
        xs = 2*__q.x;  ys = 2*__q.y;  zs = 2*__q.z;\
        wx = __q.w*xs; wy = __q.w*ys; wz = __q.w*zs;\
        xx = __q.x*xs; xy = __q.x*ys; xz = __q.x*zs;\
        yy = __q.y*ys; yz = __q.y*zs; zz = __q.z*zs;\
        m[0][0] = 1 - (yy + zz); m[0][1] = xy - wz;      m[0][2] = xz + wy;\
        m[1][0] = xy + wz;       m[1][1] = 1 - (xx + zz);m[1][2] = yz - wx;\
        m[2][0] = xz - wy;       m[2][1] = yz + wx;      m[2][2] = 1-(xx + yy);\
}

/* matrix multiplication */
#define VECMM(m,c) { \
	double ___x=(c).x,___y=(c).y,___z=(c).z; \
	(c).x= m[0][0]*___x + m[0][1]*___y + m[0][2]*___z; \
	(c).y= m[1][0]*___x + m[1][1]*___y + m[1][2]*___z; \
	(c).z= m[2][0]*___x + m[2][1]*___y + m[2][2]*___z; \
}


/* next define rotates vector c with rotation vector r and angle */
/*  after section 5.8 of the VRML`97 spec			 */

#define VECROTATE(rx,ry,rz,angle,nc) { \
	double ___x=(nc).x,___y=(nc).y,___z=(nc).z; \
	double ___c=cos(angle),  ___s=sin(angle), ___t=1-___c; \
	(nc).x=   (___t*((rx)*(rx))+___c)     *___x    \
	        + (___t*(rx)*(ry)  -___s*(rz))*___y    \
	        + (___t*(rx)*(rz)  +___s*(ry))*___z ;  \
	(nc).y=   (___t*(rx)*(ry)  +___s*(rz))*___x    \
	        + (___t*((ry)*(ry))+___c)     *___y    \
	        + (___t*(ry)*(rz)  -___s*(rx))*___z ;  \
	(nc).z=   (___t*(rx)*(rz)  -___s*(ry))*___x    \
	        + (___t*(ry)*(rz)  +___s*(rx))*___y    \
	        + (___t*((rz)*(rz))+___c)     *___z ;  \
	}


/*
#define VECROTATE(rx,ry,rz,angle,c) { \
	double ___c=cos(angle),  ___s=sin(angle), ___t=1-___c; \
	(c).x=   (___t*((rx)*(rx))+___c)     *(c).x    \
	       + (___t*(rx)*(ry)  +___s*(rz))*(c).y    \
	       + (___t*(rx)*(rz)  -___s*(ry))*(c).z ;  \
	(c).y=   (___t*(rx)*(ry)  -___s*(rz))*(c).x    \
	       + (___t*((ry)*(ry))+___c)     *(c).y    \
	       + (___t*(ry)*(rz)  +___s*(rx))*(c).z ;  \
	(c).z=   (___t*(rx)*(rz)  +___s*(ry))*(c).x    \
	       + (___t*(ry)*(rz)  -___s*(rx))*(c).y    \
	       + (___t*((rz)*(rz))+ ___c)    *(c).z ;  \
	}

*/
/* next define abbreviates VECROTATE with use of the SFRotation struct	*/
#define VECRROTATE(ro,c) VECROTATE((ro).c[0],(ro).c[1],(ro).c[2],(ro).c[3],c)


#define calc_vector_length(pt) veclength(pt)

float veclength( struct point_XYZ p );

/* returns vector length, too */
GLDOUBLE vecnormal(struct point_XYZ*r, struct point_XYZ* v);

#define normalize_vector(pt) vecnormal(pt,pt)

float calc_angle_between_two_vectors(struct point_XYZ a, struct point_XYZ b);

double vecangle(struct point_XYZ* V1, struct point_XYZ* V2);


#define calc_vector_product(a,b,c) veccross(c,a,b);

void veccross(struct point_XYZ *c , struct point_XYZ a, struct point_XYZ b);


double * vecaddd(double *c, double *a, double *b);
double *vecdifd(double *c, double* a, double *b);
double * veccrossd(double *c, double *a, double *b);
double veclengthd( double *p );
double vecdotd(double *a, double *b);
double* vecscaled(double* r, double* v, double s);
double vecnormald(double *r, double *v);
double *veccopyd(double *c, double *a);
double *vecnegated(double *b, double *a);

double * vecadd2d(double *c, double *a, double *b);
double *vecdif2d(double *c, double* a, double *b);
double veclength2d( double *p );
double vecdot2d(double *a, double *b);
double* vecscale2d(double* r, double* v, double s);
double vecnormal2d(double *r, double *v);

float *veccopy2f(float *b, float *a);
float * vecadd2f(float *c, float *a, float *b);
float *vecdif2f(float *c, float* a, float *b);
float veclength2f( float *p );
float vecdot2f(float *a, float *b);
float* vecscale2f(float* r, float* v, float s);
float vecnormal2f(float *r, float *v);
float *vecmult2f(float *c, float *a, float *b);

float *veccopy3f(float *b, float *a);
float *vecset3f(float *b, float x, float y, float z);
float *vecadd3f(float *c, float *a, float *b);
float *vecdif3f(float *c, float *a, float *b);
float vecdot3f(float *a, float *b);
float *veccross3f(float *c, float *a, float *b);
float *vecscale3f(float *b, float *a, float scale);
float *vecmult3f(float *c, float *a, float *b);
float veclength3f(float *a);
float *vecnormalize3f(float *b, float *a);
float det3f(float *a, float *b, float *c);
float *axisangle_rotate3f(float* b, float *a, float *axisangle);
BOOL line_intersect_line_3f(float *p1, float *v1, float *p2, float *v2, float *t, float *s, float *x1, float *x2);
BOOL line_intersect_planed_3f(float *p, float *v, float *N, float d, float *pi, float *t);
BOOL line_intersect_plane_3f(float *p, float *v, float *N, float *pp, float *pi, float *t);
BOOL line_intersect_cylinder_3f(float *p, float *v, float radius, float *pi);

float vecdot4f( float *a, float *b );
float *vecscale4f(float *b, float *a, float scale);

GLDOUBLE det3x3(GLDOUBLE* data);

struct point_XYZ* transform(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
float* transformf(float* r, const float* a, const GLDOUBLE* b);

float* matmultvec3f(float* r3, float *mat3, float* a3 );
float* vecmultmat3f(float* r3, float* a3, float *mat3 );
BOOL matrix3x3_inverse_float(float *inn, float *outt);
float* vecmultmat4f(float* r4, float *a4, float *mat4);
float* matmultvec4f(float* r4, float *mat4, float* a4 );


struct point_XYZ* transformAFFINE(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
GLDOUBLE* pointxyz2double(double* r, struct point_XYZ *p); /* instead of casting struct to array, this is more rigorous */
struct point_XYZ* double2pointxyz(struct point_XYZ* r, double* p); /* ditto */
double *transformAFFINEd(double *r, double *a, const GLDOUBLE* mat); /* same as transformAFFINE which is the same as transform() - just different parameter types */

/*only transforms using the rotation component.
  Usefull for transforming normals, and optimizing when you know there's no translation */
struct point_XYZ* transform3x3(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);

struct point_XYZ* vecscale(struct point_XYZ* r, struct point_XYZ* v, GLDOUBLE s);

double vecdot(struct point_XYZ* a, struct point_XYZ* b);

#define calc_vector_scalar_product(a,b) vecdot(&(a),&(b))

double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2);

struct point_XYZ* vecadd(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);

struct point_XYZ* vecdiff(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);

/*specify a direction "n", and you get two vectors i, and j, perpendicular to n and themselfs. */
void make_orthogonal_vector_space(struct point_XYZ* i, struct point_XYZ* j, struct point_XYZ n);

GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* m);
GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* m);
GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* m);

float* matinverse4f(float* res, float* mm);
GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* m);
float* mattranspose4f(float* res, float* mm);
float *matidentity3f(float *b);
float* matmultiply3f(float* r, float* mm , float* nn);
float* mattranspose3f(float* res, float* mm);

struct point_XYZ* polynormal(struct point_XYZ* r, struct point_XYZ* p1, struct point_XYZ* p2, struct point_XYZ* p3);
/*simple wrapper for now. optimize later */
struct point_XYZ* polynormalf(struct point_XYZ* r, float* p1, float* p2, float* p3);

GLDOUBLE* matrotate(GLDOUBLE* Result, double Theta, double x, double y, double z);

/*rotates dv back on iv*/
double matrotate2v(GLDOUBLE* res, struct point_XYZ iv/*original*/, struct point_XYZ dv/*result*/);
void rotate_v2v_axisAngled(double* axis, double* angle, double *orig, double *result);

GLDOUBLE* mattranslate(GLDOUBLE* r, double dx, double dy, double dz);

GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
float* matmultiply4f(float* r, float* mm , float* nn);
float *axisangle2matrix4f(float *b, float *axisangle);
float *matidentity4f(float *b);
void matrixFromAxisAngle4d(double *mat, double rangle, double x, double y, double z);

void scale_to_matrix (double *mat, struct point_XYZ *scale);
void loadIdentityMatrix (double *mat);
double *matcopy(double *r, double*mat);
void printmatrix2(GLDOUBLE* mat,char* description );
void printmatrix3(GLDOUBLE *mat, char *description, int row_major);
void general_slerp(double *ret, double *p1, double *p2, int size, const double t);
void point_XYZ_slerp(struct point_XYZ *ret, struct point_XYZ *p1, struct point_XYZ *p2, const double t);
#endif /* __FREEWRL_LINEAR_ALGEBRA_H__ */
