diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index 834daf0845..3f0d969402 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -80,23 +80,29 @@ public: Dcm(const Quaternion &q) { Dcm &dcm = *this; - Type a = q(0); - Type b = q(1); - Type c = q(2); - Type d = q(3); - Type aSq = a * a; - Type bSq = b * b; - Type cSq = c * c; - Type dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2 * (b * c - a * d); - dcm(0, 2) = 2 * (a * c + b * d); - dcm(1, 0) = 2 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2 * (c * d - a * b); - dcm(2, 0) = 2 * (b * d - a * c); - dcm(2, 1) = 2 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; + const Type a = q(0); + const Type b = q(1); + const Type c = q(2); + const Type d = q(3); + const Type aa = a * a; + const Type ab = a * b; + const Type ac = a * c; + const Type ad = a * d; + const Type bb = b * b; + const Type bc = b * c; + const Type bd = b * d; + const Type cc = c * c; + const Type cd = c * d; + const Type dd = d * d; + dcm(0, 0) = aa + bb - cc - dd; + dcm(0, 1) = 2 * (bc - ad); + dcm(0, 2) = 2 * (ac + bd); + dcm(1, 0) = 2 * (bc + ad); + dcm(1, 1) = aa - bb + cc - dd; + dcm(1, 2) = 2 * (cd - ab); + dcm(2, 0) = 2 * (bd - ac); + dcm(2, 1) = 2 * (ab + cd); + dcm(2, 2) = aa - bb - cc + dd; } /**