Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
0
        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);
        }
Exemplo n.º 3
0
        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);
            }
        }