public SharpEngine(
PhysicsEngineParameters simulationParameters,
CollisionEngineParameters collisionEngineParameters,
SolverParameters solverParameters)
{
SolverParameters = solverParameters;
CollisionEngineParam = collisionEngineParameters;
EngineParameters = simulationParameters;
// Default solver
SetSolverType(SolverType.ProjectedGaussSeidel);
Shapes = new IShape[0];
CollisionEngine = new CollisionDetectionEngine(
collisionEngineParameters,
EngineParameters.CollisionDistance);
contactPartitioningEngine = new ContactPartitioningEngine();
CollisionShapes = new List <ICollisionShape>();
CollisionJoints = new List <ICollisionJoint>();
Joints = new List <IConstraint> ();
HsGenerator = new HashGenerator();
LinearSystemBuilder = new LinearProblemBuilderEngine(EngineParameters);
IntegrateVelocityEngine = new IntegrateVelocity(EngineParameters);
IntegratePositionEngine = new IntegratePosition(EngineParameters);
contactConstraintBuilder = new ContactConstraintBuilder(EngineParameters);
rayCastEngine = new RayCastingEngine();
warmStartEngine = new WarmStartEngine(EngineParameters);
ccdEngine = new ConservativeAdvancement();
}
private JacobianConstraint[] AddFrictionConstraints(
IShape objectA,
IShape objectB,
PhysicsEngineParameters simulationParameters,
Vector3d normal,
Vector3d relativeVelocity,
Vector3d ra,
Vector3d rb,
SoftShapePoint softShapePoint)
{
JacobianConstraint[] friction = new JacobianConstraint[simulationParameters.FrictionDirections];
Vector3d[] frictionDirection = GetFrictionCone(normal, simulationParameters.FrictionDirections);
double constraintLimit = 0.0;
Vector3d tangentialVelocity = relativeVelocity -
normal.Dot(relativeVelocity) *
normal;
if (Vector3d.Length(tangentialVelocity) > simulationParameters.ShiftToStaticFrictionTolerance)
{
constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
}
else
{
constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
}
for (int i = 0; i < simulationParameters.FrictionDirections; i++)
{
var linearComponentA = frictionDirection[i];
var linearComponentB = -1.0 * linearComponentA;
var angularComponentA = ra.Cross(linearComponentA);
var angularComponentB = -1.0 * rb.Cross(linearComponentA);
friction[i] = JacobianCommon.GetDOF(
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
(objectA is ISoftShape) ? (IShapeCommon)softShapePoint: objectA,
(objectB is ISoftShape) ? (IShapeCommon)softShapePoint: objectB,
0.0,
0.0,
simulationParameters.FrictionCFM,
constraintLimit,
ConstraintType.Friction,
null);
}
return(friction);
}
private JacobianConstraint[] GetJacobianConstraints(
CollisionPointStructure[] collisionPointsStruct,
List <IConstraint> simulationJointList,
IShape[] simulationObjs,
PhysicsEngineParameters simulationParameters)
{
List <JacobianConstraint> jacobianConstraints = new List <JacobianConstraint>();
////Collision Contact
jacobianConstraints.AddRange(
GetCollisionConstraints(
collisionPointsStruct,
simulationObjs));
////Joints
if (simulationJointList.Count > 0)
{
List <JacobianConstraint>[] jConstraints = new List <JacobianConstraint> [simulationJointList.Count];
var rangePartitioner = Partitioner.Create(0, simulationJointList.Count, Convert.ToInt32(simulationJointList.Count / EngineParameters.MaxThreadNumber) + 1);
Parallel.ForEach(
rangePartitioner,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
jConstraints[i] = simulationJointList[i].BuildJacobian();
}
});
jacobianConstraints.AddRange(jConstraints.SelectMany(f => f));
}
return(jacobianConstraints.ToArray());
}
public ContactConstraintBuilder(
PhysicsEngineParameters simulationParameters)
{
this.simulationParameters = simulationParameters;
}
