Example #1
0
		public static JacobianContact[] FindJointConstraints(JacobianContact[] list)
		{
			var result = new List<JacobianContact>();

			foreach (JacobianContact jc in list)
			{
				if (jc.Type != ConstraintType.Friction &&
					jc.Type != ConstraintType.Collision)
					result.Add(jc);
			}

			return result.ToArray();
		}
Example #2
0
		public static JacobianContact[] PruneConstraintsFromSoftJoint(
			JacobianContact[] list)
		{
			var result = new List<JacobianContact>();

			foreach (JacobianContact jc in list)
			{
				if (jc.Type != ConstraintType.SoftJoint)
					result.Add(jc);
			}

			return result.ToArray();
		}
Example #3
0
        public static JacobianContact[] FilterConstraints(
			JacobianContact[] list,
			ConstraintType typeA)
		{
			var result = new List<JacobianContact>();

			foreach (JacobianContact jc in list)
			{
				if (jc.Type == typeA)
					result.Add(jc);
			}

			return result.ToArray();
		}
Example #4
0
		public static JacobianContact[] FindConstraintsWithError(
			JacobianContact[] list,
			ConstraintType typeA,
			ConstraintType typeB)
		{
			var result = new List<JacobianContact>();

			foreach (JacobianContact jc in list)
			{
				if ((jc.Type == typeA ||
				    jc.Type == typeB) &&
				    Math.Abs(jc.CorrectionValue) > 1E-100)
					result.Add(jc);
			}

			return result.ToArray();
		}
		private SolutionValues[] BuildMatrixAndExecuteSolver(
			JacobianContact[] contactConstraints,
			LinearProblemProperties linearProblemProperties,
			int nIterations)
		{
			if (linearProblemProperties != null)
			{
				solver.GetSolverParameters().SetSolverMaxIteration(nIterations);

				SolutionValues[]  solutionValues = solver.Solve(linearProblemProperties);

				for (int j = 0; j < contactConstraints.Length; j++)
				{
					contactConstraints[j].StartImpulse.SetStartValue(solutionValues[j].X);
				}

				return solutionValues;
			}

			return null;
		}
		/// <summary>
		/// Builds the LCP matrix for solver.
		/// </summary>
		private LinearProblemProperties BuildLCPMatrix(
			JacobianContact[] contact,
			bool positionStabilization = false)
		{
			if (contact.Length > 0) 
			{
				SparseElement[] M = new SparseElement[contact.Length];
				double[] B = new double[contact.Length];
				SolutionValues[] X = new SolutionValues[contact.Length];
				double[] D = new double[contact.Length];
				ConstraintType[] constraintsType = new ConstraintType[contact.Length];
				double[] constraintsLimit = new double[contact.Length];
				List<int?>[] constraints = new List<int?>[contact.Length];
                
				List<int>[] index = new List<int>[contact.Length];
				List<double>[] value = new List<double>[contact.Length];

				for (int i = 0; i < contact.Length; i++) 
				{
					index [i] = new List<int> ();
					value [i] = new List<double> ();
                    constraints[i] = new List<int?>();
                }

				//Critical section variable
				var sync = new object ();

				Parallel.For (0, 
					contact.Length, 
					new ParallelOptions { MaxDegreeOfParallelism = SimulationEngineParameters.MaxThreadNumber }, 
					i => {

						JacobianContact contactA = contact [i];

						if (positionStabilization)
							B[i] = contactA.CorrectionValue;
						else
							B[i] = -(contactA.B - ((contactA.CorrectionValue) < 0 ? Math.Max(contactA.CorrectionValue, -SimulationEngineParameters.MaxCorrectionValue):
                                                                                    Math.Min(contactA.CorrectionValue, SimulationEngineParameters.MaxCorrectionValue)));
						
						X[i].X = contactA.StartImpulse.StartImpulseValue;

                        
                        if (contactA.ContactReference.HasValue)
                            constraints[i].Add(contactA.ContactReference);
                        
						constraintsLimit [i] = contactA.ConstraintLimit;
						constraintsType [i] = contactA.Type;
                        
						double mValue = addLCPValue(contactA,
													contactA);

						//Diagonal value
						mValue += contactA.CFM +
								  SimulationEngineParameters.CFM +
								  1E-40;

                        D[i] = 1.0 / mValue;

                        	for (int j = i + 1; j < contact.Length; j++) 
						{
							JacobianContact contactB = contact[j];
							
							if (contactA.ObjectA == contactB.ObjectA ||
								contactA.ObjectB == contactB.ObjectB ||
								contactA.ObjectA == contactB.ObjectB ||
								contactA.ObjectB == contactB.ObjectA)
							{
                                if (contactA.Type == contactB.Type && 
                                    contactB.Type == ConstraintType.Collision && 
                                    contactA.ObjectA == contactB.ObjectA && 
                                    contactA.ObjectB == contactB.ObjectB)
                                {
                                    constraints[i].Add(j);
                                    constraints[j].Add(i);
                                }

								mValue = addLCPValue(
									contactA,
									contactB);
                                
								if (Math.Abs(mValue) > 1E-30)
								{
									lock (sync)
									{
										index[i].Add(j);
										value[i].Add(mValue);
										index[j].Add(i);
										value[j].Add(mValue);
									}
								}
							}
						}
					});


                int?[][] constraintsArray = new int?[contact.Length][];
                for (int i = 0; i < contact.Length; i++) 
				{
					M [i] = new SparseElement (
						value [i].ToArray (),
						index [i].ToArray (),
                        contact.Length);

                    constraintsArray[i] = constraints[i].ToArray();
				}
                

				return new LinearProblemProperties (
					M,
					B,
					X,
					D,
					constraintsLimit,
					constraintsType,
                    constraintsArray,
                    	contact.Length);
			}

			return null;
		}
        private JacobianContact[] ContactSorting(JacobianContact[] jacobianContact)
        {
            var sorted = jacobianContact.Select((x, i) => new KeyValuePair<JacobianContact, int>(x, i)).
                OrderBy(x => Math.Abs(x.Key.B)).ToArray();

            int[] sortedIndex = sorted.Select(x => x.Value).ToArray();
            JacobianContact[] sortedContact = sorted.Select(x => x.Key).ToArray();

            int[] randomIndex = new int[jacobianContact.Length];
            for (int i = 0; i < randomIndex.Length; i++)
                randomIndex[sortedIndex[i]] = i;

            for (int i = 0; i < sortedContact.Length; i++)
            {
                if (sortedContact[i].Type == ConstraintType.Friction)
                    sortedContact[i].SetContactReference(randomIndex[sortedContact[i].ContactReference.Value]);
            }

            return sortedContact;
        }
        private void UpdatePositionBasedVelocity(
            JacobianContact[] contact,
            SimulationObject[] simulationObj,
            SolutionValues[] X)
        {
            for (int i = 0; i < contact.Length; i++)
            {
                double impulse = X[i].X;

                JacobianContact ct = contact[i];

                SetPositionBasedVelocity(
                    simulationObj,
                    ct.LinearComponentA,
                    ct.AngularComponentA,
                    impulse,
                    ct.ObjectA);

                SetPositionBasedVelocity(
                    simulationObj,
                    ct.LinearComponentB,
                    ct.AngularComponentB,
                    impulse,
                    ct.ObjectB);
            }
        }
        /// <summary>
        /// Updates velocity of the simulations objects.
        /// </summary>
        private void UpdateVelocity(
			JacobianContact[] contact,
			SimulationObject[] simulationObj,
			SolutionValues[] X)
		{
			for (int i =0; i< contact.Length;i++) 
			{
                if (Math.Abs(X[i].X) > 1E-50)
                {
                    double impulse = X[i].X;

                    JacobianContact ct = contact[i];

                    UpdateObjectVelocity(
                        simulationObj,
                        ct.LinearComponentA,
                        ct.AngularComponentA,
                        impulse,
                        ct.ObjectA);

                    UpdateObjectVelocity(
                        simulationObj,
                        ct.LinearComponentB,
                        ct.AngularComponentB,
                        impulse,
                        ct.ObjectB);

                    ct.StartImpulse.SetStartValue(impulse * SimulationEngineParameters.WarmStartingValue);
                }
			}
		}
		private double addLCPValue(
			JacobianContact contactA,
			JacobianContact contactB)
		{
			double linearA = 0.0;
			double angularA = 0.0;

			if (contactA.ObjectA == contactB.ObjectA) {

				linearA = contactA.LinearComponentA.Dot (
					contactB.LinearComponentA * simulationObjects [contactA.ObjectA].InverseMass);
				
				angularA = contactA.AngularComponentA.Dot (
					simulationObjects [contactA.ObjectA].InertiaTensor * contactB.AngularComponentA);

			} else if (contactB.ObjectB == contactA.ObjectA) {

				linearA = contactA.LinearComponentA.Dot (
					contactB.LinearComponentB * simulationObjects [contactA.ObjectA].InverseMass);
				
				angularA = contactA.AngularComponentA.Dot (
					simulationObjects [contactA.ObjectA].InertiaTensor * contactB.AngularComponentB);
			}

			double linearB = 0.0;
			double angularB = 0.0;

			if (contactB.ObjectA == contactA.ObjectB) {
				
				linearB = contactA.LinearComponentB.Dot (
					contactB.LinearComponentA * simulationObjects [contactA.ObjectB].InverseMass);
				
				angularB = contactA.AngularComponentB.Dot(
					simulationObjects [contactA.ObjectB].InertiaTensor * contactB.AngularComponentA);
				
			} else if (contactB.ObjectB == contactA.ObjectB) {
				
				linearB = contactA.LinearComponentB.Dot (
					contactB.LinearComponentB * simulationObjects [contactA.ObjectB].InverseMass);
				
				angularB = contactA.AngularComponentB.Dot (
					simulationObjects [contactA.ObjectB].InertiaTensor * contactB.AngularComponentB);
			}

			return (linearA + angularA) +
				   (linearB + angularB);
		}
		private static JacobianContact[] addFriction(
			SimulationObject objectA,
			SimulationObject objectB,
			SimulationParameters simulationParameters,
			int indexA,
			int indexB,
			Vector3 normal,
			Vector3 relativeVelocity,
			Vector3 ra,
			Vector3 rb,
			List<StartImpulseProperties> startImpulseProperties)
		{
			JacobianContact[] friction = new JacobianContact[2];

			var tx = new Vector3 ();
			var ty = new Vector3 ();

			GeometryUtilities.ComputeBasis(
				normal,
				ref tx,
				ref ty);

			double constraintLimit = 0.0;

            Vector3 tangentialVelocity = relativeVelocity -
                                         (normal.Dot(relativeVelocity)) *
                                         normal;

            #region Get start friction direction

            if (Vector3.Length (tangentialVelocity) >
				simulationParameters.ShiftToStaticFrictionTolerance) 
				constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
			else 
				constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
			
			#endregion

			#region Tangential Direction 1

			var linearComponentA = tx;
			var linearComponentB = -1.0 * linearComponentA;

			var angularComponentA = ra.Cross (linearComponentA);
			var angularComponentB = -1.0 * rb.Cross (linearComponentA);

			friction [0] = JacobianCommon.GetDOF (
				indexA,
				indexB,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				objectA,
				objectB,
				0.0,
				0.0,
				simulationParameters.FrictionCFM,
				constraintLimit,
				ConstraintType.Friction,
                	null,
				startImpulseProperties[1]);

			#endregion

			#region Tangential Direction 2

			linearComponentA = ty;
			linearComponentB = -1.0 * linearComponentA;

			angularComponentA = ra.Cross (linearComponentA);
			angularComponentB = -1.0 * rb.Cross (linearComponentA);

			friction [1] = JacobianCommon.GetDOF (
				indexA,
				indexB,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				objectA,
				objectB,
				0.0,
				0.0,
				simulationParameters.FrictionCFM,
				constraintLimit,
				ConstraintType.Friction,
                null,
				startImpulseProperties[2]);

			#endregion

			return friction;
		}