static void TestQuatproduct()
{
Vector <double> q_1 = v_builder.DenseOfArray(new[] { 0.63867877, 0.52251797, 0.56156573, 0.06089615 });
Vector <double> q_2 = v_builder.DenseOfArray(new[] { 0.35764716, 0.61051424, 0.11540801, 0.69716703 });
Matrix <double> R_t = GeneralRoboticsToolbox.Q2R(q_1).Multiply(GeneralRoboticsToolbox.Q2R(q_2));
Vector <double> q_t = GeneralRoboticsToolbox.R2Q(R_t);
Vector <double> q = GeneralRoboticsToolbox.Quatproduct(q_1).Multiply(q_2);
if (!AlmostEqualsVector(q, q_t, 1e-6))
{
Console.WriteLine("Quatproduct failed");
}
else
{
Console.WriteLine("Quatproduct succeeded");
}
}
static void TestR2Q()
{
Matrix <double> rot = m_builder.DenseOfRowArrays(
new[] { -0.5057639, -0.1340537, 0.8521928 },
new[] { 0.6456962, -0.7139224, 0.2709081 },
new[] { 0.5720833, 0.6872731, 0.4476342 });
Vector <double> q_t = v_builder.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 });
Vector <double> q = GeneralRoboticsToolbox.R2Q(rot);
if (!AlmostEqualsVector(q, q_t, 1e-6))
{
Console.WriteLine("R2Q failed");
}
else
{
Console.WriteLine("R2Q succeeded");
}
}