private static Vector3d KM_GimbalTorqueVector(PartModule p, int i, Vector3d CoM)
{
KM_Gimbal_3 gimbal = p as KM_Gimbal_3;
Vector3d torque = Vector3d.zero;
if (!gimbal.enableGimbal)
{
return(Vector3d.zero);
}
float maxRollGimbalRange = Math.Max(gimbal.pitchGimbalRange, gimbal.yawGimbalRange);
Vector3d position = gimbal.gimbalTransforms[i].position - CoM;
double distance = position.magnitude;
double radius = Vector3.ProjectOnPlane(position, Vector3.Project(position, p.vessel.ReferenceTransform.up)).magnitude;
torque.x = Math.Sin(Math.Abs(gimbal.pitchGimbalRange) * Math.PI / 180d) * distance;
torque.z = Math.Sin(Math.Abs(gimbal.yawGimbalRange) * Math.PI / 180d) * distance;
if (gimbal.enableRoll)
{
torque.y = Math.Sin(Math.Abs(maxRollGimbalRange) * Math.PI / 180d) * radius;
}
return(torque);
}
private static Quaternion KM_GimbalInitialRot(PartModule p, Transform engineTransform, int i)
{
KM_Gimbal_3 gimbal = p as KM_Gimbal_3;
// Save the current local rot
Quaternion save = gimbal.gimbalTransforms[i].localRotation;
// Apply the default rot and let unity compute the world rot
gimbal.gimbalTransforms[i].localRotation = gimbal.initRots[i];
Quaternion initalRot = gimbal.gimbalTransforms[i].rotation;
// Restore the current local rot
gimbal.gimbalTransforms[i].localRotation = save;
return(initalRot);
}
private static bool KM_GimbalIsValid(PartModule p)
{
KM_Gimbal_3 gimbal = p as KM_Gimbal_3;
return(gimbal.initRots.Any());
}
