```// @(#)root/mathcore:\$Id\$
// Authors: W. Brown, M. Fischler, L. Moneta    2005

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

// Header file for rotation in 3 dimensions, represented by a quaternion
// Created by: Mark Fischler Thurs June 9  2005
//
// Last update: \$Id\$
//
#ifndef ROOT_Math_GenVector_Quaternion
#define ROOT_Math_GenVector_Quaternion  1

#include "Math/GenVector/Cartesian3D.h"
#include "Math/GenVector/DisplacementVector3D.h"
#include "Math/GenVector/PositionVector3D.h"
#include "Math/GenVector/LorentzVector.h"
#include "Math/GenVector/3DConversions.h"
#include "Math/GenVector/3DDistances.h"

#include <algorithm>
#include <cassert>

namespace ROOT {
namespace Math {

//__________________________________________________________________________________________
/**
Rotation class with the (3D) rotation represented by
a unit quaternion (u, i, j, k).
This is the optimal representation for multiplication of multiple
rotations, and for computation of group-manifold-invariant distance
between two rotations.

@ingroup GenVector
*/

class Quaternion {

public:

typedef double Scalar;

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

/**
Default constructor (identity rotation)
*/
Quaternion()
: fU(1.0)
, fI(0.0)
, fJ(0.0)
, fK(0.0)
{ }

/**
Construct given a pair of pointers or iterators defining the
beginning and end of an array of four Scalars
*/
template<class IT>
Quaternion(IT begin, IT end) { SetComponents(begin,end); }

// ======== Construction From other Rotation Forms ==================

/**
Construct from another supported rotation type (see gv_detail::convert )
*/
template <class OtherRotation>
explicit Quaternion(const OtherRotation & r) {gv_detail::convert(r,*this);}

/**
Construct from four Scalars representing the coefficients of u, i, j, k
*/
Quaternion(Scalar u, Scalar i, Scalar j, Scalar k) :
fU(u), fI(i), fJ(j), fK(k) { }

// The compiler-generated copy ctor, copy assignment, and dtor are OK.

/**
Re-adjust components to eliminate small deviations from |Q| = 1
orthonormality.
*/
void Rectify();

/**
Assign from another supported rotation type (see gv_detail::convert )
*/
template <class OtherRotation>
Quaternion & operator=( OtherRotation const  & r ) {
gv_detail::convert(r,*this);
return *this;
}

// ======== Components ==============

/**
Set the four components given an iterator to the start of
the desired data, and another to the end (4 past start).
*/
template<class IT>
#ifndef NDEBUG
void SetComponents(IT begin, IT end) {
#else
void SetComponents(IT begin, IT ) {
#endif
fU = *begin++;
fI = *begin++;
fJ = *begin++;
fK = *begin++;
assert (end==begin);
}

/**
Get the components into data specified by an iterator begin
and another to the end of the desired data (4 past start).
*/
template<class IT>
#ifndef NDEBUG
void GetComponents(IT begin, IT end) const {
#else
void GetComponents(IT begin, IT ) const {
#endif
*begin++ = fU;
*begin++ = fI;
*begin++ = fJ;
*begin++ = fK;
assert (end==begin);
}

/**
Get the components into data specified by an iterator begin
*/
template<class IT>
void GetComponents(IT begin ) const {
*begin++ = fU;
*begin++ = fI;
*begin++ = fJ;
*begin   = fK;
}

/**
Set the components based on four Scalars.  The sum of the squares of
these Scalars should be 1; no checking is done.
*/
void SetComponents(Scalar u, Scalar i, Scalar j, Scalar k) {
fU=u; fI=i; fJ=j; fK=k;
}

/**
Get the components into four Scalars.
*/
void GetComponents(Scalar & u, Scalar & i, Scalar & j, Scalar & k) const {
u=fU; i=fI; j=fJ; k=fK;
}

/**
U() is the coefficient of the identity Pauli matrix,
I(), J() and K() are the coefficients of sigma_x, sigma_y, sigma_z
*/
Scalar U() const { return fU; }
Scalar I() const { return fI; }
Scalar J() const { return fJ; }
Scalar K() const { return fK; }

// =========== operations ==============

/**
Rotation operation on a cartesian vector
*/
typedef  DisplacementVector3D<Cartesian3D<double>, DefaultCoordinateSystemTag > XYZVector;
XYZVector operator() (const XYZVector & v) const {

const Scalar alpha = fU*fU - fI*fI - fJ*fJ - fK*fK;
const Scalar twoQv = 2*(fI*v.X() + fJ*v.Y() + fK*v.Z());
const Scalar twoU  = 2 * fU;
return XYZVector  (  alpha * v.X() + twoU * (fJ*v.Z() - fK*v.Y()) + twoQv * fI ,
alpha * v.Y() + twoU * (fK*v.X() - fI*v.Z()) + twoQv * fJ ,
alpha * v.Z() + twoU * (fI*v.Y() - fJ*v.X()) + twoQv * fK );
}

/**
Rotation operation on a displacement vector in any coordinate system
*/
template <class CoordSystem,class Tag>
DisplacementVector3D<CoordSystem,Tag>
operator() (const DisplacementVector3D<CoordSystem,Tag> & v) const {
DisplacementVector3D< Cartesian3D<double> > xyz(v.X(), v.Y(), v.Z());
DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
DisplacementVector3D< CoordSystem,Tag > vNew;
vNew.SetXYZ( rxyz.X(), rxyz.Y(), rxyz.Z() );
return vNew;
}

/**
Rotation operation on a position vector in any coordinate system
*/
template <class CoordSystem, class Tag>
PositionVector3D<CoordSystem,Tag>
operator() (const PositionVector3D<CoordSystem,Tag> & p) const {
DisplacementVector3D< Cartesian3D<double>,Tag > xyz(p);
DisplacementVector3D< Cartesian3D<double>,Tag > rxyz = operator()(xyz);
return PositionVector3D<CoordSystem,Tag> ( rxyz );
}

/**
Rotation operation on a Lorentz vector in any 4D coordinate system
*/
template <class CoordSystem>
LorentzVector<CoordSystem>
operator() (const LorentzVector<CoordSystem> & v) const {
DisplacementVector3D< Cartesian3D<double> > xyz(v.Vect());
xyz = operator()(xyz);
LorentzVector< PxPyPzE4D<double> > xyzt (xyz.X(), xyz.Y(), xyz.Z(), v.E());
return LorentzVector<CoordSystem> ( xyzt );
}

/**
Rotation operation on an arbitrary vector v.
Preconditions:  v must implement methods x(), y(), and z()
and the arbitrary vector type must have a constructor taking (x,y,z)
*/
template <class ForeignVector>
ForeignVector
operator() (const  ForeignVector & v) const {
DisplacementVector3D< Cartesian3D<double> > xyz(v);
DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
return ForeignVector ( rxyz.X(), rxyz.Y(), rxyz.Z() );
}

/**
Overload operator * for rotation on a vector
*/
template <class AVector>
inline
AVector operator* (const AVector & v) const
{
return operator()(v);
}

/**
Invert a rotation in place
*/
void Invert() { fI = -fI; fJ = -fJ; fK = -fK; }

/**
Return inverse of a rotation
*/
Quaternion Inverse() const { return Quaternion(fU, -fI, -fJ, -fK); }

// ========= Multi-Rotation Operations ===============

/**
Multiply (combine) two rotations
*/
/**
Multiply (combine) two rotations
*/
Quaternion operator * (const Quaternion  & q) const {
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 operator * (const Rotation3D  & r) const;
Quaternion operator * (const AxisAngle   & a) const;
Quaternion operator * (const EulerAngles & e) const;
Quaternion operator * (const RotationZYX & r) const;
Quaternion operator * (const RotationX  & rx) const;
Quaternion operator * (const RotationY  & ry) const;
Quaternion operator * (const RotationZ  & rz) const;

/**
Post-Multiply (on right) by another rotation :  T = T*R
*/
template <class R>
Quaternion & operator *= (const R & r) { return *this = (*this)*r; }

/**
Distance between two rotations in Quaternion form
Note:  The rotation group is isomorphic to a 3-sphere
with diametrically opposite points identified.
The (rotation group-invariant) is the smaller
of the two possible angles between the images of
the two totations on that sphere.  Thus the distance
is never greater than pi/2.
*/

Scalar Distance(const Quaternion & q) const ;

/**
Equality/inequality operators
*/
bool operator == (const Quaternion & rhs) const {
if( fU != rhs.fU )  return false;
if( fI != rhs.fI )  return false;
if( fJ != rhs.fJ )  return false;
if( fK != rhs.fK )  return false;
return true;
}
bool operator != (const Quaternion & rhs) const {
return ! operator==(rhs);
}

private:

Scalar fU;
Scalar fI;
Scalar fJ;
Scalar fK;

};  // Quaternion

// ============ Class Quaternion ends here ============

/**
Distance between two rotations
*/
template <class R>
inline
typename Quaternion::Scalar
Distance ( const Quaternion& r1, const R & r2) {return gv_detail::dist(r1,r2);}

/**
Multiplication of an axial rotation by an AxisAngle
*/
Quaternion operator* (RotationX const & r1, Quaternion const & r2);
Quaternion operator* (RotationY const & r1, Quaternion const & r2);
Quaternion operator* (RotationZ const & r1, Quaternion const & r2);

/**
Stream Output and Input
*/
// TODO - I/O should be put in the manipulator form

std::ostream & operator<< (std::ostream & os, const Quaternion & q);

}  // namespace Math
}  // namespace ROOT

#endif // ROOT_Math_GenVector_Quaternion
```
Quaternion.h:1
Quaternion.h:2
Quaternion.h:3
Quaternion.h:4
Quaternion.h:5
Quaternion.h:6
Quaternion.h:7
Quaternion.h:8
Quaternion.h:9
Quaternion.h:10
Quaternion.h:11
Quaternion.h:12
Quaternion.h:13
Quaternion.h:14
Quaternion.h:15
Quaternion.h:16
Quaternion.h:17
Quaternion.h:18
Quaternion.h:19
Quaternion.h:20
Quaternion.h:21
Quaternion.h:22
Quaternion.h:23
Quaternion.h:24
Quaternion.h:25
Quaternion.h:26
Quaternion.h:27
Quaternion.h:28
Quaternion.h:29
Quaternion.h:30
Quaternion.h:31
Quaternion.h:32
Quaternion.h:33
Quaternion.h:34
Quaternion.h:35
Quaternion.h:36
Quaternion.h:37
Quaternion.h:38
Quaternion.h:39
Quaternion.h:40
Quaternion.h:41
Quaternion.h:42
Quaternion.h:43
Quaternion.h:44
Quaternion.h:45
Quaternion.h:46
Quaternion.h:47
Quaternion.h:48
Quaternion.h:49
Quaternion.h:50
Quaternion.h:51
Quaternion.h:52
Quaternion.h:53
Quaternion.h:54
Quaternion.h:55
Quaternion.h:56
Quaternion.h:57
Quaternion.h:58
Quaternion.h:59
Quaternion.h:60
Quaternion.h:61
Quaternion.h:62
Quaternion.h:63
Quaternion.h:64
Quaternion.h:65
Quaternion.h:66
Quaternion.h:67
Quaternion.h:68
Quaternion.h:69
Quaternion.h:70
Quaternion.h:71
Quaternion.h:72
Quaternion.h:73
Quaternion.h:74
Quaternion.h:75
Quaternion.h:76
Quaternion.h:77
Quaternion.h:78
Quaternion.h:79
Quaternion.h:80
Quaternion.h:81
Quaternion.h:82
Quaternion.h:83
Quaternion.h:84
Quaternion.h:85
Quaternion.h:86
Quaternion.h:87
Quaternion.h:88
Quaternion.h:89
Quaternion.h:90
Quaternion.h:91
Quaternion.h:92
Quaternion.h:93
Quaternion.h:94
Quaternion.h:95
Quaternion.h:96
Quaternion.h:97
Quaternion.h:98
Quaternion.h:99
Quaternion.h:100
Quaternion.h:101
Quaternion.h:102
Quaternion.h:103
Quaternion.h:104
Quaternion.h:105
Quaternion.h:106
Quaternion.h:107
Quaternion.h:108
Quaternion.h:109
Quaternion.h:110
Quaternion.h:111
Quaternion.h:112
Quaternion.h:113
Quaternion.h:114
Quaternion.h:115
Quaternion.h:116
Quaternion.h:117
Quaternion.h:118
Quaternion.h:119
Quaternion.h:120
Quaternion.h:121
Quaternion.h:122
Quaternion.h:123
Quaternion.h:124
Quaternion.h:125
Quaternion.h:126
Quaternion.h:127
Quaternion.h:128
Quaternion.h:129
Quaternion.h:130
Quaternion.h:131
Quaternion.h:132
Quaternion.h:133
Quaternion.h:134
Quaternion.h:135
Quaternion.h:136
Quaternion.h:137
Quaternion.h:138
Quaternion.h:139
Quaternion.h:140
Quaternion.h:141
Quaternion.h:142
Quaternion.h:143
Quaternion.h:144
Quaternion.h:145
Quaternion.h:146
Quaternion.h:147
Quaternion.h:148
Quaternion.h:149
Quaternion.h:150
Quaternion.h:151
Quaternion.h:152
Quaternion.h:153
Quaternion.h:154
Quaternion.h:155
Quaternion.h:156
Quaternion.h:157
Quaternion.h:158
Quaternion.h:159
Quaternion.h:160
Quaternion.h:161
Quaternion.h:162
Quaternion.h:163
Quaternion.h:164
Quaternion.h:165
Quaternion.h:166
Quaternion.h:167
Quaternion.h:168
Quaternion.h:169
Quaternion.h:170
Quaternion.h:171
Quaternion.h:172
Quaternion.h:173
Quaternion.h:174
Quaternion.h:175
Quaternion.h:176
Quaternion.h:177
Quaternion.h:178
Quaternion.h:179
Quaternion.h:180
Quaternion.h:181
Quaternion.h:182
Quaternion.h:183
Quaternion.h:184
Quaternion.h:185
Quaternion.h:186
Quaternion.h:187
Quaternion.h:188
Quaternion.h:189
Quaternion.h:190
Quaternion.h:191
Quaternion.h:192
Quaternion.h:193
Quaternion.h:194
Quaternion.h:195
Quaternion.h:196
Quaternion.h:197
Quaternion.h:198
Quaternion.h:199
Quaternion.h:200
Quaternion.h:201
Quaternion.h:202
Quaternion.h:203
Quaternion.h:204
Quaternion.h:205
Quaternion.h:206
Quaternion.h:207
Quaternion.h:208
Quaternion.h:209
Quaternion.h:210
Quaternion.h:211
Quaternion.h:212
Quaternion.h:213
Quaternion.h:214
Quaternion.h:215
Quaternion.h:216
Quaternion.h:217
Quaternion.h:218
Quaternion.h:219
Quaternion.h:220
Quaternion.h:221
Quaternion.h:222
Quaternion.h:223
Quaternion.h:224
Quaternion.h:225
Quaternion.h:226
Quaternion.h:227
Quaternion.h:228
Quaternion.h:229
Quaternion.h:230
Quaternion.h:231
Quaternion.h:232
Quaternion.h:233
Quaternion.h:234
Quaternion.h:235
Quaternion.h:236
Quaternion.h:237
Quaternion.h:238
Quaternion.h:239
Quaternion.h:240
Quaternion.h:241
Quaternion.h:242
Quaternion.h:243
Quaternion.h:244
Quaternion.h:245
Quaternion.h:246
Quaternion.h:247
Quaternion.h:248
Quaternion.h:249
Quaternion.h:250
Quaternion.h:251
Quaternion.h:252
Quaternion.h:253
Quaternion.h:254
Quaternion.h:255
Quaternion.h:256
Quaternion.h:257
Quaternion.h:258
Quaternion.h:259
Quaternion.h:260
Quaternion.h:261
Quaternion.h:262
Quaternion.h:263
Quaternion.h:264
Quaternion.h:265
Quaternion.h:266
Quaternion.h:267
Quaternion.h:268
Quaternion.h:269
Quaternion.h:270
Quaternion.h:271
Quaternion.h:272
Quaternion.h:273
Quaternion.h:274
Quaternion.h:275
Quaternion.h:276
Quaternion.h:277
Quaternion.h:278
Quaternion.h:279
Quaternion.h:280
Quaternion.h:281
Quaternion.h:282
Quaternion.h:283
Quaternion.h:284
Quaternion.h:285
Quaternion.h:286
Quaternion.h:287
Quaternion.h:288
Quaternion.h:289
Quaternion.h:290
Quaternion.h:291
Quaternion.h:292
Quaternion.h:293
Quaternion.h:294
Quaternion.h:295
Quaternion.h:296
Quaternion.h:297
Quaternion.h:298
Quaternion.h:299
Quaternion.h:300
Quaternion.h:301
Quaternion.h:302
Quaternion.h:303
Quaternion.h:304
Quaternion.h:305
Quaternion.h:306
Quaternion.h:307
Quaternion.h:308
Quaternion.h:309
Quaternion.h:310
Quaternion.h:311
Quaternion.h:312
Quaternion.h:313
Quaternion.h:314
Quaternion.h:315
Quaternion.h:316
Quaternion.h:317
Quaternion.h:318
Quaternion.h:319
Quaternion.h:320
Quaternion.h:321
Quaternion.h:322
Quaternion.h:323
Quaternion.h:324
Quaternion.h:325
Quaternion.h:326
Quaternion.h:327
Quaternion.h:328
Quaternion.h:329
Quaternion.h:330
Quaternion.h:331
Quaternion.h:332
Quaternion.h:333
Quaternion.h:334
Quaternion.h:335
Quaternion.h:336
Quaternion.h:337
Quaternion.h:338
Quaternion.h:339
Quaternion.h:340
Quaternion.h:341
Quaternion.h:342
Quaternion.h:343
Quaternion.h:344
Quaternion.h:345
Quaternion.h:346
Quaternion.h:347
Quaternion.h:348
Quaternion.h:349
Quaternion.h:350
Quaternion.h:351
Quaternion.h:352
Quaternion.h:353
Quaternion.h:354