//-------------------------------------------------------------------------------------
void AxisLines::initAxis(Ogre::String boneName, Ogre::Entity* entity, Ogre::SceneManager* mSceneManager)
{
if(isXVisible) /* red */
{
xLine = new DynamicLines(Ogre::RenderOperation::OT_LINE_LIST);
entity->attachObjectToBone(boneName, xLine);
xLine->setMaterial(color1);
}
if(isYVisible) /* green */
{
yLine = new DynamicLines(Ogre::RenderOperation::OT_LINE_LIST);
entity->attachObjectToBone(boneName, yLine);
yLine->setMaterial(color2);
}
if(isZVisible) /* blue */
{
zLine = new DynamicLines(Ogre::RenderOperation::OT_LINE_LIST);
entity->attachObjectToBone(boneName, zLine);
zLine->setMaterial(color3);
}
Ogre::Bone* bone = entity->getSkeleton()->getBone(boneName);
Ogre::Quaternion q = bone->getOrientation();
this->updateLines(q.xAxis(), q.yAxis(), q.zAxis());
}
bool Framework::frameRenderingQueued(const Ogre::FrameEvent& evt)
{
mTrayMgr->frameRenderingQueued(evt);
if (!mTrayMgr->isDialogVisible())
{
mCameraMan->frameRenderingQueued(evt); // if dialog isn't up, then update the camera
if (mDetailsPanel->isVisible()) // if details panel is visible, then update its contents
{
mDetailsPanel->setParamValue(0, Ogre::StringConverter::toString(mCamera->getDerivedPosition().x));
mDetailsPanel->setParamValue(1, Ogre::StringConverter::toString(mCamera->getDerivedPosition().y));
mDetailsPanel->setParamValue(2, Ogre::StringConverter::toString(mCamera->getDerivedPosition().z));
mDetailsPanel->setParamValue(4, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().w));
mDetailsPanel->setParamValue(5, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().x));
mDetailsPanel->setParamValue(6, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().y));
mDetailsPanel->setParamValue(7, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().z));
#ifdef USE_RTSHADER_SYSTEM
mDetailsPanel->setParamValue(14, Ogre::StringConverter::toString(mShaderGenerator->getVertexShaderCount()));
mDetailsPanel->setParamValue(15, Ogre::StringConverter::toString(mShaderGenerator->getFragmentShaderCount()));
#endif
Ogre::Quaternion q = mCamera->getDerivedOrientation();
mDetailsPanel->setParamValue(16, Ogre::StringConverter::toString(q.xAxis() ) );
mDetailsPanel->setParamValue(17, Ogre::StringConverter::toString(q.yAxis() ));
mDetailsPanel->setParamValue(18, Ogre::StringConverter::toString(q.zAxis() ));
}
}
return true;
}
void Vehicle::jet(NxReal stiff)
{
Ogre::Quaternion quat = UtilityFunc::NxQuat_ToOgre_Quat(mActor->getGlobalOrientationQuat());
NxVec3 nxv3 = UtilityFunc::OgreVec3_To_NxVec3(quat.zAxis());
nxv3.normalize();
mActor->addForce(nxv3 * stiff, NX_IMPULSE);
}
void SoundListener::updateListener()
{
Ogre::Vector3 position = this->getPosition().Meter() ;
alListener3f(AL_POSITION, (float)position.x, (float)position.y, (float)position.z);
Ogre::Quaternion orientation = this->getOrientation().getQuaternion();
ALfloat openal_orientation[6] ;
openal_orientation[0] = orientation.zAxis().x ;
openal_orientation[1] = orientation.zAxis().y ;
openal_orientation[2] = orientation.zAxis().z ;
openal_orientation[3] = orientation.yAxis().x ;
openal_orientation[4] = orientation.yAxis().y ;
openal_orientation[5] = orientation.yAxis().z ;
alListenerfv(AL_ORIENTATION, openal_orientation) ;
Ogre::Vector3 speed = this->getSpeed().MeterPerSecond();
alListener3f(AL_VELOCITY, (float)speed.x, (float)speed.y, (float)speed.z) ;
alListenerf(AL_GAIN,getGain()) ;
}
/*/////////////////////////////////////////////////////////////////*/
void OgreOggListener::setOrientation(const Ogre::Quaternion& q) {
Ogre::Vector3 vDirection = q.zAxis();
Ogre::Vector3 vUp = q.yAxis();
mOrientation[0] = -vDirection.x;
mOrientation[1] = -vDirection.y;
mOrientation[2] = -vDirection.z;
mOrientation[3] = vUp.x;
mOrientation[4] = vUp.y;
mOrientation[5] = vUp.z;
alListenerfv(AL_ORIENTATION, mOrientation);
}
QColor SparseOccupancyGridArrayDisplay::axisColor(const Ogre::Quaternion& q,
const Ogre::Vector3& p)
{
Ogre::Vector3 zaxis = q.zAxis();
Ogre::Vector3 reference = p.normalisedCopy();
double dot = zaxis.dotProduct(reference);
if (dot < -1) {
dot = -1.0;
}
else if (dot > 1) {
dot = 1.0;
}
double scale = (dot + 1) / 2.0;
return gridColor(scale);
}
void srs_ui_but::CButProjection::cameraInfoCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info)
{
// std::cerr << "Camera callback. Frame id: " << cam_info->header.frame_id << std::endl;
boost::mutex::scoped_lock( m_cameraInfoLock );
// Get camera info
ROS_DEBUG("OctMapPlugin: Set camera info: %d x %d\n", cam_info->height, cam_info->width);
if( !m_camera_model.fromCameraInfo(*cam_info) )
return;
m_camera_size = m_camera_model.fullResolution();
Ogre::Vector3 position;
Ogre::Quaternion orientation;
vis_manager_->getFrameManager()->getTransform(cam_info->header, position, orientation);
// const cv::Mat_<double> pm( m_camera_model.projectionMatrix() );
// convert vision (Z-forward) frame to ogre frame (Z-out)
orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);
// Get z-axis
Ogre::Vector3 normal( orientation.zAxis() );
normal.normalise();
// Compute camera plane equation
Ogre::Vector4 equation( normal.x, normal.y, normal.z, -normal.dotProduct( position) );
float width = cam_info->width;
float height = cam_info->height;
// Possibly malformed camera info...
if( width == 0.0 || height == 0.0 )
return;
double fx = cam_info->P[0];
double fy = cam_info->P[5];
/*
Ogre::Radian fovy( 2.0*atan(height / (2.0 * fy)) );
if( fovy != fovy)
return; // NAN
double aspect_ratio = width / height;
if( aspect_ratio != aspect_ratio )
return; //NaN
*/
// Add the camera's translation relative to the left camera (from P[3]);
// Tx = -1*(P[3] / P[0])
double tx = -1.0 * (cam_info->P[3] / fx);
Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
position = position + (right * tx);
// std::cerr << right * tx << std::endl;
double ty = -1 * (cam_info->P[7] / fy);
Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
position = position + (down * ty);
if( !rviz::validateFloats( position ))
{
return;
}
if( m_projectionData != 0 )
{
m_projectionData->setProjectorPosition( position );
m_projectionData->setProjectorOrientation( orientation );
// f.setFOVy( fovX );
// m_projectionData->setFOVy( fovy );
// m_projectionData->setAspectRatio( aspect_ratio );
m_projectionData->setCameraModel( *cam_info );
m_projectionData->setProjectorEquation( equation );
m_projectionData->updateMatrices();
}
}
//-------------------------------------------------------------------------------------
void ControllableCharacter::transformBone(Ogre::String boneName, NuiManager::NuiJointIndex jointIdx)
{
int state = 0;
state = (int)controller->getJointStatus(jointIdx);
if(state == 2)
{
Ogre::Bone* bone = skeleton->getBone(boneName);
Ogre::Quaternion qI = bone->getInitialOrientation();
Ogre::Quaternion newQ = jointCalc->getSkeletonJointOrientation(jointIdx);
bone->resetOrientation();
newQ = bone->convertWorldToLocalOrientation(newQ);
bone->setOrientation(newQ * qI);
Ogre::Quaternion resQ = bone->getOrientation();
if(showBoneOrientationAxes) axisLines[jointIdx]->updateLines(resQ.xAxis(), resQ.yAxis(), resQ.zAxis()); // debug
}
}