359 lines
11 KiB
C++
359 lines
11 KiB
C++
#include "vec.hpp"
|
|
|
|
namespace geometry {
|
|
|
|
template <typename T, int L>
|
|
constexpr inline
|
|
T line_plane_intersection_d(const vec<L, T>& plane_point, // p0
|
|
const vec<L, T>& plane_normal, // n
|
|
const vec<L, T>& line_start, // l0
|
|
const vec<L, T>& line_vector // l
|
|
)
|
|
{
|
|
const T intersection = // d
|
|
dot(plane_point - line_start, plane_normal)
|
|
/ dot(line_vector, plane_normal);
|
|
|
|
return intersection;
|
|
}
|
|
|
|
template <typename T, int L>
|
|
constexpr inline
|
|
vec<L, T> line_plane_intersection(const vec<L, T>& plane_point, // p0
|
|
const vec<L, T>& plane_normal, // n
|
|
const vec<L, T>& line_start, // l0
|
|
const vec<L, T>& line_end
|
|
)
|
|
{
|
|
const auto line_vector = line_end - line_start; // l
|
|
|
|
const T intersection = line_plane_intersection_d(plane_point, plane_normal, line_start, line_vector); // d
|
|
|
|
return line_start + line_vector * intersection;
|
|
}
|
|
|
|
template <typename T, int L>
|
|
constexpr inline
|
|
vec<L, T> interpolate(const vec<L, T>& line_start,
|
|
const vec<L, T>& line_end,
|
|
T intersection
|
|
)
|
|
{
|
|
const auto line_vector = line_end - line_start; // l
|
|
|
|
return line_start + line_vector * intersection;
|
|
}
|
|
|
|
template <typename T, int L>
|
|
T clip_boundary(const vec<L, T>& plane_point, // X
|
|
const vec<L, T>& plane_normal, // Nc
|
|
const vec<L, T>& line_point
|
|
)
|
|
{
|
|
return dot(plane_normal, line_point - plane_point);
|
|
}
|
|
|
|
template <typename T>
|
|
inline T positive_modulo(T a, T b)
|
|
{
|
|
return (a % b + b) % b;
|
|
}
|
|
|
|
template <int polygon_len, typename T, int L>
|
|
inline int clip_polygon1(vec<L, T> * output,
|
|
const vec<L, T> plane_point,
|
|
const vec<L, T> plane_normal,
|
|
const vec<L, T> * polygon,
|
|
const int ix_s,
|
|
const int ix_f,
|
|
const bool last_inside)
|
|
{
|
|
const vec<L, T>& s = polygon[ix_s];
|
|
const vec<L, T>& f = polygon[ix_f];
|
|
|
|
bool this_inside = 0.f < clip_boundary<T, L>(plane_point,
|
|
plane_normal,
|
|
f);
|
|
|
|
int length = 0;
|
|
switch ((last_inside << 1) | (this_inside << 0)) {
|
|
case 0b00: // no output
|
|
length = 0;
|
|
break;
|
|
case 0b01: // I, F
|
|
length = 2;
|
|
{
|
|
const auto i = line_plane_intersection<T, L>(plane_point, plane_normal, s, f);
|
|
*output++ = i;
|
|
*output++ = f;
|
|
}
|
|
break;
|
|
case 0b10: // I
|
|
length = 1;
|
|
{
|
|
const auto i = line_plane_intersection<T, L>(plane_point, plane_normal, s, f);
|
|
*output++ = i;
|
|
}
|
|
break;
|
|
case 0b11: // F
|
|
length = 1;
|
|
*output++ = f;
|
|
break;
|
|
}
|
|
|
|
bool end_of_polygon = ix_f == (polygon_len - 1);
|
|
if (!end_of_polygon) {
|
|
return length +
|
|
clip_polygon1<polygon_len, T, L>(output,
|
|
plane_point,
|
|
plane_normal,
|
|
polygon,
|
|
ix_f,
|
|
ix_f + 1,
|
|
this_inside);
|
|
} else {
|
|
return length;
|
|
}
|
|
}
|
|
|
|
template <int polygon_len, int L, typename T>
|
|
int clip_polygon(vec<L, T> * output,
|
|
const vec<L, T>& plane_point,
|
|
const vec<L, T>& plane_normal,
|
|
const vec<L, T> * polygon
|
|
)
|
|
{
|
|
const vec<L, T> f = polygon[polygon_len - 1];
|
|
|
|
// It would be nice to remove the extra dot product, but the
|
|
// alternative seems likely uglier.
|
|
bool this_inside = 0.f < clip_boundary<T, L>(plane_point,
|
|
plane_normal,
|
|
f);
|
|
|
|
return clip_polygon1<polygon_len, T, L>(output,
|
|
plane_point,
|
|
plane_normal,
|
|
polygon,
|
|
polygon_len - 1, // ix_s
|
|
0, // ix_f
|
|
this_inside);
|
|
}
|
|
|
|
template <int polygon_len, typename T, int L, int M>
|
|
inline int clip_polygon1_2(vec<L, T> * output_1,
|
|
vec<M, T> * output_2,
|
|
const vec<L, T> plane_point,
|
|
const vec<L, T> plane_normal,
|
|
const vec<L, T> * polygon_1,
|
|
const vec<M, T> * polygon_2,
|
|
const int ix_s,
|
|
const int ix_f,
|
|
const bool last_inside)
|
|
{
|
|
const vec<L, T>& s_1 = polygon_1[ix_s];
|
|
const vec<L, T>& f_1 = polygon_1[ix_f];
|
|
|
|
const vec<M, T>& s_2 = polygon_2[ix_s];
|
|
const vec<M, T>& f_2 = polygon_2[ix_f];
|
|
|
|
bool this_inside = 0.f < clip_boundary<T, L>(plane_point,
|
|
plane_normal,
|
|
f_1);
|
|
|
|
int length = 0;
|
|
int control = (last_inside << 1) | (this_inside << 0);
|
|
switch (control) {
|
|
case 0b00: // no output
|
|
break;
|
|
case 0b10: // I
|
|
[[fallthrough]];
|
|
case 0b01: // I, F
|
|
{
|
|
const auto& i_1_start = s_1;
|
|
const auto i_1_vector = f_1 - s_1; // l
|
|
const T intersection = line_plane_intersection_d<T, L>(plane_point, plane_normal,
|
|
i_1_start, i_1_vector);
|
|
const vec<L, T> i_1 = i_1_start + i_1_vector * intersection;
|
|
|
|
*output_1++ = i_1;
|
|
*output_2++ = interpolate(s_2, f_2, intersection);
|
|
|
|
if (control == 0b01) { // I, F
|
|
*output_1++ = f_1;
|
|
*output_2++ = f_2;
|
|
length = 2;
|
|
} else {
|
|
length = 1;
|
|
}
|
|
}
|
|
break;
|
|
case 0b11: // F
|
|
*output_1++ = f_1;
|
|
*output_2++ = f_2;
|
|
length = 1;
|
|
break;
|
|
}
|
|
|
|
bool end_of_polygon = ix_f == (polygon_len - 1);
|
|
if (!end_of_polygon) {
|
|
return length +
|
|
clip_polygon1_2<polygon_len, T, L, M>(output_1,
|
|
output_2,
|
|
plane_point,
|
|
plane_normal,
|
|
polygon_1,
|
|
polygon_2,
|
|
ix_f,
|
|
ix_f + 1,
|
|
this_inside);
|
|
} else {
|
|
return length;
|
|
}
|
|
}
|
|
|
|
template <int polygon_len, typename T, int L, int M>
|
|
int clip_polygon_2(vec<L, T> * output_1,
|
|
vec<M, T> * output_2,
|
|
const vec<L, T>& plane_point,
|
|
const vec<L, T>& plane_normal,
|
|
const vec<L, T> * polygon_1,
|
|
const vec<M, T> * polygon_2
|
|
)
|
|
{
|
|
const vec<L, T> f = polygon_1[polygon_len - 1];
|
|
|
|
// It would be nice to remove the extra dot product, but the
|
|
// alternative seems likely uglier.
|
|
bool this_inside = 0.f < clip_boundary<T, L>(plane_point,
|
|
plane_normal,
|
|
f);
|
|
|
|
return clip_polygon1_2<polygon_len, T, L, M>(output_1,
|
|
output_2,
|
|
plane_point,
|
|
plane_normal,
|
|
polygon_1,
|
|
polygon_2,
|
|
polygon_len - 1, // ix_s
|
|
0, // ix_f
|
|
this_inside);
|
|
}
|
|
|
|
template <int polygon_len, typename T, int L, int M, int N>
|
|
inline int clip_polygon1_3(vec<L, T> * output_1,
|
|
vec<M, T> * output_2,
|
|
vec<N, T> * output_3,
|
|
const vec<L, T> plane_point,
|
|
const vec<L, T> plane_normal,
|
|
const vec<L, T> * polygon_1,
|
|
const vec<M, T> * polygon_2,
|
|
const vec<N, T> * polygon_3,
|
|
const int ix_s,
|
|
const int ix_f,
|
|
const bool last_inside)
|
|
{
|
|
const vec<L, T>& s_1 = polygon_1[ix_s];
|
|
const vec<L, T>& f_1 = polygon_1[ix_f];
|
|
|
|
const vec<M, T>& s_2 = polygon_2[ix_s];
|
|
const vec<M, T>& f_2 = polygon_2[ix_f];
|
|
|
|
const vec<N, T>& s_3 = polygon_3[ix_s];
|
|
const vec<N, T>& f_3 = polygon_3[ix_f];
|
|
|
|
bool this_inside = 0.f < clip_boundary<T, L>(plane_point,
|
|
plane_normal,
|
|
f_1);
|
|
|
|
int length = 0;
|
|
int control = (last_inside << 1) | (this_inside << 0);
|
|
switch (control) {
|
|
case 0b00: // no output
|
|
length = 0;
|
|
break;
|
|
case 0b10: // I
|
|
[[fallthrough]];
|
|
case 0b01: // I, F
|
|
{
|
|
const auto& i_1_start = s_1;
|
|
const auto i_1_vector = f_1 - s_1; // l
|
|
const T intersection = line_plane_intersection_d<T, L>(plane_point, plane_normal,
|
|
i_1_start, i_1_vector);
|
|
const vec<L, T> i_1 = i_1_start + i_1_vector * intersection;
|
|
|
|
*output_1++ = i_1;
|
|
*output_2++ = interpolate(s_2, f_2, intersection);
|
|
*output_3++ = interpolate(s_3, f_3, intersection);
|
|
|
|
if (control == 0b01) { // I, F
|
|
*output_1++ = f_1;
|
|
*output_2++ = f_2;
|
|
*output_3++ = f_3;
|
|
length = 2;
|
|
} else {
|
|
length = 1;
|
|
}
|
|
}
|
|
break;
|
|
case 0b11: // F
|
|
*output_1++ = f_1;
|
|
*output_2++ = f_2;
|
|
*output_3++ = f_3;
|
|
length = 1;
|
|
break;
|
|
}
|
|
|
|
bool end_of_polygon = ix_f == (polygon_len - 1);
|
|
if (!end_of_polygon) {
|
|
return length +
|
|
clip_polygon1_3<polygon_len, T, L, M, N>(output_1,
|
|
output_2,
|
|
output_3,
|
|
plane_point,
|
|
plane_normal,
|
|
polygon_1,
|
|
polygon_2,
|
|
polygon_3,
|
|
ix_f,
|
|
ix_f + 1,
|
|
this_inside);
|
|
} else {
|
|
return length;
|
|
}
|
|
}
|
|
|
|
template <int polygon_len, typename T, int L, int M, int N>
|
|
int clip_polygon_3(vec<L, T> * output_1,
|
|
vec<M, T> * output_2,
|
|
vec<N, T> * output_3,
|
|
const vec<L, T>& plane_point,
|
|
const vec<L, T>& plane_normal,
|
|
const vec<L, T> * polygon_1,
|
|
const vec<M, T> * polygon_2,
|
|
const vec<N, T> * polygon_3
|
|
)
|
|
{
|
|
const vec<L, T> f = polygon_1[polygon_len - 1];
|
|
|
|
// It would be nice to remove the extra dot product, but the
|
|
// alternative seems likely uglier.
|
|
bool this_inside = 0.f < clip_boundary<T, L>(plane_point,
|
|
plane_normal,
|
|
f);
|
|
|
|
return clip_polygon1_3<polygon_len, T, L, M>(output_1,
|
|
output_2,
|
|
output_3,
|
|
plane_point,
|
|
plane_normal,
|
|
polygon_1,
|
|
polygon_2,
|
|
polygon_3,
|
|
polygon_len - 1, // ix_s
|
|
0, // ix_f
|
|
this_inside);
|
|
}
|
|
|
|
}
|