public Partial(Entity e1, Entity e2, Position pos1, Position pos2, Geometry.Manifold m, bool useContact1)
{
this.e1 = e1;
this.e2 = e2;
var contactWithId = useContact1 ? m.contact1 : (Geometry.Contact)m.contact2;
float2 contact = contactWithId.point;
id = contactWithId.id.GetHashCode();
{ // Normal precomputation
J_n = new Float6(
new float3(-m.normal, -Lin.Cross(contact - pos1.pos, m.normal)),
new float3(m.normal, Lin.Cross(contact - pos2.pos, m.normal))
);
delta = -m.overlap;
}
{ // Tangent precomputation (friction)
float2 tangent = Lin.Cross(m.normal, -1);
J_t = new Float6(
new float3(tangent, Lin.Cross(contact - pos1.pos, tangent)),
new float3(-tangent, -Lin.Cross(contact - pos2.pos, tangent)));
}
}
public void EmitDebuggableConstraint(Geometry.Manifold m, TwoWayPenFricConstraint.Partial p, TwoWayPenFricConstraint c, bool useContact1, float dt)
{
if (debuggableConstraints != null)
{
debuggableConstraints.Add(new DebuggableConstraint(m, p, c, useContact1, dt: dt));
}
}
public DebuggableConstraint(Geometry.Manifold m, TwoWayPenFricConstraint.Partial p, TwoWayPenFricConstraint c, bool useContact1, float dt)
{
this.m = m;
this.useContact1 = useContact1;
constraint = c;
this.dt = dt;
this.p = p;
}
private void Compare(Geometry.Manifold m1, Geometry.Manifold m2)
{
Compare(m1.normal, m2.normal);
if (m1.contact2 == null || m2.contact2 == null)
{
Assert.IsNull(m2.contact2);
}
else
{
Assert.IsTrue(
(Equal(m1.contact1, m2.contact1) && Equal((Geometry.Contact)m1.contact2, (Geometry.Contact)m2.contact2)) ||
(Equal(m1.contact1, (Geometry.Contact)m2.contact2) && Equal((Geometry.Contact)m1.contact2, m2.contact1)));
}
}
