PCCMath.h 12.4 KB
Newer Older
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
1
/* The copyright in this software is being made available under the BSD
David Flynn's avatar
David Flynn committed
2
3
4
 * Licence, included below.  This software may be subject to other third
 * party and contributor rights, including patent rights, and no such
 * rights are granted under this licence.
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
5
 *
David Flynn's avatar
David Flynn committed
6
 * Copyright (c) 2017-2018, ISO/IEC
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
7
8
9
10
11
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
David Flynn's avatar
David Flynn committed
12
13
14
15
16
17
18
19
20
21
 * * Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the distribution.
 *
 * * Neither the name of the ISO/IEC nor the names of its contributors
 *   may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
22
23
24
25
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
David Flynn's avatar
David Flynn committed
26
27
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
28
29
30
31
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
David Flynn's avatar
David Flynn committed
32
33
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
34
35
36
37
38
39
 */

#ifndef PCCMath_h
#define PCCMath_h

#include <assert.h>
David Flynn's avatar
David Flynn committed
40
#include <cstddef>
41
42
#include <iostream>
#include <limits>
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
43
44
45
#include <math.h>
#include <string.h>

46
#include "PCCMisc.h"
47
#include "tables.h"
48

Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
49
50
namespace pcc {
/// Vector dim 3
51
template<typename T>
52
class Vec3 {
53
public:
54
55
56
57
58
59
  T* begin() { return &data[0]; }
  const T* begin() const { return &data[0]; }

  T* end() { return &data[3]; }
  const T* end() const { return &data[3]; }

60
61
  T& operator[](size_t i)
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
62
63
64
    assert(i < 3);
    return data[i];
  }
65
66
  const T& operator[](size_t i) const
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
67
68
69
70
    assert(i < 3);
    return data[i];
  }
  size_t getElementCount() const { return 3; }
71

72
73
74
75
76
77
  T& x() { return data[0]; }
  T& y() { return data[1]; }
  T& z() { return data[2]; }
  const T& x() const { return data[0]; }
  const T& y() const { return data[1]; }
  const T& z() const { return data[2]; }
78

Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
79
80
  T getNorm() const { return static_cast<T>(sqrt(getNorm2())); }
  T getNorm2() const { return (*this) * (*this); }
81
82
83
84
  T getNormInf() const
  {
    return std::max(data[2], std::max(abs(data[0]), abs(data[1])));
  }
85
  Vec3& operator=(const Vec3& rhs)
86
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
87
88
89
    memcpy(data, rhs.data, sizeof(data));
    return *this;
  }
90
  Vec3& operator+=(const Vec3& rhs)
91
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
92
93
94
95
96
    data[0] += rhs.data[0];
    data[1] += rhs.data[1];
    data[2] += rhs.data[2];
    return *this;
  }
97
  Vec3& operator-=(const Vec3& rhs)
98
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
99
100
101
102
103
    data[0] -= rhs.data[0];
    data[1] -= rhs.data[1];
    data[2] -= rhs.data[2];
    return *this;
  }
104
  Vec3& operator-=(const T a)
105
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
106
107
108
109
110
    data[0] -= a;
    data[1] -= a;
    data[2] -= a;
    return *this;
  }
111
  Vec3& operator+=(const T a)
112
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
113
114
115
116
117
    data[0] += a;
    data[1] += a;
    data[2] += a;
    return *this;
  }
118
  Vec3& operator<<=(int val)
119
  {
120
121
122
123
124
    data[0] <<= val;
    data[1] <<= val;
    data[2] <<= val;
    return *this;
  }
125
  Vec3& operator>>=(int val)
126
127
128
129
130
131
  {
    data[0] >>= val;
    data[1] >>= val;
    data[2] >>= val;
    return *this;
  }
132
  Vec3& operator/=(const T a)
133
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
134
135
136
137
138
139
    assert(a != 0);
    data[0] /= a;
    data[1] /= a;
    data[2] /= a;
    return *this;
  }
140
  Vec3& operator*=(const T a)
141
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
142
143
144
145
146
    data[0] *= a;
    data[1] *= a;
    data[2] *= a;
    return *this;
  }
