void CPlanarRotationConstraint::applyConstraint(CSimuEntity *pobj, const double ¤t_tm, const double &dt) { const Vector3d zerovec(0, 0, 0); double angle=m_av * dt; Matrix2d mat; RotationAxisMatrix(m_norm, angle, mat); VertexInfo *pvert = pobj->getVertexInfoPtr(); if (m_nVertexCount>0){ for (int i=0; i<m_nVertexCount; i++){ const int vid = m_pVertexIDArray[i]; Vector3d & x0 = pvert[vid].m_tmp0; Vector3d & x = pvert[vid].m_pos; x = _roateAPoint(x0, m_center, mat); x0 = x; } return; } if (m_nVertexCount<0){//all the vertices are involved const int nv = pobj->getVertexCount(); for (int i=0; i<nv; i++){ const int vid = i; Vector3d & x0 = pvert[vid].m_tmp0; Vector3d & x = pvert[vid].m_pos; x = _roateAPoint(x0, m_center, mat); x0 = x; } } }
void CMatrix::Rotate(double x, double y, double z, double angle) { CMatrix matM; matM = RotationAxisMatrix(CVector3D(x, y, z), angle); /* build otation matrix */ *this *= matM; /* multiply matrices */ }