void FactorQTZ::factor( const Matrix_<ELT>& m ){ delete rep; // if user does not supply rcond set it to max(nRow,nCol)*(eps)^7/8 (similar to matlab) int mnmax = (m.nrow() > m.ncol()) ? m.nrow() : m.ncol(); rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, mnmax*NTraits<typename CNT<ELT>::Precision>::getSignificant()); }
// Same thing but for comparing matrices where one has Vec3 elements. static void compareElementwise(const Matrix_<Vec3>& JS, const Matrix& JSf) { const int m = JS.nrow(), n = JS.ncol(); SimTK_TEST(JSf.nrow()==3*m && JSf.ncol()==n); for (int b=0; b<m; ++b) { const int r = 3*b; // row start for JSf for (int i=0; i<3; ++i) for (int j=0; j<n; ++j) { SimTK_TEST_EQ(JS (b, j)[i], JSf(r+i,j)); } } }
// Compare two representations of the same matrix: one as an mXn matrix // of SpatialVecs, the other as a 6mXn matrix of scalars. Note that this // will also work if the first actual parameter is a Vector_<SpatialVec> // or RowVector_<SpatialVec> since those have implicit conversions to mX1 // or 1Xn Matrix_<SpatialVec>, resp. static void compareElementwise(const Matrix_<SpatialVec>& J, const Matrix& Jf) { const int m = J.nrow(), n = J.ncol(); SimTK_TEST(Jf.nrow()==6*m && Jf.ncol()==n); for (int b=0; b<m; ++b) { const int r = 6*b; // row start for Jf for (int i=0; i<6; ++i) for (int j=0; j<n; ++j) { SimTK_TEST_EQ(J (b, j)[i/3][i%3], Jf(r+i,j)); } } }
void FactorQTZRep<T>::inverse( Matrix_<T>& inverse ) const { Matrix_<T> iden(mn,mn); inverse.resize(mn,mn); iden = 1.0; doSolve( iden, inverse ); return; }
void FactorQTZRep<T>::solve( const Matrix_<T>& b, Matrix_<T>& x ) const { SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve", "No matrix was passed to FactorQTZ. \n" ); SimTK_APIARGCHECK2_ALWAYS(b.nrow()==nRow,"FactorQTZ","solve", "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n", b.nrow(), nRow ); x.resize(nCol, b.ncol() ); Matrix_<T> tb; tb.resize(maxmn, b.ncol() ); for(int j=0;j<b.ncol();j++) for(int i=0;i<b.nrow();i++) tb(i,j) = b(i,j); doSolve(tb, x); }
int main () { double errnorm; try { // Default precision (Real, normally double) test. Matrix a(4,4, A); Vector_<std::complex<double> > expectedValues(4); for(int i=0;i<4;i++) expectedValues[i] = expEigen[i]; Matrix_<std::complex<double> > expectedVectors(4,4); for(int i=0;i<4;i++) for(int j=0;j<4;j++) expectedVectors(i,j) = expVectors[j*4+i]; Vector_<std::complex<double> > values; // should get sized automatically to 4 by getAllEigenValuesAndVectors() Vector_<std::complex<double> > expectVec(4); Vector_<std::complex<double> > computeVec(4); Matrix_<std::complex<double> > vectors; // should get sized automatically to 4x4 by getAllEigenValuesAndVectors() Eigen es(a); // setup the eigen system es.getAllEigenValuesAndVectors( values, vectors ); // solve for the eigenvalues and eigenvectors of the system printf("\n ***** NON-SYMMETRIC: ***** \n\n" ); cout << " Real SOLUTION: " << values << " errnorm=" << absNormComplex(values,expectedValues) << endl; ASSERT(absNormComplex(values,expectedValues) < 0.001); cout << "Vectors = " << endl; for(int i=0;i<4;i++) { computeVec = vectors(i); expectVec = expectedVectors(i); errnorm = absNormComplex( computeVec, expectVec ); cout << computeVec << " errnorm=" << errnorm << endl; ASSERT( errnorm < 0.00001 ); } cout << endl << endl; Vector_<std::complex<float> > expectedValuesf(4); Matrix_<std::complex<float> > expectedVectorsf(4,4); Vector_<std::complex<float> > expectVecf(4); Vector_<std::complex<float> > computeVecf(4); Matrix_<std::complex<float> > vectorsf; // should get sized automatically to 4x4 by getAllEigenValuesAndVectors() Vector_<std::complex<float> > valuesf; // should get sized automatically to 4 by getAllEigenValuesAndVectors() Matrix_<float> af(4,4); for (int i=0; i<4; ++i) for (int j=0; j<4; ++j) af(i,j)=(float)a(i,j); for(int i=0;i<4;i++) expectedValuesf[i] = (std::complex<float>)expEigen[i]; for(int i=0;i<4;i++) for(int j=0;j<4;j++) expectedVectorsf(i,j) = (std::complex<float>)expVectors[j*4+i]; Eigen esf(af); // setup the eigen system esf.getAllEigenValuesAndVectors( valuesf, vectorsf); // solve for the eigenvalues and vectors of the system cout << " float SOLUTION: " << valuesf << " errnorm=" << absNormComplex(valuesf,expectedValuesf) << endl; ASSERT(absNormComplex(valuesf,expectedValuesf) < 0.001); cout << "Vectors = " << endl; for(int i=0;i<4;i++) { computeVecf = vectorsf(i); expectVecf = expectedVectorsf(i); errnorm = absNormComplex( computeVecf, expectVecf ); cout << computeVecf << " errnorm=" << errnorm << endl; ASSERT( errnorm < 0.0001 ); } Real Z[4] = { 0.0, 0.0, 0.0, 0.0 }; Matrix z(2,2, Z); Eigen zeigen(z); zeigen.getAllEigenValuesAndVectors( values, vectors); cout << "Matrix of all zeros" << endl << "eigenvalues: " << values << endl; cout << "Eigen vectors: " << endl; for(int i=0;i<vectors.nrow();i++ ) { cout << vectors(i) << endl; } Matrix_<double> z0; Eigen z0Eigen(z0); zeigen.getAllEigenValuesAndVectors( values, vectors); cout << "Matrix of dimension 0,0" << endl << "eigenvalues: " << values << endl; cout << "Eigen vectors: " << endl; for(int i=0;i<vectors.nrow();i++ ) { cout << vectors(i) << endl; } /* // // SYMMETRIC TESTS: // Real S[16] = { 1.0, 2.0, 3.0, 4.0, 0.0, 2.0, 3.0, 4.0, 0.0, 0.0, 3.0, 4.0, 0.0, 0.0, 0.0, 4.0 }; Real expectSymVals[4] = { -2.0531, -0.5146, -0.2943, 12.8621 }; Real expectSymVecs[16] = { -0.7003, -0.5144, 0.2767, -0.4103, -0.3592, 0.4851, -0.6634, -0.4422, 0.1569, 0.5420, 0.6504, -0.5085, 0.5965, -0.4543, -0.2457, -0.6144 }; Matrix as(4,4, S); as.setMatrixStructure( MatrixStructures::Symmetric ) ; Eigen esym(as); // setup the eigen system Vector symValues; Vector expectedSymValues(4); for(int i=0;i<4;i++) expectedSymValues[i] = expectSymVals[i]; Matrix symVectors; Vector symVector; Vector expectedSymVector; Matrix expectedSymVectors(4,4,expectSymVecs ); esym.getAllEigenValuesAndVectors( symValues, symVectors ); // solve for the eigenvalues and eigenvectors of the system printf("\n ***** SYMMETRIC: ***** \n\n" ); cout << " Real SOLUTION: All Values and Vectors" << symValues << " errnorm=" << absNorm(symValues,expectedSymValues) << endl; cout << " Eigen Vectors = " << endl; for(int i=0;i<4;i++) { symVector = symVectors(i); expectedSymVector = expectedSymVectors(i); errnorm = absNorm(symVector,expectedSymVector); cout << symVectors(i) << " errnorm=" << errnorm << endl; // cout << expectedSymVectors(i) << endl; } Eigen efewi(as); // setup the eigen system efewi.getFewEigenValuesAndVectors( symValues, symVectors, 2, 3 ); // solve for few eigenvalues and eigenvectors of the system cout << " Real SOLUTION: Values/Vectors between indices 2 and 3" << symValues << endl; for(int j=0;j<symVectors.ncol();j++) cout << symVectors(j) << endl; Eigen efewr(as); // setup the eigen system // solve for few eigenvalues/vectors between -1, 1 efewr.getFewEigenValuesAndVectors( symValues, symVectors, -1.0, 1.0 ); cout << " Real SOLUTION: Values/Vectors between -1, 1 " << symValues << endl; for(int j=0;j<symVectors.ncol();j++) cout << symVectors(j) << endl; */ return 0; } catch (std::exception& e) { std::printf("FAILED: %s\n", e.what()); return 1; } }
void FactorQTZRep<T>::factor(const Matrix_<ELT>&mat ) { SimTK_APIARGCHECK2_ALWAYS(mat.nelt() > 0,"FactorQTZ","factor", "Can't factor a matrix that has a zero dimension -- got %d X %d.", (int)mat.nrow(), (int)mat.ncol()); // allocate and initialize the matrix we pass to LAPACK // converts (negated,conjugated etc.) to LAPACK format LapackConvert::convertMatrixToLapack( qtz.data, mat ); int info; const int smallestSingularValue = 2; const int largestSingularValue = 1; typedef typename CNT<T>::TReal RealType; RealType smlnum, bignum, smin, smax; if (mat.nelt() == 0) return; // Compute optimal size for work space for dtzrzf and dgepq3. The // arguments here should match the calls below, although we'll use maxRank // rather than rank since we don't know the rank yet. T workSz; const int maxRank = std::min(nRow, nCol); LapackInterface::tzrzf<T>(maxRank, nCol, 0, nRow, 0, &workSz, -1, info); const int lwork1 = (int)NTraits<T>::real(workSz); LapackInterface::geqp3<T>(nRow, nCol, 0, nRow, 0, 0, &workSz, -1, info); const int lwork2 = (int)NTraits<T>::real(workSz); TypedWorkSpace<T> work(std::max(lwork1, lwork2)); LapackInterface::getMachinePrecision<RealType>( smlnum, bignum); // scale the input system of equations anrm = (RealType)LapackInterface::lange<T>('M', nRow, nCol, qtz.data, nRow); if (anrm > 0 && anrm < smlnum) { scaleLinSys = true; linSysScaleF = smlnum; } else if( anrm > bignum ) { scaleLinSys = true; linSysScaleF = bignum; } if (anrm == 0) { // matrix all zeros rank = 0; } else { if (scaleLinSys) { LapackInterface::lascl<T>('G', 0, 0, anrm, linSysScaleF, nRow, nCol, qtz.data, nRow, info ); } // compute QR factorization with column pivoting: A = Q * R // Q * R is returned in qtz.data LapackInterface::geqp3<T>(nRow, nCol, qtz.data, nRow, pivots.data, tauGEQP3.data, work.data, work.size, info ); // compute Rank smax = CNT<T>::abs( qtz.data[0] ); smin = smax; if( CNT<T>::abs(qtz.data[0]) == 0 ) { rank = 0; actualRCond = 0; } else { T s1,s2,c1,c2; RealType smaxpr,sminpr; // Determine rank using incremental condition estimate Array_<T> xSmall(mn), xLarge(mn); // temporaries xSmall[0] = xLarge[0] = 1; for (rank=1,smaxpr=0.0,sminpr=1.0; rank<mn && smaxpr*rcond < sminpr; ) { LapackInterface::laic1<T>(smallestSingularValue, rank, xSmall.begin(), smin, &qtz.data[rank*nRow], qtz.data[(rank*nRow)+rank], sminpr, s1, c1); LapackInterface::laic1<T>(largestSingularValue, rank, xLarge.begin(), smax, &qtz.data[rank*nRow], qtz.data[(rank*nRow)+rank], smaxpr, s2, c2); if (smaxpr*rcond < sminpr) { for(int i=0; i<rank; i++) { xSmall[i] *= s1; xLarge[i] *= s2; } xSmall[rank] = c1; xLarge[rank] = c2; smin = sminpr; smax = smaxpr; actualRCond = (double)(smin/smax); rank++; } } // R => T * Z // Matrix is rank deficient so complete the orthogonalization by // applying orthogonal transformations from the right to // factor R from an upper trapezoidal to an upper triangular matrix // T is returned in qtz.data and Z is returned in tauORMQR.data if (rank < nCol) { LapackInterface::tzrzf<T>(rank, nCol, qtz.data, nRow, tauORMQR.data, work.data, work.size, info); } } } }
void FactorQTZRep<T>::doSolve(Matrix_<T>& b, Matrix_<T>& x) const { int info; typedef typename CNT<T>::TReal RealType; RealType bnrm, smlnum, bignum; int nrhs = b.ncol(); int n = nCol; int m = nRow; typename CNT<T>::TReal rhsScaleF; // scale factor applied to right hand side bool scaleRHS = false; // true if right hand side should be scaled if (rank == 0) return; // Ask the experts for their optimal workspace sizes. The size parameters // here must match the calls below. T workSz; LapackInterface::ormqr<T>('L', 'T', nRow, b.ncol(), mn, 0, nRow, 0, 0, b.nrow(), &workSz, -1, info ); const int lwork1 = (int)NTraits<T>::real(workSz); LapackInterface::ormrz<T>('L', 'T', nCol, b.ncol(), rank, nCol-rank, 0, nRow, 0, 0, b.nrow(), &workSz, -1, info ); const int lwork2 = (int)NTraits<T>::real(workSz); TypedWorkSpace<T> work(std::max(lwork1, lwork2)); // compute norm of RHS bnrm = (RealType)LapackInterface::lange<T>('M', m, nrhs, &b(0,0), b.nrow()); LapackInterface::getMachinePrecision<RealType>(smlnum, bignum); // compute scale for RHS if (bnrm > Zero && bnrm < smlnum) { scaleRHS = true; rhsScaleF = smlnum; } else if (bnrm > bignum) { scaleRHS = true; rhsScaleF = bignum; } if (scaleRHS) { // apply scale factor to RHS LapackInterface::lascl<T>('G', 0, 0, bnrm, rhsScaleF, b.nrow(), nrhs, &b(0,0), b.nrow(), info ); } // b1 = Q'*b0 LapackInterface::ormqr<T>('L', 'T', nRow, b.ncol(), mn, qtz.data, nRow, tauGEQP3.data, &b(0,0), b.nrow(), work.data, work.size, info ); // b2 = T^-1*b1 = T^-1 * Q' * b0 LapackInterface::trsm<T>('L', 'U', 'N', 'N', rank, b.ncol(), 1.0, qtz.data, nRow, &b(0,0), b.nrow() ); // zero out elements of RHS for rank deficient systems for(int j = 0; j<nrhs; ++j) { for(int i = rank; i<n; ++i) b(i,j) = 0; } if (rank < nCol) { // b3 = Z'*b2 = Z'*T^-1*Q'*b0 LapackInterface::ormrz<T>('L', 'T', nCol, b.ncol(), rank, nCol-rank, qtz.data, nRow, tauORMQR.data, &b(0,0), b.nrow(), work.data, work.size, info ); } // adjust for pivoting Array_<T> b_pivot(n); for(int j = 0; j<nrhs; ++j) { for(int i = 0; i<n; ++i) b_pivot[pivots.data[i]-1] = b(i,j); LapackInterface::copy<T>(n, b_pivot.begin(), 1, &x(0,j), 1 ); } // compensate for scaling of linear system if (scaleLinSys) { LapackInterface::lascl<T>('g', 0, 0, anrm, linSysScaleF, nCol, x.ncol(), &x(0,0), nCol, info ); } // compensate for scaling of RHS if (scaleRHS) { LapackInterface::lascl<T>('g', 0, 0, bnrm, rhsScaleF, nCol, x.ncol(), &x(0,0), nCol, info); } }
FactorQTZRep<T>::FactorQTZRep( const Matrix_<ELT>& mat, typename CNT<T>::TReal rc) : mn( (mat.nrow() < mat.ncol()) ? mat.nrow() : mat.ncol() ), maxmn( (mat.nrow() > mat.ncol()) ? mat.nrow() : mat.ncol() ), nRow( mat.nrow() ), nCol( mat.ncol() ), scaleLinSys(false), linSysScaleF(NTraits<typename CNT<T>::Precision>::getNaN()), anrm(NTraits<typename CNT<T>::Precision>::getNaN()), rcond(rc), pivots(mat.ncol()), qtz( mat.nrow()*mat.ncol() ), tauGEQP3(mn), tauORMQR(mn) { for(int i=0; i<mat.ncol(); ++i) pivots.data[i] = 0; FactorQTZRep<T>::factor( mat ); isFactored = true; }
void testTaskJacobians() { MultibodySystem system; MyForceImpl* frcp; makeSystem(false, system, frcp); const SimbodyMatterSubsystem& matter = system.getMatterSubsystem(); State state = system.realizeTopology(); const int nq = state.getNQ(); const int nu = state.getNU(); const int nb = matter.getNumBodies(); // Attainable accuracy drops with problem size. const Real Slop = nu*SignificantReal; system.realizeModel(state); // Randomize state. state.updQ() = Test::randVector(nq); state.updU() = Test::randVector(nu); system.realize(state, Stage::Position); Matrix_<SpatialVec> J; Matrix Jmat, Jmat2; matter.calcSystemJacobian(state, J); SimTK_TEST_EQ(J.nrow(), nb); SimTK_TEST_EQ(J.ncol(), nu); matter.calcSystemJacobian(state, Jmat); SimTK_TEST_EQ(Jmat.nrow(), 6*nb); SimTK_TEST_EQ(Jmat.ncol(), nu); // Unpack J into Jmat2 and compare with Jmat. Jmat2.resize(6*nb, nu); for (int row=0; row < nb; ++row) { const int nxtr = 6*row; // row index into scalar matrix for (int col=0; col < nu; ++col) { for (int k=0; k<3; ++k) { Jmat2(nxtr+k, col) = J(row,col)[0][k]; Jmat2(nxtr+3+k, col) = J(row,col)[1][k]; } } } // These should be exactly the same. SimTK_TEST_EQ_TOL(Jmat2, Jmat, SignificantReal); Vector randU = 100.*Test::randVector(nu), resultU1, resultU2; Vector_<SpatialVec> randF(nb), resultF1, resultF2; for (int i=0; i<nb; ++i) randF[i] = 100.*Test::randSpatialVec(); matter.multiplyBySystemJacobian(state, randU, resultF1); resultF2 = J*randU; SimTK_TEST_EQ_TOL(resultF1, resultF2, Slop); matter.multiplyBySystemJacobianTranspose(state, randF, resultU1); resultU2 = ~J*randF; SimTK_TEST_EQ_TOL(resultU1, resultU2, Slop); // See if Station Jacobian can be used to duplicate the translation // rows of the System Jacobian, and if Frame Jacobian can be used to // duplicate the whole thing. Array_<MobilizedBodyIndex> allBodies(nb); for (int i=0; i<nb; ++i) allBodies[i]=MobilizedBodyIndex(i); Array_<Vec3> allOrigins(nb, Vec3(0)); Matrix_<Vec3> JS, JS2, JSbyrow; Matrix_<SpatialVec> JF, JF2, JFbyrow; matter.calcStationJacobian(state, allBodies, allOrigins, JS); matter.calcFrameJacobian(state, allBodies, allOrigins, JF); for (int i=0; i<nb; ++i) { for (int j=0; j<nu; ++j) { SimTK_TEST_EQ(JS(i,j), J(i,j)[1]); SimTK_TEST_EQ(JF(i,j), J(i,j)); } } // Now use random stations to calculate JS & JF. Array_<Vec3> randS(nb); for (int i=0; i<nb; ++i) randS[i] = 10.*Test::randVec3(); matter.calcStationJacobian(state, allBodies, randS, JS); matter.calcFrameJacobian(state, allBodies, randS, JF); // Recalculate one row at a time to test non-contiguous memory handling. // Do it backwards just to show off. JSbyrow.resize(nb, nu); JFbyrow.resize(nb, nu); for (int i=nb-1; i >= 0; --i) { matter.calcStationJacobian(state, allBodies[i], randS[i], JSbyrow[i]); matter.calcFrameJacobian(state, allBodies[i], randS[i], JFbyrow[i]); } SimTK_TEST_EQ(JS, JSbyrow); SimTK_TEST_EQ(JF, JFbyrow); // Calculate JS2=JS and JF2=JF again using multiplication by mobility-space // unit vectors. JS2.resize(nb, nu); JF2.resize(nb, nu); Vector zeroU(nu, 0.); for (int i=0; i < nu; ++i) { zeroU[i] = 1; matter.multiplyByStationJacobian(state, allBodies, randS, zeroU, JS2(i)); matter.multiplyByFrameJacobian(state, allBodies, randS, zeroU, JF2(i)); zeroU[i] = 0; } SimTK_TEST_EQ_TOL(JS2, JS, Slop); SimTK_TEST_EQ_TOL(JF2, JF, Slop); // Calculate JS2t=~JS using multiplication by force-space unit vectors. Matrix_<Row3> JS2t(nu,nb); Vector_<Vec3> zeroF(nb, Vec3(0)); // While we're at it, let's test non-contiguous vectors by filling in // this scalar version and using its non-contig rows as column temps. Matrix JS3mat(3*nb,nu); for (int b=0; b < nb; ++b) { for (int k=0; k<3; ++k) { zeroF[b][k] = 1; RowVectorView JS3matr = JS3mat[3*b+k]; matter.multiplyByStationJacobianTranspose(state, allBodies, randS, zeroF, ~JS3matr); zeroF[b][k] = 0; for (int u=0; u < nu; ++u) JS2t(u,b)[k] = JS3matr[u]; } } SimTK_TEST_EQ_TOL(JS2, ~JS2t, Slop); // we'll check JS3mat below // Calculate JF2t=~JF using multiplication by force-space unit vectors. Matrix_<SpatialRow> JF2t(nu,nb); Vector_<SpatialVec> zeroSF(nb, SpatialVec(Vec3(0))); // While we're at it, let's test non-contiguous vectors by filling in // this scalar version and using its non-contig rows as column temps. Matrix JF3mat(6*nb,nu); for (int b=0; b < nb; ++b) { for (int k=0; k<6; ++k) { zeroSF[b][k/3][k%3] = 1; RowVectorView JF3matr = JF3mat[6*b+k]; matter.multiplyByFrameJacobianTranspose(state, allBodies, randS, zeroSF, ~JF3matr); zeroSF[b][k/3][k%3] = 0; for (int u=0; u < nu; ++u) JF2t(u,b)[k/3][k%3] = JF3matr[u]; } } SimTK_TEST_EQ_TOL(JF2, ~JF2t, Slop); // we'll check JS3mat below // All three methods match. Now let's see if they are right by shifting // the System Jacobian to the new stations. for (int i=0; i<nb; ++i) { const MobilizedBody& mobod = matter.getMobilizedBody(allBodies[i]); const Rotation& R_GB = mobod.getBodyRotation(state); const Vec3 S_G = R_GB*randS[i]; for (int j=0; j<nu; ++j) { const Vec3 w = J(i,j)[0]; const Vec3 v = J(i,j)[1]; const Vec3 vJ = v + w % S_G; // Shift const Vec3 vS = JS2(i,j); const SpatialVec vF = JF2(i,j); SimTK_TEST_EQ(vS, vJ); SimTK_TEST_EQ(vF, SpatialVec(w, vJ)); } } // Now create a scalar version of JS and make sure it matches the Vec3 one. Matrix JSmat, JSmat2, JFmat, JFmat2; matter.calcStationJacobian(state, allBodies, randS, JSmat); matter.calcFrameJacobian(state, allBodies, randS, JFmat); SimTK_TEST_EQ(JSmat.nrow(), 3*nb); SimTK_TEST_EQ(JSmat.ncol(), nu); SimTK_TEST_EQ(JFmat.nrow(), 6*nb); SimTK_TEST_EQ(JFmat.ncol(), nu); SimTK_TEST_EQ_TOL(JSmat, JS3mat, Slop); // same as above? SimTK_TEST_EQ_TOL(JFmat, JF3mat, Slop); // same as above? // Unpack JS into JSmat2 and compare with JSmat. JSmat2.resize(3*nb, nu); for (int row=0; row < nb; ++row) { const int nxtr = 3*row; // row index into scalar matrix for (int col=0; col < nu; ++col) { for (int k=0; k<3; ++k) { JSmat2(nxtr+k, col) = JS(row,col)[k]; } } } // These should be exactly the same. SimTK_TEST_EQ_TOL(JSmat2, JSmat, SignificantReal); // Unpack JF into JFmat2 and compare with JFmat. JFmat2.resize(6*nb, nu); for (int row=0; row < nb; ++row) { const int nxtr = 6*row; // row index into scalar matrix for (int col=0; col < nu; ++col) { for (int k=0; k<6; ++k) { JFmat2(nxtr+k, col) = JF(row,col)[k/3][k%3]; } } } // These should be exactly the same. SimTK_TEST_EQ_TOL(JFmat2, JFmat, SignificantReal); }