18 #ifndef _SDF_TYPES_HH_
19 #define _SDF_TYPES_HH_
25 #define SDF_DEPRECATED(version) __attribute__((deprecated))
26 #define SDF_FORCEINLINE __attribute__((always_inline))
28 #define SDF_DEPRECATED(version)
29 #define SDF_FORCEINLINE __forceinline
31 #define SDF_DEPRECATED(version)
32 #define SDF_FORCEINLINE
42 inline bool equal(
const T &_a,
const T &_b,
43 const T &_epsilon = 1e-6)
45 return std::fabs(_a - _b) <= _epsilon;
56 public:
Color(
double _r = 0.0,
double _g = 0.0,
57 double _b = 0.0,
double _a = 1.0)
58 :
r(_r),
g(_g),
b(_b),
a(_a) {}
64 public:
friend std::ostream &
operator<< (std::ostream &_out,
67 _out << _pt.
r <<
" " << _pt.
g <<
" " << _pt.
b <<
" " << _pt.
a;
77 _in.setf(std::ios_base::skipws);
78 _in >> _pt.
r >> _pt.
g >> _pt.
b >> _pt.
a;
87 return equal(this->
r, _clr.
r) &&
122 _out << _pt.
x <<
" " << _pt.
y;
134 _in.setf(std::ios_base::skipws);
135 _in >> _pt.
x >> _pt.
y;
144 return this->
x == _pt.
x && this->
y == _pt.
y;
170 _out << _pt.
x <<
" " << _pt.
y;
182 _in.setf(std::ios_base::skipws);
183 _in >> _pt.
x >> _pt.
y;
212 :
x(_v.
x),
y(_v.
y),
z(_v.
z) {}
218 public:
Vector3(
double _x = 0.0,
double _y = 0.0,
double _z = 0.0)
219 :
x(_x),
y(_y),
z(_z) {}
226 return Vector3(this->
x + _v.
x, this->y + _v.
y, this->z + _v.
z);
235 c.
x = this->
y * _pt.
z - this->
z * _pt.
y;
236 c.
y = this->
z * _pt.
x - this->
x * _pt.
z;
237 c.
z = this->
x * _pt.
y - this->
y * _pt.
x;
247 return Vector3(this->
x * _v, this->
y * _v, this->
z * _v);
257 _out << _pt.
x <<
" " << _pt.
y <<
" " << _pt.
z;
279 return equal(this->
x, _pt.
x, 0.001) &&
280 equal(this->
y, _pt.
y, 0.001) &&
292 _in.setf(std::ios_base::skipws);
293 _in >> _pt.
x >> _pt.
y >> _pt.
z;
317 :
x(_q.
x),
y(_q.
y),
z(_q.
z),
w(_q.
w) {}
319 public:
Quaternion(
const double &_roll,
const double &_pitch,
330 public:
Quaternion(
double _w,
double _x,
double _y,
double _z)
331 :
x(_x),
y(_y),
z(_z),
w(_w) {}
369 this->
w*_q.
w - this->x*_q.
x - this->y*_q.
y - this->z*_q.
z,
370 this->w*_q.
x + this->x*_q.
w + this->y*_q.
z - this->z*_q.
y,
371 this->w*_q.
y - this->x*_q.
z + this->y*_q.
w + this->z*_q.
x,
372 this->w*_q.
z + this->x*_q.
y - this->y*_q.
x + this->z*_q.
w);
383 s = q.
w * q.
w + q.
x * q.
x + q.
y * q.
y + q.
z * q.
z;
418 squ = copy.
w * copy.
w;
419 sqx = copy.
x * copy.
x;
420 sqy = copy.
y * copy.
y;
421 sqz = copy.
z * copy.
z;
424 vec.
x = atan2(2 * (copy.
y*copy.
z + copy.
w*copy.
x),
425 squ - sqx - sqy + sqz);
428 double sarg = -2 * (copy.
x*copy.
z - copy.
w * copy.
y);
429 vec.
y = sarg <= -1.0 ? -0.5*M_PI :
430 (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
433 vec.
z = atan2(2 * (copy.
x*copy.
y + copy.
w*copy.
z),
434 squ + sqx - sqy - sqz);
443 double phi, the, psi;
449 this->
w = cos(phi) * cos(the) * cos(psi) + sin(phi) *
451 this->
x = sin(phi) * cos(the) * cos(psi) - cos(phi) *
453 this->
y = cos(phi) * sin(the) * cos(psi) + sin(phi) *
455 this->
z = cos(phi) * cos(the) * sin(psi) - sin(phi) *
466 s = sqrt(this->
w * this->
w + this->
x * this->
x +
504 _out << v.
x <<
" " << v.y <<
" " << v.z;
515 uuv = qvec.
Cross(uv);
516 uv *= (2.0f * this->
w);
519 return _v + uv + uuv;
529 double roll, pitch, yaw;
532 _in.setf(std::ios_base::skipws);
533 _in >> roll >> pitch >> yaw;
550 if (!std::isfinite(this->
x))
552 if (!std::isfinite(this->
y))
554 if (!std::isfinite(this->
z))
556 if (!std::isfinite(this->
w))
584 :
pos(0, 0, 0),
rot(1, 0, 0, 0)
600 public:
Pose(
double _x,
double _y,
double _z,
601 double _roll,
double _pitch,
double _yaw)
602 :
pos(_x, _y, _z),
rot(_roll, _pitch, _yaw)
614 _out << _pose.
pos <<
" " << _pose.
rot;
626 _in.setf(std::ios_base::skipws);
627 _in >> _pose.
pos >> _pose.
rot;
655 result.
x = _pose.
pos.
x + tmp.
x;
656 result.
y = _pose.
pos.
y + tmp.
y;
657 result.
z = _pose.
pos.
z + tmp.
z;
667 return this->
pos == _pose.
pos && this->
rot == _pose.
rot;
690 public:
Time(int32_t _sec, int32_t _nsec)
702 _out << _time.
sec <<
" " << _time.
nsec;
714 _in.setf(std::ios_base::skipws);
715 _in >> _time.
sec >> _time.
nsec;