コード例 #1
0
ファイル: FSPulleyJoint.cs プロジェクト: migreva/CocoBounce
        /// <summary>
        /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
        /// This requires two ground anchors,
        /// two dynamic body anchor points, max lengths for each side,
        /// and a pulley ratio.
        /// </summary>
        /// <param name="bA">The first body.</param>
        /// <param name="bB">The second body.</param>
        /// <param name="groundA">The ground anchor for the first body.</param>
        /// <param name="groundB">The ground anchor for the second body.</param>
        /// <param name="anchorA">The first body anchor.</param>
        /// <param name="anchorB">The second body anchor.</param>
        /// <param name="ratio">The ratio.</param>
        public FSPulleyJoint(FSBody bA, FSBody bB, FVector2 groundA, FVector2 groundB, FVector2 anchorA, FVector2 anchorB, float ratio)
            : base(bA, bB)
        {
            JointType = JointType.Pulley;

            GroundAnchorA = groundA;
            GroundAnchorB = groundB;
            LocalAnchorA  = anchorA;
            LocalAnchorB  = anchorB;

            Debug.Assert(ratio != 0.0f);
            Debug.Assert(ratio > FSSettings.Epsilon);

            Ratio = ratio;

            FVector2 dA = BodyA.GetWorldPoint(anchorA) - groundA;

            LengthA = dA.Length();

            FVector2 dB = BodyB.GetWorldPoint(anchorB) - groundB;

            LengthB = dB.Length();

            m_constant = LengthA + ratio * LengthB;

            _impulse = 0.0f;
        }
コード例 #2
0
        /// <summary>Initializes this joint with the specified parameters.</summary>
        internal void Initialize(
            WorldPoint anchor,
            Vector2 axis,
            float frequency,
            float dampingRatio,
            float maxMotorTorque,
            float motorSpeed,
            bool enableMotor)
        {
            _localAnchorA = BodyA.GetLocalPoint(anchor);
            _localAnchorB = BodyB.GetLocalPoint(anchor);
            _localXAxisA  = BodyA.GetLocalVector(axis);
            _localYAxisA  = Vector2Util.Cross(1.0f, ref _localXAxisA);

            _frequency    = frequency;
            _dampingRatio = dampingRatio;

            _maxMotorTorque = maxMotorTorque;
            _motorSpeed     = motorSpeed;
            _enableMotor    = enableMotor;

            _impulse       = 0;
            _motorImpulse  = 0;
            _springImpulse = 0;
        }
コード例 #3
0
        public void ApplyForce()
        {
            //Note: The spring force has a direction which is given by the vector between BodyB and BodyA
            // The magnitude of the spring force is caculated using the Hooke's law


            //Vector between the two masses attached to the spring
            Vector2D s_vec = BodyB.Position - BodyA.Position;

            if (s_vec.Length() == 0)
            {
                return;
            }


            //Distance between the two masses, i.e. the length of the spring
            float lengthDifference = s_vec.Length() - RestLength;


            //Compute the spring force based on Hooke's law
            float force = Stiffness * -1 * lengthDifference * Dampen;


            //Apply the spring force to the two bodies joined by the spring

            appliedForce = s_vec * (force / s_vec.Length());

            BodyA.SetForce(BodyA.Force - appliedForce);

            BodyB.SetForce(BodyB.Force + appliedForce);
        }
コード例 #4
0
        /// <summary>
        /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
        /// This requires two ground anchors,
        /// two dynamic body anchor points, max lengths for each side,
        /// and a pulley ratio.
        /// </summary>
        /// <param name="bA">The first body.</param>
        /// <param name="bB">The second body.</param>
        /// <param name="groundA">The ground anchor for the first body.</param>
        /// <param name="groundB">The ground anchor for the second body.</param>
        /// <param name="anchorA">The first body anchor.</param>
        /// <param name="anchorB">The second body anchor.</param>
        /// <param name="ratio">The ratio.</param>
        public PulleyJoint(Body bA, Body bB, Vector2 groundA, Vector2 groundB, Vector2 anchorA, Vector2 anchorB, float ratio)
            : base(bA, bB)
        {
            JointType = JointType.Pulley;

            GroundAnchorA = groundA;
            GroundAnchorB = groundB;
            LocalAnchorA  = BodyA.GetLocalPoint(anchorA);
            LocalAnchorB  = BodyB.GetLocalPoint(anchorB);

            Debug.Assert(ratio != 0.0f);
            Debug.Assert(ratio > Settings.Epsilon);

            Ratio = ratio;

            Vector2 dA = anchorA - groundA;

            LengthA = dA.Length();
            Vector2 dB = anchorB - groundB;

            LengthB = dB.Length();

            _constant = LengthA + ratio * LengthB;

            _impulse = 0.0f;
        }
