/*-=-=-=-=-=-=-=-=-=-=-=- 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 <stdio.h>
#include <math.h>

#include "../h/cdefs.h"
#include "../h/rccl.h"	/* XXX MUST include rccl.h before grf4.h */
#include "../h/puma_kynvar.h"

#include "grf4.h"

#define NUM_CIRC_EDGES		12
#define NUM_LINKS 		7

#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)

static int capacity;

#define BEIGE		RGBPACK(225,160, 90)
#define DARK_BROWN	RGBPACK( 85, 55, 35)
#define DARK_GREY	RGBPACK( 50, 50, 50)
#define BLACK		RGBPACK(  0,  0,  0)
#define WHITE		RGBPACK(255,255,255)
#define SILVER		RGBPACK(200,200,220)
#define AMBER		RGBPACK(255,125,  0)

/* PUMA 560 stuff */

#define P560_BASE_PLATE_RADIUS		18.25
#define P560_BASE_PLATE_HEIGHT		 6.5
#define P560_PEDESTAL_RADIUS		 9.0
#define P560_PEDESTAL_HEIGHT		52.0	 

#define P560_LINK1_RADIUS		 8.0
#define P560_LINK1_DEPTH		29.8
#define P560_LINK1_OFFSET		(1.5+P560_PEDESTAL_RADIUS)
	
#define P560_LINK2_DEPTH		10.2
#define P560_LINK2_OFFSET		(19.3+P560_LINK2_DEPTH)
#define P560_LINK3_DEPTH		 9.0
#define P560_LINK4_RADIUS		 4.1
#define P560_LINK4_DEPTH		 9.0

#define P560_HAND_SIZE			10.0

static float p560_l2bot[] = {
	 0.0,	51.0,
	-5.33,  48.9,
	-7.5,	43.7,
       -14.85,	 2.6,
       -14.85, -16.9,
	-7.9,  -21.0,
	 0.0,  -22.4,
	 7.9,  -21.0,
	14.85, -16.9,
	14.85,   2.6,
	 7.5,   43.7,
	 5.33,  48.9,
};

static char* p560_l2hidden = "111001110011";

static float p560_l3bot[] = {
       	-6.28,  39.21,
	-8.1,   -8.5,
	-4.1,  -11.0,
	 0.8,  -11.7,
	 5.5,  -10.4,
	 9.1,   -7.3,
	 2.22,  39.21,
};

static char* p560_l3hidden = "0011100";

static multA1();
static multA2();
static multA3();
static multA4();
static multA5();
static multA6();

drawPumaHand (size, open)
float size;
int open;
{
	float m, h;

	m = 0.5*size;
	h = (open ? m : m/4);

	grf4Color (SILVER);

	/* draw the stem and the slide */

	grf4Move (0.0, 0.0, 0.0);
	grf4Draw (  m, 0.0, 0.0);
	grf4Move (  m,  -m, 0.0);
	grf4Draw (  m,   m, 0.0);

	/* draw the two fingers */

	grf4Move (size, -h, 0.0);
	grf4Draw (   m, -h, 0.0);
	grf4Move (size,  h, 0.0);
	grf4Draw (   m,  h, 0.0);
}

Grf4Object *p560basePlate;
Grf4Object *p560link[NUM_LINKS];

defineP560Links()
{
	void *baseMat;
	void *bodyMat;
	void *wristMat;

	baseMat = grf4DefMaterial(DARK_BROWN, DARK_BROWN, 0, 0, 0.0, 1.0);
	bodyMat = grf4DefMaterial(BEIGE, BEIGE, 0, 0, 0.0, 1.0);
	wristMat = grf4DefMaterial(DARK_GREY, DARK_GREY, 0,0x808080,50.0,1.0);

	p560basePlate = grf4GenDisc(NUM_CIRC_EDGES, P560_BASE_PLATE_RADIUS, 
				P560_BASE_PLATE_HEIGHT, baseMat);

	p560link[0] = grf4GenDisc(NUM_CIRC_EDGES, P560_PEDESTAL_RADIUS, 
			      P560_PEDESTAL_HEIGHT, baseMat);

	p560link[1] = grf4GenDisc(NUM_CIRC_EDGES, P560_LINK1_RADIUS, 
			      P560_LINK1_DEPTH, bodyMat);

	p560link[2] = grf4GenPrism(ARRAY_SIZE(p560_l2bot)/2, p560_l2bot, 
			      (float*)0, p560_l2hidden, P560_LINK2_DEPTH,
			      bodyMat);

	p560link[3] = grf4GenPrism(ARRAY_SIZE(p560_l3bot)/2, p560_l3bot, 
			      (float*)0, p560_l3hidden, P560_LINK3_DEPTH, 
			      bodyMat);

	p560link[4] = grf4GenDisc(NUM_CIRC_EDGES, P560_LINK4_RADIUS, 
			      P560_LINK4_DEPTH, wristMat);

	capacity = grf4Capacity();
}

