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));
}