81 lines
1.6 KiB
C#
81 lines
1.6 KiB
C#
using System;
|
|
using OpenTK;
|
|
|
|
namespace org.budnhead.core
|
|
{
|
|
public static class Linear
|
|
{
|
|
static public bool Solve4x3(Vector3 X,Vector3 Y,Vector3 Z,Vector3 W,out Vector3 c)
|
|
{
|
|
Vector4[] rows = new Vector4[3];
|
|
rows[0] = new Vector4(X.X,Y.X,Z.X,W.X);
|
|
rows[1] = new Vector4(X.Y,Y.Y,Z.Y,W.Y);
|
|
rows[2] = new Vector4(X.Z,Y.Z,Z.Z,W.Z);
|
|
c = new Vector3();
|
|
|
|
//Console.WriteLine("New Linear Equation System to solve:");
|
|
//dumpLinearSystem(rows);
|
|
|
|
if (rows[0].Z == 0){
|
|
Vector4 t = rows[0];
|
|
rows[0] = rows[2];
|
|
rows[2] = t;
|
|
}
|
|
if (rows[0].Z == 0){
|
|
Vector4 t = rows[0];
|
|
rows[0] = rows[1];
|
|
rows[1] = t;
|
|
}
|
|
|
|
if (rows[0].Z == 0){
|
|
return false;
|
|
} else {
|
|
rows[1] -= rows[0] * (rows[1].Z / rows[0].Z);
|
|
rows[2] -= rows[0] * (rows[2].Z / rows[0].Z);
|
|
}
|
|
|
|
//dumpLinearSystem(rows);
|
|
|
|
if (rows[1].Y == 0){
|
|
Vector4 t = rows[1];
|
|
rows[1] = rows[2];
|
|
rows[2] = t;
|
|
}
|
|
|
|
if (rows[1].Y == 0){
|
|
return false;
|
|
} else {
|
|
rows[2] -= rows[1] * (rows[2].Y / rows[1].Y);
|
|
}
|
|
|
|
//dumpLinearSystem(rows);
|
|
|
|
rows[2] /= rows[2].X;
|
|
c.X = rows[2].W;
|
|
|
|
rows[1].W -= rows[1].X * c.X;
|
|
rows[1].X = 0;
|
|
rows[1] /= rows[1].Y;
|
|
c.Y = rows[1].W;
|
|
|
|
rows[0].W -= rows[0].X * c.X;
|
|
rows[0].X = 0;
|
|
rows[0].W -= rows[0].Y * c.Y;
|
|
rows[0].Y = 0;
|
|
rows[0] /= rows[0].Z;
|
|
c.Z = rows[0].W;
|
|
|
|
//dumpLinearSystem(rows);
|
|
//Console.WriteLine("Solved to {0}",c);
|
|
return true;
|
|
}
|
|
|
|
static void dumpLinearSystem(Vector4[] rows){
|
|
Console.WriteLine("Linear Equation System Dump:");
|
|
foreach (Vector4 v4 in rows){
|
|
Console.WriteLine(" {0}",v4);
|
|
}
|
|
}
|
|
}
|
|
}
|