private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            // See if there is anything to do
            if (_desiredOrientation == null)
            {
                return;
            }

            //TODO: Implement this.Offset
            //TODO: Allow rotations in the destination axis

            Vector3D current = e.Body.DirectionToWorld(new Vector3D(0, 0, 1));
            Quaternion rotation = Math3D.GetRotation(current, _desiredOrientation.Value);

            if (rotation.IsIdentity)
            {
                // Don't set anything.  If they are rotating along the allowed axis, then no problem.  If they try
                // to rotate off that axis, another iteration of this method will rotate back
                //e.Body.AngularVelocity = new Vector3D(0, 0, 0);
                return;
            }

            // According to the newton wiki, angular velociy is radians per second
            Vector3D newAngVel = rotation.Axis.ToUnit() * Math1D.DegreesToRadians(rotation.Angle);
            newAngVel *= _multiplier;

            if (this.MaxVelocity != null && newAngVel.LengthSquared > this.MaxVelocity.Value * this.MaxVelocity.Value)
            {
                newAngVel = newAngVel.ToUnit() * this.MaxVelocity.Value;
            }

            e.Body.AngularVelocity = newAngVel;
        }
Ejemplo n.º 2
0
        //TODO: Finish ChaseObject_?????? instead of hard coding here
        private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            //TrackedItem item = _items.FirstOrDefault(o => o.MapObject.Token == e.Body.Token);
            //if (item != null && item.ShouldLimitRotation)
            //{
            //    //LimitRotation(e.Body, _rotate_ToWorld, _rotate_FromWorld);
            //}

            //LimitLinear(e.Body, _rotate_ToWorld);
        }
