Rigid body box stacking

Please don't post Bullet support questions here, use the above forums instead.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Rigid body box stacking

Post by Dirk Gregorius »

You need two friction axes. Imagine you apply an impulse orthogonal to t1 + t2 inside the contact plane. The object would have no resistance and could move freely. Just keep your friction basis constant over a frame and also don't recompute the effective friction mass and you should be fine! :)
RandyGaul
Posts: 43
Joined: Mon May 20, 2013 8:01 am
Location: Redmond, WA

Re: Rigid body box stacking

Post by RandyGaul »

Dirk Gregorius wrote:I have tested this in real production code that actually shipped and trust me it is a real world problem. If you want to push an object from A to B in a game and it moves on an arc rather than a straight line this will be indeed a problem. If objects slide on planes and actually also rotate around the up-axis that is a problem. Sorry student projects and personal testing don't count! :) You also might want to come up with better test cases!
Cool thanks a lot for the perspective Dirk, I appreciate it :)
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Thanks Dirk, none of that seems to make a difference. I debugged it thoroughly with a simple case (box falling straight on a plane without friction) and found that the accumulated impulses from the four contact points come out as 1.108, 0.665, 1.108, 0.665 on the contacts diagonal to each other. Do you expect that the accumulated impulses should be same at all points, from a very small drop height? These are the accumulated impulses after 10 iterations, however the impulses at all of the points converge to zero.

Warm starting with those uneven accumulated impulses seems wrong.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Ok I think I found the problem, it's the clipping algorithm epsilon. I use Sutherland-Hodgman and it returns 4 contacts, however when I clip them against the face plane, sometimes it retains 3 contacts, which is most likely what causes the box to wobble. The impulse solver is fine as it converges, then when there's 3 contact points, it starts up with the bouncing again. In Box2D it's easier for this not to happen because there's a maximum of only 2 contact points to deal with.

I used the smallest penetration depth from SAT for all contacts, retaining all of the points returned from the clipping algorithm and the stack is stable, but the boxes look unevenly stacked as expected.

I am not sure if this is a common problem, any ideas for a solution? The clipping of the line segment to a plane or the clipping points to the face plane epsilon need stability but I am not sure if this is simply achieved by thicker planes, as inevitably clipping against the face plane will remove contacts.
RandyGaul
Posts: 43
Joined: Mon May 20, 2013 8:01 am
Location: Redmond, WA

Re: Rigid body box stacking

Post by RandyGaul »

c0der wrote:Ok I think I found the problem, it's the clipping algorithm epsilon. I use Sutherland-Hodgman and it returns 4 contacts, however when I clip them against the face plane, sometimes it retains 3 contacts, which is most likely what causes the box to wobble. The impulse solver is fine as it converges, then when there's 3 contact points, it starts up with the bouncing again. In Box2D it's easier for this not to happen because there's a maximum of only 2 contact points to deal with.

I used the smallest penetration depth from SAT for all contacts, retaining all of the points returned from the clipping algorithm and the stack is stable, but the boxes look unevenly stacked as expected.

I am not sure if this is a common problem, any ideas for a solution? The clipping of the line segment to a plane or the clipping points to the face plane epsilon need stability but I am not sure if this is simply achieved by thicker planes, as inevitably clipping against the face plane will remove contacts.
I'm assuming you're using sutherland hodgman clipping? This is the routine I always hear people recommend. There's a robust implementation in Ericson's Orange Book. The idea of making the clipping routine robust is to have 3 cases of line segment to plane for each point: in front, behind and on the plane. With these three cases I've found the most robust clipping implementation for myself thus far.

You shouldn't need too big of an epsilon, however you do need to make sure that you have a penetration slop for contact resolution that is upheld (you don't want the clipping epsilon higher than the penetration slop, or clipping will lower your frame coherence).
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Rigid body box stacking

Post by Dirk Gregorius »

From the SAT you obtain the axis of minimum penetration. This defines your reference face. Now you find the most anti-parallel face on the other body. Next you clip the incident face against the side planes of the reference plane and finally keep the points below the reference face. The penetration distance for each point is the distance of that point to the reference plane and not the distance returned from SAT.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Dirk, that's exactly what I did. The step where I discard contacts that are above the reference face causes a loss of contact point, which causes the instability.

Randy, if I have those 3 cases, I am discarding points infront of the face plane (outside the OBB) technically, so the only valid case will be "behind the plane". How do your contacts look? Do you always get 4 if it's a straight face on face or the occasional 3 or 1 contact?

