1 #ifndef _RIGIDBODY_HPP 2 #define _RIGIDBODY_HPP 3 #include <simgear/props/props.hxx> 4 #include "Vector.hpp" 5 #include "Math.hpp" 6 7 namespace yasim { 8 9 // 10 // A RigidBody object maintains all "internal" state about an object, 11 // accumulates force and torque information from external sources, and 12 // calculates the resulting accelerations. 13 // 14 // 15 // Units note: obviously, the choice of mass, time and distance units 16 // is up to the user. If you provide mass in kilograms, forces in 17 // newtons, and torques in newton-meters, you'll get your 18 // accelerations back in m/s^2. The angular units, however, are 19 // UNIFORMLY radians. Angular velocities are radians per <time unit>, 20 // the angular acceleration you get back is radians per <time unit>^2, 21 // and the angular momenta supplied to setGyro must be in radians, 22 // too. Radians, not degrees. Don't forget. 23 // 24 class RigidBody 25 { 26 SGPropertyNode_ptr _bodyN; 27 public: 28 RigidBody(); 29 ~RigidBody(); 30 31 // Adds a point mass to the system. Returns a handle so the gyro 32 // can be later modified via setMass(). 33 int addMass(float mass, const float* pos, bool isStatic = false); 34 35 // Modifies a previously-added point mass (fuel tank running dry, 36 // gear going up, swing wing swinging, pilot bailing out, etc...) 37 void setMass(int handle, float mass); 38 void setMass(int handle, float mass, const float* pos, bool isStatic = false); 39 numMasses() const40 int numMasses() const { return _nMasses; } getMass(int handle) const41 float getMass(int handle) const { return _masses[handle].m; } 42 void getMassPosition(int handle, float* out) const; getTotalMass() const43 float getTotalMass() const { return _totalMass; } 44 45 // The velocity, in local coordinates, of the specified point on a 46 // body rotating about its c.g. with velocity rot. 47 void pointVelocity(const float* pos, const float* rot, float* out); 48 49 // Sets the "gyroscope" for the body. This is the total 50 // "intrinsic" angular momentum of the body; that is, rotations of 51 // sub-objects, NOT rotation of the whole body within the global 52 // frame. Because angular momentum is additive in this way, we 53 // don't need to specify specific gyro objects; just add all their 54 // momenta together and set it here. setGyro(const float * angularMomentum)55 void setGyro(const float* angularMomentum) { Math::set3(angularMomentum, _gyro); } 56 57 58 // When masses are moved or changed, this object needs to 59 // regenerate its internal tables. This step is expensive, so 60 // it's exposed to the client who can amortize the call across 61 // multiple changes. see also _recalcStatic() 62 /// calculate the total mass, centre of gravity and inertia tensor 63 void recalc(); 64 65 /// Resets the current force/torque parameters to zero. 66 void reset(); 67 68 /// Applies a force at the center of gravity. addForce(const float * force)69 void addForce(const float* force) { Math::add3(_force, force, _force); } 70 71 /// Applies a force at the specified position. 72 void addForce(const float* pos, const float* force); 73 74 /// Adds a torque with the specified axis and magnitude addTorque(const float * torque)75 void addTorque(const float* torque) { Math::add3(_torque, torque, _torque); } setTorque(const float * torque)76 void setTorque(const float* torque) { Math::set3(torque, _torque); } 77 78 // Sets the rotation rate of the body (about its c.g.) within the 79 // surrounding environment. This is needed to compute torque on 80 // the body due to the centripetal forces involved in the 81 // rotation. NOTE: the rotation vector, like all other 82 // coordinates used here, is specified IN THE LOCAL COORDINATE 83 // SYSTEM. setBodySpin(const float * rotation)84 void setBodySpin(const float* rotation) { Math::set3(rotation, _spin); } 85 86 // Returns the center of gravity of the masses, in the body 87 // coordinate system. valid only after recalc() getCG(float * cgOut) const88 void getCG(float* cgOut) const { Math::set3(_cg, cgOut); } 89 90 // Returns the acceleration of the body's c.g. relative to the 91 // rest of the world, specified in local coordinates. getAccel(float * accelOut) const92 void getAccel(float* accelOut) const { Math::mul3(1/_totalMass, _force, accelOut); } 93 94 // Returns the acceleration of a specific location in local 95 // coordinates. If the body is rotating, this will be different 96 // from the c.g. acceleration due to the centripetal accelerations 97 // of points not on the rotation axis. 98 void getAccel(const float* pos, float* accelOut) const; 99 100 // Returns the instantaneous rate of change of the angular 101 // velocity, as a vector in local coordinates. 102 void getAngularAccel(float* accelOut) const; 103 104 // Returns the intertia tensor in a float[9] allocated by caller. 105 void getInertiaMatrix(float* inertiaOut) const; 106 107 private: 108 /** 109 Most of the mass points do not change after compilation of the aircraft so 110 they can be replaced by one aggregated mass at the c.g. of the static masses. 111 The isStatic flag is used to mark those masses. 112 */ 113 struct Mass { float m; float p[3]; bool isStatic; }; 114 void _recalcStatic(); /// aggregate static masses 115 Mass _staticMass; /// aggregated static masses, calculated once 116 Mass* _masses; /// mass elements 117 int _nMasses; /// number of masses 118 int _massesAlloced; /// counter for memory allocation 119 120 float _totalMass; 121 float _cg[3]; 122 float _gyro[3]; 123 124 // Inertia tensor, and its inverse. Computed from the above. 125 float _tI_static[9]; 126 float _tI[9]; 127 float _invI[9]; 128 129 // Externally determined quantities 130 float _force[3]; 131 float _torque[3]; 132 float _spin[3]; 133 }; 134 135 }; // namespace yasim 136 #endif // _RIGIDBODY_HPP 137