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

#include "tomahawk.h"


void InterpolateAngle(int o[3],int a1[3],int a2[3],int a,int b)
{
	int e1[3],u1[3],e2[3],u2[3],et[3],ut[3];

	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(int o[3],int v1[3],int v2[3],int a,int b)
{
	int ea[3],d;

	if(a+b==0)
	{
		o[0]=v1[0];
		o[1]=v1[1];
		o[2]=v1[2];
		return;
	}

	VectorToAngle(ea,v1,v2);
	RotGtoVHq(1,o,v2,ea);
	d=FxAngle(o[2],o[1])*a/(a+b);
	o[0]=0;
	o[1]=FxSinHq(d);
	o[2]=FxCosHq(d);
	RotVtoGHq(1,o,o,ea);
}

void AngleToVector(int e[3],int u[3],int a[3])
{
	e[0]=0;
	e[1]=0;
	e[2]=0x10000;
	RotVtoGHq(1,e,e,a);
	u[0]=0;
	u[1]=0x10000;
	u[2]=0;
	RotVtoGHq(1,u,u,a);
}

void VectorToAngle(int an[3],int ev[3],int uv[3])
{
	int t[3];

	an[0]=FxAngle(ev[2],-ev[0]);
	an[1]=FxAngle(FxLength(ev[0],ev[2]),ev[1]);
	an[2]=0;

	RotGtoVHq(1,t,uv,an);
	an[2]=FxAngle(t[1],-t[0]);
}
