Verified Commit 97d4dec7 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

joint/revolute: change order of operations

parent af26ea70
......@@ -120,16 +120,16 @@ namespace pinocchio
}
case 1:
{
res.rotation().col(1) = m.rotation().col(1);
res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
res.rotation().col(1) = m.rotation().col(1);
res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
break;
}
case 2:
{
res.rotation().col(2) = m.rotation().col(2);
res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
res.rotation().col(2) = m.rotation().col(2);
break;
}
default:
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment