Transform3DPJ.cc

Go to the documentation of this file.

// @(#)root/mathcore:$Name: V02-02-29 $:$Id: Transform3DPJ.cc,v 1.2 2008/01/22 20:41:27 muzaffar Exp $
// Authors: W. Brown, M. Fischler, L. Moneta    2005

/**********************************************************************
 *                                                                    *
 * Copyright (c) 2005 , LCG ROOT MathLib Team                         *
 *                                                                    *
 *                                                                    *
 **********************************************************************/

// implementation file for class Transform3D
//
// Created by: Lorenzo Moneta  October 27 2005
//
//

#include "FastSimulation/CaloGeometryTools/interface/Transform3DPJ.h"

#include <cmath>
#include <algorithm>

namespace ROOT {

  namespace Math {

    typedef Transform3DPJ::Point XYZPoint;
    typedef Transform3DPJ::Vector XYZVector;

    // ========== Constructors and Assignment =====================

    // construct from two ref frames
    Transform3DPJ::Transform3DPJ(const XYZPoint &fr0,
                                 const XYZPoint &fr1,
                                 const XYZPoint &fr2,
                                 const XYZPoint &to0,
                                 const XYZPoint &to1,
                                 const XYZPoint &to2) {
      // takes impl. from CLHEP ( E.Chernyaev). To be checked

      XYZVector x1, y1, z1, x2, y2, z2;
      x1 = (fr1 - fr0).Unit();
      y1 = (fr2 - fr0).Unit();
      x2 = (to1 - to0).Unit();
      y2 = (to2 - to0).Unit();

      //   C H E C K   A N G L E S

      double cos1, cos2;
      cos1 = x1.Dot(y1);
      cos2 = x2.Dot(y2);

      if (std::fabs(1.0 - cos1) <= 0.000001 || std::fabs(1.0 - cos2) <= 0.000001) {
        std::cerr << "Transform3DPJ: Error : zero angle between axes" << std::endl;
        SetIdentity();
      } else {
        if (std::fabs(cos1 - cos2) > 0.000001) {
          std::cerr << "Transform3DPJ: Warning: angles between axes are not equal" << std::endl;
        }

        //   F I N D   R O T A T I O N   M A T R I X

        z1 = (x1.Cross(y1)).Unit();
        y1 = z1.Cross(x1);

        z2 = (x2.Cross(y2)).Unit();
        y2 = z2.Cross(x2);

        double x1x = x1.X(), x1y = x1.Y(), x1z = x1.Z();
        double y1x = y1.X(), y1y = y1.Y(), y1z = y1.Z();
        double z1x = z1.X(), z1y = z1.Y(), z1z = z1.Z();

        double detxx = (y1y * z1z - z1y * y1z);
        double detxy = -(y1x * z1z - z1x * y1z);
        double detxz = (y1x * z1y - z1x * y1y);
        double detyx = -(x1y * z1z - z1y * x1z);
        double detyy = (x1x * z1z - z1x * x1z);
        double detyz = -(x1x * z1y - z1x * x1y);
        double detzx = (x1y * y1z - y1y * x1z);
        double detzy = -(x1x * y1z - y1x * x1z);
        double detzz = (x1x * y1y - y1x * x1y);

        double x2x = x2.X(), x2y = x2.Y(), x2z = x2.Z();
        double y2x = y2.X(), y2y = y2.Y(), y2z = y2.Z();
        double z2x = z2.X(), z2y = z2.Y(), z2z = z2.Z();

        double txx = x2x * detxx + y2x * detyx + z2x * detzx;
        double txy = x2x * detxy + y2x * detyy + z2x * detzy;
        double txz = x2x * detxz + y2x * detyz + z2x * detzz;
        double tyx = x2y * detxx + y2y * detyx + z2y * detzx;
        double tyy = x2y * detxy + y2y * detyy + z2y * detzy;
        double tyz = x2y * detxz + y2y * detyz + z2y * detzz;
        double tzx = x2z * detxx + y2z * detyx + z2z * detzx;
        double tzy = x2z * detxy + y2z * detyy + z2z * detzy;
        double tzz = x2z * detxz + y2z * detyz + z2z * detzz;

        //   S E T    T R A N S F O R M A T I O N

        double dx1 = fr0.X(), dy1 = fr0.Y(), dz1 = fr0.Z();
        double dx2 = to0.X(), dy2 = to0.Y(), dz2 = to0.Z();

        SetComponents(txx,
                      txy,
                      txz,
                      dx2 - txx * dx1 - txy * dy1 - txz * dz1,
                      tyx,
                      tyy,
                      tyz,
                      dy2 - tyx * dx1 - tyy * dy1 - tyz * dz1,
                      tzx,
                      tzy,
                      tzz,
                      dz2 - tzx * dx1 - tzy * dy1 - tzz * dz1);
      }
    }

