Commit aecfc6bb by jcarpent

### [Spatial] Avoid division - use multiplacation instead

parent 9d7a3d7f
 ... ... @@ -318,19 +318,21 @@ namespace se3 */ const double & mab = m+Yb.m; const double mab_inv = 1./mab; const Vector3 & AB = (c-Yb.c).eval(); return InertiaTpl( mab, (m*c+Yb.m*Yb.c)/mab, I+Yb.I - (m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB)); (m*c+Yb.m*Yb.c)*mab_inv, I+Yb.I - (m*Yb.m*mab_inv)* typename Symmetric3::SkewSquare(AB)); } InertiaTpl& __pequ__(const InertiaTpl &Yb) { const InertiaTpl& Ya = *this; const double & mab = Ya.m+Yb.m; const double mab_inv = 1./mab; const Vector3 & AB = (Ya.c-Yb.c).eval(); c *= m; c += Yb.m*Yb.c; c /= mab; I += Yb.I; I -= (Ya.m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB); c *= (m*mab_inv); c += (Yb.m*mab_inv)*Yb.c; //c *= mab_inv; I += Yb.I; I -= (Ya.m*Yb.m*mab_inv)* typename Symmetric3::SkewSquare(AB); m = mab; return *this; } ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!