C++ Vector2::DotPerp方法代码示例

int IntrLine2Segment2<Real>::Classify (Real* afS, Vector2<Real>* pkDiff,
    Vector2<Real>* pkDiffN)
{
    // The intersection of two lines is a solution to P0+s0*D0 = P1+s1*D1.
    // Rewrite this as s0*D0 - s1*D1 = P1 - P0 = Q.  If D0.Dot(Perp(D1)) = 0,
    // the lines are parallel.  Additionally, if Q.Dot(Perp(D1)) = 0, the
    // lines are the same.  If D0.Dot(Perp(D1)) is not zero, then
    //   s0 = Q.Dot(Perp(D1))/D0.Dot(Perp(D1))
    // produces the point of intersection.  Also,
    //   s1 = Q.Dot(Perp(D0))/D0.Dot(Perp(D1))

    Vector2<Real> kDiff = m_pkSegment->Origin - m_pkLine->Origin;
    if (pkDiff)
    {
        *pkDiff = kDiff;
    }

    Real fD0DotPerpD1 = m_pkLine->Direction.DotPerp(m_pkSegment->Direction);
    if (Math<Real>::FAbs(fD0DotPerpD1) > Math<Real>::ZERO_TOLERANCE)
    {
        // lines intersect in a single point
        if (afS)
        {
            Real fInvD0DotPerpD1 = ((Real)1.0)/fD0DotPerpD1;
            Real fDiffDotPerpD0 = kDiff.DotPerp(m_pkLine->Direction);
            Real fDiffDotPerpD1 = kDiff.DotPerp(m_pkSegment->Direction);
            afS[0] = fDiffDotPerpD1*fInvD0DotPerpD1;
            afS[1] = fDiffDotPerpD0*fInvD0DotPerpD1;
        }
        return IT_POINT;
    }

    // lines are parallel
    kDiff.Normalize();
    if (pkDiffN)
    {
        *pkDiffN = kDiff;
    }

    Real fDiffNDotPerpD1 = kDiff.DotPerp(m_pkSegment->Direction);
    if (Math<Real>::FAbs(fDiffNDotPerpD1) <= Math<Real>::ZERO_TOLERANCE)
    {
        // lines are colinear
        return IT_SEGMENT;
    }

    // lines are parallel, but distinct
    return IT_EMPTY;
}

int IntrSegment2Segment2<Real>::Classify (Real* s, Vector2<Real>* diff,
    Vector2<Real>* diffN)
{
    // The intersection of two lines is a solution to P0+s0*D0 = P1+s1*D1.
    // Rewrite this as s0*D0 - s1*D1 = P1 - P0 = Q.  If D0.Dot(Perp(D1)) = 0,
    // the lines are parallel.  Additionally, if Q.Dot(Perp(D1)) = 0, the
    // lines are the same.  If D0.Dot(Perp(D1)) is not zero, then
    //   s0 = Q.Dot(Perp(D1))/D0.Dot(Perp(D1))
    // produces the point of intersection.  Also,
    //   s1 = Q.Dot(Perp(D0))/D0.Dot(Perp(D1))

    Vector2<Real> originDiff = mSegment1->Center - mSegment0->Center;
    if (diff)
    {
        *diff = originDiff;
    }

    Real D0DotPerpD1 = mSegment0->Direction.DotPerp(mSegment1->Direction);
    if (Math<Real>::FAbs(D0DotPerpD1) > Math<Real>::ZERO_TOLERANCE)
    {
        // Lines intersect in a single point.
        if (s)
        {
            Real invD0DotPerpD1 = ((Real)1)/D0DotPerpD1;
            Real diffDotPerpD0 = originDiff.DotPerp(mSegment0->Direction);
            Real diffDotPerpD1 = originDiff.DotPerp(mSegment1->Direction);
            s[0] = diffDotPerpD1*invD0DotPerpD1;
            s[1] = diffDotPerpD0*invD0DotPerpD1;
        }
        return IT_POINT;
    }

    // Lines are parallel.
    originDiff.Normalize();
    if (diffN)
    {
        *diffN = originDiff;
    }

    Real diffNDotPerpD1 = originDiff.DotPerp(mSegment1->Direction);
    if (Math<Real>::FAbs(diffNDotPerpD1) <= Math<Real>::ZERO_TOLERANCE)
    {
        // Lines are colinear.
        return IT_SEGMENT;
    }

    // Lines are parallel, but distinct.
    return IT_EMPTY;
}