    // inversion (from CLHEP)
    void Transform3DPJ::Invert() {
      //
      // Name: Transform3DPJ::inverse                     Date:    24.09.96
      // Author: E.Chernyaev (IHEP/Protvino)            Revised:
      //
      // Function: Find inverse affine transformation.

      double detxx = fM[kYY] * fM[kZZ] - fM[kYZ] * fM[kZY];
      double detxy = fM[kYX] * fM[kZZ] - fM[kYZ] * fM[kZX];
      double detxz = fM[kYX] * fM[kZY] - fM[kYY] * fM[kZX];
      double det = fM[kXX] * detxx - fM[kXY] * detxy + fM[kXZ] * detxz;
      if (det == 0) {
        std::cerr << "Transform3DPJ::inverse error: zero determinant" << std::endl;
        return;
      }
      det = 1. / det;
      detxx *= det;
      detxy *= det;
      detxz *= det;
      double detyx = (fM[kXY] * fM[kZZ] - fM[kXZ] * fM[kZY]) * det;
      double detyy = (fM[kXX] * fM[kZZ] - fM[kXZ] * fM[kZX]) * det;
      double detyz = (fM[kXX] * fM[kZY] - fM[kXY] * fM[kZX]) * det;
      double detzx = (fM[kXY] * fM[kYZ] - fM[kXZ] * fM[kYY]) * det;
      double detzy = (fM[kXX] * fM[kYZ] - fM[kXZ] * fM[kYX]) * det;
      double detzz = (fM[kXX] * fM[kYY] - fM[kXY] * fM[kYX]) * det;
      SetComponents(detxx,
                    -detyx,
                    detzx,
                    -detxx * fM[kDX] + detyx * fM[kDY] - detzx * fM[kDZ],
                    -detxy,
                    detyy,
                    -detzy,
                    detxy * fM[kDX] - detyy * fM[kDY] + detzy * fM[kDZ],
                    detxz,
                    -detyz,
                    detzz,
                    -detxz * fM[kDX] + detyz * fM[kDY] - detzz * fM[kDZ]);
    }

    // get rotations and translations
    void Transform3DPJ::GetDecomposition(Rotation3D &r, XYZVector &v) const {
      // decompose a trasfomation in a 3D rotation and in a 3D vector (cartesian coordinates)
      r.SetComponents(fM[kXX], fM[kXY], fM[kXZ], fM[kYX], fM[kYY], fM[kYZ], fM[kZX], fM[kZY], fM[kZZ]);

      v.SetCoordinates(fM[kDX], fM[kDY], fM[kDZ]);
    }

    // transformation on Position Vector (rotation + translations)
    XYZPoint Transform3DPJ::operator()(const XYZPoint &p) const {
      // pass through rotation class (could be implemented directly to be faster)

      Rotation3D r;
      XYZVector t;
      GetDecomposition(r, t);
      XYZPoint pnew = r(p);
      pnew += t;
      return pnew;
    }

    // transformation on Displacement Vector (only rotation)
    XYZVector Transform3DPJ::operator()(const XYZVector &v) const {
      // pass through rotation class ( could be implemented directly to be faster)

      Rotation3D r;
      XYZVector t;
      GetDecomposition(r, t);
      // only rotation
      return r(v);
    }

