//
// $Id: SphericalMatrix.cpp,v 1.6 2000/03/03 22:15:28 hobbs Exp $
//
// File: SphericalMatrix.cpp
// Purpose: 
// Created: 05-JUN-1998   John Hobbs
//
// $Revision: 1.6 $
//
//

// Include files
#include <geometry_system/components/SphericalMatrix.hpp>
#include <geometry_system/components/SphericalCoordinate.hpp>

using namespace::std;
using namespace::dgs;

// Global definitions
SphericalMatrix::SphericalMatrix(const MatrixD& errorMatrix, 
				 const SphericalCoordinate& lpt): MatrixD(3,3)
//
// Purpose: Construct a matrix in Cartesian coordinates given the matrix in
//          cylindrical coordinates.  
{
  double terms[9];
  double r = lpt.rxyz();
  double theta = lpt.theta();
  double phi = lpt.phi();

  terms[0] = sin(theta)*cos(phi);    // dx/dr    
  terms[1] = -r*sin(theta)*sin(phi); // dx/dphi  
  terms[2] = r*cos(theta)*cos(phi);  // dx/dtheta
  terms[3] = sin(theta)*sin(phi);    // dy/dr    
  terms[4] = r*sin(theta)*cos(phi);  // dy/dphi  
  terms[5] = r*cos(theta)*sin(phi);  // dy/dtheta
  terms[6] = cos(theta);             // dz/dr    
  terms[7] = 0.0;                    // dz/dphi  
  terms[8] = -r*sin(theta);          // dz/dtheta
  MatrixD T(3,terms);
  MatrixD Tinv=T.inverse();
  (MatrixD)(*this)=T*errorMatrix*Tinv;
}