C++ Vec3d::length2方法代码示例

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;
}