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;
}
}
