public void TestNegativeClamp()
{
Vector2D aVector = new Vector2D(-7, -7);
aVector.Clamp();
Assert.IsTrue(aVector.Equals(new Vector2D(-1, -1)));
}
public void TestPositiveClamp()
{
Vector2D aVector = new Vector2D(7, 7);
aVector.Clamp();
Assert.IsTrue(aVector.Equals(new Vector2D(1, 1)));
}
public void ClampStatic2()
{
Vector2D clamped = new Vector2D(-10, 1);
clamped = Vector2D.Clamp(clamped, -1, 0);
Assert.AreEqual(-1, clamped.X);
Assert.AreEqual(0, clamped.Y);
}
public void Clamp2()
{
Vector2D clamped = new Vector2D(-10, 1);
clamped.Clamp(-1, 0);
Assert.AreEqual(-1, clamped.X);
Assert.AreEqual(0, clamped.Y);
}
public void Intersects(ref BoundingRectangle rect, out bool result)
{
Vector2D proj;
Vector2D.Clamp(ref Position, ref rect.Min, ref rect.Max, out proj);
Scalar distSq;
Vector2D.DistanceSq(ref Position, ref proj, out distSq);
result = distSq <= Radius * Radius;
}
public void Vector2ClampTest()
{
Vector2D <float> a = new Vector2D <float>(0.5f, 0.3f);
Vector2D <float> min = new Vector2D <float>(0.0f, 0.1f);
Vector2D <float> max = new Vector2D <float>(1.0f, 1.1f);
// Normal case.
// Case N1: specified value is in the range.
Vector2D <float> expected = new Vector2D <float>(0.5f, 0.3f);
Vector2D <float> actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
// Normal case.
// Case N2: specified value is bigger than max value.
a = new Vector2D <float>(2.0f, 3.0f);
expected = max;
actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
// Case N3: specified value is smaller than max value.
a = new Vector2D <float>(-1.0f, -2.0f);
expected = min;
actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
// Case N4: combination case.
a = new Vector2D <float>(-2.0f, 4.0f);
expected = new Vector2D <float>(min.X, max.Y);
actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
// User specified min value is bigger than max value.
max = new Vector2D <float>(0.0f, 0.1f);
min = new Vector2D <float>(1.0f, 1.1f);
// Case W1: specified value is in the range.
a = new Vector2D <float>(0.5f, 0.3f);
expected = max;
actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
// Normal case.
// Case W2: specified value is bigger than max and min value.
a = new Vector2D <float>(2.0f, 3.0f);
expected = max;
actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
// Case W3: specified value is smaller than min and max value.
a = new Vector2D <float>(-1.0f, -2.0f);
expected = max;
actual = Vector2D.Clamp(a, min, max);
Assert.True(MathHelper.Equal(expected, actual), "Vector2f.Clamp did not return the expected value.");
}
private void OnMove(Vector2D pos)
{
try
{
pos = new Vector2D(Math.Round(pos.X, Rounding), Math.Round(pos.Y, Rounding));
pos = Vector2D.Clamp(pos, Min, Max);
Moving?.Invoke(pos);
UpdateTitle();
}
catch (Exception e)
{
Log.Error(e);
}
}
private void OnSubmit(Vector2D pos)
{
try
{
pos = new Vector2D(Math.Round(pos.X, Rounding), Math.Round(pos.Y, Rounding));
pos = Vector2D.Clamp(pos, Min, Max);
Setter?.Invoke(pos);
Item.Origin = pos;
UpdateTitle();
}
catch (Exception e)
{
Log.Error(e);
}
}
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 void Clamp2()
{
Vector2D clamped = new Vector2D(-10, 1);
clamped.Clamp(-1, 0);
Assert.AreEqual(-1, clamped.X);
Assert.AreEqual(0, clamped.Y);
}
