Пример #1
0
        //velocity + friction
        //response  between two dynamic objects with friction
        public static float ResolveSingleCollisionCombined(
            RigidBody bodyA,
            RigidBody bodyB,
            ManifoldPoint contactPoint,
            ContactSolverInfo solverInfo)
        {
            Vector3 posA   = contactPoint.PositionWorldOnA;
            Vector3 posB   = contactPoint.PositionWorldOnB;
            Vector3 normal = contactPoint.NormalWorldOnB;

            Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
            Vector3 relPosB = posB - bodyB.CenterOfMassPosition;

            Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
            Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
            Vector3 vel  = velA - velB;
            float   relVel;

            relVel = Vector3.Dot(normal, vel);

            float Kfps = 1f / solverInfo.TimeStep;

            //float damping = solverInfo.m_damping;
            float Kerp = solverInfo.Erp;
            float Kcor = Kerp * Kfps;

            ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;

            if (cpd == null)
            {
                throw new BulletException();
            }

            float distance        = cpd.Penetration;
            float positionalError = Kcor * -distance;
            float velocityError   = cpd.Restitution - relVel;          // * damping;

            float penetrationImpulse = positionalError * cpd.JacDiagABInv;

            float velocityImpulse = velocityError * cpd.JacDiagABInv;

            float normalImpulse = penetrationImpulse + velocityImpulse;

            // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
            float oldNormalImpulse = cpd.AppliedImpulse;
            float sum = oldNormalImpulse + normalImpulse;

            cpd.AppliedImpulse = 0 > sum ? 0 : sum;

            normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;

            if (bodyA.InverseMass != 0)
            {
                bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
            }
            if (bodyB.InverseMass != 0)
            {
                bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
            }

            {
                //friction
                Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
                Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
                Vector3 vel3  = vel12 - vel22;

                relVel = Vector3.Dot(normal, vel3);


                Vector3 latVel      = vel3 - normal * relVel;
                float   lat_rel_vel = latVel.Length();

                float combinedFriction = cpd.Friction;

                if (cpd.AppliedImpulse > 0)
                {
                    if (lat_rel_vel > float.Epsilon)
                    {
                        latVel /= lat_rel_vel;
                        Vector3 temp1            = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
                        Vector3 temp2            = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
                        float   friction_impulse = lat_rel_vel /
                                                   (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
                        float normal_impulse = cpd.AppliedImpulse * combinedFriction;

                        MathHelper.SetMin(ref friction_impulse, normal_impulse);
                        MathHelper.SetMin(ref friction_impulse, -normal_impulse);
                        bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
                        bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
                    }
                }
            }
            return(normalImpulse);
        }
Пример #2
0
        public static float ResolveSingleFrictionOriginal(
            RigidBody bodyA,
            RigidBody bodyB,
            ManifoldPoint contactPoint,
            ContactSolverInfo solverInfo)
        {
            Vector3 posA = contactPoint.PositionWorldOnA;
            Vector3 posB = contactPoint.PositionWorldOnB;

            Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
            Vector3 relPosB = posB - bodyB.CenterOfMassPosition;

            ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;

            if (cpd == null)
            {
                throw new BulletException();
            }

            float combinedFriction = cpd.Friction;

            float limit = cpd.AppliedImpulse * combinedFriction;

            //if (contactPoint.m_appliedImpulse>0.f)
            //friction
            {
                //apply friction in the 2 tangential directions

                {
                    // 1st tangent
                    Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
                    Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
                    Vector3 vel  = velA - velB;

                    float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);

                    // calculate j that moves us to zero relative velocity
                    float j     = -vrel * cpd.JacDiagABInvTangentA;
                    float total = cpd.AccumulatedTangentImpulseA + j;
                    if (limit < total)
                    {
                        total = limit;
                    }
                    if (total < -limit)
                    {
                        total = -limit;
                    }
                    j = total - cpd.AccumulatedTangentImpulseA;
                    cpd.AccumulatedTangentImpulseA = total;
                    bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
                    bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
                }


                {
                    // 2nd tangent
                    Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
                    Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
                    Vector3 vel  = velA - velB;

                    float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);

                    // calculate j that moves us to zero relative velocity
                    float j     = -vrel * cpd.JacDiagABInvTangentB;
                    float total = cpd.AccumulatedTangentImpulseB + j;
                    if (limit < total)
                    {
                        total = limit;
                    }
                    if (total < -limit)
                    {
                        total = -limit;
                    }
                    j = total - cpd.AccumulatedTangentImpulseB;
                    cpd.AccumulatedTangentImpulseB = total;
                    bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
                    bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
                }
            }

            return(cpd.AppliedImpulse);
        }