drawP560 (T, kyn, var, drawHand, handpos)
register TRSF *T;
KYN *kyn;
void *var;
int drawHand;
int handpos;
{
	/* clear the display and draw a robot, given a set of joint angles */

	PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	TRSF trans;

	grf4DrawObject (p560basePlate);

	grf4GetTrans (&trans);
	multTrsfXyz (&trans, 0.0, 0.0,P560_BASE_PLATE_HEIGHT);
	grf4SetTrans (&trans);
	grf4DrawObject (p560link[0]);

	multTrsfXyz (&trans, 0.0, 0.0,P560_PEDESTAL_HEIGHT+P560_LINK1_RADIUS);
	T[0] = trans;
	multA1 (&trans, &T[0], kyn, pvar);
	T[1] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -P560_LINK1_OFFSET);
	grf4SetTrans (&trans);
	grf4DrawObject (p560link[1]);

	multA2 (&trans, &T[1], kyn, pvar);
	T[2] = trans;
	multTrsfXyz (&trans, -A2, 0.0, P560_LINK2_OFFSET);
	multTrsfRpy (&trans, 90.0, 0.0, 180.0);
	grf4SetTrans (&trans);
	grf4DrawObject (p560link[2]);

	if (capacity & GRF_HARDWARE_A)
	 { grf4Color (DARK_BROWN);
	   grf4Move ( 1.5, 49.0, -0.2);
	   grf4Draw ( 1.5,-21.0, -0.2);
	   grf4Move (-1.5, 49.0, -0.2);
	   grf4Draw (-1.5,-21.0, -0.2);
	 }

	multA3 (&trans, &T[2], kyn, pvar);
	T[3] = trans;
	multTrsfXyz (&trans, -A3, P560_LINK3_DEPTH/2.0, -D4);
	multTrsfRpy (&trans, 0.0, 0.0, 90.0);
	grf4SetTrans (&trans);
	grf4DrawObject (p560link[3]);

	multA4 (&trans, &T[3], kyn, pvar);
	T[4] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -P560_LINK4_DEPTH/2.0);
	grf4SetTrans (&trans);
	grf4DrawObject (p560link[4]);

	multA5 (&trans, &T[4], kyn, pvar);
	T[5] = trans;
	multA6 (&trans, &T[5], kyn, pvar);
	T[6] = trans;

	if (drawHand)
	 { multTrsfRpy (&trans, 0.0, -90.0, 90.0);
	   multTrsfXyz (&trans, P560_LINK4_RADIUS, 0.0, 0.0);
	   grf4SetTrans (&trans);
	   drawPumaHand (P560_HAND_SIZE, handpos);
	 }
}

p560ViewSize (r, b, h, kyn)
float *r, *b, *h;
KYN *kyn;
{
	GEN_KYN *gen = kyn->gen;

	*r = A2 + D4 + P560_LINK4_RADIUS + P560_HAND_SIZE;
	*b = P560_BASE_PLATE_HEIGHT + P560_PEDESTAL_HEIGHT + 
	     P560_LINK1_RADIUS;
	*h = *r;
}

p560LinkFrames (T, B, kyn, var)
TRSF *T, *B;
KYN *kyn;
void *var;
{
	PUMA_VAR *pvar = (PUMA_VAR*)var;

	T[0] = *B;
	multTrsfXyz (&T[0], 0.0, 0.0, P560_BASE_PLATE_HEIGHT + 
				      P560_PEDESTAL_HEIGHT+P560_LINK1_RADIUS);
	multA1 (&T[1], &T[0], kyn, pvar);
	multA2 (&T[2], &T[1], kyn, pvar);
	multA3 (&T[3], &T[2], kyn, pvar);
	multA4 (&T[4], &T[3], kyn, pvar);
	multA5 (&T[5], &T[4], kyn, pvar);
	multA6 (&T[6], &T[5], kyn, pvar);
}

