//  Copyright (c) CNES  2008
//
//  This software is part of CelestLab, a CNES toolbox for Scilab
//
//  This software is governed by the CeCILL  license under French law and
//  abiding by the rules of distribution of free software.  You can  use,
//  modify and/ or redistribute the software under the terms of the CeCILL
//  license as circulated by CEA, CNRS and INRIA at the following URL
//  'http://www.cecill.info'.

function [M] = CL_fr_topoNMat(orig)
// Topocentric North frame transformation matrix
//
// Calling Sequence
// M = CL_fr_topoNMat(orig)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the frame transformation matrix (<b>M</b>) to the local topocentric North frame 
// of some location defined by its elliptical (i.e. geodetic) coordinates. </p>
// <p></p>
// <p>By convention, multiplying <b>M</b> by coordinates relative to the initial frame 
// yields coordinates relative to the topocentric North frame. The "initial" frame is by definition
// the frame in which the location is defined. </p>
// <p></p></listitem>
// <listitem>
// <p>See <link linkend="Local frames">Local frames</link> for more details on the definition of 
// local frames.</p> 
// </listitem>
// </itemizedlist>
//
// Parameters
// orig: Origin of the Topocentric North frame in elliptical (geodetic) coordinates [rad;rad;m] (3xN)
// M : Frame transformation matrix to the topocentric North frame (3x3xN)
//
// Bibliography
// 1) CNES - MSLIB FORTRAN 90, Volume T (mt_def_topo_N)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Conversion from ECF to topoN (change of origin included): 
// orig = [CL_deg2rad(0) ; CL_deg2rad(40) ; 0]; // ECF 
// M = CL_fr_topoNMat(orig);
// pos = [ 1000.e3 ; 6578.e3 ; 2000.e3 ]; // ECF 
// pos_orig = CL_co_ell2car(orig); // orig in cartesian coord. 
// pos_topoN = M * (pos - pos_orig)


// Declarations:

// Code:

// check number of output arguments 
[lhs,rhs] = argn();

if (lhs > 1)
  CL__error("Invalid number of output arguments");
end

if (rhs < 1)
  CL__error("Invalid number of input arguments");
end

N = size(orig,2);


// NB: the vectors u,v,w are the COLUMNS of the transformation
// matrix from terrestrial frame to topocentric North frame 

lon = orig(1,:);
lat = orig(2,:);

u = [ -sin(lat).*cos(lon); sin(lon); cos(lat).*cos(lon) ]; 
v = [ -sin(lat).*sin(lon); -cos(lon); cos(lat).*sin(lon) ]; 
w = [ cos(lat); zeros(lon);  sin(lat)] ;

M = [];

if (N > 0) 
  M = (hypermat([3,3,N], [u; v; w])); 
  if (N == 1); M = M(:,:,1); end
end


endfunction