Added "Atmo" button to compact build engineer
[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
// 
//     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;
 
namespace KerbalEngineer
{
    // a (force, application point) tuple
    public class AppliedForce
    {
        public Vector3d vector;
        public Vector3d applicationPoint;
 
        public AppliedForce(Vector3d vector, Vector3d applicationPoint) {
            this.vector = vector;
            this.applicationPoint = applicationPoint;
        }
    }
 
        // 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());
        }
        }
}