LSSTApplications  17.0+124,17.0+14,17.0+73,18.0.0+37,18.0.0+80,18.0.0-4-g68ffd23+4,18.1.0-1-g0001055+12,18.1.0-1-g03d53ef+5,18.1.0-1-g1349e88+55,18.1.0-1-g2505f39+44,18.1.0-1-g5315e5e+4,18.1.0-1-g5e4b7ea+14,18.1.0-1-g7e8fceb+4,18.1.0-1-g85f8cd4+48,18.1.0-1-g8ff0b9f+4,18.1.0-1-ga2c679d+1,18.1.0-1-gd55f500+35,18.1.0-10-gb58edde+2,18.1.0-11-g0997b02+4,18.1.0-13-gfe4edf0b+12,18.1.0-14-g259bd21+21,18.1.0-19-gdb69f3f+2,18.1.0-2-g5f9922c+24,18.1.0-2-gd3b74e5+11,18.1.0-2-gfbf3545+32,18.1.0-26-g728bddb4+5,18.1.0-27-g6ff7ca9+2,18.1.0-3-g52aa583+25,18.1.0-3-g8ea57af+9,18.1.0-3-gb69f684+42,18.1.0-3-gfcaddf3+6,18.1.0-32-gd8786685a,18.1.0-4-gf3f9b77+6,18.1.0-5-g1dd662b+2,18.1.0-5-g6dbcb01+41,18.1.0-6-gae77429+3,18.1.0-7-g9d75d83+9,18.1.0-7-gae09a6d+30,18.1.0-9-gc381ef5+4,w.2019.45
LSSTDataManagementBasePackage
Vector3d.cc
Go to the documentation of this file.
1 /*
2  * LSST Data Management System
3  * Copyright 2014-2015 AURA/LSST.
4  *
5  * This product includes software developed by the
6  * LSST Project (http://www.lsst.org/).
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the LSST License Statement and
19  * the GNU General Public License along with this program. If not,
20  * see <https://www.lsstcorp.org/LegalNotices/>.
21  */
22 
25 
26 #include "lsst/sphgeom/Vector3d.h"
27 
28 #if !defined(NO_SIMD) && defined(__x86_64__)
29  #include <x86intrin.h>
30 #endif
31 #include <cstdio>
32 #include <ostream>
33 
34 #include "lsst/sphgeom/Angle.h"
36 
37 
38 namespace lsst {
39 namespace sphgeom {
40 
42  static constexpr uint8_t UNUSED = 255;
43  // Given a 3 component vector (x, y, z), this LUT provides the indexes
44  // of the components in order of smallest absolute value to largest.
45  // The index into the LUT must be computed as:
46  //
47  // ((|x| > |z|) << 2) +
48  // ((|x| > |y|) << 1) +
49  // (|y| > |z|)
50  static uint8_t const COMPONENT[8][4] = {
51  {0, 1, 2, UNUSED},
52  {0, 2, 1, UNUSED},
53  {1, 0, 2, UNUSED},
54  {UNUSED, UNUSED, UNUSED, UNUSED},
55  {UNUSED, UNUSED, UNUSED, UNUSED},
56  {2, 0, 1, UNUSED},
57  {1, 2, 0, UNUSED},
58  {2, 1, 0, UNUSED}
59  };
60 #if defined(NO_SIMD) || !defined(__x86_64__)
61  double ax = std::fabs(_v[0]);
62  double ay = std::fabs(_v[1]);
63  double az = std::fabs(_v[2]);
64  int index = ((ax > az) << 2) +
65  ((ax > ay) << 1) +
66  (ay > az);
67  double w = _v[COMPONENT[index][2]];
68  if (w == 0.0) {
69  throw std::runtime_error("Cannot normalize zero vector");
70  }
71  // Divide components by the absolute value of the largest
72  // component to avoid overflow/underflow.
73  double maxabs = std::fabs(w);
74  double u = _v[COMPONENT[index][0]] / maxabs;
75  double v = _v[COMPONENT[index][1]] / maxabs;
76  w = std::copysign(1.0, w);
77  double d = u * u + v * v;
78  double norm = std::sqrt(1.0 + d);
79  _v[COMPONENT[index][0]] = u / norm;
80  _v[COMPONENT[index][1]] = v / norm;
81  _v[COMPONENT[index][2]] = w / norm;
82  return norm * maxabs;
83 #else
84  static __m128d const m0m0 = _mm_set_pd(-0.0, -0.0);
85  __m128d ayaz = _mm_andnot_pd(m0m0, _mm_loadu_pd(_v + 1));
86  __m128d axax = _mm_andnot_pd(m0m0, _mm_set1_pd(_v[0]));
87  __m128d az = _mm_unpackhi_pd(ayaz, _mm_setzero_pd());
88  int index = (_mm_movemask_pd(_mm_cmpgt_pd(axax, ayaz)) << 1) |
89  _mm_movemask_pd(_mm_cmplt_sd(az, ayaz));
90  // The lower double in uv contains the vector component
91  // with the lowest absolute value. The higher double contains
92  // the component with absolute value betweem the lowest and
93  // highest absolute values.
94  __m128d uv = _mm_set_pd(_v[COMPONENT[index][1]],
95  _v[COMPONENT[index][0]]);
96  // ww contains two copies of the vector component with the
97  // highest absolute value.
98  __m128d ww = _mm_set1_pd(_v[COMPONENT[index][2]]);
99  __m128d maxabs = _mm_andnot_pd(m0m0, ww);
100  if (_mm_ucomieq_sd(ww, _mm_setzero_pd())) {
101  throw std::runtime_error("Cannot normalize zero vector");
102  }
103  // Divide components by the absolute value of the largest
104  // component to avoid overflow/underflow.
105  uv = _mm_div_pd(uv, maxabs);
106  ww = _mm_or_pd(_mm_and_pd(m0m0, ww), _mm_set1_pd(1.0));
107  __m128d norm = _mm_mul_pd(uv, uv);
108  norm = _mm_sqrt_sd(
109  _mm_setzero_pd(),
110  _mm_add_sd(
111  _mm_set_sd(1.0),
112  _mm_add_sd(norm, _mm_unpackhi_pd(norm, _mm_setzero_pd()))
113  )
114  );
115  // Normalize components and store the results.
116  ww = _mm_div_sd(ww, norm);
117  uv = _mm_div_pd(uv, _mm_shuffle_pd(norm, norm, 0));
118  _mm_store_sd(&_v[COMPONENT[index][0]], uv);
119  _mm_storeh_pd(&_v[COMPONENT[index][1]], uv);
120  _mm_store_sd(&_v[COMPONENT[index][2]], ww);
121  return _mm_cvtsd_f64(_mm_mul_sd(norm, maxabs));
122 #endif
123 }
124 
126  // Use Rodrigues' rotation formula.
127  Vector3d const & v = *this;
128  double s = sin(a);
129  double c = cos(a);
130  return v * c + k.cross(v) * s + k * (k.dot(v) * (1.0 - c));
131 }
132 
134  char buf[128];
135  std::snprintf(buf, sizeof(buf), "[%.17g, %.17g, %.17g]",
136  v.x(), v.y(), v.z());
137  return os << buf;
138 }
139 
140 }} // namespace lsst::sphgeom
double normalize()
normalize scales this vector to have unit norm and returns its norm prior to scaling.
Definition: Vector3d.cc:41
table::Key< int > a
T norm(const T &x)
Definition: Integrate.h:194
std::ostream & operator<<(std::ostream &, Angle const &)
Definition: Angle.cc:34
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:125
Vector3d is a vector in ℝ³ with components stored in double precision.
Definition: Vector3d.h:44
double sin(Angle const &a)
Definition: Angle.h:102
double y() const
Definition: Vector3d.h:68
double cos(Angle const &a)
Definition: Angle.h:103
This file declares a class for representing vectors in ℝ³.
This file declares a class for representing unit vectors in ℝ³.
Vector3d cross(Vector3d const &v) const
cross returns the cross product of this unit vector and v.
Definition: UnitVector3d.h:155
double x() const
Definition: Vector3d.h:66
A base class for image defects.
T copysign(T... args)
T fabs(T... args)
This file declares a class for representing angles.
double w
Definition: CoaddPsf.cc:69
Angle represents an angle in radians.
Definition: Angle.h:43
double z() const
Definition: Vector3d.h:70
UnitVector3d is a unit vector in ℝ³ with components stored in double precision.
Definition: UnitVector3d.h:55
T sqrt(T... args)
std::ostream * os
Definition: Schema.cc:746
double dot(Vector3d const &v) const
dot returns the inner product of this unit vector and v.
Definition: UnitVector3d.h:152
T snprintf(T... args)
STL class.