SpaceVectorR.cc

Go to the documentation of this file.

// -*- C++ -*-
// ---------------------------------------------------------------------------
//
// This file is a part of the CLHEP - a Class Library for High Energy Physics.
//
// This is the implementation of the subset of those methods of the Hep3Vector 
// class which originated from the ZOOM SpaceVector class *and* which involve
// the concepts of rotation.
//

#ifdef GNUPRAGMA
#pragma implementation
#endif

#include "CLHEP/Vector/ThreeVector.h"
#include "CLHEP/Vector/AxisAngle.h"
#include "CLHEP/Vector/EulerAngles.h"

namespace CLHEP  {

//-************************
// rotate about axis
//-************************

Hep3Vector & Hep3Vector::rotate (const Hep3Vector & axis,
                                   double ddelta) {
  double r1 = axis.mag();
  if ( r1 == 0 ) {
    std::cerr << "Hep3Vector::rotate() - "
      << "Attempt to rotate around a zero vector axis! " << std::endl;
    return *this;
  }
  register double scale=1.0/r1;
  register double ux = scale*axis.getX();
  register double uy = scale*axis.getY();
  register double uz = scale*axis.getZ();
  double cd = std::cos(ddelta);
  double sd = std::sin(ddelta);
  register double ocd = 1 - cd;
  double rx;
  double ry;
  double rz;

  { register double  ocdux = ocd * ux;
    rx = dx * ( cd + ocdux * ux           ) +
         dy * (      ocdux * uy - sd * uz ) +
         dz * (      ocdux * uz + sd * uy ) ;
  }

  { register double  ocduy = ocd * uy;
    ry = dy * ( cd + ocduy * uy           ) +
         dz * (      ocduy * uz - sd * ux ) +
         dx * (      ocduy * ux + sd * uz ) ;
  }

  { register double  ocduz = ocd * uz;
    rz = dz * ( cd + ocduz * uz           ) +
         dx * (      ocduz * ux - sd * uy ) +
         dy * (      ocduz * uy + sd * ux ) ;
  }

  dx = rx;
  dy = ry;
  dz = rz;

  return *this;
} /* rotate */

//-****************************
// rotate by three euler angles
//-****************************


Hep3Vector & Hep3Vector::rotate (double phi1, 
                                 double theta1, 
                                 double psi1)  {

  double rx;
  double ry;
  double rz;

  register double sinPhi   = std::sin( phi1   ), cosPhi   = std::cos( phi1   );
  register double sinTheta = std::sin( theta1 ), cosTheta1 = std::cos( theta1 );
  register double sinPsi   = std::sin( psi1   ), cosPsi   = std::cos( psi1   );

  rx =  (cosPsi * cosPhi   - cosTheta1 * sinPsi * sinPhi)   * dx  +
        (cosPsi * sinPhi   + cosTheta1 * sinPsi * cosPhi)   * dy  +
        (sinPsi * sinTheta)                                * dz  ;

  ry =  (- sinPsi * cosPhi - cosTheta1 * cosPsi * sinPhi)   * dx  +
        (- sinPsi * sinPhi + cosTheta1 * cosPsi * cosPhi)   * dy  +
        (cosPsi * sinTheta)                                * dz  ;

  rz =  (sinTheta * sinPhi)                                * dx  +
        (- sinTheta * cosPhi)                              * dy  +
        (cosTheta1)                                        * dz  ;

  dx = rx;
  dy = ry;
  dz = rz;

  return *this;

} /* rotate */


//-*******************
// rotate(HepAxisAngle)
// rotate(HepEulerAngles)
//-*******************

Hep3Vector & Hep3Vector::rotate (const HepAxisAngle & ax ) {
  return rotate( ax.getAxis(), ax.delta() );
}

Hep3Vector & Hep3Vector::rotate (const HepEulerAngles & ex ) {
  return rotate( ex.phi(), ex.theta(), ex.psi() );
}


//-***********************
// rotationOf(HepAxisAngle)
// rotationOf(HepEulerAngles)
// and coordinate axis rotations
//-***********************

Hep3Vector rotationOf (const Hep3Vector & vec, const HepAxisAngle & ax) {
  Hep3Vector vv(vec);
  return vv.rotate (ax);
}

Hep3Vector rotationOf (const Hep3Vector & vec,
                       const Hep3Vector & axis, double ddelta) {
  Hep3Vector vv(vec);
  return vv.rotate(axis, ddelta);
}

Hep3Vector rotationOf (const Hep3Vector & vec, const HepEulerAngles & ex) {
  Hep3Vector vv(vec);
  return vv.rotate (ex);
}

Hep3Vector rotationOf (const Hep3Vector & vec,
                       double phi, double theta, double psi) {
  Hep3Vector vv(vec);
  return vv.rotate(phi, theta, psi);
}

Hep3Vector rotationXOf (const Hep3Vector & vec, double ddelta) {
  Hep3Vector vv(vec);
  return vv.rotateX (ddelta);
}

Hep3Vector rotationYOf (const Hep3Vector & vec, double ddelta) {
  Hep3Vector vv(vec);
  return vv.rotateY (ddelta);
}

Hep3Vector rotationZOf (const Hep3Vector & vec, double ddelta) {
  Hep3Vector vv(vec);
  return vv.rotateZ (ddelta);
}

}  // namespace CLHEP