コード例 #5
0
        /// <inheritdoc/>
        protected override bool OnApplyImpulse()
        {
            // Compute relative velocity.
            Vector3F vA = BodyA.GetVelocityOfWorldPoint(_anchorAWorld);
            Vector3F vB = BodyB.GetVelocityOfWorldPoint(_anchorBWorld);
            Vector3F relativeVelocity = (vB - vA);

            // Compute constraint impulse.
            Vector3F impulse = _kInverse * (_targetVelocity - relativeVelocity - Softness / _deltaTime * _constraintImpulse);

            // Impulse accumulation and clamping.
            Vector3F oldConstraintImpulse = _constraintImpulse;

            _constraintImpulse += impulse;
            float impulseMagnitude    = _constraintImpulse.Length;
            float maxImpulseMagnitude = MaxForce * _deltaTime;

            if (impulseMagnitude > maxImpulseMagnitude)
            {
                _constraintImpulse = _constraintImpulse / impulseMagnitude * maxImpulseMagnitude;
                impulse            = _constraintImpulse - oldConstraintImpulse;
            }

            // Apply impulses
            BodyA.ApplyImpulse(-impulse, _anchorAWorld);
            BodyB.ApplyImpulse(impulse, _anchorBWorld);

            return(impulse.LengthSquared > Simulation.Settings.Constraints.MinConstraintImpulseSquared);
        }
コード例 #6
0
ファイル: PrismaticJoint.cs プロジェクト: matrix4x4/Space
        /// <summary>Initializes this joint with the specified parameters.</summary>
        internal void Initialize(
            WorldPoint anchor,
            Vector2 axis,
            float lowerTranslation = 0,
            float upperTranslation = 0,
            float maxMotorForce    = 0,
            float motorSpeed       = 0,
            bool enableLimit       = false,
            bool enableMotor       = false)
        {
            _localAnchorA = BodyA.GetLocalPoint(anchor);
            _localAnchorB = BodyB.GetLocalPoint(anchor);
            _localXAxisA  = BodyA.GetLocalVector(axis);
            _localXAxisA.Normalize();
            _localYAxisA      = Vector2Util.Cross(1.0f, ref _localXAxisA);
            _referenceAngle   = BodyB.Angle - BodyA.Angle;
            _lowerTranslation = lowerTranslation;
            _upperTranslation = upperTranslation;
            _maxMotorForce    = maxMotorForce;
            _motorSpeed       = motorSpeed;
            _enableLimit      = enableLimit;
            _enableMotor      = enableMotor;

            _impulse      = Vector3.Zero;
            _motorImpulse = 0.0f;
            _limitState   = LimitState.Inactive;
        }
コード例 #7
0
        /// <summary>
        /// This requires defining an
        /// anchor shipPosition on both bodies and the non-zero length of the
        /// distance joint. If you don't supply a length, the local anchor vectors2
        /// is used so that the initial configuration can violate the constraint
        /// slightly. This helps when saving and loading a game.
        /// Warning Do not use a zero or short length.
        /// </summary>
        /// <param slotName="bodyA">The first body</param>
        /// <param slotName="bodyB">The second body</param>
        /// <param slotName="anchorA">The first body anchor</param>
        /// <param slotName="anchorB">The second body anchor</param>
        /// <param slotName="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
        ///

        public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB)
        {
            JointType    = JointType.Distance;
            LocalAnchorA = anchorA;
            LocalAnchorB = anchorB;
            Length       = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length();
        }
コード例 #8
0
        /// Get the current length of the segment attached to bodyB.
        public float GetCurrentLengthB()
        {
            var p = BodyB.GetWorldPoint(_localAnchorB);
            var s = _groundAnchorB;
            var d = p - s;

            return(d.Length());
        }
コード例 #9
0
    protected override void Initialise2(IntPtr world)
    {
        Vector3 anchorA = transform.position - BodyA.transform.position;
        Vector3 anchorB = transform.position - BodyB.transform.position;

        LPAPIJoint.CreateWeldJoint(world, BodyA.GetComponent <LPBody>().GetPtr(), BodyB.GetComponent <LPBody>().GetPtr()
                                   , anchorA.x, anchorA.y, anchorB.x, anchorB.y, CollideConnected);
    }
