/*!
* Return avatar position expressed in the environment reference frame.
* With respect to the function getPosition(),
* getAvatarPosition() takes into account offsets
* and motor scale factors.
*/
vpPoseVector vpVirtuose::getAvatarPosition() const
{
if (!m_is_init) {
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
}
float position_[7];
vpPoseVector position;
vpTranslationVector translation;
vpQuaternionVector quaternion;
if (virtGetAvatarPosition(m_virtContext, position_)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtGetAvatarPosition: error code %d", err));
}
else
{
for (int i=0; i<3; i++)
translation[i] = position_[i];
for (int i=0; i<4; i++)
quaternion[i] = position_[3+i];
vpThetaUVector thetau(quaternion);
position.buildFrom(translation, thetau);
return position;
}
}
void HaptionDriver::onAnimateBeginEvent()
{
if(connection_device)
{
if(!initCallback && myData.forceFeedback)
{
initCallback = true;
virtStartLoop(myData.m_virtContext);
}
float position[7] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f };
virtGetAvatarPosition(myData.m_virtContext, position);
VecCoord& posR = (*rigidDOF->x0.beginEdit());
posR[0].getCenter() = Vec3d(scale.getValue()*position[0],scale.getValue()*position[1],scale.getValue()*position[2]);
posR[0].getOrientation() = Quat(position[3],position[4],position[5],position[6]);
rigidDOF->x0.endEdit();
//button_state
int buttonState[1];
virtGetButton(myData.m_virtContext,1,buttonState);
state_button.setValue(buttonState[0]);
//visu
if(visuActif)
{
VecCoord& posH = (*visualHaptionDOF->x.beginEdit());
posH[1].getCenter() = Vec3d(scale.getValue()*position[0],scale.getValue()*position[1],scale.getValue()*position[2]);
posH[1].getOrientation() = Quat(position[3],position[4],position[5],position[6]);
visualHaptionDOF->x.endEdit();
}
if(haptionVisu.getValue() && !visuActif)
{
cout<<"add visu called"<<endl;
sofa::simulation::tree::GNode *parent = dynamic_cast<sofa::simulation::tree::GNode*>(this->getContext());
parent->getParent()->addChild(nodeHaptionVisual);
nodeHaptionVisual->updateContext();
visuActif=true;
cout<<"add visu ok"<<endl;
}
if(!haptionVisu.getValue() && visuActif)
{
cout<<"remove visu called"<<endl;
sofa::simulation::tree::GNode *parent = dynamic_cast<sofa::simulation::tree::GNode*>(this->getContext());
parent->getParent()->removeChild(nodeHaptionVisual);
nodeHaptionVisual->updateContext();
visuActif=false;
cout<<"remove visu ok"<<endl;
}
}
}