147
  Vec3& operator=(const T a)
148
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
149
150
151
152
153
    data[0] = a;
    data[1] = a;
    data[2] = a;
    return *this;
  }
154
  Vec3& operator=(const T* const rhs)
155
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
156
157
158
159
160
    data[0] = rhs[0];
    data[1] = rhs[1];
    data[2] = rhs[2];
    return *this;
  }
161
  T operator*(const Vec3& rhs) const
162
163
164
165
  {
    return (
      data[0] * rhs.data[0] + data[1] * rhs.data[1] + data[2] * rhs.data[2]);
  }
166
167
  Vec3 operator-() const { return Vec3<T>(-data[0], -data[1], -data[2]); }
  friend Vec3 operator+(const Vec3& lhs, const Vec3& rhs)
168
  {
169
    return Vec3<T>(
170
171
172
      lhs.data[0] + rhs.data[0], lhs.data[1] + rhs.data[1],
      lhs.data[2] + rhs.data[2]);
  }
173
  friend Vec3 operator+(const T lhs, const Vec3& rhs)
174
  {
175
    return Vec3<T>(lhs + rhs.data[0], lhs + rhs.data[1], lhs + rhs.data[2]);
176
  }
177
  friend Vec3 operator+(const Vec3& lhs, const T rhs)
178
  {
179
    return Vec3<T>(lhs.data[0] + rhs, lhs.data[1] + rhs, lhs.data[2] + rhs);
180
  }
181
  friend Vec3 operator-(const Vec3& lhs, const Vec3& rhs)
182
  {
183
    return Vec3<T>(
184
185
186
      lhs.data[0] - rhs.data[0], lhs.data[1] - rhs.data[1],
      lhs.data[2] - rhs.data[2]);
  }
187
  friend Vec3 operator-(const T lhs, const Vec3& rhs)
188
  {
189
    return Vec3<T>(lhs - rhs.data[0], lhs - rhs.data[1], lhs - rhs.data[2]);
190
  }
191
  friend Vec3 operator-(const Vec3& lhs, const T rhs)
192
  {
193
    return Vec3<T>(lhs.data[0] - rhs, lhs.data[1] - rhs, lhs.data[2] - rhs);
194
  }
195
  friend Vec3 operator*(const T lhs, const Vec3& rhs)
196
  {
197
    return Vec3<T>(lhs * rhs.data[0], lhs * rhs.data[1], lhs * rhs.data[2]);
198
  }
199
  friend Vec3 operator*(const Vec3& lhs, const T rhs)
200
  {
201
    return Vec3<T>(lhs.data[0] * rhs, lhs.data[1] * rhs, lhs.data[2] * rhs);
202
  }
203
  friend Vec3 operator/(const Vec3& lhs, const T rhs)
204
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
205
    assert(rhs != 0);
206
    return Vec3<T>(lhs.data[0] / rhs, lhs.data[1] / rhs, lhs.data[2] / rhs);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
207
  }
208
  friend Vec3 operator<<(const Vec3& lhs, int val)
209
  {
210
    return Vec3<T>(lhs.data[0] << val, lhs.data[1] << val, lhs.data[2] << val);
211
  }
212
  friend Vec3 operator>>(const Vec3& lhs, int val)
213
  {
214
    return Vec3<T>(lhs.data[0] >> val, lhs.data[1] >> val, lhs.data[2] >> val);
215
  }
216
  bool operator<(const Vec3& rhs) const
217
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
218
219
220
221
222
223
224
225
    if (data[0] == rhs.data[0]) {
      if (data[1] == rhs.data[1]) {
        return (data[2] < rhs.data[2]);
      }
      return (data[1] < rhs.data[1]);
    }
    return (data[0] < rhs.data[0]);
  }
226
  bool operator>(const Vec3& rhs) const
227
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
228
229
230
231
232
233
234
235
    if (data[0] == rhs.data[0]) {
      if (data[1] == rhs.data[1]) {
        return (data[2] > rhs.data[2]);
      }
      return (data[1] > rhs.data[1]);
    }
    return (data[0] > rhs.data[0]);
  }
