Unverified Commit 98335a94 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #779 from jmirabel/devel

Fix first order normalization of unit complex.
parents 2fbc086f 1111ec33
......@@ -156,8 +156,10 @@ namespace pinocchio
out << cosOmega * ca - sinOmega * sa,
sinOmega * ca + cosOmega * sa;
const Scalar norm2 = q.squaredNorm();
out *= (3 - norm2) / 2; // First order approximation of the normalization of the quaternion
// First order approximation of the normalization of the unit complex
// See quaternion::firstOrderNormalize for equations.
const Scalar norm2 = out.squaredNorm();
out *= (3 - norm2) / 2;
}
template <class Config_t, class Jacobian_t>
......
Markdown is supported
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