//  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 [psol1,psol2,numsol] = CL_gm_inters3dLineEllips(pos,u, posc,er,obla);
// Intersection of a half-ray with an ellipsoid.
// 
// Calling Sequence
// [psol1,psol2,numsol] = CL_gm_inters3dLineEllips(pos,u [,posc,er,obla])
//
// Description
// <itemizedlist>
// <listitem>
// <p>Computes the intersection(s) of a half-ray (i.e. half-line) with an ellipsoid of revolution.</p>
// <p>The half-ray is defined by its origin <b>pos</b> and direction <b>u</b>. </p>
// <p>The ellipsoid is an ellipsoid of revolution around the z-axis, and is defined by its centre <b>posc</b>, 
// radius <b>er</b> and oblateness <b>obla</b>.</p>
// <p>There can be 0,1 or 2 intersections.</p>
// <p>If there are 2 intersections, psol1 is the closest to pos (and in the direction of u).</p> 
// <p>If psol1 or psol2 do not exist, [%nan;%nan;%nan] is returned. </p>
// <p></p></listitem>
// <listitem>
// <p><b>Note</b>: Due to numerical errors, the altitudes with respect to the ellipsoid might be slightly 
// negative. One way around is to force the altitude to 0 (after a call to CL_co_car2ell). </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos: Origin of the half-ray (3xN or 3x1)
// u: Direction of the half-ray (not necessarily a unit vector)  (3xN or 3x1)
// posc: (optional) Position of the centre of the ellipsoid. Default is [0;0;0]. (3x1)  
// er: (optional) Ellipsoid equatorial radius [m]. Default is %CL_eqRad. (1x1)
// obla: (optional) Ellipsoid oblateness. Default is %CL_obla. (1x1)
// psol1: First solution, %nan*[1;1;1] if there is no intersection. (3xN) 
// psol2: Second solution, %nan*[1;1;1] if there are less than 2 intersections. (3xN) 
// numsol: Number of solutions for convenience: 0,1 or 2 (1xN)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Intersection of line of sight with Earth
// pos = [ 10000 ; 100 ; 500 ] * 1000; // origin, meters
// u = [ -1 ; 0.2 ; 0.6]; // direction of line of sight
// [psol1] = CL_gm_inters3dLineEllips(pos,u);
// // Check results :
// CL_co_car2ell(psol1) // => third coordinate (altitude) == 0
// CL_vectAngle(u, psol1-pos) // => 0

// Declarations:

// Code:
if (~exists('posc', 'local')); posc = [0;0;0]; end
if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("obla", "local")); obla = CL__dataGetEnv("obla"); end

if (find(CL_dot(u) == 0) <> [])
  CL__error("Invalid u vector (zero norm)"); 
end

// Handle of [] arguments
if (pos == [] | u == [] | posc == [] | er == [] | obla == [])
  psol1 = [];
  psol2 = [];
  numsol = [];
  return;
end

[pos, u, posc, N] = CL__checkInputs(pos,3, u,3, posc,3);

psol1 = %nan * ones(pos);
psol2 = %nan * ones(pos);
numsol = %nan * ones(pos(1,:));

// half ray : parametric equation (parameter = t) :
// x = xp + ux*t
// y = yp + uy*t
// z = zp + uz*t
//
// ellipsoid : 
// ((x-xc)/er)^2 + ((y-yc)/er)^2 + ((z-zc)/pr)^2 = 1 
// with pr = er*(1-obla) // polar radius
//
// --> equation of second degree in t:
//

pr = er*(1-obla);
U = CL_dMult(u, 1 ./ [er;er;pr]);
POS = CL_dMult(pos-posc, 1 ./ [er;er;pr]);

a = CL_dot(U);  
b = CL_dot(POS,U); // = b' = half value of deg 1 coefficient
c = CL_dot(POS) - 1; 

D = b.^2 - a.*c; // discriminant

I = find(D >= 0);
numsol(find(D < 0)) = 0; // index that are not %nan and we are sure have no intersections
numsol(I) = 0; // index that are not %nan and may have an intersection 

if (I <> [])
  
  t1 = (-b - real(sqrt(D))) ./ a; // NB: only t1(I) is valid
  t2 = (-b + real(sqrt(D))) ./ a; // NB: only t2(I) is valid
  
  // keep positive solutions only (half-ray only)
  
  // case: 2 positive solutions
  J = I(find(t1(I) >= 0));
  numsol(J) = 2;
  psol1(:,J) = pos(:,J) + CL_dMult(t1(J), u(:,J)); 
  psol2(:,J) = pos(:,J) + CL_dMult(t2(J), u(:,J));
    
  // case: 1 positive solution
  J = I(find(t1(I) < 0 & t2(I) >=0));
  numsol(J) = 1; 
  psol1(:,J) = pos(:,J) + CL_dMult(t2(J), u(:,J)); 

end


endfunction