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

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	transFunc
 *
 * DESCRIPTION:	Functions to implement the robot specific operations.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1987
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include "cdefs.h"
#include "types.h"
#include "error.h"

matrix *talloc_homo();
matrix *talloc_matrix();

rotTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *m1, *mr;
	double v[3], d;
 
	if (a[0].type == MATRIX)
	 { m1 = a[0].val.m;
	   if (!(m1->row == 1 && m1->col >= 4) &&
 	       !(m1->col == 1 && m1->row >= 4))
	    { 
error (0, "%s() takes a 4-vector or a scalar and a 3-vector for input\n",
	      r->name);
	      return (-1);
	    }
 	   if (unitVector(v, &m1->buf[1], 3) == -1)
	    { error (0, "%s() -- zero vector detected\n", r->name);
	      return (-1);
	    }
	   d = m1->buf[0];
	 }
 	else
	 { m1 = a[1].val.m;
	   if (!(m1->row == 1 && m1->col >= 3) &&
 	       !(m1->col == 1 && m1->row >= 3))
	    { 
error (0, "%s() takes a 4-vector or a scalar and a 3-vector for input\n",
	      r->name);
	      return (-1);
	    }
 	   if (unitVector(v, m1->buf, 3) == -1)
	    { error (0, "%s() -- zero vector detected\n", r->name);
	      return (-1);
	    }
	   d = a[0].val.d;
	 }
	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	if (rotToTrsf (mr->buf, v, d) == -1)
 	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

eulTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;

	m1 = a[0].val.m;
	if ((m1->row != 1 || m1->col != 3) &&
	    (m1->row != 3 || m1->col != 1))
	 { error (0, "%s() -- 3 vector required for input\n", r->name);
	   return (-1);
	 }

	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	if (eulToTrsf (mr->buf, m1->buf[0], m1->buf[1], m1->buf[2]) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

rpyTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;

	m1 = a[0].val.m;
	if ((m1->row != 1 || m1->col != 3) &&
	    (m1->row != 3 || m1->col != 1))
	 { error (0, "%s() -- 3 vector required for input\n", r->name);
	   return (-1);
	 }

	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	if (rpyToTrsf (mr->buf, m1->buf[0], m1->buf[1], m1->buf[2]) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

trslTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr;

	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	mr->buf[3] =  a[0].val.d;
	mr->buf[7] =  a[1].val.d;
	mr->buf[11] = a[2].val.d;
	
	r->val.m = mr;
	return (1);
}

quaternionTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *m1, *mr;
	double q[4];

	m1 = a[0].val.m;
		
	if (!(m1->row == 1 && m1->col >= 4) &&
	    !(m1->col == 1 && m1->row >= 4))
	 { error (0, "%s() -- non-quaternion input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	if (unitVector (q, m1->buf, 4) == -1)
	 { error (0, "%s() -- zero vector detected\n", r->name);
	   return (-1);
	 }
	if (qtrnToTrsf (mr->buf, q) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

vaoTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *mo, *ma;

	ma = a[0].val.m;
	mo = a[1].val.m;

	if (!(mo->row == 1 && mo->col >= 3) &&
	    !(mo->col == 1 && mo->row >= 3))
	 { error (0, "%s() requires 2 3-vectors for input\n", r->name);
	   return (-1);
	 }
	if (!(ma->row == 1 && ma->col >= 3) &&
	    !(ma->col == 1 && ma->row >= 3))
	 { error (0, "%s() requires 2 3-vectors for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	if (vaoToTrsf (mr->buf, ma->buf, mo->buf) == -1)
	 { error (0, "%s() -- zero input vectors\n", r->name);
	   return (-1);
	 }
	r->val.m = mr;
	return (1);
}

transToRot (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *t1;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_matrix (1, 4)) == NULL)
	 { return (-1);
	 }
	if (noatorot (mr->buf, t1->buf) == -1)
	 { error (0, "%s() -- divide by zero detected\n", r->name);
	   return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

transToRot2 (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *t1;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_matrix (1, 4)) == NULL)
	 { return (-1);
	 }
	if (noatorot2 (mr->buf, t1->buf) == -1)
	 { error (0, "%s() -- divide by zero detected\n", r->name);
	   return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

transToRpy (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *t1;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_matrix (1, 3)) == NULL)
	 { return (-1);
	 }
	if (trsfToRpy (mr->buf, t1->buf) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

transToEul (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *t1;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_matrix (1, 3)) == NULL)
	 { return (-1);
	 }
	if (trsfToEul (mr->buf, t1->buf) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

transToQuaternion (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *t1;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_matrix (1, 4)) == NULL)
	 { return (-1);
	 }
	if (trsfToQtrn (mr->buf, t1->buf) == -1)
	 { error (0, "%s() -- divide by zero detected\n", r->name);
	   return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

forceTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *t1, *tr;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	 }
	if ((tr = talloc_matrix (6,6)) == NULL)
	 { return (-1);
	 }
 	ftrans (tr, t1);
	r->val.m = tr;
	return (1);
}

velocityTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{	
	matrix *t1, *tr;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	 }
	if ((tr = talloc_matrix (6,6)) == NULL)
	 { return (-1);
	 }
 	vtrans (tr, t1);
	r->val.m = tr;
	return (1);
}

crossProduct (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1, *m2;
	int leftVector = 0;
	int rightVector = 0;
	int j;

	/* find at least one three vector */	

	m1 = a[0].val.m;
	m2 = a[1].val.m;
	if ((m1->row == 1 && m1->col == 3) ||
	    (m1->row == 3 && m1->col == 1))
	 { leftVector = 1;
	 }
	if ((m2->row == 1 && m2->col == 3) ||
	    (m2->row == 3 && m2->col == 1))
	 { rightVector = 1;
	 }
	if (!leftVector && !rightVector)
	 { error (0, "%s() requires at least 1 3-vector for input\n",r->name);
	 }
	else if (leftVector && !rightVector)
	 { if (m2->row != 3)
	    { error (0,"%s() -- row dimension of matrix must be 3\n",r->name);
	      return (-1);
	    }
	   if ((mr = talloc_matrix (3, m2->col))==NULL)
	    { return (-1);
	    }
	   transpose_matrix (mr, m2);
	   for (j=0; j<m2->col; j++)
	    { crossVector (&m2->buf[j*3], m1->buf, &mr->buf[j*3]);
	    }
	   m2->row = m2->col;
	   m2->col = 3;
	   transpose_matrix (mr, m2);
	 }
 	else if (rightVector && !leftVector)
	 { if (m1->row != 3)
	    { error (0,"%s() -- row dimension of matrix must be 3\n",r->name);
	      return (-1);
	    }
	   if ((mr = talloc_matrix (3, m1->col))==NULL)
	    { return (-1);
	    }
	   transpose_matrix (mr, m1);
	   for (j=0; j<m1->col; j++)
	    { crossVector (&m1->buf[j*3], &mr->buf[j*3], m2->buf);
	    }
	   m1->row = m1->col;
	   m1->col = 3;
	   transpose_matrix (mr, m1);
	 }
	else
 	 { if ((mr = talloc_matrix (3, 1))==NULL)
	    { return (-1);
	    }
	   crossVector (mr->buf, m1->buf, m2->buf);
	 }
	r->val.m = mr;
	return (1);
}

matrix *talloc_homo()
{
	int i, j;
	matrix *r;
	
	if ((r = talloc_matrix(4,4)) == NULL)
	 { return (NULL);
	 }
	for (i=0; i<4; i++)
 	 { for (j=0; j<4; j++)
	    { if (j == i)
	       { r->buf[i*4+j] = 1.0;
	       }
	      else
	       { r->buf[i*4+j] = 0.0;
	       }
	    }
	 }
	return (r);
}

transToDrive (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *t1;

	t1 = a[0].val.m;
	if (t1->col != 4 || t1->row != 4)
	 { error (0, "%s() requires 4 X 4 matrix for input\n", r->name);
	   return (-1);
	 }
	if ((mr = talloc_matrix (1, 6)) == NULL)
	 { return (-1);
	 }
	if (setDriveparams (mr->buf, t1->buf) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}

driveToTrans (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;

	m1 = a[0].val.m;
	if ((m1->row != 1 || m1->col != 6) &&
	    (m1->row != 6 || m1->col != 1))
	 { error (0, "%s() -- 6 vector required for input\n", r->name);
	   return (-1);
	 }

	if ((mr = talloc_homo()) == NULL)
	 { return (-1);
	 }
	if (driveFunction (mr->buf, m1->buf) == -1)
	 { return (-1);
	 }
	r->val.m = mr;
 	return (1);
}
