//----------------------------------------------------------------------------
void BouncingTetrahedra::Reposition (int t0, int t1, Contact& contact)
{
    RigidTetra& tetra0 = *mTetras[t0];
    RigidTetra& tetra1 = *mTetras[t1];

    // Compute the centroids of the tetrahedra.
    Vector3f vertices0[4], vertices1[4];
    tetra0.GetVertices(vertices0);
    tetra1.GetVertices(vertices1);
    Vector3f centroid0 = Vector3f::ZERO;
    Vector3f centroid1 = Vector3f::ZERO;
    int i;
    for (i = 0; i < 4; ++i)
    {
        centroid0 += vertices0[i];
        centroid1 += vertices1[i];
    }
    centroid0 *= 0.25f;
    centroid1 *= 0.25f;

    // Randomly perturb the tetrahedra vertices by a small amount.  This is
    // done to help prevent the LCP solver from getting into cycles and
    // degenerate cases.
    const float reduction = 0.95f;
    float reduceI = reduction*Mathf::IntervalRandom(0.9999f, 1.0001f);
    float reduceJ = reduction*Mathf::IntervalRandom(0.9999f, 1.0001f);
    for (i = 0; i < 4; ++i)
    {
        vertices0[i] = centroid0 + (vertices0[i] - centroid0)*reduceI;
        vertices1[i] = centroid1 + (vertices1[i] - centroid1)*reduceJ;
    }

    // Compute the distance between the tetrahedra.
    float dist = 1.0f;
    int statusCode = 0;
    Vector3f closest[2];
    LCPPolyDist3(4, vertices0, 4, mFaces, 4, vertices1, 4, mFaces,
        statusCode, dist, closest);
    ++mLCPCount;

    // In theory, LCPPolyDist<3> should always find a valid distance, but just
    // in case numerical round-off errors cause problems, let us trap it.
    assertion(dist >= 0.0f, "LCP polyhedron distance calculator failed.\n");

    // Reposition the tetrahedra to the theoretical points of contact.
    closest[0] = centroid0 + (closest[0] - centroid0)/reduceI;
    closest[1] = centroid1 + (closest[1] - centroid1)/reduceJ;
    for (i = 0; i < 4; ++i)
    {
        vertices0[i] = centroid0 + (vertices0[i] - centroid0)/reduceI;
        vertices1[i] = centroid1 + (vertices1[i] - centroid1)/reduceJ;
    }

    // Numerical round-off errors can cause interpenetration.  Move the
    // tetrahedra to back out of this situation.  The length of diff
    // estimates the depth of penetration when dist > 0 was reported.
    Vector3f diff = closest[0] - closest[1];

    // Apply the separation distance along the line containing the centroids
    // of the tetrahedra.
    Vector3f diff2 = centroid1 - centroid0;
    diff = diff2/diff2.Length()*diff.Length();

    // Move each tetrahedron by half of kDiff when the distance was large,
    // but move each by twice kDiff when the distance is really small.
    float mult = (dist >= mTolerance ? 0.5f : 1.0f);
    Vector3f delta = mult*diff;

    // Undo the interpenetration.
    if (tetra0.Moved && !tetra1.Moved)
    {
        // Tetra t0 has moved but tetra t1 has not moved.
        tetra1.SetPosition(tetra1.GetPosition() + 2.0f*delta);
        tetra1.Moved = true;
    }
    else if (!tetra0.Moved && tetra1.Moved)
    {
        // Tetra t1 has moved but tetra t0 has not moved.
        tetra0.SetPosition(tetra0.GetPosition() - 2.0f*delta);
        tetra0.Moved = true;
    }
    else
    {
        // Both tetras moved or both tetras did not move.
        tetra0.SetPosition(tetra0.GetPosition() - delta);
        tetra0.Moved = true;
        tetra1.SetPosition(tetra1.GetPosition() + delta);
        tetra1.Moved = true;
    }

    // Test whether the two tetrahedra intersect in a vertex-face
    // configuration.
    contact.IsVFContact = IsVertex(vertices0, closest[0]);
    if (contact.IsVFContact)
    {
        contact.A = mTetras[t1];
        contact.B = mTetras[t0];
        CalculateNormal(vertices1, closest[1], contact);
    }
    else
    {
        contact.IsVFContact = IsVertex(vertices1, closest[1]);
        if (contact.IsVFContact)
        {
            contact.A = mTetras[t0];
            contact.B = mTetras[t1];
            CalculateNormal(vertices0, closest[0], contact);
        }
    }

    // Test whether the two tetrahedra intersect in an edge-edge
    // configuration.
    if (!contact.IsVFContact)
    {
        contact.A = mTetras[t0];
        contact.B = mTetras[t1];
        Vector3f otherVertexA = Vector3f::UNIT_X;
        Vector3f otherVertexB = Vector3f::ZERO;
        contact.EA = ClosestEdge(vertices0, closest[0], otherVertexA);
        contact.EB = ClosestEdge(vertices1, closest[1], otherVertexB);
        Vector3f normal = contact.EA.UnitCross(contact.EB);
        if (normal.Dot(otherVertexA - closest[0]) < 0.0f)
        {
            contact.N = normal;
        }
        else
        {
            contact.N = -normal;
        }
    }

    // Reposition results to correspond to relocaton of tetra.
    contact.PA = closest[0] - delta;
    contact.PB = closest[1] + delta;
}
예제 #2
0
//----------------------------------------------------------------------------
void PolyhedronDistance::UpdateSegments ()
{
    // Two segments make the line easier to see.
    Vector3f U[2][4];

    // Offset the polyhedra so far into the first octant that we are unlikely
    // to translate them out of that octant during a run.
    mOffsetMagnitude = 20.0f;
    Vector3f offset = mOffsetMagnitude*Vector3f::ONE;

    VertexBufferAccessor vba;
    int i;
    for (i = 0; i < 2; ++i)
    {
        const APoint& wTrn = mTetras[i]->WorldTransform.GetTranslate();
        const HMatrix& wRot = mTetras[i]->WorldTransform.GetRotate();
        vba.ApplyTo(mTetras[i]);
        for (int j = 0; j < 4; ++j)
        {
            AVector relPosition = vba.Position<Float3>(j);
            APoint position = wTrn + wRot*relPosition;
            U[i][j] = Vector3f(position[0] + offset[0],
                position[1] + offset[1], position[2] + offset[2]);
        }
    }

    vba.ApplyTo(mSegments[0]);
    Vector3f vertex[2] =
    {
        vba.Position<Vector3f>(0),
        vba.Position<Vector3f>(1)
    };

    int statusCode;

    LCPPolyDist3(4, U[0], 4, mFaces, 4, U[1], 4, mFaces, statusCode,
        mSeparation, vertex);

    vba.Position<Vector3f>(0) = vertex[0];
    vba.Position<Vector3f>(1) = vertex[1];

    if (statusCode != LCPPolyDist3::SC_FOUND_SOLUTION &&
        statusCode != LCPPolyDist3::SC_TEST_POINTS_TEST_FAILED &&
        statusCode != LCPPolyDist3::SC_FOUND_TRIVIAL_SOLUTION ||
        mSeparation < 0.0f)
    {
        // Do not draw the line joining nearest points if returns from
        // LCPPolyDist are not appropriate.
        for (i = 0; i < 2; ++i)
        {
            vba.Position<Vector3f>(i) = -offset;
        }
    }

    // Correct for the offset and set up endpoints for the segment.
    for (i = 0; i < 2; ++i)
    {
        vba.Position<Vector3f>(i) -= offset;

        // The adjustment with mSmall "centers" the endpoint tetra on the
        // solution points.
        Vector3f temp = vba.Position<Vector3f>(i) -
            (mSmall/3.0f)*Vector3f::ONE;
        mTetras[i + 2]->LocalTransform.SetTranslate(temp);
    }

    // Double-up the line for better visibility.
    vertex[0] = vba.Position<Vector3f>(0);
    vertex[1] = vba.Position<Vector3f>(1);
    vba.ApplyTo(mSegments[1]);
    const float epsilon = 0.002f;
    for (i = 0; i < 2; ++i)
    {
        vba.Position<Vector3f>(i) = vertex[i] + epsilon*Vector3f::ONE;
    }

    for (i = 0; i < 2; ++i)
    {
        mSegments[i]->UpdateModelSpace(Visual::GU_MODEL_BOUND_ONLY);
        mSegments[i]->Update();
        mRenderer->Update(mSegments[i]->GetVertexBuffer());
    }

    mScene->Update();
}
//----------------------------------------------------------------------------
void BouncingTetrahedra::DoCollisionDetection ()
{
    int i, j;
    Contact contact;
    mContacts.clear();

    // Test for tetrahedron-boundary collisions.
    for (i = 0; i < NUM_TETRA; ++i)
    {
        mTetras[i]->Moved = false;
        if (FarFromBoundary(i))
        {
            continue;
        }

        // These checks are done in pairs under the assumption that the tetra 
        // have smaller diameters than the separation of opposite boundaries, 
        // hence only one of each opposite pair of boundaries may be touched 
        // at any one time.
        Vector3f vertices[4];
        float distances[4];
        mTetras[i]->GetVertices(vertices);
        float radius = mTetras[i]->GetRadius();
        Vector3f position = mTetras[i]->GetPosition();

        // rear[0] and front[5] boundaries
        if (position.X() - radius < mBoundaryLocations[0].X())
        {
            for (j = 0; j < 4; ++j)
            {
                distances[j] = vertices[j].X() - mBoundaryLocations[0].X();
            }
            TetraBoundaryIntersection(i, 0, distances, contact);
        }
        else if (position.X() + radius > mBoundaryLocations[5].X())
        {
            for (j = 0; j < 4; ++j)
            {
                distances[j] = mBoundaryLocations[5].X() - vertices[j].X();
            }
            TetraBoundaryIntersection(i, 5, distances, contact);
        }

        // left[1] and right[3] boundaries
        if (position.Y() - radius < mBoundaryLocations[1].Y())
        {
            for (j = 0; j < 4; ++j)
            {
                distances[j] = vertices[j].Y() - mBoundaryLocations[1].Y();
            }
            TetraBoundaryIntersection(i, 1, distances, contact);
        }
        else if (position.Y() + radius > mBoundaryLocations[3].Y())
        {
            for (j = 0; j < 4; ++j)
            {
                distances[j] = mBoundaryLocations[3].Y() - vertices[j].Y();
            }
            TetraBoundaryIntersection(i, 3, distances, contact);
        }

        // bottom[2] and top[4] boundaries
        if (position.Z() - radius < mBoundaryLocations[2].Z())
        {
            for (j = 0; j < 4; ++j)
            {
                distances[j] = vertices[j].Z() - mBoundaryLocations[2].Z();
            }
            TetraBoundaryIntersection(i, 2, distances, contact);
        }
        else if (position.Z() + radius > mBoundaryLocations[4].Z())
        {
            for (j = 0; j < 4; ++j)
            {
                distances[j] = mBoundaryLocations[4].Z() - vertices[j].Z();
            }
            TetraBoundaryIntersection(i, 4, distances, contact);
        }
    }    

    // Test for tetrahedron-tetrahedron collisions.
    mLCPCount = 0;
    for (i = 0; i < NUM_TETRA-1; ++i)
    {
        Vector3f vertices0[4];
        mTetras[i]->GetVertices(vertices0);

        for (j = i + 1; j < NUM_TETRA; ++j)
        {
            Vector3f vertices1[4];
            mTetras[j]->GetVertices(vertices1);

            if (!FarApart(i, j))
            {
                float dist = 1.0f;
                int statusCode = 0;
                Vector3f closest[2];
                LCPPolyDist3(4, vertices0, 4,mFaces, 4, vertices1, 4,
                    mFaces, statusCode, dist, closest);
                ++mLCPCount;
                if (dist <= mTolerance)
                {
                    // Collision with good LCPPolyDist results.
                    Reposition(i, j, contact);
                    mContacts.push_back(contact);
                }
            }
        }
    }

    mNumContacts = (int)mContacts.size();
}