LSSTApplications  20.0.0
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
lsst::sphgeom::sin
double sin(Angle const &a)
Definition: Angle.h:102
std::fabs
T fabs(T... args)
Angle.h
This file declares a class for representing angles.
lsst::afw::math::details::norm
T norm(const T &x)
Definition: Integrate.h:194
UnitVector3d.h
This file declares a class for representing unit vectors in ℝ³.
lsst::sphgeom::UnitVector3d::dot
double dot(Vector3d const &v) const
dot returns the inner product of this unit vector and v.
Definition: UnitVector3d.h:152
std::sqrt
T sqrt(T... args)
lsst::sphgeom::Vector3d
Vector3d is a vector in ℝ³ with components stored in double precision.
Definition: Vector3d.h:44
lsst::sphgeom::UnitVector3d
UnitVector3d is a unit vector in ℝ³ with components stored in double precision.
Definition: UnitVector3d.h:55
std::snprintf
T snprintf(T... args)
lsst::sphgeom::Vector3d::z
double z() const
Definition: Vector3d.h:70
std::ostream
STL class.
lsst::sphgeom::Vector3d::normalize
double normalize()
normalize scales this vector to have unit norm and returns its norm prior to scaling.
Definition: Vector3d.cc:41
Vector3d.h
This file declares a class for representing vectors in ℝ³.
std::copysign
T copysign(T... args)
lsst::sphgeom::UnitVector3d::cross
Vector3d cross(Vector3d const &v) const
cross returns the cross product of this unit vector and v.
Definition: UnitVector3d.h:155
std::runtime_error
STL class.
lsst
A base class for image defects.
Definition: imageAlgorithm.dox:1
lsst::sphgeom::Vector3d::rotatedAround
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
os
std::ostream * os
Definition: Schema.cc:746
lsst::sphgeom::Vector3d::y
double y() const
Definition: Vector3d.h:68
a
table::Key< int > a
Definition: TransmissionCurve.cc:466
w
double w
Definition: CoaddPsf.cc:69
lsst::sphgeom::Angle
Angle represents an angle in radians.
Definition: Angle.h:43
lsst::sphgeom::operator<<
std::ostream & operator<<(std::ostream &, Angle const &)
Definition: Angle.cc:34
lsst::sphgeom::cos
double cos(Angle const &a)
Definition: Angle.h:103
lsst::sphgeom::Vector3d::x
double x() const
Definition: Vector3d.h:66