// // 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 . // using System; using System.Collections.Generic; using KerbalEngineer.VesselSimulator; namespace KerbalEngineer { // a (force, application point) tuple public class AppliedForce { private static readonly Pool pool = new Pool(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(); } } }