void BallTree<_Scalar>::split(const IndexArray& indices, const AxisAlignedBoxType& aabbLeft, const AxisAlignedBoxType& aabbRight, IndexArray& iLeft, IndexArray& iRight) { for (std::vector<int>::const_iterator it=indices.begin(), end=indices.end() ; it!=end ; ++it) { unsigned int i = *it; if (vcg::PointFilledBoxDistance(mPoints[i], aabbLeft) < mRadii[i]*mRadiusScale) iLeft.push_back(i); if (vcg::PointFilledBoxDistance(mPoints[i], aabbRight) < mRadii[i]*mRadiusScale) iRight.push_back(i); } }
IndexArray Dijkstra::shortestPathTo(Index node) const { IndexArray way; Index parentNode = -1, endNode = node; while (parentNode != root_) { parentNode = pathMatrix_[endNode].start; way.push_back(endNode); endNode = pathMatrix_[endNode].start; } way.push_back(root_); IndexArray rway(way.size()); for (Index i = 0; i < way.size(); i ++) rway[i] = way[way.size() - i - 1]; return rway; }
IndexArray HuboDescription::getJointIndices(StringArray joint_names) const { IndexArray result; for(size_t i=0; i < joint_names.size(); ++i) { result.push_back(getJointIndex(joint_names[i])); } return result; }
bool HuboPlus::comIK( KState& state, const vec3& dcom, const Transform3 manipXforms[NUM_MANIPULATORS], const IKMode mode[NUM_MANIPULATORS], const bool globalIK[NUM_MANIPULATORS], Transform3Array& work, real ascl, real fscl, bool* ikvalid ) const { bool ok = false; MatX gT, gpT, gxT, fxT, fpT, lambda, gxxpT(6, 3), deltap; MatX gfT, deltaf; const real alpha = 0.5; IndexArray pdofs; for (size_t i=DOF_POS_X; i<=DOF_ROT_Z; ++i) { pdofs.push_back(i); } IndexArray fdofs; for (int i=0; i<4; ++i) { if (fscl && mode[i] == IK_MODE_FREE) { const IndexArray& jidx = kbody.manipulators[i].jointIndices; for (size_t j=0; j<jidx.size(); ++j) { fdofs.push_back(jidx[j]); } } } for (size_t iter=0; iter<DEFAULT_COM_ITER; ++iter) { // try doing IK ok = stanceIK( state, manipXforms, mode, globalIK, work, ikvalid ); // compute the COM pos kbody.transforms(state.jvalues, work); vec3 com = state.xform() * kbody.com(work); // get the error vec3 comerr = dcom - com; if (comerr.norm() < DEFAULT_COM_PTOL) { debug << "breaking after " << iter << " iterations\n"; break; } else { ok = false; } if (ascl == 0) { state.body_pos += alpha * comerr; } else { // get jacobians ftw kbody.comJacobian(work, pdofs, gpT); gpT.block(3, 0, 3, 3) *= ascl; debug << "gpT=" << gpT << "\n\n"; if (!fdofs.empty()) { kbody.comJacobian(work, fdofs, gfT); debug << "gfT=" << gfT << "\n\n"; } gxxpT.setZero(); for (int i=0; i<4; ++i) { if (mode[i] == IK_MODE_WORLD || mode[i] == IK_MODE_SUPPORT) { const std::string& name = kbody.manipulators[i].name; kbody.comJacobian(work, kbody.manipulators[i].jointIndices, gxT); kbody.manipulatorJacobian(work, i, pdofs, fpT); kbody.manipulatorJacobian(work, i, fxT); lambda = fxT.colPivHouseholderQr().solve(gxT); fpT.block(3, 0, 3, 6) *= ascl; debug << "gxT[" << name << "]=\n" << gxT << "\n\n"; debug << "fpT[" << name << "]=\n" << fpT << "\n\n"; debug << "fxT[" << name << "]=\n" << fxT << "\n\n"; debug << "lambda[" << name << "]=\n" << lambda << "\n\n"; gxxpT += fpT * lambda; } } gT = gpT - gxxpT; Eigen::Vector3d cerr(comerr[0], comerr[1], comerr[2]); deltap = alpha * gT * cerr; debug << "gxxpT = \n" << gxxpT << "\n\n"; debug << "gT = \n" << gT << "\n\n"; debug << "deltap = \n" << deltap.transpose() << "\n\n"; vec3 dp(deltap(0), deltap(1), deltap(2)); vec3 dq(deltap(3), deltap(4), deltap(5)); state.body_pos += dp; state.body_rot = quat::fromOmega(-dq) * state.body_rot; } if (!fdofs.empty()) { Eigen::Vector3d cerr(comerr[0], comerr[1], comerr[2]); deltaf = fscl * gfT * cerr; debug << "deltaf = \n" << deltaf.transpose() << "\n\n"; for (size_t i=0; i<fdofs.size(); ++i) { state.jvalues[fdofs[i]] += deltaf(i); } } } return ok; }
//---------------------------------------------------------------------------- void Binary2D::ExtractBoundary (int x0, int y0, ImageInt2D& image, IndexArray& boundary) { // Incremental 2D offsets for 8-connected neighborhood of (x,y). const int dx[8] = { -1, 0, +1, +1, +1, 0, -1, -1 }; const int dy[8] = { -1, -1, -1, 0, +1, +1, +1, 0 }; // Insert the initial boundary point. boundary.push_back(image.GetIndex(x0, y0)); // Compute the direction from background (0) to boundary pixel (1). int cx = x0, cy = y0; int nx = 0, ny = 0, dir; for (dir = 0; dir < 8; ++dir) { nx = cx + dx[dir]; ny = cy + dy[dir]; if (image(nx, ny) == 0) { dir = (dir + 1) % 8; break; } } // Traverse boundary in clockwise order. Mark visited pixels with 1. image(cx, cy) = 1; bool notDone = true; while (notDone) { int i, nbr; for (i = 0, nbr = dir; i < 8; ++i, nbr = (nbr + 1) % 8) { nx = cx + dx[nbr]; ny = cy + dy[nbr]; if (image(nx, ny)) { // The next boundary pixel was found. break; } } if (i == 8) { // (cx,cy) is an isolated pixel. notDone = false; continue; } if (nx == x0 && ny == y0) { // The boundary traversal is completed. notDone = false; continue; } // (nx,ny) is the next boundary point, so add the pixel to the list. boundary.push_back(image.GetIndex(nx, ny)); // Mark visited pixels with 1. image(nx, ny) = 1; // Start the search for the next boundary point. cx = nx; cy = ny; dir = (i + 5 + dir) % 8; } }