Ejemplo n.º 3
0
        private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            #region Thrusters

            //NOTE: The brain is running in another thread, so this method just applies the thrusters based on the current neural outputs

            foreach (Thruster thruster in _thrusters)
            {
                // Look at the thrusts from the last firing
                Vector3D?[] thrusts = thruster.FiredThrustsLastUpdate;
                if (thrusts == null)
                {
                    continue;
                }

                for (int cntr = 0; cntr < thrusts.Length; cntr++)
                {
                    if (thrusts[cntr] != null)
                    {
                        // Apply force
                        Vector3D bodyForce = e.Body.DirectionToWorld(thrusts[cntr].Value);
                        Point3D bodyPoint = e.Body.PositionToWorld(thruster.Position);
                        e.Body.AddForceAtPoint(bodyForce, bodyPoint);
                    }
                }
            }

            #endregion

            #region Kick from guns

            foreach (ProjectileGun gun in _projectileGuns)
            {
                // Look at the kick from this gun firing (the vector builds up if multiple shots were fired between updates.
                // Calling GetKickLastUpdate clears the kick for next time)
                Vector3D? kick = gun.GetKickLastUpdate();

                if (kick == null)
                {
                    continue;
                }

                // Apply force
                Vector3D bodyForce = e.Body.DirectionToWorld(kick.Value);
                Point3D bodyPoint = e.Body.PositionToWorld(gun.Position);
                e.Body.AddForceAtPoint(bodyForce, bodyPoint);
            }

            #endregion
        }
 private void Egg_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
 {
     //TODO: Fluid
 }
        private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            // See if there is anything to do
            if (_desiredPoint == null)
            {
                return;
            }

            Point3D current = e.Body.PositionToWorld(this.Offset.ToPoint());
            Vector3D direction = _desiredPoint.Value - current;

            Vector3D newVelocity = direction * _multiplier;

            if (this.MaxVelocity != null && newVelocity.LengthSquared > this.MaxVelocity.Value * this.MaxVelocity.Value)
            {
                newVelocity = newVelocity.ToUnit(false) * this.MaxVelocity.Value;
            }

            e.Body.Velocity = newVelocity;
        }
        private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            try
            {
                #region Boundry Force

                Vector3D position = e.Body.Position.ToVector();

                //NOTE: Even though the boundry is square, this turns it into a circle (so the corners of the map are even harder
                //to get to - which is good, the map feel circular)
                if (position.LengthSquared > _boundryForceBeginSquared)
                {
                    // See how far into the boundry the item is
                    double distaceInto = position.Length - _boundryForceBegin;

                    // I want an acceleration of zero when distFromMax is 1, but an accel of 10 when it's boundryMax
                    //NOTE: _boundryMax.X is to the edge of the box.  If they are in the corner the distance will be greater, so the accel will be greater
                    double accel = UtilityCore.GetScaledValue(0, 30, 0, _boundryWidth, distaceInto);

                    // Apply a force toward the center
                    Vector3D force = position.ToUnit();
                    force *= accel * e.Body.Mass * -1d;

                    e.Body.AddForce(force);
                }

                #endregion
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
            }
        }
 private void Body_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
 {
     UpdateFluidForces();
 }
            /// <summary>
            /// This is an attempt to fix 1, but I didn't get very far
            /// </summary>
            private List<Tuple<Thruster, int, double>> FireThrustLinear2(BodyApplyForceAndTorqueArgs e, double elapsedTime, Vector3D direction)
            {
                Point3D center = _bot.PhysicsBody.CenterOfMass;
                MassMatrix massMatrix = _bot.PhysicsBody.MassMatrix;

                ThrustContribution[] contributions = GetThrusterContributions(center);

                #region Find along and against

                List<Tuple<ThrustContribution, double>> along = new List<Tuple<ThrustContribution, double>>();
                List<Tuple<ThrustContribution, double>> against = new List<Tuple<ThrustContribution, double>>();

                foreach (ThrustContribution contribution in contributions)
                {
                    double dot = Vector3D.DotProduct(contribution.Thruster.ThrusterDirectionsShip[contribution.Index], direction);

                    // Don't include zero
                    if (dot > 0d)
                    {
                        along.Add(new Tuple<ThrustContribution, double>(contribution, dot));
                    }
                    else if (dot < 0d)
                    {
                        against.Add(new Tuple<ThrustContribution, double>(contribution, dot));
                    }
                }

                #endregion

                Tuple<Vector3D, Vector3D> alongCombinedForceAndTorque = GetCombinedForceAndTorque(along.Select(o => new Tuple<Vector3D, Vector3D>(o.Item1.TranslationForce, o.Item1.Torque)));


                // I think that fixing torque drif is much more important than linear drift


                if (!Math1D.IsNearZero(alongCombinedForceAndTorque.Item2.LengthSquared))
                {
                    // There is a net torque




                }



                if (!Math1D.IsNearValue(Vector3D.DotProduct(alongCombinedForceAndTorque.Item1.ToUnit(), direction), 1d))
                {
                    // There is a linear drift



                }







                // Exit Function
                return along.Select(o => new Tuple<Thruster, int, double>(o.Item1.Thruster, o.Item1.Index, 1d)).ToList();
            }
            private List<Tuple<Thruster, int, double>> FireThrustLinear3(BodyApplyForceAndTorqueArgs e, double elapsedTime, Vector3D direction)
            {
                //TODO: Expose these as properties of the class, or take as args to the method call
                const double PERFECTALIGN = .01;

                EnsureThrustSetsCalculated();

                // Find the best thruster set for the requested direction, may need to combine sets

                var matches = _zeroTorqueSets.Select(o => new Tuple<ThrustSet, double>(o, Vector3D.DotProduct(o.Translation, direction))).OrderByDescending(o => o.Item2).ToArray();

                if (matches.Length > 0)
                {
                    // Find set that are perfectly aligned (direction is a unit vector, so if they are perfectly aligned, the dot product will be the translation's length)
                    var perfectAlign = matches.Where(o => IsNearValue(o.Item2, o.Item1.TranslationLength, o.Item1.TranslationLength * PERFECTALIGN)).OrderByDescending(o => o.Item2).ToArray();

                    if (perfectAlign.Length > 0)
                    {
                        ThrustSet set = GetBestPerfectAlign(perfectAlign);

                        return set.Thrusters.Select(o => new Tuple<Thruster, int, double>(o.Thruster, o.Index, o.Percent)).ToList();
                    }


                    //TODO: Combine solutions to get the requested direction

                    ThrustSet set2 = matches.First().Item1;

                    return set2.Thrusters.Select(o => new Tuple<Thruster, int, double>(o.Thruster, o.Index, o.Percent)).ToList();



                }

                return new List<Tuple<Thruster, int, double>>();
            }
            public void ApplyForce2(BodyApplyForceAndTorqueArgs e)
            {
                _lines.Clear();

                if (!_isUpPressed && !_isDownPressed)
                {
                    return;
                }

                _bot.Fuel.QuantityCurrent = _bot.Fuel.QuantityMax;

                double elapsedTime = 1d;
                DateTime curTick = DateTime.UtcNow;
                if (_lastTick != null)
                {
                    elapsedTime = (curTick - _lastTick.Value).TotalSeconds;
                }

                _lastTick = curTick;

                // Keeping each button's contribution in a set so they are easier to normalize
                List<Tuple<Thruster, int, double>[]> thrusterSets = new List<Tuple<Thruster, int, double>[]>();

                Vector3D direction;

                if (_isUpPressed)
                {
                    direction = new Vector3D(0, 0, 1);

                    if (_useSimple)
                    {
                        thrusterSets.Add(FireThrustLinear1(e, elapsedTime, direction).ToArray());
                    }
                    else
                    {
                        thrusterSets.Add(FireThrustLinear3(e, elapsedTime, direction).ToArray());
                    }
                }

                if (_isDownPressed)
                {
                    direction = new Vector3D(0, 0, -1);

                    if (_useSimple)
                    {
                        thrusterSets.Add(FireThrustLinear1(e, elapsedTime, direction).ToArray());
                    }
                    else
                    {
                        thrusterSets.Add(FireThrustLinear3(e, elapsedTime, direction).ToArray());
                    }
                }

                //TODO: Normalize these so no thruster will fire above 100% (keep the ratios though)

                #region Fire them

                foreach (var thruster in thrusterSets.SelectMany(o => o))
                {
                    double percent = thruster.Item3;
                    Vector3D? force = thruster.Item1.Fire(ref percent, thruster.Item2, elapsedTime);
                    if (force != null)
                    {
                        Vector3D bodyForce = e.Body.DirectionToWorld(force.Value);
                        Point3D bodyPoint = e.Body.PositionToWorld(thruster.Item1.Position);
                        e.Body.AddForceAtPoint(bodyForce, bodyPoint);

                        Vector3D lineVect = GetThrustLine(bodyForce, _itemOptions.Thruster_StrengthRatio);		// this returns a vector in the opposite direction, so the line looks like a flame
                        Point3D lineStart = bodyPoint + (lineVect.ToUnit() * thruster.Item1.ThrustVisualStartRadius);
                        _lines.AddLine(lineStart, lineStart + lineVect);
                    }
                    else
                    {
                        int seven = -2;
                    }
                }

                #endregion
            }
            //NOTE: direction must be a unit vector
            /// <summary>
            /// This fires any thruster that contributes to the direction at 100%
            /// </summary>
            private List<Tuple<Thruster, int, double>> FireThrustLinear1(BodyApplyForceAndTorqueArgs e, double elapsedTime, Vector3D direction)
            {
                #region Get contributing thrusters

                // Get a list of thrusters that will contribute to the direction

                List<Tuple<Thruster, int, Vector3D, double>> contributing = new List<Tuple<Thruster, int, Vector3D, double>>();

                foreach (Thruster thruster in _thrusters)
                {
                    for (int cntr = 0; cntr < thruster.ThrusterDirectionsShip.Length; cntr++)
                    {
                        Vector3D thrustDirUnit = thruster.ThrusterDirectionsShip[cntr].ToUnit();
                        double dot = Vector3D.DotProduct(thrustDirUnit, direction);

                        if (dot > 0d)
                        {
                            contributing.Add(new Tuple<Thruster, int, Vector3D, double>(thruster, cntr, thrustDirUnit, dot));
                        }
                    }
                }

                #endregion

                List<Tuple<Thruster, int, double>> retVal = new List<Tuple<Thruster, int, double>>();

                retVal.AddRange(contributing.Select(o => Tuple.Create(o.Item1, o.Item2, 1d)));      // 1 for 100%

                return retVal;

                //Point3D center = _ship.PhysicsBody.CenterOfMass;
                //MassMatrix massMatrix = _ship.PhysicsBody.MassMatrix;

                //// Figure out the drift


                //// See which thrusters can most reduce that drift

                //#region Fire them

                //foreach (var contribute in contributing)
                //{
                //    double percent = 1d;
                //    Vector3D? force = contribute.Item1.Fire(ref percent, contribute.Item2, elapsedTime);
                //    if (force != null)
                //    {
                //        Vector3D bodyForce = e.Body.DirectionToWorld(force.Value);
                //        Point3D bodyPoint = e.Body.PositionToWorld(contribute.Item1.Position);
                //        e.Body.AddForceAtPoint(bodyForce, bodyPoint);

                //        _lines.AddLine(bodyPoint, bodyPoint - bodyForce);		// subtracting, so the line looks like a flame
                //    }
                //    else
                //    {
                //        int seven = -2;
                //    }
                //}

                //#endregion
            }
            /// <summary>
            /// This fires every thruster at 100%
            /// </summary>
            public void ApplyForce1(BodyApplyForceAndTorqueArgs e)
            {
                _lines.Clear();

                if (!_isUpPressed && !_isDownPressed)
                {
                    return;
                }

                _bot.Fuel.QuantityCurrent = _bot.Fuel.QuantityMax;

                double elapsedTime = 1d;
                DateTime curTick = DateTime.UtcNow;
                if (_lastTick != null)
                {
                    elapsedTime = (curTick - _lastTick.Value).TotalSeconds;
                }

                _lastTick = curTick;

                foreach (Thruster thruster in _thrusters)
                {
                    double percent = 1d;
                    Vector3D? force = thruster.Fire(ref percent, 0, elapsedTime);
                    if (force != null)
                    {
                        Vector3D bodyForce = e.Body.DirectionToWorld(force.Value);
                        Point3D bodyPoint = e.Body.PositionToWorld(thruster.Position);
                        e.Body.AddForceAtPoint(bodyForce, bodyPoint);

                        _lines.AddLine(bodyPoint, bodyPoint - bodyForce);		// subtracting, so the line looks like a flame
                    }
                    else
                    {
                        int seven = -2;
                    }
                }
            }