#define MANU_BASE_PLATE_RADIUS		20.0
#define MANU_BASE_PLATE_HEIGHT		18.0
#define MANU_PEDESTAL_RADIUS		 8.0
#define MANU_PEDESTAL_HEIGHT		36.5

#define MANU_MOTOR_HEIGHT		18.0
#define MANU_MOTOR_RADIUS		 5.25

#define MANU_LINK1_RADIUS		 8.5
#define MANU_LINK1_DEPTH		23.6
#define MANU_LINK1_OFFSET	        11.8

#define MANU_LINK2_DEPTH		10.5
#define MANU_LINK2_OFFSET	        11.8
#define MANU_LINK3_DEPTH		 9.0
#define MANU_LINK4_RADIUS		 3.0
#define MANU_LINK4_DEPTH		 7.5

#define MANU_LINK3X_RADIUS	 6.0
#define MANU_LINK3X_DEPTH	 ((MANU_LINK1_DEPTH-MANU_LINK3_DEPTH)/2)

#define MANU_LINK4X_LEN		24.0
#define MANU_LINK4X_DEPTH	 6.0

#define MANU_HAND_SIZE			10.0

static float manutec_l2bot[] = {
	13.5,   12.0,
	 7.794, 39.5,
	 4.5,   42.794,
	 0.0,   44.0,
	-4.5,   42.794,
	-7.794, 39.5,
	-9.0,   35.0,
	-9.0,   -9.0,
	13.5,   -9.0,
};

static char* manutec_l2hidden = "011111100";

static float manutec_l3bot[] = {
	12.0,  -13.5,
	12.0,    3.0,
	 4.0,    8.0,
	-4.0,    8.0,
       -12.0,    3.0,
       -12.0,  -13.5,
	-6.0,  -15.0,
	 6.0,  -15.0,
};

static char* manutec_l3hidden = "00000011";

Grf4Object *manutecbasePlate;
Grf4Object *manutecMotorCase;
Grf4Object *manuteclink[NUM_LINKS];
Grf4Object *manuteclink4x;
Grf4Object *manuteclink3x;

defineManutecLinks()
{
	void *baseMat;
	void *bodyMat;

	baseMat = grf4DefMaterial(DARK_BROWN, DARK_BROWN, 0, 0, 0.0, 1.0);
	bodyMat = grf4DefMaterial(BEIGE, BEIGE, 0, 0, 0.0, 1.0);

	manutecbasePlate = grf4GenDisc(NUM_CIRC_EDGES, MANU_BASE_PLATE_RADIUS, 
				MANU_BASE_PLATE_HEIGHT, baseMat);

	manuteclink[0] = grf4GenDisc(NUM_CIRC_EDGES, MANU_PEDESTAL_RADIUS, 
			      MANU_PEDESTAL_HEIGHT, baseMat);

	manuteclink[1] = grf4GenDisc(NUM_CIRC_EDGES, MANU_LINK1_RADIUS, 
			      MANU_LINK1_DEPTH, bodyMat);

	manuteclink[2] = grf4GenPrism(ARRAY_SIZE(manutec_l2bot)/2, manutec_l2bot, 
			      (float*)0, manutec_l2hidden, MANU_LINK2_DEPTH,
			      bodyMat);

	manutecMotorCase = grf4GenDisc(NUM_CIRC_EDGES, MANU_MOTOR_RADIUS,
				       MANU_MOTOR_HEIGHT, baseMat);

	manuteclink[3] = grf4GenPrism(ARRAY_SIZE(manutec_l3bot)/2, manutec_l3bot, 
			      (float*)0, manutec_l3hidden, MANU_LINK3_DEPTH, 
			      bodyMat);
	manuteclink3x = grf4GenDisc(NUM_CIRC_EDGES, MANU_LINK3X_RADIUS, 
			      MANU_LINK3X_DEPTH, bodyMat);

	manuteclink[4] = grf4GenDisc(NUM_CIRC_EDGES, MANU_LINK4_RADIUS, 
			      MANU_LINK4_DEPTH, bodyMat);

	manuteclink4x = grf4GenBox(MANU_LINK4X_DEPTH, MANU_LINK4X_LEN, 
				   MANU_LINK4X_DEPTH, bodyMat);

	capacity = grf4Capacity();
}

