Template code for adding weighted average of joints for C++