public static CollisionPointStructure Find(
CollisionPointStructure[] collisionPoints,
ContactIndex contactIndex)
{
foreach (CollisionPointStructure cps in collisionPoints)
{
if (cps.ObjectA == contactIndex.IndexA &&
cps.ObjectB == contactIndex.IndexB)
{
return cps;
}
}
return null;
}
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;
}
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;
}
public List<SpatialPartition> CalculateSpatialPartitioning(
CollisionPointStructure[] collisionPoints,
List<IConstraint> simulationJoints,
SimulationObject[] simulationObjects)
{
if (collisionPoints.Length > 0 ||
simulationJoints.Count > 0)
{
var partitions = new List<SpatialPartition> ();
var contactIndex = new List<ContactIndex> ();
// Add contacts
int keyIndex = 0;
foreach (CollisionPointStructure cps in collisionPoints)
{
contactIndex.Add (new ContactIndex (
cps.ObjectA,
cps.ObjectB,
ContactGroupType.Collision,
keyIndex));
keyIndex++;
}
// Add joints
foreach (IConstraint smj in simulationJoints)
{
contactIndex.Add (new ContactIndex (
smj.GetObjectIndexA(),
smj.GetObjectIndexB(),
ContactGroupType.Joint,
smj.GetKeyIndex()));
}
while (contactIndex.Count != 0) {
var partition = new List<ContactIndex> ();
partition.Add (contactIndex [0]);
recursiveSearch (
contactIndex [0],
contactIndex,
partition,
simulationObjects);
foreach(ContactIndex cIndex in partition)
{
var index = new List<int> ();
int indexVealue = 0;
foreach(ContactIndex cntIndex in contactIndex)
{
if (cntIndex.IndexA == cIndex.IndexA &&
cntIndex.IndexB == cIndex.IndexB &&
cntIndex.Type == cIndex.Type)
{
index.Add (indexVealue);
}
indexVealue++;
}
index.Sort((a, b) => -1 * a.CompareTo(b));
for (int j = 0; j < index.Count; j++)
{
contactIndex.RemoveAt (index [j]);
}
}
partitions.Add (new SpatialPartition (partition));
}
return partitions;
}
return null;
}