    Transform3DPJ &Transform3DPJ::operator*=(const Transform3DPJ &t) {
      // combination of transformations

      SetComponents(fM[kXX] * t.fM[kXX] + fM[kXY] * t.fM[kYX] + fM[kXZ] * t.fM[kZX],
                    fM[kXX] * t.fM[kXY] + fM[kXY] * t.fM[kYY] + fM[kXZ] * t.fM[kZY],
                    fM[kXX] * t.fM[kXZ] + fM[kXY] * t.fM[kYZ] + fM[kXZ] * t.fM[kZZ],
                    fM[kXX] * t.fM[kDX] + fM[kXY] * t.fM[kDY] + fM[kXZ] * t.fM[kDZ] + fM[kDX],

                    fM[kYX] * t.fM[kXX] + fM[kYY] * t.fM[kYX] + fM[kYZ] * t.fM[kZX],
                    fM[kYX] * t.fM[kXY] + fM[kYY] * t.fM[kYY] + fM[kYZ] * t.fM[kZY],
                    fM[kYX] * t.fM[kXZ] + fM[kYY] * t.fM[kYZ] + fM[kYZ] * t.fM[kZZ],
                    fM[kYX] * t.fM[kDX] + fM[kYY] * t.fM[kDY] + fM[kYZ] * t.fM[kDZ] + fM[kDY],

                    fM[kZX] * t.fM[kXX] + fM[kZY] * t.fM[kYX] + fM[kZZ] * t.fM[kZX],
                    fM[kZX] * t.fM[kXY] + fM[kZY] * t.fM[kYY] + fM[kZZ] * t.fM[kZY],
                    fM[kZX] * t.fM[kXZ] + fM[kZY] * t.fM[kYZ] + fM[kZZ] * t.fM[kZZ],
                    fM[kZX] * t.fM[kDX] + fM[kZY] * t.fM[kDY] + fM[kZZ] * t.fM[kDZ] + fM[kDZ]);

      return *this;
    }

    void Transform3DPJ::SetIdentity() {
      //set identity ( identity rotation and zero translation)
      fM[kXX] = 1.0;
      fM[kXY] = 0.0;
      fM[kXZ] = 0.0;
      fM[kDX] = 0.0;
      fM[kYX] = 0.0;
      fM[kYY] = 1.0;
      fM[kYZ] = 0.0;
      fM[kDY] = 0.0;
      fM[kZX] = 0.0;
      fM[kZY] = 0.0;
      fM[kZZ] = 1.0;
      fM[kDZ] = 0.0;
    }

    void Transform3DPJ::AssignFrom(const Rotation3D &r, const XYZVector &v) {
      // assignment  from rotation + translation

      double rotData[9];
      r.GetComponents(rotData, rotData + 9);
      // first raw
      for (int i = 0; i < 3; ++i)
        fM[i] = rotData[i];
      // second raw
      for (int i = 0; i < 3; ++i)
        fM[kYX + i] = rotData[3 + i];
      // third raw
      for (int i = 0; i < 3; ++i)
        fM[kZX + i] = rotData[6 + i];

      // translation data
      double vecData[3];
      v.GetCoordinates(vecData, vecData + 3);
      fM[kDX] = vecData[0];
      fM[kDY] = vecData[1];
      fM[kDZ] = vecData[2];
    }

    void Transform3DPJ::AssignFrom(const Rotation3D &r) {
      // assign from only a rotation  (null translation)
      double rotData[9];
      r.GetComponents(rotData, rotData + 9);
      for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j)
          fM[4 * i + j] = rotData[3 * i + j];
        // empty vector data
        fM[4 * i + 3] = 0;
      }
    }

    void Transform3DPJ::AssignFrom(const XYZVector &v) {
      // assign from a translation only (identity rotations)
      fM[kXX] = 1.0;
      fM[kXY] = 0.0;
      fM[kXZ] = 0.0;
      fM[kDX] = v.X();
      fM[kYX] = 0.0;
      fM[kYY] = 1.0;
      fM[kYZ] = 0.0;
      fM[kDY] = v.Y();
      fM[kZX] = 0.0;
      fM[kZY] = 0.0;
      fM[kZZ] = 1.0;
      fM[kDZ] = v.Z();
    }

    Plane3D Transform3DPJ::operator()(const Plane3D &plane) const {
      // transformations on a 3D plane
      XYZVector n = plane.Normal();
      // take a point on the plane. Use origin projection on the plane
      // ( -ad, -bd, -cd) if (a**2 + b**2 + c**2 ) = 1
      double d = plane.HesseDistance();
      XYZPoint p(-d * n.X(), -d * n.Y(), -d * n.Z());
      return Plane3D(operator()(n), operator()(p));
    }

    std::ostream &operator<<(std::ostream &os, const Transform3DPJ &t) {
      // TODO - this will need changing for machine-readable issues
      //        and even the human readable form needs formatiing improvements

      double m[12];
      t.GetComponents(m, m + 12);
      os << "\n" << m[0] << "  " << m[1] << "  " << m[2] << "  " << m[3];
      os << "\n" << m[4] << "  " << m[5] << "  " << m[6] << "  " << m[7];
      os << "\n" << m[8] << "  " << m[9] << "  " << m[10] << "  " << m[11] << "\n";
      return os;
    }

  }  // end namespace Math
}  // end namespace ROOT