public SimulationParameters ReadEngineConfig()
		{
			XmlDocument xmlDoc = new XmlDocument();
			xmlDoc.Load(this.FileNameEngineProperties);

			XmlNodeList xmlList = xmlDoc.SelectNodes(nodePathObjects);

			SimulationParameters simulationParameters = new SimulationParameters (
                                Convert.ToDouble (xmlList [0] [this.timeStep].InnerText),
                                Convert.ToDouble (xmlList [0] [this.CFM].InnerText),
                                Convert.ToDouble (xmlList [0] [this.baumStabilization].InnerText),
                                Convert.ToDouble (xmlList [0] [this.linearVelocityStabilization].InnerText),
                                Convert.ToDouble (xmlList [0] [this.angularVelocityStabilization].InnerText),
                                Convert.ToDouble (xmlList [0] [this.shiftToStaticFrictionTolerance].InnerText),
                                Convert.ToBoolean (xmlList [0] [this.discreteCCD].InnerText),
                                Convert.ToDouble (xmlList [0] [this.collisionDistance].InnerText),
								//TODO
								Convert.ToDouble(xmlList[0][this.collisionDistance].InnerText),
								0.5,
                                new Vector3 (Convert.ToDouble (xmlList [0] [this.externalForce].Attributes ["x"].Value),
                                    Convert.ToDouble (xmlList [0] [this.externalForce].Attributes ["y"].Value),
                                    Convert.ToDouble (xmlList [0] [this.externalForce].Attributes ["z"].Value)),
								Convert.ToInt32 (xmlList [0] [this.maxThreadNumber].InnerText));

			return simulationParameters;
		}
		public PhysicsEngine GetPhysicsEnvironment()
		{
			var simulationParam = new SimulationParameters();
			var solverParameters = new SolverParameters();
			var collisionEngineParam = new CollisionEngineParameters();

			var physicsEnvironment = new PhysicsEngine(
													simulationParam,
													collisionEngineParam,
													solverParameters);

			physicsEnvironment.AddObject(getSimulationObjects()[0]);
			physicsEnvironment.RemoveObject(0);

			return physicsEnvironment;
		}
Пример #3
0
		public PhysicsEngine (
			SimulationParameters simulationParameters,
			CollisionEngineParameters collisionEngineParameters,
			SolverParameters solverParameters)
		{
			SolverParam = solverParameters;

			SetSolver(SolverType.NonLinearConjugateGradient);

			CollisionEngineParam = collisionEngineParameters;

			collisionEngine = new CollisionDetectionEngine(collisionEngineParameters);

			SimulationEngineParameters = simulationParameters;

			contactPartitioningEngine = new ContactPartitioningEngine();

			simulationObjects = new SimulationObject[0];
			simulationJoints = new List<IConstraint> ();
		}
Пример #4
0
		public List<JacobianContact> GetJacobianConstraint(
			CollisionPointStructure[] collisionPointsStruct,
			List<IConstraint> simulationJointList,
			SimulationObject[] simulationObjs,
			SimulationParameters simulationParameters)
		{
			var constraint = new List<JacobianContact>();

            #region Collision Contact

            constraint.AddRange(
                ContactConstraint.BuildJoints(
                    collisionPointsStruct,
                    simulationObjs,
                    simulationParameters));

            #endregion

            #region Joint

            foreach (IConstraintBuilder constraintItem in simulationJointList)
				constraint.AddRange(constraintItem.BuildJacobian(simulationObjs));

            #endregion

            return constraint;
		}
