69 lines
1.5 KiB
C#
69 lines
1.5 KiB
C#
using System;
|
|
using OpenTK;
|
|
|
|
namespace org.niclasundharald.engine
|
|
{
|
|
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("Solver Unsolved:");
|
|
Console.WriteLine("X: {0}",rows[0]);
|
|
Console.WriteLine("Y: {0}",rows[1]);
|
|
Console.WriteLine("Z: {0}",rows[2]);
|
|
*/
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
rows[2] /= rows[2].X;
|
|
|
|
c.X = rows[2].W;
|
|
c.Y = (rows[1].W - (rows[1].X * c.X)) / rows[1].Y;
|
|
c.Z = (rows[0].W - (rows[0].X * c.X) - (rows[0].Y * c.Y)) / rows[0].Z;
|
|
/*
|
|
Console.WriteLine("Solver Solved:");
|
|
Console.WriteLine("X: {0}",rows[0]);
|
|
Console.WriteLine("Y: {0}",rows[1]);
|
|
Console.WriteLine("Z: {0}",rows[2]);
|
|
Console.WriteLine("P: {0}",c);
|
|
*/
|
|
return true;
|
|
}
|
|
}
|
|
}
|