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

#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/robot_kynvar.h"
#include "../h/puma_kynvar.h"

#define J1	0
#define J2	1
#define J3	2
#define J4	3
#define J5	4
#define J6	5

#define A2	(gen->dh[1].a/10)
#define D3	(gen->dh[2].d/10)
#define A3	(gen->dh[2].a/10)
#define D4	(gen->dh[3].d/10)

/* Routines for calculating the frame matrices for PUMA robots */

static pumaMultA1Fwd(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
void *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C;
	register float S;

	C = ((PUMA_VAR*)var)->c[J1];
	S = ((PUMA_VAR*)var)->s[J1];

	if (((PUMA_KYN*)(kyn->par))->oldCoordDefs)
	 { tr->n.x = C*t1->n.x + S*t1->o.x;
	   tr->o.x = -t1->a.x;
	   tr->a.x = -S*t1->n.x + C*t1->o.x;
	   tr->p.x = t1->p.x;

	   tr->n.y = C*t1->n.y + S*t1->o.y;
	   tr->o.y = -t1->a.y;
	   tr->a.y = -S*t1->n.y + C*t1->o.y;
	   tr->p.y = t1->p.y;

	   tr->n.z = C*t1->n.z + S*t1->o.z;
	   tr->o.z = -t1->a.z;
	   tr->a.z = -S*t1->n.z + C*t1->o.z;
	   tr->p.z = t1->p.z;
	 }
	else
	 { tr->n.x = C*t1->n.x + S*t1->o.x;
	   tr->o.x = t1->a.x;
	   tr->a.x = S*t1->n.x - C*t1->o.x;
	   tr->p.x = t1->p.x;

	   tr->n.y = C*t1->n.y + S*t1->o.y;
	   tr->o.y = t1->a.y;
	   tr->a.y = S*t1->n.y - C*t1->o.y;
	   tr->p.y = t1->p.y;

	   tr->n.z = C*t1->n.z + S*t1->o.z;
	   tr->o.z = t1->a.z;
	   tr->a.z = S*t1->n.z - C*t1->o.z;
	   tr->p.z = t1->p.z;
	 }
}

static pumaMultA2Fwd(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
void *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C;
	register float S;

	C = ((PUMA_VAR*)var)->c[J2];
	S = ((PUMA_VAR*)var)->s[J2];

	tr->n.x = C*t1->n.x + S*t1->o.x;
	tr->o.x = -S*t1->n.x + C*t1->o.x;
	tr->a.x = t1->a.x;
	tr->p.x = A2*C*t1->n.x + A2*S*t1->o.x + t1->p.x;

	tr->n.y = C*t1->n.y + S*t1->o.y;
	tr->o.y = -S*t1->n.y + C*t1->o.y;
	tr->a.y = t1->a.y;
	tr->p.y = A2*C*t1->n.y + A2*S*t1->o.y + t1->p.y;

	tr->n.z = C*t1->n.z + S*t1->o.z;
	tr->o.z = -S*t1->n.z + C*t1->o.z;
	tr->a.z = t1->a.z;
	tr->p.z = A2*C*t1->n.z + A2*S*t1->o.z + t1->p.z;
}

static pumaMultA3Fwd(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
void *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C;
	register float S;
	register float tempx, tempy;

	C = ((PUMA_VAR*)var)->c[J3];
	S = ((PUMA_VAR*)var)->s[J3];

	if (((PUMA_KYN*)(kyn->par))->oldCoordDefs)
	 { tempx = D4*S + A3*C;
	   tempy = A3*S - C*D4;

	   tr->n.x = C*t1->n.x + S*t1->o.x;
	   tr->o.x = t1->a.x;
	   tr->a.x = S*t1->n.x - C*t1->o.x;
	   tr->p.x = tempx*t1->n.x + tempy*t1->o.x + D3*t1->a.x + t1->p.x;

	   tr->n.y = C*t1->n.y + S*t1->o.y;
	   tr->o.y = t1->a.y;
	   tr->a.y = S*t1->n.y - C*t1->o.y;
	   tr->p.y = tempx*t1->n.y + tempy*t1->o.y + D3*t1->a.y + t1->p.y;

	   tr->n.z = C*t1->n.z + S*t1->o.z;
	   tr->o.z = t1->a.z;
	   tr->a.z = S*t1->n.z - C*t1->o.z;
	   tr->p.z = tempx*t1->n.z + tempy*t1->o.z + D3*t1->a.z + t1->p.z;
	 }
	else
	 { tempx = - D4*S + A3*C;
	   tempy = A3*S + C*D4;

	   tr->n.x = C*t1->n.x + S*t1->o.x;
	   tr->o.x = -t1->a.x;
	   tr->a.x = -S*t1->n.x + C*t1->o.x;
	   tr->p.x = tempx*t1->n.x + tempy*t1->o.x + D3*t1->a.x + t1->p.x;

	   tr->n.y = C*t1->n.y + S*t1->o.y;
	   tr->o.y = -t1->a.y;
	   tr->a.y = -S*t1->n.y + C*t1->o.y;
	   tr->p.y = tempx*t1->n.y + tempy*t1->o.y + D3*t1->a.y + t1->p.y;

	   tr->n.z = C*t1->n.z + S*t1->o.z;
	   tr->o.z = -t1->a.z;
	   tr->a.z = -S*t1->n.z + C*t1->o.z;
	   tr->p.z = tempx*t1->n.z + tempy*t1->o.z + D3*t1->a.z + t1->p.z;
	 }
}

