Revision 4bee12876976bec72820e6464bf8665e58e624d0 authored by Gerardo Ganis on 24 July 2009, 07:33:50 UTC, committed by Gerardo Ganis on 24 July 2009, 07:33:50 UTC
git-svn-id: http://root.cern.ch/svn/root/branches/v5-24-00-patches@29570 27541ba8-7e3a-0410-8455-c3a389f83636
1 parent 3517aba
Transform3D.cxx
// @(#)root/mathcore:$Id$
// 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 "Math/GenVector/GenVectorIO.h"
#include "Math/GenVector/Transform3D.h"
#include "Math/GenVector/Plane3D.h"
#include <cmath>
#include <algorithm>
namespace ROOT {
namespace Math {
typedef Transform3D::Point XYZPoint;
typedef Transform3D::Vector XYZVector;
// ========== Constructors and Assignment =====================
// construct from two ref frames
Transform3D::Transform3D(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 << "Transform3D: Error : zero angle between axes" << std::endl;
SetIdentity();
} else {
if (std::fabs(cos1-cos2) > 0.000001) {
std::cerr << "Transform3D: 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(); double x1y = x1.y(); double x1z = x1.z();
double y1x = y1.x(); double y1y = y1.y(); double y1z = y1.z();
double z1x = z1.x(); double z1y = z1.y(); double z1z = z1.z();
double x2x = x2.x(); double x2y = x2.y(); double x2z = x2.z();
double y2x = y2.x(); double y2y = y2.y(); double y2z = y2.z();
double z2x = z2.x(); double z2y = z2.y(); double z2z = z2.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 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 Transform3D::Invert()
{
//
// Name: Transform3D::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 << "Transform3D::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]);
}
void Transform3D::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 Transform3D::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 Transform3D::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 Transform3D::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 Transform3D::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 Transform3D & t)
{
// TODO - this will need changing for machine-readable issues
// and even the human readable form needs formatting 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
![swh spinner](/static/img/swh-spinner.gif)
Computing file changes ...