@@ -234,22 +234,35 @@ namespace roboptim
234234
235235 // Copy jacobian values from internal sparse matrices.
236236 int idx = 0 ;
237- for (int k = 0 ;
238- k < ((StorageOrder == Eigen::ColMajor)?
239- solver_.problem ().function ().inputSize ()
240- : solver_.problem ().function ().outputSize ());
241- ++k)
242- for (constraintJacobians_t::const_iterator
243- g = constraintJacobians_.begin ();
244- g != constraintJacobians_.end (); ++g)
245- {
246- for (function_t ::jacobian_t ::InnerIterator it (*g, k);
247- it; ++it)
248- {
249- assert (idx < nele_jac);
250- values[idx++] = it.value ();
251- }
252- }
237+
238+ if (StorageOrder == Eigen::ColMajor)
239+ {
240+ for (int k = 0 ; k < solver_.problem ().function ().inputSize (); ++k)
241+ for (constraintJacobians_t::const_iterator
242+ g = constraintJacobians_.begin ();
243+ g != constraintJacobians_.end (); ++g)
244+ {
245+ for (function_t ::jacobian_t ::InnerIterator it (*g, k);
246+ it; ++it)
247+ {
248+ assert (idx < nele_jac);
249+ values[idx++] = it.value ();
250+ }
251+ }
252+ }
253+ else
254+ {
255+ for (constraintJacobians_t::const_iterator
256+ g = constraintJacobians_.begin ();
257+ g != constraintJacobians_.end (); ++g)
258+ for (int k = 0 ; k < g->outerSize (); ++k)
259+ for (function_t ::jacobian_t ::InnerIterator it (*g, k);
260+ it; ++it)
261+ {
262+ assert (idx < nele_jac);
263+ values[idx++] = it.value ();
264+ }
265+ }
253266
254267 return true ;
255268 }
0 commit comments