236
  bool operator==(const Vec3& rhs) const
237
238
239
240
  {
    return (
      data[0] == rhs.data[0] && data[1] == rhs.data[1]
      && data[2] == rhs.data[2]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
241
  }
242
  bool operator!=(const Vec3& rhs) const
243
244
245
246
  {
    return (
      data[0] != rhs.data[0] || data[1] != rhs.data[1]
      || data[2] != rhs.data[2]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
247
  }
248
  friend std::ostream& operator<<(std::ostream& os, const Vec3& vec)
249
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
250
251
252
    os << vec[0] << " " << vec[1] << " " << vec[2] << std::endl;
    return os;
  }
253
  friend std::istream& operator>>(std::istream& is, Vec3& vec)
254
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
255
256
257
    is >> vec[0] >> vec[1] >> vec[2];
    return is;
  }
258
259
  Vec3(const T a) { data[0] = data[1] = data[2] = a; }
  Vec3(const T x, const T y, const T z)
260
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
261
262
263
264
    data[0] = x;
    data[1] = y;
    data[2] = z;
  }
265
  Vec3(const Vec3& vec)
266
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
267
268
269
270
    data[0] = vec.data[0];
    data[1] = vec.data[1];
    data[2] = vec.data[2];
  }
271
272
  Vec3() = default;
  ~Vec3(void) = default;
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
273

274
private:
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
275
276
277
  T data[3];
};

278
template<typename T>
279
struct Box3 {
280
281
282
  Vec3<T> min;
  Vec3<T> max;
  bool contains(const Vec3<T> point) const
283
284
285
286
  {
    return !(
      point.x() < min.x() || point.x() > max.x() || point.y() < min.y()
      || point.y() > max.y() || point.z() < min.z() || point.z() > max.z());
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
287
288
  }

289
  Box3 merge(const Box3& box)
290
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
291
292
293
294
295
296
297
298
299
    min.x() = std::min(min.x(), box.min.x());
    min.y() = std::min(min.y(), box.min.y());
    min.z() = std::min(min.z(), box.min.z());
    max.x() = std::max(max.x(), box.max.x());
    max.y() = std::max(max.y(), box.max.y());
    max.z() = std::max(max.z(), box.max.z());
    return box;
  }

300
  bool intersects(const Box3& box) const
301
302
303
304
  {
    return max.x() >= box.min.x() && min.x() <= box.max.x()
      && max.y() >= box.min.y() && min.y() <= box.max.y()
      && max.z() >= box.min.z() && min.z() <= box.max.z();
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
305
306
  }

307
  T getDist2(const Vec3<T>& point) const
308
309
310
311
312
313
314
  {
    const T dx = std::max(std::max(min[0] - point[0], 0.0), point[0] - max[0]);
    const T dy = std::max(std::max(min[1] - point[1], 0.0), point[1] - max[1]);
    const T dz = std::max(std::max(min[2] - point[2], 0.0), point[2] - max[2]);
    return dx * dx + dy * dy + dz * dz;
  }

315
  friend std::ostream& operator<<(std::ostream& os, const Box3& box)
316
317
318
  {
    os << box.min[0] << " " << box.min[1] << " " << box.min[2] << " "
       << box.max[0] << " " << box.max[1] << " " << box.max[2] << std::endl;
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
319
320
    return os;
  }
321
  friend std::istream& operator>>(std::istream& is, Box3& box)
322
323
324
  {
    is >> box.min[0] >> box.min[1] >> box.min[2] >> box.max[0] >> box.max[1]
      >> box.max[2];
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
325
326
327
328
    return is;
  }
};

329
330
331
332
333
334
335
336
337
338
//---------------------------------------------------------------------------
// element-wise multiplication of two vectors

template<typename T>
Vec3<T>
times(Vec3<T> lhs, const Vec3<T>& rhs)
{
  return Vec3<T>{lhs[0] * rhs[0], lhs[1] * rhs[1], lhs[2] * rhs[2]};
}

339
340
341
342
//---------------------------------------------------------------------------

