You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
480 lines
20 KiB
480 lines
20 KiB
// David Eberly, Geometric Tools, Redmond WA 98052
|
|
// Copyright (c) 1998-2021
|
|
// Distributed under the Boost Software License, Version 1.0.
|
|
// https://www.boost.org/LICENSE_1_0.txt
|
|
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
|
|
// Version: 5.10.2021.04.30
|
|
|
|
#pragma once
|
|
|
|
#include <Mathematics/TIQuery.h>
|
|
#include <Mathematics/Triangle.h>
|
|
#include <Mathematics/Cylinder3.h>
|
|
#include <Mathematics/Vector2.h>
|
|
#include <Mathematics/Vector3.h>
|
|
|
|
// An algorithm for the test-intersection query between a triangle and a
|
|
// finite cylinder is described in
|
|
// https://www.geometrictools.com/Documentation/IntersectionTriangleCylinder.pdf
|
|
// The code here is an implementation of that algorithm. The comments include
|
|
// references to Figure 1 of the PDF.
|
|
|
|
namespace gte
|
|
{
|
|
template <typename Real>
|
|
class TIQuery<Real, Triangle3<Real>, Cylinder3<Real>>
|
|
{
|
|
public:
|
|
struct Result
|
|
{
|
|
Result(bool inIntersect = false)
|
|
:
|
|
intersect(inIntersect)
|
|
{
|
|
}
|
|
|
|
bool intersect;
|
|
};
|
|
|
|
Result operator()(Triangle3<Real> const& triangle, Cylinder3<Real> const& cylinder)
|
|
{
|
|
// Get a right-handed orthonormal basis from the cylinder axis
|
|
// direction.
|
|
std::array<Vector3<Real>, 3> basis{}; // {U2,U0,U1}
|
|
basis[0] = cylinder.axis.direction;
|
|
ComputeOrthogonalComplement(1, basis.data());
|
|
|
|
// Compute coordinates of the triangle vertices in the coordinate
|
|
// system {C;U0,U1,U2}, where C is the cylinder center and U2 is
|
|
// the cylinder direction. The basis {U0,U1,U2} is orthonormal and
|
|
// right-handed.
|
|
std::array<Vector3<Real>, 3> P{};
|
|
for (size_t i = 0; i < 3; ++i)
|
|
{
|
|
Vector3<Real> delta = triangle.v[i] - cylinder.axis.origin;
|
|
P[i][0] = Dot(basis[1], delta); // x[i]
|
|
P[i][1] = Dot(basis[2], delta); // y[i]
|
|
P[i][2] = Dot(basis[0], delta); // z[i]
|
|
}
|
|
|
|
// Sort the triangle vertices so that z[0] <= z[1] <= z[2].
|
|
size_t j0, j1, j2;
|
|
if (P[0][2] < P[1][2])
|
|
{
|
|
if (P[2][2] < P[0][2])
|
|
{
|
|
j0 = 2;
|
|
j1 = 0;
|
|
j2 = 1;
|
|
}
|
|
else if (P[2][2] < P[1][2])
|
|
{
|
|
j0 = 0;
|
|
j1 = 2;
|
|
j2 = 1;
|
|
}
|
|
else
|
|
{
|
|
j0 = 0;
|
|
j1 = 1;
|
|
j2 = 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (P[2][2] < P[1][2])
|
|
{
|
|
j0 = 2;
|
|
j1 = 1;
|
|
j2 = 0;
|
|
}
|
|
else if (P[2][2] < P[0][2])
|
|
{
|
|
j0 = 1;
|
|
j1 = 2;
|
|
j2 = 0;
|
|
}
|
|
else
|
|
{
|
|
j0 = 1;
|
|
j1 = 0;
|
|
j2 = 2;
|
|
}
|
|
}
|
|
|
|
std::array<Real, 3> z = { P[j0][2], P[j1][2], P[j2][2] };
|
|
|
|
// Maintain the xy-components and z-components separately. The
|
|
// z-components are used for clipping against bottom and top
|
|
// planes of the cylinder. The xy-components are used for
|
|
// disk-containment tests x * x + y * y <= r * r.
|
|
|
|
// Attempt an early exit by testing whether the triangle is
|
|
// strictly outside the cylinder slab -h/2 < z < h/2.
|
|
Real const hhalf = static_cast<Real>(0.5) * cylinder.height;
|
|
if (z[2] < -hhalf)
|
|
{
|
|
// The triangle is strictly below the bottom-disk plane of
|
|
// the cylinder. See case 0a of Figure 1 in the PDF.
|
|
return Result(false);
|
|
}
|
|
|
|
if (z[0] > hhalf)
|
|
{
|
|
// The triangle is strictly above the top-disk plane of the
|
|
// cylinder. See case 0b of Figure 1 in the PDF.
|
|
return Result(false);
|
|
}
|
|
|
|
// Project the triangle vertices onto the xy-plane.
|
|
std::array<Vector2<Real>, 3> Q
|
|
{
|
|
Vector2<Real>{ P[j0][0], P[j0][1] },
|
|
Vector2<Real>{ P[j1][0], P[j1][1] },
|
|
Vector2<Real>{ P[j2][0], P[j2][1] }
|
|
};
|
|
|
|
// Attempt an early exit when the triangle does not have to be
|
|
// clipped.
|
|
Real const& radius = cylinder.radius;
|
|
if (-hhalf <= z[0] && z[2] <= hhalf)
|
|
{
|
|
// The triangle is between the planes of the top-disk and
|
|
// the bottom disk of the cylinder. Determine whether the
|
|
// projection of the triangle onto a plane perpendicular
|
|
// to the cylinder axis overlaps the disk of projection
|
|
// of the cylinder onto the same plane. See case 3a of
|
|
// Figure 1 of the PDF.
|
|
return Result(DiskOverlapsPolygon(3, Q.data(), radius));
|
|
}
|
|
|
|
// Clip against |z| <= h/2. At this point we know that z2 >= -h/2
|
|
// and z0 <= h/2 with either z0 < -h/2 or z2 > h/2 or both. The
|
|
// test-intersection query involves testing for overlap between
|
|
// the xy-projection of the clipped triangle and the xy-projection
|
|
// of the cylinder (a disk in the projection plane). The code
|
|
// below computes the vertices of the projection of the clipped
|
|
// triangle. The t-values of the triangle-edge parameterizations
|
|
// satisfy 0 <= t <= 1.
|
|
if (z[0] < -hhalf)
|
|
{
|
|
if (z[2] > hhalf)
|
|
{
|
|
if (z[1] >= hhalf)
|
|
{
|
|
// Cases 4a and 4b of Figure 1 in the PDF.
|
|
//
|
|
// The edge <V0,V1> is parameterized by V0+t*(V1-V0).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z0 + t * (z1 - z0)
|
|
// t = (-h/2 - z0) / (z1 - z0) = numerNeg0 / denom10
|
|
// and on the tob of the slab,
|
|
// +h/2 = z0 + t * (z1 - z0)
|
|
// t = (+h/2 - z0) / (z1 - z0) = numerPos0 / denom10
|
|
//
|
|
// The edge <V0,V2> is parameterized by V0+t*(V2-V0).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z0 + t * (z2 - z0)
|
|
// t = (-h/2 - z0) / (z2 - z0) = numerNeg0 / denom20
|
|
// and on the tob of the slab,
|
|
// +h/2 = z0 + t * (z2 - z0)
|
|
// t = (+h/2 - z0) / (z2 - z0) = numerPos0 / denom20
|
|
Real numerNeg0 = -hhalf - z[0];
|
|
Real numerPos0 = +hhalf - z[0];
|
|
Real denom10 = z[1] - z[0];
|
|
Real denom20 = z[2] - z[0];
|
|
Vector2<Real> dir20 = (Q[2] - Q[0]) / denom20;
|
|
Vector2<Real> dir10 = (Q[1] - Q[0]) / denom10;
|
|
std::array<Vector2<Real>, 4> polygon
|
|
{
|
|
Q[0] + numerNeg0 * dir20,
|
|
Q[0] + numerNeg0 * dir10,
|
|
Q[0] + numerPos0 * dir10,
|
|
Q[0] + numerPos0 * dir20
|
|
};
|
|
return Result(DiskOverlapsPolygon(4, polygon.data(), radius));
|
|
}
|
|
else if (z[1] <= -hhalf)
|
|
{
|
|
// Cases 4c and 4d of Figure 1 of the PDF.
|
|
//
|
|
// The edge <V2,V0> is parameterized by V0+t*(V2-V0).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z2 + t * (z0 - z2)
|
|
// t = (-h/2 - z2) / (z0 - z2) = numerNeg2 / denom02
|
|
// and on the tob of the slab,
|
|
// +h/2 = z2 + t * (z0 - z2)
|
|
// t = (+h/2 - z2) / (z0 - z2) = numerPos2 / denom02
|
|
//
|
|
// The edge <V2,V1> is parameterized by V2+t*(V1-V2).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z2 + t * (z1 - z2)
|
|
// t = (-h/2 - z2) / (z1 - z2) = numerNeg2 / denom12
|
|
// and on the top of the slab,
|
|
// +h/2 = z2 + t * (z1 - z2)
|
|
// t = (+h/2 - z2) / (z1 - z2) = numerPos2 / denom12
|
|
Real numerNeg2 = -hhalf - z[2];
|
|
Real numerPos2 = +hhalf - z[2];
|
|
Real denom02 = z[0] - z[2];
|
|
Real denom12 = z[1] - z[2];
|
|
Vector2<Real> dir02 = (Q[0] - Q[2]) / denom02;
|
|
Vector2<Real> dir12 = (Q[1] - Q[2]) / denom12;
|
|
std::array<Vector2<Real>, 4> polygon
|
|
{
|
|
Q[2] + numerNeg2 * dir02,
|
|
Q[2] + numerNeg2 * dir12,
|
|
Q[2] + numerPos2 * dir12,
|
|
Q[2] + numerPos2 * dir02
|
|
};
|
|
return Result(DiskOverlapsPolygon(4, polygon.data(), radius));
|
|
}
|
|
else // -hhalf < z[1] < hhalf
|
|
{
|
|
// Case 5 of Figure 1 of the PDF.
|
|
//
|
|
// The edge <V0,V2> is parameterized by V0+t*(V2-V0).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z0 + t * (z2 - z0)
|
|
// t = (-h/2 - z0) / (z2 - z0) = numerNeg0 / denom20
|
|
// and on the tob of the slab,
|
|
// +h/2 = z0 + t * (z2 - z0)
|
|
// t = (+h/2 - z0) / (z2 - z0) = numerPos0 / denom20
|
|
//
|
|
// The edge <V1,V0> is parameterized by V1+t*(V0-V1).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z1 + t * (z0 - z1)
|
|
// t = (-h/2 - z1) / (z0 - z1) = numerNeg1 / denom01
|
|
//
|
|
// The edge <V1,V2> is parameterized by V1+t*(V2-V1).
|
|
// On the top of the slab,
|
|
// +h/2 = z1 + t * (z2 - z1)
|
|
// t = (+h/2 - z1) / (z2 - z1) = numerPos1 / denom21
|
|
Real numerNeg0 = -hhalf - z[0];
|
|
Real numerPos0 = +hhalf - z[0];
|
|
Real numerNeg1 = -hhalf - z[1];
|
|
Real numerPos1 = +hhalf - z[1];
|
|
Real denom20 = z[2] - z[0];
|
|
Real denom01 = z[0] - z[1];
|
|
Real denom21 = z[2] - z[1];
|
|
Vector2<Real> dir20 = (Q[2] - Q[0]) / denom20;
|
|
Vector2<Real> dir01 = (Q[0] - Q[1]) / denom01;
|
|
Vector2<Real> dir21 = (Q[2] - Q[1]) / denom21;
|
|
std::array<Vector2<Real>, 5> polygon
|
|
{
|
|
Q[0] + numerNeg0 * dir20,
|
|
Q[1] + numerNeg1 * dir01,
|
|
Q[1],
|
|
Q[1] + numerPos1 * dir21,
|
|
Q[0] + numerPos0 * dir20
|
|
};
|
|
return Result(DiskOverlapsPolygon(5, polygon.data(), radius));
|
|
}
|
|
}
|
|
else if (z[2] > -hhalf)
|
|
{
|
|
if (z[1] <= -hhalf)
|
|
{
|
|
// Cases 3b and 3c of Figure 1 of the PDF.
|
|
//
|
|
// The edge <V2,V0> is parameterized by V2+t*(V0-V2).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z2 + t * (z0 - z2)
|
|
// t = (-h/2 - z2) / (z0 - z2) = numerNeg2 / denom02
|
|
//
|
|
// The edge <V2,V1> is parameterized by V2+t*(V1-V2).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z2 + t * (z1 - z2)
|
|
// t = (-h/2 - z2) / (z1 - z2) = numerNeg2 / denom12
|
|
Real numerNeg2 = -hhalf - z[2];
|
|
Real denom02 = z[0] - z[2];
|
|
Real denom12 = z[1] - z[2];
|
|
Vector2<Real> dir02 = (Q[0] - Q[2]) / denom02;
|
|
Vector2<Real> dir12 = (Q[1] - Q[2]) / denom12;
|
|
std::array<Vector2<Real>, 3> polygon
|
|
{
|
|
Q[2],
|
|
Q[2] + numerNeg2 * dir02,
|
|
Q[2] + numerNeg2 * dir12
|
|
};
|
|
return Result(DiskOverlapsPolygon(3, polygon.data(), radius));
|
|
}
|
|
else // z[1] > -hhalf
|
|
{
|
|
// Case 4e of Figure 1 of the PDF.
|
|
//
|
|
// The edge <V0,V1> is parameterized by V0+t*(V1-V0).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z0 + t * (z1 - z0)
|
|
// t = (-h/2 - z0) / (z1 - z0) = numerNeg0 / denom10
|
|
//
|
|
// The edge <V0,V2> is parameterized by V0+t*(V2-V0).
|
|
// On the bottom of the slab,
|
|
// -h/2 = z0 + t * (z2 - z0)
|
|
// t = (-h/2 - z0) / (z2 - z0) = numerNeg0 / denom20
|
|
Real numerNeg0 = -hhalf - z[0];
|
|
Real denom10 = z[1] - z[0];
|
|
Real denom20 = z[2] - z[0];
|
|
Vector2<Real> dir20 = (Q[2] - Q[0]) / denom20;
|
|
Vector2<Real> dir10 = (Q[1] - Q[0]) / denom10;
|
|
std::array<Vector2<Real>, 4> polygon
|
|
{
|
|
Q[0] + numerNeg0 * dir20,
|
|
Q[0] + numerNeg0 * dir10,
|
|
Q[1],
|
|
Q[2]
|
|
};
|
|
return Result(DiskOverlapsPolygon(4, polygon.data(), radius));
|
|
}
|
|
}
|
|
else // z[2] == -hhalf
|
|
{
|
|
if (z[1] < -hhalf)
|
|
{
|
|
// Case 1a of Figure 1 of the PDF.
|
|
return Result(DiskOverlapsPoint(Q[2], radius));
|
|
}
|
|
else
|
|
{
|
|
// Case 2a of Figure 1 of the PDF.
|
|
return Result(DiskOverlapsSegment(Q[1], Q[2], radius));
|
|
}
|
|
}
|
|
}
|
|
else if (z[0] < hhalf)
|
|
{
|
|
if (z[1] >= hhalf)
|
|
{
|
|
// Cases 3d and 3e of Figure 1 of the PDF.
|
|
//
|
|
// The edge <V0,V1> is parameterized by V0+t*(V1-V0).
|
|
// On the top of the slab,
|
|
// +h/2 = z0 + t * (z1 - z0)
|
|
// t = (+h/2 - z0) / (z1 - z0) = numerPos0 / denom10
|
|
//
|
|
// The edge <V0,V2> is parameterized by V0+t*(V2-V0).
|
|
// On the top of the slab,
|
|
// +h/2 = z0 + t * (z2 - z0)
|
|
// t = (+h/2 - z0) / (z2 - z0) = numerPos0 / denom20
|
|
Real numerPos0 = +hhalf - z[0];
|
|
Real denom10 = z[1] - z[0];
|
|
Real denom20 = z[2] - z[0];
|
|
Vector2<Real> dir10 = (Q[1] - Q[0]) / denom10;
|
|
Vector2<Real> dir20 = (Q[2] - Q[0]) / denom20;
|
|
std::array<Vector2<Real>, 3> polygon
|
|
{
|
|
Q[0],
|
|
Q[0] + numerPos0 * dir10,
|
|
Q[0] + numerPos0 * dir20
|
|
};
|
|
return Result(DiskOverlapsPolygon(3, polygon.data(), radius));
|
|
}
|
|
else // z[1] < hhalf
|
|
{
|
|
// Case 4f of Figure 1 of the PDF.
|
|
//
|
|
// The edge <V2,V0> is parameterized by V2+t*(V0-V2).
|
|
// On the top of the slab,
|
|
// +h/2 = z2 + t * (z0 - z2)
|
|
// t = (+h/2 - z2) / (z0 - z2) = numerPos2 / denom02
|
|
//
|
|
// The edge <V2,V1> is parameterized by V2+t*(V1-V2).
|
|
// On the top of the slab,
|
|
// +h/2 = z2 + t * (z1 - z2)
|
|
// t = (+h/2 - z2) / (z1 - z2) = numerPos2 / denom12
|
|
Real numerPos2 = +hhalf - z[2];
|
|
Real denom02 = z[0] - z[2];
|
|
Real denom12 = z[1] - z[2];
|
|
Vector2<Real> dir02 = (Q[0] - Q[2]) / denom02;
|
|
Vector2<Real> dir12 = (Q[1] - Q[2]) / denom12;
|
|
std::array<Vector2<Real>, 4> polygon
|
|
{
|
|
Q[0],
|
|
Q[1],
|
|
Q[2] + numerPos2 * dir12,
|
|
Q[2] + numerPos2 * dir02
|
|
};
|
|
return Result(DiskOverlapsPolygon(4, polygon.data(), radius));
|
|
}
|
|
}
|
|
else // z[0] == hhalf
|
|
{
|
|
if (z[1] > hhalf)
|
|
{
|
|
// Case 1b of Figure 1 of the PDF.
|
|
return Result(DiskOverlapsPoint(Q[0], radius));
|
|
}
|
|
else
|
|
{
|
|
// Case 2b of Figure 1 of the PDF.
|
|
return Result(DiskOverlapsSegment(Q[0], Q[1], radius));
|
|
}
|
|
}
|
|
}
|
|
|
|
private:
|
|
// Support for the static test query.
|
|
bool DiskOverlapsPoint(Vector2<Real> const& Q, Real const& radius) const
|
|
{
|
|
return Dot(Q, Q) <= radius * radius;
|
|
}
|
|
|
|
bool DiskOverlapsSegment(Vector2<Real> const& Q0, Vector2<Real> const& Q1,
|
|
Real const& radius) const
|
|
{
|
|
Real sqrRadius = radius * radius;
|
|
Vector2<Real> direction = Q0 - Q1;
|
|
Real dot = Dot(Q0, direction);
|
|
if (dot <= static_cast<Real>(0))
|
|
{
|
|
return Dot(Q0, Q0) <= sqrRadius;
|
|
}
|
|
|
|
Real sqrLength = Dot(direction, direction);
|
|
if (dot >= sqrLength)
|
|
{
|
|
return Dot(Q1, Q1) <= sqrRadius;
|
|
}
|
|
|
|
dot = DotPerp(direction, Q0);
|
|
return dot * dot <= sqrLength * sqrRadius;
|
|
}
|
|
|
|
bool DiskOverlapsPolygon(size_t numVertices, Vector2<Real> const* Q,
|
|
Real const& radius) const
|
|
{
|
|
// Test whether the polygon contains (0,0).
|
|
Real const zero = static_cast<Real>(0);
|
|
size_t positive = 0, negative = 0;
|
|
size_t i0, i1;
|
|
for (i0 = numVertices - 1, i1 = 0; i1 < numVertices; i0 = i1++)
|
|
{
|
|
Real dot = DotPerp(Q[i0], Q[i0] - Q[i1]);
|
|
if (dot > zero)
|
|
{
|
|
++positive;
|
|
}
|
|
else if (dot < zero)
|
|
{
|
|
++negative;
|
|
}
|
|
}
|
|
if (positive == 0 || negative == 0)
|
|
{
|
|
// The polygon contains (0,0), so the disk and polygon
|
|
// overlap.
|
|
return true;
|
|
}
|
|
|
|
// Test whether any edge is overlapped by the polygon.
|
|
for (i0 = numVertices - 1, i1 = 0; i1 < numVertices; i0 = i1++)
|
|
{
|
|
if (DiskOverlapsSegment(Q[i0], Q[i1], radius))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
};
|
|
}
|
|
|