drawManutec (T, kyn, var, drawHand, handpos)
register TRSF *T;
KYN *kyn;
void *var;
int drawHand;
int handpos;
{
	/* clear the display and draw a robot, given a set of joint angles */

	PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	TRSF trans;

	grf4DrawObject (manutecbasePlate);

	grf4GetTrans (&trans);
	multTrsfXyz (&trans, 0.0, 0.0,MANU_BASE_PLATE_HEIGHT);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink[0]);

	multTrsfXyz (&trans, 0.0, 0.0,MANU_PEDESTAL_HEIGHT+MANU_LINK1_RADIUS);
	T[0] = trans;
	multA1 (&trans, &T[0], kyn, pvar);
	T[1] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -MANU_LINK1_OFFSET);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink[1]);

	multA2 (&trans, &T[1], kyn, pvar);
	T[2] = trans;
	multTrsfXyz (&trans, -A2, 0.0, MANU_LINK1_OFFSET+MANU_LINK2_DEPTH);
	multTrsfRpy (&trans, 90.0, 0.0, 180.0);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink[2]);

#if 0
	if (capacity & GRF_HARDWARE_A)
	 { grf4Color (DARK_BROWN);
	   grf4Move ( 1.5, 49.0, -0.2);
	   grf4Draw ( 1.5,-21.0, -0.2);
	   grf4Move (-1.5, 49.0, -0.2);
	   grf4Draw (-1.5,-21.0, -0.2);
	 }
#endif

	multA3 (&trans, &T[2], kyn, pvar);
	T[3] = trans;
	multTrsfXyz (&trans, -A3, MANU_LINK3_DEPTH/2.0, -D4);
	multTrsfRpy (&trans, 0.0, 0.0, 90.0);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink[3]);
	multTrsfXyz (&trans, 0.0, 0.0, MANU_LINK3_DEPTH);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink3x);

	multA4 (&trans, &T[3], kyn, pvar);
	T[4] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -MANU_LINK4_DEPTH/2.0);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink[4]);
	multTrsfXyz (&trans, -MANU_LINK4X_DEPTH/2,
			     -(MANU_LINK4X_LEN+MANU_LINK4_RADIUS), 
			     (MANU_LINK4_DEPTH-MANU_LINK4X_DEPTH)/2.0);
	grf4SetTrans (&trans);
	grf4DrawObject (manuteclink4x);

	multA5 (&trans, &T[4], kyn, pvar);
	T[5] = trans;
	multA6 (&trans, &T[5], kyn, pvar);
	T[6] = trans;

	if (drawHand)
	 { multTrsfRpy (&trans, 0.0, -90.0, 90.0);
	   multTrsfXyz (&trans, MANU_LINK4_RADIUS, 0.0, 0.0);
	   grf4SetTrans (&trans);
	   drawPumaHand (MANU_HAND_SIZE, handpos);
	 }
}

manutecViewSize (r, b, h, kyn)
float *r, *b, *h;
KYN *kyn;
{
	GEN_KYN *gen = kyn->gen;

	*r = A2 + D4 + MANU_LINK4_RADIUS + MANU_HAND_SIZE;
	*b = MANU_BASE_PLATE_HEIGHT + MANU_PEDESTAL_HEIGHT + 
	     MANU_LINK1_RADIUS;
	*h = *r;
}

manutecLinkFrames (T, B, kyn, var)
TRSF *T, *B;
KYN *kyn;
void *var;
{
	PUMA_VAR *pvar = (PUMA_VAR*)var;

	T[0] = *B;
	multTrsfXyz (&T[0], 0.0, 0.0, MANU_BASE_PLATE_HEIGHT + 
				      MANU_PEDESTAL_HEIGHT+MANU_LINK1_RADIUS);
	multA1 (&T[1], &T[0], kyn, pvar);
	multA2 (&T[2], &T[1], kyn, pvar);
	multA3 (&T[3], &T[2], kyn, pvar);
	multA4 (&T[4], &T[3], kyn, pvar);
	multA5 (&T[5], &T[4], kyn, pvar);
	multA6 (&T[6], &T[5], kyn, pvar);
}

/* PUMA 260 stuff */

