C++ Segment3::Origin方法代码示例

//----------------------------------------------------------------------------
float VertexCollapse::GetWeight (int iM, int iZ, int iP, Vector3* akVertex)
{
    Segment3 kSegment;
    kSegment.Origin() = akVertex[iM];
    kSegment.Direction() = akVertex[iP] - akVertex[iM];
    float fSqrDist = SqrDistance(akVertex[iZ],kSegment);
    float fSqrLen = kSegment.Direction().SquaredLength();

    return ( fSqrLen > 0.0f ? fSqrDist/fSqrLen : FLT_MAX );
}

//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Segment3& rkSegment,
    const Triangle3& rkTriangle, Vector3& rkPoint)
{
    Real fSegP;
    if ( SqrDistance(rkSegment,rkTriangle,&fSegP) <= gs_fEpsilon )
    {
        rkPoint = rkSegment.Origin() + fSegP*rkSegment.Direction();
        return true;
    }
    return false;
}

//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Segment3& rkSegment, const Box3& rkBox)
{
    Real fAWdU[3], fADdU[3], fAWxDdU[3], fRhs;
    Vector3 kSDir = 0.5f*rkSegment.Direction();
    Vector3 kSCen = rkSegment.Origin() + kSDir;

    Vector3 kDiff = kSCen - rkBox.Center();

    fAWdU[0] = Math::FAbs(kSDir.Dot(rkBox.Axis(0)));
    fADdU[0] = Math::FAbs(kDiff.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(0) + fAWdU[0];
    if ( fADdU[0] > fRhs )
        return false;

    fAWdU[1] = Math::FAbs(kSDir.Dot(rkBox.Axis(1)));
    fADdU[1] = Math::FAbs(kDiff.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(1) + fAWdU[1];
    if ( fADdU[1] > fRhs )
        return false;

    fAWdU[2] = Math::FAbs(kSDir.Dot(rkBox.Axis(2)));
    fADdU[2] = Math::FAbs(kDiff.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(2) + fAWdU[2];
    if ( fADdU[2] > fRhs )
        return false;

    Vector3 kWxD = kSDir.Cross(kDiff);

    fAWxDdU[0] = Math::FAbs(kWxD.Dot(rkBox.Axis(0)));
    fRhs = rkBox.Extent(1)*fAWdU[2] + rkBox.Extent(2)*fAWdU[1];
    if ( fAWxDdU[0] > fRhs )
        return false;

    fAWxDdU[1] = Math::FAbs(kWxD.Dot(rkBox.Axis(1)));
    fRhs = rkBox.Extent(0)*fAWdU[2] + rkBox.Extent(2)*fAWdU[0];
    if ( fAWxDdU[1] > fRhs )
        return false;

    fAWxDdU[2] = Math::FAbs(kWxD.Dot(rkBox.Axis(2)));
    fRhs = rkBox.Extent(0)*fAWdU[1] + rkBox.Extent(1)*fAWdU[0];
    if ( fAWxDdU[2] > fRhs )
        return false;

    return true;
}

//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Triangle3& rkTri0, const Triangle3& rkTri1,
    Real* pfTri0P0, Real* pfTri0P1, Real* pfTri1P0, Real* pfTri1P1)
{
    Real fS, fT, fU, fV, fS0, fT0, fU0, fV0, fSqrDist, fSqrDist0;
    Segment3 kSeg;

    // compare edges of tri0 against all of tri1
    kSeg.Origin() = rkTri0.Origin();
    kSeg.Direction() = rkTri0.Edge0();
    fSqrDist = SqrDistance(kSeg,rkTri1,&fS,&fU,&fV);
    fT = 0.0f;

    kSeg.Direction() = rkTri0.Edge1();
    fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
    fS0 = 0.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = kSeg.Origin() + rkTri0.Edge0();
    kSeg.Direction() = kSeg.Direction() - rkTri0.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkTri1,&fT0,&fU0,&fV0);
    fS0 = 1.0f-fT0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    // compare edges of tri1 against all of tri0
    kSeg.Origin() = rkTri1.Origin();
    kSeg.Direction() = rkTri1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkTri0,&fU0,&fS0,&fT0);
    fV0 = 0.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Direction() = rkTri1.Edge1();
    fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
    fU0 = 0.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = kSeg.Origin() + rkTri1.Edge0();
    kSeg.Direction() = kSeg.Direction() - rkTri1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkTri0,&fV0,&fS0,&fT0);
    fU0 = 1.0f-fV0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    if ( pfTri0P0 )
        *pfTri0P0 = fS;

    if ( pfTri0P1 )
        *pfTri0P1 = fT;

    if ( pfTri1P0 )
        *pfTri1P0 = fU;

    if ( pfTri1P1 )
        *pfTri1P1 = fV;

    return Math::FAbs(fSqrDist);
}