Ejemplo n.º 13
0
 private void Body_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
 {
 }
        private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            // See if there is anything to do
            if (_desiredPoint == null)
            {
                return;
            }

            //NOTE: Offset needs to be center of mass
            Point3D current = e.Body.PositionToWorld(e.Body.CenterOfMass + this.Offset);

            ChasePoint_GetForceArgs args = new ChasePoint_GetForceArgs(this.Item, _desiredPoint.Value - current);

            Vector3D? force = GetForce(args, this.Forces, this.Percent, this.MaxForce, this.MaxAcceleration);

            if (force != null && !force.Value.IsNearZero())
            {
                if (_shouldCauseTorque)
                {
                    e.Body.AddForceAtPoint(force.Value, current);
                }
                else
                {
                    e.Body.AddForce(force.Value);
                }
            }
        }
        private void PhysicsBody_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            // See if there is anything to do
            if (_desiredOrientation == null)
            {
                return;
            }

            //TODO: Offset
            Vector3D current = e.Body.DirectionToWorld(new Vector3D(0, 0, 1));
            Quaternion rotation = Math3D.GetRotation(current, _desiredOrientation.Value);

            if (rotation.IsIdentity)
            {
                // Don't set anything.  If they are rotating along the allowed axis, then no problem.  If they try
                // to rotate off that axis, another iteration of this method will rotate back
                //e.Body.AngularVelocity = new Vector3D(0, 0, 0);
                return;
            }

            ChaseOrientation_GetTorqueArgs args = new ChaseOrientation_GetTorqueArgs(this.Item, rotation);

            Vector3D? torque = null;

            // Call each worker
            foreach (var worker in this.Torques)
            {
                Vector3D? localForce = worker.GetTorque(args);

                if (localForce != null)
                {
                    if (torque == null)
                    {
                        torque = localForce;
                    }
                    else
                    {
                        torque = torque.Value + localForce.Value;
                    }
                }
            }

            // Apply the torque
            if (torque != null)
            {
                // Limit if exceeds this.MaxForce
                if (this.MaxTorque != null && torque.Value.LengthSquared > this.MaxTorque.Value * this.MaxTorque.Value)
                {
                    torque = torque.Value.ToUnit(false) * this.MaxTorque.Value;
                }

                // Limit acceleration
                if (this.MaxAcceleration != null)
                {
                    double mass = Item.PhysicsBody.Mass;

                    //f=ma
                    double accel = torque.Value.Length / mass;

                    if (accel > this.MaxAcceleration.Value)
                    {
                        torque = torque.Value.ToUnit(false) * (this.MaxAcceleration.Value * mass);
                    }
                }

                torque = torque.Value * this.Percent;

                e.Body.AddTorque(torque.Value);
            }
        }
 private void Ship_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
 {
     if (_thrustController != null)
     {
         //_thrustController.ApplyForce1(e);
         _thrustController.ApplyForce2(e);
     }
 }
        private void Bean_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            if (e.Body.MaterialGroupID == _material_Bean && !e.Body.IsAsleep)
            {
                // Apply gravity
                e.Body.AddForce(_options.GravityField.Gravity * e.Body.Mass);

                // Apply an in-facing cylindrical force
                e.Body.AddForce(_boundryField.GetForce(e.Body.Position));

                // Nearby explosions
                foreach (Explosion explosion in _explosions)
                {
                    explosion.ApplyForceToBody(e.Body);
                }
            }
        }
 private void Asteroid_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
 {
     e.Body.AddForce(_gravityField.GetForce(e.Body.Position));
 }
