12 #include <type_traits> 38 template <
typename InputType,
typename ComputeType>
52 bool useRotatingCalipers =
62 bool useRotatingCalipers =
68 inline std::vector<int>
const&
GetHull()
const;
70 inline InputType
GetArea()
const;
108 Box const& box,
std::array<std::pair<ComputeType, int>, 4>& A,
115 std::array<std::pair<ComputeType, int>, 4>
const& A,
int numA)
const;
118 int numA, std::array<int, 4>
const& sort,
120 std::vector<bool>& visited,
Box& box);
150 template <
typename InputType,
typename ComputeType>
159 mHalf((InputType)0.5)
164 template <
typename InputType,
typename ComputeType>
185 minBox.
extent[0] = (InputType)0;
186 minBox.
extent[1] = (InputType)0;
199 InputType tmin = (InputType)0, tmax = (InputType)0;
200 int imin = 0, imax = 0;
218 ((InputType)0.5)*(tmin + tmax) * line.
direction;
219 minBox.
extent[0] = ((InputType)0.5)*(tmax - tmin);
220 minBox.
extent[1] = (InputType)0;
231 std::vector<Vector2<ComputeType>> computePoints(
mHull.size());
232 for (
size_t i = 0; i <
mHull.size(); ++i)
234 computePoints[i] = queryPoints[
mHull[i]];
240 if (useRotatingCalipers)
253 template <
typename InputType,
typename ComputeType>
256 int const*
indices,
bool useRotatingCalipers)
262 if (numPoints < 3 || !points || (indices && numIndices < 3))
273 mHull.resize(numIndices);
274 std::copy(indices, indices + numIndices,
mHull.begin());
278 numIndices = numPoints;
279 mHull.resize(numIndices);
280 for (
int i = 0; i < numIndices; ++i)
286 std::vector<Vector2<ComputeType>> computePoints(numIndices);
287 for (
int i = 0; i < numIndices; ++i)
290 computePoints[i][0] = (ComputeType)points[h][0];
291 computePoints[i][1] = (ComputeType)points[h][1];
297 if (useRotatingCalipers)
310 template <
typename InputType,
typename ComputeType>
inline 316 template <
typename InputType,
typename ComputeType>
inline 323 template <
typename InputType,
typename ComputeType>
inline 324 std::vector<int>
const&
330 template <
typename InputType,
typename ComputeType>
inline 331 std::array<int, 4>
const&
337 template <
typename InputType,
typename ComputeType>
inline 343 template <
typename InputType,
typename ComputeType>
347 std::vector<Vector2<ComputeType>> tmpVertices = vertices;
349 int const numVertices =
static_cast<int>(vertices.size());
350 int numNoncollinear = 0;
352 for (
int i0 = 0, i1 = 1; i0 < numVertices; ++i0)
356 ComputeType dp =
DotPerp(ePrev, eNext);
359 vertices[numNoncollinear++] = tmpVertices[i0];
363 if (++i1 == numVertices)
369 vertices.resize(numNoncollinear);
372 template <
typename InputType,
typename ComputeType>
379 int const numIndices =
static_cast<int>(vertices.size());
380 for (
int i0 = numIndices - 1, i1 = 0; i1 < numIndices; i0 = i1++)
391 template <
typename InputType,
typename ComputeType>
403 std::vector<bool> visited(vertices.size());
404 std::fill(visited.begin(), visited.end(),
false);
414 visited[minBox.index[0]] =
true;
418 for (
size_t i = 0; i < vertices.size(); ++i)
420 std::array<std::pair<ComputeType, int>, 4> A;
429 std::array<int, 4> sort =
SortAngles(A, numA);
440 if (box.
area < minBox.area)
449 template <
typename InputType,
typename ComputeType>
455 box.
U[0] = vertices[i1] - vertices[i0];
456 box.
U[1] = -
Perp(box.
U[0]);
457 box.
index = { i1, i1, i1, i1 };
462 for (
int j = 0; j < 4; ++j)
468 for (
auto const& vertex : vertices)
479 if (v[0] > support[1][0] ||
480 (v[0] == support[1][0] && v[1] > support[1][1]))
487 if (v[1] > support[2][1] ||
488 (v[1] == support[2][1] && v[0] < support[2][0]))
495 if (v[0] < support[3][0] ||
496 (v[0] == support[3][0] && v[1] < support[3][1]))
509 ComputeType scaledWidth = support[1][0] - support[3][0];
510 ComputeType scaledHeight = support[2][1];
515 template <
typename InputType,
typename ComputeType>
518 std::array<std::pair<ComputeType, int>, 4>& A,
int& numA)
const 520 int const numVertices =
static_cast<int>(vertices.size());
522 for (
int k0 = 3, k1 = 0; k1 < 4; k0 = k1++)
528 ((k0 & 2) ? -box.
U[k0 & 1] : box.
U[k0 & 1]);
529 int j0 = box.
index[k0], j1 = j0 + 1;
530 if (j1 == numVertices)
535 ComputeType dp =
DotPerp(D, E);
536 ComputeType esqrlen =
Dot(E, E);
537 ComputeType sinThetaSqr = (dp * dp) / esqrlen;
538 A[numA++] = std::make_pair(sinThetaSqr, k0);
544 template <
typename InputType,
typename ComputeType>
546 std::array<std::pair<ComputeType, int>, 4>
const& A,
int numA)
const 548 std::array<int, 4> sort = { 0, 1, 2, 3 };
555 std::swap(sort[0], sort[1]);
562 std::swap(sort[0], sort[1]);
564 if (A[sort[0]].first > A[sort[2]].first)
566 std::swap(sort[0], sort[2]);
568 if (A[sort[1]].first > A[sort[2]].first)
570 std::swap(sort[1], sort[2]);
577 std::swap(sort[0], sort[1]);
579 if (A[sort[2]].first > A[sort[3]].first)
581 std::swap(sort[2], sort[3]);
583 if (A[sort[0]].first > A[sort[2]].first)
585 std::swap(sort[0], sort[2]);
587 if (A[sort[1]].first > A[sort[3]].first)
589 std::swap(sort[1], sort[3]);
591 if (A[sort[1]].first > A[sort[2]].first)
593 std::swap(sort[1], sort[2]);
600 template <
typename InputType,
typename ComputeType>
602 std::array<std::pair<ComputeType, int>, 4>
const& A,
int numA,
603 std::array<int, 4>
const& sort,
605 std::vector<bool>& visited,
Box& box)
609 int const numVertices =
static_cast<int>(vertices.size());
610 auto const& amin = A[sort[0]];
611 for (
int k = 0; k < numA; ++k)
613 auto const&
a = A[sort[k]];
614 if (
a.first == amin.first)
616 if (++box.
index[
a.second] == numVertices)
636 std::array<int, 4> nextIndex;
637 for (
int k = 0; k < 4; ++k)
639 nextIndex[k] = box.
index[(amin.second + k) % 4];
641 box.
index = nextIndex;
644 int j1 = box.
index[0], j0 = j1 - 1;
647 j0 = numVertices - 1;
649 box.
U[0] = vertices[j1] - vertices[j0];
650 box.
U[1] = -
Perp(box.
U[0]);
656 vertices[box.
index[1]] - vertices[box.
index[3]],
657 vertices[box.
index[2]] - vertices[box.
index[0]]
663 template <
typename InputType,
typename ComputeType>
671 computePoints[minBox.
index[1]] + computePoints[minBox.
index[3]],
672 computePoints[minBox.
index[2]] + computePoints[minBox.
index[0]]
677 computePoints[minBox.
index[1]] - computePoints[minBox.
index[3]],
678 computePoints[minBox.
index[2]] - computePoints[minBox.
index[0]]
682 Dot(minBox.
U[0], sum[0]) * minBox.
U[0] +
688 for (
int i = 0; i < 2; ++i)
690 sqrExtent[i] =
mHalf *
Dot(minBox.
U[i], difference[i]);
691 sqrExtent[i] *= sqrExtent[i];
695 for (
int i = 0; i < 2; ++i)
697 itMinBox.
center[i] = (InputType)center[i];
698 itMinBox.
extent[i] = sqrt((InputType)sqrExtent[i]);
706 ComputeType invCMax =
mOne / cmax;
707 for (
int j = 0; j < 2; ++j)
709 itMinBox.
axis[i][j] = (InputType)(axis[j] * invCMax);
std::vector< int > const & GetHull() const
Box SmallestBox(int i0, int i1, std::vector< Vector2< ComputeType >> const &vertices)
std::array< Vector< N, Real >, N > axis
gte::BSNumber< UIntegerType > abs(gte::BSNumber< UIntegerType > const &number)
void ConvertTo(Box const &minBox, std::vector< Vector2< ComputeType >> const &computePoints, OrientedBox2< InputType > &itMinBox)
Box ComputeBoxForEdgeOrderN(std::vector< Vector2< ComputeType >> const &vertices)
std::array< int, 4 > SortAngles(std::array< std::pair< ComputeType, int >, 4 > const &A, int numA) const
std::array< int, 4 > index
std::array< int, 4 > const & GetSupportIndices() const
Vector2< InputType > const * mPoints
PrimalQuery2< ComputeType > const & GetQuery() const
GLsizei const GLfloat * value
InputType GetArea() const
bool ComputeAngles(std::vector< Vector2< ComputeType >> const &vertices, Box const &box, std::array< std::pair< ComputeType, int >, 4 > &A, int &numA) const
Line2< InputType > const & GetLine() const
Vector< N, Real > direction
GLboolean GLboolean GLboolean GLboolean a
Vector2< InputType > const * GetPoints() const
std::vector< int > const & GetHull() const
GLfixed GLfixed GLint GLint GLfixed points
Vector2< ComputeType > U[2]
Vector2< Real > const * GetVertices() const
GLsizei GLenum const void * indices
OrientedBox2< InputType > operator()(int numPoints, Vector2< InputType > const *points, bool useRotatingCalipers=!std::is_floating_point< ComputeType >::value)
Real DotPerp(Vector2< Real > const &v0, Vector2< Real > const &v1)
void RemoveCollinearPoints(std::vector< Vector2< ComputeType >> &vertices)
DualQuaternion< Real > Dot(DualQuaternion< Real > const &d0, DualQuaternion< Real > const &d1)
Real Normalize(GVector< Real > &v, bool robust=false)
bool UpdateSupport(std::array< std::pair< ComputeType, int >, 4 > const &A, int numA, std::array< int, 4 > const &sort, std::vector< Vector2< ComputeType >> const &vertices, std::vector< bool > &visited, Box &box)
Vector2< Real > Perp(Vector2< Real > const &v)
GLfloat GLfloat GLfloat GLfloat h
Box ComputeBoxForEdgeOrderNSqr(std::vector< Vector2< ComputeType >> const &vertices)
std::array< int, 4 > mSupportIndices
static Vector Unit(int d)