/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	gentrans.c
 *
 * DESCRIPTION:	Routines to allocate a transform and initialize it with
 *		values.
 * 
 * AUTHOR/DATE:	Vincent Hayward, Purdue University, 1983;
 *		Hacked upon by John Lloyd for Multi-RCCL since then.
 *
 *------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/rcclDefs.h"

#include "../rccl/transform.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"

extern TRSF *doAllocTrans();

TRSF *allocTransRot (name, pool, tx, ty, tz, vector, h)
const char *name;
int pool;
double tx, ty, tz, h;
VECT *vector;
{
	TRSF *t;
	int ee;
	char *fxnName = "allocTransRot()";

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if ((t = doAllocTrans (name, pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	rotToTrsf (xyzToTrsf (t, tx, ty, tz), vector, h);
	RETURN_NORMAL (t);
}

TRSF *allocTransXyz(name, pool, tx, ty, tz)
const char *name;
int pool;
double tx, ty, tz;
{
	TRSF *t;
	int ee;
	char *fxnName = "allocTransXyz()";

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if ((t = doAllocTrans (name, pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	xyzToTrsf (t, tx, ty, tz);
	RETURN_NORMAL (t);
}

TRSF *allocTransEul(name, pool, tx, ty, tz, phi, the, psi)
const char *name;
int pool;
double tx, ty, tz;
double phi, the, psi;
{
	TRSF *t;
	int ee;
	char *fxnName = "allocTransEul()";

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if ((t = doAllocTrans (name, pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	eulToTrsf (xyzToTrsf(t, tx, ty, tz), phi, the, psi);
	RETURN_NORMAL(t);
}

TRSF *allocTransRpy(name, pool, tx, ty, tz, roll, pitch, yaw)
const char *name;
int pool;
double tx, ty, tz;
double roll, pitch, yaw;
{
	TRSF *t;
	int ee;
	char *fxnName = "allocTransRpy()";

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if ((t = doAllocTrans (name, pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	rpyToTrsf (xyzToTrsf (t, tx, ty, tz), roll, pitch, yaw);
	RETURN_NORMAL (t);
}

TRSF *allocTransPao(name, pool, px, py, pz, ax, ay, az, ox, oy, oz)
const char *name;
int pool;
double px, py, pz, ax, ay, az, ox, oy, oz;
{
	TRSF *t;
	int ee;
	char *fxnName = "allocTransPao()";

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if ((t = doAllocTrans (name, pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	vaoToTrsf (xyzToTrsf (t, px, py, pz), ax,ay,az,ox,oy,oz);
	RETURN_NORMAL (t);
}