Пример #5
0
		void initProgram()
		{
			try
			{
                //var env = new BuildEnvironment();
                //env.GetPhysicsEnvironment();

                //LoadObject loadObject = new LoadObject ("startJoint.xml");
                //LoadObject loadObject = new LoadObject ("configJoint.xml");
                //var loadObject = new LoadObject ("startConfig.xml");
                //var loadObject = new LoadObject ("carConfig.xml");
                //var loadObject = new LoadObject("testJointBridge.xml");
                var loadObject = new LoadObject("compositeObjectConfig.xml");

                simulationObjects = loadObject.LoadSimulationObjects ();
				simulationJoints = loadObject.LoadSimulationJoints (simulationObjects);

				displayList = loadObject.GetOpenGLObjectList ();

				//Carico le texture
				textureID = loadObject.LoadTexture ();
				redTexture = OpenGLUtilities.LoadTexture ("red.bmp");

				//Set Collision Detection
				collisionEngineParameters = new CollisionEngineParameters();

				//Set Solver
				solverParameters = new SolverParameters();
				
				//Set Physics engine
				simulationParameters = new SimulationParameters();
				
				physicsEngine = new PhysicsEngine(
					simulationParameters,
					collisionEngineParameters,
					solverParameters);

				physicsEngine.SetSolver(SolverType.NonLinearConjugateGradient);

				for (int i = 0; i < simulationObjects.Count (); i++) 
				{
					physicsEngine.AddObject (simulationObjects [i]);
				}

				for (int i = 0; i < simulationJoints.Count (); i++) 
				{
					physicsEngine.AddJoint (simulationJoints [i]);
				}

				pause = true;

				//var obj = physicsEngine.GetJointsList();
//
//				obj.Add(null);

				#region Object Removing

				//physicsEngine.RemoveAllJoints();

				//physicsEngine.RemoveAllObjects();

				//int objectIndex = 2;

				//physicsEngine.RemoveObject(objectIndex);

				////Graphics engine
				//var buf = displayList.ToList();
				//buf.RemoveAt(objectIndex);
				//displayList = buf.ToArray();

				//var buf1 = textureID.ToList();
				//buf1.RemoveAt(objectIndex);
				//textureID = buf1.ToArray();

				#endregion


				collPoint = new List<CollisionPointStructure> ();
                collisionPartitionedPoints = new List<List<CollisionPointStructure>>();

			}
			catch (Exception e) 
			{
				throw new Exception (e.StackTrace);
			}
		}
		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;
		}
		public static List<JacobianContact> BuildJoints(
			CollisionPointStructure[] collisionPointsStruct,
			SimulationObject[] simulationObjs,
			SimulationParameters simulationParameters)
		{
			var contactConstraints = new List<JacobianContact> ();

			for (int i = 0; i < collisionPointsStruct.Length; i++) 
			{
				CollisionPointStructure collisionPointStr = collisionPointsStruct [i];

				int indexA = collisionPointStr.ObjectA;
				int indexB = collisionPointStr.ObjectB;

				SimulationObject objectA = simulationObjs[indexA];
				SimulationObject objectB = simulationObjs[indexB];

				double restitutionCoefficient =
					(simulationObjs[indexA].RestitutionCoeff +
					 simulationObjs[indexB].RestitutionCoeff) * 0.5;

				double baumgarteStabilizationValue = 
					(simulationObjs[indexA].BaumgarteStabilizationCoeff +
					 simulationObjs[indexB].BaumgarteStabilizationCoeff) * 0.5;

                for (int k = 0; k < collisionPointStr.CollisionPoints.Length; k++)
                {
                    Vector3 ra = collisionPointStr.CollisionPoints[k].CollisionPointA - objectA.Position;
                    Vector3 rb = collisionPointStr.CollisionPoints[k].CollisionPointB - objectB.Position;

                    Vector3 linearComponentA = (-1.0 * collisionPointStr.CollisionPoints[k].CollisionNormal).Normalize();
                    Vector3 linearComponentB = -1.0 * linearComponentA;

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

                    Vector3 velocityA = objectA.LinearVelocity +
                                        objectA.AngularVelocity.Cross(ra);

                    Vector3 velocityB = objectB.LinearVelocity +
                                        objectB.AngularVelocity.Cross(rb);

                    Vector3 relativeVelocity = velocityB - velocityA;

                    if (relativeVelocity.Length() < 1E-12 &&
                        collisionPointStr.Intersection &&
                        collisionPointStr.ObjectDistance < 1E-10)
                        continue;
                    
                    #region Normal direction contact

                    double linearComponent = linearComponentA.Dot(relativeVelocity);

                    double uCollision = restitutionCoefficient * Math.Max(0.0, linearComponent - simulationParameters.VelocityTolerance);

                    double correctionParameter = 0.0;

                    // Console.WriteLine("coll " + linearComponent);
                    if (collisionPointStr.Intersection)
                    {
                        //Limit the Baum stabilization jitter effect 
                        correctionParameter = Math.Max(Math.Max(collisionPointStr.ObjectDistance - simulationParameters.CompenetrationTolerance, 0.0) *
                                                baumgarteStabilizationValue - uCollision, 0.0);
                    }

                    double correctedBounce = uCollision;

                    JacobianContact normalContact = JacobianCommon.GetDOF(
                        indexA,
                        indexB,
                        linearComponentA,
                        linearComponentB,
                        angularComponentA,
                        angularComponentB,
                        objectA,
                        objectB,
                        correctedBounce,
                        correctionParameter,
                        simulationParameters.NormalCFM,
                        0.0,
                        ConstraintType.Collision,
                        null,
                        collisionPointStr.CollisionPoints[k].StartImpulseValue[0]);

                    #endregion

                    #region Friction Contact

                    JacobianContact[] frictionContact =
                        addFriction(
                            objectA,
                            objectB,
                            simulationParameters,
                            indexA,
                            indexB,
                            linearComponentA,
                            relativeVelocity,
                            ra,
                            rb,
                            collisionPointStr.CollisionPoints[k].StartImpulseValue);

                    #endregion

                    contactConstraints.Add(normalContact);

                    int normalIndex = contactConstraints.Count - 1;
                    foreach (JacobianContact jc in frictionContact)
                    {
                        jc.SetContactReference(normalIndex);
                        contactConstraints.Add(jc);
                    }
                }
				
			}
			return contactConstraints;
		}