public void TestSphericalEarthSurface() { GroundCallback cb = new DefaultGroundCallback(RadiusReference, RadiusReference); Location loc, contact; Vector3D normal, v, w; Vector3D zero = new Vector3D(0.0, 0.0, 0.0); // Check that, for a point located, on the sea level radius the AGL is 0.0 for (double lat = -90.0; lat <= 90.0; lat += 30.0) { for (double lon = 0.0; lon <= 360.0; lon += 45.0) { double lon_rad = lon * Math.PI / 180.0; double lat_rad = lat * Math.PI / 180.0; loc = new Location(lon_rad, lat_rad, RadiusReference); double agl = cb.GetAGLevel(loc, out contact, out normal, out v, out w); Assert.AreEqual(0.0, agl, 1e-8); AssertVectorEqual(v, zero); AssertVectorEqual(w, zero); Vector3D vLoc = (Vector3D)loc; Vector3D vContact = (Vector3D)contact; Assert.AreEqual(vContact.Magnitude(), RadiusReference, tolerance); Assert.AreEqual(vLoc.X, vContact.X, 1e-8); Assert.AreEqual(vLoc.Y, vContact.Y, 1e-8); Assert.AreEqual(vLoc.Z, vContact.Z, 1e-8); Assert.AreEqual(normal.X, Math.Cos(lat_rad) * Math.Cos(lon_rad), tolerance); Assert.AreEqual(normal.Y, Math.Cos(lat_rad) * Math.Sin(lon_rad), tolerance); Assert.AreEqual(normal.Z, Math.Sin(lat_rad), tolerance); vContact.Normalize(); AssertVectorEqual(vContact, normal); } } }
public void CheckLocation(Location loc, Vector3D vec, double delta = default_tolerance) { Quaternion qloc = new Quaternion(Quaternion.EulerAngles.eTht, -0.5 * Math.PI); Quaternion q = new Quaternion(); double r = vec.Magnitude(); Assert.AreEqual(vec.X, loc[1], r * delta); Assert.AreEqual(vec.Y, loc[2], r * delta); Assert.AreEqual(vec.Z, loc[3], r * delta); Assert.AreEqual(r, loc.Radius, r * delta); vec.Normalize(); double lon = Math.Atan2(vec.Y, vec.X); double lat = Math.Asin(vec.Z); Assert.AreEqual(lon, loc.Longitude, delta); Assert.AreEqual(lat, loc.Latitude, delta); Assert.AreEqual(Math.Sin(lon), loc.SinLongitude, delta); Assert.AreEqual(Math.Cos(lon), loc.CosLongitude, delta); Assert.AreEqual(Math.Sin(lat), loc.SinLatitude, delta); Assert.AreEqual(Math.Cos(lat), loc.CosLatitude, delta); Assert.AreEqual(Math.Tan(lat), loc.TanLatitude, delta); q = new Quaternion(0.0, -lat, lon); Matrix3D m = (q * qloc).GetTransformationMatrix(); AssertMatrixEqual(m, loc.GetTec2l()); AssertMatrixEqual(m.GetTranspose(), loc.GetTl2ec()); }
static HDCallbackCode GravityWellCallback(IntPtr pUserData) { const double kStiffness = 0.175; //(N/mm) const double kGravityWellInfluence = 50; //Box Size:50x50x50(mm) Vector3D position = new Vector3D(); Vector3D force = new Vector3D(); Vector3D positionTwell = new Vector3D(); uint hHD = HDAPI.hdGetCurrentDevice(); //触觉技术框架开始。(一般来说,所有与状态相关的触觉调用都应该在一个框架内进行。) HDAPI.hdBeginFrame(hHD); //获取设备的当前位置 HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, ref position); //Console.WriteLine("Vector3D:{0} {1} {2}", position.X, position.Y, position.Z); force.ResetZero(); // positionTwell = wellPos - position //创建一个从设备位置到重力井中心的矢量 Vector3D.Subtrace(ref positionTwell, ref wellPos, ref position); //如果装置位置在重力井中心一定距离内,则向重力井中心施加弹簧力。 //力的计算不同于传统的重力体,因为装置离中心越近,井所施加的力就越小; //该装置的行为就像弹簧连接在它自己和井的中心。 if (InForce && Vector3D.Magnitude(ref positionTwell) < kGravityWellInfluence) { // F = k * x //F:力的单位为牛顿(N) //k: 井的刚度(N / mm) //x: 从设备端点位置到井中心的向量。 Vector3D.Scale(ref force, ref positionTwell, kStiffness); } if (!InForce && Vector3D.Magnitude(ref positionTwell) >= kGravityWellInfluence) { Vector3D.Scale(ref force, ref positionTwell, kStiffness); } //把力传送到设备上 HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, ref force); //End haptics frame. HDAPI.hdEndFrame(hHD); //check error HDErrorInfo error = HDAPI.hdGetError(); if (error.CheckedError()) { Console.WriteLine("渲染重力时检测到错误"); if (error.IsSchedulerError()) { Console.WriteLine("调度器错误。"); return(HDCallbackCode.HD_CALLBACK_DONE); } } return(HDCallbackCode.HD_CALLBACK_CONTINUE); }
/// <summary> 求点到线段最近的点 </summary> public Vector3D GetNearLestPoint(Vector3D startLine, Vector3D endLine, Vector3D point) { Vector3D first = point - startLine; Vector3D second = point - endLine; Vector3D line = endLine - startLine; //double firstCross = Vector3D.Angle(first, line); //double secondCross = Vector3D.Angle(second,line); double dot1 = first.Dot(line); double dot2 = second.Dot(line); // Todo :在外边 if (dot1 * dot2 < 0) { return(this.GetProjectivePoint(startLine, endLine, point)); } if (first.Magnitude() <= second.Magnitude()) { return(startLine); } return(endLine); }
public void Vectors() { // Default Value Test var vectorDefault = new Vector3D(); var vectorZero = new Vector3D() { X = 0.0, Y = 0.0, Z = 0.0 }; Assert.AreEqual(vectorDefault.X, vectorZero.X); Assert.AreEqual(vectorDefault.Y, vectorZero.Y); Assert.AreEqual(vectorDefault.Z, vectorZero.Z); // Constructor Test var vectorNonZero = new Vector3D() { X = 1.2, Y = 3.4, Z = 5.6 }; Assert.AreEqual(1.2, vectorNonZero.X); Assert.AreEqual(3.4, vectorNonZero.Y); Assert.AreEqual(5.6, vectorNonZero.Z); Assert.AreEqual(Math.Sqrt(1.2 * 1.2 + 3.4 * 3.4 + 5.6 * 5.6), vectorNonZero.Magnitude()); // Normalize Test var normalizedVector = vectorNonZero.Normalize(); Assert.AreEqual(1.0, normalizedVector.Magnitude()); }
/// <summary> // Compute body frame rotational accelerations based on the current body moments /// /// vPQRdot is the derivative of the absolute angular velocity of the vehicle /// (body rate with respect to the ECEF frame), expressed in the body frame, /// where the derivative is taken in the body frame. /// J is the inertia matrix /// Jinv is the inverse inertia matrix /// vMoments is the moment vector in the body frame /// in.vPQRi is the total inertial angular velocity of the vehicle /// expressed in the body frame. /// Reference: See Stevens and Lewis, "Aircraft Control and Simulation", /// Second edition (2004), eqn 1.5-16e (page 50) /// </summary> private void CalculatePQRdot() { if (gravTorque) { // Compute the gravitational torque // Reference: See Harris and Lyle "Spacecraft Gravitational Torques", // NASA SP-8024 (1969) eqn (2) (page 7) Vector3D R = inputs.Ti2b * inputs.vInertialPosition; double invRadius = 1.0 / R.Magnitude(); R *= invRadius; inputs.Moment += (3.0 * inputs.vGravAccel.Magnitude() * invRadius) * (R * (inputs.J * R)); } // Compute body frame rotational accelerations based on the current body // moments and the total inertial angular velocity expressed in the body // frame. // if (HoldDown && !FDMExec->GetTrimStatus()) { if (FDMExec.GetHoldDown()) { // The rotational acceleration in ECI is calculated so that the rotational // acceleration is zero in the body frame. vPQRdot = Vector3D.Zero; vPQRidot = inputs.vPQRi * (inputs.Ti2b * inputs.vOmegaPlanet); } else { vPQRidot = inputs.Jinv * (inputs.Moment - inputs.vPQRi * (inputs.J * inputs.vPQRi)); vPQRdot = vPQRidot - inputs.vPQRi * (inputs.Ti2b * inputs.vOmegaPlanet); } }
public void Simple() { var vector = new Vector3D(23, -21, 4); vector.Normalize(); Assert.AreEqual(1, vector.Magnitude()); }
public void Simple() { var vector3D = new Vector3D(4, 3, 12); Assert.AreEqual(13, vector3D.Magnitude()); Assert.AreEqual(4, vector3D.X); Assert.AreEqual(3, vector3D.Y); Assert.AreEqual(12, vector3D.Z); }
public void TestNormalize() { var v1 = new Vector3D(2.2F, -6.1F, 7.4F); Assert.Equal(9.8392072851F, (float)v1.Magnitude()); var normalized = v1.Normalize(); Assert.Equal(1.0F, (float)normalized.Magnitude()); }
static void Main(string[] args) { Console.WriteLine("Vector3DTest3"); Vector3D a = new Vector3D(2,3,4); Console.WriteLine("a =" + a); Console.WriteLine("|a|="); Console.WriteLine(a.Magnitude()); Console.Write("enter to quit"); Console.ReadLine(); }
public void Normalize() { double num = Vector3D.Magnitude(this); if (num > 1E-05f) { this /= num; } else { this = Vector3D.zero; } }
public void Normalize() { double num = Vector3D.Magnitude(this); if (num > 9.99999974737875E-06) { this = this / num; } else { this = Vector3D.zero; } }
public static Vector3D Normalize(Vector3D value) { double num = Vector3D.Magnitude(value); if (num > 9.99999974737875E-06) { return(value / num); } else { return(Vector3D.zero); } }
public static Vector3D Normalize(Vector3D value) { double num = Vector3D.Magnitude(value); if (num > 1E-25f) { return(value / num); } else { Debug.LogWarning("Error normalizing " + value); } return(Vector3D.zero); }
static HDCallbackCode FrictionlessSphereCallback(IntPtr pUserData) { uint hHD = HDAPI.hdGetCurrentDevice(); HDAPI.hdBeginFrame(hHD); //获取坐标位置 Vector3D position = new Vector3D(); HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, ref position); //Console.WriteLine("{0} {1} {2}", position.X, position.Y, position.Z); //计算设备和球体中心之间的距离。 Vector3D res = position - spherePosition; double distance = Vector3D.Magnitude(ref res); //如果用户在球体内,即用户到球体中心的距离小于球体半径, //则用户正在穿透球体,应命令一个力将用户推向表面。 if (distance < radius) { //计算穿透距离。 double penetrationDistance = radius - distance; //在力的方向上创建一个单位矢量,它总是从球体的中心通过用户的位置向外。 Vector3D forceDirection = (position - spherePosition) / distance; //使用 F = k * x 创建一个远离中心的力向量 //球体与穿透距离成正比,并受物体刚度的制约。 double k = stiffness; Vector3D x = penetrationDistance * forceDirection; Vector3D f = k * x; HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, ref f); } HDAPI.hdEndFrame(hHD); HDErrorInfo error = HDAPI.hdGetError(); if (error.CheckedError()) { Console.WriteLine("主调度程序回调期间出错."); if (error.IsSchedulerError()) { return(HDCallbackCode.HD_CALLBACK_DONE); } } return(HDCallbackCode.HD_CALLBACK_CONTINUE); }
public void TestSphericalEarthAltitudeWithTerrainElevation() { GroundCallback cb = new DefaultGroundCallback(RadiusReference, RadiusReference); Location loc, contact; Vector3D normal, v, w; Vector3D zero = new Vector3D(0.0, 0.0, 0.0); double h = 100000.0; double elevation = 2000.0; cb.SetTerrainElevation(elevation); // Check that, for a point located, on the sea level radius the AGL is 0.0 for (double lat = -90.0; lat <= 90.0; lat += 30.0) { for (double lon = 0.0; lon <= 360.0; lon += 45.0) { double lon_rad = lon * Math.PI / 180.0; double lat_rad = lat * Math.PI / 180.0; loc = new Location(lon_rad, lat_rad, RadiusReference + h); double agl = cb.GetAGLevel(loc, out contact, out normal, out v, out w); Assert.AreEqual((h - elevation) / agl, 1.0, tolerance * 100.0); AssertVectorEqual(v, zero); AssertVectorEqual(w, zero); Vector3D vLoc = (Vector3D)loc; Vector3D vContact = (Vector3D)contact; Assert.AreEqual(vContact.Magnitude() / (RadiusReference + elevation), 1.0, tolerance); AssertVectorEqual(vLoc / (RadiusReference + h), vContact / (RadiusReference + elevation)); Assert.AreEqual(normal.X, Math.Cos(lat_rad) * Math.Cos(lon_rad), tolerance); Assert.AreEqual(normal.Y, Math.Cos(lat_rad) * Math.Sin(lon_rad), tolerance); Assert.AreEqual(normal.Z, Math.Sin(lat_rad), tolerance); vContact.Normalize(); AssertVectorEqual(vContact, normal); } } }
public static Vector3D GetNormalized(this Vector3D vector) { return(Vector3D.Divide(vector, vector.Magnitude())); }
public void MagnitudeExample() { var vector3D = new Vector3D(4, 3, 12); Assert.AreEqual(13, vector3D.Magnitude()); }
double DistancePointLine(Vector3D point, Vector3D lineStart, Vector3D lineEnd) { return(Vector3D.Magnitude(ProjectPointLine(point, lineStart, lineEnd) - point)); }
private void ButtonCalcular_Click(object sender, EventArgs e) { if (!CamposVazios()) { if (textBoxAX.Text.Trim() != "" & textBoxAY.Text.Trim() != "" & textBoxAZ.Text.Trim() != "") { //LE VETOR A float vetAX = float.Parse(textBoxAX.Text); float vetAY = float.Parse(textBoxAY.Text); float vetAZ = float.Parse(textBoxAZ.Text); //instancia o vetor A Vector3D vectorA = new Vector3D(vetAX, vetAY, vetAZ); Vector3D aux = new Vector3D(); //verifica se tem segundo vetor como entrada (ACIONA METODOS QUE TRABALHA COM 2 VETORES) if (textBoxBX.Text.Trim() != "" & textBoxBY.Text.Trim() != "" & textBoxBZ.Text.Trim() != "") { //LE VETOR B float vetBX = float.Parse(textBoxBX.Text); float vetBY = float.Parse(textBoxBY.Text); float vetBZ = float.Parse(textBoxBZ.Text); Vector3D vectorB = new Vector3D(vetBX, vetBY, vetBZ); float escalar = float.Parse(textBoxEscalar.Text); String multEAX = Math.Round(aux.MultiplicacaoEscalar(vectorA, escalar).GetX(), 1).ToString().Replace(",", "."); String multEAY = Math.Round(aux.MultiplicacaoEscalar(vectorA, escalar).GetY(), 1).ToString().Replace(",", "."); String multEAZ = Math.Round(aux.MultiplicacaoEscalar(vectorA, escalar).GetZ(), 1).ToString().Replace(",", "."); mascaraMultiplicacaoEscalar_A.Text = $"({multEAX}, {multEAY}, {multEAZ})"; String multEBX = Math.Round(aux.MultiplicacaoEscalar(vectorB, escalar).GetX(), 1).ToString().Replace(",", "."); String multEBY = Math.Round(aux.MultiplicacaoEscalar(vectorB, escalar).GetY(), 1).ToString().Replace(",", "."); String multEBZ = Math.Round(aux.MultiplicacaoEscalar(vectorB, escalar).GetZ(), 1).ToString().Replace(",", "."); mascaraMultiplicacaoEscalar_B.Text = $"({multEBX}, {multEBY}, {multEBZ})"; String divEAX = Math.Round(aux.DivisaoEscalar(vectorA, escalar).GetX(), 1).ToString().Replace(",", "."); String divEAY = Math.Round(aux.DivisaoEscalar(vectorA, escalar).GetY(), 1).ToString().Replace(",", "."); String divEAZ = Math.Round(aux.DivisaoEscalar(vectorA, escalar).GetZ(), 1).ToString().Replace(",", "."); mascaraDivisaoEscalar_A.Text = $"({divEAX}, {divEAY}, {divEAZ})"; String divEBX = Math.Round(aux.DivisaoEscalar(vectorB, escalar).GetX(), 1).ToString().Replace(",", "."); String divEBY = Math.Round(aux.DivisaoEscalar(vectorB, escalar).GetY(), 1).ToString().Replace(",", "."); String divEBZ = Math.Round(aux.DivisaoEscalar(vectorB, escalar).GetZ(), 1).ToString().Replace(",", "."); mascaraDivisaoEscalar_B.Text = $"({divEBX}, {divEBY}, {divEBZ})"; String somX = Math.Round(aux.Soma(vectorA, vectorB).GetX(), 1).ToString().Replace(",", "."); String somY = Math.Round(aux.Soma(vectorA, vectorB).GetY(), 1).ToString().Replace(",", "."); String somZ = Math.Round(aux.Soma(vectorA, vectorB).GetZ(), 1).ToString().Replace(",", "."); mascaraSoma.Text = $"({somX}, {somY}, {somZ})"; String subX = Math.Round(aux.Subtracao(vectorA, vectorB).GetX(), 1).ToString().Replace(",", "."); String subY = Math.Round(aux.Subtracao(vectorA, vectorB).GetY(), 1).ToString().Replace(",", "."); String subZ = Math.Round(aux.Subtracao(vectorA, vectorB).GetZ(), 1).ToString().Replace(",", "."); mascaraSubtracao.Text = $"({subX}, {subY}, {subZ})"; mascaraDot.Text = Math.Round(aux.Dot(vectorA, vectorB), 1).ToString(); //tratamento pra substituir virgula por ponto (somente no calculo) String projX = Math.Round(aux.Projecao(vectorB, vectorA).GetX(), 1).ToString().Replace(",", "."); String projY = Math.Round(aux.Projecao(vectorB, vectorA).GetY(), 1).ToString().Replace(",", "."); String projZ = Math.Round(aux.Projecao(vectorB, vectorA).GetZ(), 1).ToString().Replace(",", "."); mascaraProjecao.Text = $"({projX}, {projY}, {projZ})"; String crossX = Math.Round(aux.Cross(vectorA, vectorB).GetX(), 1).ToString().Replace(",", "."); String crossY = Math.Round(aux.Cross(vectorA, vectorB).GetY(), 1).ToString().Replace(",", "."); String crossZ = Math.Round(aux.Cross(vectorA, vectorB).GetZ(), 1).ToString().Replace(",", "."); mascaraCross.Text = $"({crossX}, {crossY}, {crossZ})"; if (aux.Igualdade(vectorA, vectorB)) { textBoxIguais.Text = "Sim"; } else { textBoxIguais.Text = "Não"; } textBoxMagnitude_A.Text = Math.Round(aux.Magnitude(vectorA), 1).ToString(); textBoxMagnitude_B.Text = Math.Round(aux.Magnitude(vectorB), 1).ToString(); String normaAX = Math.Round(aux.Normalizar(vectorA).GetX(), 1).ToString().Replace(",", "."); String normaAY = Math.Round(aux.Normalizar(vectorA).GetY(), 1).ToString().Replace(",", "."); String normaAZ = Math.Round(aux.Normalizar(vectorA).GetZ(), 1).ToString().Replace(",", "."); mascaraUnitario_A.Text = $"({normaAX}, {normaAY}, {normaAZ})"; String normaBX = Math.Round(aux.Normalizar(vectorB).GetX(), 1).ToString().Replace(",", "."); String normaBY = Math.Round(aux.Normalizar(vectorB).GetY(), 1).ToString().Replace(",", "."); String normaBZ = Math.Round(aux.Normalizar(vectorB).GetZ(), 1).ToString().Replace(",", "."); mascaraUnitario_B.Text = $"({normaBX}, {normaBY}, {normaBZ})"; } } } }