void IntrLine2Triangle2<Real>::TriangleLineRelations (
    const Vector2<Real>& origin, const Vector2<Real>& direction,
    const Triangle2<Real>& triangle, Real dist[3], int sign[3],
    int& positive, int& negative, int& zero)
{
    positive = 0;
    negative = 0;
    zero = 0;
    for (int i = 0; i < 3; ++i)
    {
        Vector2<Real> diff = triangle.V[i] - origin;
        dist[i] = diff.DotPerp(direction);
        if (dist[i] > Math<Real>::ZERO_TOLERANCE)
        {
            sign[i] = 1;
            ++positive;
        }
        else if (dist[i] < -Math<Real>::ZERO_TOLERANCE)
        {
            sign[i] = -1;
            ++negative;
        }
        else
        {
            dist[i] = (Real)0;
            sign[i] = 0;
            ++zero;
        }
    }
}

void IntrLine2Triangle2<Real>::TriangleLineRelations (
    const Vector2<Real>& rkOrigin, const Vector2<Real>& rkDirection,
    const Triangle2<Real>& rkTriangle, Real afDist[3], int aiSign[3],
    int& riPositive, int& riNegative, int& riZero)
{
    riPositive = 0;
    riNegative = 0;
    riZero = 0;
    for (int i = 0; i < 3; i++)
    {
        Vector2<Real> kDiff = rkTriangle.V[i] - rkOrigin;
        afDist[i] = kDiff.DotPerp(rkDirection);
        if (afDist[i] > Math<Real>::ZERO_TOLERANCE)
        {
            aiSign[i] = 1;
            riPositive++;
        }
        else if (afDist[i] < -Math<Real>::ZERO_TOLERANCE)
        {
            aiSign[i] = -1;
            riNegative++;
        }
        else
        {
            afDist[i] = (Real)0.0;
            aiSign[i] = 0;
            riZero++;
        }
    }
}

    bool IntrTriangle3Cylinder3<Real>::DiskOverlapsSegment (
        const Vector2<Real>& Q0, const Vector2<Real>& Q1 ) const
    {
        Real rSqr = mCylinder->Radius * mCylinder->Radius;
        Vector2<Real> D = Q0 - Q1;
        Real dot = Q0.Dot( D );
        if ( dot <= ( Real )0 )
        {
            return Q0.Dot( Q0 ) <= rSqr;
        }

        Real lenSqr = D.Dot( D );
        if ( dot >= lenSqr )
        {
            return Q1.Dot( Q1 ) <= rSqr;
        }

        dot = D.DotPerp( Q0 );
        return dot * dot <= lenSqr * rSqr;
    }

