public int CalculateMassMatrix(bool fixedBase, float[] q, bool update_kinematics, bool initialize_matrix, bool set_lower_triangular_matrix, float[] mass_matrix) { int baseDofs; if (fixedBase) { baseDofs = 0; } else { baseDofs = 6; } int numDof = q.Length - baseDofs; //if (mass_matrix.Length != q.Length * q.Length) return -1; return(UnsafeNativeMethodsInverseDynamics.MultiBodyTree_calculateMassMatrix(Native, numDof, baseDofs, q, update_kinematics, initialize_matrix, set_lower_triangular_matrix, mass_matrix)); }