/*-=-=-=-=-=-=-=-=-=-=-=- 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:	multTrsf
 * 
 * DESCRIPTION:	Functions for performing transform inversion and 
 *		multiplication. They are heavily optimized for VAXes,
 *		in ways that will become obvious.
 * 
 * AUTHOR/DATE:	John Lloyd, May, 1986
 *
 *-------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------
 *		
 * SYNOPSIS:	TRSF *multTrsf (r, t1, t2)
 *		  TRSF *r, *t1, *t2;
 *
 *		TRSF *multLiTrsf (r, t1, t2)
 *		  TRSF *r, *t1, *t2;
 *
 *		TRSF *multRiTrsf (r, t1, t2)
 *		  TRSF *r, *t1, *t2;
 *
 *		TRSF *invertTrsf (i, t1)
 *		  TRSF *i, *t1;
 *
 * DESCRIPTION:	'multTrsf' computes the transform product r = t1 t2.
 *
 *		'multLiTrsf' computes the product r = inverse(t1) t2.
 *
 *		'multRiTrsf' computes the product r = t1 inverse(t2).
 *
 *		'invertTrsf' computes the inverse of t1.
 * 
 * ARGUMENTS:	t1, t2 -- pointers to input transforms.
 *
 *		r, i -- pointers to result transforms.
 *
 * RETURN VAL:	Each function returns a pointer to the result.
 *
 * NOTES:	These functions permit multiplication and inversion IN PLACE,
 *		i.e., it is possible for the input and output arguments to
 *		point to the same transformations.
 *
 * SPEED:	Using the VAX assembly options, the multiply routines execute
 *		with a typical speed of .280 msec, while the invertTrsf routine 
 *		takes about .130 msec. 
 *
 *		The C versions of the routines run at about .480 and .19 msec,
 *		respectively.
 *
 *		The consolidation of the inversion and multiplication in
 *		the 'invMult' routines results in a considerable speed up
 *		over the old versions, which inverted and multiplied 
 *		separately, and had execution speeds of between .8 and 1.1 
 *		msec.
 *
 *-------------------------------------------------------------------------*/

#include "../h/robotTypes.h"

#if vax
	asm("#define nx 0");
	asm("#define ny 4");
	asm("#define nz 8");
	asm("#define ox 12");
	asm("#define oy 16");
	asm("#define oz 20");
	asm("#define ax 24");
	asm("#define ay 28");
	asm("#define az 32");
	asm("#define px 36");
	asm("#define py 40");
	asm("#define pz 44");
	asm("#define TRSF_SIZE 48");
#endif

