private void IntegreationForce(RigidBody obj)
{
if (PhyMath.EpsilonEqual(obj.InverseMass, 0.0f))
{
return;
}
obj.Velocity += (obj.InverseMass * obj.Fource + this.Gravity * obj.GravityScale) * this.UpdateTime;
obj.AngularVelocity += obj.InverseInertia * obj.Torque * this.UpdateTime;
}
public List <Pair <RigidBody> > GetPairs()
{
List <Pair <RigidBody> > pairs = new List <Pair <RigidBody> >();
for (int i = 0; i < this.mColliders.Count; ++i)
{
AABBCollider A = this.mColliders[i];
for (int j = i + 1; j < this.mColliders.Count; ++j)
{
AABBCollider B = this.mColliders[j];
if (PhyMath.EpsilonEqual(A.Body.InverseMass + B.Body.InverseMass, 0.0f))
{
continue;
}
if (A.AABB.IsIntersection(B.AABB))
{
pairs.Add(new Pair <RigidBody>(A.Body, B.Body));
}
}
}
return(pairs);
}
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);
}
}