Пример #3
0
		public static float ResolveSingleFrictionOriginal(
			RigidBody bodyA,
			RigidBody bodyB,
			ManifoldPoint contactPoint,
			ContactSolverInfo solverInfo)
		{
			Vector3 posA = contactPoint.PositionWorldOnA;
			Vector3 posB = contactPoint.PositionWorldOnB;

			Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
			Vector3 relPosB = posB - bodyB.CenterOfMassPosition;

			ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
			if (cpd == null)
				throw new BulletException();

			float combinedFriction = cpd.Friction;

			float limit = cpd.AppliedImpulse * combinedFriction;
			//if (contactPoint.m_appliedImpulse>0.f)
			//friction
			{
				//apply friction in the 2 tangential directions

				{
					// 1st tangent
					Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
					Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
					Vector3 vel = velA - velB;

					float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);

					// calculate j that moves us to zero relative velocity
					float j = -vrel * cpd.JacDiagABInvTangentA;
					float total = cpd.AccumulatedTangentImpulseA + j;
					if (limit < total)
						total = limit;
					if (total < -limit)
						total = -limit;
					j = total - cpd.AccumulatedTangentImpulseA;
					cpd.AccumulatedTangentImpulseA = total;
					bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
					bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
				}


				{
					// 2nd tangent
					Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
					Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
					Vector3 vel = velA - velB;

					float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);

					// calculate j that moves us to zero relative velocity
					float j = -vrel * cpd.JacDiagABInvTangentB;
					float total = cpd.AccumulatedTangentImpulseB + j;
					if (limit < total)
						total = limit;
					if (total < -limit)
						total = -limit;
					j = total - cpd.AccumulatedTangentImpulseB;
					cpd.AccumulatedTangentImpulseB = total;
					bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
					bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
				}
			}

			return cpd.AppliedImpulse;
		}