Attached the most stable case, where the clipping algorithm for flat face on face returns some higher points than others, maybe due to the initial impulse accumulation being different at 2 contacts. Do you expect to get the same accumulated impulse at the four contacts to warmstart with?
Attachments
Engine.png
Engine.png (664.23 KiB) Viewed 23101 times
RandyGaul
Posts: 43
Joined: Mon May 20, 2013 8:01 am
Location: Redmond, WA

Re: Rigid body box stacking

Post by RandyGaul »

I will have 4 to 5 contacts if boxes are resting on each other, usually 4. With sutherland-hodgman the algorithm should not reject any points that you need. If you like I can type out the various cases in psuedo code if I have some spare time tomorrow.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Thanks alot. I reverted back to what Dirk said, which is the correct way I had before. What's really weird is that when I have 10 iterations, a stack of 5 is almost instantly stable, but when I increase iterations to 15, energy gets introduced into the system. Something seriously wrong here. I don't really think it's the clipping anymore because I toggled a breakpoint when the number of contacts become 1, and this happens after a bit of jiggling among the boxes causes them to be uneven. It's just some really weird problem that makes no logical sense.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Ran another test, I zero'd the velocities (6 box stack), then unzero'd them and they started up again. So I disabled the bias and the impulses converged at 10 iterations.

Another test, disabled friction, stepped through, and the bias & penetration depths were uniform on all 4 contacts for the first few frames until the boxes slipped off each other. It now appears that the problem lies in the friction computation, maybe a creeping error in the alignment of the tangent vectors, will investigate further.

Here is my code to compute the two tangents from the normal vector:

Code: Select all

	if (fabsf(x)>=fabsf(y))
	{
		// x or z is the largest magnitude component, swap them
		AMG3DScalar sInvLength = 1 / (x*x + z*z);
		(*pTangent1).x = -z * sInvLength;
		(*pTangent1).y = 0.0f;
		(*pTangent1).z = x * sInvLength;
		(*pTangent2).x = y * (*pTangent1).z;
		(*pTangent2).y = z*(*pTangent1).x - x*(*pTangent1).z;
		(*pTangent2).z = -y * (*pTangent1).x;
	}
	else
	{
		// y or z is the largest magnitude component, swap them
		AMG3DScalar sInvLength = 1 / (y*y + z*z);
		(*pTangent1).x = 0.0f;
		(*pTangent1).y = z * sInvLength;
		(*pTangent1).z = -y * sInvLength;
		(*pTangent2).x = y*(*pTangent1).z - z*(*pTangent1).y;
		(*pTangent2).y = -x * (*pTangent1).z;
		(*pTangent2).z = x * (*pTangent1).y;
	}
RandyGaul
Posts: 43
Joined: Mon May 20, 2013 8:01 am
Location: Redmond, WA

Re: Rigid body box stacking

Post by RandyGaul »

At a glance the idea of what you have looks okay. Try swapping it out with a little bit of my code and see if you notice a difference, maybe this can help you isolate your bug:

Code: Select all

inline void GenerateOrthonormalBasis( const Vec3& w, Vec3 *u, Vec3 *v )
{
  real absX = std::abs( w.x );
  if(absX > std::abs( w.y ) &&
     absX > std::abs( w.z ))
  {
    u->x = -w.y;
    u->y = w.x;
    u->z = 0.0f;
  }
  else
  {
    u->x = 0.0f;
    u->y = w.z;
    u->z = -w.y;
  }

  u->NormalizeThis( );
  *v = Cross( w, *u );
  v->NormalizeThis( );
}
Edit: And then Erin posted something better:
http://box2d.org/2014/02/computing-a-basis/
Last edited by RandyGaul on Tue Feb 04, 2014 10:58 pm, edited 1 time in total.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Thanks a lot Randy, it still behaves the same way. Is this the approach you follow (see code)? I will attach the impulse code after as it's long.

Code: Select all