Ejemplo n.º 19
0
 private void body_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
 {
     if (_boundryMin != null && e.Body.Mass > 0)// && !e.Body.IsAsleep)
     {
         FixPositionsAndVelocities(e.Body);
     }
 }
        private void Mineral_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            const double POW = 1d / 2.5d;

            Vector3D force = _gravityField.GetForce(e.Body.Position);

            double origLength = force.Length;
            if (Math1D.IsNearZero(origLength))
            {
                return;
            }

            //double newLength = Math.Sqrt(origLength);
            double newLength = Math.Pow(origLength, POW);
            double multiplier = newLength / origLength;

            e.Body.AddForce(new Vector3D(force.X * multiplier, force.Y * multiplier, force.Z * multiplier));
        }
Ejemplo n.º 21
0
        private void Body_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            Interlocked.Increment(ref this.MainThreadCounter);

            Tuple<Vector3D?, Vector3D?> currentForceTorque = _applyForceTorque;

            if (currentForceTorque != null)
            {
                double seconds = Math.Min(e.Timestep, 1);       // cap at one in case there is a big lag

                if (currentForceTorque.Item1 != null)
                {
                    e.Body.AddForce(currentForceTorque.Item1.Value / seconds);
                }

                if (currentForceTorque.Item2 != null)
                {
                    e.Body.AddTorque(currentForceTorque.Item2.Value / seconds);
                }
            }
        }