コード例 #10
0
ファイル: RopeJoint.cs プロジェクト: matrix4x4/Space
        /// <summary>Initializes this joint with the specified parameters.</summary>
        internal void Initialize(WorldPoint anchorA, WorldPoint anchorB, float length)
        {
            _localAnchorA = BodyA.GetLocalPoint(anchorA);
            _localAnchorB = BodyB.GetLocalPoint(anchorB);
            _maxLength    = length;

            _impulse = 0;
            _length  = 0;
        }
コード例 #11
0
ファイル: DistanceJoint.cs プロジェクト: Deusald/SharpBox2D
        public float GetCurrentLength()
        {
            var pA     = BodyA.GetWorldPoint(_localAnchorA);
            var pB     = BodyB.GetWorldPoint(_localAnchorB);
            var d      = pB - pA;
            var length = d.Length();

            return(length);
        }
コード例 #12
0
 /// <summary>
 /// This requires defining an
 /// anchor point on both bodies and the non-zero length of the
 /// distance joint. If you don't supply a length, the local anchor points
 /// is used so that the initial configuration can violate the constraint
 /// slightly. This helps when saving and loading a game.
 /// Warning Do not use a zero or short length.
 /// </summary>
 /// <param name="bodyA">The first body</param>
 /// <param name="bodyB">The second body</param>
 /// <param name="anchorA">The first body anchor</param>
 /// <param name="anchorB">The second body anchor</param>
 public DistanceJoint(EntityUid bodyA, EntityUid bodyB, Vector2 anchorA, Vector2 anchorB)
     : base(bodyA, bodyB)
 {
     Length       = MathF.Max(PhysicsConstants.LinearSlop, (BodyB.GetWorldPoint(anchorB) - BodyA.GetWorldPoint(anchorA)).Length);
     _minLength   = _length;
     _maxLength   = _length;
     LocalAnchorA = anchorA;
     LocalAnchorB = anchorB;
 }
コード例 #13
0
    public override void ApplyForce(float dt)
    {
        Vector2 force = SpringForce(BodyA.Position, BodyB.Position, RestLength, SpringPower);

        // modifier modifies the amount of force to use, if there is a static object then apply 100% force to the objects if they are not static then split the force 50/50
        float modifier = (BodyA.Type == BodyTypeEnumRef.eType.Static || BodyB.Type == BodyTypeEnumRef.eType.Static) ? 1.0f : 0.5f;

        BodyA.ApplyForce(-force * modifier, PhysicsBody.eForceMode.IMPULSE);
        BodyB.ApplyForce(force * modifier, PhysicsBody.eForceMode.IMPULSE);
    }
コード例 #14
0
        public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis)
            : base(body)
        {
            JointType = JointType.FixedLine;

            BodyB = BodyA;

            LocalAnchorA = worldAnchor;
            LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
            LocalXAxis   = axis;
        }
コード例 #15
0
        /// <summary>Initializes this joint with the specified parameters.</summary>
        internal void Initialize(WorldPoint anchor, float frequency, float dampingRatio)
        {
            _localAnchorA   = BodyA.GetLocalPoint(anchor);
            _localAnchorB   = BodyB.GetLocalPoint(anchor);
            _referenceAngle = BodyB.Angle - BodyA.Angle;

            _frequency    = frequency;
            _dampingRatio = dampingRatio;

            _impulse = Vector3.Zero;
        }
コード例 #16
0
        /// Get the current joint translation, usually in meters.
        public float GetJointTranslation()
        {
            var pA   = BodyA.GetWorldPoint(LocalAnchorA);
            var pB   = BodyB.GetWorldPoint(LocalAnchorB);
            var d    = pB - pA;
            var axis = BodyA.GetWorldVector(LocalXAxisA);

            var translation = Vector2.Dot(d, axis);

            return(translation);
        }
コード例 #17
0
 /// <summary>Initializes the specified local anchor A.</summary>
 /// <param name="anchorA">The world anchor point for the first body.</param>
 /// <param name="anchorB">The world anchor point for the second body.</param>
 /// <param name="frequency">The mass-spring-damper frequency in Hertz. A value of 0 disables softness.</param>
 /// <param name="dampingRatio">The damping ratio. 0 = no damping, 1 = critical damping.</param>
 internal void Initialize(
     WorldPoint anchorA,
     WorldPoint anchorB,
     float frequency,
     float dampingRatio)
 {
     _localAnchorA = BodyA.GetLocalPoint(anchorA);
     _localAnchorB = BodyB.GetLocalPoint(anchorB);
     _length       = System.Math.Max(0.1f, WorldPoint.Distance(anchorA, anchorB));
     _frequency    = System.Math.Max(0, frequency);
     _dampingRatio = System.Math.Max(0, dampingRatio);
 }
