PCCMath.h 14.2 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
#include <math.h>
#include <string.h>
45
#include <type_traits>
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
46

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

50
51
#include <algorithm>

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

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

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

75
76
77
78
79
80
  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]; }
81

82
83
84
85
86
87
  template<typename ResultT>
  ResultT getNorm2() const
  {
    return Vec3<ResultT>(*this) * Vec3<ResultT>(*this);
  }

88
89
90
91
  T getNormInf() const
  {
    return std::max(data[2], std::max(abs(data[0]), abs(data[1])));
  }
92

93
  Vec3& operator=(const Vec3& rhs)
94
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
95
96
97
    memcpy(data, rhs.data, sizeof(data));
    return *this;
  }
98
99
100

  template<typename U>
  Vec3& operator+=(const typename pcc::Vec3<U>& rhs)
101
  {
102
103
104
    data[0] += rhs[0];
    data[1] += rhs[1];
    data[2] += rhs[2];
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
105
106
    return *this;
  }
107
108
109

  template<typename U>
  Vec3& operator-=(const typename pcc::Vec3<U>& rhs)
110
  {
111
112
113
    data[0] -= rhs[0];
    data[1] -= rhs[1];
    data[2] -= rhs[2];
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
114
115
    return *this;
  }
116
117
118

  template<typename U>
  Vec3& operator-=(U a)
119
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
120
121
122
123
124
    data[0] -= a;
    data[1] -= a;
    data[2] -= a;
    return *this;
  }
125
126
127

  template<typename U>
  Vec3& operator+=(U a)
128
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
129
130
131
132
133
    data[0] += a;
    data[1] += a;
    data[2] += a;
    return *this;
  }
134

135
  Vec3& operator<<=(int val)
136
  {
137
138
139
140
141
    data[0] <<= val;
    data[1] <<= val;
    data[2] <<= val;
    return *this;
  }
142

143
  Vec3& operator>>=(int val)
144
145
146
147
148
149
  {
    data[0] >>= val;
    data[1] >>= val;
    data[2] >>= val;
    return *this;
  }
150
151
152

  template<typename U>
  Vec3& operator/=(U a)
153
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
154
155
156
157
158
159
    assert(a != 0);
    data[0] /= a;
    data[1] /= a;
    data[2] /= a;
    return *this;
  }
160
161
162

  template<typename U>
  Vec3& operator*=(U a)
163
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
164
165
166
167
168
    data[0] *= a;
    data[1] *= a;
    data[2] *= a;
    return *this;
  }
169
170
171

  template<typename U>
  Vec3& operator=(const U a)
172
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
173
174
175
176
177
    data[0] = a;
    data[1] = a;
    data[2] = a;
    return *this;
  }
178
179
180

  template<typename U>
  Vec3& operator=(const U* rhs)
181
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
182
183
184
185
186
    data[0] = rhs[0];
    data[1] = rhs[1];
    data[2] = rhs[2];
    return *this;
  }
187
188
189
190
191
192

  Vec3 operator-() const { return Vec3<T>(-data[0], -data[1], -data[2]); }

  template<typename U>
  friend Vec3<typename std::common_type<T, U>::type>
  operator+(const Vec3& lhs, const typename pcc::Vec3<U>& rhs)
193
  {
194
195
    return Vec3<typename std::common_type<T, U>::type>(
      lhs[0] + rhs[0], lhs[1] + rhs[1], lhs[2] + rhs[2]);
196
  }
197
198
199
200
201
202

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator+(const U lhs, const Vec3& rhs)
203
  {
204
205
    return Vec3<typename std::common_type<T, U>::type>(
      lhs + rhs[0], lhs + rhs[1], lhs + rhs[2]);
206
  }
207
208
209
210
211
212

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator+(const Vec3& lhs, const U rhs)
213
  {
214
215
    return Vec3<typename std::common_type<T, U>::type>(
      lhs[0] + rhs, lhs[1] + rhs, lhs[2] + rhs);
216
  }
217
218
219
220

  template<typename U>
  friend Vec3<typename std::common_type<T, U>::type>
  operator-(const Vec3& lhs, const typename pcc::Vec3<U>& rhs)
221
  {
222
223
    return Vec3<typename std::common_type<T, U>::type>(
      lhs[0] - rhs[0], lhs[1] - rhs[1], lhs[2] - rhs[2]);
224
  }