void AMG3DRigidBodyCollisionResponder::computeMasses(const AMG3DScalar& dt)
{
	if(m_bSkipCollision)
		return;

	if(dt<=0.0f)
		return;
	
	const AMG3DScalar k_allowedPenetration = 0.01f;
	AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_bPositionCorrection) ? 0.1f : 0.0f;

	for(int i=0; i<m_cdContactData.iNumContacts; ++i) {
		AMG3DVector4 normal = m_cdContactData.contacts[i].vContactNormal;
		
		m_rap[i] = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody1->m_vPosition;
		m_rbp[i] = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody2->m_vPosition;

		m_rapcrossn[i] = m_rap[i].cross(normal);
		m_rbpcrossn[i] = m_rbp[i].cross(normal);

		// Compute the normal mass
		AMG3DScalar Kn = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
		Kn += ( m_pRigidBody1->m_matInvIWorld * m_rapcrossn[i].cross(m_rap[i]) + 
				m_pRigidBody2->m_matInvIWorld*m_rbpcrossn[i].cross(m_rbp[i]) ).dot(normal);
		m_cdContactData.contacts[i].massNormal = 1.0f/Kn;

		// Compute the bias (prevents sinking, useful for stacking)
		m_cdContactData.contacts[i].bias = k_biasFactor / dt * 
							AMG3D_MATH_MAX(0.0f, m_cdContactData.contacts[i].sPenetrationDepth - k_allowedPenetration);

		// Calculate the tangent vector. To Do: Research relative velocity tangent vector
		AMG3DVector4 vap = m_pRigidBody1->m_vVelocity + m_pRigidBody1->m_vAngularVelocity.cross(m_rap[i]);
		AMG3DVector4 vbp = m_pRigidBody2->m_vVelocity + m_pRigidBody2->m_vAngularVelocity.cross(m_rbp[i]);

		// Relative velocity
		AMG3DVector4 vab = vap - vbp;

		AMG3DVector4 vabn = normal*(vab.dot(normal));
		AMG3DVector4 vabt = vab - vabn;

		//if(vabt.magnitude()>0) {
		//	vabt.normalize();
		//	m_t1 = vabt;
		//	AMG3DVector4Cross(&m_t2, normal, m_t1);
		//}
		//else {
			
			normal.buildOrthoBasis(&m_t1, &m_t2);
		//}

		m_rapcrosst1[i] = m_rap[i].cross(m_t1);
		m_rbpcrosst1[i] = m_rbp[i].cross(m_t1);

		// Compute the tangent mass in the first direction
		AMG3DScalar Kt1 = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
		Kt1 += ( m_pRigidBody1->m_matInvIWorld * m_rapcrosst1[i].cross(m_rap[i]) + 
				 m_pRigidBody2->m_matInvIWorld * m_rbpcrosst1[i].cross(m_rbp[i]) ).dot(m_t1);
		m_cdContactData.contacts[i].massTangent1 = 1.0f/Kt1;

		m_rapcrosst2[i] = m_rap[i].cross(m_t2);
		m_rbpcrosst2[i] = m_rbp[i].cross(m_t2);

		// Compute the tangent mass in the second direction
		AMG3DScalar Kt2 = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
		Kt2 += ( m_pRigidBody1->m_matInvIWorld * m_rapcrosst2[i].cross(m_rap[i]) + 
				 m_pRigidBody2->m_matInvIWorld * m_rbpcrosst2[i].cross(m_rbp[i]) ).dot(m_t2);
		m_cdContactData.contacts[i].massTangent2 = 1.0f/Kt2;
		
		if(AMG3DPhysicsWorld::g_AMG3D_bWarmStarting) {
			//AMG3DVector4 P = m_cdContactData.contacts[i].Pn*normal + m_cdContactData.contacts[i].Pt1*m_t1 + m_cdContactData.contacts[i].Pt2*m_t2;

			// Project old friction impulse onto new tangent basis
			AMG3DVector4 oldt1 = m_cdContactData.contacts[i].Pt1 * m_cdContactData.contacts[i].t1; 
			AMG3DVector4 oldt2 = m_cdContactData.contacts[i].Pt2 * m_cdContactData.contacts[i].t2;

			AMG3DVector4 P = m_cdContactData.contacts[i].Pn*normal + oldt1.dot(m_t1) * m_t1 + oldt2.dot(m_t2) * m_t2;

			// Store old tangent vectors to be used for next frame
			m_cdContactData.contacts[i].t1 = m_t1;
			m_cdContactData.contacts[i].t2 = m_t2;

			m_pRigidBody1->m_vVelocity			+= P*m_pRigidBody1->m_sInvMass;
			m_pRigidBody1->m_vAngularVelocity	+= m_pRigidBody1->m_matInvIWorld*m_rap[i].cross(P);
		
			m_pRigidBody2->m_vVelocity			-= P*m_pRigidBody2->m_sInvMass;
			m_pRigidBody2->m_vAngularVelocity	-= m_pRigidBody2->m_matInvIWorld*m_rbp[i].cross(P);
		}	
	}
}
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

