8#include "RangeConvex.h"
10#include "SpatialGeneral.h"
12#define N(n) index_->nodes_[(n)]
13#define NC(n, m) index_->nodes_[(n)].childID_[(m)]
14#define NV(m) index_->nodes_[id].v_[(m)]
15#define V(m) index_->vertices_[(m)]
17#define SGN(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
42 float64
s1 = a1 * (*v1);
43 float64
s2 =
a2 * (*v2);
44 float64
s3 =
a3 * (*v3);
76 for (i = 0,
k = 0; i < 4; i++)
77 for (
j = i + 1;
j < 4;
j++,
k++)
79 d[
k] = (*v[i]) ^ (*v[
j]);
81 for (l = 0,
m = 0; l < 4; l++)
83 s[
k][
m++] = d[
k] * (*v[l]);
92 for (i = 0; i < 6; i++)
93 if (s[i][0] * s[i][1] > 0.0)
94 constraints_.push_back(
SpatialConstraint((s[i][0] > 0.0 ? d[i] : (-1 * d[i])), 0.0));
101 if (constraints_.size() == 2)
103 for (i = 0; i < 6; i++)
104 if (s[i][0] == 0.0 || s[i][1] == 0.0)
106 constraints_.push_back(
SpatialConstraint(((s[i][0] + s[i][1]) > 0.0 ? d[i] : (-1 * d[i])), 0.0));
119 constraints_.push_back(c);
123 for (
size_t i = constraints_.size() - 1; i > 0; i--)
125 if (constraints_[i].s_ < constraints_[i - 1].s_)
128 constraints_[i] = constraints_[i - 1];
129 constraints_[i - 1] =
tmp;
133 if (constraints_.size() == 1)
187void RangeConvex::simplify0()
192 std::vector<SpatialVector> corner;
193 if (constraints_.size() == 1)
195 boundingCircle_ = constraints_[0];
201 else if (constraints_.size() == 2)
204 if (constraints_[0].a_ == constraints_[1].a_)
206 constraints_.erase(constraints_.end() - 1);
207 boundingCircle_ = constraints_[0];
211 if (constraints_[0].a_ == (-1.0) * constraints_[1].a_)
213 constraints_.clear();
216 boundingCircle_ =
SpatialConstraint(constraints_[0].v() + constraints_[1].v(), 0);
221 for (i = 0; i < constraints_.size() - 1; i++)
224 for (
j = i + 1;
j < constraints_.size();
j++)
227 if (constraints_[i].a_ == constraints_[
j].a_)
230 if (constraints_[i].a_ == (-1.0) * constraints_[
j].a_)
232 constraints_.clear();
236 vi1 = constraints_[i].a_ ^ constraints_[
j].a_;
242 for (
k = 0;
k < constraints_.size();
k++)
244 if (
k == i ||
k ==
j)
246 if (
vi1ok &&
vi1 * constraints_[
k].a_ <= 0.0)
248 if (
vi2ok &&
vi2 * constraints_[
k].a_ <= 0.0)
255 corner.push_back(
vi1);
262 corner.push_back(
vi2);
280 corners_.push_back(corner[0]);
326 if (((
vi1 - corner[0]) ^ constraints_[i].a_) * corner[0] > 0)
328 corners_.push_back(
vi1);
334 corners_.push_back(
vi2);
358 corners_.push_back(corner[
k]);
366 corners_.push_back(corner[
k]);
374 constraints_.erase(constraints_.end() -
removeConstr[i] - 1);
380 boundingCircle_.d_ = 1.0;
381 if (constraints_.size() >= 3)
383 for (i = 0; i < corners_.size(); i++)
384 for (
j = i + 1;
j < corners_.size();
j++)
385 for (
k =
j + 1;
k < corners_.size();
k++)
387 SpatialVector v = (corners_[
j] - corners_[i]) ^ (corners_[
k] - corners_[
j]);
393 float64 d = v * corners_[i];
394 if (boundingCircle_.d_ > d)
437 clen = constraints_.size();
439 for (i = 0; i <
clen; i++)
441 for (
j = 0;
j < i;
j++)
446 if (constraints_[i].sign_ == zERO && constraints_[
j].sign_ == zERO)
450 if ((constraints_[i].sign_ == pOS || constraints_[i].sign_ == zERO) &&
451 (constraints_[
j].sign_ == pOS || constraints_[
j].sign_ == zERO))
453 if ((test = testConstraints(i,
j)) == 0)
457 constraints_.clear();
462 constraints_.erase(constraints_.end() - i - 1);
464 constraints_.erase(constraints_.end() -
j - 1);
472 if ((constraints_[i].sign_ == nEG) && (constraints_[
j].sign_ == nEG))
474 if ((test = testConstraints(i,
j)) <= 0)
478 constraints_.erase(constraints_.end() - 1 -
j);
480 constraints_.erase(constraints_.end() - 1 - i);
488 if ((test = testConstraints(i,
j)) == 0)
492 if (constraints_[i].sign_ == nEG)
493 constraints_.erase(constraints_.end() - 1 - i);
495 constraints_.erase(constraints_.end() - 1 -
j);
500 if ((constraints_[i].sign_ == nEG && test == 2) || (constraints_[
j].sign_ == nEG && test == 1))
503 constraints_.clear();
512 sign_ = constraints_[0].sign_;
513 for (i = 1; i < constraints_.size(); i++)
518 if (constraints_[i].sign_ == pOS)
522 if (constraints_[i].sign_ == nEG)
526 sign_ = constraints_[i].sign_;
533 if (constraints_.size() == 1)
534 boundingCircle_ = constraints_[0];
535 else if (sign_ == pOS)
536 boundingCircle_ = constraints_[0];
546int RangeConvex::testConstraints(
size_t i,
size_t j)
548 float64
phi = ((constraints_[i].sign_ == nEG ? (-1 * constraints_[i].a_) : constraints_[i].a_) *
549 (constraints_[
j].sign_ == nEG ? (-1 * constraints_[
j].a_) : constraints_[
j].a_));
551 float64 a1 = (constraints_[i].sign_ == pOS ? constraints_[i].s_ : gPi - constraints_[i].s_);
552 float64
a2 = (constraints_[
j].sign_ == pOS ? constraints_[
j].s_ : gPi - constraints_[
j].s_);
570 addlevel_ =
idx->maxlevel_ -
idx->buildlevel_;
574 if (constraints_.empty())
578 for (
uint32 i = 1; i <= 8; i++)
595 IDHIGHBIT = IDHIGHBIT << 63;
596 IDHIGHBIT2 = IDHIGHBIT2 << 62;
599 for (i = 0; i < IDSIZE; i += 2)
601 if ((
htmid << i) & IDHIGHBIT)
605 level = (IDSIZE - i) >> 1;
620 hr->mergeRange(
lo,
hi);
636 const struct SpatialIndex::QuadNode *
indexNode = &index_->nodes_[id];
694 testPartial(addlevel_, N(
id).id_, V(NV(0)), V(NV(1)), V(NV(2)), 0);
698 saveTrixel(N(
id).id_);
739 m[0] = testNode(
v0,
w2, w1);
740 m[1] = testNode(
v1, w0,
w2);
741 m[2] = testNode(
v2, w1, w0);
742 m[3] = testNode(w0, w1,
w2);
744 F = (
m[0] == fULL) + (
m[1] == fULL) + (
m[2] == fULL) + (
m[3] == fULL);
745 P = (
m[0] == pARTIAL) + (
m[1] == pARTIAL) + (
m[2] == pARTIAL) + (
m[3] == pARTIAL);
754 if ((level-- <= 0) || ((P == 4) || (F >= 2) || (P == 3 && F == 1) || (P > 1 &&
PPrev == 3)))
762 for (
int i = 0; i < 4; i++)
772 testPartial(level,
ids[0],
v0,
w2, w1, P);
774 testPartial(level,
ids[1],
v1, w0,
w2, P);
776 testPartial(level,
ids[2],
v2, w1, w0, P);
778 testPartial(level,
ids[3], w0, w1,
w2, P);
785SpatialMarkup RangeConvex::testNode(
uint64 id)
791 const struct SpatialIndex::QuadNode *
indexNode = &index_->nodes_[id];
794 v0 = &index_->vertices_[
m];
797 v1 = &index_->vertices_[
m];
800 v2 = &index_->vertices_[
m];
803 int vsum = testVertex(
v0) + testVertex(
v1) + testVertex(
v2);
805 SpatialMarkup mark = testTriangle(*
v0, *
v1, *
v2,
vsum);
809 if (mark == dONTKNOW)
818 int vsum = testVertex(
v0) + testVertex(
v1) + testVertex(
v2);
820 SpatialMarkup mark = testTriangle(
v0,
v1,
v2,
vsum);
824 if (mark == dONTKNOW)
857 if (sign_ == pOS || sign_ == zERO)
902 if (!testBoundingCircle(
v0,
v1,
v2))
905 if (sign_ == pOS || sign_ == mIXED || (sign_ == zERO && constraints_.size() <= 2))
908 if (testEdgeConstraint(
v0,
v1,
v2, 0))
919 else if (constraints_[
cIndex].contains(
v0))
926 if (sign_ == pOS || sign_ == zERO)
934 if (sign_ == pOS || sign_ == zERO)
937 if (testConstraintInside(
v0,
v1,
v2, 0))
946 else if (sign_ == zERO)
948 if (corners_.size() > 0 && testEdge0(
v0,
v1,
v2))
961 for (
size_t i = 0; i < constraints_.size(); i++)
962 if ((constraints_[i].a_ * v) < constraints_[i].d_)
969 for (
size_t i = 0; i < constraints_.size(); i++)
970 if ((constraints_[i].a_ * *v) < constraints_[i].d_)
985 for (
size_t i = 0; i < constraints_.size(); i++)
987 if (constraints_[i].sign_ == nEG)
994 if (((
v0 ^
v1) * constraints_[i].a_) > 0.0L)
996 if (((
v1 ^
v2) * constraints_[i].a_) > 0.0L)
998 if (((
v2 ^
v0) * constraints_[i].a_) > 0.0L)
1036 edge[0].e =
v0 ^
v1;
1039 edge[1].e =
v1 ^
v2;
1042 edge[2].e =
v2 ^
v0;
1049 for (
size_t i = 0; i < corners_.size(); i++)
1052 if (i < corners_.size() - 1)
1061 a1 = (edge[
iedge].e) ^ (corners_[i] ^ corners_[
j]);
1067 for (
size_t k = 0;
k < 2;
k++)
1069 l1 =
acos(corners_[i] * a1);
1075 if (
l1 - edge[
iedge].l <= gEpsilon &&
l2 - edge[
iedge].l <= gEpsilon)
1082 return testVectorInside(
v0,
v1,
v2, corners_[0]);
1092 for (
size_t i = 0; i < constraints_.size(); i++)
1094 if (constraints_[i].sign_ == nEG)
1096 if (eSolve(
v0,
v1, i))
1098 if (eSolve(
v1,
v2, i))
1100 if (eSolve(
v2,
v0, i))
1116 float64
u2 = (1 -
mu) / (1 +
mu);
1122 float64 D = b * b - 4 * a * c;
1129 float64 q = -0.5L * (b + (SGN(b) *
sqrt(D)));
1134 if (a > gEpsilon || a < -gEpsilon)
1139 if (q > gEpsilon || q < -gEpsilon)
1170 float64 d =
acos(c *
v0);
1178 if (((
tst = c * boundingCircle_.a_) < -1.0L + gEpsilon ? gPi :
acos(
tst)) > (d + boundingCircle_.s_))
1188 for (i = 0; i < constraints_.size(); i++)
1190 if (((c * constraints_[i].a_) < -1.0L + gEpsilon ? gPi :
acos(c * constraints_[i].a_)) >
1191 (d + constraints_[i].s_))
1219 while (i < constraints_.size() && constraints_[i].sign_ == pOS)
1221 if (!testEdgeConstraint(
v0,
v1,
v2, i))
1234 return testVectorInside(
v0,
v1,
v2, constraints_[i].a_);
1247 if ((((
v0 ^
v1) * v) < 0) || (((
v1 ^
v2) * v) < 0) || (((
v2 ^
v0) * v) < 0))
void simplify()
Simplify the convex, remove redundancies.
SpatialMarkup testTrixel(uint64 nodeIndex)
void intersect(const SpatialIndex *index, HtmRange *hr)
Intersect with index.
void add(SpatialConstraint &)
Add a constraint.
RangeConvex()
Default Constructor.
The Constraint is really a cone on the sky-sphere.
SpatialIndex is a quad tree of spherical triangles.
SpatialVector is a 3D vector usually living on the surface of the sphere.
void normalize()
Normalize vector length to 1.
QStringView level(QStringView ifopt)