/* Vector manipulation main code V1.71 16/06/07
   See vec.h for details
   Copyright 2008 Jeffrey Lee
   This file is part of WOUM.
   WOUM is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   WOUM is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with WOUM.  If not, see <http://www.gnu.org/licenses/>.
*/

void fix_op(NAME,_add,NAME *a,const NAME *b)
{
	a->x += b->x;
	a->y += b->y;
	a->z += b->z;
}

void fix_op(NAME,_sub,NAME *a,const NAME *b)
{
	a->x -= b->x;
	a->y -= b->y;
	a->z -= b->z;
}

void fix_op(NAME,_neg,NAME *a)
{
	a->x = -a->x;
	a->y = -a->y;
	a->z = -a->z;
}

void fix_op(NAME,_mul,NAME *a,const NAME *b)
{
	a->x = fix_op(TYPE,_mul,a->x,b->x);
	a->y = fix_op(TYPE,_mul,a->y,b->y);
	a->z = fix_op(TYPE,_mul,a->z,b->z);
}

void fix_op(NAME,_div,NAME *a,const NAME *b)
{
	a->x = fix_op(TYPE,_div,a->x,b->x);
	a->y = fix_op(TYPE,_div,a->y,b->y);
	a->z = fix_op(TYPE,_div,a->z,b->z);
}

void fix_op(NAME,_mul2,NAME *a,TYPE b)
{
	a->x = fix_op(TYPE,_mul,a->x,b);
	a->y = fix_op(TYPE,_mul,a->y,b);
	a->z = fix_op(TYPE,_mul,a->z,b);
}

void fix_op(NAME,_div2,NAME *a,TYPE b)
{
	fix_op(TYPE,_tridiv,&(a->x),&(a->y),&(a->z),b);
}

void fix_op(NAME,_div3,TYPE a,NAME *b)
{
	b->x = fix_op(TYPE,_div,a,b->x);
	b->y = fix_op(TYPE,_div,a,b->y);
	b->z = fix_op(TYPE,_div,a,b->z);
}

TYPE fix_op(NAME,_len,const NAME *v)
{
	return fix_op(TYPE,_vlen,v->x,v->y,v->z);
}

void fix_op(NAME,_normalize,NAME *v)
{
	fix_op(TYPE,_tridiv,&(v->x),&(v->y),&(v->z),fix_op(NAME,_len,v));
}

TYPE fix_op(NAME,_dotprod,const NAME *a,const NAME *b)
{
	return fix_op(TYPE,_mul,a->x,b->x) + fix_op(TYPE,_mul,a->y,b->y) + fix_op(TYPE,_mul,a->z,b->z);
}

void fix_op(NAME,_crossprod,const NAME *a,const NAME *b,NAME *c)
{
	c->x = fix_op(TYPE,_mul,a->y,b->z) - fix_op(TYPE,_mul,a->z,b->y);
	c->y = fix_op(TYPE,_mul,a->z,b->x) - fix_op(TYPE,_mul,a->x,b->z);
	c->z = fix_op(TYPE,_mul,a->x,b->y) - fix_op(TYPE,_mul,a->y,b->x);
}

TYPE fix_op(NAME,_movetox,NAME *p,const NAME *d,TYPE tx,TYPE recdx)
{
	TYPE n;
	n = fix_op(TYPE,_mul,(tx - p->x),recdx);
	p->x = tx;
	p->y += fix_op(TYPE,_mul,n,d->y);
	p->z += fix_op(TYPE,_mul,n,d->z);
	return n;
}

TYPE fix_op(NAME,_movetoy,NAME *p,const NAME *d,TYPE ty,TYPE recdy)
{
	TYPE n;
	n = fix_op(TYPE,_mul,(ty - p->y),recdy);
	p->y = ty;
	p->x += fix_op(TYPE,_mul,n,d->x);
	p->z += fix_op(TYPE,_mul,n,d->z);
	return n;
}

