// N_VCompare // Compare components of x to scalar c and return // z such that zi=1 if |xi|>=c, else 0. static void nvcompare_SimTK(realtype c, N_Vector nvx, N_Vector nvz) { const Vector& x = N_Vector_SimTK::getVector(nvx); Vector& z = N_Vector_SimTK::updVector(nvz); const int sz = x.size(); assert(z.size() == sz); const Real* xp = x.getContiguousScalarData(); Real* zp = z.updContiguousScalarData(); for (int i=0; i<sz; ++i) zp[i] = Real(std::abs(xp[i]) >= c ? 1 : 0); }
static bool equalToTol(Real v1, Real v2, Real tol) { const Real scale = std::max(std::max(std::abs(v1), std::abs(v2)), Real(1)); return std::abs(v1-v2) < scale*tol; }