Merge pull request #1 from CYBUTEK/master
[VesselSimulator.git] / KerbalEngineer / Helpers / ForceAccumulator.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
// 
//     Kerbal Engineer Redux
// 
//     Copyright (C) 2014 CYBUTEK
// 
//     This program is free software: you can redistribute it and/or modify
//     it under the terms of the GNU General Public License as published by
//     the Free Software Foundation, either version 3 of the License, or
//     (at your option) any later version.
// 
//     This program is distributed in the hope that it will be useful,
//     but WITHOUT ANY WARRANTY; without even the implied warranty of
//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//     GNU General Public License for more details.
// 
//     You should have received a copy of the GNU General Public License
//     along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
 
using System;
using System.Collections.Generic;
using KerbalEngineer.VesselSimulator;
 
namespace KerbalEngineer
{
    // a (force, application point) tuple
    public class AppliedForce
    {
        private static readonly Pool<AppliedForce> pool = new Pool<AppliedForce>(Create, Reset);
 
        public Vector3d vector;
        public Vector3d applicationPoint;
 
        static private AppliedForce Create()
        {
            return new AppliedForce();
        }
 
        static private void Reset(AppliedForce appliedForce) { }
 
        static public AppliedForce New(Vector3d vector, Vector3d applicationPoint)
        {
            AppliedForce force = pool.Borrow();
            force.vector = vector;
            force.applicationPoint = applicationPoint;
            return force;
        }
 
        public void Release()
        {
            pool.Release(this);
        }
 
 
    }
 
        // This class was mostly adapted from FARCenterQuery, part of FAR, by ferram4, GPLv3
        // https://github.com/ferram4/Ferram-Aerospace-Research/blob/master/FerramAerospaceResearch/FARCenterQuery.cs
    // Also see https://en.wikipedia.org/wiki/Resultant_force
 
        // It accumulates forces and their points of applications, and provides methods for
    // calculating the effective torque at any position, as well as the minimum-torque net force application point.
    //
    // The latter is a non-trivial issue; there is a 1-dimensional line of physically-equivalent solutions parallel
    // to the resulting force vector; the solution closest to the weighted average of force positions is chosen.
        // In the case of non-parallel forces, there usually is an infinite number of such lines, all of which have
        // some amount of residual torque. The line with the least amount of residual torque is chosen.
        public class ForceAccumulator
        {
            // Total force.
                private Vector3d totalForce = Vector3d.zero;
                // Torque needed to compensate if force were applied at origin.
                private Vector3d totalZeroOriginTorque = Vector3d.zero;
 
                // Weighted average of force application points.
                private WeightedVectorAverager avgApplicationPoint = new WeightedVectorAverager();
 
                // Feed an force to the accumulator.
                public void AddForce(Vector3d applicationPoint, Vector3d force)
                {
                        totalForce += force;
                        totalZeroOriginTorque += Vector3d.Cross(applicationPoint, force);
                        avgApplicationPoint.Add(applicationPoint, force.magnitude);
                }
 
        public Vector3d GetAverageForceApplicationPoint() {
            return avgApplicationPoint.Get();
        }
 
        public void AddForce(AppliedForce force) {
            AddForce(force.applicationPoint, force.vector);
        }
 
                // Residual torque for given force application point.
                public Vector3d TorqueAt(Vector3d origin)
                {
                        return totalZeroOriginTorque - Vector3d.Cross(origin, totalForce);
                }
 
        // Total force vector.
        public Vector3d GetTotalForce()
        {
            return totalForce;
        }
 
        // Returns the minimum-residual-torque force application point that is closest to origin.
        // Note that TorqueAt(GetMinTorquePos()) is always parallel to totalForce.
        public Vector3d GetMinTorqueForceApplicationPoint(Vector3d origin)
        {
            double fmag = totalForce.sqrMagnitude;
            if (fmag <= 0) {
                return origin;
            }
 
            return origin + Vector3d.Cross(totalForce, TorqueAt(origin)) / fmag;
        }
 
        public Vector3d GetMinTorqueForceApplicationPoint()
        {
            return GetMinTorqueForceApplicationPoint(avgApplicationPoint.Get());
        }
 
            public void Reset()
            {
                totalForce = Vector3d.zero;
                totalZeroOriginTorque = Vector3d.zero;
            avgApplicationPoint.Reset();
            }
        }
}