7.5.1 3D Point

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109

/*

A 3D point class template supporting vector arithmetic, dot and cross products, distances, angles,
unit vectors, normals, and rotation about an axis. Point equality and ordering are exact, while
`EQ(p, q)` provides epsilon equality for floating-point coordinates. Integer coordinates are
suitable for the exact algebraic operations `dot()`, `cross()`, and `sqnorm()` as long as
intermediate products do not overflow; metric operations preserve floating-point coordinates and
otherwise convert them to `double`.

`unit()` requires a nonzero point, `normal(p)` requires nonparallel vectors, and
`rotate(angle, axis)` requires a nonzero axis.

Overflow warning: the exact products `dot()`, `cross()`, and `sqnorm()` grow like the squared
coordinate magnitude. With `TPoint3<int>` these overflow a 32-bit `int` once coordinates exceed a
few tens of thousands, so use `Point3L` (`TPoint3<int64_t>`) for larger integer coordinates.

Type aliases:
- `Point3I = TPoint3<int>`
- `Point3L = TPoint3<int64_t>`
- `Point3D = TPoint3<double>`
- `Point3LD = TPoint3<long double>`

Time Complexity:
- O(1) per operation.

Space Complexity:
- O(1) for storage and auxiliary.

*/

#include <cassert>
#include <cmath>
#include <cstdint>
#include <tuple>
#include <type_traits>

const double EPS = 1e-9;

template<typename T, typename U, typename C = std::common_type_t<T, U>>
bool EQ(T a, U b) {
  if constexpr (std::is_floating_point_v<C>) return C(a) == C(b) || std::fabs(C(a) - C(b)) <= EPS;
  return C(a) == C(b);
}

template<typename T>
struct TPoint3 {
  using fp_t = std::conditional_t<std::is_floating_point<T>::value, T, double>;

  T x, y, z;

  TPoint3(T x = 0, T y = 0, T z = 0) : x(x), y(y), z(z) {}
  bool operator==(const TPoint3 &p) const { return std::tie(x, y, z) == std::tie(p.x, p.y, p.z); }

  friend bool EQ(const TPoint3 &a, const TPoint3 &b) {
    return EQ(a.x, b.x) && EQ(a.y, b.y) && EQ(a.z, b.z);
  }

  bool operator!=(const TPoint3 &p) const { return !(*this == p); }
  bool operator<(const TPoint3 &p) const { return std::tie(x, y, z) < std::tie(p.x, p.y, p.z); }
  TPoint3 operator+(const TPoint3 &p) const { return {x + p.x, y + p.y, z + p.z}; }
  TPoint3 operator-(const TPoint3 &p) const { return {x - p.x, y - p.y, z - p.z}; }
  TPoint3 operator*(T k) const { return {x * k, y * k, z * k}; }
  TPoint3<fp_t> operator/(fp_t k) const { return {(fp_t)x / k, (fp_t)y / k, (fp_t)z / k}; }
  T dot(const TPoint3 &p) const { return x * p.x + y * p.y + z * p.z; }  // Overflow warning.
  T sqnorm() const { return x * x + y * y + z * z; }                     // Overflow warning.

  fp_t norm() const {
    return std::hypot(std::hypot(static_cast<fp_t>(x), static_cast<fp_t>(y)), static_cast<fp_t>(z));
  }

  fp_t phi() const { return std::atan2(static_cast<fp_t>(y), static_cast<fp_t>(x)); }
  TPoint3<fp_t> unit() const { return *this / norm(); }

  TPoint3 cross(const TPoint3 &p) const {
    return {y * p.z - z * p.y, z * p.x - x * p.z, x * p.y - y * p.x};  // Overflow warning.
  }

  TPoint3<fp_t> normal(const TPoint3 &p) const { return cross(p).unit(); }

  fp_t theta() const {
    return std::atan2(std::hypot(static_cast<fp_t>(x), static_cast<fp_t>(y)), static_cast<fp_t>(z));
  }

  TPoint3<fp_t> rotate(fp_t angle, const TPoint3 &axis) const {
    fp_t s = std::sin(angle), c = std::cos(angle);
    TPoint3<fp_t> u = axis.unit(), v((fp_t)x, (fp_t)y, (fp_t)z);
    return u * v.dot(u) * (1 - c) + v * c - v.cross(u) * s;
  }
};

using Point3I = TPoint3<int>;
using Point3L = TPoint3<int64_t>;
using Point3D = TPoint3<double>;
using Point3LD = TPoint3<long double>;

/*** Example Usage ***/

using namespace std;

int main() {
  Point3I a(1, 0, 0), b(0, 1, 0);
  assert(a.dot(b) == 0);
  assert(a.cross(b) == Point3I(0, 0, 1));
  assert(a.sqnorm() == 1);

  Point3D p(1, 0, 0);
  assert(EQ(p.rotate(acos(-1.0) / 2, Point3D(0, 0, 1)), Point3D(0, 1, 0)));
  return 0;
}