#pragma On(Align_all_labels)
#pragma On(Align_labels)

#include "../tornado.h"

void InterpolateAngle(IANGLE *o,IANGLE *a1,IANGLE *a2,int a,int b)
{
	IPOINT e1,u1,e2,u2,et,ut;

	AngleToVector(&e1,&u1,a1);
	AngleToVector(&e2,&u2,a2);

	InterpolateVector(&et,&e1,&e2,a,b);
	InterpolateVector(&ut,&u1,&u2,a,b);
	VectorToAngle(o,&et,&ut);
}

void InterpolateVector(IPOINT *o,IPOINT *v1,IPOINT *v2,int a,int b)
{
	IANGLE ea;
	int d;

	if(a+b==0)
	{
		*o = *v1;
		return;
	}

	VectorToAngle(&ea,v1,v2);
	RotGtoLHq(1,o,v2,&ea);
	d=Angle2(o->z,o->y)*a/(a+b);
	o->x=0;
	o->y=FxSinHq(d);
	o->z=FxCosHq(d);
	RotLtoGHq(1,o,o,&ea);
}

void AngleToVector(IPOINT *e,IPOINT *u,IANGLE *a)
{
	e->x=0;
	e->y=0;
	e->z=0x10000;
	RotLtoGHq(1,e,e,a);
	u->x=0;
	u->y=0x10000;
	u->z=0;
	RotLtoGHq(1,u,u,a);
}

void VectorToAngle(IANGLE *an,IPOINT *ev,IPOINT *uv)
{
	IPOINT t;

	an->h=Angle2(ev->z,- ev->x);
	an->p=Angle2(Length2(ev->x,ev->z),ev->y);
	an->b=0;

	RotGtoLHq(1,&t,uv,an);
	an->b=Angle2(t.y,-t.x);
}