///<summary>
/// Removes a contact from the constraint.
///</summary>
///<param name="contact">Contact to remove.</param>
public override void RemoveContact(Contact contact)
{
ContactPenetrationConstraint penetrationConstraint = null;
for (int i = 0; i < penetrationConstraints.Count; i++)
{
if ((penetrationConstraint = penetrationConstraints.Elements[i]).contact == contact)
{
penetrationConstraint.CleanUp();
penetrationConstraints.RemoveAt(i);
penetrationConstraintPool.Push(penetrationConstraint);
break;
}
}
for (int i = frictionConstraints.Count - 1; i >= 0; i--)
{
ContactFrictionConstraint frictionConstraint = frictionConstraints[i];
if (frictionConstraint.PenetrationConstraint == penetrationConstraint)
{
frictionConstraint.CleanUp();
frictionConstraints.RemoveAt(i);
frictionConstraintPool.Push(frictionConstraint);
break;
}
}
}
///<summary>
/// Cleans upt he friction constraint.
///</summary>
public void CleanUp()
{
accumulatedImpulse = 0;
contactManifoldConstraint = null;
penetrationConstraint = null;
entityA = null;
entityB = null;
IsActive = false;
}
public unsafe static void Test()
{
ContactPenetrationConstraint constraint = new ContactPenetrationConstraint();
Contact contact = new Contact {
Normal = new Vector3(0, 1, 0), PenetrationDepth = 0, Position = new Vector3()
};
var pairHandler = new BoxPairHandler();
var a = new Entity(new BoxShape(1, 1, 1), 1)
{
Position = new Vector3(0, 0, 0),
Orientation = Quaternion.Identity,
LinearVelocity = new Vector3(0, 0, 0)
};
var b = new Entity(new BoxShape(1, 1, 1), 1)
{
Position = new Vector3(0, 1, 0),
Orientation = Quaternion.Identity,
LinearVelocity = new Vector3(0, 0, 0)
};
pairHandler.Initialize(a.CollisionInformation, b.CollisionInformation);
ContactManifoldConstraint manifoldConstraint = new ConvexContactManifoldConstraint(pairHandler);
manifoldConstraint.Initialize(a, b);
constraint.Setup(manifoldConstraint, contact);
float dt = 1 / 60f;
float inverseDt = 1 / dt;
constraint.Update(dt);
constraint.ExclusiveUpdate();
constraint.SolveIteration();
const int testCount = VectorizedConstraintTest.TestCount * 4;
const int iterationCount = VectorizedConstraintTest.IterationCount;
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < testCount; ++i)
{
constraint.Update(dt);
constraint.ExclusiveUpdate();
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
constraint.SolveIteration();
}
}
var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"Scalar Old: {endtime - startTime}");
}
///<summary>
/// Configures the friction constraint for a new contact.
///</summary>
///<param name="contactManifoldConstraint">Manifold to which the constraint belongs.</param>
///<param name="penetrationConstraint">Penetration constraint associated with this friction constraint.</param>
public void Setup(ContactManifoldConstraint contactManifoldConstraint, ContactPenetrationConstraint penetrationConstraint)
{
this.contactManifoldConstraint = contactManifoldConstraint;
this.penetrationConstraint = penetrationConstraint;
IsActive = true;
linearAX = 0;
linearAY = 0;
linearAZ = 0;
entityA = contactManifoldConstraint.EntityA;
entityB = contactManifoldConstraint.EntityB;
}
///<summary>
/// Constructs a new nonconvex manifold constraint.
///</summary>
public NonConvexContactManifoldConstraint(CollidablePairHandler pairHandler)
: base(pairHandler)
{
//All of the constraints are always in the solver group. Some of them are just deactivated sometimes.
//This reduces some bookkeeping complications.
penetrationConstraints = new RawList <ContactPenetrationConstraint>(4);
frictionConstraints = new RawList <ContactFrictionConstraint>(4);
for (int i = 0; i < 4; i++)
{
var penetrationConstraint = new ContactPenetrationConstraint();
penetrationConstraintPool.Push(penetrationConstraint);
Add(penetrationConstraint);
var frictionConstraint = new ContactFrictionConstraint();
frictionConstraintPool.Push(frictionConstraint);
Add(frictionConstraint);
}
}
///<summary>
/// Constructs a new convex contact manifold constraint.
///</summary>
public ConvexContactManifoldConstraint(CollidablePairHandler pairHandler)
: base(pairHandler)
{
//All of the constraints are always in the solver group. Some of them are just deactivated sometimes.
//This reduces some bookkeeping complications.
penetrationConstraints = new RawList <ContactPenetrationConstraint>(4);
//Order matters in this adding process. Sliding friction computes some information used by the twist friction, and both use penetration impulses.
for (int i = 0; i < 4; i++)
{
var penetrationConstraint = new ContactPenetrationConstraint();
Add(penetrationConstraint);
penetrationConstraintPool.Push(penetrationConstraint);
}
slidingFriction = new SlidingFrictionTwoAxis();
Add(slidingFriction);
twistFriction = new TwistFrictionConstraint();
Add(twistFriction);
}