#define P260_BASE_PLATE_HEIGHT		 2.5
#define P260_BASE_PLATE_WIDTH		 8.9
#define P260_BASE_RING_HEIGHT		 2.0
#define P260_PEDESTAL_RADIUS		 3.5
#define P260_PEDESTAL_HEIGHT		27.12

#define P260_MOTOR_HEIGHT		 9.0
#define P260_MOTOR_RADIUS		 2.7

#define P260_LINK1_RADIUS		 3.4
#define P260_LINK1_DEPTH		 9.5
#define P260_LINK1_OFFSET		 5.9
	
#define P260_LINK2_DEPTH		 6.7
#define P260_LINK2_OFFSET		(3.6)
#define P260_LINK3_DEPTH		 4.648
#define P260_LINK4_RADIUS		 3.0

#define P260_HAND_SIZE			 6.0

static float p260_base[] = {
	 P260_BASE_PLATE_WIDTH,    9.4,
	-P260_BASE_PLATE_WIDTH,    9.4,
	-P260_BASE_PLATE_WIDTH,  -17.3,
	 P260_BASE_PLATE_WIDTH,  -17.3,
};
	
static float p260_l2bot[] = {
	23.77,  -1.91,
	22.97,    .83,
	20.91,	 1.58,
	  .75,	 5.45,
	-5.45,	 5.45,
	-5.45,	-5.45,
	20.23,  -5.45,
	22.74,  -4.41,
};

static char* p260_l2hidden = "11100011";

static float p260_l3bot[] = {
	17.32,   2.9,
	-9.01,   4.6,
	-9.87,	 2.37,
       -10.16,	 0.0,
	-9.87,	-2.37,
	-9.01,	-4.6,
	17.32,	-2.9,
};

static char* p260_l3hidden = "0011100";

Grf4Object *p260motorCase;
Grf4Object *p260baseRing;
Grf4Object *p260basePlate;
Grf4Object *p260link[NUM_LINKS];
Grf4Object *p260wrist;

defineP260Links()
{
	void *baseMat;
	void *bodyMat;

	baseMat = grf4DefMaterial(DARK_BROWN, DARK_BROWN, 0, 0, 0.0, 1.0);
	bodyMat = grf4DefMaterial(BEIGE, BEIGE, 0, 0, 0.0, 1.0);

	p260basePlate = grf4GenPrism(ARRAY_SIZE(p260_base)/2, p260_base, 
				(float*)0, (char*)0, P260_BASE_PLATE_HEIGHT, 
				baseMat);

	p260baseRing = grf4GenDisc(NUM_CIRC_EDGES, P260_BASE_PLATE_WIDTH,
				P260_BASE_RING_HEIGHT, baseMat);

	p260link[0] = grf4GenDisc(NUM_CIRC_EDGES, P260_PEDESTAL_RADIUS, 
			      	P260_PEDESTAL_HEIGHT, baseMat);

	p260link[1] = grf4GenDisc(NUM_CIRC_EDGES, P260_LINK1_RADIUS, 
			      	P260_LINK1_DEPTH, bodyMat);

	p260link[2] = grf4GenPrism(ARRAY_SIZE(p260_l2bot)/2, p260_l2bot, 
				(float*)0, p260_l2hidden, P260_LINK2_DEPTH, 
				bodyMat);

	p260motorCase = grf4GenDisc(NUM_CIRC_EDGES, P260_MOTOR_RADIUS,
						P260_MOTOR_HEIGHT, bodyMat);

	p260link[3] = grf4GenPrism(ARRAY_SIZE(p260_l3bot)/2, p260_l3bot, 
				(float*)0, p260_l3hidden, P260_LINK3_DEPTH, 
				bodyMat);

	p260wrist = grf4GenSphere (8, P260_LINK4_RADIUS, bodyMat);

	capacity = grf4Capacity();
}

