static void TestRot2Q()
{
Tuple <Vector <double>, double> rot = GeneralRoboticsToolbox.R2rot(m_builder.DenseOfRowArrays(
new[] { -0.5057639, -0.1340537, 0.8521928 },
new[] { 0.6456962, -0.7139224, 0.2709081 },
new[] { 0.5720833, 0.6872731, 0.4476342 }));
double[] k = new double[3];
for (int i = 0; i < rot.Item1.Count; i++)
{
k[i] = (double)rot.Item1[i];
}
float theta = (float)rot.Item2;
Vector <double> q_t = v_builder.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 });
Vector <double> q = GeneralRoboticsToolbox.Rot2Q(k, theta);
if (!AlmostEqualsVector(q, q_t, 1e-6))
{
Console.WriteLine("Rot2Q failed");
}
else
{
Console.WriteLine("Rot2Q succeeded");
}
}
static void TestR2Rot()
{
void _R2rot_test(Vector <double> k, double theta1)
{
Matrix <double> R = GeneralRoboticsToolbox.Rot(k, theta1);
Tuple <Vector <double>, double> r2_vals = GeneralRoboticsToolbox.R2rot(R);
Vector <double> _k2 = r2_vals.Item1;
double theta2 = r2_vals.Item2;
if (Math.Abs(theta1 - theta2) > (theta1 + theta2))
{
_k2 = -_k2;
theta2 = -theta2;
}
if (!AlmostEquals(theta1, theta2, 1e-6))
{
Console.WriteLine("R2Rot failed with theta = " + theta1);
}
if (Math.Abs(theta1) < 1e-9)
{
Console.WriteLine("R2Rot succeeded");
return;
}
if ((Math.Abs(theta1) - Math.PI) < 1e-9)
{
if ((k + _k2).L2Norm() < 1e-6)
{
if (!AlmostEqualsVector(k, -_k2, 1e-6))
{
Console.WriteLine("test1 failed");
//Console.WriteLine((-_k2).ToString());
Console.WriteLine("R2Rot failed with -_k2 = " + (-_k2).ToString());
}
else
{
Console.WriteLine("R2Rot succeeded");
}
return;
}
if (!AlmostEqualsVector(k, _k2, 1e-6))
{
Console.WriteLine("test2 failed");
//Console.WriteLine(_k2.ToString());
Console.WriteLine("R2Rot failed with _k2 = " + _k2.ToString());
}
else
{
Console.WriteLine("R2Rot succeeded");
}
return;
}
if (!AlmostEqualsVector(k, _k2, 1e-6))
{
Console.WriteLine("test3 failed");
//Console.WriteLine(_k2.ToString());
Console.WriteLine("R2Rot failed with _k2 = " + _k2.ToString());
return;
}
Console.WriteLine("R2Rot succeeded");
}
_R2rot_test(v_builder.DenseOfArray(new[] { 1.0, 0, 0 }), Math.PI / 2.0);
_R2rot_test(v_builder.DenseOfArray(new[] { 0, 1.0, 0 }), Math.PI / 2.0);
_R2rot_test(v_builder.DenseOfArray(new[] { 0, 0, 1.0 }), Math.PI / 2.0);
_R2rot_test(v_builder.DenseOfArray(new[] { 0.4490221, 0.30207945, 0.84090853 }), 2.65949884);
//Singularities
Vector <double> k1 = v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 }) / v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 }).L2Norm();
_R2rot_test(k1, 1e-10);
Vector <double> k2 = v_builder.DenseOfArray(new[] { 2.0, -1.0, 3.0 }) / v_builder.DenseOfArray(new[] { 2.0, -1.0, 3.0 }).L2Norm();
_R2rot_test(k2, Math.PI + 1e-10);
Vector <double> k3 = v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }) / v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }).L2Norm();
_R2rot_test(k3, Math.PI + 1e-10);
Vector <double> k4 = v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }) / v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }).L2Norm();
_R2rot_test(k4, Math.PI + 1e-10);
Vector <double> k5 = v_builder.DenseOfArray(new[] { 0, -1.0, -3.0 }) / v_builder.DenseOfArray(new[] { 0, -1.0, -3.0 }).L2Norm();
_R2rot_test(k5, Math.PI + 1e-10);
Vector <double> k6 = v_builder.DenseOfArray(new[] { 0, 0, 1.0 });
_R2rot_test(k6, Math.PI + 1e-10);
}