コード例 #1
0
        // Apply force on a physical object
        public override Impulse ApplyForceOn(RigidBody body)
        {
            // Entry logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Entering method for {0} with {1}", this.Name, body.Name));
      #endif

            // Initialize result as zero
            Impulse result = new Impulse(Vector2.Zero, Vector2.Zero);

            // If the object is outside the field's area of effect, there is no force
            if (!this.Geometry.IntersectsWith(body.Geometry.MinBoundingBox))
            {
                goto exit;
            }

            // For now, we will assume the object is wholly contained by the field,
            // and hence there is only a translational force.  In the future, we may
            // consider torques exerted by 'partially contained' objects, e.g.
            // sticking your arm out the window of a fast-moving car.
            result.Momentum = this.CalculateForceOn(body.Mass, body.Geometry.Centroid);
            result.Point    = body.Geometry.Centroid;


            // [*]  Exit trap
            exit:

            // Exit logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Exiting method for {0} with {1}", this.Name, body.Name));
      #endif

            // Return result
            return(result);
        }
コード例 #2
0
        // Add field impulses to queue
        public override void GetFieldImpulses(float dt)
        {
            // Entry logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Entering method for {0}", this.Name));
      #endif

            // Loop through all force fields in scene
            for (int i = 0; i < this.Scene.ForceFields.Count; i++)
            {
                // Get field impulse
                Impulse impulse = this.Scene.ForceFields[i].GetImpulse(this, dt);

                // If non-zero, apply it
                if (impulse.Momentum != Vector2.Zero)
                {
                    this.AppliedImpulses.Add(impulse);
                }
            }

            // Exit logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Exiting method for {0}", this.Name));
      #endif
        }
コード例 #3
0
        // Decompose an impulse into a translational force and torque
        public ForceAndTorque InterpretImpulse(Impulse appliedImpulse)
        {
            // Entry logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Entering method for {0}", this.Name));
      #endif

            // Get force, the center-of-mass, translational force
            Vector2 force = appliedImpulse.Momentum;
            // Initialize torque as zero
            float torque = 0;

            // Point to impulse vector
            Vector2 impulse = appliedImpulse.Momentum;
            // Point to point of impact
            Vector2 point = appliedImpulse.Point;

            // If the point of impact is equal to centroid, e.g. an object wholly
            // contained by a force field, then this is easy.  There is no torque,
            // only a translational force.
            if (point == this.Geometry.Centroid)
            {
                goto exit;
            }

            // Get radialAxis, the pointOfImpact-to-rotationalAxis vector
            Vector2 radialAxis = (this.RotationalAxis - point).Unit;
            // Get leverArm, the length of this vector
            float leverArm = (this.RotationalAxis - point).Length;

            // Get the tangential force magnitude
            float rotationalForce = Vector2.ComponentMagnitude(impulse, radialAxis.Normal);
            // Compute the torque
            torque = rotationalForce * leverArm;


            // [*]  Exit trap
exit:

            // Exit logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Exiting method for {0}", this.Name));
      #endif

            // Return result
            return(new ForceAndTorque(force, torque));
        }
コード例 #4
0
        // =====
        #region Methods

        // If close enough, apply force on a physical object
        public override Impulse ApplyForceOn(RigidBody body)
        {
            // Entry logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Entering method for {0} with {1}", this.Name, body.Name));
      #endif

            // Initialize result as zero
            Impulse result = new Impulse(Vector2.Zero, Vector2.Zero);

            // Universal fields, by definition, wholly contain all objects
            result.Momentum = this.CalculateForceOn(body.Mass, body.Geometry.Centroid);
            result.Point    = body.Geometry.Centroid;

            // Exit logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Exiting method for {0} with {1}", this.Name, body.Name));
      #endif

            // Return result
            return(result);
        }
