Реализация QEF возвращает неожиданные результаты по значению Z. ⇐ C++

[Anonymous]

У меня есть такая реализация QEF

Код: Выделить всё

float atA[3][3] = { 0 };
FVector atb = FVector::ZeroVector;

for (const FEdgeIntersection& e : IntersectionList)
{
const FVector& n = e.Normal;
float b_i = FVector::DotProduct(n, e.Position);

atA[0][0] += n.X * n.X;
atA[0][1] += n.X * n.Y;
atA[0][2] += n.X * n.Z;
atA[1][0] += n.Y * n.X;
atA[1][1] += n.Y * n.Y;
atA[1][2] += n.Y * n.Z;
atA[2][0] += n.Z * n.X;
atA[2][1] += n.Z * n.Y;
atA[2][2] += n.Z * n.Z;

atb.X += n.X * b_i;
atb.Y += n.Y * b_i;
atb.Z += n.Z * b_i;
}

FMatrix M(
FPlane(atA[0][0], atA[0][1], atA[0][2], 0),
FPlane(atA[1][0], atA[1][1], atA[1][2], 0),
FPlane(atA[2][0], atA[2][1], atA[2][2], 0),
FPlane(0, 0, 0, 1)
);
FVector Result = M.InverseFast().TransformVector(atb);

Когда у меня есть IntersectionList с этими значениями

Код: Выделить всё

{Position={X=-960.00006103515625 Y=766.10652868112652 Z=-24.615383148193359} Normal={X=0.97101487888310145 Y=0.0000000000000000 Z=0.23901904733229035} }

{Position={X=-1006.2012716118422 Y=812.30773925781250 Z=-24.615383148193359} Normal={X=0.69663297076236907 Y=0.69663297076236907 Z=0.17147888527043917} }

{Position={X=-960.00006103515625 Y=812.30773925781250 Z=-65.753435217289166} Normal={X=0.0000000000000000 Y=0.97101488224128463 Z=0.23901903368967076} }

Я получаю следующий результат:

Код: Выделить всё

{X=-996.07524496255644 Y=766.10633524904335 Z=121.94025333520403}

Теперь, глядя на это, Z слишком далеко от того места, где должно быть от -24/-65 до 121!!!
Я ожидаю, что оно будет в пределах близкого диапазона от введенного мной значения.
Есть идеи. Что не так с моими расчетами!!
Заранее спасибо

Подробнее здесь: https://stackoverflow.com/questions/798 ... he-z-value