static pumaMultA4Fwd(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
void *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C;
	register float S;

	C = ((PUMA_VAR*)var)->c[J4];
	S = ((PUMA_VAR*)var)->s[J4];

	if (((PUMA_KYN*)(kyn->par))->oldCoordDefs)
	 { tr->n.x = C*t1->n.x + S*t1->o.x;
	   tr->o.x = -t1->a.x;
	   tr->a.x = -S*t1->n.x + C*t1->o.x;
	   tr->p.x = t1->p.x;

	   tr->n.y = C*t1->n.y + S*t1->o.y;
	   tr->o.y = -t1->a.y;
	   tr->a.y = -S*t1->n.y + C*t1->o.y;
	   tr->p.y = t1->p.y;

	   tr->n.z = C*t1->n.z + S*t1->o.z;
	   tr->o.z = -t1->a.z;
	   tr->a.z = -S*t1->n.z + C*t1->o.z;
	   tr->p.z = t1->p.z;
	 }
	else
	 { tr->n.x = C*t1->n.x + S*t1->o.x;
	   tr->o.x = t1->a.x;
	   tr->a.x = S*t1->n.x - C*t1->o.x;
	   tr->p.x = t1->p.x;

	   tr->n.y = C*t1->n.y + S*t1->o.y;
	   tr->o.y = t1->a.y;
	   tr->a.y = S*t1->n.y - C*t1->o.y;
	   tr->p.y = t1->p.y;

	   tr->n.z = C*t1->n.z + S*t1->o.z;
	   tr->o.z = t1->a.z;
	   tr->a.z = S*t1->n.z - C*t1->o.z;
	   tr->p.z = t1->p.z;
	 }
}

static pumaMultA5Fwd(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
void *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C, S;

	C = ((PUMA_VAR*)var)->c[J5];
	S = ((PUMA_VAR*)var)->s[J5];

	if (((PUMA_KYN*)(kyn->par))->oldCoordDefs)
	 { tr->n.x =   C*t1->n.x + S*t1->o.x;
	   tr->o.x =   t1->a.x;
	   tr->a.x =   S*t1->n.x - C*t1->o.x;
	   tr->p.x =   t1->p.x;

	   tr->n.y =   C*t1->n.y + S*t1->o.y;
	   tr->o.y =   t1->a.y;
	   tr->a.y =   S*t1->n.y - C*t1->o.y;
	   tr->p.y =   t1->p.y;

	   tr->n.z =   C*t1->n.z + S*t1->o.z;
	   tr->o.z =   t1->a.z;
	   tr->a.z =   S*t1->n.z - C*t1->o.z;
	   tr->p.z =   t1->p.z;
	 }
	else
	 { tr->n.x =   C*t1->n.x + S*t1->o.x;
	   tr->o.x =  -t1->a.x;
	   tr->a.x =  -S*t1->n.x + C*t1->o.x;
	   tr->p.x =   t1->p.x;

	   tr->n.y =   C*t1->n.y + S*t1->o.y;
	   tr->o.y =  -t1->a.y;
	   tr->a.y =  -S*t1->n.y + C*t1->o.y;
	   tr->p.y =   t1->p.y;

	   tr->n.z =   C*t1->n.z + S*t1->o.z;
	   tr->o.z =  -t1->a.z;
	   tr->a.z =  -S*t1->n.z + C*t1->o.z;
	   tr->p.z =   t1->p.z;
	 }
}

static pumaMultA6Fwd(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
void *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C, S;

	C = ((PUMA_VAR*)var)->c[J6];
	S = ((PUMA_VAR*)var)->s[J6];

	/* solution is the same in new and old coordinates */
	 { tr->n.x =  C*t1->n.x + S*t1->o.x;
	   tr->o.x = -S*t1->n.x + C*t1->o.x;
	   tr->a.x =  t1->a.x;
	   tr->p.x =  t1->p.x;

	   tr->n.y =  C*t1->n.y + S*t1->o.y;
	   tr->o.y = -S*t1->n.y + C*t1->o.y;
	   tr->a.y =  t1->a.y;
	   tr->p.y =  t1->p.y;

	   tr->n.z =  C*t1->n.z + S*t1->o.z;
	   tr->o.z = -S*t1->n.z + C*t1->o.z;
	   tr->a.z =  t1->a.z;
	   tr->p.z =  t1->p.z;
	 }
}

