コード例 #1
0
ファイル: verlet.cpp プロジェクト: kaikai2/tankwarca
    size_t collide(CircleShape &circle, BoxShape &box, float dt, Contact *pxContacts, size_t numMaxContacts)
    {
        dt, pxContacts, numMaxContacts;
        const Point2f &p1 = circle.body.getPosition();
        const Point2f &p2 = box.body.getPosition();

        // work in the box's coordinate system
        const Point2f kDiff = p1 - p2;

        // compute squared distance and closest point on box
        float fSqrDistance = 0.0f, fDelta;
        Point2f kClosest(kDiff * box.GetDir(0), kDiff * box.GetDir(1));
        const Point2f &extents = box.GetExtents();
        if (kClosest.x < -extents.x)
        {
            fDelta = kClosest.x + extents.x;
            fSqrDistance += fDelta*fDelta;
            kClosest.x = -extents.x;
        }
        else if (kClosest.x > extents.x)
        {
            fDelta = kClosest.x - extents.x;
            fSqrDistance += fDelta*fDelta;
            kClosest.x = extents.x;
        }
        if (kClosest.y < -extents.y)
        {
            fDelta = kClosest.y + extents.y;
            fSqrDistance += fDelta*fDelta;
            kClosest.y = -extents.y;
        }
        else if (kClosest.y > extents.y)
        {
            fDelta = kClosest.y - extents.y;
            fSqrDistance += fDelta*fDelta;
            kClosest.y = extents.y;
        }
        if (fSqrDistance > circle.GetRadius() * circle.GetRadius())
        {
            return 0;
        }
        Point2f d = p2 + kClosest - p1;
        d.Normalize();

        pxContacts[0] = Contact(p1 + d * circle.GetRadius(), p2 + kClosest, &circle.body, &box.body);
        return 1;
    }
コード例 #2
0
Real DistVector3Ellipsoid3<Real>::GetSquared ()
{
    // compute coordinates of point in ellipsoid coordinate system
    Vector3<Real> kDiff = m_rkVector - m_rkEllipsoid.Center;
    Vector3<Real> kEPoint(kDiff.Dot(m_rkEllipsoid.Axis[0]),
        kDiff.Dot(m_rkEllipsoid.Axis[1]),kDiff.Dot(m_rkEllipsoid.Axis[2]));

    const Real* afExtent = m_rkEllipsoid.Extent;
    Real fA2 = afExtent[0]*afExtent[0];
    Real fB2 = afExtent[1]*afExtent[1];
    Real fC2 = afExtent[2]*afExtent[2];
    Real fU2 = kEPoint.X()*kEPoint.X();
    Real fV2 = kEPoint.Y()*kEPoint.Y();
    Real fW2 = kEPoint.Z()*kEPoint.Z();
    Real fA2U2 = fA2*fU2, fB2V2 = fB2*fV2, fC2W2 = fC2*fW2;

    // initial guess
    Real fURatio = kEPoint.X()/afExtent[0];
    Real fVRatio = kEPoint.Y()/afExtent[1];
    Real fWRatio = kEPoint.Z()/afExtent[2];
    Real fT;
    if (fURatio*fURatio+fVRatio*fVRatio+fWRatio*fWRatio < (Real)1.0)
    {
        fT = (Real)0.0;
    }
    else
    {
        Real fMax = afExtent[0];
        if (afExtent[1] > fMax)
        {
            fMax = afExtent[1];
        }
        if (afExtent[2] > fMax)
        {
            fMax = afExtent[2];
        }

        fT = fMax*kEPoint.Length();
    }

    // Newton's method
    const int iMaxIteration = 64;
    Real fP, fQ, fR;
    for (int i = 0; i < iMaxIteration; i++)
    {
        fP = fT+fA2;
        fQ = fT+fB2;
        fR = fT+fC2;
        Real fP2 = fP*fP;
        Real fQ2 = fQ*fQ;
        Real fR2 = fR*fR;
        Real fS = fP2*fQ2*fR2-fA2U2*fQ2*fR2-fB2V2*fP2*fR2-fC2W2*fP2*fQ2;
        if (Math<Real>::FAbs(fS) < Math<Real>::ZERO_TOLERANCE)
        {
            break;
        }

        Real fPQ = fP*fQ, fPR = fP*fR, fQR = fQ*fR, fPQR = fP*fQ*fR;
        Real fDS = ((Real)2.0)*(fPQR*(fQR+fPR+fPQ)-fA2U2*fQR*(fQ+fR)-
            fB2V2*fPR*(fP+fR)-fC2W2*fPQ*(fP+fQ));
        fT -= fS/fDS;
    }

    Vector3<Real> kClosest(fA2*kEPoint.X()/fP,fB2*kEPoint.Y()/fQ,
        fC2*kEPoint.Z()/fR);
    kDiff = kClosest - kEPoint;
    Real fSqrDistance = kDiff.SquaredLength();

    m_kClosestPoint0 = m_rkVector;
    m_kClosestPoint1 = m_rkEllipsoid.Center +
        kClosest.X()*m_rkEllipsoid.Axis[0] +
        kClosest.Y()*m_rkEllipsoid.Axis[1] +
        kClosest.Z()*m_rkEllipsoid.Axis[2];

    return fSqrDistance;
}