Example #1
0
 public static void PrintSolverConstraint(StreamWriter writer, SolverConstraint constraint, int index)
 {
     if (writer != null)
     {
         writer.WriteLine("SolverConstraint[{0}][{1}][{2}]", index, (String)constraint.m_solverBodyA.GetUserPointer(), (String)constraint.m_solverBodyB.GetUserPointer());
         MathUtil.PrintVector3(writer, "relPos1CrossNormal", constraint.m_relpos1CrossNormal);
         MathUtil.PrintVector3(writer, "contactNormal", constraint.m_contactNormal);
         MathUtil.PrintVector3(writer, "m_angularComponentA", constraint.m_angularComponentA);
         MathUtil.PrintVector3(writer, "m_angularComponentB", constraint.m_angularComponentB);
         writer.WriteLine("Friction [{0:0.00000000}] jagDiag[{1:0.00000000}] rhs[{2:0.00000000}] cfm[{3:0.00000000}] lower[{4:0.00000000}] upper[{5:0.00000000}] rhsPen[{6:0.00000000}]", constraint.m_friction, constraint.m_jacDiagABInv,
                          constraint.m_rhs, constraint.m_cfm, constraint.m_lowerLimit, constraint.m_lowerLimit, constraint.m_rhsPenetration);
     }
 }
		public static void PrintSolverConstraint(StreamWriter writer, SolverConstraint constraint, int index)
		{
			if (writer != null)
			{
				writer.WriteLine("SolverConstraint[{0}][{1}][{2}]", index,(String)constraint.m_solverBodyA.GetUserPointer(),(String)constraint.m_solverBodyB.GetUserPointer());
                MathUtil.PrintVector3(writer, "relPos1CrossNormal", constraint.m_relpos1CrossNormal);
                MathUtil.PrintVector3(writer, "contactNormal", constraint.m_contactNormal);
                MathUtil.PrintVector3(writer, "m_angularComponentA", constraint.m_angularComponentA);
                MathUtil.PrintVector3(writer, "m_angularComponentB", constraint.m_angularComponentB);
                writer.WriteLine("Friction [{0:0.00000000}] jagDiag[{1:0.00000000}] rhs[{2:0.00000000}] cfm[{3:0.00000000}] lower[{4:0.00000000}] upper[{5:0.00000000}] rhsPen[{6:0.00000000}]", constraint.m_friction, constraint.m_jacDiagABInv,
                    constraint.m_rhs, constraint.m_cfm, constraint.m_lowerLimit, constraint.m_lowerLimit, constraint.m_rhsPenetration);
			}
		}
		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
						ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
						CollisionObject colObj0, CollisionObject colObj1, float relaxation)
		{
			SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
		}
		public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
								ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
								CollisionObject colObj0, CollisionObject colObj1, float relaxation,
								float desiredVelocity, float cfmSlip)
		{
			RigidBody body0 = RigidBody.Upcast(colObj0);
			RigidBody body1 = RigidBody.Upcast(colObj1);

			solverConstraint.m_contactNormal = normalAxis;

			solverConstraint.m_solverBodyA = body0 != null ? body0 : GetFixedBody();
			solverConstraint.m_solverBodyB = body1 != null ? body1 : GetFixedBody();

			solverConstraint.m_friction = cp.GetCombinedFriction();
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && (body0 != null  || body1 != null) && BulletGlobals.debugSolver)
            {
                BulletGlobals.g_streamWriter.WriteLine("SetupFrictionConstraint[{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
                MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
            }
#endif

			solverConstraint.m_originalContactPoint = null;
            //solverConstraint.m_originalContactPointConstraint = null;
			solverConstraint.m_appliedImpulse = 0f;
			solverConstraint.m_appliedPushImpulse = 0f;




			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
				solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentA = body0 != null ? body0.GetInvInertiaTensorWorld() * ftorqueAxis1 * body0.GetAngularFactor() : IndexedVector3.Zero;
            }
			{
				IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
				solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
                solverConstraint.m_angularComponentB = body1 != null ? body1.GetInvInertiaTensorWorld() * ftorqueAxis1 * body1.GetAngularFactor() : IndexedVector3.Zero;
            }

#if COMPUTE_IMPULSE_DENOM
	        float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
	        float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
			IndexedVector3 vec;
			float denom0 = 0f;
			float denom1 = 0f;
			if (body0 != null)
			{
				vec = IndexedVector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
				denom0 = body0.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}
			if (body1 != null)
			{
				vec = IndexedVector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
				denom1 = body1.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
			}