typedef DEPRECATED_MSVC Vec3<double> PCCVector3D DEPRECATED;
typedef DEPRECATED_MSVC Vec3<double> PCCPoint3D DEPRECATED;
343
typedef DEPRECATED_MSVC Box3<double> PCCBox3D DEPRECATED;
344
345
346
347
348
349
typedef DEPRECATED_MSVC Vec3<uint8_t> PCCColor3B DEPRECATED;

template<typename T>
using PCCVector3 DEPRECATED = Vec3<T>;

//---------------------------------------------------------------------------
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
350

351
352
353
354
template<typename T>
T
PCCClip(const T& n, const T& lower, const T& upper)
{
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
355
356
  return std::max(lower, std::min(n, upper));
}
357
358
359
360
361
template<typename T>
bool
PCCApproximatelyEqual(
  T a, T b, T epsilon = std::numeric_limits<double>::epsilon())
{
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
362
363
  return fabs(a - b) <= ((fabs(a) < fabs(b) ? fabs(b) : fabs(a)) * epsilon);
}
364
365

//---------------------------------------------------------------------------
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
366

367
inline int64_t
368
369
370
mortonAddr(const int32_t x, const int32_t y, const int32_t z)
{
  assert(x >= 0 && y >= 0 && z >= 0);
371
  int64_t answer = kMortonCode256X[(x >> 16) & 0xFF]
372
373
374
375
376
    | kMortonCode256Y[(y >> 16) & 0xFF] | kMortonCode256Z[(z >> 16) & 0xFF];
  answer = answer << 24 | kMortonCode256X[(x >> 8) & 0xFF]
    | kMortonCode256Y[(y >> 8) & 0xFF] | kMortonCode256Z[(z >> 8) & 0xFF];
  answer = answer << 24 | kMortonCode256X[x & 0xFF] | kMortonCode256Y[y & 0xFF]
    | kMortonCode256Z[z & 0xFF];
377
378
  return answer;
}
379
380

//---------------------------------------------------------------------------
381
// Convert a vector position to morton order address.
382
383

template<typename T>
384
int64_t
385
mortonAddr(const Vec3<T>& vec)
386
{
387
  return mortonAddr(int(vec.x()), int(vec.y()), int(vec.z()));
388
389
}

390
391
//---------------------------------------------------------------------------

392
393
394
395
396
397
inline int64_t
PCCClip(const int64_t& n, const int64_t& lower, const int64_t& upper)
{
  return std::max(lower, std::min(n, upper));
}

398
399
400
401
402
403
404
405
406
407
408
409
410
411
//---------------------------------------------------------------------------
// Integer division of @x by 2^shift, rounding intermediate half values
// to +Inf.

inline int64_t
divExp2RoundHalfUp(int64_t x, int shift)
{
  if (!shift)
    return x;

  int64_t half = 1 << (shift - 1);
  return (x + half) >> shift;
}

412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
//---------------------------------------------------------------------------
// Integer division of @scalar by 2^shift, rounding intermediate half values
// away from zero.

inline int64_t
divExp2RoundHalfInf(int64_t scalar, int shift)
{
  if (!shift)
    return scalar;

  int64_t s0 = 1 << (shift - 1);
  return scalar >= 0 ? (s0 + scalar) >> shift : -((s0 - scalar) >> shift);
}

//---------------------------------------------------------------------------
// Integer division of @scalar by 2^shift, rounding intermediate half values
// away from zero.

inline uint64_t
divExp2RoundHalfInf(uint64_t scalar, int shift)
{
  if (!shift)
    return scalar;

  return ((1 << (shift - 1)) + scalar) >> shift;
}

//---------------------------------------------------------------------------
// Component-wise integer division of @vec by 2^shift, rounding intermediate
// half values away from zero.

template<typename T>
inline Vec3<T>
divExp2RoundHalfInf(Vec3<T> vec, int shift)
{
  for (int k = 0; k < 3; ++k)
    vec[k] = divExp2RoundHalfInf(vec[k], shift);

  return vec;
}

//---------------------------------------------------------------------------

455
456
} /* namespace pcc */

Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
457
#endif /* PCCMath_h */