/* -*- C++ -*-
 * Copyright ©2004 Hugo Mills <hugo@carfax.org.uk>
 *
 * This software is distributed under the terms of the GNU GPL v3
 * For more information on the GPL, see the file COPYING or 
 * visit http://www.gnu.org/
 *
 * This software is distributed without warranty
 */

#include "magellan/xforbit.h"

#include <valarray>

void XfOrbit::setmatrices(double lon, double lat)
{
	std::valarray<double> matrix1(0.0, 9);
	std::valarray<double> matrix2(0.0, 9);

	// Rotation about the y axis
	matrix1[0] = matrix1[8] = cos(lon);
	matrix1[6] = sin(lon);
	matrix1[2] = -matrix1[6];
	matrix1[4] = 1.0;

	// Rotation about the x axis
	matrix2[4] = matrix2[8] = cos(lat);
	matrix2[7] = sin(lat);
	matrix2[5] = -matrix2[7];
	matrix2[0] = 1.0;

	mmul3(matrix, matrix2, matrix1);

	// Rotation about the y axis
	matrix1[2] = -matrix1[2];
	matrix1[6] = -matrix1[6];

	// Rotation about the x axis
	matrix2[5] = -matrix2[5];
	matrix2[7] = -matrix2[7];

	mmul3(imatrix, matrix1, matrix2);
}

bool XfOrbit::f(dvec3& result, const dvec3& ll) const
{
	vmul3(result, matrix, ll);
	return true;
}

bool XfOrbit::g(dvec3& result, const dvec3& p) const
{
	vmul3(result, imatrix, p);
	return true;
}