Ejemplo n.º 22
0
        private void Body_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            try
            {
                if (_vectorField != null)
                {
                    #region Vector Field

                    // Field Type
                    Vector3D direction;
                    switch (_vectorField.FieldType)
                    {
                        case VectorFieldType.Inward:
                            direction = e.Body.Position.ToVector().ToUnit() * _vectorField.Strength * -1d;
                            break;

                        case VectorFieldType.Inward_Radius:
                            direction = e.Body.Position.ToVector() * _vectorField.Strength * -1d;
                            break;

                        case VectorFieldType.Outward:
                            direction = e.Body.Position.ToVector().ToUnit() * _vectorField.Strength;
                            break;

                        case VectorFieldType.SwirlInward:
                            direction = e.Body.Position.ToVector() * -1;
                            direction.Normalize();
                            direction = direction.GetRotatedVector(new Vector3D(0, 0, 1), 10d);
                            direction *= _vectorField.Strength;
                            break;

                        case VectorFieldType.Swirl:
                            direction = e.Body.Position.ToVector() * -1;
                            direction.Z = 0;
                            direction.Normalize();
                            direction = direction.GetRotatedVector(new Vector3D(0, 0, 1), 90d);
                            direction *= _vectorField.Strength;
                            break;

                        case VectorFieldType.Z0Plane:
                            direction = e.Body.Position.ToVector();
                            direction.X = 0;
                            direction.Y = 0;
                            if (direction.Z > 0)
                            {
                                direction.Z = -_vectorField.Strength;
                            }
                            else
                            {
                                direction.Z = _vectorField.Strength;
                            }
                            break;

                        default:		// not looking for none, the class should be null when none
                            throw new ApplicationException("Unexpected VectorFieldType: " + _vectorField.FieldType.ToString());
                    }

                    // Apply Force/Acceleration
                    if (_vectorField.UseForce)
                    {
                        e.Body.AddForce(direction);
                    }
                    else
                    {
                        e.Body.AddForce(direction * e.Body.Mass);
                    }

                    #endregion
                }

                // Apply Gravity
                if (_bodyAttractionForces.ContainsKey(e.Body))
                {
                    e.Body.AddForce(_bodyAttractionForces[e.Body]);
                }

                if (_fluidHulls.ContainsKey(e.Body))
                {
                    foreach (FluidHull hull in _fluidHulls[e.Body])
                    {
                        hull.Transform = new MatrixTransform3D(e.Body.OffsetMatrix);
                        hull.Update();
                    }
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
            }
        }
