TransformationMatrix.cc

Go to the documentation of this file.

#include <utl/config.h>
#include <utl/TransformationMatrix.h>
#include <CLHEP/Geometry/Transform3D.h>
#include <cmath>
#include <iostream>

using namespace utl;


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Functions and operators
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

TransformationMatrix&
TransformationMatrix::operator*=(const TransformationMatrix& second)
{
  fTransform = fTransform * second.fTransform;
  return *this;
}


double
TransformationMatrix::Distance(const TransformationMatrix& second)
  const
{
  // the internal class provide access to the interal data of a HepGeom::Transform3D
  class TransformWithDistance : public HepGeom::Transform3D {
  public:
    TransformWithDistance(const HepGeom::Transform3D& t)
      : HepGeom::Transform3D(t) { }
    double Distance(const TransformWithDistance& t) {
      const double d2 =
        (xx_-t.xx_)*(xx_-t.xx_) + (xy_-t.xy_)*(xy_-t.xy_) + (xz_-t.xz_)*(xz_-t.xz_) +
        (yx_-t.yx_)*(yx_-t.yx_) + (yy_-t.yy_)*(yy_-t.yy_) + (yz_-t.yz_)*(yz_-t.yz_) +
        (zx_-t.zx_)*(zx_-t.zx_) + (zy_-t.zy_)*(zy_-t.zy_) + (zz_-t.zz_)*(zz_-t.zz_) +
        (dx_-t.dx_)*(dx_-t.dx_) + (dy_-t.dy_)*(dy_-t.dy_) + (dz_-t.dz_)*(dz_-t.dz_);
      return sqrt(d2);
    }
  };

  return TransformWithDistance(this->fTransform).Distance(second.fTransform);
}


TransformationMatrix
TransformationMatrix::Inverse()
  const
{
  return TransformationMatrix(fTransform.inverse());
}


boost::tuple<TransformationMatrix, TransformationMatrix>
TransformationMatrix::Decompose()
  const
{
  HepGeom::Scale3D scale;
  HepGeom::Rotate3D rotation;
  HepGeom::Translate3D translation;

  fTransform.getDecomposition(scale, rotation, translation);

  return boost::make_tuple(TransformationMatrix(translation),
                           TransformationMatrix(rotation));
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *  Factory functions
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

TransformationMatrix
TransformationMatrix::Translation(const double x, const double y, const double z)
{
  return TransformationMatrix(HepGeom::Translate3D(x, y, z));
}


TransformationMatrix
TransformationMatrix::Rotation(const double angle,
                               const double x, const double y, const double z)
{
  return TransformationMatrix(HepGeom::Rotate3D(angle, HepGeom::Vector3D<double>(x, y, z)));
}


TransformationMatrix
TransformationMatrix::Rotation(const double angle, const Triple& v)
{
  return
    TransformationMatrix(
      HepGeom::Rotate3D(angle, HepGeom::Vector3D<double>(v.get<0>(), v.get<1>(), v.get<2>()))
    );
}


TransformationMatrix
TransformationMatrix::RotationX(const double angle)
{
  return TransformationMatrix(HepGeom::RotateX3D(angle));
}


TransformationMatrix
TransformationMatrix::RotationY(const double angle)
{
  return TransformationMatrix(HepGeom::RotateY3D(angle));
}


TransformationMatrix
TransformationMatrix::RotationZ(const double angle)
{
  return TransformationMatrix(HepGeom::RotateZ3D(angle));
}


TransformationMatrix
TransformationMatrix::TransformToBasis(const double x1, const double y1, const double z1,
                                       const double x2, const double y2, const double z2,
                                       const double x3, const double y3, const double z3)
{
  class TC : public HepGeom::Transform3D {
  public:
    TC(const double xx, const double xy, const double xz,
       const double yx, const double yy, const double yz,
       const double zx, const double zy, const double zz)
      : HepGeom::Transform3D(xx, xy, xz, 0,
                             yx, yy, yz, 0,
                             zx, zy, zz, 0) { }
  };

  return TransformationMatrix(TC(x1, y1, z1,
                                 x2, y2, z2,
                                 x3, y3, z3));
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *  non-member functions
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

std::ostream&
utl::operator<<(std::ostream& s, const TransformationMatrix& m)
{
  s << '[';
  for (int i = 0; i < TransformationMatrix::kDimension; ++i) {
    s << (i ? " " : "") << '[';
    for (int j = 0; j < TransformationMatrix::kDimension; ++j)
      s << (i ? " " : "") << m.fTransform(i, j);
    s << ']';
  }
  return s << ']';
}


// Configure (x)emacs for this file ...
// Local Variables:
// mode: c++
// compile-command: "make -C .. -k"
// End: