osgQuat WindowsManager::corbaConfToOsgQuat (const value_type* configCorba)
{
// configurationCorba = trans (x, y, z), quat (w, x, y, z)
// osgQuat = quat (x, y, z, w)
return osgQuat (configCorba[4], configCorba[5],
configCorba[6], configCorba[3]);
}
void getStaticTransform (const boost::shared_ptr < urdf::Link >& link,
osgVector3 &static_pos, osgQuat &static_quat,
bool visual, long unsigned int visual_index)
{
if (visual || (link->visual_array.size()>1))
{
if (link->visual_array.size()>1)
{
// Set staticTransform = transform from link to visual
static_pos = osgVector3((float)link->visual_array[visual_index]->origin.position.x,
(float)link->visual_array[visual_index]->origin.position.y,
(float)link->visual_array[visual_index]->origin.position.z);
static_quat=osgQuat( (float)link->visual_array[visual_index]->origin.rotation.x,
(float)link->visual_array[visual_index]->origin.rotation.y,
(float)link->visual_array[visual_index]->origin.rotation.z,
(float)link->visual_array[visual_index]->origin.rotation.w);
}
else
{
// Set staticTransform = transform from link to visual
static_pos = osgVector3((float)link->visual->origin.position.x,
(float)link->visual->origin.position.y,
(float)link->visual->origin.position.z);
static_quat=osgQuat( (float)link->visual->origin.rotation.x,
(float)link->visual->origin.rotation.y,
(float)link->visual->origin.rotation.z,
(float)link->visual->origin.rotation.w);
}
} else {
// Set staticTransform = transform from link to visual
static_pos = osgVector3((float)link->collision->origin.position.x,
(float)link->collision->origin.position.y,
(float)link->collision->origin.position.z);
static_quat=osgQuat( (float)link->collision->origin.rotation.x,
(float)link->collision->origin.rotation.y,
(float)link->collision->origin.rotation.z,
(float)link->collision->origin.rotation.w);
}
}
osg::Quat OculusDevice::getOrientation() const
{
// Create identity quaternion
osg::Quat osgQuat(0.0f, 0.0f, 0.0f, 1.0f);
if (m_sensorFusion && m_sensorFusion->IsAttachedToSensor()) {
OVR::Quatf quat;
if (m_sensorFusion->IsPredictionEnabled()) {
quat = m_sensorFusion->GetPredictedOrientation(m_predictionDelta);
} else {
quat = m_sensorFusion->GetOrientation();
}
osgQuat.set(quat.x, quat.y, quat.z, -quat.w);
}
return osgQuat;
}
