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

// Include files
#include <cmath>
#include <geometry_system/components/CylindricalCoordinate.hpp>
#include <geometry_system/components/CylindricalMatrix.hpp>

using namespace dgs;

CylindricalMatrix::CylindricalMatrix(const MatrixD& errorMatrix, 
				     const CylindricalCoordinate& lpt):
  MatrixD(3,3)
//
// Purpose: Construct a matrix in Cartesian coordinates given the matrix in
//          cylindrical coordinates.  
{
  double terms[9];
  double phi = lpt.phi();
  double r = lpt.rxy();

  terms[0] = cos(phi);     // dx/dr
  terms[1] = -r*sin(phi);  // dx/dphi
  terms[2] = 0.0;          // dx/dz
  terms[3] = sin(phi);     // dy/dr
  terms[4] = r*cos(phi);   // dy/dphi
  terms[5] = 0.0;          // dy/dz
  terms[6] = 0.0;          // dz/dr
  terms[7] = 0.0;          // dz/dphi
  terms[8] = 1.0;          // dz/dz

  MatrixD T(3,terms);
  MatrixD Tinv=T.inverse();
  (MatrixD)(*this)=T*errorMatrix*Tinv;
}