public void EmitDebuggableConstraint(RevoluteJointManifold m, RJConstraint c, ComponentDataFromEntity <Position> positions, float dt)
{
if (debuggableConstraints != null)
{
debuggableConstraints.Add(new DebuggableConstraint(m, c, positions, dt));
}
}
public DebuggableConstraint(RevoluteJointManifold m, TwoWayTwoDOFConstraint c, ComponentDataFromEntity <Position> positions, float dt)
{
this.m = m;
constraint = c;
x1 = positions[m.e1].pos;
x2 = positions[m.e2].pos;
M_inv = c.M_inv;
this.dt = dt;
}
public TwoWayTwoDOFConstraint(RevoluteJointManifold m, Float6 M_inv, float dt)
{
e1 = m.e1;
e2 = m.e2;
id = m.id;
float2 oneCrossR1 = Lin.Cross(1, m.r1);
float2 oneCrossR2 = Lin.Cross(1, m.r2);
this.M_inv = M_inv;
constraint = new TwoDOFConstraint <Float6>(
J1: new Float6(-1, 0, -oneCrossR1.x, 1, 0, oneCrossR2.x),
J2: new Float6(0, -1, -oneCrossR1.y, 0, 1, oneCrossR2.y),
M_inv: M_inv,
bias: m.delta * m.beta / dt,
softness: m.softness
);
lambdaAccum = float2.zero;
}
public TwoWayTwoDOFConstraint(RevoluteJointManifold m, ComponentDataFromEntity <Mass> masses, float dt) :
this(m, new Float6(masses[m.e1].M_inv, masses[m.e2].M_inv), dt)
{
}
