| public static Magnitude ( Vector3D, a ) : double | ||
| a | Vector3D, | |
| return | double | |
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})";
}
}
}
}