Real Wml::SqrDistance (const Rectangle3<Real>& rkRct0,
    const Rectangle3<Real>& rkRct1, Real* pfRct0P0, Real* pfRct0P1,
    Real* pfRct1P0, Real* pfRct1P1)
{
    Real fS, fT, fS0, fT0;  // rct0 parameters
    Real fU, fV, fU0, fV0;  // rct1 parameters
    Real fSqrDist, fSqrDist0;
    Segment3<Real> kSeg;

    // compare edges of rct0 against all of rct1
    kSeg.Origin() = rkRct0.Origin();
    kSeg.Direction() = rkRct0.Edge0();
    fSqrDist = SqrDistance(kSeg,rkRct1,&fS,&fU,&fV);
    fT = (Real)0.0;

    kSeg.Direction() = rkRct0.Edge1();
    fSqrDist0 = SqrDistance(kSeg,rkRct1,&fT0,&fU0,&fV0);
    fS0 = (Real)0.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkRct0.Origin() + rkRct0.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkRct1,&fT0,&fU0,&fV0);
    fS0 = (Real)1.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkRct0.Origin() + rkRct0.Edge1();
    kSeg.Direction() = rkRct0.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkRct1,&fS0,&fU0,&fV0);
    fT0 = (Real)1.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    // compare edges of pgm1 against all of pgm0
    kSeg.Origin() = rkRct1.Origin();
    kSeg.Direction() = rkRct1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkRct0,&fU0,&fS0,&fT0);
    fV0 = (Real)0.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Direction() = rkRct1.Edge1();
    fSqrDist0 = SqrDistance(kSeg,rkRct0,&fV0,&fS0,&fT0);
    fU0 = (Real)0.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkRct1.Origin() + rkRct1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkRct0,&fV0,&fS0,&fT0);
    fU0 = (Real)1.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkRct1.Origin() + rkRct1.Edge1();
    kSeg.Direction() = rkRct1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkRct0,&fU0,&fS0,&fT0);
    fV0 = (Real)1.0;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
//.........这里部分代码省略.........

bool Mgc::FindIntersection (const Segment3& rkSegment, const Sphere& rkSphere,

    int& riQuantity, Vector3 akPoint[2])

