void ContactManager3D::Merge(ContactManifold3D &src, ContactManifold3D &dest, float persistentThresholdSQ)
{
  GV_ASSERT(src.SameColliders(dest));

  auto &colliderA = *dest.colliderA;
  auto &colliderB = *dest.colliderB;
  auto &bodyA = colliderA.Parent();
  auto &bodyB = colliderB.Parent();

  // list of valid contacts
  Contact3D *validContacts[ContactManifold3D::k_maxContacts * 2] = { 0 };
  uint8 numValidContacts = 0;

  // check persistent contacts
  {
    for (uint8 i = 0; i < dest.numContacts; ++i)
    {
      Contact3D &contact = *dest.contacts[i];
      const Vec3 localToGlobalA = bodyA.LocalToGlobal(contact.localPositionA);
      const Vec3 localToGlobalB = bodyB.LocalToGlobal(contact.localPositionB);
      const Vec3 rA = contact.globalPositionA - localToGlobalA;
      const Vec3 rB = contact.globalPositionB - localToGlobalB;
      if (!src.invalidateExistingContacts
          && contact.normal.Dot(localToGlobalB - localToGlobalA) <= 0.0f
          && rA.LengthSQ() < persistentThresholdSQ
          && rB.LengthSQ() < persistentThresholdSQ)
      {
        // contact persistent, keep
        contact.persistent = true;
        validContacts[numValidContacts++] = &contact;
      }
      else
      {
        // dispatch end contact event
        if (m_listener)
          m_listener->EndContact(colliderA, colliderB, contact);

        // contact not persistent, destroy
        contact.~Contact3D();
        m_contactAllocator.Free(&contact);
      }
    }
  } // end of check persistent contacts

  // process new contacts
  for (uint8 i = 0; i < src.numContacts; ++i)
  {
    auto newContact = src.contacts[i];

    // discard new contact if it's too close to cached contacts
    bool discard = false;
    for (uint8 j = 0; j < numValidContacts; ++j)
    {
      if ((newContact->globalPositionA - validContacts[j]->globalPositionA).LengthSQ() < persistentThresholdSQ)
      {
        discard = true;
        break;
      }
    }

    if (discard)
    {
      // contact too close to cached contacts, destroy
      newContact->~Contact3D();
      m_contactAllocator.Free(newContact);
    }
    else
    {
      // add new contact to valid list
      validContacts[numValidContacts++] = newContact;
    }
  }

  // repopulate new manifold
  if (numValidContacts <= ContactManifold3D::k_maxContacts)
  {
    // enough contact slots, merge all
    for (uint8 i = 0; i < numValidContacts; ++i)
      dest.contacts[i] = validContacts[i];
    for (uint8 i = numValidContacts; i < dest.numContacts; ++i)
      dest.contacts[i] = nullptr;
    dest.numContacts = numValidContacts;
  }
  else // not enough contact slots, pick ones furthest apart
  {
    dest.numContacts = 0;

    // 1) find deepest contact first
    Contact3D *deepest = validContacts[0];
    uint8 deepestIndex = 0;
    for (uint8 i = 1; i < numValidContacts; ++i)
    {
      if (validContacts[i]->depth > deepest->depth)
      {
        deepest = validContacts[i];
        deepestIndex = i;
      }
    }
    // move deepest contact to end of valid list
    std::swap(validContacts[deepestIndex], validContacts[--numValidContacts]);
    // add deepest contact
    dest.contacts[dest.numContacts++] = deepest;

    // 2) find furthest contact to form 1D simplex (a line)
    if (dest.numContacts < ContactManifold3D::k_maxContacts)
    {
      float distSQ = std::numeric_limits<float>::lowest();
      Contact3D *furthest = nullptr;
      uint8 furthestIndex = 0;
      for (uint8 i = 0; i < numValidContacts; ++i)
      {
        const float currDistSQ = (validContacts[i]->globalPositionA - deepest->globalPositionA).LengthSQ();
        if (currDistSQ > distSQ)
        {
          furthest = validContacts[i];
          distSQ = currDistSQ;
          furthestIndex = i;
        }
      }
      // move furthest contact to end of valid list
      std::swap(validContacts[furthestIndex], validContacts[--numValidContacts]);
      // add furthest contact
      dest.contacts[dest.numContacts++] = furthest;
    }

    // 3) expand line to a triangle
    if (   dest.numContacts == 2
        && dest.numContacts < ContactManifold3D::k_maxContacts
        && dest.numContacts < ContactManifold3D::k_maxContacts)
    {
      const Vec3 &linePos = dest.contacts[0]->globalPositionA;
      const Vec3  lineDir = (dest.contacts[1]->globalPositionA - dest.contacts[0]->globalPositionA).Normalized();
      Contact3D *furthest = nullptr;
      uint8 furthestIndex = 0;
      float distSQ = std::numeric_limits<float>::lowest();
      for (uint8 i = 0; i < numValidContacts; ++i)
      {
        // calculate distance from 1D simplex
        const Vec3 posDiff = validContacts[i]->globalPositionA - linePos;
        const float currDistSQ = (posDiff - posDiff.ProjectedUnit(lineDir)).LengthSQ();
        if (currDistSQ > distSQ)
        {
          furthest = validContacts[i];
          distSQ = currDistSQ;
          furthestIndex = i;
        }
      }
      // move furthest contact to end of valid list
      std::swap(validContacts[furthestIndex], validContacts[--numValidContacts]);
      // add furthest contact
      dest.contacts[dest.numContacts++] = furthest;
    }

    // 4) blow up manifold using furthest contact from 2D simplex (triangle)
    if (   dest.numContacts == 3
        && dest.numContacts < ContactManifold3D::k_maxContacts
        && dest.numContacts < ContactManifold3D::k_maxContacts)
    {
      const Vec3 &a = dest.contacts[0]->globalPositionA;
      const Vec3 &b = dest.contacts[1]->globalPositionA;
      const Vec3 &c = dest.contacts[2]->globalPositionA;
      Contact3D *furthest = nullptr;
      uint8 furthestIndex = 0;
      float distSQ = std::numeric_limits<float>::lowest();
      for (uint8 i = 0; i < numValidContacts; ++i)
      {
        // calculate distance from 1D simplex
        const Vec3 &p =  validContacts[i]->globalPositionA;
        const float currDistSQ = (ClosestPointTriangle(p, a, b, c) - p).LengthSQ();
        if (currDistSQ > distSQ)
        {
          furthest = validContacts[i];
          distSQ = currDistSQ;
          furthestIndex = i;
        }
      }
      // move furthest contact to end of valid list
      std::swap(validContacts[furthestIndex], validContacts[--numValidContacts]);
      // add furthest contact
      dest.contacts[dest.numContacts++] = furthest;
    }

    // 5) dispose all remaining valid contacts
    for (uint8 i = 0; i < numValidContacts; ++i)
    {
      // dispatch end contact event for persistent contacts
      if (m_listener && validContacts[i]->persistent)
        m_listener->EndContact(colliderA, colliderB, *validContacts[i]);

      validContacts[i]->~Contact3D();
      m_contactAllocator.Free(validContacts[i]);
    }
  }

  // dispatch begin begin contact event for new contacts
  if (m_listener)
    for (uint8 i = 0; i < dest.numContacts; ++i)
      if (!dest.contacts[i]->persistent)
        m_listener->BeginContact(colliderA, colliderB, *dest.contacts[i]);

  // clean up source manifold
  for (uint8 i = 0; i < ContactManifold3D::k_maxContacts; ++i)
    src.contacts[i] = nullptr;
  src.numContacts = 0;
}