// used for finding the location of a part of the body in real coordinates
// to find the location of a real point in body coordinates, set inverse to true
D3DXVECTOR3 CBody::RotateVector(const D3DXVECTOR3& vec, bool inverse) const
{
D3DXVECTOR3 returnVec;
D3DXQUATERNION qPoint(vec.x, vec.y, vec.z, 0.0f);
D3DXQUATERNION orientationConjugate;
D3DXQuaternionConjugate(&orientationConjugate, &m_orientation);
D3DXQUATERNION qFinalPoint;
if (inverse)
{
qFinalPoint = orientationConjugate * qPoint * m_orientation;
}
else
{
qFinalPoint = m_orientation * qPoint * orientationConjugate;
}
// debugf("qFinalPoint x: %f\ty:%f\tz:%f\tw:%f\n", qFinalPoint.x, qFinalPoint.y, qFinalPoint.z, qFinalPoint.w);
assert(qFinalPoint.w < 0.0001 && qFinalPoint.w > -0.0001);
returnVec.x = qFinalPoint.x;
returnVec.y = qFinalPoint.y;
returnVec.z = qFinalPoint.z;
return returnVec;
}
int FLANNAdapt::annkFRSearch(ANNpoint queryPt, float trd_dist, int nPts, ANNidxArray nnIdx, ANNdistArray dists, float epsilon){
//
int dim = m_dataPts.size().width;
std::vector<float> qPoint(dim,0);
std::vector<int> nnIdxArray(nPts);
std::vector<float> nnDistArray(nPts);
//make data copy
for(int i = 0; i < dim; qPoint[i]=queryPt[i++]);
int nnCnt = m_indexerPtr->radiusSearch(qPoint,nnIdxArray,nnDistArray,trd_dist,m_searchParam);
for(int i = 0; i < nnCnt;i++){
nnIdx[i] = nnIdxArray[i];
dists[i] = nnDistArray[i];
}
return nnCnt;
}
