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(); } } } |