LSST Applications 27.0.0,g0265f82a02+469cd937ee,g02d81e74bb+21ad69e7e1,g1470d8bcf6+cbe83ee85a,g2079a07aa2+e67c6346a6,g212a7c68fe+04a9158687,g2305ad1205+94392ce272,g295015adf3+81dd352a9d,g2bbee38e9b+469cd937ee,g337abbeb29+469cd937ee,g3939d97d7f+72a9f7b576,g487adcacf7+71499e7cba,g50ff169b8f+5929b3527e,g52b1c1532d+a6fc98d2e7,g591dd9f2cf+df404f777f,g5a732f18d5+be83d3ecdb,g64a986408d+21ad69e7e1,g858d7b2824+21ad69e7e1,g8a8a8dda67+a6fc98d2e7,g99cad8db69+f62e5b0af5,g9ddcbc5298+d4bad12328,ga1e77700b3+9c366c4306,ga8c6da7877+71e4819109,gb0e22166c9+25ba2f69a1,gb6a65358fc+469cd937ee,gbb8dafda3b+69d3c0e320,gc07e1c2157+a98bf949bb,gc120e1dc64+615ec43309,gc28159a63d+469cd937ee,gcf0d15dbbd+72a9f7b576,gdaeeff99f8+a38ce5ea23,ge6526c86ff+3a7c1ac5f1,ge79ae78c31+469cd937ee,gee10cc3b42+a6fc98d2e7,gf1cff7945b+21ad69e7e1,gfbcc870c63+9a11dc8c8f
LSST Data Management Base Package
Loading...
Searching...
No Matches
Vector3d.cc
Go to the documentation of this file.
1/*
2 * This file is part of sphgeom.
3 *
4 * Developed for the LSST Data Management System.
5 * This product includes software developed by the LSST Project
6 * (http://www.lsst.org).
7 * See the COPYRIGHT file at the top-level directory of this distribution
8 * for details of code ownership.
9 *
10 * This software is dual licensed under the GNU General Public License and also
11 * under a 3-clause BSD license. Recipients may choose which of these licenses
12 * to use; please see the files gpl-3.0.txt and/or bsd_license.txt,
13 * respectively. If you choose the GPL option then the following text applies
14 * (but note that there is still no warranty even if you opt for BSD instead):
15 *
16 * This program is free software: you can redistribute it and/or modify
17 * it under the terms of the GNU General Public License as published by
18 * the Free Software Foundation, either version 3 of the License, or
19 * (at your option) any later version.
20 *
21 * This program is distributed in the hope that it will be useful,
22 * but WITHOUT ANY WARRANTY; without even the implied warranty of
23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24 * GNU General Public License for more details.
25 *
26 * You should have received a copy of the GNU General Public License
27 * along with this program. If not, see <http://www.gnu.org/licenses/>.
28 */
29
32
34
35#if !defined(NO_SIMD) && defined(__x86_64__)
36 #include <x86intrin.h>
37#endif
38#include <cstdio>
39#include <ostream>
40
41#include "lsst/sphgeom/Angle.h"
43
44
45namespace lsst {
46namespace sphgeom {
47
49 static constexpr uint8_t UNUSED = 255;
50 // Given a 3 component vector (x, y, z), this LUT provides the indexes
51 // of the components in order of smallest absolute value to largest.
52 // The index into the LUT must be computed as:
53 //
54 // ((|x| > |z|) << 2) +
55 // ((|x| > |y|) << 1) +
56 // (|y| > |z|)
57 static uint8_t const COMPONENT[8][4] = {
58 {0, 1, 2, UNUSED},
59 {0, 2, 1, UNUSED},
60 {1, 0, 2, UNUSED},
61 {UNUSED, UNUSED, UNUSED, UNUSED},
62 {UNUSED, UNUSED, UNUSED, UNUSED},
63 {2, 0, 1, UNUSED},
64 {1, 2, 0, UNUSED},
65 {2, 1, 0, UNUSED}
66 };
67#if defined(NO_SIMD) || !defined(__x86_64__)
68 double ax = std::fabs(_v[0]);
69 double ay = std::fabs(_v[1]);
70 double az = std::fabs(_v[2]);
71 int index = ((ax > az) << 2) +
72 ((ax > ay) << 1) +
73 (ay > az);
74 double w = _v[COMPONENT[index][2]];
75 if (w == 0.0) {
76 throw std::runtime_error("Cannot normalize zero vector");
77 }
78 // Divide components by the absolute value of the largest
79 // component to avoid overflow/underflow.
80 double maxabs = std::fabs(w);
81 double u = _v[COMPONENT[index][0]] / maxabs;
82 double v = _v[COMPONENT[index][1]] / maxabs;
83 w = std::copysign(1.0, w);
84 double d = u * u + v * v;
85 double norm = std::sqrt(1.0 + d);
86 _v[COMPONENT[index][0]] = u / norm;
87 _v[COMPONENT[index][1]] = v / norm;
88 _v[COMPONENT[index][2]] = w / norm;
89 return norm * maxabs;
90#else
91 static __m128d const m0m0 = _mm_set_pd(-0.0, -0.0);
92 __m128d ayaz = _mm_andnot_pd(m0m0, _mm_loadu_pd(_v + 1));
93 __m128d axax = _mm_andnot_pd(m0m0, _mm_set1_pd(_v[0]));
94 __m128d az = _mm_unpackhi_pd(ayaz, _mm_setzero_pd());
95 int index = (_mm_movemask_pd(_mm_cmpgt_pd(axax, ayaz)) << 1) |
96 _mm_movemask_pd(_mm_cmplt_sd(az, ayaz));
97 // The lower double in uv contains the vector component
98 // with the lowest absolute value. The higher double contains
99 // the component with absolute value betweem the lowest and
100 // highest absolute values.
101 __m128d uv = _mm_set_pd(_v[COMPONENT[index][1]],
102 _v[COMPONENT[index][0]]);
103 // ww contains two copies of the vector component with the
104 // highest absolute value.
105 __m128d ww = _mm_set1_pd(_v[COMPONENT[index][2]]);
106 __m128d maxabs = _mm_andnot_pd(m0m0, ww);
107 if (_mm_ucomieq_sd(ww, _mm_setzero_pd())) {
108 throw std::runtime_error("Cannot normalize zero vector");
109 }
110 // Divide components by the absolute value of the largest
111 // component to avoid overflow/underflow.
112 uv = _mm_div_pd(uv, maxabs);
113 ww = _mm_or_pd(_mm_and_pd(m0m0, ww), _mm_set1_pd(1.0));
114 __m128d norm = _mm_mul_pd(uv, uv);
115 norm = _mm_sqrt_sd(
116 _mm_setzero_pd(),
117 _mm_add_sd(
118 _mm_set_sd(1.0),
119 _mm_add_sd(norm, _mm_unpackhi_pd(norm, _mm_setzero_pd()))
120 )
121 );
122 // Normalize components and store the results.
123 ww = _mm_div_sd(ww, norm);
124 uv = _mm_div_pd(uv, _mm_shuffle_pd(norm, norm, 0));
125 _mm_store_sd(&_v[COMPONENT[index][0]], uv);
126 _mm_storeh_pd(&_v[COMPONENT[index][1]], uv);
127 _mm_store_sd(&_v[COMPONENT[index][2]], ww);
128 return _mm_cvtsd_f64(_mm_mul_sd(norm, maxabs));
129#endif
130}
131
133 // Use Rodrigues' rotation formula.
134 Vector3d const & v = *this;
135 double s = sin(a);
136 double c = cos(a);
137 return v * c + k.cross(v) * s + k * (k.dot(v) * (1.0 - c));
138}
139
141 char buf[128];
142 std::snprintf(buf, sizeof(buf), "[%.17g, %.17g, %.17g]",
143 v.x(), v.y(), v.z());
144 return os << buf;
145}
146
147}} // namespace lsst::sphgeom
This file declares a class for representing unit vectors in ℝ³.
This file declares a class for representing vectors in ℝ³.
Angle represents an angle in radians.
Definition Angle.h:50
UnitVector3d is a unit vector in ℝ³ with components stored in double precision.
double dot(Vector3d const &v) const
dot returns the inner product of this unit vector and v.
Vector3d cross(Vector3d const &v) const
cross returns the cross product of this unit vector and v.
Vector3d is a vector in ℝ³ with components stored in double precision.
Definition Vector3d.h:51
Vector3d rotatedAround(UnitVector3d const &k, Angle a) const
rotatedAround returns a copy of this vector, rotated around the unit vector k by angle a according to...
Definition Vector3d.cc:132
double x() const
Definition Vector3d.h:73
double y() const
Definition Vector3d.h:75
double normalize()
normalize scales this vector to have unit norm and returns its norm prior to scaling.
Definition Vector3d.cc:48
double z() const
Definition Vector3d.h:77
T copysign(T... args)
T fabs(T... args)
T snprintf(T... args)
std::ostream & operator<<(std::ostream &, Angle const &)
Definition Angle.cc:41
double sin(Angle const &a)
Definition Angle.h:109
double cos(Angle const &a)
Definition Angle.h:110
This file declares a class for representing angles.
T sqrt(T... args)
double w
Definition CoaddPsf.cc:69