Vector2<Real> BiQuadToSqr<Real>::Transform (const Vector2<Real>& P)
{
    Vector2<Real> A = mP00 - P;
    Real AB = A.DotPerp(mB);
    Real AC = A.DotPerp(mC);

    // 0 = ac*bc+(bc^2+ac*bd-ab*cd)*s+bc*bd*s^2 = k0 + k1*s + k2*s^2
    Real k0 = AC*mBC;
    Real k1 = mBC*mBC + AC*mBD - AB*mCD;
    Real k2 = mBC*mBD;

    if (Math<Real>::FAbs(k2) >= Math<Real>::ZERO_TOLERANCE)
    {
        // The s-equation is quadratic.
        Real inv = ((Real)0.5)/k2;
        Real discr = k1*k1 - ((Real)4)*k0*k2;
        Real root = Math<Real>::Sqrt(Math<Real>::FAbs(discr));

        Vector2<Real> result0;
        result0.X() = (-k1 - root)*inv;
        result0.Y() = AB/(mBC + mBD*result0.X());
        Real deviation0 = Deviation(result0);
        if (deviation0 == (Real)0)
        {
            return result0;
        }

        Vector2<Real> result1;
        result1.X() = (-k1 + root)*inv;
        result1.Y() = AB/(mBC + mBD*result1.X());
        Real deviation1 = Deviation(result1);
        if (deviation1 == (Real)0)
        {
            return result1;
        }

        if (deviation0 <= deviation1)
        {
            if (deviation0 <= Math<Real>::ZERO_TOLERANCE)
            {
                return result0;
            }
        }
        else
        {
            if (deviation1 <= Math<Real>::ZERO_TOLERANCE)
            {
                return result1;
            }
        }
    }
    else
    {
        // The s-equation is linear.
        Vector2<Real> result;

        result.X() = -k0/k1;
        result.Y() = AB/(mBC + mBD*result.X());
        Real deviation = Deviation(result);
        if (deviation <= Math<Real>::ZERO_TOLERANCE)
        {
            return result;
        }
    }

    // Point is outside the quadrilateral, return invalid.
    return Vector2<Real>(Math<Real>::MAX_REAL, Math<Real>::MAX_REAL);
}

Vector2<Real> BiQuadToSqr<Real>::Transform (const Vector2<Real>& rkP)
{
    Vector2<Real> kA = m_kP00 - rkP;
    Real fAB = kA.DotPerp(m_kB);
    Real fAC = kA.DotPerp(m_kC);

    // 0 = ac*bc+(bc^2+ac*bd-ab*cd)*s+bc*bd*s^2 = k0 + k1*s + k2*s^2
    Real fK0 = fAC*m_fBC;
    Real fK1 = m_fBC*m_fBC + fAC*m_fBD - fAB*m_fCD;
    Real fK2 = m_fBC*m_fBD;

    if (Math<Real>::FAbs(fK2) >= Math<Real>::ZERO_TOLERANCE)
    {
        // s-equation is quadratic
        Real fInv = ((Real)0.5)/fK2;
        Real fDiscr = fK1*fK1 - ((Real)4.0)*fK0*fK2;
        Real fRoot = Math<Real>::Sqrt(Math<Real>::FAbs(fDiscr));

        Vector2<Real> kResult0;
        kResult0.X() = (-fK1 - fRoot)*fInv;
        kResult0.Y() = fAB/(m_fBC + m_fBD*kResult0.X());
        Real fDeviation0 = Deviation(kResult0);
        if (fDeviation0 == (Real)0.0)
        {
            return kResult0;
        }

        Vector2<Real> kResult1;
        kResult1.X() = (-fK1 + fRoot)*fInv;
        kResult1.Y() = fAB/(m_fBC + m_fBD*kResult1.X());
        Real fDeviation1 = Deviation(kResult1);
        if (fDeviation1 == (Real)0.0)
        {
            return kResult1;
        }

        if (fDeviation0 <= fDeviation1)
        {
            if (fDeviation0 <= Math<Real>::ZERO_TOLERANCE)
            {
                return kResult0;
            }
        }
        else
        {
            if (fDeviation1 <= Math<Real>::ZERO_TOLERANCE)
            {
                return kResult1;
            }
        }
    }
    else
    {
        // s-equation is linear
        Vector2<Real> kResult;

        kResult.X() = -fK0/fK1;
        kResult.Y() = fAB/(m_fBC + m_fBD*kResult.X());
        Real fDeviation = Deviation(kResult);
        if (fDeviation <= Math<Real>::ZERO_TOLERANCE)
        {
            return kResult;
        }
    }

    // point is outside the quadrilateral, return invalid
    return Vector2<Real>(Math<Real>::MAX_REAL,Math<Real>::MAX_REAL);
}