Minor code optimization in multibody forward dynamics

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -744,8 +744,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
 	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
 
-    btVector3 base_vel = getBaseVel();
+	const btVector3 base_vel = getBaseVel();
-    btVector3 base_omega = getBaseOmega();
+	const btVector3 base_omega = getBaseOmega();
 
     // Temporary matrices/vectors -- use scratch space from caller
     // so that we don't have to keep reallocating every frame
@@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     // hhat is NOT stored for the base (but ahat is)    
 	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
 	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
-	v_ptr += num_links * 2 + 2;
+	// v_ptr += num_links * 2 + 2;  // Disabled since v_ptr is not used in the rest of the code
 	//
     // Y_i, invD_i
     btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
@@ -819,13 +819,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     }
 	else 
 	{
-		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
+		const btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
-		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
+		const btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
 		//external forces		
 		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
 
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
+		const btScalar linDampMult = 1., angDampMult = 1.;
 		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
 								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
 
@@ -969,14 +969,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
 			- spatCoriolisAcc[i].dot(hDof)
 			;
-		}
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
-		{
-			btScalar *D_row = &D[dof * m_links[i].m_dofCount];			
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
 			}
 		}
@@ -999,8 +996,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			case btMultibodyLink::eSpherical:
 			case btMultibodyLink::ePlanar:
 			{
-				btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
-				btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+				const btMatrix3x3 invD3x3(D3x3.inverse());
 
 				//unroll the loop?
 				for(int row = 0; row < 3; ++row)
@@ -1026,7 +1023,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{				
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				//				
 				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
 			}
@@ -1037,7 +1034,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//determine (h*D^{-1}) * h^{T}
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{			
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
 		}
@@ -1058,7 +1055,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{				
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			spatForceVecTemps[0] += hDof * invD_times_Y[dof];			
 		}
@@ -1109,7 +1106,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//			
 			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
 		}
@@ -1169,12 +1166,12 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     }
 
     // transform base accelerations back to the world frame.
-    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
 	output[0] = omegadot_out[0];
 	output[1] = omegadot_out[1];
 	output[2] = omegadot_out[2];
 
-    btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
 	output[3] = vdot_out[0];
 	output[4] = vdot_out[1];
 	output[5] = vdot_out[2];