drawP260 (T, kyn, var, drawHand, handpos)
register TRSF *T;
KYN *kyn;
void *var;
int drawHand;
int handpos;
{
	/* clear the display and draw a robot, given a set of joint angles */

	PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	TRSF trans;

	grf4DrawObject (p260basePlate);

	grf4GetTrans (&trans);
	multTrsfXyz (&trans, 0.0, 0.0,P260_BASE_PLATE_HEIGHT);
	grf4SetTrans (&trans);

	if (capacity & GRF_HARDWARE_A)
	 { grf4DrawObject (p260baseRing);
	 }

	grf4DrawObject (p260link[0]);

	multTrsfXyz (&trans, 0.0, 0.0,P260_PEDESTAL_HEIGHT+P260_LINK1_RADIUS);
	T[0] = trans;
	multA1 (&trans, &T[0], kyn, pvar);
	T[1] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -P260_LINK1_OFFSET);
	grf4SetTrans (&trans);
	grf4DrawObject (p260link[1]);

	multA2 (&trans, &T[1], kyn, pvar);
	T[2] = trans;
	multTrsfXyz (&trans, -A2, 0.0, P260_LINK2_OFFSET);
	multTrsfRpy (&trans, 5.3794, 0.0, 0.0);	
	grf4SetTrans (&trans);
	grf4DrawObject (p260link[2]);

	if (capacity & GRF_HARDWARE_A)
	 { multTrsfRpy (&trans, 0.0, -90.0, 0.0);
	   multTrsfXyz (&trans, P260_LINK2_DEPTH/2, P260_MOTOR_RADIUS+.25,
				5.45);
	   grf4SetTrans (&trans);
	   grf4DrawObject(p260motorCase);
	   multTrsfXyz (&trans, 0.0, -2*P260_MOTOR_RADIUS-.5,0.0);
	   grf4SetTrans (&trans);
	   grf4DrawObject(p260motorCase);
	 }

	multA3 (&trans, &T[2], kyn, pvar);
	T[3] = trans;
	multTrsfXyz (&trans, 0.0, -P260_LINK3_DEPTH/2.0, -D4);
	multTrsfRpy (&trans, -90.0, -90.0, 0.0);
	grf4SetTrans (&trans);
	grf4DrawObject (p260link[3]);

	multA4 (&trans, &T[3], kyn, pvar);
	T[4] = trans;
	grf4SetTrans (&trans);
	grf4DrawObject (p260wrist);

	multA5 (&trans, &T[4], kyn, pvar);
	T[5] = trans;
	multA6 (&trans, &T[5], kyn, pvar);
	T[6] = trans;

	if (drawHand)
	 { multTrsfRpy (&trans, 0.0, -90.0, 180.0);
	   multTrsfXyz (&trans, P260_LINK4_RADIUS, 0.0, 0.0);
	   grf4SetTrans (&trans);
	   drawPumaHand (P260_HAND_SIZE, handpos);
	 }
}

p260ViewSize (r, b, h, kyn)
float *r, *b, *h;
KYN *kyn;
{
	GEN_KYN *gen = kyn->gen;

	*r = A2 + D4 + P260_LINK4_RADIUS + P260_HAND_SIZE;
	*b = P260_BASE_PLATE_HEIGHT + P260_PEDESTAL_HEIGHT + 
	     P260_LINK1_RADIUS;
	*h = *r;
}

p260LinkFrames (T, B, kyn, var)
TRSF *T, *B;
KYN *kyn;
void *var;
{
	PUMA_VAR *pvar = (PUMA_VAR*)var;

	T[0] = *B;
	multTrsfXyz (&T[0], 0.0, 0.0, P260_BASE_PLATE_HEIGHT + 
				      P260_PEDESTAL_HEIGHT+P260_LINK1_RADIUS);
	multA1 (&T[1], &T[0], kyn, pvar);
	multA2 (&T[2], &T[1], kyn, pvar);
	multA3 (&T[3], &T[2], kyn, pvar);
	multA4 (&T[4], &T[3], kyn, pvar);
	multA5 (&T[5], &T[4], kyn, pvar);
	multA6 (&T[6], &T[5], kyn, pvar);
}

/* PUMA 760 stuff */

#define P760_BASE_PLATE_HEIGHT		30.0
#define P760_BASE_PLATE_RADIUS		28.0
#define P760_PEDESTAL_RADIUS		12.5
#define P760_PEDESTAL_HEIGHT		69.0

#define P760_LINK1_RADIUS		13.0
#define P760_LINK1_DEPTH		36.6
#define P760_LINK1_OFFSET		10.3

#define P760_MOTOR_BOX_DEPTH		12.0

#define LIGHT_TOP_RADIUS		 1.5
#define LIGHT_BOTTOM_RADIUS		 3.0
#define LIGHT_HEIGHT 			 8.0
	
