VectorXd operator()(const VectorXd& x) const {
m_config->SetDOFValues(toDblVec(x.topRows(m_nDof)));
OR::Vector ptWorldA, ptWorldB, nWorldA, nWorldB;
CalcWorldPoints(x, ptWorldA, ptWorldB);
return toVector3d(ptWorldA - ptWorldB);
}
void CalcWorldPointsAndNormal(const VectorXd& x, OR::Vector& ptWorldA, OR::Vector& ptWorldB, OR::Vector& nWorldA, OR::Vector& nWorldB) const {
DblVec dofs(x.data(), x.data() + m_nDof);
OpenRAVE::Vector ptLocalA, ptLocalB;
ptLocalA.x = x(m_nDof);
ptLocalA.y = x(m_nDof + 1);
ptLocalB.x = x(m_nDof + 2);
ptLocalB.y = x(m_nDof + 3);
m_config->SetDOFValues(dofs);
ptWorldA = m_faceA.link->GetTransform() * (m_faceA.Tlf * ptLocalA);
ptWorldB = m_faceB.link->GetTransform() * (m_faceB.Tlf * ptLocalB);
nWorldA = geometry::quatRotate(geometry::quatMultiply(m_faceA.link->GetTransform().rot,m_faceA.Tlf.rot), OR::Vector(0,0,1));
nWorldB = geometry::quatRotate(geometry::quatMultiply(m_faceB.link->GetTransform().rot,m_faceB.Tlf.rot), OR::Vector(0,0,1));
}