int Perseus::getAlignments(int curSequenceIndex, vector<seqData> sequences, vector<pwAlign>& alignments, vector<vector<int> >& leftDiffs, vector<vector<int> >& leftMaps, vector<vector<int> >& rightDiffs, vector<vector<int> >& rightMaps, int& bestRefSeq, int& bestRefDiff, vector<bool>& restricted){
try {
int numSeqs = sequences.size();
//int bestSequenceMismatch = PERSEUSMAXINT;
string curSequence = sequences[curSequenceIndex].sequence;
int curFrequency = sequences[curSequenceIndex].frequency;
bestRefSeq = -1;
int bestIndex = -1;
int bestDiffs = PERSEUSMAXINT;
int comparisons = 0;
pwModel model(0, -1, -1.5);
for(int i=0;i<numSeqs;i++){
if (m->getControl_pressed()) { return 0; }
if(i != curSequenceIndex && restricted[i] != 1 && sequences[i].frequency >= 2 * curFrequency){
string refSequence = sequences[i].sequence;
leftDiffs[i].assign(curSequence.length(), 0);
leftMaps[i].assign(curSequence.length(), 0);
rightDiffs[i].assign(curSequence.length(), 0);
rightMaps[i].assign(curSequence.length(), 0);
basicPairwiseAlignSeqs(curSequence, refSequence, alignments[i].query, alignments[i].reference, model);
getDiffs(alignments[i].query, alignments[i].reference, leftDiffs[i], leftMaps[i], rightDiffs[i], rightMaps[i]);
int diffs = rightDiffs[i][curSequence.length()-1];
if(diffs < bestDiffs){
bestDiffs = diffs;
bestIndex = i;
}
comparisons++;
restricted[i] = 0;
}
else{
restricted[i] = 1;
}
}
bestRefSeq = bestIndex;
bestRefDiff = bestDiffs;
return comparisons;
}
catch(exception& e) {
m->errorOut(e, "Perseus", "getAlignments");
exit(1);
}
}
void Node::jointStateCB (sm::JointState::ConstPtr m)
{
// The first time round, figure out which joints to ignore
if (joint_ignored_.size()==0)
{
updateIgnoredJoints(*m);
writeJointNames(*m);
}
// Only save every so often
if (ros::Time::now() <= last_processed_ + processing_interval_)
return;
RobotState rs(m, getBasePose(m->header.stamp));
Diffs diffs = getDiffs(last_saved_state_, rs);
saveToDB(diffs);
last_saved_state_ = rs;
}
int Flock::m_findNearPlObjs(PL_OBJS_VEC& a_vNearPlObjs)
{
a_vNearPlObjs.clear();
cpt3f objPos = v_pObj->m_getPos();
INT_COORDS objCell = getCellForPos(objPos);
//assert(objCell != INVALID_COORDS);
if (objCell == INVALID_COORDS)
{
return 0;
}
vec3f vec = v_pObj->m_getVel();
if (ZERO_VEC == vec || INVALID_VEC == vec)
{
if (INVALID_POS == v_trgtPt)
vec = cvec3f(1.f, 0.f, 0.f);
else
vec = (v_trgtPt - objPos);
}
cvec3f dir = vec.normalize();
cfloat cellSizeX = getCellSizeX();
cfloat cellSizeY = getCellSizeY();
float fovRAD, fovANG_D;
v_pObj->m_getFOV(fovRAD, fovANG_D);
assert(fovRAD > 0.f && fovANG_D > 0.f);
const uint numDiffs = max((uint)(fovRAD / cellSizeX), (uint)1);
INT_COORDS_VEC nearCells;
getDiffs(objCell, numDiffs, nearCells);
vector<PlaceableObj*> vNearPlObjs;
int i;
for (i = 0; i < nearCells.size(); ++i)
{
INT_COORDS cell = nearCells[i];
cpt3f ccPos = getCellCenterCoords(cell); // 'c'ell 'c'enter 'pos'
cvec3f dir2Pos = (ccPos - objPos).normalize();
float angleR, sign;
dir2Pos.getAngle(dir, angleR, sign);
if (angleR < DEG2RAD(fovANG_D))
{
GridElement* pGE = getGridElement(cell);
if (pGE->m_isCovered())
{
PlaceableObj* pPlObj = pGE->m_getCoveringPlObj();
bool fnd = ((v_pObj != pPlObj) && (vNearPlObjs.end() == find(vNearPlObjs.begin(), vNearPlObjs.end(), pPlObj)));
if (fnd)
{
vNearPlObjs.push_back(pPlObj);
}
}
}
} /* for */
a_vNearPlObjs = vNearPlObjs;
return a_vNearPlObjs.size();
}