#define P760_LINK2_DEPTH		18.5
#define P760_LINK2_OFFSET	        26.3
#define P760_LINK3_DEPTH		16.4
#define P760_LINK4_RADIUS		 8.0

#define P760_HAND_SIZE			16.0

static float p760_motorbox[] = {
	 6.0,   34.5,
	-6.0,   34.5,
       -14.5,  -13.0,
	14.5,  -13.0,
};
	
static float p760_l2bot[] = {
	78.7,	 0.0,
	75.18,   9.17,
	66.43,  13.63,
	 0.0,	20.5,
       -64.0,	20.5,
       -64.0,  -20.5,
	 0.0,  -20.5,
	66.43, -13.63,
	75.18,  -9.17,
};

char *p760_l2hidden = "111000011";

static float p760_l3bot[] = {
	52.0,	 6.0,
	 0.0,	13.7,
       -39.0,   13.7,
       -39.0,  -13.7,
	 0.0,  -13.7,
	52.0,	-6.0,
};

Grf4Object *p760basePlate;
Grf4Object *p760link[NUM_LINKS];
Grf4Object *p760motorBox;
Grf4Object *p760warningLight;
Grf4Object *p760wrist;

defineP760Links()
{
	void *baseMat;
	void *bodyMat;
	void *lightMat;

	baseMat = grf4DefMaterial(DARK_BROWN, DARK_BROWN, 0, 0, 0.0, 1.0);
	bodyMat = grf4DefMaterial(BEIGE, BEIGE, 0, 0, 0.0, 1.0);
	lightMat = grf4DefMaterial(0, 0, AMBER, 0xffff, 100.0, 1.0);

	p760basePlate = grf4GenDisc(NUM_CIRC_EDGES, P760_BASE_PLATE_RADIUS,
				P760_BASE_PLATE_HEIGHT, baseMat);

	p760link[0] = grf4GenCone(NUM_CIRC_EDGES, P760_BASE_PLATE_RADIUS,
				P760_PEDESTAL_RADIUS, 
				P760_PEDESTAL_HEIGHT, baseMat);

	p760motorBox = grf4GenPrism(ARRAY_SIZE(p760_motorbox)/2,p760_motorbox,
				(float*)0, (char*)0, P760_MOTOR_BOX_DEPTH,
				bodyMat);

	p760warningLight = grf4GenCone(NUM_CIRC_EDGES, LIGHT_BOTTOM_RADIUS,
				LIGHT_TOP_RADIUS,
				LIGHT_HEIGHT, lightMat);

	p760link[1] = grf4GenDisc(NUM_CIRC_EDGES, P760_LINK1_RADIUS, 
				P760_LINK1_DEPTH, bodyMat);

	p760link[2] = grf4GenPrism(ARRAY_SIZE(p760_l2bot)/2, p760_l2bot, 
				(float*)0, p760_l2hidden, P760_LINK2_DEPTH, 
				bodyMat);

	p760link[3] = grf4GenPrism(ARRAY_SIZE(p760_l3bot)/2, p760_l3bot, 
				(float*)0, (char*)0, P760_LINK3_DEPTH, 
				bodyMat);

	p760wrist = grf4GenSphere (8, P760_LINK4_RADIUS, bodyMat);

	capacity = grf4Capacity();
}

