void PlayerCameraOgre::onRightButtonPressed()
{
if (!mRightButtonPressedLastFrame)
mMousePosLastFrame = mMouse->getPosition();
mp::Vector2i diff = mMouse->getPosition() - mMousePosLastFrame;
const mp::Vector3f &playerPos = mPlayer->model()->getPosition();
Ogre::Vector3 pivotPoint(playerPos.getX(), playerPos.getY() + mPivotHeight, playerPos.getZ());
float yaw = (float)diff.getX() * CAMERA_SPEED;
float pitch = (float)-diff.getY() * CAMERA_SPEED;
Ogre::Quaternion yawQuat;
yawQuat.FromAngleAxis(Ogre::Radian(yaw), Ogre::Vector3::UNIT_Y);
Ogre::Matrix3 yawMat;
yawQuat.ToRotationMatrix(yawMat);
Ogre::Vector3 pivotToPos = Ogre::Vector3(mRealPosition.getX(), mRealPosition.getY(), mRealPosition.getZ()) - pivotPoint;
Ogre::Matrix4 pos(1, 0, 0, pivotToPos.x,
0, 1, 0, pivotToPos.y,
0, 0, 1, pivotToPos.z,
0, 0, 0, 1);
Ogre::Vector3 xz(pivotToPos.x, 0, pivotToPos.z);
Ogre::Vector3 norm(-xz.z, 0, xz.x);
Ogre::Quaternion pitchQuat;
pitchQuat.FromAngleAxis(Ogre::Radian(pitch), norm);
Ogre::Matrix3 pitchMat;
pitchQuat.ToRotationMatrix(pitchMat);
Ogre::Matrix4 toPivot(1, 0, 0, pivotPoint.x,
0, 1, 0, pivotPoint.y,
0, 0, 1, pivotPoint.z,
0, 0, 0, 1);
Ogre::Matrix4 newPosMat = pos * pitchMat * yawMat * toPivot;
newPosMat = newPosMat.inverse();
Ogre::Vector3 newPos = newPosMat.getTrans();
mRealPosition.set(-newPos.x, -newPos.y, -newPos.z);
setPosition(mRealPosition);
lookAt(pivotPoint.x, pivotPoint.y, pivotPoint.z);
adjustDistance();
mMousePosLastFrame = mMouse->getPosition();
mRightButtonPressedLastFrame = true;
}
void MapCloudDisplay::update( float wall_dt, float ros_dt )
{
rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
if (needs_retransform_)
{
retransform();
needs_retransform_ = false;
}
{
boost::mutex::scoped_lock lock(new_clouds_mutex_);
if( !new_cloud_infos_.empty() )
{
float size;
if( mode == rviz::PointCloud::RM_POINTS ) {
size = point_pixel_size_property_->getFloat();
} else {
size = point_world_size_property_->getFloat();
}
std::map<int, CloudInfoPtr>::iterator it = new_cloud_infos_.begin();
std::map<int, CloudInfoPtr>::iterator end = new_cloud_infos_.end();
for (; it != end; ++it)
{
CloudInfoPtr cloud_info = it->second;
cloud_info->cloud_.reset( new rviz::PointCloud() );
cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
cloud_info->cloud_->setRenderMode( mode );
cloud_info->cloud_->setAlpha( alpha_property_->getFloat() );
cloud_info->cloud_->setDimensions( size, size, size );
cloud_info->cloud_->setAutoSize(false);
cloud_info->manager_ = context_->getSceneManager();
cloud_info->scene_node_ = scene_node_->createChildSceneNode();
cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
cloud_info->scene_node_->setVisible(false);
cloud_infos_.insert(*it);
}
new_cloud_infos_.clear();
}
}
{
boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
if( lock.owns_lock() )
{
if( new_xyz_transformer_ || new_color_transformer_ )
{
M_TransformerInfo::iterator it = transformers_.begin();
M_TransformerInfo::iterator end = transformers_.end();
for (; it != end; ++it)
{
const std::string& name = it->first;
TransformerInfo& info = it->second;
setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() );
setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() );
}
}
}
new_xyz_transformer_ = false;
new_color_transformer_ = false;
}
int totalPoints = 0;
int totalNodesShown = 0;
{
// update poses
boost::mutex::scoped_lock lock(current_map_mutex_);
if(!current_map_.empty())
{
for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
{
std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
if(cloudInfoIt != cloud_infos_.end())
{
totalPoints += cloudInfoIt->second->transformed_points_.size();
cloudInfoIt->second->pose_ = it->second;
Ogre::Vector3 framePosition;
Ogre::Quaternion frameOrientation;
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation))
{
// Multiply frame with pose
Ogre::Matrix4 frameTransform;
frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation);
const rtabmap::Transform & p = cloudInfoIt->second->pose_;
Ogre::Matrix4 pose(p[0], p[1], p[2], p[3],
p[4], p[5], p[6], p[7],
p[8], p[9], p[10], p[11],
0, 0, 0, 1);
frameTransform = frameTransform * pose;
Ogre::Vector3 posePosition = frameTransform.getTrans();
Ogre::Quaternion poseOrientation = frameTransform.extractQuaternion();
poseOrientation.normalise();
cloudInfoIt->second->scene_node_->setPosition(posePosition);
cloudInfoIt->second->scene_node_->setOrientation(poseOrientation);
cloudInfoIt->second->scene_node_->setVisible(true);
++totalNodesShown;
}
else
{
ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
}
}
}
//hide not used clouds
for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end(); ++iter)
{
if(current_map_.find(iter->first) == current_map_.end())
{
iter->second->scene_node_->setVisible(false);
}
}
}
}
this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
}
void BoneAim::update(float time){
if (mBone == NULL){
mBone = mGo->getEntity()->getSkeleton()->getBone(mBoneName);
mTarget = Level::getSingleton()->getCurrentSegment()->getObjectByName(mTargetName);
if (mTarget == NULL){
throw("No such object : "+mTargetName);
}
}
//Ogre::Entity* entity;
//mTarget = Level::getSingleton()->getPlayerShip();
Ogre::Matrix4 matrixA = mGo->getNode()->_getFullTransform()*mBone->_getFullTransform();
Ogre::Matrix4 matrix = matrixA;
Ogre::Matrix4 transform = matrix.inverse()*mTarget->getNode()->_getFullTransform();
Ogre::Vector3 v = transform.getTrans();
/*Util::Log(
"Bone:"+ts(matrixA.getTrans())+
" Ship:"+ts(mTarget->getNode()->_getFullTransform().getTrans())+
" Diff:"+ts(v)
,0
);*/
//targetMatrix = Ogre::Matrix4::IDENTITY;
//matrix.setTrans(mTarget->getNode()->_getFullTransform());
//Ogre::Quaternion q = transform.extractQuaternion();
Ogre::Quaternion q;
switch (mAxis){
case AXIS_X:
q.FromAngleAxis(Ogre::Math::ATan2(v.y, v.z),Ogre::Vector3(-1,0,0));
break;
case AXIS_Y:
q.FromAngleAxis(Ogre::Math::ATan2(v.x, v.z),Ogre::Vector3(0,1,0));
break;
case AXIS_Z:
q.FromAngleAxis(Ogre::Math::ATan2(v.x, v.y),Ogre::Vector3(0,0,1));
break;
case AXIS_ALL:
//q.FromAngleAxis(Ogre::Radian(0),v);
q = transform.extractQuaternion();// - matrixA.extractQuaternion();
//q = transform.extractQuaternion() - matrixA.extractQuaternion();
break;
}
mBone->setManuallyControlled(true);
//mBone->setOrientation(q);
float slerp = time*mStrength;
if (slerp > 1){
slerp = 1;
}
Ogre::Quaternion r = Quaternion::Slerp(slerp, Ogre::Quaternion::IDENTITY, q); //Level::getSingleton()->getTimeDelta()
mBone->rotate(r);
/*mBone->yaw(Ogre::Radian(Ogre::Math::RangeRandom(-Math::PI, Math::PI)));
mBone->pitch(Ogre::Radian(Ogre::Math::RangeRandom(-Math::PI, Math::PI)));
mBone->roll(Ogre::Radian(Ogre::Math::RangeRandom(-Math::PI, Math::PI)));*/
//mBone->_update(true, true);
Parent::update(time);
//mSpeed += .6*time;
//mGo->getNode()->translate(0, 0-mSpeed, 0);*/
}
void PhysicalCamera::moveRelative(const opal::Vec3r& dir, opal::real dt)
{
// Construct the actual velocity vector.
opal::Vec3r velocity = dir;
if (!opal::areEqual(velocity.lengthSquared(), 0))
{
velocity.normalize();
}
velocity *= mTranslateSpeed;
switch(mType)
{
case PHYSICAL:
{
assert(mSolid);
// TODO: handle things differently if we're in midair.
Ogre::Matrix4 camTransform;
mOgreCamera->getParentSceneNode()->getWorldTransforms(
&camTransform);
Ogre::Vector3 localDir(velocity[0], velocity[1], velocity[2]);
// Convert the local direction vector to a global direction
// vector. Subtract out the camera's position.
Ogre::Vector3 globalDir = camTransform * localDir;
globalDir -= camTransform.getTrans();
opal::Vec3r opalVec(globalDir[0], globalDir[1], globalDir[2]);
// Keep from flying.
if (opalVec[1] > 0)
{
opalVec[1] = 0;
}
// Don't use the dt in this case; let Opal take care of the
// velocity.
mSolid->setGlobalLinearVel(opalVec);
break;
}
case NON_CLIPPING:
{
Ogre::Vector3 posChange(velocity[0] * dt, velocity[1] * dt,
velocity[2] * dt);
mOgreCamera->getParentSceneNode()->translate(posChange,
Ogre::Node::TS_LOCAL);
break;
}
case NON_CLIPPING_ORBIT:
{
Ogre::Vector3 posChange(velocity[0] * dt, velocity[1] * dt,
velocity[2] * dt);
mOgreCamera->getParentSceneNode()->translate(posChange,
Ogre::Node::TS_LOCAL);
lookAt(mOrbitCenter);
break;
}
default:
assert(false);
}
}