diff --git a/src/FDM/YASim/Atmosphere.cpp b/src/FDM/YASim/Atmosphere.cpp index b66c27821..8561ec111 100644 --- a/src/FDM/YASim/Atmosphere.cpp +++ b/src/FDM/YASim/Atmosphere.cpp @@ -10,42 +10,44 @@ namespace yasim { // pretty hot for a "standard" atmosphere. // Numbers above 19000 meters calculated from src/Environment/environment.cxx // meters kelvin Pa kg/m^3 -float Atmosphere::data[][Atmosphere::numColumns] = {{ -900.0f, 293.91f, 111679.0f, 1.32353f }, - { 0.0f, 288.11f, 101325.0f, 1.22500f }, - { 900.0f, 282.31f, 90971.0f, 1.12260f }, - { 1800.0f, 276.46f, 81494.0f, 1.02690f }, - { 2700.0f, 270.62f, 72835.0f, 0.93765f }, - { 3600.0f, 264.77f, 64939.0f, 0.85445f }, - { 4500.0f, 258.93f, 57752.0f, 0.77704f }, - { 5400.0f, 253.09f, 51226.0f, 0.70513f }, - { 6300.0f, 247.25f, 45311.0f, 0.63845f }, - { 7200.0f, 241.41f, 39963.0f, 0.57671f }, - { 8100.0f, 235.58f, 35140.0f, 0.51967f }, - { 9000.0f, 229.74f, 30800.0f, 0.46706f }, - { 9900.0f, 223.91f, 26906.0f, 0.41864f }, - { 10800.0f, 218.08f, 23422.0f, 0.37417f }, - { 11700.0f, 216.66f, 20335.0f, 0.32699f }, - { 12600.0f, 216.66f, 17654.0f, 0.28388f }, - { 13500.0f, 216.66f, 15327.0f, 0.24646f }, - { 14400.0f, 216.66f, 13308.0f, 0.21399f }, - { 15300.0f, 216.66f, 11555.0f, 0.18580f }, - { 16200.0f, 216.66f, 10033.0f, 0.16133f }, - { 17100.0f, 216.66f, 8712.0f, 0.14009f }, - { 18000.0f, 216.66f, 7565.0f, 0.12165f }, - {18900.0f, 216.66f, 6570.0f, 0.10564f }, - {19812.0f, 216.66f, 5644.0f, 0.09073f }, - {20726.0f, 217.23f, 4884.0f, 0.07831f }, - {21641.0f, 218.39f, 4235.0f, 0.06755f }, - {22555.0f, 219.25f, 3668.0f, 0.05827f }, - {23470.0f, 220.12f, 3182.0f, 0.05035f }, - {24384.0f, 220.98f, 2766.0f, 0.04360f }, - {25298.0f, 221.84f, 2401.0f, 0.03770f }, - {26213.0f, 222.71f, 2087.0f, 0.03265f }, - {27127.0f, 223.86f, 1814.0f, 0.02822f }, - {28042.0f, 224.73f, 1581.0f, 0.02450f }, - {28956.0f, 225.59f, 1368.0f, 0.02112f }, - {29870.0f, 226.45f, 1196.0f, 0.01839f }, - {30785.0f, 227.32f, 1044.0f, 0.01599f }}; +float Atmosphere::data[][Atmosphere::numColumns] = { + { -900.0f, 293.91f, 111679.0f, 1.32353f }, + { 0.0f, 288.11f, 101325.0f, 1.22500f }, + { 900.0f, 282.31f, 90971.0f, 1.12260f }, + { 1800.0f, 276.46f, 81494.0f, 1.02690f }, + { 2700.0f, 270.62f, 72835.0f, 0.93765f }, + { 3600.0f, 264.77f, 64939.0f, 0.85445f }, + { 4500.0f, 258.93f, 57752.0f, 0.77704f }, + { 5400.0f, 253.09f, 51226.0f, 0.70513f }, + { 6300.0f, 247.25f, 45311.0f, 0.63845f }, + { 7200.0f, 241.41f, 39963.0f, 0.57671f }, + { 8100.0f, 235.58f, 35140.0f, 0.51967f }, + { 9000.0f, 229.74f, 30800.0f, 0.46706f }, + { 9900.0f, 223.91f, 26906.0f, 0.41864f }, + { 10800.0f, 218.08f, 23422.0f, 0.37417f }, + { 11700.0f, 216.66f, 20335.0f, 0.32699f }, + { 12600.0f, 216.66f, 17654.0f, 0.28388f }, + { 13500.0f, 216.66f, 15327.0f, 0.24646f }, + { 14400.0f, 216.66f, 13308.0f, 0.21399f }, + { 15300.0f, 216.66f, 11555.0f, 0.18580f }, + { 16200.0f, 216.66f, 10033.0f, 0.16133f }, + { 17100.0f, 216.66f, 8712.0f, 0.14009f }, + { 18000.0f, 216.66f, 7565.0f, 0.12165f }, + {18900.0f, 216.66f, 6570.0f, 0.10564f }, + {19812.0f, 216.66f, 5644.0f, 0.09073f }, + {20726.0f, 217.23f, 4884.0f, 0.07831f }, + {21641.0f, 218.39f, 4235.0f, 0.06755f }, + {22555.0f, 219.25f, 3668.0f, 0.05827f }, + {23470.0f, 220.12f, 3182.0f, 0.05035f }, + {24384.0f, 220.98f, 2766.0f, 0.04360f }, + {25298.0f, 221.84f, 2401.0f, 0.03770f }, + {26213.0f, 222.71f, 2087.0f, 0.03265f }, + {27127.0f, 223.86f, 1814.0f, 0.02822f }, + {28042.0f, 224.73f, 1581.0f, 0.02450f }, + {28956.0f, 225.59f, 1368.0f, 0.02112f }, + {29870.0f, 226.45f, 1196.0f, 0.01839f }, + {30785.0f, 227.32f, 1044.0f, 0.01599f } +}; // Universal gas constant for air, in SI units. P = R * rho * T. // P in pascals (N/m^2), rho is kg/m^3, T in kelvin. @@ -124,17 +126,22 @@ float Atmosphere::calcMach(float spd, float temp) return spd / Math::sqrt(GAMMA * R * temp); } -float Atmosphere::spdFromMach(float mach, float temp) +float Atmosphere::machFromSpeed(float spd) +{ + return spd / Math::sqrt(GAMMA * R * _temperature); +} + +float Atmosphere::speedFromMach(float mach, float temp) { return mach * Math::sqrt(GAMMA * R * temp); } -float Atmosphere::spdFromMach(float mach) +float Atmosphere::speedFromMach(float mach) { - return spdFromMach(mach, _temperature); + return speedFromMach(mach, _temperature); } -float Atmosphere::spdFromVCAS(float vcas, float pressure, float temp) +float Atmosphere::speedFromVCAS(float vcas, float pressure, float temp) { // FIXME: does not account for supersonic float p0 = getStdPressure(0); @@ -144,13 +151,13 @@ float Atmosphere::spdFromVCAS(float vcas, float pressure, float temp) float cp = ((Math::pow(tmp,(7/2.))-1)/(pressure/p0)) + 1; float m2 = (Math::pow(cp,(1/3.5))-1)/0.2; - float vtas= spdFromMach(Math::sqrt(m2), temp); + float vtas= speedFromMach(Math::sqrt(m2), temp); return vtas; } -float Atmosphere::spdFromVCAS(float vcas) +float Atmosphere::speedFromVCAS(float vcas) { - return spdFromVCAS(vcas, _pressure, _temperature); + return speedFromVCAS(vcas, _pressure, _temperature); } void Atmosphere::calcStaticAir(float p0, float t0, float d0, float v, diff --git a/src/FDM/YASim/Atmosphere.hpp b/src/FDM/YASim/Atmosphere.hpp index 652de3362..0f5e6d7b3 100644 --- a/src/FDM/YASim/Atmosphere.hpp +++ b/src/FDM/YASim/Atmosphere.hpp @@ -29,7 +29,11 @@ public: float getTemperature() const { return _temperature; } float getPressure() const { return _pressure; } float getDensity() const { return _density; } + float speedFromMach(float mach); + float speedFromVCAS(float vcas); + float machFromSpeed(float spd); + // static methods static float getStdTemperature(float alt); static float getStdPressure(float alt); static float getStdDensity(float alt); @@ -39,10 +43,8 @@ public: static float calcMach(float spd, float temp); static float calcStdDensity(float pressure, float temp); - static float spdFromMach(float mach, float temp); - static float spdFromVCAS(float vcas, float pressure, float temp); - float spdFromMach(float mach); - float spdFromVCAS(float vcas); + static float speedFromMach(float mach, float temp); + static float speedFromVCAS(float vcas, float pressure, float temp); // Given ambient ("0") pressure/density/temperature values, // calculate the properties of static air (air accelerated to the diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index 67955cbdc..e0249222c 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -597,6 +597,15 @@ void FGFDM::parseWing(const XMLAttributes* a, const char* type, Airplane* airpla dragFactor = attrf(a, "effectiveness", 1); } w->setSectionDrag(_wingSection, dragFactor); + if (a->hasAttribute("flow")) { + const char* flowregime = a->getValue("flow"); + if (!strcmp(flowregime,"TRANSONIC")) { + w->setFlowRegime(FLOW_TRANSONIC); + w->setCriticalMachNumber(attrf(a, "mcrit", 0.6f)); + } + } else { + w->setFlowRegime(FLOW_SUBSONIC); + } _currObj = w; } diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp index 9d1c10635..a33b98548 100644 --- a/src/FDM/YASim/Math.hpp +++ b/src/FDM/YASim/Math.hpp @@ -2,6 +2,7 @@ #define _MATH_HPP #include +#include namespace yasim { @@ -17,6 +18,18 @@ public: static float weightedMean(float x1, float w1, float x2, float w2) { return (x1*w1 + x2*w2)/(w1+w2); } + + // calc polynomial + static float polynomial(const std::vector& coefficients, float x) { + float powX {1}; // init = x^0 = 1 + float result {0}; + for (auto &coeff : coefficients) { + result += coeff * powX; + powX *= x; + } + return result; + } + // Simple wrappers around library routines static inline float abs(float f) { return (float)::fabs(f); } static inline float sqrt(float f) { return (float)::sqrt(f); } diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 04990a4a2..102be0877 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -298,23 +298,26 @@ void Model::calcForces(State* s) // Do each surface, remembering that the local velocity at each // point is different due to rotation. - float faero[3] {0,0,0}; - for(i=0; i<_surfaces.size(); i++) { - Surface* sf = (Surface*)_surfaces.get(i); + float faero[3] {0,0,0}; + if (!_surfaces.empty()) { + // approx mach number for aircraft (instead of per surface) + float vs[3] {0,0,0}, pos[3] {0,0,0}; + localWind(pos, s, vs, alt); + float mach = _atmo.machFromSpeed(Math::mag3(vs)); + for (i=0; i<_surfaces.size(); i++) { + Surface* sf = (Surface*)_surfaces.get(i); + // Vsurf = wind - velocity + (rot cross (cg - pos)) + sf->getPosition(pos); + localWind(pos, s, vs, alt); - // Vsurf = wind - velocity + (rot cross (cg - pos)) - float vs[3], pos[3]; - sf->getPosition(pos); - localWind(pos, s, vs, alt); + float force[3], torque[3]; + sf->calcForce(vs, _atmo.getDensity(), mach, force, torque); + Math::add3(faero, force, faero); - float force[3], torque[3]; - sf->calcForce(vs, _atmo.getDensity(), force, torque); - Math::add3(faero, force, faero); - - _body.addForce(pos, force); - _body.addTorque(torque); + _body.addForce(pos, force); + _body.addTorque(torque); + } } - for (j=0; j<_rotorgear.getRotors()->size();j++) { Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j); diff --git a/src/FDM/YASim/Surface.cpp b/src/FDM/YASim/Surface.cpp index dc21656de..a4a4e13aa 100644 --- a/src/FDM/YASim/Surface.cpp +++ b/src/FDM/YASim/Surface.cpp @@ -34,6 +34,10 @@ Surface::Surface(Version* version, const float* pos, float c0 = 1 ) : _incidenceN->setFloatValue(0); _twistN = _surfN->getNode("twist-deg", true); _twistN->setFloatValue(0); + _pgCorrectionN = _surfN->getNode("pg-correction", true); + _pgCorrectionN->setFloatValue(1); + _dcdwaveN = _surfN->getNode("wavedrag", true); + _dcdwaveN->setFloatValue(1); _surfN->getNode("pos-x", true)->setFloatValue(pos[0]); _surfN->getNode("pos-y", true)->setFloatValue(pos[1]); _surfN->getNode("pos-z", true)->setFloatValue(pos[2]); @@ -123,10 +127,12 @@ void Surface::setSpoilerPos(float pos) } } + // Calculate the aerodynamic force given a wind vector v (in the -// aircraft's "local" coordinates) and an air density rho. Returns a -// torque about the Y axis ("pitch"), too. -void Surface::calcForce(const float* v, const float rho, float* out, float* torque) +// aircraft's "local" coordinates), an air density rho and the freestream +// mach number (for compressibility correction). Returns a torque about +// the Y axis ("pitch"), too. +void Surface::calcForce(const float* v, const float rho, float mach, float* out, float* torque) { // initialize outputs to zero Math::zero3(out); @@ -172,6 +178,21 @@ void Surface::calcForce(const float* v, const float rho, float* out, float* torq out[2] += stallLift; out[2] += flaplift; + //compute Prandtl/Glauert compressibility factor + float pg_correction {1}; + float wavedrag {0}; + if (_flow == FLOW_TRANSONIC) { + pg_correction = Math::polynomial(pg_coefficients, mach); + out[2] *= pg_correction; + + // Add mach dependent wave drag (Perkins and Hage) + if (mach > _Mcrit) { + wavedrag = 9.5f * Math::pow(mach-_Mcrit, 2.8f) + 0.00193f; + out[0] += wavedrag; + } + } + + // Airfoil lift (pre-stall and zero-alpha) torques "up" (negative // torque) around the Y axis, while flap lift pushes down. Both // forces are considered to act at one third chord from the @@ -191,6 +212,9 @@ void Surface::calcForce(const float* v, const float rho, float* out, float* torq // Diddle the induced drag Math::mul3(-1*_inducedDrag*out[2]*lwind[2], lwind, lwind); Math::add3(lwind, out, out); + + // Add mach dependent wave drag + // Reverse the incidence rotation to get back to surface // coordinates. Since out[] is now the force vector and is @@ -217,6 +241,8 @@ void Surface::calcForce(const float* v, const float rho, float* out, float* torq _fzN->setFloatValue(out[2]); _alphaN->setFloatValue(_alpha); _stallAlphaN->setFloatValue(_stallAlpha); + _pgCorrectionN->setFloatValue(pg_correction); + _dcdwaveN->setFloatValue(wavedrag); } } @@ -265,11 +291,11 @@ float Surface::stallFunc(float* v) // consider slat position, moves the stall aoa some degrees if(i == 0) { - if( _version->isVersionOrNewer( Version::YASIM_VERSION_32 )) { - _stallAlpha += _slatPos * _slatAlpha; - } else { - _stallAlpha += _slatAlpha; - } + if( _version->isVersionOrNewer( Version::YASIM_VERSION_32 )) { + _stallAlpha += _slatPos * _slatAlpha; + } else { + _stallAlpha += _slatAlpha; + } } // Beyond the stall diff --git a/src/FDM/YASim/Surface.hpp b/src/FDM/YASim/Surface.hpp index ef4acf58b..c3b771416 100644 --- a/src/FDM/YASim/Surface.hpp +++ b/src/FDM/YASim/Surface.hpp @@ -4,6 +4,7 @@ #include #include "Version.hpp" #include "Math.hpp" +#include "Wing.hpp" namespace yasim { @@ -83,11 +84,17 @@ public: // Induced drag multiplier void setInducedDrag(float mul) { _inducedDrag = mul; } - void calcForce(const float* v, const float rho, float* out, float* torque); + void calcForce(const float* v, const float rho, float mach, float* out, float* torque); float getAlpha() const { return _alpha; }; float getStallAlpha() const { return _stallAlpha; }; + void setFlowRegime(FlowRegime flow) { _flow = flow; }; + FlowRegime getFlowRegime() { return _flow; }; + + void setCriticalMachNumber(float mach) { _Mcrit = mach; }; + float getCriticalMachNumber() const { return _Mcrit; }; + private: SGPropertyNode_ptr _surfN; Version * _version; @@ -127,6 +134,12 @@ private: float _stallAlpha {0}; float _alpha {0}; + FlowRegime _flow{FLOW_SUBSONIC}; + float _Mcrit {1.0f}; + + std::vector pg_coefficients {1, -0.163f, 5.877f, -39.157f, 104.694f, + -111.838f, 46.749f, -5.313f}; + SGPropertyNode* _fxN; SGPropertyNode* _fyN; SGPropertyNode* _fzN; @@ -138,6 +151,8 @@ private: SGPropertyNode* _fabsN; SGPropertyNode* _incidenceN; SGPropertyNode* _twistN; + SGPropertyNode* _pgCorrectionN; + SGPropertyNode* _dcdwaveN; }; }; // namespace yasim diff --git a/src/FDM/YASim/Wing.cpp b/src/FDM/YASim/Wing.cpp index 926bc84d6..7f7fdb703 100644 --- a/src/FDM/YASim/Wing.cpp +++ b/src/FDM/YASim/Wing.cpp @@ -69,6 +69,9 @@ int Wing::addWingSection(float* base, float chord, float wingLength, float taper } _chord2float(ws->_tipChord, _tip); _wingSpan = 2 * _tip[1]; + if (_area > 0.0f) { + _aspectRatio = _wingSpan*_wingSpan/_area; + } _taper = ws->_tipChord.length / ((WingSection*)_sections.get(0))->_rootChord.length; return ws->_id; } @@ -205,12 +208,12 @@ void Wing::setSectionStallParams(int section, StallParams sp) void Wing::setFlapPos(WingFlaps type,float lval, float rval) { - float min {-1}; + float min {-1.0f}; if (type == WING_SPOILER || type == WING_SLAT) { - min = 0; + min = 0.0f; } - lval = Math::clamp(lval, min, 1); - rval = Math::clamp(rval, min, 1); + lval = Math::clamp(lval, min, 1.0f); + rval = Math::clamp(rval, min, 1.0f); WingSection* ws; for (int section=0; section < _sections.size(); section++) { @@ -340,12 +343,12 @@ void Wing::compile() float twist = ws->_twist * frac; ws->newSurface(_version, pos, ws->_orient, chord, - hasFlap0, hasFlap1, hasSlat, hasSpoiler, weight, twist); + hasFlap0, hasFlap1, hasSlat, hasSpoiler, weight, twist, _flow, _Mcrit); if(_mirror) { pos[1] = -pos[1]; ws->newSurface(_version, pos, ws->_rightOrient, chord, - hasFlap0, hasFlap1, hasSlat, hasSpoiler, weight, twist); + hasFlap0, hasFlap1, hasSlat, hasSpoiler, weight, twist, _flow, _Mcrit); } } } @@ -428,12 +431,13 @@ void Wing::WingSection::multiplyLiftRatio(float factor) setLiftRatio(_liftRatio * factor); } -void Wing::WingSection::newSurface(Version* _version, float* pos, float* orient, float chord, bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler, float weight, float twist) +void Wing::WingSection::newSurface(Version* _version, float* pos, float* orient, float chord, bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler, float weight, float twist, FlowRegime flow, float mcrit) { Surface* s = new Surface(_version, pos, weight); s->setOrientation(orient); s->setChord(chord); + s->setFlowRegime(flow); // Camber is expressed as a fraction of stall peak, so convert. s->setZeroAlphaLift(_camber*_stallParams.peak); @@ -480,7 +484,10 @@ void Wing::WingSection::newSurface(Version* _version, float* pos, float* orient, if(hasSpoiler) _flapSurfs[WING_SPOILER].add(s); s->setInducedDrag(_inducedDrag); - + if(flow == FLOW_TRANSONIC) { + s->setCriticalMachNumber(mcrit); + } + SurfRec *sr = new SurfRec(); sr->surface = s; sr->weight = weight; diff --git a/src/FDM/YASim/Wing.hpp b/src/FDM/YASim/Wing.hpp index 90d6be87f..7a730694b 100644 --- a/src/FDM/YASim/Wing.hpp +++ b/src/FDM/YASim/Wing.hpp @@ -41,6 +41,11 @@ enum WingFlaps { WING_SLAT, }; +enum FlowRegime { + FLOW_SUBSONIC, + FLOW_TRANSONIC +}; + class Wing { SGPropertyNode_ptr _wingN {nullptr}; @@ -110,10 +115,11 @@ class Wing { // segment to that along the X (drag) direction. void setLiftRatio(float ratio); void multiplyLiftRatio(float factor); + float getLiftRatio() const { return _liftRatio; }; void newSurface(Version* _version, float* pos, float* orient, float chord, bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler, - float weight, float twist); + float weight, float twist, FlowRegime flow, float mcrit); int numSurfaces() const { return _surfs.size(); } Surface* getSurface(int n) { return ((SurfRec*)_surfs.get(n))->surface; } float getSurfaceWeight(int n) const { return ((SurfRec*)_surfs.get(n))->weight; } @@ -133,13 +139,16 @@ class Wing { float _aspectRatio {0}; float _meanChord {0}; Chord _mac; - float _taper {1}; + float _taper {1.0f}; float _incidence {0}; float _incidenceMin {INCIDENCE_MIN}; float _incidenceMax {INCIDENCE_MAX}; float _weight {0}; float _sweepLEMin {0}; float _sweepLEMax {0}; + FlowRegime _flow {FLOW_SUBSONIC}; + float _Mcrit {1.0f}; + //-- private methods //copy float[3] to chord x,y,z @@ -155,7 +164,8 @@ public: ~Wing(); int addWingSection(float* base, float chord, float wingLength, - float taper = 1, float sweep = 0, float dihedral = 0, float twist = 0, float camber = 0, float idrag = 1, float incidence = 0); + float taper = 1, float sweep = 0, float dihedral = 0, float twist = 0, + float camber = 0, float idrag = 1, float incidence = 0); void setFlapParams(int section, WingFlaps type, FlapParams fp); void setSectionDrag(int section, float pdrag); @@ -172,6 +182,8 @@ public: void setIncidenceMax(float max) { _incidenceMax = max; }; float getIncidenceMin() const { return _incidenceMin; }; float getIncidenceMax() const { return _incidenceMax; }; + void setFlowRegime(FlowRegime flow) { _flow = flow; }; + void setCriticalMachNumber(float m) { _Mcrit = m; }; // write mass (= _weight * scale) to property tree void weight2mass(float scale); @@ -189,7 +201,9 @@ public: float getMACz() const { return _mac.z; }; // get z-coord of MAC leading edge float getSweepLEMin() const { return _sweepLEMin; }; //min sweep angle of leading edge float getSweepLEMax() const { return _sweepLEMax; }; //max sweep angle of leading edge - + FlowRegime getFlowRegime() const { return _flow; }; + + float getCriticalMachNumber() const { return _Mcrit; }; //----------------------------- // propergate the control axes value for the sub-surfaces void setFlapPos(WingFlaps type, float lval, float rval = 0); diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index 33897b70d..60c2eb425 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -335,14 +335,14 @@ void YASim::copyToYASim(bool copyState) Math::tmul33(s.orient, v, v); break; case KNOTS: - v[0] = atmo.spdFromVCAS(get_V_calibrated_kts()/MPS2KTS); + v[0] = atmo.speedFromVCAS(get_V_calibrated_kts()/MPS2KTS); v[1] = 0; v[2] = 0; Math::tmul33(s.orient, v, v); needCopy = true; break; case MACH: - v[0] = atmo.spdFromMach(get_Mach_number()); + v[0] = atmo.speedFromMach(get_Mach_number()); v[1] = 0; v[2] = 0; Math::tmul33(s.orient, v, v);