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;
}
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> ();
}
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;
}
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;
}