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)); }