source: 2014/24/EemeliK/Zombieland/Jypeli/Physics2DDotNet/PhysicsLogics/MoveToPointLogic.cs @ 5974

Revision 5974, 5.4 KB checked in by empaheik, 5 years ago (diff)
Line 
1#region MIT License
2/*
3 * Copyright (c) 2005-2008 Jonathan Mark Porter. http://physics2d.googlepages.com/
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights to
8 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
9 * the Software, and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
16 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
17 * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
18 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
20 * OTHER DEALINGS IN THE SOFTWARE.
21 */
22#endregion
23
24
25#if UseDouble
26using Scalar = System.Double;
27#else
28using Scalar = System.Single;
29#endif
30using System;
31
32using AdvanceMath;
33
34namespace Physics2DDotNet.PhysicsLogics
35{
36    /// <summary>
37    /// A class that will apply Force to move an object to a certain point and stop it once it gets there.
38    /// </summary>
39#if !(WINDOWS_PHONE || XBOX)
40    [Serializable]
41#endif
42    public sealed class MoveToPointLogic : PhysicsLogic
43    {
44        Scalar maxAcceleration;
45        Scalar maxVelocity;
46        Vector2D destination;
47        Body body;
48        /// <summary>
49        /// Creates a new MoveToPointLogic object.
50        /// </summary>
51        /// <param name="body">The Body this logic will act on.</param>
52        /// <param name="destination">The Point it will move the Body too.</param>
53        /// <param name="maxAcceleration">The maximum acceleration to be applied to the Body</param>
54        public MoveToPointLogic(Body body, Vector2D destination, Scalar maxAcceleration) : this(body, destination, maxAcceleration, Scalar.MaxValue) { }
55        /// <summary>
56        /// Creates a new MoveToPointLogic object.
57        /// </summary>
58        /// <param name="body">The Body this logic will act on.</param>
59        /// <param name="destination">The Point it will move the Body too.</param>
60        /// <param name="maxAcceleration">The maximum acceleration to be applied to the Body</param>
61        /// <param name="maxVelocity">The maximum velocity this logic will accelerate the Body too.</param>
62        public MoveToPointLogic(Body body, Vector2D destination, Scalar maxAcceleration, Scalar maxVelocity)
63            : base(new Lifespan())
64        {
65            if (maxAcceleration <= 0) { throw new ArgumentOutOfRangeException("maxAcceleration", "maxAcceleration must be greater then zero"); }
66            if (maxVelocity <= 0) { throw new ArgumentOutOfRangeException("maxVelocity", "maxVelocity must be greater then zero"); }
67            if (body == null) { throw new ArgumentNullException("body"); }
68            this.maxAcceleration = maxAcceleration;
69            this.maxVelocity = maxVelocity;
70            this.destination = destination;
71            this.body = body;
72        }
73
74        protected internal override void RunLogic(TimeStep step)
75        {
76            Vector2D diff, normal, tangent;
77            Scalar distance, velocity, tangentVelocity;
78
79            Vector2D.Subtract(ref destination, ref body.State.Position.Linear, out diff);
80            Vector2D.Normalize(ref diff, out distance, out normal);
81            Vector2D.GetRightHandNormal(ref normal, out tangent);
82            Vector2D.Dot(ref body.State.Velocity.Linear, ref normal, out velocity);
83            Vector2D.Dot(ref body.State.Velocity.Linear, ref tangent, out tangentVelocity);
84
85            if (distance < Math.Abs(step.Dt * (step.Dt * maxAcceleration - Math.Abs(velocity))))
86            {
87                body.State.Velocity.Linear = Vector2D.Zero;
88                body.State.Position.Linear = destination;
89                this.Lifetime.IsExpired = true;
90            }
91            else
92            {
93                Vector2D forceVector;
94                //Normal Velocity
95                Scalar accelNeeded = Math.Sign(velocity) * (velocity * velocity) / (2 * distance);
96                Scalar trueAccel = (accelNeeded >= maxAcceleration) ? (-maxAcceleration) : (maxAcceleration);
97                if (Math.Abs(trueAccel * step.Dt + velocity) > maxVelocity) // enforce Max velocity
98                {
99                    trueAccel = (maxVelocity - velocity) * step.DtInv;
100                }
101                Scalar force = body.Mass.Mass * trueAccel;
102                Vector2D.Multiply(ref force, ref normal, out forceVector);
103                body.ApplyForce(ref forceVector);
104
105                //Tangent Velocity
106                Scalar tangetAccelNeeded = -Math.Sign(tangentVelocity) * Math.Min(step.Dt * maxAcceleration, Math.Abs(tangentVelocity)) * step.DtInv;
107                Scalar tangetForce = tangetAccelNeeded * body.Mass.Mass;
108                Vector2D.Multiply(ref tangetForce, ref tangent, out forceVector);
109                body.ApplyForce(ref forceVector);
110            }
111        }
112    }
113}
Note: See TracBrowser for help on using the repository browser.