I tried a simple case, box falling on another box of equal size and slightly off-center with each other, without friction. I also disabled the angular velocity update, so then the box is stable. Once I enable angular velocity update, the box keeps sliding and eventually falls off. I would expect that if the box is stationary, this shouldn't happen.

If I enable angular velocity, the error keeps creeping slowly over time, hence this could be a problem with my integrator. Here is my code, any suggestions?

Code: Select all

void AMG3DRBCuboid::updatePosition(const AMG3DScalar& dt)
{
	if(dt<=0.0f)
		return;

	// May want to update the velocity of a kinematic body
	// e.g. Camera/player interaction
//	if(m_sMass<=0.0f)
//		return;

	// Dynamic and kinematic bodies can move
	if(m_eType==AMG3D_PHYSICS_RIGID_BODY_TYPE_STATIC)
		return;

	if(m_eState==AMG3D_PHYSICS_RIGID_BODY_STATE_SLEEPING)
		return;

	m_vPosition += m_vVelocity*dt;
	
	m_quatOrientation.addScaledVector(m_vAngularVelocity, dt);
	m_quatOrientation.normalize();
	
	updateWorldTransformMatrix();

	m_OBB.transformBy(m_matWorldTransform);

	if(AMG3DPhysicsWorld::g_AMG3D_bAutoClearForces) {
		clearNetForce();
		clearNetTorque();
	}

	if(AMG3DPhysicsWorld::g_AMG3D_bEnableSleeping)
		updateSleepState(dt);
}
RandyGaul
Posts: 43
Joined: Mon May 20, 2013 8:01 am
Location: Redmond, WA

Re: Rigid body box stacking

Post by RandyGaul »

I'd need to see the functions

Code: Select all

m_quatOrientation.addScaledVector(m_vAngularVelocity, dt);
   m_quatOrientation.normalize();
Maybe we can look at your velocity update as well.

This all can also come from an incorrect calculation in radii during collision resolution. Maybe you could double check these terms specifically. I see you pasted this above but I don't have time to comb through it.

If it comes down to it maybe you can set up a very simple test case (like two boxes) and do the math by-hand and figure out what your impulses should be. If you can do this and step through your code you can easily see where the mistake(s) are in the first couple iterations.
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Rigid body box stacking

Post by c0der »

Thanks Randy, will do that tonight. Here is the rest of the code

Code: Select all

void AMG3DQuaternion::addScaledVector(const AMG3DVector4& v, const AMG3DScalar& scale)
{
	// To Do: Derive this
	AMG3DQuaternion q(0, v.x*scale, v.y*scale, v.z*scale);
	q *= *this;
	
	w += q.w * 0.5f;
	x += q.x * 0.5f;
	y += q.y * 0.5f;
	z += q.z * 0.5f;
}

Code: Select all

void AMG3DQuaternion::normalize()
{
	AMG3DScalar s = magnitude();
	
	if(s>0) {
		w /= s;
		x /= s;
		y /= s;
		z /= s;
	}
}

Code: Select all

AMG3DScalar AMG3DQuaternion::magnitude() const
{
	return sqrtf(w*w + x*x + y*y + z*z);
}

Code: Select all

void AMG3DRigidBody::integrateForces(const AMG3DScalar& dt)
{
	if(dt<=0.0f)
		return; 

	// Just check if the body is dynamic, zero mass body = zero acceleration anyway
	// Some forces can wake the body (user added forces), some can't (gravity), leave that to the force updater
//	if(m_sMass<=0.0f)
//		return;

	// Cannot update forces for kinematic or immovable bodies
	if(m_eType!=AMG3D_PHYSICS_RIGID_BODY_TYPE_DYNAMIC)
		return;

	// Body is sleeping, do not integrate forces
//	if(m_eState==AMG3D_PHYSICS_RIGID_BODY_STATE_SLEEPING)
//		return;

	m_vAcceleration			= m_vNetForce*m_sInvMass;
	m_vAngularAcceleration	= m_vNetTorque*m_matInvIWorld;

	m_vVelocity			+= m_vAcceleration*dt;
	m_vAngularVelocity	+= m_vAngularAcceleration*dt;
}
Post Reply