646 lines
20 KiB
C++
646 lines
20 KiB
C++
|
// Begin License:
|
||
|
|
// Copyright (C) 2006-2008 Tobias Sargeant (tobias.sargeant@gmail.com).
|
||
|
|
// All rights reserved.
|
||
|
|
//
|
||
|
|
// This file is part of the Carve CSG Library (http://carve-csg.com/)
|
||
|
|
//
|
||
|
|
// This file may be used under the terms of the GNU General Public
|
||
|
|
// License version 2.0 as published by the Free Software Foundation
|
||
|
|
// and appearing in the file LICENSE.GPL2 included in the packaging of
|
||
|
|
// this file.
|
||
|
|
//
|
||
|
|
// This file is provided "AS IS" with NO WARRANTY OF ANY KIND,
|
||
|
|
// INCLUDING THE WARRANTIES OF DESIGN, MERCHANTABILITY AND FITNESS FOR
|
||
|
|
// A PARTICULAR PURPOSE.
|
||
|
|
// End:
|
||
|
|
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <carve/carve.hpp>
|
||
|
|
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
namespace carve {
|
||
|
|
namespace geom {
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
struct _uninitialized { };
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct base { double v[ndim]; };
|
||
|
|
|
||
|
|
template<> struct base<2> {union { double v[2]; struct { double x, y; }; }; };
|
||
|
|
template<> struct base<3> {union { double v[3]; struct { double x, y, z; }; }; };
|
||
|
|
template<> struct base<4> {union { double v[4]; struct { double x, y, z, w; }; }; };
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct vector : public base<ndim> {
|
||
|
|
static vector ZERO() { vector<ndim> r; r.setZero(); return r; }
|
||
|
|
double length2() const;
|
||
|
|
double length() const;
|
||
|
|
vector<ndim> &normalize();
|
||
|
|
vector<ndim> normalized() const;
|
||
|
|
bool exactlyZero() const;
|
||
|
|
bool isZero(double epsilon = EPSILON) const;
|
||
|
|
void setZero();
|
||
|
|
void fill(double val);
|
||
|
|
vector<ndim> &scaleBy(double d);
|
||
|
|
vector<ndim> &invscaleBy(double d);
|
||
|
|
vector<ndim> scaled(double d) const;
|
||
|
|
vector<ndim> invscaled(double d) const;
|
||
|
|
vector<ndim> &negate();
|
||
|
|
vector<ndim> negated() const;
|
||
|
|
double &operator[](unsigned i);
|
||
|
|
const double &operator[](unsigned i) const;
|
||
|
|
template<typename assign_t>
|
||
|
|
vector<ndim> &operator=(const assign_t &t);
|
||
|
|
std::string asStr() const;
|
||
|
|
vector() { setZero(); }
|
||
|
|
vector(noinit_t) { }
|
||
|
|
};
|
||
|
|
|
||
|
|
static inline vector<2> VECTOR(double x, double y) { vector<2> r; r.x = x; r.y = y; return r; }
|
||
|
|
static inline vector<3> VECTOR(double x, double y, double z) { vector<3> r; r.x = x; r.y = y; r.z = z; return r; }
|
||
|
|
static inline vector<4> VECTOR(double x, double y, double z, double w) { vector<4> r; r.x = x; r.y = y; r.z = z; r.w = w; return r; }
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
double dot(const vector<ndim> &a, const val_t &b) {
|
||
|
|
double r = 0.0;
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) r += a[i] * b[i];
|
||
|
|
return r;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator-(const vector<ndim> &a) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = -a[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &operator*=(vector<ndim> &a, double s) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] *= s;
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &operator/=(vector<ndim> &a, double s) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] /= s;
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator*(const vector<ndim> &a, double s) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] * s;
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator*(double s, const vector<ndim> &a) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] * s;
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator/(const vector<ndim> &a, double s) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] / s;
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &operator+=(vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] += b[i];
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
vector<ndim> &operator+=(vector<ndim> &a, const val_t &b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] += b[i];
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &operator+=(vector<ndim> &a, double b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] += b;
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator+(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
vector<ndim> operator+(const val_t &a, const vector<ndim> &b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
vector<ndim> operator+(const vector<ndim> &a, const val_t &b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator+(const vector<ndim> &a, double b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b;
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &operator-=(vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] -= b[i];
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
vector<ndim> &operator-=(vector<ndim> &a, const val_t &b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] -= b[i];
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &operator-=(vector<ndim> &a, double b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] -= b;
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator-(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
vector<ndim> operator-(const vector<ndim> &a, const val_t &b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
vector<ndim> operator-(const val_t &a, const vector<ndim> &b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b[i];
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> operator-(const vector<ndim> &a, double b) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b;
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> abs(const vector<ndim> &a) {
|
||
|
|
vector<ndim> c(NOINIT);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) c[i] = fabs(a[i]);
|
||
|
|
return c;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename assign_t, typename oper_t>
|
||
|
|
vector<ndim> &assign_op(vector<ndim> &a, const assign_t &t, oper_t op) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] = op(t[i]);
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename assign1_t, typename assign2_t, typename oper_t>
|
||
|
|
vector<ndim> &assign_op(vector<ndim> &a, const assign1_t &t1, const assign2_t &t2, oper_t op) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) a[i] = op(t1[i], t2[i]);
|
||
|
|
return a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename iter_t, typename adapt_t>
|
||
|
|
void bounds(iter_t begin, iter_t end, adapt_t adapt, vector<ndim> &min, vector<ndim> &max) {
|
||
|
|
if (begin == end) {
|
||
|
|
min.setZero();
|
||
|
|
max.setZero();
|
||
|
|
} else {
|
||
|
|
min = max = adapt(*begin);
|
||
|
|
while (++begin != end) {
|
||
|
|
vector<ndim> v = adapt(*begin);
|
||
|
|
assign_op(min, min, v, carve::util::min_functor());
|
||
|
|
assign_op(max, max, v, carve::util::max_functor());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename iter_t>
|
||
|
|
void bounds(iter_t begin, iter_t end, vector<ndim> &min, vector<ndim> &max) {
|
||
|
|
if (begin == end) {
|
||
|
|
min.setZero();
|
||
|
|
max.setZero();
|
||
|
|
} else {
|
||
|
|
min = max = *begin;
|
||
|
|
while (++begin != end) {
|
||
|
|
vector<ndim> v = *begin;
|
||
|
|
assign_op(min, min, v, carve::util::min_functor());
|
||
|
|
assign_op(max, max, v, carve::util::max_functor());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool operator==(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) { if (a[i] != b[i]) return false; }
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool operator!=(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return !(a == b);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool operator<(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) { if (a[i] < b[i]) return true; if (a[i] > b[i]) return false; }
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool operator<=(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return !(b < a);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool operator>(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return b < a;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool operator>=(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return !(a < b);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double distance2(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return (b - a).length2();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double distance(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return (b - a).length();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool equal(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return (b - a).isZero();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
int smallestAxis(const vector<ndim> &a) {
|
||
|
|
int x = 0;
|
||
|
|
double y = fabs(a[0]);
|
||
|
|
for (unsigned i = 1; i < ndim; ++i) {
|
||
|
|
double z = fabs(a[i]);
|
||
|
|
if (z <= y) { y = z; x = i; }
|
||
|
|
}
|
||
|
|
return x;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
int largestAxis(const vector<ndim> &a) {
|
||
|
|
int x = 0;
|
||
|
|
double y = fabs(a[0]);
|
||
|
|
for (unsigned i = 1; i < ndim; ++i) {
|
||
|
|
double z = fabs(a[i]);
|
||
|
|
if (z > y) { y = z; x = i; }
|
||
|
|
}
|
||
|
|
return x;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double vector<ndim>::length2() const { return dot(*this, *this); }
|
||
|
|
template<unsigned ndim>
|
||
|
|
double vector<ndim>::length() const { return sqrt(dot(*this, *this)); }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &vector<ndim>::normalize() { *this /= length(); return *this; }
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> vector<ndim>::normalized() const { return *this / length(); }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool vector<ndim>::exactlyZero() const {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) if (this->v[i]) return false;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
template<unsigned ndim>
|
||
|
|
bool vector<ndim>::isZero(double epsilon) const {
|
||
|
|
return length2() < epsilon * epsilon;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
void vector<ndim>::setZero() { for (size_t i = 0; i < ndim; ++i) this->v[i] = 0.0; }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
void vector<ndim>::fill(double val) { for (size_t i = 0; i < ndim; ++i) this->v[i] = val; }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &vector<ndim>::scaleBy(double d) { for (unsigned i = 0; i < ndim; ++i) this->v[i] *= d; return *this; }
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &vector<ndim>::invscaleBy(double d) { for (unsigned i = 0; i < ndim; ++i) this->v[i] /= d; return *this; }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> vector<ndim>::scaled(double d) const { return *this * d; }
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> vector<ndim>::invscaled(double d) const { return *this / d; }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> &vector<ndim>::negate() { for (unsigned i = 0; i < ndim; ++i) this->v[i] = -this->v[i]; return *this; }
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<ndim> vector<ndim>::negated() const { return -*this; }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double &vector<ndim>::operator[](unsigned i) { return this->v[i]; }
|
||
|
|
template<unsigned ndim>
|
||
|
|
const double &vector<ndim>::operator[](unsigned i) const { return this->v[i]; }
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
template<typename assign_t>
|
||
|
|
vector<ndim> &vector<ndim>::operator=(const assign_t &t) {
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) this->v[i] = t[i];
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
std::string vector<ndim>::asStr() const {
|
||
|
|
std::ostringstream out;
|
||
|
|
out << '<';
|
||
|
|
out << std::setprecision(24);
|
||
|
|
for (unsigned i = 0; i < ndim; ++i) { if (i) out << ','; out << this->v[i]; }
|
||
|
|
out << '>';
|
||
|
|
return out.str();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename iter_t, typename adapt_t>
|
||
|
|
void centroid(iter_t begin, iter_t end, adapt_t adapt, vector<ndim> &c) {
|
||
|
|
c.setZero();
|
||
|
|
int n = 0;
|
||
|
|
while (begin != end) { c += adapt(*begin++); ++n; }
|
||
|
|
c /= double(n);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<2> select(const vector<ndim> &a, int a1, int a2) {
|
||
|
|
vector<2> r(NOINIT);
|
||
|
|
r.v[0] = a.v[a1]; r.v[1] = a.v[a2];
|
||
|
|
return r;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
vector<3> select(const vector<ndim> &a, int a1, int a2, int a3) {
|
||
|
|
vector<3> r(NOINIT);
|
||
|
|
r.v[0] = a.v[a1]; r.v[1] = a.v[a2]; r.v[2] = a.v[a3];
|
||
|
|
return r;
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline vector<3> cross(const vector<3> &a, const vector<3> &b) {
|
||
|
|
// Compute a x b
|
||
|
|
return VECTOR(+(a.y * b.z - a.z * b.y),
|
||
|
|
-(a.x * b.z - a.z * b.x),
|
||
|
|
+(a.x * b.y - a.y * b.x));
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline double cross(const vector<2> &a, const vector<2> &b) {
|
||
|
|
// Compute a x b
|
||
|
|
return a.x * b.y - b.x * a.y;
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline double dotcross(const vector<3> &a, const vector<3> &b, const vector<3> &c) {
|
||
|
|
// Compute a . (b x c)
|
||
|
|
return
|
||
|
|
(a.x * b.y * c.z + a.y * b.z * c.x + a.z * b.x * c.y) -
|
||
|
|
(a.x * b.z * c.y + a.y * b.x * c.z + a.z * b.y * c.x);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
struct axis_pos {
|
||
|
|
int axis;
|
||
|
|
double pos;
|
||
|
|
|
||
|
|
axis_pos(int _axis, double _pos) : axis(_axis), pos(_pos) {
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double distance(const axis_pos &a, const vector<ndim> &b) {
|
||
|
|
return fabs(b[a.axis] - a.pos);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double distance2(const axis_pos &a, const vector<ndim> &b) {
|
||
|
|
double r = fabs(b[a.axis] - a.pos);
|
||
|
|
return r * r;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim> bool operator<(const axis_pos &a, const vector<ndim> &b) { return a.pos < b[a.axis]; }
|
||
|
|
template<unsigned ndim> bool operator<(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] < b.pos; }
|
||
|
|
|
||
|
|
template<unsigned ndim> bool operator<=(const axis_pos &a, const vector<ndim> &b) { return a.pos <= b[a.axis]; }
|
||
|
|
template<unsigned ndim> bool operator<=(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] <= b.pos; }
|
||
|
|
|
||
|
|
template<unsigned ndim> bool operator>(const axis_pos &a, const vector<ndim> &b) { return a.pos > b[a.axis]; }
|
||
|
|
template<unsigned ndim> bool operator>(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] > b.pos; }
|
||
|
|
|
||
|
|
template<unsigned ndim> bool operator>=(const axis_pos &a, const vector<ndim> &b) { return a.pos >= b[a.axis]; }
|
||
|
|
template<unsigned ndim> bool operator>=(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] >= b.pos; }
|
||
|
|
|
||
|
|
template<unsigned ndim> bool operator==(const axis_pos &a, const vector<ndim> &b) { return a.pos == b[a.axis]; }
|
||
|
|
template<unsigned ndim> bool operator==(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] == b.pos; }
|
||
|
|
|
||
|
|
template<unsigned ndim> bool operator!=(const axis_pos &a, const vector<ndim> &b) { return a.pos != b[a.axis]; }
|
||
|
|
template<unsigned ndim> bool operator!=(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] != b.pos; }
|
||
|
|
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct ray {
|
||
|
|
typedef vector<ndim> vector_t;
|
||
|
|
|
||
|
|
vector_t D, v;
|
||
|
|
|
||
|
|
ray() { }
|
||
|
|
ray(vector_t _D, vector_t _v) : D(_D), v(_v) { }
|
||
|
|
bool OK() const { return !D.isZero(); }
|
||
|
|
};
|
||
|
|
|
||
|
|
static inline double distance2(const ray<3> &r, const vector<3> &v) {
|
||
|
|
return cross(r.D, v - r.v).length2() / r.D.length2();
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline double distance(const ray<3> &r, const vector<3> &v) {
|
||
|
|
return sqrt(distance2(r, v));
|
||
|
|
}
|
||
|
|
|
||
|
|
inline double distance2(const ray<2> &r, const vector<2> &v) {
|
||
|
|
double t = cross(r.D, v - r.v);
|
||
|
|
return (t * t) / r.D.length2();
|
||
|
|
}
|
||
|
|
|
||
|
|
inline double distance(const ray<2> &r, const vector<2> &v) {
|
||
|
|
return sqrt(distance2(r, v));
|
||
|
|
}
|
||
|
|
template<unsigned ndim>
|
||
|
|
ray<ndim> rayThrough(const vector<ndim> &a, const vector<ndim> &b) {
|
||
|
|
return ray<ndim>(b - a, a);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct linesegment {
|
||
|
|
typedef vector<ndim> vector_t;
|
||
|
|
|
||
|
|
vector_t v1;
|
||
|
|
vector_t v2;
|
||
|
|
vector_t midpoint;
|
||
|
|
vector_t half_length;
|
||
|
|
|
||
|
|
linesegment(const vector_t &_v1, const vector_t &_v2) : v1(_v1), v2(_v2) {
|
||
|
|
update();
|
||
|
|
}
|
||
|
|
|
||
|
|
void update() {
|
||
|
|
midpoint = (v2 + v1) / 2.0;
|
||
|
|
half_length = (v2 - v1) / 2.0;
|
||
|
|
}
|
||
|
|
bool OK() const {
|
||
|
|
return !half_length.isZero();
|
||
|
|
}
|
||
|
|
void flip() {
|
||
|
|
std::swap(v1, v2);
|
||
|
|
half_length = (v2 - v1) / 2.0;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double distance2(const linesegment<ndim> &l, const vector<ndim> &v) {
|
||
|
|
vector<ndim> D = l.v2 - l.v1;
|
||
|
|
double t = dot(v - l.v1, D) / dot(D, D);
|
||
|
|
if (t <= 0.0) return (v - l.v1).length2();
|
||
|
|
if (t >= 1.0) return (v - l.v2).length2();
|
||
|
|
vector<ndim> vc = D * t + l.v1;
|
||
|
|
return (v - vc).length2();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
double distance(const linesegment<ndim> &l, const vector<ndim> &v) {
|
||
|
|
return sqrt(distance2(l, v));
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct plane {
|
||
|
|
typedef vector<ndim> vector_t;
|
||
|
|
|
||
|
|
vector_t N;
|
||
|
|
double d;
|
||
|
|
|
||
|
|
// plane() { }
|
||
|
|
plane() { N.setZero(); N[0] = 1.0; d= 0.0; }
|
||
|
|
plane(const vector_t &_N, vector_t _p) : N(_N), d(-dot(_p, _N)) { }
|
||
|
|
plane(const vector_t &_N, double _d) : N(_N), d(_d) { }
|
||
|
|
void negate() { N.negate(); d = -d; }
|
||
|
|
};
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
inline plane<ndim> operator-(const plane<ndim> &p) {
|
||
|
|
return plane<ndim>(-p.N, -p.d);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
double distance(const plane<ndim> &plane, const val_t &point) {
|
||
|
|
return dot(plane.N, point) + plane.d;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
static inline vector<ndim> closestPoint(const plane<ndim> &p, const vector<ndim> &v) {
|
||
|
|
return v - p.N * (p.d + dot(p.N, v)) / dot(p.N, p.N);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct sphere {
|
||
|
|
typedef vector<ndim> vector_t;
|
||
|
|
|
||
|
|
vector_t C;
|
||
|
|
double r;
|
||
|
|
|
||
|
|
// sphere() { }
|
||
|
|
sphere() { C.setZero(); r = 1.0; }
|
||
|
|
sphere(const vector_t &_C, double _r) : C(_C), r(_r) { }
|
||
|
|
};
|
||
|
|
|
||
|
|
template<unsigned ndim, typename val_t>
|
||
|
|
double distance(const sphere<ndim> &sphere, const val_t &point) {
|
||
|
|
return std::max(0.0, distance(sphere.C, point) - sphere.r);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
static inline vector<ndim> closestPoint(const sphere<ndim> &sphere, const vector<ndim> &point) {
|
||
|
|
return (point - sphere.C).normalized() * sphere.r;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ========================================================================
|
||
|
|
template<unsigned ndim>
|
||
|
|
struct tri {
|
||
|
|
typedef vector<ndim> vector_t;
|
||
|
|
|
||
|
|
vector_t v[3];
|
||
|
|
|
||
|
|
tri(vector_t _v[3]) {
|
||
|
|
std::copy(v, v+3, _v);
|
||
|
|
}
|
||
|
|
tri(const vector_t &a, const vector_t &b, const vector_t &c) {
|
||
|
|
v[0] = a; v[1] = b; v[2] = c;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
inline std::ostream &operator<<(std::ostream &o, const vector<ndim> &v) {
|
||
|
|
o << v.asStr();
|
||
|
|
return o;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
inline std::ostream &operator<<(std::ostream &o, const carve::geom::plane<ndim> &p) {
|
||
|
|
o << p.N << ";" << p.d;
|
||
|
|
return o;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
std::ostream &operator<<(std::ostream &o, const carve::geom::sphere<ndim> &sphere) {
|
||
|
|
o << "{sphere " << sphere.C << ";" << sphere.r << "}";
|
||
|
|
return o;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<unsigned ndim>
|
||
|
|
std::ostream &operator<<(std::ostream &o, const carve::geom::tri<ndim> &tri) {
|
||
|
|
o << "{tri " << tri.v[0] << ";" << tri.v[1] << ";" << tri.v[2] << "}";
|
||
|
|
return o;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|