|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
#include <iostream>
#include "DataFormats/GeometrySurface/interface/TkRotation.h"
#include "DataFormats/GeometrySurface/interface/GloballyPositioned.h"
#include "DataFormats/GeometrySurface/interface/BoundPlane.h"
//#include "CommonReco/RKPropagators/interface/FrameChanger.h"
#include "DataFormats/GeometrySurface/interface/Cylinder.h"
#include <cmath>
using namespace std;
template <typename T>
void go() {
typedef TkRotation<T> Rotation;
typedef GloballyPositioned<T> Frame;
typedef typename Frame::PositionType Position;
typedef typename Frame::GlobalVector GlobalVector;
typedef typename Frame::GlobalPoint GlobalPoint;
typedef typename Frame::LocalVector LocalVector;
typedef typename Frame::LocalPoint LocalPoint;
std::cout << "size of Rot " << sizeof(Rotation) << std::endl;
std::cout << "size of Pos " << sizeof(Position) << std::endl;
std::cout << "size of Point " << sizeof(GlobalPoint) << std::endl;
std::cout << "size of Frame " << sizeof(Frame) << std::endl;
double a = 0.01;
double ca = cos(a);
double sa = sin(a);
Rotation r1(ca, sa, 0, -sa, ca, 0, 0, 0, 1);
;
Frame f1(Position(2, 3, 4), r1);
cout << "f1.position() " << f1.position() << endl;
cout << "f1.rotation() " << endl << f1.rotation() << endl;
Rotation r2(GlobalVector(0, 1, 0), GlobalVector(0, 0, 1));
Frame f2(Position(5, 6, 7), r2);
cout << "f2.position() " << f2.position() << endl;
cout << "f2.rotation() " << endl << f2.rotation() << endl;
// transform f2 to f1 so that f1 becomes the "global" frame of f3
// Rotation r3 = r2.multiplyInverse(r1);
// Rotation r3 = r2*r1;
// Rotation r3 = r1*r2;
// Rotation r3 = r1*r2.transposed();
// Rotation r3 = r1.transposed()*r2;
// Rotation r3 = r1.transposed()*r2.transposed();
// Rotation r3 = r2*r1;
Rotation r3 = r2 * r1.transposed();
GlobalPoint pos2(f2.position());
LocalPoint lp3 = f1.toLocal(pos2);
Frame f3(GlobalPoint(lp3.basicVector()), r3);
cout << "f3.position() " << f3.position() << endl;
cout << "f3.rotation() " << endl << f3.rotation() << endl;
// test
GlobalPoint gp(11, 22, 33);
LocalPoint p_in1 = f1.toLocal(gp);
typename Frame::ToLocal ff1(f1);
LocalPoint p_in2 = f2.toLocal(gp);
LocalPoint p_in3 = f3.toLocal(GlobalPoint(p_in1.basicVector()));
cout << "p_in1 " << p_in1 << endl;
cout << "p_in1 " << ff1.toLocal(gp) << endl;
cout << "p_in2 " << p_in2 << endl;
cout << "p_in3 " << p_in3 << endl;
LocalPoint p_in1_from3(f3.toGlobal(p_in3).basicVector());
cout << "p_in1_from3 + " << p_in1_from3 << endl;
BoundPlane plane(f2.position(), f2.rotation());
// FrameChanger<double> changer;
// FrameChanger<double>::PlanePtr pp = changer.toFrame( plane, f1);
/*
FrameChanger changer;
FrameChanger::PlanePtr pp = changer.transformPlane( plane, f1);
LocalPoint p_in2p = plane.toLocal( gp);
LocalPoint p_in3p = pp->toLocal( GlobalPoint(p_in1.basicVector()));
cout << "p_in2p " << p_in2p << endl;
cout << "p_in3p " << p_in3p << endl;
*/
}
void cyl() {
using T = float;
typedef TkRotation<T> Rotation;
typedef GloballyPositioned<T> Frame;
typedef typename Frame::PositionType Position;
typedef typename Frame::GlobalVector GlobalVector;
typedef typename Frame::GlobalPoint GlobalPoint;
typedef typename Frame::LocalVector LocalVector;
typedef typename Frame::LocalPoint LocalPoint;
// cylinder
{
std::cout << " Trivial Cylinder" << std::endl;
Rotation ll(GlobalVector(1, 0, 0), GlobalVector(0, 1, 0));
Position p0(0, 0, 0);
Cylinder cyl(5., p0, ll);
Plane t = cyl.fastTangent(GlobalPoint(3., 4., 1.));
std::cout << t.position() << '\n' << t.rotation() << std::endl;
std::cout << t.rotation().x() * cyl.rotation().z().cross((t.position() - cyl.position()).basicVector()).unit()
<< std::endl;
}
{
std::cout << " rotated, displaced Cylinder" << std::endl;
Rotation ll(Basic3DVector<T>(1, 1, 1), .3);
Cylinder cyl(5.f, Position(2, -1, 3), ll);
Plane t = cyl.fastTangent(LocalPoint(3., 4., 1.));
std::cout << t.position() << '\n' << t.rotation() << std::endl;
std::cout << t.rotation().x() * cyl.rotation().z().cross((t.position() - cyl.position()).basicVector()).unit()
<< std::endl;
}
}
int main() {
go<float>();
cyl();
std::cout << std::endl;
go<double>();
}
|