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 ...@@ -120,16 +120,16 @@ namespace pinocchio
} }
case 1: 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(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)); res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
break; break;
} }
case 2: 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(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(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
res.rotation().col(2) = m.rotation().col(2);
break; break;
} }
default: 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