Computing Vector от Quaternion работает, вычисления Quaternion от Vector — нет

Поэтому я использую кватернион, чтобы создать сегмент из двух точек в трехмерном пространстве, и пытаюсь позже вычислить аналогичный кватернион (один, представляющий один и тот же вектор в пространстве; я знаю, что вращение сегмента вокруг себя не определено). Я создаю сегмент как таковой:

sf::Vector3<float> Start(0, 0, 0);
sf::Vector3<float> End = Start;

//Create a vector from the start to the end
sf::Vector3<float> Translation = Orientation.MultVect(sf::Vector3<float>(0, 1, 0));

//Add that vector onto the start position
End.x += Translation.x * Length;
End.y += Translation.y * Length;
End.z += Translation.z * Length;

Где Orientation :: MultVect () выглядит так:

sf::Vector3<float> Quaternion::MultVect(sf::Vector3<float> Vector)
{
//From http://www.idevgames.com/articles/quaternions
Quaternion VectorQuat = Quaternion();
VectorQuat.x = Vector.x;
VectorQuat.y = Vector.y;
VectorQuat.z = Vector.z;
VectorQuat.w = 0.0;

Quaternion Inverse = (*this);
Inverse.Invert();

Quaternion Result = Inverse * VectorQuat * (*this);

sf::Vector3<float> ResultVector;
ResultVector.x = Result.x;
ResultVector.y = Result.y;
ResultVector.z = Result.z;

return ResultVector;
}

Теперь эта функция, кажется, работает довольно хорошо в других контекстах, поэтому я не думаю, что проблема здесь, но вы никогда не знаете. Я должен также упомянуть, что точка заканчивается там, где я ожидаю, учитывая кватернион, который я кормлю, если (который я строю из углов Эйлера, иногда с умножением на другие кватернионы).

Мне кажется, что проблема заключается в пересчете кватерниона из Start а также End, Для этого я использую эту функцию, которая хорошо работает, когда объекты на сцене ориентированы на другие объекты (если только рассматриваемые объекты не расположены точно на одной оси Y, в этом случае я получаю кватернионы со значениями NaN). Вот как я это делаю:

Quaternion Quaternion::FromLookVector(sf::Vector3<float> FromPoint, sf::Vector3<float> ToPoint)
{
///Based on this post:
///http://stackoverflow.com/questions/13014973/quaternion-rotate-to
//Get the normalized vector from origin position to ToPoint
sf::Vector3<double> VectorTo(ToPoint.x - FromPoint.x,
ToPoint.y - FromPoint.y,
ToPoint.z - FromPoint.z);
//Get the length of VectorTo
double VectorLength = sqrt(VectorTo.x*VectorTo.x +
VectorTo.y*VectorTo.y +
VectorTo.z*VectorTo.z);
//Normalize VectorTo
VectorTo.x /= -VectorLength;
VectorTo.y /= -VectorLength;
VectorTo.z /= -VectorLength;

//Define a unit up vector
sf::Vector3<double> VectorUp(0, -1, 0);

//The X axis is the cross product of both
//Get the cross product as the axis of rotation
sf::Vector3<double> AxisX(VectorTo.y*VectorUp.z - VectorTo.z*VectorUp.y,
VectorTo.z*VectorUp.x - VectorTo.x*VectorUp.z,
VectorTo.x*VectorUp.y - VectorTo.y*VectorUp.x);
//Normalize the axis
//Get the length of VectorTo
double AxisXLength = sqrt(AxisX.x*AxisX.x +
AxisX.y*AxisX.y +
AxisX.z*AxisX.z);

//Normalize VectorTo
AxisX.x /= AxisXLength;
AxisX.y /= AxisXLength;
AxisX.z /= AxisXLength;

//Get the adjusted Y vector
//Get the cross product of the other two axes
sf::Vector3<double> AxisY(VectorTo.y*AxisX.z - VectorTo.z*AxisX.y,
VectorTo.z*AxisX.x - VectorTo.x*AxisX.z,
VectorTo.x*AxisX.y - VectorTo.y*AxisX.x);
//Normalize the axis
//Get the length of VectorTo
double AxisYLength = sqrt(AxisY.x*AxisY.x +
AxisY.y*AxisY.y +
AxisY.z*AxisY.z);
//Normalize VectorTo
AxisY.x /= AxisYLength;
AxisY.y /= AxisYLength;
AxisY.z /= AxisYLength;

//A matrix representing the Thing's orientation
GLfloat RotationMatrix[16] = {(float)AxisX.x,
(float)AxisX.y,
(float)AxisX.z,
0,
(float)AxisY.x,
(float)AxisY.y,
(float)AxisY.z,
0,
(float)VectorTo.x,
(float)VectorTo.y,
(float)VectorTo.z,
0,
0,
0,
0,
1};

Quaternion LookQuat = Quaternion::FromMatrix(RotationMatrix);

//Reset the quaternion orientation
return LookQuat;
}

Поэтому, когда я вычисляю сегменты, я также проверяю их восстановленные значения, например:

sf::Vector3<float> Start(0, 0, 0);
sf::Vector3<float> End = Start;

//Create a vector from the start to the end
sf::Vector3<float> Translation = Orientation.MultVect(sf::Vector3<float>(0, 1, 0));
//Add that vector onto the start position
End.x += Translation.x * Length;
End.y += Translation.y * Length;
End.z += Translation.z * Length;

std::cout << "STATIC END     (";
std::cout << End.x << ",";
std::cout << End.y << ",";
std::cout << End.z << ")\n";

///TEST
Quaternion Reconstructed = Quaternion::FromLookVector(Start, End);
Translation = Reconstructed.MultVect(sf::Vector3<float>(0, 1, 0));
sf::Vector3<float> TestEnd = Start;
TestEnd.x += Translation.x * Length;
TestEnd.y += Translation.y * Length;
TestEnd.z += Translation.z * Length;

std::cout << "RECONSTRUCTED END (";
std::cout << TestEnd.x << ",";
std::cout << TestEnd.y << ",";
std::cout << TestEnd.z << ")\n";

И эти два не совпадают. Например, если статическая конечная точка равна (0,14.3998,0.0558498), то пересчитанная точка равна (0,8.05585, -6.39976). Оба должны быть идентичны, хотя. Неопределенная часть вращения не должна изменять положение конечной точки, только поворот (или Z-вращение, или как вы хотите это называть), что, поскольку это сегмент, не имеет значения.

Обратите внимание, что когда я в конечном итоге использую это для вещей, отличных от простых сегментов, бросок будут независимо от того, почему я использую вектор вверх, чтобы убедиться, что у объектов, которые я размещаю вдоль этих сегментов, вершины всегда будут как можно выше направлены вверх (объекты, смотрящие прямо вверх или вниз, могут иметь специальный произвольный бросок, определяемый отдельно, если это необходимо ). Другая цель — создать несколько сегментов, соединенных вместе, каждый из которых вращается относительно ориентации того, который был до него, а не вращается относительно глобального пространства.

Так что я тут делаю не так? Почему я не могу пересчитать второй кватернион, который выполняет тот же перевод, что и первый?