///<summary>
/// Configures the penetration constraint.
///</summary>
///<param name="contactManifoldConstraint">Owning manifold constraint.</param>
///<param name="contact">Contact associated with the penetration constraint.</param>
public void Setup(ContactManifoldConstraint contactManifoldConstraint, Contact contact)
{
this.contactManifoldConstraint = contactManifoldConstraint;
this.contact = contact;
isActive = true;
entityA = contactManifoldConstraint.EntityA;
entityB = contactManifoldConstraint.EntityB;
}
///<summary>
/// Cleans up the constraint.
///</summary>
public void CleanUp()
{
accumulatedImpulse = 0;
contactManifoldConstraint = null;
contact = null;
entityA = null;
entityB = null;
isActive = false;
}
///<summary>
/// Configures the penetration constraint.
///</summary>
///<param name="contactManifoldConstraint">Owning manifold constraint.</param>
///<param name="contact">Contact associated with the penetration constraint.</param>
public void Setup(ContactManifoldConstraint contactManifoldConstraint, Contact contact)
{
this.contactManifoldConstraint = contactManifoldConstraint;
this.contact = contact;
isActive = true;
entityA = contactManifoldConstraint.EntityA;
entityB = contactManifoldConstraint.EntityB;
entityADynamic = entityA != null && entityA.isDynamic;
entityBDynamic = entityB != null && entityB.isDynamic;
}
public static unsafe 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}");
}
void IPairHandlerParent.OnContactAdded(Contact contact)
{
contactCount++;
OnContactAdded(contact);
}
protected virtual void OnContactRemoved(Contact contact)
{
//Children manage the removal of the contact from the constraint, if any.
if (!suppressEvents)
{
CollidableA.EventTriggerer.OnContactRemoved(CollidableB, this, contact);
CollidableB.EventTriggerer.OnContactRemoved(CollidableA, this, contact);
}
if (Parent != null)
Parent.OnContactRemoved(contact);
}
protected override void OnContactAdded(Contact contact)
{
ContactConstraint.AddContact(contact);
base.OnContactAdded(contact);
}
protected override void OnContactRemoved(Contact contact)
{
ContactConstraint.RemoveContact(contact);
base.OnContactRemoved(contact);
}
//TODO: PROBLEM IS that the add contact/remove contact, when they go from 0 -> !0 or !0 -> 0, the whole constraint is added/removed from the solver.
//The Added/Removed contact methods here will run ambiguously before or after they are removed from the solver.
//That ambiguous order doesn't really matter though, since everything that these add/remove methods do is local to this solver object and its children.
//It doesn't go out and modify any external values on referenced entities. That only happens when it's added or removed from the solver by whatever owns this object!
//To avoid ANY ambiguity, some third party is now responsible for adding and removing contacts from this.
///<summary>
/// Adds a contact to be managed by the constraint.
///</summary>
///<param name="contact">Contact to add.</param>
public override void AddContact(Contact contact)
{
contact.Validate();
var penetrationConstraint = penetrationConstraintPool.Pop();
penetrationConstraint.Setup(this, contact);
penetrationConstraints.Add(penetrationConstraint);
var frictionConstraint = frictionConstraintPool.Pop();
frictionConstraint.Setup(this, penetrationConstraint);
frictionConstraints.Add(frictionConstraint);
}
///<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> /// Adds a contact to be managed by the constraint. ///</summary> ///<param name="contact">Contact to add.</param> public abstract void AddContact(Contact contact);
///<summary> /// Removes a contact from the constraint. ///</summary> ///<param name="contact">Contact to remove.</param> public abstract void RemoveContact(Contact contact);
void IPairHandlerParent.OnContactRemoved(Contact contact)
{
contactCount--;
OnContactRemoved(contact);
}
protected virtual void OnContactAdded(Contact contact)
{
//Children manage the addition of the contact to the constraint, if any.
if (!suppressEvents)
{
CollidableA.EventTriggerer.OnContactCreated(CollidableB, this, contact);
CollidableB.EventTriggerer.OnContactCreated(CollidableA, this, contact);
}
if (Parent != null)
Parent.OnContactAdded(contact);
}
///<summary>
/// Removes a contact from the constraint.
///</summary>
///<param name="contact">Contact to remove.</param>
public override void RemoveContact(Contact contact)
{
for (int i = 0; i < penetrationConstraints.Count; i++)
{
ContactPenetrationConstraint penetrationConstraint;
if ((penetrationConstraint = penetrationConstraints.Elements[i]).contact == contact)
{
penetrationConstraint.CleanUp();
penetrationConstraints.RemoveAt(i);
penetrationConstraintPool.Push(penetrationConstraint);
break;
}
}
if (penetrationConstraints.Count == 0)
{
//No more contacts. Disable everything.
//Don't have to worry about speculative contacts here; if there existed a regular manifold contact, there couldn't now exist a speculative contact.
twistFriction.CleanUp();
slidingFriction.CleanUp();
}
}
///<summary>
/// Adds a contact to be managed by the constraint.
///</summary>
///<param name="contact">Contact to add.</param>
public override void AddContact(Contact contact)
{
var penetrationConstraint = penetrationConstraintPool.Pop();
penetrationConstraint.Setup(this, contact);
penetrationConstraints.Add(penetrationConstraint);
if (!twistFriction.isActive)
{
//This is the first real contact. All constraints need to become active.
twistFriction.Setup(this);
slidingFriction.Setup(this);
}
}
///<summary>
/// Removes a contact from the constraint.
///</summary>
///<param name="contact">Contact to remove.</param>
public override void RemoveContact(Contact contact)
{
for (int i = 0; i < penetrationConstraints.count; i++)
{
ContactPenetrationConstraint penetrationConstraint;
if ((penetrationConstraint = penetrationConstraints.Elements[i]).contact == contact)
{
penetrationConstraint.CleanUp();
penetrationConstraints.RemoveAt(i);
penetrationConstraintPool.Push(penetrationConstraint);
break;
}
}
if (penetrationConstraints.count == 0)
{
//No more contacts. Disable everything.
twistFriction.CleanUp();
slidingFriction.CleanUp();
}
}
