It is not yet the detailed explanation I promised, but after 3 month vacation I sat down and looked to this problem again:
- each cuboid bottom face center is the next cuboid top face center and its rotational point. The function that calculates that works well and is tested;
- Rotations of each cube is represented by axis-angle representation. The function that enables rotation, takes axis-angle representation, converts that to quaternions, do the math, gives the new rotation. Works well for the single cuboid in any way, or for the simple geometries. The function is given below;
- and what I noticed FINALLY, that that simple idea of just multiply the old rotation with new rotation works perfectly well if all the axis-angle vector values are positive! It does not work only when it is negative. So it means the unit vector is just opposite and everything must be recalculated, or smth.
So for example, I can rotate around X axis 0.7 rad with {1.0, 0.0, 0.0, 0.7} this structure and it works well:
{{0.79948, 0.504813, 0.325568, 0.0888429},
{ 0.941624, 0.0857279, 0.325568, 6.06663},
{0.940697, 0.0953601, 0.325568, 0.454249}}
But this wont rotate and will turn simply to smth else:
{{-0.79948, 0.504813, 0.325568, 0.0888429},
{ -0.941624, 0.0857279, 0.325568, 6.06663},
{-0.940697, -0.0953601, 0.325568, 0.454249}}
One guy who is good in math (i think so) tried to derive the correct unit vector for negative values and failed (he said it works in the MOST rotation, when I need it to work in ALL rotations). So I am very looking to any kind of solution. Maybe my rotation function that converts axis-angle is not good ? But it works well with negative values for a single cuboid. Really looking for some insights how to solve this. Thank you very much for your time. Main function which does the rotation taking two axis-angle representation is below:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55
|
void
ExponentialMap::to_quaternion (float& x,
float& y,
float& z,
float& w) const
{
float s = std::sin(_theta / 2);
x = _coord_x * s;
y = _coord_y * s;
z = _coord_z * s;
w = std::cos(_theta / 2);
}
void
ExponentialMap::from_quaternion (float& xcoord,
float& ycoord,
float& zcoord,
float& angle,
float x,
float y,
float z,
float w) const
{
const auto m_pi = getPiValue();
angle = acosf(w);
float sinz = sinf(angle);
if (fabsf(sinz) > 1e-4f) {
sinz = 1.0f / sinz;
xcoord = x * sinz;
ycoord = y * sinz;
zcoord = z * sinz;
angle *= 2.0f;
if (angle > m_pi)
angle = 2 * m_pi - angle;
} else {
angle = 0.0f;
xcoord = 1.0f;
ycoord = 0.0f;
zcoord = 0.0f;
}
}
ExponentialMap&
ExponentialMap::operator+=(const ExponentialMap& other)
{
float x0, y0, z0, w0;
to_quaternion(x0, y0, z0, w0);
float x1, y1, z1, w1;
other.to_quaternion(x1, y1, z1, w1);
auto w2 = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1;
auto x2 = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1;
auto y2 = w0 * y1 + y0 * w1 + z0 * x1 - x0 * z1;
auto z2 = w0 * z1 + z0 * w1 + x0 * y1 - y0 * x1;
from_quaternion(_coord_x, _coord_y, _coord_z, _theta, x2, y2, z2, w2);
return *this;
}
|