VECTOR3 getThrGroupDir(THGROUP_TYPE thgt)
{
VECTOR3 dirTotal = {0,0,0};
for(unsigned int i=0; i<v->GetGroupThrusterCount(thgt); i++)
{
THRUSTER_HANDLE th=v->GetGroupThruster(thgt,i);
VECTOR3 dir;
v->GetThrusterDir(th,dir);
dirTotal = dirTotal + dir;
}
dirTotal = dirTotal / v->GetGroupThrusterCount(thgt);
return dirTotal;
}
double getEngineVectorDownward(THGROUP_TYPE thgt)
{
VECTOR3 edirLocal = getThrGroupDir(thgt);
VECTOR3 edirGlobal = {0,0,0};
v->HorizonRot(edirLocal,edirGlobal);
return -edirGlobal.y;
}
double getEngineVectorDownward(THRUSTER_HANDLE th)
{
VECTOR3 edirLocal;
v->GetThrusterDir(th, edirLocal);
VECTOR3 edirGlobal = {0,0,0};
v->HorizonRot(edirLocal,edirGlobal);
return -edirGlobal.y;
}
double getMaxThrGroupAcceleration(THGROUP_TYPE thg)
{
return getMaxThrGroupForce(thg) / v->GetMass();
}
double getMaxThrGroupForce(THGROUP_TYPE thgt)
{
double force = 0;
for(unsigned int i=0; i<v->GetGroupThrusterCount(thgt); i++)
{
THRUSTER_HANDLE th=v->GetGroupThruster(thgt,i);
force = force + v->GetThrusterMax(th);
}
return force * length(getThrGroupDir(thgt));
}
VECTOR3 getThrGroupForce(THGROUP_TYPE thgt)
{
return getThrGroupDir(thgt) * v->GetThrusterGroupLevel(thgt) * getMaxThrGroupForce(thgt);
}
VECTOR3 getThrusterGroupTypeForceVector(THGROUP_TYPE thgt)
{
VECTOR3 totalEngineForce = {0,0,0};
for(unsigned int i=0; i<v->GetGroupThrusterCount(thgt); i++)
{
THRUSTER_HANDLE th=v->GetGroupThruster(thgt,i);
VECTOR3 dir;
v->GetThrusterDir(th,dir);
double currentForce = v->GetThrusterMax(th) * v->GetThrusterLevel(th);
totalEngineForce = totalEngineForce + (dir * currentForce);
}
return totalEngineForce;
}
void setVerticalACCByThrusterGroup(double acceleration,THGROUP_TYPE thgt)
{
double max_acceleration = getMaxThrGroupAcceleration(thgt);
VECTOR3 totalForce, force, thrustv;
v->GetForceVector(force);
v->GetThrustVector(thrustv);
//totalForce = force - thrustv; //Using this, RCS Translation can change horizontal velocity
totalForce = force - getThrusterGroupTypeForceVector(thgt); //Using this, RCS Translation etc. has no effect
VECTOR3 v3_spd;
double x, y, r;
v->GetEquPos(x,y,r);
v->GetHorizonAirspeedVector(v3_spd);
double lng, lat, r2;
v->GetEquPos(lng,lat,r2);
VECTOR3 vgv = oapiGetGroundVector(v->GetGravityRef(),lng,lat);
v3_spd += vgv;
double vhsqr = (v3_spd.x * v3_spd.x) + (v3_spd.z * v3_spd.z);
double fz = (v->GetMass() * vhsqr) / r; //centrifugal force
double dweight = sqrt((totalForce.x * totalForce.x) + (totalForce.y * totalForce.y) + (totalForce.z * totalForce.z));
double thrust = -(((dweight / v->GetMass()) - (-acceleration + (fz/ v->GetMass()) )) / max_acceleration);
thrust = thrust / (getEngineVectorDownward(thgt));
v->SetThrusterGroupLevel(thgt,thrust);
}
