/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#ifndef _TRSF_H_
#define _TRSF_H_

typedef struct {		/* Basic spatial 3-vector		*/
	float x, y, z;
} VECT, *VECT_PTR;

typedef struct {		/* 4X4 homogenous transform:		*/
	VECT n, o, a, p;	/*   the 4 column vectors of the matrix	*/
} TRSF, *TRSF_PTR;

typedef struct force {		/* Force descriptor with 6 components:	*/
	VECT f, m;		/*   'f' is 3 linear forces, and	*/
} FORCE, *FORCE_PTR;		/*   'm' is 3 moments			*/

typedef struct diff {		/* Descriptor for differential motion:	*/
	VECT t, r;		/*   't' is 3 linear displacements, and */
} DIFF, *DIFF_PTR;		/*   'r' is 3 differential rotations.	*/

typedef struct {		/* Quaternion descriptor		*/
	float c;		/*   'c' - scaler part ( cos(theta/2) ) */
	VECT v;			/*   'v' - vector part ( u * sin(theta/2) )*/
} QTRN, *QTRN_PTR;

typedef struct {               	/* drive parameters:            	*/
	float dx, dy, dz;	/*  translations			*/
	float dthe, dphi, dpsi;	/*  angles				*/
} DRIVEP, *DRIVEP_PTR;

typedef struct {		/* Rotation/position-vector pair  	*/
	VECT p;			/*  translation vector 			*/
	float a;		/*  angle of rotation (radians)		*/
	VECT u;			/*  axis of rotation (0 if theta == 0)  */
} DSPL, *DSPL_PTR;

/* definitions for compatibility with the old system */

#define rot(t,v,h)		rotToTrsf(t,v,h)
#define eul(t,a,b,c)		eulToTrsf(t,a,b,c)
#define rpy(t,a,b,c)		rpyToTrsf(t,a,b,c)
#define trsl(t,x,y,z)		xyzToTrsf(t,x,y,z)
#define vao(t,a,b,c,d,e,f) 	vaoToTrsf(t,a,b,c,d,e,f)
#define rotm(t,v,h)		multTrsfRot(t,v,h)
#define eulm(t,a,b,c)		multTrsfEul(t,a,b,c)
#define rpym(t,a,b,c)		multTrsfRpy(t,a,b,c)
#define trslm(t,x,y,z)		multTrsfXyz(t,x,y,z)
#define vaom(t,a,b,c,d,e,f) 	multTrsfVao(t,a,b,c,d,e,f)
#define takerot(r,t)		copyTrsfNoa(r,t)
#define taketrsl(r,t)		copyTrsfXyz(r,t)
#define noatoeul(a,b,c,t)	trsfToEul(a,b,c,t)
#define noatorpy(a,b,c,t)	trsfToRpy(a,b,c,t)

#ifndef PROTO
#ifndef NO_PROTOTYPES
#define PROTO(x) x
#else
#define PROTO(x) ()
#endif
#endif

#ifndef NO_PROTOTYPES
#include <stdio.h>	/* need FILE* for a prototype */
#endif

extern multQtrn PROTO((QTRN*, QTRN*, QTRN*));
extern multLiQtrn PROTO((QTRN*, QTRN*, QTRN*));
extern multRiQtrn PROTO((QTRN*, QTRN*, QTRN*));
extern invertQtrn PROTO((QTRN*, QTRN*));
extern unitQtrn PROTO((QTRN*, QTRN*));
extern float normQtrn PROTO((QTRN*));

extern TRSF *multTrsf PROTO((TRSF*, TRSF*, TRSF*));
extern TRSF *multLiTrsf PROTO((TRSF*, TRSF*, TRSF*));
extern TRSF *multRiTrsf PROTO((TRSF*, TRSF*, TRSF*));
extern TRSF *invertTrsf PROTO((TRSF*, TRSF*));
extern TRSF *identTrsf PROTO((TRSF*));
extern TRSF *unitTrsf PROTO((TRSF*));

extern TRSF *copyTrsfXyz PROTO((TRSF*, TRSF*));
extern TRSF *copyTrsfNoa PROTO((TRSF*, TRSF*));

extern TRSF *diffToTrsf PROTO((TRSF*, DIFF*));
extern DIFF *trsfToDiff PROTO((DIFF*, TRSF*));

extern DSPL *trsfToDspl PROTO((DSPL*, TRSF*));
extern TRSF *dsplToTrsf PROTO((TRSF*, DSPL*));
extern DSPL *scaleDspl PROTO((DSPL*, double, DSPL*));
extern TRSF *extrapTrsfByDspl PROTO((TRSF*, double, DSPL*));
extern DSPL *unitDspl PROTO((DSPL*, DSPL*));
extern TRSF *eulToTrsf PROTO((TRSF*, double, double, double));
extern TRSF *multTrsfEul PROTO((TRSF*, double, double, double));

extern trsfToEul PROTO((float*, float*, float*, TRSF*));
extern trsfToRpy PROTO((float*, float*, float*, TRSF*));

extern printTrsf PROTO((const char*, TRSF*));
extern fprintTrsf PROTO((FILE*, const char*, TRSF*));
extern TRSF *rotToTrsf PROTO((TRSF*, VECT*, double));
extern TRSF *multTrsfRot PROTO((TRSF*, VECT*, double));
extern trsfToRot PROTO((VECT*, float*, TRSF*));
extern TRSF *rpyToTrsf PROTO((TRSF*, double, double, double));
extern TRSF *multTrsfRpy PROTO((TRSF*, double, double, double));

extern DIFF *transDiff PROTO((DIFF*, DIFF*, TRSF*));
extern FORCE *transForce PROTO((FORCE*, FORCE*, TRSF*));

extern trsfToQtrn PROTO((QTRN*, TRSF*));
extern qtrnToTrsf PROTO((TRSF*, QTRN*));
extern qtrnToVect PROTO((VECT*, QTRN*));

extern TRSF *vaoToTrsf PROTO((TRSF*, double, double, double, 
		double, double, double));
extern TRSF *multTrsfVao PROTO((TRSF*, double, double, double, 
		double, double, double));

extern VECT *crossVect PROTO((VECT*, VECT*, VECT*));
extern VECT *unitVect PROTO((VECT*, VECT*));
extern float dotVect PROTO((VECT*, VECT*));
extern VECT *multTrsfVect PROTO((VECT*, TRSF*, VECT*));
extern VECT *multLiTrsfVect PROTO((VECT*, TRSF*, VECT*));
extern VECT *addVect PROTO((VECT*, VECT*, VECT*));
extern VECT *subtractVect PROTO((VECT*, VECT*, VECT*));
extern VECT *scaleVect PROTO((VECT*, double, VECT*));
extern VECT *combineVect PROTO((VECT*,double,VECT*,double,VECT*));
extern float normVect PROTO((VECT*));
extern VECT *zeroVect PROTO((VECT*));

extern TRSF *xyzToTrsf PROTO((TRSF*, double, double, double));
extern TRSF *multTrsfXyz PROTO((TRSF*, double, double, double));

float cmpTrsf PROTO((TRSF*, TRSF*));

extern rciPrintTrsf PROTO((const char*, TRSF*));

#endif
