private void Init()
{
var placement = _model.Get <Axis2Placement3D>(_surface.PointId);
var cartesianPoint = _model.Get <CartesianPoint>(placement.PointIds[0]);
var direction = _model.Get <DirectionPoint>(placement.PointIds[1]);
var directionX = _model.Get <DirectionPoint>(placement.PointIds[2]);
Points = new List <Vector3> {
cartesianPoint.Vector
};
var x = new Vector3(1, 0, 0);
var y = new Vector3(0, 1, 0);
var ax = Math.Acos(Vector3.Dot(direction.Vector, x) / (direction.Vector.Norm * x.Norm));
var ay = Math.Acos(Vector3.Dot(direction.Vector, y) / (direction.Vector.Norm * y.Norm));
var rotationMatrix = Matrix3x3.CreateFromYawPitchRoll((float)(Math.PI / 2 - ax), (float)(Math.PI / 2 - ay), 0);
for (int i = 0; i < Sides; i++)
{
var angle = 360 - (i * 360d / Sides);
angle = angle * 2 * Math.PI / 360d;
// calculate point on unit circle and multiply by radius
var vector = new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0) * (float)_surface.Radius;
// change normal vector to direction vector
vector = rotationMatrix * vector;
// add midpoint position vector
vector = vector + cartesianPoint.Vector;
Points.Add(vector);
}
Indices = Enumerable.Range(1, Sides).Select(i => new int[] { 0, i, (i % Sides) + 1 }).SelectMany(d => d).ToList();
}
public void CreateFromYawPitchRollTest(float yaw, float pitch, float roll)
{
float radiansYaw = (float)(yaw * System.Math.PI / 180);
float radiansPitch = (float)(pitch * System.Math.PI / 180);
float radiansRoll = (float)(roll * System.Math.PI / 180);
Matrix3x3 matrix = Matrix3x3.CreateFromYawPitchRoll(radiansYaw, radiansPitch, radiansRoll);
Matrix3x3 xMatrix = Matrix3x3.CreateRotationX(radiansPitch);
Matrix3x3 yMatrix = Matrix3x3.CreateRotationY(radiansYaw);
Matrix3x3 zMatrix = Matrix3x3.CreateRotationZ(radiansRoll);
Matrix3x3 rotationMatrix = (yMatrix * xMatrix) * zMatrix;
CompareMatrixWithArray(matrix, rotationMatrix.ToArray());
}
private void Init()
{
var s = _model.Get <CartesianPoint>(_line.Point1Id);
var e = _model.Get <VectorPoint>(_line.Point2Id);
var direction = _model.Get <DirectionPoint>(e.PointId);
var endVector = s.Vector + direction.Vector * (float)e.Length;
var x = new Vector3(1, 0, 0);
var y = new Vector3(0, 1, 0);
var ax = Math.Acos(Vector3.Dot(direction.Vector, x) / (direction.Vector.Norm * x.Norm));
var ay = Math.Acos(Vector3.Dot(direction.Vector, y) / (direction.Vector.Norm * y.Norm));
Points = new List <Vector3> {
s.Vector
};
var rotationMatrix = Matrix3x3.CreateFromYawPitchRoll((float)(Math.PI / 2 - ax), (float)(Math.PI / 2 - ay), 0);
for (var i = 0; i < Sides; i++)
{
var angle = 360 - (i * 360d / Sides);
angle = angle * 2 * Math.PI / 360d;
// calculate point on unit circle and multiply by radius
var vector = new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0) * (float)Thickness / 2;
// change normal vector to direction vector
vector = rotationMatrix * vector;
// add midpoint position vector
vector = vector + s.Vector;
Points.Add(vector);
}
Points.Add(endVector);
for (var i = 0; i < Sides; i++)
{
var angle = 360 - (i * 360d / Sides);
angle = angle * 2 * Math.PI / 360d;
// calculate point on unit circle and multiply by radius
var vector = new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0) * (float)Thickness / 2;
// change normal vector to direction vector
vector = rotationMatrix * vector;
// add midpoint position vector
vector = vector + endVector;
Points.Add(vector);
}
Indices = new List <int> {
0, 1, 2, 0, 2, 3, 0, 3, 4, 0, 4, 1, 5, 6, 7, 5, 7, 8, 5, 8, 9, 5, 9, 6, 1, 6, 7, 7, 2, 1, 2, 7, 8, 8, 3, 2, 3, 8, 9, 9, 4, 3, 4, 9, 6, 6, 1, 4
};
}
public void ExtractYawPitchRollTest(float yaw, float pitch, float roll)
{
float radiansYaw = (float)(yaw * System.Math.PI / 180);
float radiansPitch = (float)(pitch * System.Math.PI / 180);
float radiansRoll = (float)(roll * System.Math.PI / 180);
Matrix3x3 matrix = Matrix3x3.CreateFromYawPitchRoll(radiansYaw, radiansPitch, radiansRoll);
float extractedYaw;
float extractedPitch;
float extractedRoll;
matrix.ExtractYawPitchRoll(out extractedYaw, out extractedPitch, out extractedRoll);
Assert.AreEqual(radiansYaw, extractedYaw, Epsilon);
Assert.AreEqual(radiansPitch, extractedPitch, Epsilon);
Assert.AreEqual(radiansRoll, extractedRoll, Epsilon);
}
public winchDirection getForceDirection(winchPosition _winchPosition, PlaneInfoResponse _planeInfoResponse)
{
winchDirection _winchDirection = new winchDirection();
double globalX = (_winchPosition.location.Longitude - _planeInfoResponse.Longitude) * Math.Cos(_winchPosition.location.Latitude) * 6378137;
double globalY = _winchPosition.alt - _planeInfoResponse.Altitude;
double globalZ = (_winchPosition.location.Latitude - _planeInfoResponse.Latitude) * 180 / Math.PI * 111694;
Vector3 globalToWinch = new Vector3((float)globalX, (float)globalY, (float)globalZ);
Vector3 globalToWinchNorm = globalToWinch;
globalToWinchNorm.Normalize();
_winchDirection.climbAngle = Math.Abs(Math.Asin(globalToWinchNorm.Y));
Matrix3x3 attitude = Matrix3x3.CreateFromYawPitchRoll((float)_planeInfoResponse.PlaneHeading, (float)_planeInfoResponse.PlanePitch, (float)_planeInfoResponse.PlaneBank);
_winchDirection.localForceDirection = Matrix3x3.Multiply(attitude.Inverse(), globalToWinchNorm);
_winchDirection.localForceDirection.Normalize();
_winchDirection.heading = Math.Atan2(_winchDirection.localForceDirection.X, _winchDirection.localForceDirection.Z);
_winchDirection.pitch = Math.Asin(_winchDirection.localForceDirection.Y);
_winchDirection.distance = (double)(globalToWinch.Norm);
globalToWinch.Y = 0;
_winchDirection.groundDistance = (double)(globalToWinch.Norm);
if (_winchDirection.heading > Math.PI)
{
_winchDirection.heading -= 2 * Math.PI;
}
if (_winchDirection.heading < -Math.PI)
{
_winchDirection.heading += 2 * Math.PI;
}
if (_winchDirection.pitch > Math.PI)
{
_winchDirection.pitch -= 2 * Math.PI;
}
if (_winchDirection.pitch < -Math.PI)
{
_winchDirection.pitch += 2 * Math.PI;
}
return(_winchDirection);
}