225
226
227
228
229
230

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator-(const U lhs, const Vec3& rhs)
231
  {
232
233
    return Vec3<typename std::common_type<T, U>::type>(
      lhs - rhs[0], lhs - rhs[1], lhs - rhs[2]);
234
  }
235
236
237
238
239
240

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator-(const Vec3& lhs, const U rhs)
241
  {
242
243
    return Vec3<typename std::common_type<T, U>::type>(
      lhs[0] - rhs, lhs[1] - rhs, lhs[2] - rhs);
244
  }
245
246
247
248
249
250

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator*(const U lhs, const Vec3& rhs)
251
  {
252
253
    return Vec3<typename std::common_type<T, U>::type>(
      lhs * rhs[0], lhs * rhs[1], lhs * rhs[2]);
254
  }
255
256
257
258
259

  // todo(df): make this elementwise?
  template<typename U>
  friend typename std::common_type<T, U>::type
  operator*(pcc::Vec3<T> lhs, const pcc::Vec3<U>& rhs)
260
  {
261
    return (lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2]);
262
  }
263
264
265
266
267
268

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator*(const Vec3& lhs, const U rhs)
269
  {
270
271
    return Vec3<typename std::common_type<T, U>::type>(
      lhs[0] * rhs, lhs[1] * rhs, lhs[2] * rhs);
272
  }
273
274
275
276
277
278

  template<typename U>
  friend Vec3<typename std::enable_if<
    std::is_arithmetic<U>::value,
    typename std::common_type<T, U>::type>::type>
  operator/(const Vec3& lhs, const U rhs)
279
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
280
    assert(rhs != 0);
281
282
    return Vec3<typename std::common_type<T, U>::type>(
      lhs[0] / rhs, lhs[1] / rhs, lhs[2] / rhs);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
283
  }
284

285
  friend Vec3 operator<<(const Vec3& lhs, int val)
286
  {
287
    return Vec3<T>(lhs[0] << val, lhs[1] << val, lhs[2] << val);
288
  }
289

290
  friend Vec3 operator>>(const Vec3& lhs, int val)
291
  {
292
    return Vec3<T>(lhs[0] >> val, lhs[1] >> val, lhs[2] >> val);
293
  }
294

295
  bool operator<(const Vec3& rhs) const
