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 ApplyImpulses(ref Velocity v1, ref Velocity v2, float dt)
{
Float6 v = GetV(ref v1, ref v2);
Float6 P = constraint.GetImpulse(v, ref lambdaAccum);
ApplyImpulse(P, ref v1, ref v2);
}
private void ApplyImpulse(Float6 impulse, ref Velocity v1, ref Velocity v2)
{
impulse = impulse.Mult(M_inv);
v1.vel += impulse.v1.xy;
v1.angVel += impulse.v1.z;
v2.vel += impulse.v2.xy;
v2.angVel += impulse.v2.z;
}
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 void WarmStart(ComponentDataFromEntity <Velocity> vels, float dt, Lambda prevLambda)
{
lambdaAccum = prevLambda;
var v1 = vels[e1];
var v2 = vels[e2];
if (CollisionSystem.accumulateImpulses)
{
Float6 P_n = constraint.GetImpulse(lambdaAccum);
ApplyImpulse(P_n, ref v1, ref v2);
}
vels[e1] = v1;
vels[e2] = v2;
}
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;
}
