/// <summary>
/// Computes the inertia tensor of the vessel. Uses the parallel axis theorem to
/// sum the contributions to the inertia tensor from every part in the vessel.
/// It (ab)uses the Matrix4x4 class in order to do 3x3 matrix operations.
/// </summary>
Matrix4x4 ComputeInertiaTensor()
{
var vessel = InternalVessel;
Matrix4x4 inertiaTensor = Matrix4x4.zero;
Vector3 CoM = vessel.findWorldCenterOfMass();
// Use the part ReferenceTransform because we want pitch/roll/yaw relative to controlling part
Transform vesselTransform = vessel.GetTransform();
foreach (var part in vessel.parts)
{
if (part.rb != null)
{
Matrix4x4 partTensor = part.rb.inertiaTensor.ToDiagonalMatrix();
// translate: inertiaTensor frame to part frame, part frame to world frame, world frame to vessel frame
Quaternion rot = Quaternion.Inverse(vesselTransform.rotation) * part.transform.rotation * part.rb.inertiaTensorRotation;
Quaternion inv = Quaternion.Inverse(rot);
Matrix4x4 rotMatrix = Matrix4x4.TRS(Vector3.zero, rot, Vector3.one);
Matrix4x4 invMatrix = Matrix4x4.TRS(Vector3.zero, inv, Vector3.one);
// add the part inertiaTensor to the ship inertiaTensor
inertiaTensor = inertiaTensor.Add(rotMatrix * partTensor * invMatrix);
Vector3 position = vesselTransform.InverseTransformDirection(part.rb.position - CoM);
// add the part mass to the ship inertiaTensor
inertiaTensor = inertiaTensor.Add((part.rb.mass * position.sqrMagnitude).ToDiagonalMatrix());
// add the part distance offset to the ship inertiaTensor
inertiaTensor = inertiaTensor.Add(position.OuterProduct(-part.rb.mass * position));
}
}
return(inertiaTensor.MultiplyScalar(1000f));
}
//Compute the Quadric Error Metric at point v
//The error for v1, v2 is given by v^T * (Q1 + Q2) * v
//where v = [v.x, v.y, v.z, 1] is the optimal contraction target position
private float CalculateQEM(Vector3 v, Matrix4x4 Q1, Matrix4x4 Q2)
{
//qem = v^T * (Q1 + Q2) * v
Matrix4x4 Q = Q1.Add(Q2);
float x = v.x;
float y = v.y;
float z = v.z;
//v^T * Q * v
//Verify that this is true (was found at bottom in research paper)
float qemCalculations = 0f;
qemCalculations += (1f * Q[0, 0] * x * x);
qemCalculations += (2f * Q[0, 1] * x * y);
qemCalculations += (2f * Q[0, 2] * x * z);
qemCalculations += (2f * Q[0, 3] * x);
qemCalculations += (1f * Q[1, 1] * y * y);
qemCalculations += (2f * Q[1, 2] * y * z);
qemCalculations += (2f * Q[1, 3] * y);
qemCalculations += (1f * Q[2, 2] * z * z);
qemCalculations += (2f * Q[2, 3] * z);
qemCalculations += (1f * Q[3, 3]);
float qem = qemCalculations;
return(qem);
}
public HmdMatrix34_t ToHmdMatrix34_t()
{
// Return this transform as HmdMatrix34_t
Matrix4x4 rotation = Matrix4x4.CreateFromYawPitchRoll(DegreesToRadians(RotationY), DegreesToRadians(RotationX), DegreesToRadians(RotationZ));
Matrix4x4 translation = Matrix4x4.CreateTranslation(PositionX, PositionY, PositionZ);
return(Matrix4x4.Add(rotation, translation).ToHmdMatrix34_t());
}
public void Add()
{
var m = Matrix4x4.Identity;
Matrix4x4[] input = { m };
Matrix4x4[] expectedOutput = { Matrix4x4.Identity * 2 };
Matrix4x4[] output1 = input.AsCompute().Select(x => Matrix4x4.Add(x, Matrix4x4.Identity)).ToArray();
Matrix4x4[] output2 = input.AsCompute().Select(x => x + Matrix4x4.Identity).ToArray();
Assert.Equal(expectedOutput, output1);
Assert.Equal(expectedOutput, output2);
}
/// Get average rotation.
public static Quaternion Average(this IList <Quaternion> elements)
{
// transform all quaternions to matrices 4x4
IList <Matrix4x4> rotations = elements.Select(e => Matrix4x4.TRS(Vector3.zero, e, Vector3.one)).ToList();
Matrix4x4 rotations_sum = Matrix4x4.zero;
foreach (Matrix4x4 rotation in rotations)
{
rotations_sum = rotations_sum.Add(rotation);
}
Matrix4x4 average = rotations_sum.Divide(rotations.Count);
return(average.rotation);
}
public Matrix4x4 AddBenchmark() => Matrix4x4.Add(Matrix4x4.Identity, Matrix4x4.Identity);
//Calculate the Q matrix for a vertex if we know all edges pointing to the vertex
public static Matrix4x4 CalculateQMatrix(HashSet <HalfEdge3> edgesPointingToVertex, bool normalizeTriangles)
{
Matrix4x4 Q = Matrix4x4.zero;
//Calculate a Kp matrix for each triangle attached to this vertex and add it to the sumOfKp
foreach (HalfEdge3 e in edgesPointingToVertex)
{
//To calculate the Kp matric we need all vertices
Vector3 p1 = e.v.position;
Vector3 p2 = e.nextEdge.v.position;
Vector3 p3 = e.nextEdge.nextEdge.v.position;
//...and a normal
Vector3 normal = _Geometry.CalculateTriangleNormal(p1, p2, p3);
if (float.IsNaN(normal.x) || float.IsNaN(normal.y) || float.IsNaN(normal.z))
{
Debug.LogWarning("This normal has length 0");
//TestAlgorithmsHelpMethods.DisplayMyVector3(p1);
//TestAlgorithmsHelpMethods.DisplayMyVector3(p2);
//TestAlgorithmsHelpMethods.DisplayMyVector3(p3);
//Temp solution if the normal is zero
Q = Q.Add(Matrix4x4.zero);
continue;
}
//To calculate the Kp matrix, we have to define the plane on the form:
//ax + by + cz + d = 0 where a^2 + b^2 + c^2 = 1
//a, b, c are given by the normal:
float a = normal.x;
float b = normal.y;
float c = normal.z;
//To calculate d we just use one of the points on the plane (in the triangle)
//d = -(ax + by + cz)
float d = -(a * p1.x + b * p1.y + c * p1.z);
//This built-in matrix is initialized by giving it columns
Matrix4x4 Kp = new Matrix4x4(
new Vector4(a * a, a * b, a * c, a * d),
new Vector4(a * b, b * b, b * c, b * d),
new Vector4(a * c, b * c, c * c, c * d),
new Vector4(a * d, b * d, c * d, d * d)
);
//You can multiply this Kp with the area of the triangle to get a weighted-Kp which may improve the result
//This is only needed if the triangles have very different size
if (normalizeTriangles)
{
float triangleArea = _Geometry.CalculateTriangleArea(p1, p2, p3);
Kp = Kp.Multiply(triangleArea);
}
//Q is the sum of all Kp around the vertex
Q = Q.Add(Kp);
}
return(Q);
}
// Данная программа создает матрицы при помощи System.Numerics.Matrix4x4, складывает их и выводит затраченное время на выполнение 1000 итераций цикла сложения.
static void Main(string[] args)
{
Console.WriteLine($"{ "Данное приложение сгенерирует 2 матрицы с числами диапозоном от 0,01 до 1.\n",92}" +
$"{"каждый последующий результат сложения двух матриц будет складываться со\n",90}" +
$"{"следующей сгенерированной матрицей.\n",72}" +
$"{"Продолжение цикла 1000 итераций.\n",70}" +
$"{"Нажмите любую клавишу для продолжения.\n",74}");
Console.ReadKey();
Stopwatch Time = new Stopwatch(); // Объявление экземпляра класса подсчета времени
Time.Start(); // Начало отсчета времени выполнения программы
float min = 0.01f; // Минимальное число матрицы
float max = 1f; // Максимальное число матрицы
Random r = new Random(); // Генерирование случайных чисел для матриц
float randomNumber = (float)r.NextDouble() * (max - min) + min;
float randomNumberTwo = (float)r.NextDouble() * (max - min) + min;
void output(Matrix4x4 temp) // метод вывода матрицы в консоль
{
Console.Write(temp.M11 + "| " + temp.M12 + "| " + temp.M13 + "| " + temp.M14 + "| \n" +
temp.M21 + "| " + temp.M22 + "| " + temp.M23 + "| " + temp.M24 + "| \n" +
temp.M31 + "| " + temp.M32 + "| " + temp.M33 + "| " + temp.M34 + "| \n" +
temp.M41 + "| " + temp.M42 + "| " + temp.M43 + "| " + temp.M44 + "| \n");
}
// создание первой матрицы
Matrix4x4 mas = new Matrix4x4(randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber, randomNumber);
// Создание второй матрицы
Matrix4x4 masTwo = new Matrix4x4();
Console.WriteLine("\nПервая матрица\n");
output(mas); // Вызов метода вывода матрицы в консоль
void createMatrix() // Метод создания последующих матриц для сложения
{
// Создание последующих матриц для сложения
masTwo = new Matrix4x4(randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo, randomNumberTwo);
}
for (int i = 0; i < 1000; i++) // Цикл сложения матриц
{
createMatrix(); // Вызов метода создания матрицы
Console.WriteLine("\nСледующая матрица\n");
output(masTwo); // Вызов метода вывода матрицы в консоль
mas = Matrix4x4.Add(mas, masTwo); // Сложение матриц
Console.WriteLine("\nрезультат\n");
output(mas); // Вызов метода вывода результата сложения в консоль
}
Time.Stop(); // Конец отсчета времени выполнения программы
Console.WriteLine("Время затраченное на выполнение операций: " + Time.Elapsed.TotalSeconds + "\n"); // Вывод затраченного времени
Console.WriteLine("Нажмите любую кнопку для выхода.");
Console.ReadKey();
}
public static Matrix4x4 Add(this Matrix4x4 first, Matrix4x4 second)
{
return(Matrix4x4.Add(first, second));
}
