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

#include "../tornado.h"

int PitchUp(IANGLE *dst,IANGLE *src,int d)
{
	IPOINT ev,uv;

	if(d==0)
	{
		*dst=*src;
		return 0;
	}

	/* Make Eye Line Vector in src Axis*/
	ev.x= 0;
	ev.y= 512*FxSinHq(d);
	ev.z= 512*FxCosHq(d);

	/* Rotate To Global Axis */
	RotLtoGHq(1,&ev,&ev,src);


	/* Make Up Vector in src Axis*/
	uv.x= 0;
	uv.y= 512*FxCosHq(d);
	uv.z=-512*FxSinHq(d);

	/* Rotate To Global Axis */
	RotLtoGHq(1,&uv,&uv,src);


	dst->h= Angle2(ev.z,- ev.x);
	dst->p= Angle2(Length2(ev.x,ev.z),ev.y);
	dst->b= 0;
	RotGtoLHq(1,&uv,&uv,dst);

	dst->b= Angle2(uv.y,-uv.x);

	return 0;
}



int YawLeft(IANGLE *dst,IANGLE *src,int d)
{
	IPOINT ev,uv;

	if(d==0)
	{
		*dst=*src;
		return 0;
	}

	ev.x=-512*FxSinHq(d);  /* Eye Line Vector */
	ev.y= 0;
	ev.z= 512*FxCosHq(d);
	RotLtoGHq(1,&ev,&ev,src);

	uv.x=0;            /* Up Vector */
	uv.y=0x2000000;
	uv.z=0;
	RotLtoGHq(1,&uv,&uv,src);

	dst->h= Angle2(ev.z,-ev.x);
	dst->p= Angle2(Length2(ev.x,ev.z),ev.y);
	dst->b= 0;
	RotGtoLHq(1,&uv,&uv,dst);

	dst->b= Angle2(uv.y,-uv.x);

	return 0;
}