mgcdistlin3pgm3.cpp

3D Game Engine Design Source Code非常棒

    MgcReal fA12 = rkPgm.Edge0().Dot(rkPgm.Edge1());
    MgcReal fA22 = rkPgm.Edge1().SquaredLength();
    MgcReal fB0  = -kDiff.Dot(rkSeg.Direction());
    MgcReal fB1  = kDiff.Dot(rkPgm.Edge0());
    MgcReal fB2  = kDiff.Dot(rkPgm.Edge1());
    MgcReal fCof00 = fA11*fA22-fA12*fA12;
    MgcReal fCof01 = fA02*fA12-fA01*fA22;
    MgcReal fCof02 = fA01*fA12-fA02*fA11;
    MgcReal fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;

    MgcSegment3 kSegPgm;
    MgcVector3 kPt;
    MgcReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;

    if ( MgcMath::Abs(fDet) >= gs_fTolerance )
    {
        MgcReal fCof11 = fA00*fA22-fA02*fA02;
        MgcReal fCof12 = fA02*fA01-fA00*fA12;
        MgcReal fCof22 = fA00*fA11-fA01*fA01;
        MgcReal fInvDet = 1.0/fDet;
        MgcReal fRhs0 = -fB0*fInvDet;
        MgcReal fRhs1 = -fB1*fInvDet;
        MgcReal fRhs2 = -fB2*fInvDet;

        fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
        fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
        fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;

        if ( fR < 0.0 )
        {
            if ( fS < 0.0 )
            {
                if ( fT < 0.0 )  // region 6m
                {
                    // min on face s=0 or t=0 or r=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else if ( fT <= 1.0 )  // region 5m
                {
                    // min on face s=0 or r=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else  // region 4m
                {
                    // min on face s=0 or t=1 or r=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 1.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
            }
            else if ( fS <= 1.0 )
            {
                if ( fT < 0.0 )  // region 7m
                {
                    // min on face t=0 or r=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fS);
                    fT = 0.0;
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else if ( fT <= 1.0 )  // region 0m
                {
                    // min on face r=0
                    fSqrDist = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS,&fT);
                    fR = 0.0;
                }
                else  // region 3m
                {
                    // min on face t=1 or r=0
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fS);
                    fT = 1.0;
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
            }
            else
            {
                if ( fT < 0.0 )  // region 8m
                {
                    // min on face s=1 or t=0 or r=0
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 1.0;
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else if ( fT <= 1.0 )  // region 1m
                {
                    // min on face s=1 or r=0
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 1.0;
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else  // region 2m
                {
                    // min on face s=1 or t=1 or r=0
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 1.0;
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 1.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                    fSqrDist0 = MgcSqrDistance(rkSeg.Origin(),rkPgm,&fS0,
                        &fT0);
                    fR0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
            }
        }
        else if ( fR <= 1.0 )
        {
            if ( fS < 0.0 )
            {
                if ( fT < 0.0 )  // region 6
                {
                    // min on face s=0 or t=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else if ( fT <= 1 )  // region 5
                {
                    // min on face s=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 0.0;
                }
                else // region 4
                {
                    // min on face s=0 or t=1
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 0.0;
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 1.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
            }
            else if ( fS <= 1.0 )
            {
                if ( fT < 0.0 )  // region 7
                {
                    // min on face t=0
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fS);
                    fT = 0.0;
                }
                else if ( fT <= 1.0 )  // region 0
                {
                    // global minimum is interior
                    fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0*fB0)
                          +fS*(fA01*fR+fA11*fS+fA12*fT+2.0*fB1)
                          +fT*(fA02*fR+fA12*fS+fA22*fT+2.0*fB2)
                          +kDiff.SquaredLength();
                }
                else  // region 3
                {
                    // min on face t=1
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge1();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fS);
                    fT = 1.0;
                }
            }
            else
            {
                if ( fT < 0.0 )  // region 8
                {
                    // min on face s=1 or t=0
                    kSegPgm.Origin() = rkPgm.Origin()+rkPgm.Edge0();
                    kSegPgm.Direction() = rkPgm.Edge1();
                    fSqrDist = MgcSqrDistance(rkSeg,kSegPgm,&fR,&fT);
                    fS = 1.0;
                    kSegPgm.Origin() = rkPgm.Origin();
                    kSegPgm.Direction() = rkPgm.Edge0();
                    fSqrDist0 = MgcSqrDistance(rkSeg,kSegPgm,&fR0,&fS0);
                    fT0 = 0.0;
                    if ( fSqrDist0 < fSqrDist )
                    {
                        fSqrDist = fSqrDist0;
                        fR = fR0;
                        fS = fS0;
                        fT = fT0;
                    }
                }
                else if ( fT <= 1.0 )  // region 1
                {
                    // min on face s=1