コード例 #18
0
ファイル: GearJoint.cs プロジェクト: jwmcglynn/float
        public override float GetReactionTorque(float inv_dt)
        {
            Transform xf1;

            BodyB.GetTransform(out xf1);

            Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter);
            Vector2 P = _impulse * _J.LinearB;
            float   L = _impulse * _J.AngularB - MathUtils.Cross(r, P);

            return(inv_dt * L);
        }
コード例 #19
0
        /// <summary>
        /// This requires defining an
        /// anchor point on both bodies and the non-zero length of the
        /// distance joint. If you don't supply a length, the local anchor points
        /// is used so that the initial configuration can violate the constraint
        /// slightly. This helps when saving and loading a game.
        /// Warning Do not use a zero or short length.
        /// </summary>
        /// <param name="bodyA">The first body</param>
        /// <param name="bodyB">The second body</param>
        /// <param name="anchorA">The first body anchor</param>
        /// <param name="anchorB">The second body anchor</param>
        public DistanceJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, Vector2 anchorA, Vector2 anchorB)
            : base(bodyA, bodyB)
        {
            LocalAnchorA = anchorA;
            LocalAnchorB = anchorB;
            // TODO: Just pass this into the ctor.
            var configManager = IoCManager.Resolve <IConfigurationManager>();

            Length       = MathF.Max(configManager.GetCVar(CVars.LinearSlop), (BodyB.GetWorldPoint(anchorB) - BodyA.GetWorldPoint(anchorA)).Length);
            WarmStarting = configManager.GetCVar(CVars.WarmStarting);
            _linearSlop  = configManager.GetCVar(CVars.LinearSlop);
            _minLength   = _length;
            _maxLength   = _length;
        }
コード例 #20
0
 /// <summary>Constructor for FrictionJoint.</summary>
 /// <param name="bodyA"></param>
 /// <param name="bodyB"></param>
 /// <param name="anchor"></param>
 /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
 public FrictionJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
     : base(bodyA, bodyB, JointType.Friction)
 {
     if (useWorldCoordinates)
     {
         LocalAnchorA = BodyA.GetLocalPoint(anchor);
         LocalAnchorB = BodyB.GetLocalPoint(anchor);
     }
     else
     {
         LocalAnchorA = anchor;
         LocalAnchorB = anchor;
     }
 }
コード例 #21
0
 public FrictionJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, bool useWorldCoordinates = false)
     : base(bodyA.Owner, bodyB.Owner)
 {
     if (useWorldCoordinates)
     {
         LocalAnchorA = BodyA.GetLocalPoint(Vector2.Zero);
         LocalAnchorB = BodyB.GetLocalPoint(Vector2.Zero);
     }
     else
     {
         LocalAnchorA = Vector2.Zero;
         LocalAnchorB = Vector2.Zero;
     }
 }
コード例 #22
0
 /// <summary>
 /// Constructor for FrictionJoint.
 /// </summary>
 /// <param name="bodyA"></param>
 /// <param name="bodyB"></param>
 /// <param name="anchor"></param>
 /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
 public FrictionJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, Vector2 anchor, bool useWorldCoordinates = false)
     : base(bodyA.Owner.Uid, bodyB.Owner.Uid)
 {
     if (useWorldCoordinates)
     {
         LocalAnchorA = BodyA.GetLocalPoint(anchor);
         LocalAnchorB = BodyB.GetLocalPoint(anchor);
     }
     else
     {
         LocalAnchorA = anchor;
         LocalAnchorB = anchor;
     }
 }
コード例 #23
0
ファイル: MouseJoint.cs プロジェクト: Deusald/SharpBox2D
        internal MouseJoint(MouseJointDef def)
            : base(def)
        {
            Target        = def.Target;
            _localAnchorB = MathUtils.MulT(BodyB.GetTransform(), Target);

            MaxForce  = def.MaxForce;
            Stiffness = def.Stiffness;
            Damping   = def.Damping;

            _impulse.SetZero();
            _beta  = 0.0f;
            _gamma = 0.0f;
        }