Пример #4
0
		//velocity + friction
		//response  between two dynamic objects with friction
		public static float ResolveSingleCollisionCombined(
			RigidBody bodyA,
			RigidBody bodyB,
			ManifoldPoint contactPoint,
			ContactSolverInfo solverInfo)
		{

			Vector3 posA = contactPoint.PositionWorldOnA;
			Vector3 posB = contactPoint.PositionWorldOnB;
			Vector3 normal = contactPoint.NormalWorldOnB;

			Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
			Vector3 relPosB = posB - bodyB.CenterOfMassPosition;

			Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
			Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
			Vector3 vel = velA - velB;
			float relVel;
			relVel = Vector3.Dot(normal, vel);

			float Kfps = 1f / solverInfo.TimeStep;

			//float damping = solverInfo.m_damping;
			float Kerp = solverInfo.Erp;
			float Kcor = Kerp * Kfps;

			ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
			if (cpd == null)
				throw new BulletException();

			float distance = cpd.Penetration;
			float positionalError = Kcor * -distance;
			float velocityError = cpd.Restitution - relVel;// * damping;

			float penetrationImpulse = positionalError * cpd.JacDiagABInv;

			float velocityImpulse = velocityError * cpd.JacDiagABInv;

			float normalImpulse = penetrationImpulse + velocityImpulse;

			// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
			float oldNormalImpulse = cpd.AppliedImpulse;
			float sum = oldNormalImpulse + normalImpulse;
			cpd.AppliedImpulse = 0 > sum ? 0 : sum;

			normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;

			if (bodyA.InverseMass != 0)
			{
				bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
			}
			if (bodyB.InverseMass != 0)
			{
				bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
			}

			{
				//friction
				Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
				Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
				Vector3 vel3 = vel12 - vel22;

				relVel = Vector3.Dot(normal, vel3);


				Vector3 latVel = vel3 - normal * relVel;
				float lat_rel_vel = latVel.Length();

				float combinedFriction = cpd.Friction;

				if (cpd.AppliedImpulse > 0)
					if (lat_rel_vel > float.Epsilon)
					{
						latVel /= lat_rel_vel;
						Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
						Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
						float friction_impulse = lat_rel_vel /
							(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
						float normal_impulse = cpd.AppliedImpulse * combinedFriction;

						MathHelper.SetMin(ref friction_impulse, normal_impulse);
						MathHelper.SetMin(ref friction_impulse, -normal_impulse);
						bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
						bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
					}
			}
			return normalImpulse;
		}
Пример #5
0
        protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
        {
            RigidBody body0 = manifold.BodyA as RigidBody;
            RigidBody body1 = manifold.BodyB as RigidBody;


            //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
            {
                manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);

                int numpoints = manifold.ContactsCount;

                _totalContactPoints += numpoints;

                Vector3 color = new Vector3(0, 1, 0);
                for (int i = 0; i < numpoints; i++)
                {
                    ManifoldPoint cp = manifold.GetContactPoint(i);
                    if (cp.Distance <= 0)
                    {
                        Vector3 pos1 = cp.PositionWorldOnA;
                        Vector3 pos2 = cp.PositionWorldOnB;

                        Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
                        Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;


                        //this jacobian entry is re-used for all iterations
                        JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
                                                              MatrixOperations.Transpose(body1.CenterOfMassTransform),
                                                              rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
                                                              body1.InvInertiaDiagLocal, body1.InverseMass);

                        float jacDiagAB = jac.Diagonal;

                        ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
                        if (cpd != null)
                        {
                            //might be invalid
                            cpd.PersistentLifeTime++;
                            if (cpd.PersistentLifeTime != cp.LifeTime)
                            {
                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                                cpd = new ConstraintPersistentData();
                                cpd.PersistentLifeTime = cp.LifeTime;
                            }
                        }
                        else
                        {
                            cpd = new ConstraintPersistentData();
                            _totalCpd++;
                            //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                            cp.UserPersistentData  = cpd;
                            cpd.PersistentLifeTime = cp.LifeTime;
                            //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
                        }
                        if (cpd == null)
                        {
                            throw new BulletException();
                        }

                        cpd.JacDiagABInv = 1f / jacDiagAB;

                        //Dependent on Rigidbody A and B types, fetch the contact/friction response func
                        //perhaps do a similar thing for friction/restutution combiner funcs...

                        cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
                        cpd.ContactSolverFunc  = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];

                        Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
                        Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
                        Vector3 vel  = vel1 - vel2;
                        float   rel_vel;
                        rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);

                        float combinedRestitution = cp.CombinedRestitution;

                        cpd.Penetration = cp.Distance;
                        cpd.Friction    = cp.CombinedFriction;
                        cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
                        if (cpd.Restitution < 0f)
                        {
                            cpd.Restitution = 0.0f;
                        }
                        ;

                        //restitution and penetration work in same direction so
                        //rel_vel

                        float penVel = -cpd.Penetration / info.TimeStep;

                        if (cpd.Restitution > penVel)
                        {
                            cpd.Penetration = 0;
                        }


                        float relaxation = info.Damping;
                        if ((_solverMode & SolverMode.UseWarmstarting) != 0)
                        {
                            cpd.AppliedImpulse *= relaxation;
                        }
                        else
                        {
                            cpd.AppliedImpulse = 0f;
                        }

                        //for friction
                        cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;

                        //re-calculate friction direction every frame, todo: check if this is really needed
                        Vector3 fwta = cpd.FrictionWorldTangentialA;
                        Vector3 fwtb = cpd.FrictionWorldTangentialB;
                        MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
                        cpd.FrictionWorldTangentialA = fwta;
                        cpd.FrictionWorldTangentialB = fwtb;

                        cpd.AccumulatedTangentImpulseA = 0;
                        cpd.AccumulatedTangentImpulseB = 0;
                        float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
                        float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
                        float denom  = relaxation / (denom0 + denom1);
                        cpd.JacDiagABInvTangentA = denom;


                        denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
                        denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
                        denom  = relaxation / (denom0 + denom1);
                        cpd.JacDiagABInvTangentB = denom;

                        Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;

                        {
                            Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
                            cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
                            Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
                            cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
                        }
                        {
                            Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
                            cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
                        }
                        {
                            Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
                            cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
                        }
                        {
                            Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
                            cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
                        }
                        {
                            Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
                            cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
                        }


                        //apply previous frames impulse on both bodies
                        body0.ApplyImpulse(totalImpulse, rel_pos1);
                        body1.ApplyImpulse(-totalImpulse, rel_pos2);
                    }
                }
            }
        }