|
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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
|
#ifndef DataFormat_Math_SSERot_H
#define DataFormat_Math_SSERot_H
#include "DataFormats/Math/interface/SSEVec.h"
namespace mathSSE {
template <typename T>
struct OldRot {
T R11, R12, R13;
T R21, R22, R23;
T R31, R32, R33;
} __attribute__((aligned(16)));
template <typename T>
struct Rot3 {
Vec4<T> axis[3];
Rot3() {
axis[0].arr[0] = 1;
axis[1].arr[1] = 1;
axis[2].arr[2] = 1;
}
Rot3(Vec4<T> ix, Vec4<T> iy, Vec4<T> iz) {
axis[0] = ix;
axis[1] = iy;
axis[2] = iz;
}
Rot3(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) {
axis[0].set(xx, xy, xz);
axis[1].set(yx, yy, yz);
axis[2].set(zx, zy, zz);
}
Rot3 transpose() const {
return Rot3(axis[0].arr[0],
axis[1].arr[0],
axis[2].arr[0],
axis[0].arr[1],
axis[1].arr[1],
axis[2].arr[1],
axis[0].arr[2],
axis[1].arr[2],
axis[2].arr[2]);
}
Vec4<T> x() { return axis[0]; }
Vec4<T> y() { return axis[1]; }
Vec4<T> z() { return axis[2]; }
// toLocal...
Vec4<T> rotate(Vec4<T> v) const { return transpose().rotateBack(v); }
// toGlobal...
Vec4<T> rotateBack(Vec4<T> v) const {
return v.template get1<0>() * axis[0] + v.template get1<1>() * axis[1] + v.template get1<2>() * axis[2];
}
Rot3 rotate(Rot3 const& r) const {
Rot3 tr = transpose();
return Rot3(tr.rotateBack(r.axis[0]), tr.rotateBack(r.axis[1]), tr.rotateBack(r.axis[2]));
}
Rot3 rotateBack(Rot3 const& r) const {
return Rot3(rotateBack(r.axis[0]), rotateBack(r.axis[1]), rotateBack(r.axis[2]));
}
};
typedef Rot3<float> Rot3F;
typedef Rot3<double> Rot3D;
#ifdef CMS_USE_SSE4
template <>
inline Vec4<float> Rot3<float>::rotate(Vec4<float> v) const {
return _mm_or_ps(_mm_or_ps(_mm_dp_ps(axis[0].vec, v.vec, 0x71), _mm_dp_ps(axis[1].vec, v.vec, 0x72)),
_mm_dp_ps(axis[2].vec, v.vec, 0x74));
}
template <>
inline Rot3<float> Rot3<float>::rotate(Rot3<float> const& r) const {
return Rot3<float>(rotate(r.axis[0]), rotate(r.axis[1]), rotate(r.axis[2]));
}
#endif
} // namespace mathSSE
template <typename T>
inline mathSSE::Rot3<T> operator*(mathSSE::Rot3<T> const& rh, mathSSE::Rot3<T> const& lh) {
// return Rot3(lh.rotateBack(rh.axis[0]),lh.rotateBack(rh.axis[1]),lh.rotateBack(rh.axis[2]));
return lh.rotateBack(rh);
}
namespace mathSSE {
template <typename T>
struct Rot2 {
Vec2<T> axis[2];
Rot2() {
axis[0].arr[0] = 1;
axis[1].arr[1] = 1;
}
Rot2(Vec2<T> ix, Vec2<T> iy) {
axis[0] = ix;
axis[1] = iy;
}
Rot2(T xx, T xy, T yx, T yy) {
axis[0].set(xx, xy);
axis[1].set(yx, yy);
}
Rot2 transpose() const { return Rot2(axis[0].arr[0], axis[1].arr[0], axis[0].arr[1], axis[1].arr[1]); }
Vec2<T> x() { return axis[0]; }
Vec2<T> y() { return axis[1]; }
// toLocal...
Vec2<T> rotate(Vec2<T> v) const { return transpose().rotateBack(v); }
// toGlobal...
Vec2<T> rotateBack(Vec2<T> v) const { return v.template get1<0>() * axis[0] + v.template get1<1>() * axis[1]; }
Rot2 rotate(Rot2 const& r) const {
Rot2 tr = transpose();
return Rot2(tr.rotateBack(r.axis[0]), tr.rotateBack(r.axis[1]));
}
Rot2 rotateBack(Rot2 const& r) const { return Rot2(rotateBack(r.axis[0]), rotateBack(r.axis[1])); }
};
typedef Rot2<float> Rot2F;
typedef Rot2<double> Rot2D;
} // namespace mathSSE
template <typename T>
inline mathSSE::Rot2<T> operator*(mathSSE::Rot2<T> const& rh, mathSSE::Rot2<T> const& lh) {
return lh.rotateBack(rh);
}
#include <iosfwd>
std::ostream& operator<<(std::ostream& out, mathSSE::Rot3F const& v);
std::ostream& operator<<(std::ostream& out, mathSSE::Rot3D const& v);
std::ostream& operator<<(std::ostream& out, mathSSE::Rot2F const& v);
std::ostream& operator<<(std::ostream& out, mathSSE::Rot2D const& v);
#endif // DataFormat_Math_SSERot_H
|