示例#1
0
文件: sat.cpp 项目: lanixXx/scratch
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;
}
示例#2
0
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;
}
示例#3
0
// 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;
}
示例#4
0
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;
    }
}