TRSF *multTrsf(r, t1, t2) /*::*/
register TRSF *r;		/* 4(ap) -> r11 */
register TRSF *t1;		/* 8(ap) -> r10 */
register TRSF *t2;		/* 12(ap) -> r9 */
{
#if vax
/* And here we go....! */
	asm("pushr	$0x1c0");

	asm ("cmpl	r10,r11");
	asm ("bneq	1f");
	asm ("subl2	$TRSF_SIZE,sp");
	asm ("movc3	$TRSF_SIZE,(r10),(sp)");
	asm ("movl	sp,r10");
	asm ("1:");

	asm ("movf	nx(r10),r8");
	asm ("movf	ny(r10),r7");
	asm ("movf	nz(r10),r6");

	asm ("movf	nx(r9),r5");
	asm ("movf	ny(r9),r4");
	asm ("movf	nz(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute nx */
 	asm ("mulf3	ox(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r10),r3,r1");
	asm ("addf3	r1,r0,nx(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute ny */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r10),r3,r1");
	asm ("addf3	r1,r0,ny(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute nz */
 	asm ("mulf3	oz(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,nz(r11)");

	asm ("movf	ox(r9),r5");
	asm ("movf	oy(r9),r4");
	asm ("movf	oz(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute ox */
 	asm ("mulf3	ox(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r10),r3,r1");
	asm ("addf3	r1,r0,ox(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute oy */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r10),r3,r1");
	asm ("addf3	r1,r0,oy(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute oz */
 	asm ("mulf3	oz(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,oz(r11)");

	asm ("movf	ax(r9),r5");
	asm ("movf	ay(r9),r4");
	asm ("movf	az(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute ax */
 	asm ("mulf3	ox(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r10),r3,r1");
	asm ("addf3	r1,r0,ax(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute ay */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r10),r3,r1");
	asm ("addf3	r1,r0,ay(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute az */
 	asm ("mulf3	oz(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,az(r11)");

	asm ("movf	px(r9),r5");
	asm ("movf	py(r9),r4");
	asm ("movf	pz(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute px */
 	asm ("mulf3	ox(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r10),r3,r1");
	asm ("addf2	r1,r0");
	asm ("addf3	px(r10),r0,px(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute ay */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r10),r3,r1");
	asm ("addf2	r1,r0");
 	asm ("addf3	py(r10),r0,py(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute az */
 	asm ("mulf3	oz(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf2	r1,r0");
 	asm ("addf3	pz(r10),r0,pz(r11)");

	asm ("cmpl	4(ap),8(ap)");
	asm ("bneq	1f");
	asm ("addl2	$TRSF_SIZE,sp");
	asm ("1:");
	asm ("popr	$0x1c0");	
#else
	float x, y, z;
	TRSF temp;

	if (t1 == r)
	 { temp = *t1;
	   t1 = &temp;
	 }

	x = t2->n.x; y = t2->n.y; z = t2->n.z;	
	r->n.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->n.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->n.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	x = t2->o.x; y = t2->o.y; z = t2->o.z;	
	r->o.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->o.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->o.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	x = t2->a.x; y = t2->a.y; z = t2->a.z;	
	r->a.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->a.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->a.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	x = t2->p.x; y = t2->p.y; z = t2->p.z;	
	r->p.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->p.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->p.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	r->p.x += t1->p.x;
	r->p.y += t1->p.y;
	r->p.z += t1->p.z;
#endif
	return(r);
}

TRSF *invertTrsf(i, t)
register TRSF *i;		/* 4(ap) -> r11 */
register TRSF *t;		/* 8(ap) -> r10 */
{
#if vax
	asm ("cmpl	r10,r11");
 	asm ("bneq	1f");
	asm ("subl2	$TRSF_SIZE,sp");
	asm ("movc3	$TRSF_SIZE,(r10),(sp)");
	asm ("movl	sp,r10");
	asm ("1:");

	asm ("movf	nx(r10),nx(r11)");
	asm ("movf	ny(r10),ox(r11)");
	asm ("movf	nz(r10),ax(r11)");
 	asm ("movf	ox(r10),ny(r11)");
	asm ("movf	oy(r10),oy(r11)");
	asm ("movf	oz(r10),ay(r11)");
 	asm ("movf	ax(r10),nz(r11)");
	asm ("movf	ay(r10),oz(r11)");
	asm ("movf	az(r10),az(r11)");

	asm ("mnegf	px(r10),r4");
	asm ("mnegf	py(r10),r3");
	asm ("mnegf	pz(r10),r2");

	asm ("mulf3	nx(r10),r4,r0");
	asm ("mulf3	ny(r10),r3,r1");
	asm ("addf2	r1,r0");
	asm ("mulf3	nz(r10),r2,r1");
	asm ("addf3	r1,r0,px(r11)");

	asm ("mulf3	ox(r10),r4,r0");
	asm ("mulf3	oy(r10),r3,r1");
	asm ("addf2	r1,r0");
	asm ("mulf3	oz(r10),r2,r1");
	asm ("addf3	r1,r0,py(r11)");

	asm ("mulf3	ax(r10),r4,r0");
	asm ("mulf3	ay(r10),r3,r1");
	asm ("addf2	r1,r0");
	asm ("mulf3	az(r10),r2,r1");
	asm ("addf3	r1,r0,pz(r11)");

	asm ("cmpl	4(ap),8(ap)");
	asm ("bneq	1f");
	asm ("addl2	$TRSF_SIZE,sp");
	asm ("1:");
#else
	TRSF temp;

	if (i == t)
	 { temp = *t;
	   t = &temp;
	 }

	i->n.x = t->n.x;
	i->n.y = t->o.x;
	i->n.z = t->a.x;
	i->o.x = t->n.y;
	i->o.y = t->o.y;
	i->o.z = t->a.y;
	i->a.x = t->n.z;
	i->a.y = t->o.z;
	i->a.z = t->a.z;

	i->p.x = - (t->p.x * t->n.x + t->p.y * t->n.y + t->p.z * t->n.z);
	i->p.y = - (t->p.x * t->o.x + t->p.y * t->o.y + t->p.z * t->o.z);
	i->p.z = - (t->p.x * t->a.x + t->p.y * t->a.y + t->p.z * t->a.z);
#endif
	return(i);
}


TRSF *multLiTrsf (r, t1, t2)
register TRSF *r;		/* 4(ap) -> r11 */
register TRSF *t1;		/* 8(ap) -> r10 */
register TRSF *t2;		/* 12(ap) -> r9 */
{
#if vax
	asm("pushr	$0x1c0");

	asm ("cmpl	r10,r11");
	asm ("bneq	1f");
	asm ("subl2	$TRSF_SIZE,sp");
	asm ("movc3	$TRSF_SIZE,(r10),(sp)");
	asm ("movl	sp,r10");
	asm ("1:");

	asm ("movf	nx(r10),r8");
	asm ("movf	ny(r10),r7");
	asm ("movf	nz(r10),r6");

	asm ("movf	nx(r9),r5");
	asm ("movf	ny(r9),r4");
	asm ("movf	nz(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute nx */
 	asm ("mulf3	r7,r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	r6,r3,r1");
	asm ("addf3	r1,r0,nx(r11)");

	asm ("mulf3	ox(r10),r5,r0");	/* compute ny */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	oz(r10),r3,r1");
	asm ("addf3	r1,r0,ny(r11)");

	asm ("mulf3	ax(r10),r5,r0");	/* compute nz */
 	asm ("mulf3	ay(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,nz(r11)");

	asm ("movf	ox(r9),r5");
	asm ("movf	oy(r9),r4");
	asm ("movf	oz(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute ox */
 	asm ("mulf3	r7,r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	r6,r3,r1");
	asm ("addf3	r1,r0,ox(r11)");

	asm ("mulf3	ox(r10),r5,r0");	/* compute oy */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	oz(r10),r3,r1");
	asm ("addf3	r1,r0,oy(r11)");

	asm ("mulf3	ax(r10),r5,r0");	/* compute oz */
 	asm ("mulf3	ay(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,oz(r11)");

	asm ("movf	ax(r9),r5");
	asm ("movf	ay(r9),r4");
	asm ("movf	az(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute ax */
 	asm ("mulf3	r7,r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	r6,r3,r1");
	asm ("addf3	r1,r0,ax(r11)");

	asm ("mulf3	ox(r10),r5,r0");	/* compute ay */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	oz(r10),r3,r1");
	asm ("addf3	r1,r0,ay(r11)");

	asm ("mulf3	ax(r10),r5,r0");	/* compute az */
 	asm ("mulf3	ay(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,az(r11)");

	asm ("subf3	px(r10),px(r9),r5");
	asm ("subf3	py(r10),py(r9),r4");
	asm ("subf3	pz(r10),pz(r9),r3");

	asm ("mulf3	r8,r5,r0");		/* compute px */
 	asm ("mulf3	r7,r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	r6,r3,r1");
	asm ("addf3	r1,r0,px(r11)");

	asm ("mulf3	ox(r10),r5,r0");	/* compute py */
 	asm ("mulf3	oy(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	oz(r10),r3,r1");
	asm ("addf3	r1,r0,py(r11)");

	asm ("mulf3	ax(r10),r5,r0");	/* compute pz */
 	asm ("mulf3	ay(r10),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r10),r3,r1");
	asm ("addf3	r1,r0,pz(r11)");

	asm ("cmpl	4(ap), 8(ap)");
	asm ("bneq	1f");
	asm ("addl2	$TRSF_SIZE,sp");
	asm ("1:");
	asm ("popr	$0x1c0");	
#else
	float x, y, z;
	TRSF temp;

	if (t1 == r)
	 { temp = *t1;
	   t1 = &temp;
	 }

	x = t2->n.x; y = t2->n.y; z = t2->n.z;
	r->n.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->n.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->n.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	x = t2->o.x; y = t2->o.y; z = t2->o.z;
	r->o.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->o.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->o.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	x = t2->a.x; y = t2->a.y; z = t2->a.z;
	r->a.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->a.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->a.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	x = t2->p.x - t1->p.x; y = t2->p.y - t1->p.y; z = t2->p.z - t1->p.z;
	r->p.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->p.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->p.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

#endif
	return (r);
}

TRSF *multRiTrsf (r, t1, t2)
register TRSF *r;		/* 4(ap) -> r11 */
register TRSF *t1;		/* 8(ap) -> r10 */
register TRSF *t2;		/* 12(ap) -> r9 */
{
#if vax
	asm("pushr	$0x1c0");

	asm ("cmpl	r9,r11");
	asm ("bneq	1f");
	asm ("subl2	$TRSF_SIZE,sp");
	asm ("movc3	$TRSF_SIZE,(r9),(sp)");
	asm ("movl	sp,r9");
	asm ("1:");

	asm ("movf	nx(r9),r8");
	asm ("movf	ny(r9),r7");
	asm ("movf	nz(r9),r6");

	asm ("movf	nx(r10),r5");
	asm ("movf	ox(r10),r4");
	asm ("movf	ax(r10),r3");

	asm ("mulf3	r8,r5,r0");		/* compute nx */
 	asm ("mulf3	ox(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r9),r3,r1");
	asm ("addf3	r1,r0,nx(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute ox */
 	asm ("mulf3	oy(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r9),r3,r1");
	asm ("addf3	r1,r0,ox(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute ax */
 	asm ("mulf3	oz(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r9),r3,r1");
	asm ("addf3	r1,r0,ax(r11)");

	asm ("movf	ny(r10),r5");
	asm ("movf	oy(r10),r4");
	asm ("movf	ay(r10),r3");

	asm ("mulf3	r8,r5,r0");		/* compute ny */
 	asm ("mulf3	ox(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r9),r3,r1");
	asm ("addf3	r1,r0,ny(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute oy */
 	asm ("mulf3	oy(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r9),r3,r1");
	asm ("addf3	r1,r0,oy(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute ay */
 	asm ("mulf3	oz(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r9),r3,r1");
	asm ("addf3	r1,r0,ay(r11)");

	asm ("movf	nz(r10),r5");
	asm ("movf	oz(r10),r4");
	asm ("movf	az(r10),r3");

	asm ("mulf3	r8,r5,r0");		/* compute nz */
 	asm ("mulf3	ox(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r9),r3,r1");
	asm ("addf3	r1,r0,nz(r11)");

	asm ("mulf3	r7,r5,r0");		/* compute oz */
 	asm ("mulf3	oy(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r9),r3,r1");
	asm ("addf3	r1,r0,oz(r11)");

	asm ("mulf3	r6,r5,r0");		/* compute az */
 	asm ("mulf3	oz(r9),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r9),r3,r1");
	asm ("addf3	r1,r0,az(r11)");

	asm ("mnegf	px(r9),r5");
	asm ("mnegf	py(r9),r4");
	asm ("mnegf	pz(r9),r3");

	asm ("mulf3	nx(r11),r5,r0");	/* compute px */
	asm ("addf2	px(r10),r0");
 	asm ("mulf3	ox(r11),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ax(r11),r3,r1");
	asm ("addf3	r1,r0,px(r11)");

	asm ("mulf3	ny(r11),r5,r0");	/* compute py */
	asm ("addf2	py(r10),r0");
 	asm ("mulf3	oy(r11),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	ay(r11),r3,r1");
	asm ("addf3	r1,r0,py(r11)");

	asm ("mulf3	nz(r11),r5,r0");	/* compute pz */
	asm ("addf2	pz(r10),r0");
 	asm ("mulf3	oz(r11),r4,r1");
	asm ("addf2	r1,r0");
 	asm ("mulf3	az(r11),r3,r1");
	asm ("addf3	r1,r0,pz(r11)");

	asm ("cmpl	4(ap),12(ap)");
	asm ("bneq	1f");
	asm ("addl2	$TRSF_SIZE,sp");
	asm ("1:");
	asm ("popr	$0x1c0");	
#else
	float n, o, a, px, py, pz;
	TRSF temp;

	if (t2 == r)
	 { temp = *t2;
	   t2 = &temp;
	 }
	
	n = t1->n.x; o = t1->o.x; a = t1->a.x;
	r->n.x = n * t2->n.x + o * t2->o.x + a * t2->a.x;
	r->o.x = n * t2->n.y + o * t2->o.y + a * t2->a.y;
	r->a.x = n * t2->n.z + o * t2->o.z + a * t2->a.z;

	n = t1->n.y; o = t1->o.y; a = t1->a.y;
	r->n.y = n * t2->n.x + o * t2->o.x + a * t2->a.x;
	r->o.y = n * t2->n.y + o * t2->o.y + a * t2->a.y;
	r->a.y = n * t2->n.z + o * t2->o.z + a * t2->a.z;

	n = t1->n.z; o = t1->o.z; a = t1->a.z;
	r->n.z = n * t2->n.x + o * t2->o.x + a * t2->a.x;
	r->o.z = n * t2->n.y + o * t2->o.y + a * t2->a.y;
	r->a.z = n * t2->n.z + o * t2->o.z + a * t2->a.z;

	px = -t2->p.x; py = -t2->p.y; pz = -t2->p.z;
	r->p.x = t1->p.x + px * r->n.x + py * r->o.x + pz * r->a.x;
	r->p.y = t1->p.y + px * r->n.y + py * r->o.y + pz * r->a.y;
	r->p.z = t1->p.z + px * r->n.z + py * r->o.z + pz * r->a.z;
#endif
	return (r);
}

