// *******************************************************
// display(): called once per frame, whenever OpenGL decides it's time to redraw.
virtual void display( float animation_delta_time, Vector2d &window_steering )
{
if( animate ) animation_time += animation_delta_time;
update_camera( animation_delta_time , window_steering );
int basis_id = 0;
float atime = animation_time * 10;
Matrix4d model_transform = Matrix4d::Identity();
Matrix4d std_model = model_transform;
*m_tree = Tree(Matrix4d::Identity(), atime);
// Start coding here!!!!
m_bee->timepassby(atime);
// Plane
model_transform = std_model * Affine3d(Translation3d(5, -5, -60)).matrix(); // Position
glUniform4fv(g_addrs->color_loc, 1, Vector4f(.0f, .7f, .9f, 1).data()); // Color
m_plane->draw ( projection_transform, camera_transform, model_transform, "" );
// Tree
model_transform = std_model * Affine3d(Translation3d(4, -5, -40)).matrix(); // Position
glUniform4fv( g_addrs->color_loc, 1, Vector4f( .0f, .6f ,.2f ,1 ).data()); // Color
m_tree->draw( basis_id++, projection_transform, camera_transform, model_transform, "");
// Leg
model_transform = std_model * Affine3d(Translation3d(4, 1+2*(abs(20.0 - ((int)atime % 41)))/10, -40)).matrix(); // Position
model_transform *= Affine3d(AngleAxisd((-PI / 60 * atime), Vector3d(0, 1, 0))).matrix();
model_transform *= Affine3d(Translation3d(20, 0, 0)).matrix();
m_bee->draw(basis_id++, projection_transform, camera_transform, model_transform, "");
}
RTC::ReturnCode_t CoordinateTransformer::onActivated(RTC::UniqueId ec_id)
{
//initialize
m_InputV << 0, 0, 0;
m_OutputV << 0, 0, 0;
//deg ⇒ rad
m_rot_degX = m_rot_degX* M_PI / 180.0;
m_rot_degY = m_rot_degY* M_PI / 180.0;
m_rot_degZ = m_rot_degZ* M_PI / 180.0;
//各軸回転クオータニオン
m_qX = AngleAxisd(m_rot_degX, Vector3d::UnitX());
m_qY = AngleAxisd(m_rot_degY, Vector3d::UnitY());
m_qZ = AngleAxisd(m_rot_degZ, Vector3d::UnitZ());
//平行移動
m_trans = Translation3d(m_transX, m_transY, m_transZ);
if (m_mirrorXY){
m_scaleZ *= -1;
}
if (m_mirrorYZ){
m_scaleX *= -1;
}
if (m_mirrorZX){
m_scaleY *= -1;
}
return RTC::RTC_OK;
}
void PR2EihMapping::publish_home_pose() {
// publish home pose for check_handle_grasps
Matrix4d home_pose = arm->fk(home_joints);
Affine3d home_pose_affine = Translation3d(home_pose.block<3,1>(0,3)) * AngleAxisd(home_pose.block<3,3>(0,0));
geometry_msgs::PoseStamped home_pose_msg;
tf::Pose tf_pose;
tf::poseEigenToTF(home_pose_affine, tf_pose);
tf::poseTFToMsg(tf_pose, home_pose_msg.pose);
home_pose_msg.header.frame_id = "base_link";
home_pose_msg.header.stamp = ros::Time::now();
home_pose_pub.publish(home_pose_msg);
}
void update_camera( float animation_delta_time, Vector2d &window_steering )
{
const unsigned leeway = 70, border = 50;
float degrees_per_frame = .02f * animation_delta_time;
float meters_per_frame = 10.f * animation_delta_time;
cout << animation_time << endl;
// Determine camera rotation movement first
Vector2f movement_plus ( window_steering[0] + leeway, window_steering[1] + leeway ); // movement[] is mouse position relative to canvas center; leeway is a tolerance from the center.
Vector2f movement_minus ( window_steering[0] - leeway, window_steering[1] - leeway );
bool outside_border = false;
for( int i = 0; i < 2; i++ )
if ( abs( window_steering[i] ) > g.get_window_size()[i]/2 - border ) outside_border = true; // Stop steering if we're on the outer edge of the canvas.
for( int i = 0; looking && outside_border == false && i < 2; i++ ) // Steer according to "movement" vector, but don't start increasing until outside a leeway window from the center.
{
float angular_velocity = ( ( movement_minus[i] > 0) * movement_minus[i] + ( movement_plus[i] < 0 ) * movement_plus[i] ) * degrees_per_frame; // Use movement's quantity conditionally
camera_transform = Affine3d( AngleAxisd( angular_velocity, Vector3d( i, 1-i, 0 ) ) ) * camera_transform; // On X step, rotate around Y axis, and vice versa.
}
camera_transform = Affine3d(Translation3d( meters_per_frame * thrust )) * camera_transform; // Now translation movement of camera, applied in local camera coordinate frame
}
