//----------------------------------------------------------------------------
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;
}
//----------------------------------------------------------------------------
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();
}