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; }