File indexing completed on 2024-05-10 02:20:34
0001 #include "DataFormats/Math/interface/ExtVec.h"
0002
0003 #include <cmath>
0004 #include <vector>
0005
0006 #include <iostream>
0007
0008 #ifdef __AVX2__
0009 #define CMS_USE_AVX2
0010 #endif
0011
0012 void addScaleddiff(Vec3F& res, float s, Vec3F const& a, Vec3F const& b) { res = res + s * (a - b); }
0013
0014 float dotV(Vec3F const& a, Vec3F const& b) { return dot(a, b); }
0015
0016 float dotSimple(Vec3F const& a, Vec3F const& b) {
0017 Vec3F res = a * b;
0018 return res[0] + res[1] + res[2];
0019 }
0020
0021 double dotSimple(Vec3D const& a, Vec3D const& b) {
0022 Vec3D res = a * b;
0023 return res[0] + res[1] + res[2];
0024 }
0025
0026 float norm(Vec3F const& a) { return std::sqrt(dot(a, a)); }
0027
0028 Vec3F toLocal(Vec3F const& a, Rot3<float> const& r) { return r.rotate(a); }
0029
0030 Vec3F toGlobal(Vec3F const& a, Rot3<float> const& r) { return r.rotateBack(a); }
0031
0032
0033 template <typename T>
0034 struct BaVec {
0035 typedef BaVec<T> self;
0036
0037 BaVec() : theX(0), theY(0), theZ(0), theW(0) {}
0038
0039 BaVec(float f1, float f2, float f3) : theX(f1), theY(f2), theZ(f3), theW(0) {}
0040
0041 self& operator+=(self const& rh) { return *this; }
0042
0043 T theX;
0044 T theY;
0045 T theZ;
0046 T theW;
0047 } __attribute__((aligned(16)));
0048
0049 typedef BaVec<float> BaVecF;
0050
0051 static_assert(sizeof(BaVecF) == sizeof(Vec3F));
0052
0053 struct makeVec3F {
0054 makeVec3F(BaVecF& bv) : v(reinterpret_cast<Vec3F&>(bv)) {}
0055 Vec3F& v;
0056 };
0057 struct makeVec3FC {
0058 makeVec3FC(BaVecF const& bv) : v(reinterpret_cast<Vec3F const&>(bv)) {}
0059 Vec3F const& v;
0060 };
0061
0062 template <>
0063 inline BaVecF& BaVecF::operator+=(BaVecF const& rh) {
0064 makeVec3FC v(rh);
0065 makeVec3F s(*this);
0066 s.v = s.v + v.v;
0067 return *this;
0068 }
0069
0070 void sum(BaVecF& lh, BaVecF const& rh) { lh += rh; }
0071
0072 void testBa() {
0073 std::cout << " test BA of size " << sizeof(BaVecF) << std::endl;
0074 BaVecF vx(2.0, 4.0, 5.0);
0075 BaVecF vy(-3.0, 2.0, -5.0);
0076 vx += vy;
0077 std::cout << vx.theX << ", " << vx.theY << ", " << vx.theZ << std::endl;
0078 }
0079
0080 template <typename T>
0081 void go2d() {
0082 typedef Vec2<T> Vec2d;
0083 typedef Vec4<T> Vec3d;
0084 static_assert(sizeof(Vec2d) == 2 * sizeof(T));
0085 static_assert(sizeof(Vec3d) == 4 * sizeof(T));
0086
0087 std::cout << "\n2d" << std::endl;
0088 std::cout << sizeof(Vec2d) << ' ' << alignof(Vec2d) << std::endl;
0089
0090 std::vector<Vec2d> vec1;
0091 vec1.reserve(50);
0092 std::vector<T> vect(23);
0093 std::vector<Vec2d> vec2(53);
0094 std::vector<Vec2d> vec3;
0095 vec3.reserve(50234);
0096
0097 constexpr Vec2d k{-2.0, 3.14};
0098 std::cout << k << std::endl;
0099 std::cout << k + k << std::endl;
0100 std::cout << k * k << std::endl;
0101 Vec3d x{2.0, 4.0, 5.0};
0102 Vec3d y{-3.0, 2.0, -5.0};
0103 std::cout << x << std::endl;
0104 std::cout << y << std::endl;
0105
0106 Vec2d x2 = xy(x);
0107 Vec2d y2 = xy(y);
0108 std::cout << x2 << std::endl;
0109 Vec2d xx2 = xy(x);
0110 std::cout << xx2 << std::endl;
0111 std::cout << y2 << std::endl;
0112
0113 std::cout << Vec2d(T(3.) * x2) << std::endl;
0114 std::cout << Vec2d(y2 * T(0.1)) << std::endl;
0115 std::cout << Vec2d(T(0.5) * (x2 + y2)) << std::endl;
0116 std::cout << apply(x2, [](T x) { return std::sqrt(x); }) << std::endl;
0117
0118 std::cout << dot(x2, y2) << " = 2?" << std::endl;
0119 std::cout << dot2(x2, y) << " = 2?" << std::endl;
0120 std::cout << dot2(x, y) << " = 2?" << std::endl;
0121
0122 T z = cross2(x2, y2);
0123 std::cout << z << " = 16?" << std::endl;
0124 z = cross2(x, y);
0125 std::cout << z << " = 16?" << std::endl;
0126
0127 std::cout << std::sqrt(z) << " = 4?" << std::endl;
0128 }
0129
0130 template <typename T>
0131 void go(bool dovec = true) {
0132 typedef Vec4<T> Vec;
0133 typedef Vec2<T> Vec2D;
0134
0135 std::cout << std::endl;
0136 std::cout << sizeof(Vec) << ' ' << alignof(Vec) << std::endl;
0137
0138 if (dovec) {
0139 std::vector<Vec> vec1;
0140 vec1.reserve(50);
0141 std::vector<T> vect(23);
0142 std::vector<Vec> vec2(53);
0143 std::vector<Vec> vec3;
0144 vec3.reserve(50234);
0145 }
0146
0147 constexpr Vec zero{0, 0, 0, 0};
0148 constexpr Vec x{2.0f, 4.0f, 5.0f};
0149 constexpr Vec y{-3.0f, 2.0f, -5.0f};
0150 Vec x0 = x[0] + zero;
0151 Vec4<float> f = convert<Vec4<float>>(x);
0152 Vec4<double> d = convert<Vec4<double>>(x);
0153
0154 const Vec xx = T(3.3) + zero;
0155 std::cout << x << std::endl;
0156 std::cout << (Vec4<float>){float(x[0]), float(x[1]), float(x[2]), float(x[3])} << std::endl;
0157 std::cout << (Vec4<double>){x[0], x[1], x[2], x[3]} << std::endl;
0158 std::cout << f << std::endl;
0159 std::cout << d << std::endl;
0160 std::cout << -x << std::endl;
0161 std::cout << Vec{x[2]} << std::endl;
0162
0163 std::cout << x0 << std::endl;
0164 std::cout << xx << std::endl;
0165 std::cout << Vec{} + x[2] << std::endl;
0166 std::cout << y << std::endl;
0167 std::cout << T(3.) * x << std::endl;
0168 std::cout << y * T(0.1) << std::endl;
0169 std::cout << (T(1) - y * T(0.1)) << std::endl;
0170 std::cout << apply(x, [](T x) { return std::sqrt(x); }) << std::endl;
0171
0172 std::cout << dot(x, y) << std::endl;
0173 std::cout << dot3(x, y) << std::endl;
0174 std::cout << dotSimple(x, y) << std::endl;
0175
0176
0177
0178
0179 Vec z = cross3(x, y);
0180 std::cout << z << std::endl;
0181 std::cout << cross2(x, y) << std::endl;
0182
0183 std::cout << "\nrotations" << std::endl;
0184
0185
0186 constexpr T ca = 0.9999500004166653;
0187 constexpr T sa = 0.009999833334166664;
0188
0189 constexpr Rot3<T> r1(ca, sa, 0, -sa, ca, 0, 0, 0, 1);
0190
0191 constexpr Rot2<T> r21(ca, sa, -sa, ca);
0192
0193 constexpr Rot3<T> r2((Vec){0, 1, 0, 0}, (Vec){0, 0, 1, 0}, (Vec){1, 0, 0, 0});
0194 constexpr Rot2<T> r22((Vec2D){0, 1}, (Vec2D){1, 0});
0195
0196 {
0197 std::cout << "\n3D rot" << std::endl;
0198 Vec xr = r1.rotate(x);
0199 std::cout << x << std::endl;
0200 std::cout << xr << std::endl;
0201 std::cout << r1.rotateBack(xr) << std::endl;
0202
0203 Rot3<T> rt = r1.transpose();
0204 Vec xt = rt.rotate(xr);
0205 std::cout << x << std::endl;
0206 std::cout << xt << std::endl;
0207 std::cout << rt.rotateBack(xt) << std::endl;
0208
0209 std::cout << r1 << std::endl;
0210 std::cout << rt << std::endl;
0211 std::cout << r1 * rt << std::endl;
0212 std::cout << r2 << std::endl;
0213 std::cout << r1 * r2 << std::endl;
0214 std::cout << r2 * r1 << std::endl;
0215 std::cout << r1 * r2.transpose() << std::endl;
0216 std::cout << r1.transpose() * r2 << std::endl;
0217 }
0218
0219 {
0220 std::cout << "\n2D rot" << std::endl;
0221 Vec2D xr = r21.rotate(xy(x));
0222 std::cout << xy(x) << std::endl;
0223 std::cout << xr << std::endl;
0224 std::cout << r21.rotateBack(xr) << std::endl;
0225
0226 Rot2<T> rt = r21.transpose();
0227 Vec2D xt = rt.rotate(xr);
0228 std::cout << xy(x) << std::endl;
0229 std::cout << xt << std::endl;
0230 std::cout << rt.rotateBack(xt) << std::endl;
0231
0232 std::cout << r21 << std::endl;
0233 std::cout << rt << std::endl;
0234 std::cout << r21 * rt << std::endl;
0235 std::cout << r22 << std::endl;
0236 std::cout << r21 * r22 << std::endl;
0237 std::cout << r22 * r21 << std::endl;
0238 std::cout << r21 * r22.transpose() << std::endl;
0239 std::cout << r21.transpose() * r22 << std::endl;
0240 }
0241 }
0242
0243 int main() {
0244 #ifdef CMS_USE_AVX2
0245 std::cout << "using AVX" << std::endl;
0246 #endif
0247 testBa();
0248 go<float>();
0249 #ifdef CMS_USE_AVX2
0250 go<double>(false);
0251 #else
0252 go<double>();
0253 #endif
0254 go2d<float>();
0255 go2d<double>();
0256
0257 return 0;
0258 }