#endif //COMPUTE_IMPULSE_DENOM
			float denom = relaxation / (denom0 + denom1);
			solverConstraint.m_jacDiagABInv = denom;
			MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
#if _USE_JACOBIAN
	        solverConstraint.m_jac =  new JacobianEntry (
		        ref rel_pos1,ref rel_pos2,ref solverConstraint.m_contactNormal,
		        body0.getInvInertiaDiagLocal(),
		        body0.getInvMass(),
		        body1.getInvInertiaDiagLocal(),
		        body1.getInvMass());
#endif //_USE_JACOBIAN


			{
				float rel_vel;
				float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : IndexedVector3.Zero);
				float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : IndexedVector3.Zero)
					+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : IndexedVector3.Zero);

				rel_vel = vel1Dotn + vel2Dotn;

				//float positionalError = 0f;

				float velocityError = desiredVelocity - rel_vel;
				float damper = 1f;
				float velocityImpulse = (velocityError * solverConstraint.m_jacDiagABInv) * damper;
				solverConstraint.m_rhs = velocityImpulse;
				solverConstraint.m_cfm = cfmSlip;
				solverConstraint.m_lowerLimit = 0;
				solverConstraint.m_upperLimit = 1e10f;
			}
		}
		protected void ResolveSplitPenetrationImpulseCacheFriendly(
			RigidBody body1,
			RigidBody body2,
			ref SolverConstraint c)
		{
			if (c.m_rhsPenetration != 0f)
			{
				gNumSplitImpulseRecoveries++;
				float deltaImpulse = c.m_rhsPenetration - (c.m_appliedPushImpulse * c.m_cfm);
				float deltaVel1Dotn = IndexedVector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
				float deltaVel2Dotn = -IndexedVector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetTurnVelocity());

				deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
				deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
				float sum = c.m_appliedPushImpulse + deltaImpulse;
				if (sum < c.m_lowerLimit)
				{
					deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
					c.m_appliedPushImpulse = c.m_lowerLimit;
				}
				else
				{
					c.m_appliedPushImpulse = sum;
				}
				body1.InternalApplyPushImpulse(c.m_contactNormal * body1.InternalGetInvMass(), c.m_angularComponentA, deltaImpulse);
				body2.InternalApplyPushImpulse(-c.m_contactNormal * body2.InternalGetInvMass(), c.m_angularComponentB, deltaImpulse);
			}
		}
		protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
		{
            m_lowerLimitCount++;
			//check magniture of applied impulse from SolverConstraint 
			float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;

            if (deltaImpulse > 200)
            {
                int ibreak = 0;
            }

            float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
            (c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);

            float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
            (c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);

			deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
			deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;

			float sum = c.m_appliedImpulse + deltaImpulse;

			if (sum < c.m_lowerLimit)
			{
				deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
				c.m_appliedImpulse = c.m_lowerLimit;
			}
			else
			{
				c.m_appliedImpulse = sum;
			}

            IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X,c.m_contactNormal.Y * body1.m_invMass.Y,c.m_contactNormal.Z * body1.m_invMass.Z);
            body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body1");

            temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
            body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body2");

		}
		protected void ResolveSingleConstraintRowGeneric(RigidBody body1, RigidBody body2, ref SolverConstraint c)
		{
            m_genericCount++;
                float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;

                float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
                (c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);

                float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
                (c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);


                float originalDeltaImpulse = deltaImpulse;

                deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
                deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;

#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
                {
                    BulletGlobals.g_streamWriter.WriteLine("ResolveSingleConstraintRowGeneric start [{0}][{1}][{2}][{3}].", originalDeltaImpulse, deltaVel1Dotn, deltaVel2Dotn, c.m_jacDiagABInv);
                }
#endif
                float sum = c.m_appliedImpulse + deltaImpulse;
                if (sum < c.m_lowerLimit)
                {
                    deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
                    c.m_appliedImpulse = c.m_lowerLimit;
                }
                else if (sum > c.m_upperLimit)
                {
                    deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
                    c.m_appliedImpulse = c.m_upperLimit;
                }
                else
                {
                    c.m_appliedImpulse = sum;
                }

                IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X, c.m_contactNormal.Y * body1.m_invMass.Y, c.m_contactNormal.Z * body1.m_invMass.Z);

                body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintGeneric-body1");

                temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);

                body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintGeneric-body2");
		}
		protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1,
										 ManifoldPoint cp, ContactSolverInfo infoGlobal)
		{
			if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING))
			{
				{
					SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
					if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
					{
						frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
						if (rb0 != null)
						{
                            rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse,"SetupFriction-rb0");
						}
						if (rb1 != null)
						{
                            rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse, "SetupFriction-rb1");
						}
					}
					else
					{
						frictionConstraint1.m_appliedImpulse = 0f;
					}
					m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;

				}

				if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
				{
					SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
					if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
					{
						frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
						if (rb0 != null)
						{
							rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse,"SetFriction-rb0");
						}
						if (rb1 != null)
						{
							rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse,"SetFriction-rb1");
						}
					}
					else
					{
						frictionConstraint2.m_appliedImpulse = 0f;
					}
					m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
				}
			}
			else
			{
				SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
				frictionConstraint1.m_appliedImpulse = 0f;
				if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
				{
					SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
					frictionConstraint2.m_appliedImpulse = 0f;
					m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
				}
				m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;

			}
		}
		protected void SetupContactConstraint(ref SolverConstraint solverConstraint, CollisionObject colObj0, CollisionObject colObj1, ManifoldPoint cp,
								ContactSolverInfo infoGlobal, ref IndexedVector3 vel, ref float rel_vel, ref float relaxation,
								out IndexedVector3 rel_pos1, out IndexedVector3 rel_pos2)
		{
            RigidBody rb0 = colObj0 as RigidBody;// RigidBody.Upcast(colObj0);
            RigidBody rb1 = colObj1 as RigidBody;//RigidBody.Upcast(colObj1);

			IndexedVector3 pos1 = cp.GetPositionWorldOnA();
			IndexedVector3 pos2 = cp.GetPositionWorldOnB();

			rel_pos1 = pos1 - colObj0.m_worldTransform._origin;
			rel_pos2 = pos2 - colObj1.m_worldTransform._origin;

			relaxation = 1f;

            // cross

            IndexedVector3 torqueAxis0 = new IndexedVector3(rel_pos1.Y *cp.m_normalWorldOnB.Z - rel_pos1.Z * cp.m_normalWorldOnB.Y,
                rel_pos1.Z *cp.m_normalWorldOnB.X - rel_pos1.X * cp.m_normalWorldOnB.Z,
                rel_pos1.X *cp.m_normalWorldOnB.Y - rel_pos1.Y * cp.m_normalWorldOnB.X);

            IndexedVector3 torqueAxis1 = new IndexedVector3(rel_pos2.Y *cp.m_normalWorldOnB.Z - rel_pos2.Z * cp.m_normalWorldOnB.Y,
                rel_pos2.Z *cp.m_normalWorldOnB.X - rel_pos2.X * cp.m_normalWorldOnB.Z,
                rel_pos2.X *cp.m_normalWorldOnB.Y - rel_pos2.Y * cp.m_normalWorldOnB.X);


            solverConstraint.m_angularComponentA = rb0 != null ? rb0.GetInvInertiaTensorWorld() * torqueAxis0 * rb0.GetAngularFactor() : IndexedVector3.Zero;
            //IndexedVector3 torqueAxis1 = IndexedVector3.Cross(ref rel_pos2, ref cp.m_normalWorldOnB);
            solverConstraint.m_angularComponentB = rb1 != null ? rb1.GetInvInertiaTensorWorld() * -torqueAxis1 * rb1.GetAngularFactor() : IndexedVector3.Zero;


            //IndexedVector3 torqueAxis0 = IndexedVector3.Cross(ref rel_pos1, ref cp.m_normalWorldOnB);
            //solverConstraint.m_angularComponentA = rb0 != null ? rb0.GetInvInertiaTensorWorld() * torqueAxis0 * rb0.GetAngularFactor() : IndexedVector3.Zero;
            //IndexedVector3 torqueAxis1 = IndexedVector3.Cross(ref rel_pos2, ref cp.m_normalWorldOnB);
            //solverConstraint.m_angularComponentB = rb1 != null ? rb1.GetInvInertiaTensorWorld() * -torqueAxis1 * rb1.GetAngularFactor() : IndexedVector3.Zero;

			{
#if COMPUTE_IMPULSE_DENOM
		        float denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
		        float denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
				IndexedVector3 vec;
				float denom0 = 0f;
				float denom1 = 0f;
				if (rb0 != null)
				{
					vec = IndexedVector3.Cross(ref solverConstraint.m_angularComponentA, ref rel_pos1);
                    denom0 = rb0.GetInvMass() + IndexedVector3.Dot(cp.m_normalWorldOnB, vec);
				}
				if (rb1 != null)
				{
					vec = IndexedVector3.Cross((-solverConstraint.m_angularComponentB), rel_pos2);
                    denom1 = rb1.GetInvMass() + IndexedVector3.Dot(cp.m_normalWorldOnB, vec);
				}
#endif //COMPUTE_IMPULSE_DENOM

				float denom = relaxation / (denom0 + denom1);
				MathUtil.SanityCheckFloat(denom);
				solverConstraint.m_jacDiagABInv = denom;
			}

			solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
			solverConstraint.m_relpos1CrossNormal = IndexedVector3.Cross(rel_pos1, cp.m_normalWorldOnB);
			solverConstraint.m_relpos2CrossNormal = IndexedVector3.Cross(rel_pos2, -cp.m_normalWorldOnB);



			IndexedVector3 vel1 = rb0 != null ? rb0.GetVelocityInLocalPoint(ref rel_pos1) : IndexedVector3.Zero;
			IndexedVector3 vel2 = rb1 != null ? rb1.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;

			vel = vel1 - vel2;

            rel_vel = IndexedVector3.Dot(cp.m_normalWorldOnB, vel);

			float penetration = cp.GetDistance() + infoGlobal.m_linearSlop;


			solverConstraint.m_friction = cp.GetCombinedFriction();

			float restitution = 0f;

			if (cp.GetLifeTime() > infoGlobal.m_restingContactRestitutionThreshold)
			{
				restitution = 0f;
			}
			else
			{
				restitution = RestitutionCurve(rel_vel, cp.GetCombinedResitution());
				if (restitution <= 0f)
				{
					restitution = 0f;
				}
			}

			///warm starting (or zero if disabled)
			if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
			{
				solverConstraint.m_appliedImpulse = cp.GetAppliedImpulse() * infoGlobal.m_warmstartingFactor;
				if (rb0 != null)
				{
					IndexedVector3 contactNormalTemp = solverConstraint.m_contactNormal;
                    rb0.InternalApplyImpulse(solverConstraint.m_contactNormal * rb0.GetInvMass() * rb0.GetLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse, "SetupContactConstraint-rb0");
                }
				if (rb1 != null)
				{
                    rb1.InternalApplyImpulse(solverConstraint.m_contactNormal * rb1.GetInvMass() * rb1.GetLinearFactor(), -solverConstraint.m_angularComponentB, -solverConstraint.m_appliedImpulse,"SetupContactConstraint-rb1");
                }
			}
			else
			{
				solverConstraint.m_appliedImpulse = 0f;
			}
			solverConstraint.m_appliedPushImpulse = 0f;
			{
				float rel_vel2 = 0f;
				float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, (rb0 != null ? rb0.GetLinearVelocity() : IndexedVector3.Zero))
					+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, (rb0 != null ? rb0.GetAngularVelocity() : IndexedVector3.Zero));
				float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, (rb1 != null ? rb1.GetLinearVelocity() : IndexedVector3.Zero))
					+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, (rb1 != null ? rb1.GetAngularVelocity() : IndexedVector3.Zero));

				rel_vel2 = vel1Dotn + vel2Dotn;

				float positionalError = 0f;

                if (rel_vel2 > 20)
                {
                    int ibreak = 0;
                }

				float velocityError = restitution - rel_vel2;// * damping;

				if (penetration > 0f)
				{
					positionalError = 0f;
					velocityError -= penetration / infoGlobal.m_timeStep;
				}
				else
				{
					positionalError = -penetration * infoGlobal.m_erp / infoGlobal.m_timeStep;
				}
				
				float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
				float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
				if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
				{
					//combine position and velocity into rhs
					solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
					solverConstraint.m_rhsPenetration = 0f;
				}
				else
				{
					//split position and velocity into rhs and m_rhsPenetration
					solverConstraint.m_rhs = velocityImpulse;
					solverConstraint.m_rhsPenetration = penetrationImpulse;
				}
				solverConstraint.m_cfm = 0f;
				solverConstraint.m_lowerLimit = 0;
				solverConstraint.m_upperLimit = 1e10f;
			}
		}