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
Quaternion.cxx
// @(#)root/mathcore:$Id$
// Authors: W. Brown, M. Fischler, L. Moneta 2005
/**********************************************************************
* *
* Copyright (c) 2005 , LCG ROOT FNAL MathLib Team *
* *
* *
**********************************************************************/
// Implementation file for rotation in 3 dimensions, represented by quaternion
//
// Created by: Mark Fischler Thurs June 9 2005
//
// Last update: $Id$
//
#include "Math/GenVector/Quaternion.h"
#include <cmath>
#include "Math/GenVector/Cartesian3D.h"
#include "Math/GenVector/DisplacementVector3D.h"
#include "Math/GenVector/Quaternion.h"
#include "Math/GenVector/Rotation3Dfwd.h"
#include "Math/GenVector/AxisAnglefwd.h"
#include "Math/GenVector/EulerAnglesfwd.h"
namespace ROOT {
namespace Math {
// ========== Constructors and Assignment =====================
void Quaternion::Rectify()
{
// The vector should be a unit vector, and the first element should be
// non-negative (though negative fU quaternions would work just fine,
// being isomorphic to a quaternion with positive fU).
if ( fU < 0 ) {
fU = - fU; fI = - fI; fJ = - fJ; fK = - fK;
}
Scalar a = 1.0 / std::sqrt(fU*fU + fI*fI + fJ*fJ + fK*fK);
fU *= a;
fI *= a;
fJ *= a;
fK *= a;
} // Rectify()
// ========== Operations =====================
// DisplacementVector3D< Cartesian3D<double> >
// Quaternion::operator() (const DisplacementVector3D< Cartesian3D<double> > & v) const
// {
// // apply to a 3D Vector
// }
// Quaternion Quaternion::operator * (const Quaternion & q) const {
// // combination of rotations
// return Quaternion (
// fU*q.fU - fI*q.fI - fJ*q.fJ - fK*q.fK
// , fU*q.fI + fI*q.fU + fJ*q.fK - fK*q.fJ
// , fU*q.fJ - fI*q.fK + fJ*q.fU + fK*q.fI
// , fU*q.fK + fI*q.fJ - fJ*q.fI + fK*q.fU );
//}
Quaternion Quaternion::operator * (const Rotation3D & r) const {
// combination of rotations
return operator* ( Quaternion(r) );
}
Quaternion Quaternion::operator * (const AxisAngle & a) const {
// combination of rotations
return operator* ( Quaternion(a) );
}
Quaternion Quaternion::operator * (const EulerAngles & e) const {
// combination of rotations
return operator* ( Quaternion(e) );
}
Quaternion Quaternion::operator * (const RotationZYX & r) const {
// combination of rotations
return operator* ( Quaternion(r) );
}
Quaternion::Scalar Quaternion::Distance(const Quaternion & q) const {
// distance
Scalar chordLength = std::fabs(fU*q.fU + fI*q.fI + fJ*q.fJ + fK*q.fK);
if (chordLength > 1) chordLength = 1; // in case roundoff fouls us up
return acos(chordLength);
}
// ========== I/O =====================
std::ostream & operator<< (std::ostream & os, const Quaternion & q) {
// TODO - this will need changing for machine-readable issues
// and even the human readable form may need formatiing improvements
os << "\n{" << q.U() << " " << q.I()
<< " " << q.J() << " " << q.K() << "}\n";
return os;
}
} //namespace Math
} //namespace ROOT
![swh spinner](/static/img/swh-spinner.gif)
Computing file changes ...