// ******************************************************* // 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 }