drawP760 (T, kyn, var, drawHand, handpos)
register TRSF *T;
KYN *kyn;
void *var;
int drawHand;
int handpos;
{
	/* clear the display and draw a robot, given a set of joint angles */

	PUMA_VAR *pvar = (PUMA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	TRSF trans;

	grf4DrawObject (p760basePlate);

	grf4GetTrans (&trans);
	multTrsfXyz (&trans, 0.0, 0.0, P760_BASE_PLATE_HEIGHT);
	grf4SetTrans (&trans);
	grf4DrawObject (p760link[0]);

	multTrsfXyz (&trans, 0.0, 0.0,P760_PEDESTAL_HEIGHT+P760_LINK1_RADIUS);

	T[0] = trans;
	multA1 (&trans, &T[0], kyn, pvar);
	T[1] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -P760_LINK1_OFFSET);
	grf4SetTrans (&trans);
	grf4DrawObject (p760link[1]);
	multTrsfXyz (&trans, 0.0, 0.0, -P760_MOTOR_BOX_DEPTH);
	grf4SetTrans (&trans);
	grf4DrawObject (p760motorBox);

	if (capacity & GRF_HARDWARE_A)
	 { multTrsfXyz (&trans, 0.0, 34.5, P760_MOTOR_BOX_DEPTH/2);
	   multTrsfRpy (&trans, 0.0, 0.0, -90.0);
	   grf4SetTrans (&trans);
	   grf4DrawObject (p760warningLight);
	 }

	multA2 (&trans, &T[1], kyn, pvar);
	T[2] = trans;
	multTrsfXyz (&trans, -A2, 0.0, P760_LINK2_OFFSET);
	grf4SetTrans (&trans);
	grf4DrawObject (p760link[2]);

	if (capacity & GRF_HARDWARE_A)
	 { grf4Color (DARK_BROWN);
	   grf4Move ( 78.0, 1.5, P760_LINK2_DEPTH+0.2);
	   grf4Draw (-64.0, 1.5, P760_LINK2_DEPTH+0.2);
	   grf4Move ( 78.0,-1.5, P760_LINK2_DEPTH+0.2);
	   grf4Draw (-64.0,-1.5, P760_LINK2_DEPTH+0.2);
	 }

	multA3 (&trans, &T[2], kyn, pvar);
	T[3] = trans;
	multTrsfXyz (&trans, 0.0, -P760_LINK3_DEPTH/2.0, -D4);
	multTrsfRpy (&trans, -90.0, -90.0, 0.0);
	grf4SetTrans (&trans);
	grf4DrawObject (p760link[3]);

	multA4 (&trans, &T[3], kyn, pvar);
	T[4] = trans;
	grf4SetTrans (&trans);
	grf4DrawObject (p760wrist);

	multA5 (&trans, &T[4], kyn, pvar);
	T[5] = trans;
	multA6 (&trans, &T[5], kyn, pvar);
	T[6] = trans;

	if (drawHand)
	 { multTrsfRpy (&trans, 0.0, -90.0, 180.0);
	   multTrsfXyz (&trans, P760_LINK4_RADIUS, 0.0, 0.0);
	   grf4SetTrans (&trans);
	   drawPumaHand (P760_HAND_SIZE, handpos);
	 }
}

p760ViewSize (r, b, h, kyn)
float *r, *b, *h;
KYN *kyn;
{
	GEN_KYN *gen = kyn->gen;

	*r = A2 + D4 + P760_LINK4_RADIUS + P760_HAND_SIZE;
	*b = P760_BASE_PLATE_HEIGHT + P760_PEDESTAL_HEIGHT + 
	     P760_LINK1_RADIUS;
	*h = *r;
}

p760LinkFrames (T, B, kyn, var)
TRSF *T, *B;
KYN *kyn;
void *var;
{
	PUMA_VAR *pvar = (PUMA_VAR*)var;
	T[0] = *B;
	multTrsfXyz (&T[0], 0.0, 0.0, P760_BASE_PLATE_HEIGHT + 
				      P760_PEDESTAL_HEIGHT+P760_LINK1_RADIUS);
	multA1 (&T[1], &T[0], kyn, pvar);
	multA2 (&T[2], &T[1], kyn, pvar);
	multA3 (&T[3], &T[2], kyn, pvar);
	multA4 (&T[4], &T[3], kyn, pvar);
	multA5 (&T[5], &T[4], kyn, pvar);
	multA6 (&T[6], &T[5], kyn, pvar);
}

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

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

	register float C;
	register float S;

	C = var->c[J1];
	S = var->s[J1];

	if (((PUMA_KYN*)(kyn->ext))->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 multA2(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
PUMA_VAR *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C;
	register float S;

	C = var->c[J2];
	S = 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 multA3(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
PUMA_VAR *var;
{
	GEN_KYN *gen = kyn->gen;

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

	C = var->c[J3];
	S = var->s[J3];

	if (((PUMA_KYN*)(kyn->ext))->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 multA4(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
PUMA_VAR *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C;
	register float S;

	C = var->c[J4];
	S = var->s[J4];

	if (((PUMA_KYN*)(kyn->ext))->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 multA5(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
PUMA_VAR *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C, S;

	C = var->c[J5];
	S = var->s[J5];

	if (((PUMA_KYN*)(kyn->ext))->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 multA6(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
PUMA_VAR *var;
{
	GEN_KYN *gen = kyn->gen;

	register float C, S;

	C = var->c[J6];
	S = 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;
	 }
}