Ejemplo n.º 23
0
        private void Bot_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            //TODO: Fluid

            //if (e.Body.MaterialGroupID == _material_Bot && !e.Body.IsAsleep)
            //{
            //    // Apply gravity
            //    e.Body.AddForce(_options.GravityField.Gravity * e.Body.Mass);

            //    // Apply an in-facing cylindrical force
            //    e.Body.AddForce(_boundryField.GetForce(e.Body.Position));

            //    // Nearby explosions
            //    foreach (Explosion explosion in _explosions)
            //    {
            //        explosion.ApplyForceToBody(e.Body);
            //    }
            //}

        }
        private void Body_ApplyForceAndTorque(object sender, BodyApplyForceAndTorqueArgs e)
        {
            try
            {
                #region Explosions

                if (e.Body.MaterialGroupID == _material_Brick || e.Body.MaterialGroupID == _material_Projectile)
                {
                    foreach (Explosion explosion in _explosions)
                    {
                        explosion.ApplyForceToBody(e.Body);
                    }
                }

                #endregion

                #region Gravity

                if (_hasGravity && !e.Body.IsAsleep)
                {
                    e.Body.AddForce(new Vector3D(0, 0, GRAVITY * e.Body.Mass));
                }

                #endregion

                #region Force Beam

                if (_isRayFiring && _leftClickAction == LeftClickAction.ForceBeam)		// _forceBeamSettings will never be null by this point
                {
                    Point3D bodyPosition = e.Body.Position;

                    Point3D nearestPointOnLine = Math3D.GetClosestPoint_Line_Point(_rayPoint, _rayDirection, bodyPosition);
                    Vector3D directionToLine = nearestPointOnLine - bodyPosition;

                    if (directionToLine.LengthSquared < _forceBeamSettings.Radius * _forceBeamSettings.Radius)
                    {
                        #region Calculate Force Multiplier

                        // Get a ratio from 0 to 1
                        double distanceRatio = directionToLine.Length / _forceBeamSettings.Radius;

                        double forceMultiplier;
                        if (_forceBeamSettings.IsLinearDropoff)
                        {
                            forceMultiplier = 1d - distanceRatio;
                        }
                        else
                        {
                            forceMultiplier = .01d / (distanceRatio * distanceRatio);
                            if (forceMultiplier > 1d || double.IsInfinity(forceMultiplier) || double.IsNaN(forceMultiplier))
                            {
                                forceMultiplier = 1d;
                            }
                        }

                        #endregion

                        Vector3D addForce = new Vector3D(0, 0, 0);

                        if (distanceRatio > 0d)
                        {
                            #region Away/Toward

                            Vector3D forceToward = directionToLine.ToUnit() * _forceBeamSettings.TowardAwayForce;
                            forceToward = forceToward.GetRotatedVector(_rayDirection, _forceBeamSettings.Angle);

                            addForce += forceToward;

                            #endregion
                        }

                        // Push/Pull
                        addForce += _rayDirection.ToUnit() * _forceBeamSettings.PushPullForce;

                        // Add force to the body
                        e.Body.AddForce(addForce);
                    }
                }

                #endregion
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
            }
        }