Add non-scalar constraints support in StateFunction::addToProblem

Close #6