TYPE fix_op(NAME,_movetoz,NAME *p,const NAME *d,TYPE tz,TYPE recdz)
{
	TYPE n;
	n = fix_op(TYPE,_mul,(tz - p->z),recdz);
	p->z = tz;
	p->y += fix_op(TYPE,_mul,n,d->y);
	p->x += fix_op(TYPE,_mul,n,d->x);
	return n;
}

void fix_op(NAME,_rotatex,NAME *v,TYPE a)
{
	/* simple rotation around x axis */
	TYPE ny,ca,sa;
	ca = fix_op(TYPE,_cos,a);
	sa = fix_op(TYPE,_sin,a);
	/* we want positive y to translate to positive z
	   ny = cos*y-sin*z
	   nz = sin*y+cos*z */
	ny = fix_op(TYPE,_mul,ca,v->y)-fix_op(TYPE,_mul,sa,v->z);
	v->z = fix_op(TYPE,_mul,sa,v->y)+fix_op(TYPE,_mul,ca,v->z);
	v->y = ny;
}

void fix_op(NAME,_rotatey,NAME *v,TYPE a)
{
	/* simple rotation around y axis */
	TYPE nz,ca,sa;
	ca = fix_op(TYPE,_cos,a);
	sa = fix_op(TYPE,_sin,a);
	/* we want positive z to translate to positive x
	   nz = cos*z-sin*x
	   nx = sin*z+cos*x */
	nz = fix_op(TYPE,_mul,ca,v->z)-fix_op(TYPE,_mul,sa,v->x);
	v->x = fix_op(TYPE,_mul,sa,v->z)+fix_op(TYPE,_mul,ca,v->x);
	v->z = nz;
}

void fix_op(NAME,_rotatez,NAME *v,TYPE a)
{
	/* simple rotation around z axis */
	TYPE nx,ca,sa;
	ca = fix_op(TYPE,_cos,a);
	sa = fix_op(TYPE,_sin,a);
	/* we want positive x to translate to positive y
	   nx = cos*x-sin*y
	   ny = sin*x+cos*y */
	nx = fix_op(TYPE,_mul,ca,v->x)-fix_op(TYPE,_mul,sa,v->y);
	v->y = fix_op(TYPE,_mul,sa,v->x)+fix_op(TYPE,_mul,ca,v->y);
	v->x = nx;
}

#define X1 v1->x
#define X2 v2->x
#define X3 v3->x
#define Y1 v1->y
#define Y2 v2->y
#define Y3 v3->y
#define Z1 v1->z
#define Z2 v2->z
#define Z3 v3->z

TYPE fix_op(NAME,_toplane,const NAME *v1,const NAME *v2,const NAME *v3,NAME *p)
{
	p->x = fix_op(TYPE,_mul,Y1,(Z2-Z3)) + fix_op(TYPE,_mul,Y2,(Z3-Z1)) + fix_op(TYPE,_mul,Y3,(Z1-Z2));
	p->y = fix_op(TYPE,_mul,Z1,(X2-X3)) + fix_op(TYPE,_mul,Z2,(X3-X1)) + fix_op(TYPE,_mul,Z3,(X1-X2));
	p->z = fix_op(TYPE,_mul,X1,(Y2-Y3)) + fix_op(TYPE,_mul,X2,(Y3-Y1)) + fix_op(TYPE,_mul,X3,(Y1-Y2));
	return
 -(fix_op(TYPE,_mul,X1,(fix_op(TYPE,_mul,Y2,Z3) - fix_op(TYPE,_mul,Y3,Z2)))
 + fix_op(TYPE,_mul,X2,(fix_op(TYPE,_mul,Y3,Z1) - fix_op(TYPE,_mul,Y1,Z3)))
 + fix_op(TYPE,_mul,X3,(fix_op(TYPE,_mul,Y1,Z2) - fix_op(TYPE,_mul,Y2,Z1))));
}

#undef X1
#undef X2
#undef X3
#undef Y1
#undef Y2
#undef Y3
#undef Z1
#undef Z2
#undef Z3