{

    // set up quadratic Q(t) = a*t^2 + 2*b*t + c

    Vector3 kDiff = rkSegment.Origin() - rkSphere.Center();

    Real fA = rkSegment.Direction().SquaredLength();

    Real fB = kDiff.Dot(rkSegment.Direction());

    Real fC = kDiff.SquaredLength() -

        rkSphere.Radius()*rkSphere.Radius();



    // no intersection if Q(t) has no real roots

    Real afT[2];

    Real fDiscr = fB*fB - fA*fC;

    if ( fDiscr < 0.0f )

    {

        riQuantity = 0;

        return false;

    }

    else if ( fDiscr > 0.0f )

    {

        Real fRoot = Math::Sqrt(fDiscr);

        Real fInvA = 1.0f/fA;

        afT[0] = (-fB - fRoot)*fInvA;

        afT[1] = (-fB + fRoot)*fInvA;



        // assert: t0 < t1 since A > 0



        if ( afT[0] > 1.0f || afT[1] < 0.0f )

        {

            riQuantity = 0;

            return false;

        }

        else if ( afT[0] >= 0.0f )

        {

            if ( afT[1] > 1.0f )

            {

                riQuantity = 1;

                akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();

                return true;

            }

            else

            {

                riQuantity = 2;

                akPoint[0] = rkSegment.Origin()+afT[0]*rkSegment.Direction();

                akPoint[1] = rkSegment.Origin()+afT[1]*rkSegment.Direction();

                return true;

            }

        }

        else  // afT[1] >= 0

        {

//.........这里部分代码省略.........

//----------------------------------------------------------------------------
Real Mgc::SqrDistance (const Parallelogram3& rkPgm0,
    const Parallelogram3& rkPgm1, Real* pfPgm0P0, Real* pfPgm0P1,
    Real* pfPgm1P0, Real* pfPgm1P1)
{
    Real fS, fT, fS0, fT0;  // pgm0 parameters
    Real fU, fV, fU0, fV0;  // pgm1 parameters
    Real fSqrDist, fSqrDist0;
    Segment3 kSeg;

    // compare edges of pgm0 against all of pgm1
    kSeg.Origin() = rkPgm0.Origin();
    kSeg.Direction() = rkPgm0.Edge0();
    fSqrDist = SqrDistance(kSeg,rkPgm1,&fS,&fU,&fV);
    fT = 0.0f;

    kSeg.Direction() = rkPgm0.Edge1();
    fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fT0,&fU0,&fV0);
    fS0 = 0.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkPgm0.Origin() + rkPgm0.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fT0,&fU0,&fV0);
    fS0 = 1.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkPgm0.Origin() + rkPgm0.Edge1();
    kSeg.Direction() = rkPgm0.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkPgm1,&fS0,&fU0,&fV0);
    fT0 = 1.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    // compare edges of pgm1 against all of pgm0
    kSeg.Origin() = rkPgm1.Origin();
    kSeg.Direction() = rkPgm1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fU0,&fS0,&fT0);
    fV0 = 0.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Direction() = rkPgm1.Edge1();
    fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fV0,&fS0,&fT0);
    fU0 = 0.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkPgm1.Origin() + rkPgm1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fV0,&fS0,&fT0);
    fU0 = 1.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
        fU = fU0;
        fV = fV0;
    }

    kSeg.Origin() = rkPgm1.Origin() + rkPgm1.Edge1();
    kSeg.Direction() = rkPgm1.Edge0();
    fSqrDist0 = SqrDistance(kSeg,rkPgm0,&fU0,&fS0,&fT0);
    fV0 = 1.0f;
    if ( fSqrDist0 < fSqrDist )
    {
        fSqrDist = fSqrDist0;
        fS = fS0;
        fT = fT0;
//.........这里部分代码省略.........

static bool FindTriSphrCoplanarIntersection (int iVertex, 
    const Vector3<Real> akV[3], const Vector3<Real>& /* rkNormal */,
    const Vector3<Real>& rkSideNorm, const Vector3<Real>& rkSide,
    const Sphere3<Real>& rkSphere, const Vector3<Real>& rkTriVelocity,
    const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
    int& riQuantity, Vector3<Real> akP[6])
{
    // TO DO.  The parameter rkNormal is not used here.  Is this an error?
    // Or does the caller make some adjustments to the other inputs to
    // account for the normal?

    // iVertex is the "hinge" vertex that the two potential edges that can
    // be intersected by the sphere connect to, and it indexes into akV.

    // rkSideNorm is the normal of the plane formed by (iVertex,iVertex+1)
    // and the tri norm, passed so as not to recalculate

    // check for intersections at time 0
    Vector3<Real> kDist = akV[iVertex] - rkSphere.Center();
    if ( kDist.SquaredLength() < rkSphere.Radius()*rkSphere.Radius() )
    {
        // already intersecting that vertex
        rfTFirst = (Real)0.0;
        return false;
    }

    // Tri stationary, sphere moving
    Vector3<Real> kVel = rkSphVelocity - rkTriVelocity;

    // check for easy out
    if ( kVel.Dot(kDist) <= (Real)0.0 )
    {
        // moving away
        return false;
    }

    // find intersection of velocity ray and side normal

    // project ray and plane onto the plane normal
    Real fPlane = rkSideNorm.Dot(akV[iVertex]);
    Real fCenter = rkSideNorm.Dot(rkSphere.Center());
    Real fVel = rkSideNorm.Dot(kVel);
    Real fFactor = (fPlane - fCenter)/fVel;

    Vector3<Real> kPoint = rkSphere.Center() + fFactor*kVel;

    // now, find which side of the hinge vertex this lies by projecting
    // both the vertex and this new point onto the triangle edge (the same
    // edge whose "normal" was used to find this point)

    Real fVertex = rkSide.Dot(akV[iVertex]);
    Real fPoint = rkSide.Dot(kPoint);

    Segment3<Real> kSeg;

    if ( fPoint >= fVertex )
    {
        // intersection with edge (iVertex,iVertex+1)
        kSeg.Origin() = akV[iVertex];
        kSeg.Direction() = akV[(iVertex+1)%3] - akV[iVertex];
    }
    else
    {
        // intersection with edge (iVertex-1,iVertex)
        if ( iVertex != 0 )
            kSeg.Origin() = akV[iVertex-1];
        else
            kSeg.Origin() = akV[2];
        kSeg.Direction() = akV[iVertex] - kSeg.Origin();
    }

    // This could be either an sphere-edge or a sphere-vertex intersection
    // (this test isn't enough to differentiate), so use the full-on
    // line-sphere test.
    return FindIntersection(kSeg,rkTriVelocity,rkSphere,rkSphVelocity,
        rfTFirst,fTMax,riQuantity,akP);
}

bool Wml::FindIntersection (const Triangle3<Real>& rkTri,
    const Vector3<Real>& rkTriVelocity, const Sphere3<Real>& rkSphere,
    const Vector3<Real>& rkSphVelocity, Real& rfTFirst, Real fTMax,
    int& riQuantity, Vector3<Real> akP[6])
{
    // triangle vertices
    Vector3<Real> akV[3] =
    {
        rkTri.Origin(),
        rkTri.Origin() + rkTri.Edge0(),
        rkTri.Origin() + rkTri.Edge1()
    };

    // triangle edges
    Vector3<Real> akE[3] =
    {
        akV[1] - akV[0],
        akV[2] - akV[1], 
        akV[0] - akV[2]
    };

    // triangle normal
    Vector3<Real> kN = akE[1].Cross(akE[0]);

    // sphere center projection on triangle normal
    Real fNdC = kN.Dot(rkSphere.Center());

    // Radius projected length in normal direction.  This defers the square
    // root to normalize kN until absolutely needed.
    Real fNormRadiusSqr =
        kN.SquaredLength()*rkSphere.Radius()*rkSphere.Radius();

    // triangle projection on triangle normal
    Real fNdT = kN.Dot(akV[0]);

    // Distance from sphere to triangle along the normal
    Real fDist = fNdC - fNdT;
    
    // normals for the plane formed by edge i and the triangle normal
    Vector3<Real> akExN[3] =
    {
        akE[0].Cross(kN),
        akE[1].Cross(kN),
        akE[2].Cross(kN)
    };

    Segment3<Real> kSeg;

    if ( fDist*fDist <= fNormRadiusSqr )
    {
        // sphere currently intersects the plane of the triangle

        // see which edges the sphere center is inside/outside of
        bool bInside[3];
        for (int i = 0; i < 3; i++ )
        {
            bInside[i] = ( akExN[i].Dot(rkSphere.Center()) >=
                akExN[i].Dot(akV[i]) );
        }

        if ( bInside[0] )
        {
            if ( bInside[1] )
            {
                if ( bInside[2] )
                {
                    // triangle inside sphere
                    return false;
                }
                else // !bInside[2]
                {
                    // potential intersection with edge 2
                    kSeg.Origin() = akV[2];
                    kSeg.Direction() = akE[2];
                    return FindIntersection(kSeg,rkTriVelocity,rkSphere,
                        rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
                }
            }
            else // !bInside[1]
            {
                if ( bInside[2] )
                {
                    // potential intersection with edge 1
                    kSeg.Origin() = akV[1];
                    kSeg.Direction() = akE[1];
                    return FindIntersection(kSeg,rkTriVelocity,rkSphere,
                        rkSphVelocity,rfTFirst,fTMax,riQuantity,akP);
                }
                else // !bInside[2]
                {
                    // potential intersection with edges 1,2
                    return FindTriSphrCoplanarIntersection(2,akV,kN,akExN[2],
                        akE[2],rkSphere,rkTriVelocity,rkSphVelocity,rfTFirst,
                        fTMax,riQuantity,akP);
                }            
            }
        } 
        else // !bInside[0]
        {
            if ( bInside[1] )
//.........这里部分代码省略.........