LSSTApplications  17.0+11,17.0+34,17.0+56,17.0+57,17.0+59,17.0+7,17.0-1-g377950a+33,17.0.1-1-g114240f+2,17.0.1-1-g4d4fbc4+28,17.0.1-1-g55520dc+49,17.0.1-1-g5f4ed7e+52,17.0.1-1-g6dd7d69+17,17.0.1-1-g8de6c91+11,17.0.1-1-gb9095d2+7,17.0.1-1-ge9fec5e+5,17.0.1-1-gf4e0155+55,17.0.1-1-gfc65f5f+50,17.0.1-1-gfc6fb1f+20,17.0.1-10-g87f9f3f+1,17.0.1-11-ge9de802+16,17.0.1-16-ga14f7d5c+4,17.0.1-17-gc79d625+1,17.0.1-17-gdae4c4a+8,17.0.1-2-g26618f5+29,17.0.1-2-g54f2ebc+9,17.0.1-2-gf403422+1,17.0.1-20-g2ca2f74+6,17.0.1-23-gf3eadeb7+1,17.0.1-3-g7e86b59+39,17.0.1-3-gb5ca14a,17.0.1-3-gd08d533+40,17.0.1-30-g596af8797,17.0.1-4-g59d126d+4,17.0.1-4-gc69c472+5,17.0.1-6-g5afd9b9+4,17.0.1-7-g35889ee+1,17.0.1-7-gc7c8782+18,17.0.1-9-gc4bbfb2+3,w.2019.22
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
This file declares a class for representing angles.
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)
solver_t * s
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)
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.
std::ostream * os
Definition: Schema.cc:746