コード例 #5
0
        // (Old) Exert penalty forces to all bodies
        public void Penalize()
        {
            // Entry logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Entering method for {0}", this.Name);
      #endif

            if (this.Contacts.Count > 1)
            {
                #region Error:  Multiple contacts not yet implemented

                // Create error message
                String s = "Contact error\n";
                s += "So far, we can only handle singleton contact graphs!\n";
                s += String.Format("Contacts.Count = ", this.Contacts.Count);

                // Throw exception
                throw new ArgumentException(s);

                #endregion
            }
            else
            {
                // Not for collision contacts
                if (!this.Contacts[0].IsResting)
                {
                    return;
                }

                // Get bodyA
                RigidBody bodyA = (RigidBody)this.Contacts[0].BodyA;
                // Get bodyB
                RigidBody bodyB = (RigidBody)this.Contacts[0].BodyB;
                // Get edge-normal axis
                Vector2 n = this.Contacts[0].Axis;
                // Get contact point
                Vector2 p = this.Contacts[0].Points[0];

                #region Error:  Edge-edge not yet implemented
        #if IS_ERROR_CHECKING
                // Check 'remembered' axes count
                if (this.Contacts[0].Points.Count > 1)
                {
                    if (false)
                    {
                        // Create error string
                        String s = "Contact error\n";
                        s += "Edge-edge contact resolution not yet implemented!\n";
                        s += String.Format("bodyA.Name = {0}, bodyB.Name = {1}\n", bodyA.Name, bodyB.Name);

                        // Throw exception
                        throw new SystemException(s);
                    }
                }
        #endif
                #endregion

                #region Get contact data

                // Get elasticity
                float e = 0.00f;
                // Get mass
                float mA = bodyA.Mass;
                float mB = bodyB.Mass;
                if (!bodyA.IsTranslatable)
                {
                    mA = float.MaxValue;
                }
                if (!bodyB.IsTranslatable)
                {
                    mB = float.MaxValue;
                }
                // Get moment of inertia
                float IA = bodyA.MomentOfInertia;
                float IB = bodyB.MomentOfInertia;
                if (!bodyA.IsRotatable)
                {
                    IA = float.MaxValue;
                }
                if (!bodyB.IsRotatable)
                {
                    IB = float.MaxValue;
                }
                // Get centroid point
                Vector2 cA = bodyA.Geometry.Centroid;
                Vector2 cB = bodyB.Geometry.Centroid;
                // Get centroid-to-point vector
                Vector2 rA = p - cA;
                Vector2 rB = p - cB;

                // Get initial centroid velocity
                Vector2 vA1 = bodyA.Velocity;
                Vector2 vB1 = bodyB.Velocity;
                // Get initial angular velocity
                float wA1 = bodyA.AngularVelocity;
                float wB1 = bodyB.AngularVelocity;
                // Get initial point velocity
                Vector2 vA1p = vA1 - wA1 * rA.Normal;
                Vector2 vB1p = vB1 - wB1 * rB.Normal;
                // Get initial point velocity projected
                float vA1pn = Vector2.Project(vA1p, n);
                float vB1pn = Vector2.Project(vB1p, n);
                // Get initial point velocity projected relative
                float vR1pn = vA1pn - vB1pn;

                #endregion

                #region Calculate impulses

                // Calculate the impulse parameter j
                // float j = (-1 * (1 + e) * vR1pn) / ((1 / mA) + (1 / mB) + ((float)Math.Pow((rA.X * n.Y - rA.Y * n.X), 2) / IA) + ((float)Math.Pow((rB.X * n.Y - rB.Y * n.X), 2) / IB));
                float kp = 50 * 6000.0f;
                float kv = 50 * 3000.0f;
                float d = this.Contacts[0].Distance;
                float j = kp * (-d) + kv * (-vR1pn);

                // Make sure the bodies aren't actually moving away from each other
                if (vR1pn > 0)
                {
          #if IS_LOGGING_PHYSICS
                    if (Log.Subject1 == null || (bodyA.Name == Log.Subject1 && (Log.Subject2 == null || bodyB.Name == Log.Subject2)))
                    {
                        Log.Write(String.Format("j = {0}, but we set it to zero since vRpn = {1}", j, vA1pn, vR1pn));
                    }
          #endif
                    j = 0;
                }

                // Get final centroid velocity
                Vector2 vA2 = vA1 + (j / mA) * n;
                Vector2 vB2 = vB1 - (j / mB) * n;

                // Get impulse on object A
                Vector2 F = mA * (vA2 - vA1);

                // Get final angular velocity
                float wA2 = wA1 + (rA.X * j * n.Y - rA.Y * j * n.X) / IA;
                float wB2 = wB1 - (rB.X * j * n.Y - rB.Y * j * n.X) / IB;
                // Get final point velocity
                Vector2 vA2p = vA2 - wA2 * rA.Normal;
                Vector2 vB2p = vB2 - wB2 * rB.Normal;
                // Get final point velocity projected
                float vA2pn = Vector2.Project(vA2p, n);
                float vB2pn = Vector2.Project(vB2p, n);
                // Get final point velocity projected relative
                float vR2pn = vA2pn - vB2pn;

                // Get initial point acceleration (VERIFY)
                Vector2 aA1p = -wA1 * rA.Normal - wA1 * rA;
                Vector2 aB1p = -wB1 * rB.Normal - wB1 * rB;
                // Get initial point acceleration projected
                float aA1pn = Vector2.Project(aA1p, n);
                float aB1pn = Vector2.Project(aB1p, n);
                // Get initial point acceleration projected relative
                float aR1pn = aA1pn - aB1pn;

                // Get initial contact edge
                LineSegment edge = this.Contacts[0].Edge;
                // Get edge point 1 derivative
                Vector2 e1dot = -wB1 * (edge.Point1 - cB).Normal + vB1;
                // Get edge point 2 derivative
                Vector2 e2dot = -wB1 * (edge.Point2 - cB).Normal + vB1;
                // Get edge normal derivative
                Vector2 ndot = (1 / edge.Length) * new Vector2(e2dot.Y - e1dot.Y, e1dot.X - e2dot.X);

                // Get initial point 1st derivative
                Vector2 pA1pdot = -wA1 * rA.Normal + vA1;
                Vector2 pB1pdot = -wB1 * rB.Normal + vB1;
                // Get initial point 2nd derivative
                Vector2 pA1pdot2 = -wA1 * rA.Normal + wA1 * rA + Vector2.Zero;
                Vector2 pB1pdot2 = -wB1 * rB.Normal + wB1 * rB + Vector2.Zero;

                // Get initial contact distance
                float d1 = this.Contacts[0].Distance;
                // Get initial contact distance 1st derivative
                float d1dot = Vector2.Project(Vector2.Zero, ndot) + Vector2.Project(pA1pdot - pB1pdot, n);
                // Get initial contact distance 2nd derivative
                float d1dot2 = Vector2.Dot(pA1pdot2 - pB1pdot2, n) + 2 * Vector2.Dot(pA1pdot - pB1pdot, ndot);

                // Equation:  d_i = a_ij * f + b_i
                // d_i  = contact distance acceleration
                // a_ij = force-dependent terms
                // f    = scalar force
                // b_i  = force-independent terms

                // Get 'a_ij'
                // Get 'b_i'

                #endregion

                #region Log summary

                // Test logging
        #if IS_LOGGING_PHYSICS
                if (Log.Subject1 == null || (bodyA.Name == Log.Subject1 || bodyB.Name == Log.Subject1) && (Log.Subject2 == null || (bodyA.Name == Log.Subject2 || bodyB.Name == Log.Subject2)))
                {
                    Log.Write("Now beginning collision response summary...");

                    Log.Write(String.Format("bodyA = {0}, bodyB = {1}, isResting = {2}", bodyA.Name, bodyB.Name, this.Contacts[0].IsResting));
                    Log.Write(String.Format("n = {0}, p = {1}, e = {2}", n, p, e));

                    Log.Write(String.Format("vA1   = {0}, vB1   = {1}", vA1, vB1));
                    Log.Write(String.Format("vA2   = {0}, vB2   = {1}", vA2, vB2));

                    Log.Write(String.Format("wA1   = {0:+0000.0000;-0000.0000; 0000.0000}, wB1   = {1:+0000.0000;-0000.0000; 0000.0000}", wA1, wB1));
                    Log.Write(String.Format("wA2   = {0:+0000.0000;-0000.0000; 0000.0000}, wB2   = {1:+0000.0000;-0000.0000; 0000.0000}", wA2, wB2));

                    Log.Write(String.Format("vA1p  = {0}, vB1p  = {1}", vA1p, vB1p));
                    Log.Write(String.Format("vA2p  = {0}, vB2p  = {1}", vA2p, vB2p));

                    Log.Write(String.Format("vA1pn = {0:+0000.0000;-0000.0000; 0000.0000}, vB1pn = {1:+0000.0000;-0000.0000; 0000.0000}", vA1pn, vB1pn));
                    Log.Write(String.Format("vA2pn = {0:+0000.0000;-0000.0000; 0000.0000}, vB2pn = {1:+0000.0000;-0000.0000; 0000.0000}", vA2pn, vB2pn));
                    Log.Write(String.Format("vR1pn = {0:+0000.0000;-0000.0000; 0000.0000}", vR1pn));
                    Log.Write(String.Format("vR2pn = {0:+0000.0000;-0000.0000; 0000.0000}", vR2pn));

                    /* Log.Write(String.Format("j = {0:+0.0000E+0;-0.0000E+0;+0.0000E+0}, F = {1:+0.0000E+0;-0.0000E+0;+0.0000E+0}", j, F));
                     *
                     * Log.Write(String.Format("aA1p  = {0}", aA1p));
                     * Log.Write(String.Format("aB1p  = {0}", aB1p));
                     * Log.Write(String.Format("aA1pn = {0:+0000.0000;-0000.0000; 0000.0000}", aA1pn));
                     * Log.Write(String.Format("aB1pn = {0:+0000.0000;-0000.0000; 0000.0000}", aB1pn));
                     * Log.Write(String.Format("aR1pn = {0:+0000.0000;-0000.0000; 0000.0000}", aR1pn));
                     *
                     * Log.Write(String.Format("edge    = {0}", edge));
                     * Log.Write(String.Format("e1dot   = {0}", e1dot));
                     * Log.Write(String.Format("e2dot   = {0}", e2dot));
                     * Log.Write(String.Format("ndot    = {0}", ndot));
                     *
                     * Log.Write(String.Format("pA1pdot  = {0}, pB1pdot  = {1}", pA1pdot, pB1pdot));
                     * Log.Write(String.Format("pA1pdot2 = {0}, pB1pdot2 = {1}", pA1pdot2, pB1pdot2));
                     * Log.Write(String.Format("d1     = {0:+0000.0000;-0000.0000; 0000.0000}", d1));
                     * Log.Write(String.Format("d1dot  = {0:+0000.0000;-0000.0000; 0000.0000}", d1dot));
                     * Log.Write(String.Format("d1dot2 = {0:+0000.0000;-0000.0000; 0000.0000}", d1dot2)); */
                }
        #endif

                #endregion

                // Create contact impulse
                Impulse contactImpulse = new Impulse(F, p);
                // Feel neighbor's normal force
                bodyA.AppliedImpulses.Add(contactImpulse);

                // Negate contact impulse
                contactImpulse = new Impulse(-F, p);
                // Exert normal force on neighbor
                bodyB.AppliedImpulses.Add(contactImpulse);
            }

            // Exit logging
      #if IS_LOGGING_METHODS
            Log.Write(String.Format("Exiting method for {0}", this.Name);
      #endif
        }