int CLineStepPtrArray::SetAt(int nIndex, CLineStep* pLineStep, bool& rbOrient )
{
int nNewIndex = nIndex;
#if 1
for (int i = 0; i< nIndex; i++){
int j = (int) m_hash.GetAt(i);
CLineStep* pLineStep1 = (CLineStep*)(CPtrArray::GetAt( j));
ASSERT( NULL != pLineStep1);
double dDist2 = DBL_MAX;//
try{
dDist2 = getLength2( (pLineStep->m_pPos), (pLineStep1->m_pPos));
dDist2+= getLength2( (pLineStep->m_pDir), (pLineStep1->m_pDir));
}
catch(...){
// return -1;
throw;
}
if (dDist2 < restol2) {
nNewIndex = i;
m_hash.SetAt(nIndex, (void*)nNewIndex);
rbOrient = true;
return nNewIndex;
break;
}
// try the other ends as well
try{
CCartesian_pointStep aCartesian_pointStep3( pLineStep1->m_pPos, *pLineStep1->m_pPos);
aCartesian_pointStep3+= *pLineStep1->m_pDir;
dDist2 = getLength2( (pLineStep->m_pPos), (&aCartesian_pointStep3));
CCartesian_pointStep aCartesian_pointStep4( pLineStep->m_pPos, *pLineStep->m_pPos);
aCartesian_pointStep4+= *pLineStep->m_pDir;
dDist2+= getLength2( &aCartesian_pointStep4, (pLineStep1->m_pPos));
}
catch(...){
// return -1;
throw;
}
if (dDist2 < restol2 * 4) {
nNewIndex = i;
m_hash.SetAt(nIndex, (void*)nNewIndex);
rbOrient = false;
return nNewIndex;
break;
}
}
#endif
m_hash.SetAt(nIndex, (void*)nNewIndex);
if (nNewIndex == nIndex) {
CPtrArray::SetAt( nNewIndex, pLineStep);
rbOrient = true;
}
return nIndex;
}
//-------------------------------------------------------------------------
void FKCW_3D_Node::rotate3D(const FKCW_3D_Vector4& axis,float cosA,float sinA)
{
// axis必须是个单元向量
assert(axis.w()==0);
assert(getLength2(axis)==1);
FKCW_3D_Matrix4 RMat=calculateRotationMatrix(axis, cosA, sinA);
m_mat=m_mat*RMat;
};
bool vector::withinTol()
{
return ( getLength2()< tol2);
}
bool CDirectionStep::withinTol()// remove me
{
return getLength2() < restol2;
}
double CDirectionStep::getLength()// remove me
{
double dLen = getLength2();
return sqrt(dLen);
}