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 */
}
