// ****************************************************************************
//
//  $Id: cisLine3D.cpp,v 1.1.1.1 2001/09/27 20:12:22 anton Exp $
//
//  Description:
//		
//
//  Usage:
//		
//
//  See Also:
//
//  Author(s):	Ofri Sadowsky
//  Created on: 2003-03-24
//
//
//              Developed by the Engineering Research Center for
//          Computer-Integrated Surgical Systems & Technology (CISST)
//
//               Copyright(c) 2001, The Johns Hopkins University
//                          All Rights Reserved.
//
//  Experimental Software - Not certified for clinical use.
//  For use only by CISST sponsored research projects.  For use outside of
//  CISST, please contact Dr. Russell Taylor, CISST Director, at rht@cs.jhu.edu
//  or Dr. Gabor Fichtinger, CISST Engineering Director, at gabor@cs.jhu.edu.
//  
// ****************************************************************************


#include "cisLine3D.h"

#include <assert.h>

#include "cisSegment3D.h"

const cisScalar cisLine3D::DefaultTolerance = 1.e-8;


void cisLine3D::Set(const GlobalPointType & pt, const GlobalDirectionType & dir)
{
	assert( (dir != cisDummyVec3) && (dir != cisErrorVec3) );

	Dir = dir.Normalize();

	// I thought it's faster not to re-evaluate the conditions z==0 etc.
	if (Dir.z < 0)
		Dir = -Dir;
	else if (Dir.z == 0) {
		if (Dir.y < 0)
			Dir = -Dir;
		else if (Dir.y == 0) {
			if (Dir.x < 0)
				Dir = -Dir;
		}
	}

	cisScalar proj = pt * Dir;
	Point = pt - proj * Dir;
}


cisLine3D::LocalPointType cisLine3D::ClosestPointTo(const SelfType & other) const
{
	/*****
	 * We represent the point on this line as parametrized by lambda:
	 *   p(lambda) = p1 + lambda * d1 = Point + lambda * Dir.
	 *
	 *   r(lambda) = p(lambda) - p2 = p(lambda) - other.Point
	 *
	 * The square distance from p(lambda) to the other line is given by the
	 * formula in DistanceSquarePointFromLine:
	 *   g(lambda) = || r(lambda) ||^2 - [r(lambda) * other.Dir]^2
	 *
	 * The derivative of this quadratic is:
	 *   g'(lambda) = 2(p1-p2)*d1 - 2*[(p1-p2)*d2]*[d1*d2] + 2*[1 - (d1*d2)^2]*lambda
	 *
	 * And the minimum is obtained for
	 *   lambda = (1 - (d1*d2)^2)^(-1) * [ (p1-p2)*( d2*(d1*d2) - d1 ) ]
	 *
	 * Note that if d2==d1 there is no unique solution, as the lines are parallel. In that
	 * case, we arbitrarily choose this->Point
	 */

	if (this->IsParallelTo(other))
		return this->GetPoint();

	cisVec3 pointProj(this->GetPoint() - other.GetPoint());
	cisScalar dirProj = this->GetDirection() * other.GetDirection();

	cisScalar lambda = (1 / (1 - dirProj*dirProj)) *
		(pointProj) * (  (other.Dir * dirProj) - this->Dir  );

	return this->EvalAt(lambda);
}


cisLine3D::LocalPointType cisLine3D::ClosestPointTo(const cisSegment3D & segment) const
{
	cisLine3D segmentLine(segment.GetPoint1(), segment.GetPoint2() - segment.GetPoint1());

	cisVec3 closestPointOnLine = ClosestPointTo(segmentLine);

	cisScalar point1Param = ProjectPointLocal(segment.GetPoint1());
	cisScalar point2Param = ProjectPointLocal(segment.GetPoint2());

	// we need to align the direction of the segment with the positive direction
	// of this line.
	if (point1Param > point2Param) {
		cisScalar tmp = point2Param;
		point2Param = point1Param;
		point1Param = tmp;
	}

	cisScalar closestPointParam = ProjectPointLocal(closestPointOnLine);

	if (closestPointParam <= point1Param)
		return EvalAt(point1Param);

	if (closestPointParam >= point2Param)
		return EvalAt(point2Param);

	return closestPointOnLine;
}




#if cisLine3D_with_angles
void cisLine3D::SetFromAnglesDists(cisScalar angleY, cisScalar angleZ, cisScalar distY, cisScalar distZ)
{
  cisScalar s1 = sin(angleY);
  cisScalar c1 = cos(angleY);
  cisScalar s2 = sin(angleZ);
  cisScalar c2 = cos(angleZ);

  cisVec3 pt(-s2*c1*distY + s1*distZ, c2*distY, -s1*s2*distY + c1*distZ);
  cisVec3 dir((c1*c2,s2, -s1*c2).Normalize());
  Set(pt, dir);
}



void cisLine3D::GetAngleDists(cisScalar& angleY, cisScalar& angleZ, cisScalar& distY, cisScalar& distZ)
{
  cisScalar s2 = Dir.y;
  angleZ = asin(Dir.y);
  cisScalar c2 = cos(angleZ);

  if (cisABS(c2) < cisEPSILON)
    return;

  cisScalar s1 = -Dir.z/c2;
  cisScalar c1 = Dir.x/c2;

  angleY = atan(c1/s1);
  s1 = sin(cisDEG_2_RAD(angleY));
  c1 = cos(cisDEG_2_RAD(angleY));

  distY = Point.y / c2;
  distZ = 0;

  int sumCt = 0;

  if (cisABS(c1) > cisABS(s1)) 
    {
      distZ += (Point.z + distY*s1*s2) / c1;
      sumCt ++;
    }

  else
    {
      distZ += 	(Point.x  + s2*c1*distY) / s1;
      sumCt ++;
    }

  distZ /= sumCt;
	
  angleY = cisDEG_2_RAD(angleY);
  angleZ = cisDEG_2_RAD(angleZ);
}
#endif

// ****************************************************************************
//                              Change History
// ****************************************************************************
//
//  $Log: cisLine3D.cpp,v $
//  Revision 1.1.1.1  2001/09/27 20:12:22  anton
//  Creation of new repository for CIS 2.
//
//
//  Older:
//
//  Revision 1.1  2001/03/26 19:32:05  anton
//  Import of GeomObj within CIS from Andy Bzostek code with some formating and cleaning.  Add some -inl.hpp files.
//
//
// ****************************************************************************