/*****************************************************************************
 * $Id: eng_math.cc,v 1.2 2006/10/20 03:51:47 devel Exp $
 * @file
 * @author	Jim E. Brooks  http://www.jimbrooks.org
 * @brief	Engine math functions.
 *//*
 * LEGAL:	COPYRIGHT (C) 2006 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE (GPL).
 *****************************************************************************/

#define ENG_MATH_CC 1
#include "eng.hh"
#include "eng_math.hh"
#include "gfx_stream.hh"

////////////////////////////////////////////////////////////////////////////////
// Orienting a matrix towards a vertex (target):
//
// Idea is to yaw and pitch a matrix to orient its Z axis to point towards a target.
//
// The trick is that the calculations of the angles to yaw/pitch are asymmetric.
// The yaw angle is calculated from the X coordinate vs. the 2D distance of the X,Z coordinates.
// The pitch angle is calculated from the Y coordinate vs. the full 3D distance.
//
// When target is at (1,1,1), this is the result.
// 0.5777^2 = 0.333  (1,1,1), when normalized, is (0.333,0.333,0.333)
// Xxyzw:   0.707107  -0.408248  0.577350
// Yxyzw:   0.000000   0.816497  0.577350
// Zxyzw:  -0.707107  -0.408248  0.577350
// Oxyzw:   0.000000   0.000000  0.000000
// yaw, pitch = 45.000000, 35.264389
// CheckAngle3D() = 0.000000
//
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Calculate radian to yaw a matrix towards a target vertex.
 * @return	radian
 * @param	m
 *          matrix (in/out)
 * @param	targetLocal
 *			Target translated relative to matrix space.
 *****************************************************************************/
static Radian
OrientMatrix_CalcAngleYaw( Matrix& m /*IN/OUT*/, const Vector3& targetLocal )
{
	// Calculate angle between YZ plane and X coordinate.
	fp dist = sqrtf( SQUARE(targetLocal.x)
                   + SQUARE(targetLocal.z) );
	if ( dist < 0.1 ) return 0.0;  // prevent divide error
	fp norm = targetLocal.x / dist;
	Radian rad = asin( norm );

	// Change sign of radian according to X,Z coordinate.
	// This affects and depends on the way RotateMatrixLocal() rotates.
	if ( targetLocal.z < 0 )
		rad += RADIAN_180;
	if ( (targetLocal.x < 0 && targetLocal.z < 0) || (targetLocal.x > 0 && targetLocal.z < 0) )
		rad = -rad;

	return rad;
}

/*****************************************************************************
 * Calculate radian to pitch a matrix towards a target vertex.
 * @return	radian
 * @param	m
 *          matrix (in/out)
 * @param	targetLocal
 *			Target translated relative to matrix space.
 *****************************************************************************/
static Radian
OrientMatrix_CalcAnglePitch( Matrix& m /*IN/OUT*/, const Vector3& targetLocal )
{
	// Calculate angle between Y coordinate and 3D distance.
	// Visualize the 3D distance as point the Y axis,
	// and the target is some 3D point elsewhere.
	// The angle is between the target's Y coordinate and the point on the Y axis.
	fp dist = sqrtf( SQUARE(targetLocal.x)
                   + SQUARE(targetLocal.y)
                   + SQUARE(targetLocal.z) );
	if ( dist < 0.1 ) return 0.0;  // prevent divide error
	fp norm = targetLocal.y / dist;
	return asin( norm );  // radian
}

/*****************************************************************************
 * Check 3D angle between matrix Z axis and target.
 *****************************************************************************/
#if DEBUG
static bool
OrientMatrix_Check( const Matrix& m, const Vector3& targetLocal )
{
	// Z axis coordinates are taken from Xz/Yz/Zz column (not Zx/Zy/Zx row).
	// For example, for target at (1,1,1), the right column corresponds to (1,1,1).
	// BTW 0.5777350^2 = 0.33333333
	// Xxyzw:  0.707107  -0.408248  0.577350
	// Yxyzw:  0.000000   0.816497  0.577350
	// Zxyzw:  -0.707107 -0.408248  0.577350
	Vector3 z;
	z.x = m[Xz];
	z.y = m[Yz];
	z.z = m[Zz];
	Radian rad = Angle( z, targetLocal );
	if ( EX( ABS(rad) < 0.1 ) )
	{
		return true;
	}
	else
	{
		*gStreamError << "ERROR[OrientMatrix]: angle > 0.1 " << rad << endl;
		return false;
	}
}
#endif // DEBUG

/*****************************************************************************
 * Orient a matrix so that the matrix -Z axis is pointing at a 3D point (target).
 * Only the matrix axis coefficients are changed (not the matrix origin).
 * Orient means a combination of rotations.
 *****************************************************************************/
void
OrientMatrix( Matrix& m /*IN/OUT*/, const Vector3& targetWorld )
{
	// Change only the matrix axis coefficients (not the matrix origin).
	m[Xx] = 1.0; m[Xy] = 0.0; m[Xz] = 0.0;
	m[Yx] = 0.0; m[Yy] = 1.0; m[Yz] = 0.0;
	m[Zx] = 0.0; m[Zy] = 0.0; m[Zz] = 1.0;

	// Translate target to matrix local space.
	Vector3 targetLocal;
	targetLocal.x = targetWorld.x - m[Ox];
	targetLocal.y = targetWorld.y - m[Oy];
	targetLocal.z = targetWorld.z - m[Oz];

	// A combination of yaw and pitch can orient
	// a matrix's Z axis towards a 3D vertex (target).
	Radian radYaw   = OrientMatrix_CalcAngleYaw(   m, targetLocal );
	Radian radPitch = OrientMatrix_CalcAnglePitch( m, targetLocal );
	RotateMatrixLocalRadian( AXIS_Y, radYaw,   m );
	RotateMatrixLocalRadian( AXIS_X, radPitch, m );

	DEBUG_CODE( OrientMatrix_Check( m, targetLocal ); )
}
