public void ApplyImpulse(Vector2 impulse, Vector2 contact)
{
this.Velocity += this.InverseMass * impulse;
this.AngularVelocity += this.InverseInertia * Vector2Ext.Cross(contact, impulse);
}
public void ApplyImpluse()
{
float im_sum = (this.A.InverseMass + this.B.InverseMass);
if (PhyMath.EpsilonEqual(im_sum, 0.0f))
{
this.A.Velocity = Vector2.Zero;
this.B.Velocity = Vector2.Zero;
return;
}
for (int i = 0; i < this.ContactPoints.Length; ++i)
{
Vector2 ra = this.ContactPoints[i] - this.A.Transform.Position;
Vector2 rb = this.ContactPoints[i] - this.B.Transform.Position;
Vector2 romega = Vector2Ext.Cross(this.B.AngularVelocity, rb) - Vector2Ext.Cross(this.A.AngularVelocity, ra);
Vector2 rv = this.B.Velocity - this.A.Velocity + romega;
float normal_len = Vector2.Dot(rv, this.Normal);
if (normal_len > 0)
{
return;
}
float ra_cross_normal = Vector2Ext.Cross(ra, this.Normal);
float rb_cross_normal = Vector2Ext.Cross(rb, this.Normal);
float j = -(this.Restitutoin + 1) * normal_len / (im_sum +
ra_cross_normal * ra_cross_normal * this.A.InverseInertia +
rb_cross_normal * rb_cross_normal * this.B.InverseInertia) /
this.ContactPoints.Length;
Vector2 impulse = j * this.Normal;
this.A.ApplyImpulse(-impulse, ra);
this.B.ApplyImpulse(impulse, rb);
Vector2 tangent = (rv - (normal_len * this.Normal));
if (tangent.Length == 0.0f)
{
tangent = Vector2.Zero;
}
else
{
tangent.Normalize();
}
float ra_cross_tangent = Vector2Ext.Cross(ra, tangent);
float rb_cross_tangent = Vector2Ext.Cross(rb, tangent);
float jt = -Vector2.Dot(tangent, rv) / (im_sum +
ra_cross_tangent * ra_cross_tangent * this.A.InverseInertia +
rb_cross_tangent * rb_cross_tangent * this.B.InverseInertia) /
this.ContactPoints.Length;
if (jt == 0)
{
return;
}
Vector2 friction_impulse = tangent * ((Math.Abs(jt) < j * this.us) ? jt : -(j * this.uk));
this.A.ApplyImpulse(-friction_impulse, ra);
this.B.ApplyImpulse(friction_impulse, rb);
}
}
public void ApplyForce(Vector2 force, Vector2 contact)
{
this.Fource += force;
this.Torque += Vector2Ext.Cross(contact, force);
}