3D sphere vs sphere collision resolve problem - c++

Question

I have been watching OneLoneCoder video about circle collision resolve
and use the code from git.
But the code is for 2D case, i.e. circles, and I need a solution for spheres. So I tried to add Z-component to equations but getting failures.
So I am testing the next way: I create two spheres, one above other. Lower is situated a heavy sphere, upper a tiny. The higher sphere is falling. In reality tiny should bounce up, but it just stuck, and they are falling as a single body for a while, but then the higher sphere is bouncing up. Also the sphere per each jump gets higher(see gif):
Here is the code:
/*
vertex - VERTEX **vertex=new VERTEX*[100];
vc - vertex count (how many vertices are currently created)
*/
void Update()
{
auto DoCirclesOverlap = [](float x1, float y1, float z1, float r1, float x2, float y2, float z2, float r2)
{
return ((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)) <= (r1 + r2)*(r1 + r2);
};
vector<pair<VERTEX*, VERTEX*>> vecCollidingPairs;
// Update vertex[i] Positions
for (int i = 0;i < vc;i++)
{
// Update vertex[i] physics
vertex[i]->x += vertex[i]->vx * TIME_QUANT_CONSTANT;
vertex[i]->y += vertex[i]->vy * TIME_QUANT_CONSTANT;
vertex[i]->z += vertex[i]->vz * TIME_QUANT_CONSTANT;
}
// Static collisions, i.e. overlap
for (int i(0);i < vc;i++)
{
for(int j(0);j < vc;j++)
{
if (vertex[i]->index != vertex[j]->index)
{
if (DoCirclesOverlap(vertex[i]->x, vertex[i]->y, vertex[i]->z, vertex[i]->size, vertex[j]->x, vertex[j]->y, vertex[j]->z, vertex[j]->size))
{
// Collision has occured
vecCollidingPairs.push_back({ vertex[i], vertex[j] });
// Distance between vertex[i] centers
float fDistance = sqrtf((vertex[i]->x - vertex[j]->x)*(vertex[i]->x - vertex[j]->x) + (vertex[i]->y - vertex[j]->y)*(vertex[i]->y - vertex[j]->y) + (vertex[i]->y - vertex[j]->y)*(vertex[i]->y - vertex[j]->y));
// Calculate displacement required
float fOverlap = 0.5f * (fDistance - vertex[i]->size - vertex[j]->size);
// Displace Current vertex[i] away from collision
vertex[i]->x -= fOverlap * (vertex[i]->x - vertex[j]->x) / fDistance;
vertex[i]->y -= fOverlap * (vertex[i]->y - vertex[j]->y) / fDistance;
vertex[i]->z -= fOverlap * (vertex[i]->z - vertex[j]->z) / fDistance;
// Displace vertex[j] vertex[i] away from collision
vertex[j]->x += fOverlap * (vertex[i]->x - vertex[j]->x) / fDistance;
vertex[j]->y += fOverlap * (vertex[i]->y - vertex[j]->y) / fDistance;
vertex[j]->z += fOverlap * (vertex[i]->z - vertex[j]->z) / fDistance;
}
}
}
}
// Now work out dynamic collisions
for (auto c : vecCollidingPairs)
{
VERTEX *b1 = c.first;
VERTEX *b2 = c.second;
// Distance between vertex[i]s
float fDistance = sqrtf((b1->x - b2->x)*(b1->x - b2->x) + (b1->y - b2->y)*(b1->y - b2->y) + (b1->z - b2->z)*(b1->z - b2->z));
// Normal
float nx = (b2->x - b1->x) / fDistance;
float ny = (b2->y - b1->y) / fDistance;
float nz = (b2->z - b1->z) / fDistance;
// Tangent
float tx = -ny;
float ty = nx;
float tz = nz;
// Dot Product Tangent
float dpTan1 = b1->vx * tx + b1->vy * ty + b1->vz * tz;
float dpTan2 = b2->vx * tx + b2->vy * ty + b2->vz * tz;
// Dot Product Normal
float dpNorm1 = b1->vx * nx + b1->vy * ny + b1->vz * nz;
float dpNorm2 = b2->vx * nx + b2->vy * ny + b2->vz * nz;
// Conservation of momentum in 1D
float m1 = (dpNorm1 * (b1->mass - b2->mass) + 2.0f * b2->mass * dpNorm2) / (b1->mass + b2->mass);
float m2 = (dpNorm2 * (b2->mass - b1->mass) + 2.0f * b1->mass * dpNorm1) / (b1->mass + b2->mass);
// Update ball velocities
b1->vx = tx * dpTan1 + nx * m1;
b1->vy = ty * dpTan1 + ny * m1;
b1->vz = tz * dpTan1 + nz * m1;
b2->vx = tx * dpTan2 + nx * m2;
b2->vy = ty * dpTan2 + ny * m2;
b2->vz = tz * dpTan2 + nz * m2;
}
}
What is the problem?

Answer 1

Thanks for #Tyyppi_77 from Stack GameDev, where I posted the same question, that pointed that the issue was on line
float fDistance = sqrtf((vertex[i]->x - vertex[j]->x)*(vertex[i]->x - vertex[j]->x) + (vertex[i]->y - vertex[j]->y)*(vertex[i]->y - vertex[j]->y) + (vertex[i]->y - vertex[j]->y)*(vertex[i]->y - vertex[j]->y));
And the correct one is
float fDistance = sqrtf((vertex[i]->x - vertex[j]->x)*(vertex[i]->x - vertex[j]->x) + (vertex[i]->y - vertex[j]->y)*(vertex[i]->y - vertex[j]->y) + (vertex[i]->z - vertex[j]->z)*(vertex[i]->z - vertex[j]->z));
It just solved almost all of issues. The only terrible thing is, because of static collision, the lower and much more heavier sphere is a little bit shifts down while it is hit by the sphere above. Do You know how it can be solved?