bool CalcPolysIntersectSAT(std::vector<osg::Vec3d> const &polyA_list_vx,
std::vector<osg::Vec3d> const &polyA_list_face_norms,
std::vector<osg::Vec3d> const &polyA_list_edges,
std::vector<osg::Vec3d> const &polyB_list_vx,
std::vector<osg::Vec3d> const &polyB_list_face_norms,
std::vector<osg::Vec3d> const &polyB_list_edges)
{
// Test the faces of polyA
for(auto const &polyA_face_norm : polyA_list_face_norms)
{
if(!ProjectionIntervalsOverlap(polyA_face_norm,
polyA_list_vx,
polyB_list_vx)) {
return false;
}
}
// Test the faces of polyB
for(auto const &polyB_face_norm : polyB_list_face_norms)
{
if(!ProjectionIntervalsOverlap(polyB_face_norm,
polyA_list_vx,
polyB_list_vx)) {
return false;
}
}
// Test the cross product of the edges of polyA and polyB
for(auto const &polyA_edge : polyA_list_edges) {
for(auto const &polyB_edge : polyB_list_edges)
{
osg::Vec3d const axis = polyA_edge^polyB_edge;
// Edge may be invalid -- need to handle
// this case better
if(axis.length2() < 1E-5) {
continue;
}
if(!ProjectionIntervalsOverlap(axis,
polyA_list_vx,
polyB_list_vx)) {
return false;
}
}
}
// for(auto const &interval : list_polyA_intervals) {
// std::cout << "###: min: " << interval.first
// << "max: " << interval.second << std::endl;
// }
// Intersection
return true;
}
bool
GeoMath::isPointVisible(const osg::Vec3d& eye,
const osg::Vec3d& target,
double R)
{
double r2 = R*R;
// same quadrant:
if ( eye * target >= 0.0 )
{
double d2 = eye.length2();
double horiz2 = d2 - r2;
double dist2 = (target-eye).length2();
if ( dist2 < horiz2 )
{
return true;
}
}
// different quadrants:
else
{
// there's a horizon between them; now see if the thing is visible.
// find the triangle formed by the viewpoint, the target point, and
// the center of the earth.
double a = (target-eye).length();
double b = target.length();
double c = eye.length();
// Heron's formula for triangle area:
double s = 0.5*(a+b+c);
double area = 0.25*sqrt( s*(s-a)*(s-b)*(s-c) );
// Get the triangle's height:
double h = (2*area)/a;
if ( h >= R )
{
return true;
}
}
return false;
}
// From WildMagic, (c) Geometric Tools LLC
// See Eberly, Distance between Line and Circle
void CriticalPointsLineCircle(Circle const &c,
osg::Vec3d const &line_p, // line point
osg::Vec3d const &line_d, // line dirn
osg::Vec3d &min,
osg::Vec3d &max)
{
osg::Vec3d diff = line_p - c.center;
double diffSqrLen = diff.length2();
double MdM = line_d.length2();
double DdM = diff*line_d;
double NdM = c.normal*line_d;
double DdN = diff*c.normal;
double a0 = DdM;
double a1 = MdM;
double b0 = DdM - NdM*DdN;
double b1 = MdM - NdM*NdM;
double c0 = diffSqrLen - DdN*DdN;
double c1 = b0;
double c2 = b1;
double rsqr = c.radius*c.radius;
double a0sqr = a0*a0;
double a1sqr = a1*a1;
double twoA0A1 = 2.0*a0*a1;
double b0sqr = b0*b0;
double b1Sqr = b1*b1;
double twoB0B1 = 2.0*b0*b1;
double twoC1 = 2.0*c1;
// The minimum point B+t*M occurs when t is a root of the quartic
// equation whose coefficients are defined below.
// I think index specifies polynomial degree
// ie. poly[3] = coefficient for x^3
Polynomial1<Real> poly(4);
poly[0] = a0sqr*c0 - b0sqr*rsqr;
poly[1] = twoA0A1*c0 + a0sqr*twoC1 - twoB0B1*rsqr;
poly[2] = a1sqr*c0 + twoA0A1*twoC1 + a0sqr*c2 - b1Sqr*rsqr;
poly[3] = a1sqr*twoC1 + twoA0A1*c2;
poly[4] = a1sqr*c2;
PolynomialRoots<Real> polyroots(Math<Real>::ZERO_TOLERANCE);
polyroots.FindB(poly, 6);
int count = polyroots.GetCount();
const Real* roots = polyroots.GetRoots();
Real minSqrDist = Math<Real>::MAX_REAL;
for (int i = 0; i < count; ++i)
{
// Compute distance from P(t) to circle.
Vector3<Real> P = mLine->Origin + roots[i]*mLine->Direction;
DistPoint3Circle3<Real> query(P, *mCircle);
Real sqrDist = query.GetSquared();
if (sqrDist < minSqrDist)
{
minSqrDist = sqrDist;
mClosestPoint0 = query.GetClosestPoint0();
mClosestPoint1 = query.GetClosestPoint1();
}
}
return minSqrDist;
}
void CalcTileOBB(VxTile * tile)
{
// 1. Determine the orthonormal basis
// At the poles the distance in 'x' (lon) might
// be zero or too small so choose the vec that
// has a greater magnitude:
osg::Vec3d const x_top = (*(tile->p_ecef_LT))-(*(tile->p_ecef_RT));
osg::Vec3d const x_btm = (*(tile->p_ecef_LB))-(*(tile->p_ecef_RB));
tile->obb.ori[0] = (x_top.length2() > x_btm.length2()) ? x_top : x_btm;
tile->obb.ori[2] = tile->ecef_MM;
tile->obb.ori[1] = tile->obb.ori[2]^tile->obb.ori[0];
tile->obb.ori[0].normalize();
tile->obb.ori[1].normalize();
tile->obb.ori[2].normalize();
// Get the bbox min and max
// std::vector<osg::Vec3d const *> tile_vx(5);
// tile_vx[0] = tile->p_ecef_LT;
// tile_vx[1] = tile->p_ecef_LB;
// tile_vx[2] = tile->p_ecef_RB;
// tile_vx[3] = tile->p_ecef_RT;
// tile_vx[4] = &(tile->ecef_MM);
std::vector<osg::Vec3d const *> tile_vx {
tile->p_ecef_LT,
tile->p_ecef_LB,
tile->p_ecef_RB,
tile->p_ecef_RT,
&(tile->ecef_MM)
};
// 2. Get the min and max corners
osg::Vec3d min_rst(K_MAX_POS_DBL,K_MAX_POS_DBL,K_MAX_POS_DBL);
osg::Vec3d max_rst(K_MIN_NEG_DBL,K_MIN_NEG_DBL,K_MIN_NEG_DBL);
// Project all the vertices of the model onto the
// orthonormal vectors and get min/max along them
for(auto vx_ptr : tile_vx) {
osg::Vec3d const &vx = *vx_ptr;
double const dist_r = tile->obb.ori[0]*vx;
double const dist_s = tile->obb.ori[1]*vx;
double const dist_t = tile->obb.ori[2]*vx;
min_rst.x() = std::min(min_rst.x(),dist_r);
min_rst.y() = std::min(min_rst.y(),dist_s);
min_rst.z() = std::min(min_rst.z(),dist_t);
max_rst.x() = std::max(max_rst.x(),dist_r);
max_rst.y() = std::max(max_rst.y(),dist_s);
max_rst.z() = std::max(max_rst.z(),dist_t);
}
// 3. Calc center and extents
tile->obb.center =
(tile->obb.ori[0] * (min_rst.x()+max_rst.x())*0.5) +
(tile->obb.ori[1] * (min_rst.y()+max_rst.y())*0.5) +
(tile->obb.ori[2] * (min_rst.z()+max_rst.z())*0.5);
tile->obb.ext = (max_rst-min_rst)*0.5;
// 4. Save the unique face planes
for(int i=0; i < 3; i++) {
Plane &face = tile->obb.faces[i];
face.n = tile->obb.ori[i];
face.p = (face.n * tile->obb.ext[i]) + tile->obb.center;
face.d = face.n * face.p;
}
}