コード例 #24
0
        /// <summary>
        /// This requires defining a line of
        /// motion using an axis and an anchor point. The definition uses local
        /// anchor points and a local axis so that the initial configuration
        /// can violate the constraint slightly. The joint translation is zero
        /// when the local anchor points coincide in world space. Using local
        /// anchors and a local axis helps when saving and loading a game.
        /// </summary>
        /// <param name="body">The body.</param>
        /// <param name="worldAnchor">The anchor.</param>
        /// <param name="axis">The axis.</param>
        public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
            : base(body)
        {
            JointType = JointType.FixedPrismatic;

            BodyB = BodyA;

            LocalAnchorA = worldAnchor;
            LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);

            _localXAxis1 = axis;
            _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
            _refAngle    = BodyB.Rotation;

            _limitState = LimitState.Inactive;
        }
コード例 #25
0
ファイル: DistanceJoint.cs プロジェクト: jocamar/Caravel
        /// <summary>
        /// This requires defining an
        /// anchor point on both bodies and the non-zero length of the
        /// distance joint. If you don't supply a length, the local anchor points
        /// is used so that the initial configuration can violate the constraint
        /// slightly. This helps when saving and loading a game.
        /// Warning Do not use a zero or short length.
        /// </summary>
        /// <param name="bodyA">The first body</param>
        /// <param name="bodyB">The second body</param>
        /// <param name="anchorA">The first body anchor</param>
        /// <param name="anchorB">The second body anchor</param>
        /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
        public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Distance;

            if (useWorldCoordinates)
            {
                LocalAnchorA = bodyA.GetLocalPoint(ref anchorA);
                LocalAnchorB = bodyB.GetLocalPoint(ref anchorB);
                Length       = (anchorB - anchorA).Length();
            }
            else
            {
                LocalAnchorA = anchorA;
                LocalAnchorB = anchorB;
                Length       = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length();
            }
        }
コード例 #26
0
ファイル: MouseJoint.cs プロジェクト: matrix4x4/Space
        /// <summary>Initializes the joint with the specified properties.</summary>
        /// <param name="target">The initial world target point. This is assumed to coincide with the body anchor initially.</param>
        /// <param name="maxForce">
        ///     The maximum constraint force that can be exerted to move the candidate body. Usually you will
        ///     express as some multiple of the weight (multiplier * mass * gravity).
        /// </param>
        /// <param name="frequency">The response speed in Hz.</param>
        /// <param name="dampingRatio">The damping ratio. 0 = no damping, 1 = critical damping.</param>
        internal void Initialize(WorldPoint target, float maxForce, float frequency, float dampingRatio)
        {
            System.Diagnostics.Debug.Assert(maxForce >= 0.0f);
            System.Diagnostics.Debug.Assert(frequency >= 0.0f);
            System.Diagnostics.Debug.Assert(dampingRatio >= 0.0f);

            _targetA      = target;
            _localAnchorB = BodyB.GetLocalPoint(_targetA);

            _maxForce = System.Math.Max(0, maxForce);
            _impulse  = Vector2.Zero;

            _frequency    = System.Math.Max(0, frequency);
            _dampingRatio = System.Math.Max(0, dampingRatio);

            _tmp.Beta  = 0.0f;
            _tmp.Gamma = 0.0f;
        }
コード例 #27
0
ファイル: Constraint.cs プロジェクト: DireAussie/MinimalRune
        /// <summary>
        /// Called when properties of this constraint were changed.
        /// </summary>
        protected virtual void OnChanged()
        {
            // TODO: A separate OnBodyA/BChanged would be helpful for composite joints.

            // Wake up bodies.
            if (Enabled)
            {
                if (BodyA != null)
                {
                    BodyA.WakeUp();
                }

                if (BodyB != null)
                {
                    BodyB.WakeUp();
                }
            }
        }
コード例 #28
0
ファイル: MouseJoint.cs プロジェクト: ARCHlVE/farseer-physics
        /// <summary>
        /// This requires a world target point,
        /// tuning parameters, and the time step.
        /// </summary>
        /// <param name="def"></param>
        public MouseJoint(Body bodyA, Body bodyB, Vector2 target)
            : base(bodyA, bodyB)
        {
            JointType    = JointType.Mouse;
            Frequency    = 5.0f;
            DampingRatio = 0.7f;

            Debug.Assert(target.IsValid());
            //Debug.Assert(MathUtils.IsValid(def.MaxForce) && def.MaxForce >= 0.0f);
            //Debug.Assert(MathUtils.IsValid(def.FrequencyHz) && def.FrequencyHz >= 0.0f);
            //Debug.Assert(MathUtils.IsValid(def.DampingRatio) && def.DampingRatio >= 0.0f);

            Transform xf1;

            BodyB.GetTransform(out xf1);

            LocalAnchorB = target;
            LocalAnchorA = MathUtils.MultiplyT(ref xf1, LocalAnchorB);
        }
