| static private Clamp ( double value, double min, double max ) : double | ||
| value | double | |
| min | double | |
| max | double | |
| return | double | |
private static bool FindClosestPointOnFracturedTree(Vector3D fromPositionFractureLocal, MyFracturedPiece fracture, out Vector3D closestPoint)
{
// Marko: HACK: skip stumps
closestPoint = default(Vector3D);
if (fracture == null)
{
return(false);
}
Vector4 minFour, maxFour;
fracture.Shape.GetShape().GetLocalAABB(0, out minFour, out maxFour);
var min = new Vector3D(minFour);
var max = new Vector3D(maxFour);
closestPoint = Vector3D.Clamp(fromPositionFractureLocal, min, max);
closestPoint.X = (closestPoint.X + 2 * (max.X + min.X) / 2) / 3;
closestPoint.Y = MathHelper.Clamp(closestPoint.Y + 0.25f * (closestPoint.Y - min.Y < max.Y - closestPoint.Y ? 1 : -1), min.Y, max.Y);
closestPoint.Z = (closestPoint.Z + 2 * (max.Z + min.Z) / 2) / 3;
closestPoint = Vector3D.Transform(closestPoint, fracture.PositionComp.WorldMatrix);
return(true);
}
public void UpdateThrustControl(MatrixD orientation, Vector3D linearVelocityWorld)
{
Vector3D velocity = Vector3D.TransformNormal(linearVelocityWorld, MatrixD.Transpose(orientation));
Vector3D destinationRelative = Vector3D.TransformNormal(destination - orientation.Translation, MatrixD.Transpose(orientation));
float mass = navigation.Mass;
Vector3D availableBrakingAcceleration = navigation.MaxThrustInDirection(-velocity) / mass;
Vector3D predictedStopPoint = -0.5 * velocity * velocity / availableBrakingAcceleration;
Vector3D positionError = destinationRelative - predictedStopPoint;
Vector3D desiredVelocity = Vector3D.ClampToSphere(thrustPID.Filter(positionError), SpeedCap);
Vector3D velocityError = velocity - desiredVelocity;
Vector3D desiredThrustPercentage = Vector3D.Clamp(0.5 * velocityError, -Vector3D.One, Vector3D.One);
thrustDebug?.Reset();
thrustDebug?.WriteLine($"x = {DebugPanel.ToString(destinationRelative)}");
thrustDebug?.WriteLine($"v = {DebugPanel.ToString(velocity)}");
thrustDebug?.WriteLine($"as = {DebugPanel.ToString(availableBrakingAcceleration)}");
thrustDebug?.WriteLine($"xs = {DebugPanel.ToString(predictedStopPoint)}");
thrustDebug?.WriteLine($"vt = {DebugPanel.ToString(desiredVelocity)}");
thrustDebug?.WriteLine($"v_err = {DebugPanel.ToString(velocityError)}");
thrustDebug?.WriteLine($"t = {DebugPanel.ToString(desiredThrustPercentage)}");
navigation.SetThrustPercentage(desiredThrustPercentage);
}
static void DrawSlidingLine(Vector3D startPosition, Vector3D endPosition, Color color1, Color color2)
{
float pieceLength = 0.1f;
float lineLength = Vector3.Distance(startPosition, endPosition);
Vector3D lineDir = Vector3D.Normalize(endPosition - startPosition);
Vector3D delta = lineDir * pieceLength;
Color currentColor = color1;
Vector3D min = Vector3D.Min(startPosition, endPosition);
Vector3D max = Vector3D.Max(startPosition, endPosition);
float startOffset = SlidingOffset - (pieceLength * 2.0f) * ((int)(SlidingOffset / (pieceLength * 2.0f))) - 2 * pieceLength;
Vector3D currentPos = startPosition + startOffset * lineDir;
float actualLength = 0;
while (actualLength < lineLength)
{
MyRenderProxy.DebugDrawLine3D(Vector3D.Clamp(currentPos, min, max), Vector3D.Clamp(currentPos + delta, min, max), currentColor, currentColor, false);
if (currentColor == color1)
{
currentColor = color2;
}
else
{
currentColor = color1;
}
actualLength += pieceLength;
currentPos += delta;
}
//MyRenderProxy.DebugDrawLine3D(startPosition, endPosition, color1, color2, false);
}
public static double DistanceSquared(this BoundingBoxD box, Vector3D point)
{
Vector3D closestPoint = Vector3D.Clamp(point, box.Min, box.Max);
double result;
Vector3D.DistanceSquared(ref point, ref closestPoint, out result);
return(result);
}
public static Vector3D WorldCoordToVoxelFloat(MyVoxelBase voxelMap, Vector3D worldCoords)
{
Vector3 tmp;
MyVoxelCoordSystems.WorldPositionToLocalPosition(voxelMap.PositionLeftBottomCorner, ref worldCoords, out tmp);
tmp += voxelMap.StorageMin;
return(Vector3D.Clamp(tmp, Vector3D.Zero, voxelMap.StorageMax));
}
public double DistanceSquared(Vector3D point)
{
if (this.Contains(point) == ContainmentType.Contains)
{
return(0.0);
}
return(Vector3D.DistanceSquared(Vector3D.Clamp(point, this.Min, this.Max), point));
}
public void Clamp1()
{
Vector3D clamped = new Vector3D(-10, 1, 100);
clamped.Clamp(-100, 100);
Assert.AreEqual(-10, clamped.X);
Assert.AreEqual(1, clamped.Y);
Assert.AreEqual(100, clamped.Z);
}
public void ClampStatic2()
{
Vector3D clamped = new Vector3D(-10, 1, 100);
clamped = Vector3D.Clamp(clamped, -1, 0);
Assert.AreEqual(-1, clamped.X);
Assert.AreEqual(0, clamped.Y);
Assert.AreEqual(0, clamped.Z);
}
public bool Intersects(ref BoundingSphereD sphere)
{
Vector3D vectord;
double num;
Vector3D.Clamp(ref sphere.Center, ref this.Min, ref this.Max, out vectord);
Vector3D.DistanceSquared(ref sphere.Center, ref vectord, out num);
return(num <= (sphere.Radius * sphere.Radius));
}
public void Intersects(ref BoundingSphereD sphere, out bool result)
{
Vector3D vectord;
double num;
Vector3D.Clamp(ref sphere.Center, ref this.Min, ref this.Max, out vectord);
Vector3D.DistanceSquared(ref sphere.Center, ref vectord, out num);
result = num <= (sphere.Radius * sphere.Radius);
}
public bool Intersects(BoundingBoxD box)
{
Vector3D vectord;
double num;
Vector3D.Clamp(ref this.Center, ref box.Min, ref box.Max, out vectord);
Vector3D.DistanceSquared(ref this.Center, ref vectord, out num);
return(num <= (this.Radius * this.Radius));
}
public void Intersects(ref BoundingBoxD box, out bool result)
{
Vector3D vectord;
double num;
Vector3D.Clamp(ref this.Center, ref box.Min, ref box.Max, out vectord);
Vector3D.DistanceSquared(ref this.Center, ref vectord, out num);
result = num <= (this.Radius * this.Radius);
}
private void Insert(int node)
{
var box = _tree.GetAabb(node);
Vector3D test;
Vector3D.Clamp(ref _vec, ref box.Min, ref box.Max, out test);
var dist = Vector3D.DistanceSquared(test, _vec);
_tmp.Insert(node, dist);
}
/// <summary>
/// Checks whether the current BoundingSphereD intersects a BoundingBoxD.
/// </summary>
/// <param name="box">The BoundingBoxD to check for intersection with.</param><param name="result">[OutAttribute] true if the BoundingSphereD and BoundingBoxD intersect; false otherwise.</param>
public void Intersects(ref BoundingBoxD box, out bool result)
{
Vector3D result1;
Vector3D.Clamp(ref this.Center, ref box.Min, ref box.Max, out result1);
double result2;
Vector3D.DistanceSquared(ref this.Center, ref result1, out result2);
result = (double)result2 <= (double)this.Radius * (double)this.Radius;
}
public bool Intersects(ref BoundingSphereD sphere)
{
Vector3D result1;
Vector3D.Clamp(ref sphere.Center, ref this.Min, ref this.Max, out result1);
double result2;
Vector3D.DistanceSquared(ref sphere.Center, ref result1, out result2);
return((double)result2 <= (double)sphere.Radius * (double)sphere.Radius);
}
/// <summary>
/// Checks whether the current BoundingSphereD intersects with a specified BoundingBoxD.
/// </summary>
/// <param name="box">The BoundingBoxD to check for intersection with the current BoundingSphereD.</param>
public bool Intersects(BoundingBoxD box)
{
Vector3D result1;
Vector3D.Clamp(ref this.Center, ref box.Min, ref box.Max, out result1);
double result2;
Vector3D.DistanceSquared(ref this.Center, ref result1, out result2);
return((double)result2 <= (double)this.Radius * (double)this.Radius);
}
public void Vector3ClampTest()
{
Vector3D <float> a = new Vector3D <float>(0.5f, 0.3f, 0.33f);
Vector3D <float> min = new Vector3D <float>(0.0f, 0.1f, 0.13f);
Vector3D <float> max = new Vector3D <float>(1.0f, 1.1f, 1.13f);
// Normal case.
// Case N1: specified value is in the range.
Vector3D <float> expected = new Vector3D <float>(0.5f, 0.3f, 0.33f);
Vector3D <float> actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
// Normal case.
// Case N2: specified value is bigger than max value.
a = new Vector3D <float>(2.0f, 3.0f, 4.0f);
expected = max;
actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
// Case N3: specified value is smaller than max value.
a = new Vector3D <float>(-2.0f, -3.0f, -4.0f);
expected = min;
actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
// Case N4: combination case.
a = new Vector3D <float>(-2.0f, 0.5f, 4.0f);
expected = new Vector3D <float>(min.X, a.Y, max.Z);
actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
// User specified min value is bigger than max value.
max = new Vector3D <float>(0.0f, 0.1f, 0.13f);
min = new Vector3D <float>(1.0f, 1.1f, 1.13f);
// Case W1: specified value is in the range.
a = new Vector3D <float>(0.5f, 0.3f, 0.33f);
expected = max;
actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
// Normal case.
// Case W2: specified value is bigger than max and min value.
a = new Vector3D <float>(2.0f, 3.0f, 4.0f);
expected = max;
actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
// Case W3: specified value is smaller than min and max value.
a = new Vector3D <float>(-2.0f, -3.0f, -4.0f);
expected = max;
actual = Vector3D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D<float>f.Clamp did not return the expected value.");
}
public double Distance(Vector3D point)
{
if (Contains(point) == ContainmentType.Contains)
{
return(0);
}
var clamp = Vector3D.Clamp(point, Min, Max);
return(Vector3D.Distance(clamp, point));
}
private void Insert(int node)
{
var box = _tree.GetAabb(node);
Vector3D tmp;
Vector3D.Clamp(ref _vec, ref box.Min, ref box.Max, out tmp);
var dist = Vector3D.DistanceSquared(tmp, _vec);
if (dist <= _maxDistanceSq)
{
_tmp.Insert(node, dist);
}
}
public static Surface Createbumpmap(Surface original, Vector2D offset, IShape shape, Scalar depthToCenter)
{
int width = original.Width;
int height = original.Height;
//Color[,] colors = new Color[width, height];
Vector3D mid = new Vector3D(128, 128, 128);
Vector3D min = Vector3D.Zero;
Vector3D max = new Vector3D(255, 255, 255);
Surface result = new Surface(width, height, 32, true);
for (int x = 0; x < width; ++x)
{
for (int y = 0; y < height; ++y)
{
Vector2D point = new Vector2D(x, y) - offset;
IntersectionInfo info;
Vector3D normal;
if (shape.TryGetIntersection(point, out info))
{
Scalar va = Math.Abs(info.Distance / depthToCenter);
if (va < 1)
{
Vector3D temp = new Vector3D(info.Normal.X, info.Normal.Y, .1f);
temp = Vector3D.Lerp(temp, Vector3D.ZAxis, va);
normal = temp.Normalized;
}
else
{
normal = Vector3D.ZAxis;
}
}
else
{
normal = Vector3D.ZAxis;
}
normal = Vector3D.Clamp(mid + (normal * 128), min, max);
result.Draw(
new System.Drawing.Point(x, y),
Color.FromArgb(255, (int)normal.X, (int)normal.Y, (int)normal.Z));
// colors[x, y] = Color.FromArgb(255,(int)normal.X, (int)normal.Y, (int)normal.Z);
}
}
// result.SetPixels(new System.Drawing.Point(), colors);
return(result);
}
/// <summary>
/// Tests whether the BoundingBox contains a BoundingSphere.
/// </summary>
/// <param name="sphere">The BoundingSphere to test for overlap.</param>
public ContainmentType Contains(BoundingSphereD sphere)
{
Vector3D result1;
Vector3D.Clamp(ref sphere.Center, ref this.Min, ref this.Max, out result1);
double result2;
Vector3D.DistanceSquared(ref sphere.Center, ref result1, out result2);
double num = sphere.Radius;
if ((double)result2 > (double)num * (double)num)
{
return(ContainmentType.Disjoint);
}
return((double)this.Min.X + (double)num > (double)sphere.Center.X || (double)sphere.Center.X > (double)this.Max.X - (double)num || ((double)this.Max.X - (double)this.Min.X <= (double)num || (double)this.Min.Y + (double)num > (double)sphere.Center.Y) || ((double)sphere.Center.Y > (double)this.Max.Y - (double)num || (double)this.Max.Y - (double)this.Min.Y <= (double)num || ((double)this.Min.Z + (double)num > (double)sphere.Center.Z || (double)sphere.Center.Z > (double)this.Max.Z - (double)num)) || (double)this.Max.X - (double)this.Min.X <= (double)num ? ContainmentType.Intersects : ContainmentType.Contains);
}
public void Contains(ref BoundingSphereD sphere, out ContainmentType result)
{
Vector3D vectord;
double num;
Vector3D.Clamp(ref sphere.Center, ref this.Min, ref this.Max, out vectord);
Vector3D.DistanceSquared(ref sphere.Center, ref vectord, out num);
double radius = sphere.Radius;
if (num > (radius * radius))
{
result = ContainmentType.Disjoint;
}
else
{
result = ((((((this.Min.X + radius) > sphere.Center.X) || (sphere.Center.X > (this.Max.X - radius))) || (((this.Max.X - this.Min.X) <= radius) || ((this.Min.Y + radius) > sphere.Center.Y))) || (((sphere.Center.Y > (this.Max.Y - radius)) || ((this.Max.Y - this.Min.Y) <= radius)) || (((this.Min.Z + radius) > sphere.Center.Z) || (sphere.Center.Z > (this.Max.Z - radius))))) || ((this.Max.X - this.Min.X) <= radius)) ? ContainmentType.Intersects : ContainmentType.Contains;
}
}
public ContainmentType Contains(BoundingSphereD sphere)
{
Vector3D vectord;
double num;
Vector3D.Clamp(ref sphere.Center, ref this.Min, ref this.Max, out vectord);
Vector3D.DistanceSquared(ref sphere.Center, ref vectord, out num);
double radius = sphere.Radius;
if (num > (radius * radius))
{
return(ContainmentType.Disjoint);
}
if ((((((this.Min.X + radius) <= sphere.Center.X) && (sphere.Center.X <= (this.Max.X - radius))) && (((this.Max.X - this.Min.X) > radius) && ((this.Min.Y + radius) <= sphere.Center.Y))) && (((sphere.Center.Y <= (this.Max.Y - radius)) && ((this.Max.Y - this.Min.Y) > radius)) && (((this.Min.Z + radius) <= sphere.Center.Z) && (sphere.Center.Z <= (this.Max.Z - radius))))) && ((this.Max.X - this.Min.X) > radius))
{
return(ContainmentType.Contains);
}
return(ContainmentType.Intersects);
}
public void Clamp2()
{
Vector3D clamped = new Vector3D(-10, 1, 100);
clamped.Clamp(-1, 0);
Assert.AreEqual(-1, clamped.X);
Assert.AreEqual(0, clamped.Y);
Assert.AreEqual(0, clamped.Z);
}
private void TestMotionInput()
{
if (!EngineIsON)
{
return;
}
Vector3D moveInput = Cockpit.MoveIndicator;
//ToLog("\nmoveInput: " + VectToStr(moveInput), true);
InertiaDampeners = Cockpit.DampenersOverride;
//ToLog("\nDampeners: " + InertiaDampeners.ToString(), true);
Vector3D WorldSpeed = Cockpit.GetShipVelocities().LinearVelocity;
Vector3D WorldAngVel = Cockpit.GetShipVelocities().AngularVelocity;
Vector3D Gravity = Cockpit.GetNaturalGravity();
Vector3D normVertical = -Vector3D.Normalize(Gravity);
//матрица вращения в СО, связанную с вектором гравитации
if (!PlanetMatrixReady)
{
Vector3D normForward = normVertical.Cross(Cockpit.WorldMatrix.Right);
PlanetMatrix = MatrixD.CreateWorld(Vector3D.Zero, normForward, normVertical);
PlanetMatrixInv = MatrixD.Invert(PlanetMatrix);
PlanetMatrixReady = true;
}
Vector3D PlanetSpeed = Vector3D.Rotate(WorldSpeed, PlanetMatrixInv);
//ToLog("\nSpeed 3D: " + VectToStr(PlanetSpeed), true);
Vector3D PlanetAngularVelocity = Vector3D.Rotate(WorldAngVel, PlanetMatrixInv);
//test!!!
//Vector3D vForw = Vector3D.Rotate(Cockpit.WorldMatrix.Forward, PlanetMatrixInv);
//ToLog("\nForw. test: " + VectToStr(vForw), true);
Vector3D moveInputSign = Vector3D.Sign(moveInput);
double curAltitude, deltaAlt = 0;
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out curAltitude);
if (InertiaDampeners)
{
DesiredSpeed.X = MaxSpeed * moveInputSign.X;
if (AltitudeHoldMode)
{
deltaAlt = Math.Abs(DesiredAltitude) * 0.01;
if (deltaAlt < 0.1)
{
deltaAlt = 0.1;
}
if (moveInputSign.Y != 0.0)
{
DesiredAltitude += deltaAlt * moveInputSign.Y;
if (DesiredAltitude < 0.0)
{
DesiredAltitude = 0;
}
}
deltaAlt = DesiredAltitude - curAltitude;
//ускорение потом вычислим, желаемая скорость для этого не нужна
}
DesiredSpeed.Y = MaxSpeed * moveInputSign.Y;
if (CruiseControl)
{
DesiredSpeed.Z += DeltaSpeed * moveInputSign.Z;
}
else
{
DesiredSpeed.Z = MaxSpeed * moveInputSign.Z;
}
//DesiredSpeed = Vector3D.Sign(moveInput) * MaxSpeed;
}
else
{
//DesiredSpeed = PlanetSpeed;
if (moveInput.X == 0d)
{
DesiredSpeed.X = PlanetSpeed.X;
}
else
{
DesiredSpeed.X = MaxSpeed * moveInputSign.X;
}
if (moveInput.Y == 0d)
{
DesiredSpeed.Y = PlanetSpeed.Y;
}
else
{
DesiredSpeed.Y = MaxSpeed * moveInputSign.Y;
}
if (moveInput.Z == 0d)
{
DesiredSpeed.Z = PlanetSpeed.Z;
}
else
{
if (CruiseControl)
{
DesiredSpeed.Z += DeltaSpeed * moveInputSign.Z;
}
else
{
DesiredSpeed.Z = MaxSpeed * moveInputSign.Z;
}
}
}
DesiredSpeed = Vector3D.Clamp(DesiredSpeed, MaxSpeedMin, MaxSpeedMax);
if (CruiseControl)
{
ToLog("\nСкорость: " + (-PlanetSpeed.Z).ToString("#0") + " / " + (-DesiredSpeed.Z).ToString("#0"), true);
}
else
{
ToLog("\nСкорость: " + (-PlanetSpeed.Z).ToString("#0"), true);
}
if (InertiaDampeners && AltitudeHoldMode)
{
ToLog("\nВысота: " + curAltitude.ToString("#0") + " / " + DesiredAltitude.ToString("#0"), true);
}
else
{
ToLog("\nВысота: " + curAltitude.ToString("#0"), true);
}
Vector3D SpeedDeviation = DesiredSpeed - PlanetSpeed;
//ToLog("\nDes.spd: " + VectToStr(DesiredSpeed), true);
//ToLog("\nSpd.dev: " + VectToStr(SpeedDeviation), true);
//Z - вперед, X - вправо, Y - вверх
Vector3D DesiredAccelerations;
const double SigmoidFactorAlpha = 0.5;
DesiredAccelerations.X = Sigmoid(SpeedDeviation.X, SigmoidFactorAlpha) * CurrentRegimeAccel;
DesiredAccelerations.Z = Sigmoid(SpeedDeviation.Z, SigmoidFactorAlpha) * CurrentRegimeAccel;
if (AltitudeHoldMode && InertiaDampeners)
{
ToLog("\ndeltaAlt: " + deltaAlt.ToString("#0.000000"), true);
//ToLog("\nPlanetSpeed.Y: " + PlanetSpeed.Y.ToString("#0.000000"), true);
////нужно такое ускорение, чтобы корабль остановился при нулевом отклонении по высоте
//double absDeltaAlt = Math.Abs(deltaAlt);
//if (absDeltaAlt >= 0.5)
//{
// int signDeltaAlt = Math.Sign(deltaAlt);
// //минимальное расстояние, на котором мы еще успеем затормозить
// if (Math.Sign(PlanetSpeed.Y) != signDeltaAlt)
// {
// //удаляемся
// DesiredAccelerations.Y = CurrentRegimeAccel * signDeltaAlt;
// //ToLog("\nудаляемся", true);
// }
// else
// {
// //приближаемся, оценим тормозной путь
// double temp = 0.5 * PlanetSpeed.Y * PlanetSpeed.Y;
// double brakingDist = temp / CurrentRegimeAccel;
// //ToLog("\nbrakingDist: " + brakingDist.ToString("#0.00000"), true);
// if (absDeltaAlt > brakingDist)
// {
// //еще успеваем затормозить, поэтому даем максимальное ускорение
// DesiredAccelerations.Y = CurrentRegimeAccel * signDeltaAlt;
// }
// else
// {
// //уменьшаем ускорение
// DesiredAccelerations.Y = MathHelperD.Clamp(-temp / deltaAlt, -CurrentRegimeAccel, CurrentRegimeAccel);
// }
// }
//}
//else
//{
// DesiredAccelerations.Y = Sigmoid(SpeedDeviation.Y, SigmoidFactorAlpha) * CurrentRegimeAccel;
// //ToLog("\n!!!", true);
//}
DesiredAccelerations.Y = CalcAccelToStopAtPos(deltaAlt, PlanetSpeed.Y, CurrentRegimeAccel, 0.5, 0.5);
}
else
{
DesiredAccelerations.Y = Sigmoid(SpeedDeviation.Y, SigmoidFactorAlpha) * CurrentRegimeAccel;
}
ToLog("\nDes.acc: " + VectToStr(DesiredAccelerations), true);
//double PlanetElevation;
//Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out PlanetElevation);
double MaxDownThrust = 0;
foreach (IMyThrust t in DownTrusters)
{
MaxDownThrust += t.MaxEffectiveThrust;
}
double ShipMass = Cockpit.CalculateShipMass().PhysicalMass;
//ToLog("\nMax thr: " + MaxDownThrust.ToString("#0.0"), true);
//ToLog("\nMass: " + ShipMass.ToString("#0.0"), true);
double DesiredVertivalAcc = DesiredAccelerations.Y + Gravity.Length();
if (DesiredVertivalAcc < 1.0)
{
DesiredVertivalAcc = 1.0;
}
ToLog("\nDes.vert.acc: " + DesiredVertivalAcc.ToString("#0.000"), true);
double cosGravityToUp = normVertical.Dot(Cockpit.WorldMatrix.Up);
double Thrust = ShipMass * DesiredVertivalAcc / cosGravityToUp;
//ToLog("\nThrust: " + Thrust.ToString("#0.000"), true);
//раздадим на движки
float ThrustFactor = (float)(Thrust / MaxDownThrust);
ToLog("\nThr.factor: " + ThrustFactor.ToString("#0.000"), true);
if (ThrustFactor > 1f)
{
ThrustFactor = 1f;
}
foreach (IMyThrust t in DownTrusters)
{
t.ThrustOverridePercentage = ThrustFactor;
}
//углы откладываются от вертикали, X - тангаж, Y - крен
Vector2D DesiredAngles;
DesiredAngles.X = Math.Atan2(DesiredAccelerations.Z, DesiredVertivalAcc);
DesiredAngles.Y = Math.Atan2(DesiredAccelerations.X, DesiredVertivalAcc);
DesiredAngles = Vector2D.Clamp(DesiredAngles, MaxDeviationAngleMin, MaxDeviationAngleMax);
ToLog("\nDes.angl: " + Vect2ToStr(DesiredAngles), true);
double cosGravityToForward = normVertical.Dot(Cockpit.WorldMatrix.Forward);
double cosGravityToRight = normVertical.Dot(Cockpit.WorldMatrix.Left);
//ToLog("\nCos(G) to F/R: " + cosGravityToForward.ToString("#0.000") + " / " + cosGravityToRight.ToString("#0.000"), true);
Vector2D CurrentAngles;
CurrentAngles.X = Math.Atan2(cosGravityToForward, cosGravityToUp);
CurrentAngles.Y = Math.Atan2(cosGravityToRight, cosGravityToUp);
ToLog("\nCur.angl: " + Vect2ToStr(CurrentAngles), true);
//ошибка угла
Vector2D AnglesDeviation = Vector2D.Clamp(NormAngles2D(DesiredAngles - CurrentAngles), MaxDeviationAngleMin, MaxDeviationAngleMax);
ToLog("\nAngl.dev: " + Vect2ToStr(AnglesDeviation, "#0.00000"), true);
Vector2D GyroSignals;
////демфируем сигнал при приближении отклонения к максимальному значению
//GyroSignals.X = SignalDamper(AnglesDeviation.X, MaxDeviationAngle, CurrentAngles.X, MaxDeviationAngle, 0.5);
//GyroSignals.Y = SignalDamper(AnglesDeviation.Y, MaxDeviationAngle, CurrentAngles.Y, MaxDeviationAngle, 0.5);
ToLog("\nPAV: " + VectToStr(PlanetAngularVelocity), true);
GyroSignals.X = CalcAccelToStopAtPos(AnglesDeviation.X, PlanetAngularVelocity.X, MaxAngularAcceleration, 0.5 * Math.PI / 180.0, 1);
GyroSignals.Y = CalcAccelToStopAtPos(AnglesDeviation.Y, -PlanetAngularVelocity.Z, MaxAngularAcceleration, 0.5 * Math.PI / 180.0, 1);
ToLog("\nGyroSignals: " + Vect2ToStr(GyroSignals, "#0.00000"), true);
//раздадим на гороскопы
GyroPitch = -(float)(GyroPithPID.Reaction(GyroSignals.X) * GyroFactor);
GyroRoll = (float)(GyroRollPID.Reaction(GyroSignals.Y) * GyroFactor);
//GyroPitch = -(float)(AnglesDeviation.X * GyroFactor);
//GyroRoll = (float)(AnglesDeviation.Y * GyroFactor);
ToLog("\nGyro P/Y/R: " + GyroPitch.ToString("#0.000") + " / " + GyroYaw.ToString("#0.000") + " / " + GyroRoll.ToString("#0.000"), true);
foreach (GyroDefinition g in Gyros)
{
SetGyroParams(g, true, GyroPitch, GyroYaw, GyroRoll);
}
}
public double Distance(Vector3D point)
{
var clamp = Vector3D.Clamp(point, Min, Max);
return(Vector3D.Distance(clamp, point));
}
