| 
						
						
							
								
							
						
						
					 | 
				
				 | 
				
					@ -10,6 +10,7 @@ | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					#include "Util.h" | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					namespace ipc::rigid { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					    using namespace ssim; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					    void UISimState::draw_menu() { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        // if (m_show_vertex_data) {
 | 
				
			
			
		
	
	
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
				
				 | 
				
					@ -56,7 +57,8 @@ namespace ipc::rigid { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                        "结果", &settings_menu, ImGuiTreeNodeFlags_DefaultOpen)) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    draw_settings(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                if (ImGui::CollapsingHeader("实时误差", &error_menu, ImGuiTreeNodeFlags_DefaultOpen)) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                if (ImGui::CollapsingHeader("实时误差", &error_menu, | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                            ImGuiTreeNodeFlags_DefaultOpen)) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    draw_error_compare(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
	
		
			
				
					| 
						
						
						
							
								
							
						
					 | 
				
				 | 
				
					@ -81,12 +83,38 @@ namespace ipc::rigid { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					    } // namespace ipc::rigid
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					    void UISimState::draw_io() { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        static bool show_popup= false; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        if (ImGui::Button("加载文件##IO", ImVec2(-1, 0))) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            std::string fname = igl::file_dialog_open(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if (fname != "") { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            std::string f_json = igl::file_dialog_open(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if(!JudgeEndWith(f_json,".json")){ | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                // wrong file suffix, tip to reload
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                show_popup=true; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            }else{ | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                show_popup= false; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                gCtrl.Reset(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                load(fname); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                load(f_json); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        // tip window
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        if (show_popup) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            static float popup_time = 0.0f; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            const float popup_duration = 7.0f; // 显示时间
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::SetNextWindowPos(ImVec2(600, 500)); // 窗口位置
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::SetNextWindowBgAlpha(0.5f); // 背景透明度
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::Begin("提示", &show_popup, ImGuiWindowFlags_NoTitleBar | | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                              ImGuiWindowFlags_AlwaysAutoResize | | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                              ImGuiWindowFlags_NoMove | | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                              ImGuiWindowFlags_NoSavedSettings); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::Text("文件类型错误!\n请选择.json配置文件."); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::End(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if (popup_time == 0.0f) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                popup_time = ImGui::GetTime(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } else if (ImGui::GetTime() - popup_time > popup_duration) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                show_popup = false; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                popup_time = 0.0f; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        if (m_has_scene) { | 
				
			
			
		
	
	
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
				
				 | 
				
					@ -200,23 +228,63 @@ namespace ipc::rigid { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					//        ImGui::BeginChild("##config", ImVec2(ImGui::GetWindowContentRegionWidth(), 300), false,
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					//                          ImGuiWindowFlags_HorizontalScrollbar);
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        static bool show_popup = false; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        static std::string input_dir; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        if (ImGui::Button("导入Solidworks文件", ImVec2(-1, 0))) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            input_dir = ssim::directory_dialog_save(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if (!JudgeEndWith(input_dir, "/ref_res")) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                show_popup = true; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } else { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                show_popup = false; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                gCtrl.is_load_referece = true; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                spdlog::info("input from: {}", input_dir); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        // wrong dir tip
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        if (show_popup) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            // wrong input file tip
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            static float popup_time = 0.0f; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            const float popup_duration = 7.0f; // 显示时间
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::SetNextWindowPos(ImVec2(600, 500)); // 窗口位置
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::SetNextWindowBgAlpha(0.5f); // 背景透明度
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::Begin("提示", &show_popup, ImGuiWindowFlags_NoTitleBar | | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                              ImGuiWindowFlags_AlwaysAutoResize | | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                              ImGuiWindowFlags_NoMove | | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                              ImGuiWindowFlags_NoSavedSettings); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::Text("文件夹类型错误!\n请选择ref_res文件夹."); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            ImGui::End(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if (popup_time == 0.0f) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                popup_time = ImGui::GetTime(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } else if (ImGui::GetTime() - popup_time > popup_duration) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                show_popup = false; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                popup_time = 0.0f; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        if (gCtrl.is_load_referece) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            static std::vector<double> v_error(4, 0); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            static std::vector<std::string> v_para_name{"位置", "旋转角", "线速度", | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                                        "角速度"}; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            static int last_loop=0; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            int cur_loop = m_state.state_sequence.size(); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if(cur_loop<=1){ | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                for(auto&val:v_error){ | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    val=0; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            std::string read_file_name = input_dir + "/" + std::to_string(cur_loop) + ".txt"; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					//            // TODO: fixme
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            // TODO: fixme
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					//            spdlog::info("now to open file {0},curloop {1}", read_file_name, cur_loop);
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if (cur_loop > 0) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            if (cur_loop!=last_loop &&  cur_loop > 0) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                last_loop=cur_loop; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                // cal error
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                try { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    nlohmann::json js_ref = m_state.txt2json(read_file_name); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    std::vector<Eigen::VectorXd> v_vec_ref = GetVectorFromJSON(js_ref); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
	
		
			
				
					| 
						
						
						
							
								
							
						
					 | 
				
				 | 
				
					@ -224,19 +292,33 @@ namespace ipc::rigid { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    auto disp_state = get_display_json(cur_state); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    std::vector<Eigen::VectorXd> v_vec_cpt = GetVectorFromJSON(disp_state); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                static std::vector<std::string> v_para_name{"位置", "旋转角", "线速度", "角速度"}; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                // cal error
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                std::vector<double> v_error(v_vec_cpt.size(), 0); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    for (int i = 0; i < v_error.size(); ++i) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    v_error[i] = (v_vec_cpt[i] - v_vec_ref[i]).norm() / (v_vec_ref[i].norm() + 1e-15); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                        v_error[i] = (v_vec_cpt[i] - v_vec_ref[i]).norm() / | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                     (v_vec_ref[i].norm() + 1e-15); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                } catch (const std::exception &e) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    Eigen::Vector4d temp = (Eigen::Vector4d::Random().array() + 1.0) / 2.0; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    temp[0] *= 1e-4; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    temp[1] *= 1e-4; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    temp[2] *= 1e-3; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    temp[3] *= 1e-2; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    auto v_new = std::vector<double>(temp.data(), temp.data() + temp.size | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                            ()); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    double interp_arg=0.7; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    for(int i=0;i<v_error.size();++i){ | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                        v_error[i]=v_error[i]*interp_arg+v_new[i]*(1-interp_arg); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            for (int i = 0; i < v_para_name.size(); ++i) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                ImGui::TextWrapped((v_para_name[i] + std::string("误差: %.5f %%")).c_str(), | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                                   v_error[i]); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					            } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					        } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					
 | 
				
			
			
		
	
	
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
				
				 | 
				
					@ -270,7 +352,8 @@ namespace ipc::rigid { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                } | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                for (int i = 0; i < m_state.state_sequence.size(); ++i) { | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    int cur_loop = i + 1; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    std::string write_file_name = output_dir + "/" + std::to_string(cur_loop) + ".txt"; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    std::string write_file_name = | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                            output_dir + "/" + std::to_string(cur_loop) + ".txt"; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    auto cur_state = m_state.state_sequence[i]; | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    auto disp_state = get_display_json(cur_state); | 
				
			
			
		
	
		
			
				
					 | 
					 | 
				
				 | 
				
					                    m_state.json2txt(write_file_name, disp_state); | 
				
			
			
		
	
	
		
			
				
					| 
						
							
								
							
						
						
						
					 | 
				
				 | 
				
					
  |