🔒 Locked

Problem with impulse based collisions response

Started by Black-Panther Feb 25, 2008 at 5:37 AM 9 replies 1.7k views

Original Post

Feb 25, 2008 at 5:37 AM

Hi all! I'm currently working on a physics engine. Now I just implemented the collision response system based on impulses. But there is a problem within that, which I couldn't solve the last days. The problem is as follows. It seams that collision response for colliding contact works fine (but I'm even not sure about that!). Always when a object should lie stilly on a static one, such as a floor, it begins to jump arround. BUT not only a little bit, which would be normal, because of the microimpulses, no, the amplitudes are really big. For collisiondetection and determination I use a SAT. It seems to me the contact set is computed correctly. For clearity purposes I've uploaded the actual state of my project, so you can see the problem by your self. The strange thing is, that for only one, or two objects the collision response works fine, but for eg 7 models not anymore. Here is the downloadlink for the program: Download Press F1 to get a list of all features (eg F7 for gravitation!). I uploaded two models as well: test4 (7 Objects) and test5 (3 objects). I hope someone can give mi a hint, where to find the problem. I'm really despaired by now. I hope you could understand what I intended to say. And now, the collision response code and the configuration vars:


//Gibt an ob ein linksorientierter (D3D) Vektorraum oder ein rechtsorientierter (OGL) verwendet wird
const bool				c_bLeftOrientedCoordSystem				= true;
//Soll zur Kollisionserkennung ein Kollisionsvorhersagesystem verwendet werden?
const bool				c_bUseCollisionPrediction				= true;
//Gibt die maximale Anzahl der globalen Kräfte pro Frame an
const unsigned int		c_iMaxGlobalForces						= 100;
//Entscheidet, ab welcher Anzahl von Vertizes der HillClimbing-Query
//dem linearen BruteForce vorgezogen werden soll
//Varname steht für BreakEvenPointForExtremeVertexQuery
const unsigned int		c_iBEPForExtremeVertexQuery				= 40;
//Gibt die allg. zu verwendende FloatingPoint Toleranz an
const float				c_fFloatingPointTolerance				= 0.0001f;
//Wenn die relative Geschwindigkeit zweier Kontaktpunkte in Normalrichtung betragsmäßig kleiner ist
//als dieser Wert, handelt es sich um einen Resting-Kontakt!
const float				c_fRestingContactTolerance				= sqrtf(19.62f * c_fFloatingPointTolerance);
//Gibt den Standard-Elastizitätsfaktor an
const float				c_fStdElasticyCoeff						= 0.8f;
//Toleranzwert für welchen eine Rückstoßgeschwindigkeit als erreicht betrachtet wird
const float				c_fReimpulsiveVelocityTolerance			= 0.0001f;
//Maximale Anzahl der Iterationen die verwendet werden dürfen um alle Kollisions aufzulösen! 
const unsigned int		c_iMaxCollisionImpulsIterations			= 5000;
//Maximale Anzahl der Iterationen die verwendet werden dürfen um alle RestingContacts aufzulösen! 
const unsigned int		c_iMaxRestingContactIterations			= 5000;
//Toleranzwert, ab welchem eine Gelenksbedingung als aufgelöst gilt
const float				c_fJointTolerance						= 0.0001f;
//Standardwert für Gleitreibungskoeffizient
const float				c_fStdDynamicFrictionCoeff				= 0.01f;
//Standardwert für Haftreibungskoeffizient
const float				c_fStdStaticFrictionCoeff				= 0.02f;


const bool ogpCollisionResponse::DoCollisionResponse(ogpContactList* pFirstContactPoint, const UINT iCPCount, 
													 const float fStepSize)
{
	if(iCPCount <= 0) return true;
	//Falls nötig Arrays vergrößern!
	if(iCPCount > m_iTempInfoSize)
	{
		m_iTempInfoSize = iCPCount;
		if(m_pTempInfo) OG_SAFE_DELETE_ARRAY(m_pTempInfo);
		m_pTempInfo = new STempInfos[m_iTempInfoSize];
	}
	if(iCPCount > m_iContactArraySize)
	{
		m_iContactArraySize = iCPCount;
		if(m_pContactArray) OG_SAFE_DELETE_ARRAY(m_pContactArray);
		m_pContactArray = new ogpContact[m_iContactArraySize];
	}

	//Bleibende und kollidierende Kontakte trennen. Kollidierende am Anfang und bleibende am Ende des Arrays
	ogpContactList* pContact = pFirstContactPoint;
	UINT iCCCount = 0;
	UINT iRCCount = 0;
	for(UINT i = 0; i < iCPCount; ++i)
	{
		const og3DVector urel	= pContact->Contact.pA->CalcPointVelocity(pContact->Contact.vPoint) - 
								  pContact->Contact.pB->CalcPointVelocity(pContact->Contact.vPoint);
		const float fUrelDotN	= og3DVectorDot(urel, pContact->Contact.vNormal);
		//Colliding oder RestingContact?
		if(fabsf(fUrelDotN) > c_fRestingContactTolerance) m_pContactArray[iCCCount++] = pContact->Contact;
		else m_pContactArray[iCPCount - (++iRCCount)] = pContact->Contact;
		pContact = pContact->pNext;
	}

	//Kollisionsauflösung für kollidierende Kontakte
	if(iCCCount > 0)
	{
		if(!DoCRForCollidingContact(m_pContactArray, iCCCount)) 
			OG_WARNING(L"CollidingContact-Auflösung wurde nicht ordnungsgemäß beendet!");
	}

	//Kollisionsauflösung für bleibende Kontakte
	if(iRCCount > 0)
	{
		if(!DoCRForRestingContact(m_pContactArray + iCCCount, iRCCount, fStepSize)) 
			OG_WARNING(L"RestingContact-Auflösung wurde nicht ordnungsgemäß beendet!");
	}

	return true;
}

//-----------------------------------------------------------------//

const bool ogpCollisionResponse::DoCRForCollidingContact(ogpContact* pContacts, const UINT iCPCount)
{
	//Folgende Daten müssen nur einmal berechnet werden!
	for(UINT i = 0; i < iCPCount; ++i)
	{
		//Relative Geschwindkeit berechnen
		const og3DVector urel	= pContacts.pA->CalcPointVelocity(pContacts.vPoint) -
								  pContacts.pB->CalcPointVelocity(pContacts.vPoint);
		const og3DVector vUrel	= og3DVectorDot(urel, pContacts.vNormal) * pContacts.vNormal;

		m_pTempInfo.fElasticity = pContacts.pA->GetElasticyCoeff() * pContacts.pB->GetElasticyCoeff();
		m_pTempInfo.vTargetUrel = -m_pTempInfo.fElasticity * vUrel;
		m_pTempInfo.K = pContacts.pA->CalcKMatrix(pContacts.vPoint, pContacts.vPoint) +
						   pContacts.pB->CalcKMatrix(pContacts.vPoint, pContacts.vPoint);

		//Achse in Normalrichtung schauen lassen
		if(og3DVectorDot(pContacts.vNormal, pContacts.vAxis) < 0.f) pContacts.vAxis = -pContacts.vAxis;
	}

	//iOk speichert die Anzahl der Kontaktpunkte, welche erfolgreich aufgelöst wurden!
	//Bei einer Impulsanwendung wird iOk auf 1 gesetzt!
	UINT			iOk					= 0;
	UINT			iIndex				= 0;
	UINT			iIterations			= 0;
	og3DVector		vImpulsSum			= 0.f;
	while(iOk++ < iCPCount)
	{
		//Relative Geschwindigkeit der beiden Kontaktpunkte berechnen!
		const og3DVector urel	= pContacts[iIndex].pA->CalcPointVelocity(pContacts[iIndex].vPoint) - 
								  pContacts[iIndex].pB->CalcPointVelocity(pContacts[iIndex].vPoint);
		const float fUrelDotN	= og3DVectorDot(urel, pContacts[iIndex].vNormal);
		const og3DVector vUrel	= fUrelDotN * pContacts[iIndex].vNormal;

		//1. Bedingung: Gesamtimpuls gleich Null, und Zielgeschwindigkeit überschritten --> Erfolgreiche Kollisionsauflösung
		if(vImpulsSum != og3DVector(0.f) || fUrelDotN <= og3DVectorDot(m_pTempInfo[iIndex].vTargetUrel, pContacts[iIndex].vNormal))
		{
			const og3DVector vDeltaUrel = m_pTempInfo[iIndex].vTargetUrel - vUrel;
			const float fDotDeltaUrel   = og3DVectorDot(vDeltaUrel, pContacts[iIndex].vNormal);
			//2. Bedingung: Zielgeschwindigkeit bereits erreicht!
			if(fDotDeltaUrel > c_fReimpulsiveVelocityTolerance)
			{
				//Abstoßenden Impuls berechnen und anwenden
				const float fVectorMatrixProduct = ogUtilsMatrixVectorProduct(pContacts[iIndex].vNormal, m_pTempInfo[iIndex].K);
				og3DVector p = vDeltaUrel;
				if(fabsf(fVectorMatrixProduct) > c_fFloatingPointTolerance) p /= fVectorMatrixProduct;
				pContacts[iIndex].pA->ApplyImpuls(p, pContacts[iIndex].vPoint);
				pContacts[iIndex].pB->ApplyImpuls(-p, pContacts[iIndex].vPoint);
				vImpulsSum += p;
				iOk = 1;
			}
			else if(fDotDeltaUrel < -c_fReimpulsiveVelocityTolerance && 
					og3DVectorDot(vImpulsSum, pContacts[iIndex].vNormal) > -c_fFloatingPointTolerance)
			{
				//Anziehenden KorrekturImpuls berechnen...
				const float fVectorMatrixProduct = ogUtilsMatrixVectorProduct(pContacts[iIndex].vNormal, m_pTempInfo[iIndex].K);
				og3DVector p = vDeltaUrel;
				if(fabsf(fVectorMatrixProduct) > c_fFloatingPointTolerance) p /= fVectorMatrixProduct;
				if(og3DVectorDot(vImpulsSum + p, pContacts[iIndex].vNormal) < -c_fFloatingPointTolerance)
				{
					//Impuls zu stark --> maximal gültigen anwenden!
					p = -vImpulsSum;
				}
				//... und anwenden
				pContacts[iIndex].pA->ApplyImpuls(p, pContacts[iIndex].vPoint);
				pContacts[iIndex].pB->ApplyImpuls(-p, pContacts[iIndex].vPoint);
				vImpulsSum += p;
				iOk = 1;
			}
		}

		//Maximale Anzahl an Iterationen erreicht? --> Abbruch!
		if(++iIterations > c_iMaxCollisionImpulsIterations) return false;
		//Kontaktpunkte werden wie ein Ringbuffer immer wieder durchlaufen!
		if(++iIndex >= iCPCount) iIndex = 0;
	}

	return true;
}

//-----------------------------------------------------------------//

//ANMERKUNG: fStepSize soll den künftigen Zeitschritt approximieren. Es
//ist der aktuelle Zeitschritt und es wird davon ausgegangen, dass sich
//die Schrittweite nicht groß ändert! (optimale Lösung --> FrameBasedRendering)
const bool ogpCollisionResponse::DoCRForRestingContact(ogpContact* pContacts, const UINT iCPCount, 
													   const float fStepSize)
{
	//Folgende Daten müssen nur einmal berechnet werden!
	for(UINT i = 0; i < iCPCount; ++i)
	{
		m_pTempInfo.K = pContacts.pA->CalcKMatrix(pContacts.vPoint, pContacts.vPoint) +
						   pContacts.pB->CalcKMatrix(pContacts.vPoint, pContacts.vPoint);
		//Achse in Normalrichtung schauen lassen
		if(og3DVectorDot(pContacts.vNormal, pContacts.vAxis) < 0.f) pContacts.vAxis = -pContacts.vAxis;
	}

	//iOk speichert die Anzahl der Kontaktpunkte, welche erfolgreich aufgelöst wurden!
	UINT			iOk					= 0;
	UINT			iIndex				= 0;
	UINT			iIterations			= 0;
	og3DVector		vImpulsSum			= 0.f;
	while(iOk++ < iCPCount)
	{
		//Wenn Körper ineinander driften, dann Zeit Geschwindigkeit in Normalrichtung
		const og3DVector vrel	= pContacts[iIndex].pB->GetVelocity() - pContacts[iIndex].pA->GetVelocity();
		//Positiv, falls Körper ineinander driften
		const float fVn			= og3DVectorDot(vrel, pContacts[iIndex].vAxis);
		//N * d < 0, falls sich Körper schneiden!
		const og3DVector d		= (pContacts[iIndex].fDistance - fVn * fStepSize) * pContacts[iIndex].vAxis;
		//Positiv, wenn Körper sich überlappen
		const float fdDotN		= og3DVectorDot(-d, pContacts[iIndex].vNormal);

		//1. Bedingung: GesamtImpuls gleich Null, und Abstand in Normalenrichtung positiv!
		if(vImpulsSum != og3DVector(0.f) || fdDotN > -c_fJointTolerance)
		{
			//2. Bedingung: Abgstand klein genug, dass als erfüllt angesehen werden kann
			if(fabsf(fdDotN) > c_fJointTolerance)
			{
				//Positions-Korrektur-Impuls berechnen
				const float fVectorMatrixProduct = ogUtilsMatrixVectorProduct(pContacts[iIndex].vNormal, m_pTempInfo[iIndex].K);
				og3DVector p = (fdDotN * pContacts[iIndex].vNormal) / (fVectorMatrixProduct * fStepSize);
				if(fdDotN < 0.f && og3DVectorDot(vImpulsSum + p, pContacts[iIndex].vNormal) < -c_fFloatingPointTolerance)
				{
					//Impuls zu stark --> maximal gültigen anwenden!
					p = -vImpulsSum;
				}
				//und anweden!
				pContacts[iIndex].pA->ApplyImpuls(p, pContacts[iIndex].vPoint);
				pContacts[iIndex].pB->ApplyImpuls(-p, pContacts[iIndex].vPoint);

				vImpulsSum += p;
				iOk = 1;
			}
		}

		if(++iIterations > c_iMaxRestingContactIterations) return false;
		if(++iIndex >= iCPCount) iIndex = 0;
	}

	return true;
}

manonoc

Feb 27, 2008 at 9:08 AM

I can't test your demo.

Give me a demo like mine:
http://escetlp.free.fr/moteur3D_physique.zip

that shows you what's happens

Black-Panther

Feb 27, 2008 at 2:48 PM

Sorry, I don't understand why you can't test my demo. Doesn't it work? I've tested your's, but still, I don't understand, what do you mean...?

manonoc

Feb 28, 2008 at 3:43 AM

yes, sorry i wasn't clear:

the only exe i can find in your tar, is a DialogBox of directx configuration.
I don't think it is your engine.

Black-Panther

Feb 28, 2008 at 7:48 AM

Ahh! It's a program which uses my engine! After making your configurations for D3D (in the dialog-box), the program starts, and there you can try out my physic engine.

manonoc

Feb 28, 2008 at 7:54 AM

yes,, i've tried before, but nothing has appeared, it seems to be an error.

OmegaEngine Logdatei
[Release-Version, (c) Julian Mautner (julian@mautner.it)]
Typ Beschreibung Datei:Zeile Funktion
INFO: Engine erfolgreich initialisiert omega.cpp:96 ogInit
INFO: Konfigurationsfenster wird aufgerufen ogConfig.cpp:859 ogShowConfigDlg
INFO: Konfigurationsdialog wird verlassen ogConfig.cpp:868 ogShowConfigDlg
INFO: Fenster wurde erfolgreich erstellt omega.cpp:431 ogInitWindow
INFO: Direct3DDevice-Schnittstelle erfolgreich erstellt! ogDirect3D.cpp:100 ogDirect3D::Init
WARNUNG: Direct3D wurde nicht heruntergefahren, wird automatisch erledigt... omega.cpp:289 ogExit
INFO: Direct3D wurde heruntergefahren ogDirect3D.cpp:170 ogDirect3D::Exit
INFO: Engine erfolgreich heruntergefahren! omega.cpp:297 ogExit

Dirk Gregorius

Feb 28, 2008 at 8:50 AM

The program doesn't start. There are problem with the chosen pixelformat.

Maybe it is better to describe how you compute the contact forces/impulses? Did you use any reference, e.g. the paper of Erin Catto? How do you build the contact geometry?

Black-Panther

Feb 28, 2008 at 4:18 PM

I used the work of Jan Bender as motivation.
Correction impulses are computed in an iterative manner, until all collisions are solved correctly. I differ between colliding and resting contacts, after the condition of Mirtich.
For colliding contacts I first compute the relative velocity of two collisionpoints and then the target relative velocity after newton. Then in the loop I compute for each contactpoint-pair the impluse to reach the target velocity and directly apply it to the two involved bodies. Then the same thing is done with the next collisionpair. Until all pairs have reached their target velocity.
For resting contacts I compute the interpenetration-depth and then I calculate and apply an impulse that changes the relative velocity so that next timestep the interpenetraition depth becomes zero.

I think I've found the problem. It's the varing timestep-size. Sometimes the frametime changes too much, and causes the interpenetration-depth-calculation to do an error which is too big. Then the computed correction impulse is to big and the bodies jump arround.
Reasonable?

EDIT:
I think I found anonther problem too in the algorithm of contact-geometry computation. In the instant of collision it works fine. But when two object are already interpenetrating, the computed contactpoints are sh**. How do you solve this problem? (I use a SAT!)

MrRowl

Feb 28, 2008 at 6:10 PM

Quote:
Original post by Black-Panther
For resting contacts I compute the interpenetration-depth and then I calculate and apply an impulse that changes the relative velocity so that next timestep the interpenetraition depth becomes zero.

One thing (probably unrelated to any existing problem, but worth doing) - use the penetration depth (or an estimate) at the beginning of the collision sweep, not at the end (it's not clear which you're doing). This way you won't have a constant non-zero error correction term when objects are supposed to be sitting at rest.

Black-Panther

Feb 29, 2008 at 5:56 AM

I don't know if I understood right. Instead of using the penetraition depth (an estimated one) of the next frame, I should use that of the current one? So doing no calculations about how the penetraition depth in/decreases in the next time-step?

MrRowl

Feb 29, 2008 at 6:16 AM

Quote:
Original post by Black-Panther
I don't know if I understood right. Instead of using the penetraition depth (an estimated one) of the next frame, I should use that of the current one? So doing no calculations about how the penetraition depth in/decreases in the next time-step?

Yes - that's what I'm suggesting. Imagine a particle dropping down vertically onto a plane, falling at a speed of 10 m/s. The collision will be inelastic. Your timestep is 0.1 sec. At the beginning of your step (time t0) it is 0.1 m above the plane.

When you do your collision detection you could be doing it different ways - using the positions of objects at the start of the step, using the estimated positions at the end of the step, or a sweep from the current positions to the estimated positions. Let's say you sweep from 0.1m above to 0.9m below the plane.

When you process the collision/contact you solve to get the impulse that will give you a final vertical velocity of 0 (zero restitution). However, you need to tweak this zero value to resolve penetration. Lets say you always try to resolve the penetration over one timestep (really you'd probably be more gentle than this).

So - if you use the penetration depth at the end of the collision sweep (0.9 m) you'll solve to get a vertical velocity of 0.9/0.1 = 9 m/s - i.e. your particle will bounce a LOT!

If you use the penetration depth at the start (-0.1 m) then you can either:

1. Ignore it, since it's negative. Then your particle will stop just short of the plane, and then drop down gently onto it (probably stopping a few more times on the way). Much better but not perfect.

2. Use it even though it is negative. Now you solve to get a vertical velocity of -0.1 / 0.1 = -1 m/s. This means that after the step the particle will be exactly touching the plane. On the next step the downwards velocity will be set to 0 by a perfectly inelastic collision (with zero bias since the initial penetration will be 0).

I hope this makes sense. Whether or not it's related to your implementation depends on a few other things - not checked your code snippet...

Topic Locked

This topic has been locked by a moderator. New replies are not allowed.