コード例 #29
0
        internal MouseJoint(MouseJointDef def) : base(def)
        {
            Debug.Assert(def.Target.IsValid());
            Debug.Assert(def.MaxForce.IsValid() && def.MaxForce >= 0.0f);
            Debug.Assert(def.FrequencyHz.IsValid() && def.FrequencyHz >= 0.0f);
            Debug.Assert(def.DampingRatio.IsValid() && def.DampingRatio >= 0.0f);

            _targetA      = def.Target;
            _localAnchorB = MathUtils.MulT(BodyB.GetTransform(), _targetA);

            _maxForce = def.MaxForce;
            _impulse.SetZero();

            _frequencyHz  = def.FrequencyHz;
            _dampingRatio = def.DampingRatio;

            _beta  = 0.0f;
            _gamma = 0.0f;
        }
コード例 #30
0
        /// <summary>
        /// Called when the simulation wants this force effect to apply forces to rigid bodies.
        /// </summary>
        /// <remarks>
        /// <para>
        /// This method must be implemented in derived classes. This method is only called after the
        /// force effect was added to a simulation and <see cref="ForceEffect.OnAddToSimulation"/> was
        /// called.
        /// </para>
        /// <para>
        /// This method is responsible for applying the forces of the effect to the rigid bodies. To
        /// apply a force the methods <see cref="ForceEffect.AddForce(RigidBody, Vector3, Vector3)"/>,
        /// <see cref="ForceEffect.AddForce(RigidBody, Vector3)"/> and/or
        /// <see cref="ForceEffect.AddTorque(RigidBody, Vector3)"/> of the <see cref="ForceEffect"/>
        /// base class must be used. Do not use the <strong>AddForce</strong>/<strong>AddTorque</strong>
        /// methods of the <see cref="RigidBody"/> class.
        /// </para>
        /// </remarks>
        protected override void OnApply()
        {
            Vector3 worldPosA = (BodyA != null) ? BodyA.Pose.ToWorldPosition(AttachmentPositionALocal) : AttachmentPositionALocal;
            Vector3 velA      = (BodyA != null) ? BodyA.GetVelocityOfLocalPoint(AttachmentPositionALocal) : Vector3.Zero;
            Vector3 worldPosB = (BodyB != null) ? BodyB.Pose.ToWorldPosition(AttachmentPositionBLocal) : AttachmentPositionBLocal;
            Vector3 velB      = (BodyB != null) ? BodyB.GetVelocityOfLocalPoint(AttachmentPositionBLocal) : Vector3.Zero;

            // Compute spring force.
            Vector3 springVectorAToB = worldPosB - worldPosA;
            float   currentLength    = springVectorAToB.Length;

            if (!springVectorAToB.TryNormalize())
            {
                springVectorAToB = Vector3.UnitY;
            }

            Vector3 force = SpringConstant * (currentLength - Length) * springVectorAToB;

            // Compute damping force.
            Vector3 velRel = velA - velB;

            force += -DampingConstant *Vector3.Dot(velRel, springVectorAToB) * springVectorAToB;

            // Not needed anymore. Simulation.EvaluateForce automatically wakes up rigid bodies for big
            // force changes.
            //if (BodyA != null && BodyB != null)
            //{
            //  // Make sure both are awake or sleep simultaneously.
            //  if (BodyA.IsSleeping && !BodyB.IsSleeping)
            //    BodyA.DeferSleep(Simulation.Settings.Timing.FixedTimeStep);
            //  if (!BodyA.IsSleeping && BodyB.IsSleeping)
            //    BodyB.DeferSleep(Simulation.Settings.Timing.FixedTimeStep);
            //}

            if (BodyA != null && BodyA.MotionType == MotionType.Dynamic)
            {
                AddForce(BodyA, force, worldPosA);
            }
            if (BodyB != null && BodyB.MotionType == MotionType.Dynamic)
            {
                AddForce(BodyB, -force, worldPosB);
            }
        }