296
  {
297
298
299
    if (data[0] == rhs[0]) {
      if (data[1] == rhs[1]) {
        return (data[2] < rhs[2]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
300
      }
301
      return (data[1] < rhs[1]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
302
    }
303
    return (data[0] < rhs[0]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
304
  }
305

306
  bool operator>(const Vec3& rhs) const
307
  {
308
309
310
    if (data[0] == rhs[0]) {
      if (data[1] == rhs[1]) {
        return (data[2] > rhs[2]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
311
      }
312
      return (data[1] > rhs[1]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
313
    }
314
    return (data[0] > rhs[0]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
315
  }
316

317
  bool operator==(const Vec3& rhs) const
318
  {
319
    return (data[0] == rhs[0] && data[1] == rhs[1] && data[2] == rhs[2]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
320
  }
321

322
  bool operator!=(const Vec3& rhs) const
323
  {
324
    return (data[0] != rhs[0] || data[1] != rhs[1] || data[2] != rhs[2]);
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
325
  }
326

327
  friend std::ostream& operator<<(std::ostream& os, const Vec3& vec)
328
  {
329
    os << vec[0] << " " << vec[1] << " " << vec[2];
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
330
331
    return os;
  }
332

333
  friend std::istream& operator>>(std::istream& is, Vec3& vec)
334
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
335
336
337
    is >> vec[0] >> vec[1] >> vec[2];
    return is;
  }
338

339
  Vec3(const T a) { data[0] = data[1] = data[2] = a; }
340

341
  Vec3(const T x, const T y, const T z)
342
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
343
344
345
346
    data[0] = x;
    data[1] = y;
    data[2] = z;
  }
347

348
  Vec3(const Vec3& vec)
349
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
350
351
352
353
    data[0] = vec.data[0];
    data[1] = vec.data[1];
    data[2] = vec.data[2];
  }
354
355
356
357
358
359
360
361
362

  template<typename U>
  Vec3(const typename pcc::Vec3<U>& vec)
  {
    data[0] = vec.data[0];
    data[1] = vec.data[1];
    data[2] = vec.data[2];
  }

363
364
  Vec3() = default;
  ~Vec3(void) = default;
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
365

366
private:
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
367
  T data[3];
368
369
370

  template<typename U>
  friend class pcc::Vec3;
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
371
372
};

373
template<typename T>
374
struct Box3 {
375
376
377
  Vec3<T> min;
  Vec3<T> max;
  bool contains(const Vec3<T> point) const
378
379
380
381
  {
    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
382
383
  }

384
  Box3 merge(const Box3& box)
385
  {
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
386
387
388
389
390
391
392
393
394
    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;
  }

395
  bool intersects(const Box3& box) const
396
397
398
399
  {
    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
400
401
  }

402
403
  template<typename SquaredT>
  SquaredT getDist2(const Vec3<T>& point) const
404
  {
405
406
407
408
    using U = SquaredT;
    U dx = U(std::max(std::max(min[0] - point[0], T()), point[0] - max[0]));
    U dy = U(std::max(std::max(min[1] - point[1], T()), point[1] - max[1]));
    U dz = U(std::max(std::max(min[2] - point[2], T()), point[2] - max[2]));
409
410
411
    return dx * dx + dy * dy + dz * dz;
  }

412
  friend std::ostream& operator<<(std::ostream& os, const Box3& box)
413
414
  {
    os << box.min[0] << " " << box.min[1] << " " << box.min[2] << " "
415
       << box.max[0] << " " << box.max[1] << " " << box.max[2];
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
416
417
    return os;
  }
418
  friend std::istream& operator>>(std::istream& is, Box3& box)
419
420
421
  {
    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
422
423
424
425
    return is;
  }
};

426
427
428
//---------------------------------------------------------------------------
// element-wise multiplication of two vectors

429
430
431
template<typename T, typename U>
Vec3<typename std::common_type<T, U>::type>
times(Vec3<T> lhs, const Vec3<U>& rhs)
432
{
433
434
  return Vec3<typename std::common_type<T, U>::type>(
    lhs[0] * rhs[0], lhs[1] * rhs[1], lhs[2] * rhs[2]);
435
436
}

437
438
439
440
//---------------------------------------------------------------------------

typedef DEPRECATED_MSVC Vec3<double> PCCVector3D DEPRECATED;
typedef DEPRECATED_MSVC Vec3<double> PCCPoint3D DEPRECATED;
441
typedef DEPRECATED_MSVC Box3<double> PCCBox3D DEPRECATED;
442
443
444
445
446
447
typedef DEPRECATED_MSVC Vec3<uint8_t> PCCColor3B DEPRECATED;

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

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

449
450
451
452
template<typename T>
T
PCCClip(const T& n, const T& lower, const T& upper)
{
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
453
454
  return std::max(lower, std::min(n, upper));
}
455
456
457
458
459
template<typename T>
bool
PCCApproximatelyEqual(
  T a, T b, T epsilon = std::numeric_limits<double>::epsilon())
{
Khaled Mammou's avatar
TMC3v0    
Khaled Mammou committed
460
461
  return fabs(a - b) <= ((fabs(a) < fabs(b) ? fabs(b) : fabs(a)) * epsilon);
}
462
463

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

465
inline int64_t
466
467
468
mortonAddr(const int32_t x, const int32_t y, const int32_t z)
{
  assert(x >= 0 && y >= 0 && z >= 0);
469
  int64_t answer = kMortonCode256X[(x >> 16) & 0xFF]
470
471
472
473
474
    | 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];
475
476
  return answer;
}
477
478

//---------------------------------------------------------------------------
479
// Convert a vector position to morton order address.
480
481

template<typename T>
482
int64_t
483
mortonAddr(const Vec3<T>& vec)
484
{
485
  return mortonAddr(int(vec.x()), int(vec.y()), int(vec.z()));
486
487
}

488
489
//---------------------------------------------------------------------------

490
491
492
493
494
495
inline int64_t
PCCClip(const int64_t& n, const int64_t& lower, const int64_t& upper)
{
  return std::max(lower, std::min(n, upper));
}

496
497
498
499
500
501
502
503
504
505
506
507
508
509
//---------------------------------------------------------------------------
// 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;
}

510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
//---------------------------------------------------------------------------
// 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;
}

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

553
554
} /* namespace pcc */

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