1use nalgebra as na;
2use rsbullet::*;
3use std::time::Duration;
4
5fn main() -> BulletResult<()> {
6 let mut client = PhysicsClient::connect(Mode::Gui)?;
7 client
8 .set_default_search_path()?
9 .set_physics_engine_parameter(&PhysicsEngineParametersUpdate {
10 num_solver_iterations: Some(10),
11 ..Default::default()
12 })?
13 .set_time_step(Duration::from_secs_f64(1. / 120.))?;
14
15 let log_id = client.start_state_logging(
16 LoggingType::ProfileTimings,
17 "visualShapeBench.json",
18 Default::default(),
19 )?;
20
21 let plane = client.load_urdf(
22 "plane_transparent.urdf",
23 Some(UrdfOptions {
24 use_maximal_coordinates: Some(true),
25 ..Default::default()
26 }),
27 )?;
28
29 client
30 .configure_debug_visualizer(&DebugVisualizerOptions::Flag(
31 DebugVisualizerFlag::CovEnableRendering,
32 false,
33 ))?
34 .configure_debug_visualizer(&DebugVisualizerOptions::Flag(
35 DebugVisualizerFlag::CovEnablePlanarReflection,
36 true,
37 ))?
38 .configure_debug_visualizer(&DebugVisualizerOptions::Flag(
39 DebugVisualizerFlag::CovEnableGui,
40 false,
41 ))?
42 .configure_debug_visualizer(&DebugVisualizerOptions::Flag(
43 DebugVisualizerFlag::CovEnableTinyRenderer,
44 false,
45 ))?;
46
47 let shift = [0., -0.02, 0.];
48 let mesh_scale = [0.1, 0.1, 0.1];
49
50 let visual_shape = client.create_visual_shape(
51 VisualGeometry::Mesh {
52 file: "duck.obj",
53 scale: mesh_scale,
54 },
55 VisualShapeOptions {
56 transform: shift.into(),
57 rgba: [1., 1., 1., 1.],
58 specular: [0.4, 0.4, 0.],
59 flags: None,
60 },
61 )?;
62 let collision_shape = client.create_collision_shape(
63 CollisionGeometry::Mesh {
64 file: "duck_vhacd.obj",
65 scale: mesh_scale,
66 },
67 CollisionShapeOptions {
68 transform: shift.into(),
69 flags: None,
70 },
71 )?;
72
73 let range_x = 3;
74 let range_y = 3;
75
76 for i in 0..range_x {
77 for j in 0..range_y {
78 client.create_multi_body(&MultiBodyCreateOptions {
79 base: MultiBodyBase {
80 mass: 1.,
81 collision_shape,
82 visual_shape,
83 pose: na::Isometry3::translation(
84 ((-range_x as f64 / 2.) + i as f64) * mesh_scale[0] * 2.,
85 ((-range_y as f64 / 2.) + j as f64) * mesh_scale[1] * 2.,
86 1.,
87 ),
88 inertial_pose: na::Isometry3::identity(),
89 },
90 use_maximal_coordinates: true,
91 ..Default::default()
92 })?;
93 }
94 }
95
96 client
97 .configure_debug_visualizer(&DebugVisualizerOptions::Flag(
98 DebugVisualizerFlag::CovEnableRendering,
99 true,
100 ))?
101 .stop_state_logging(log_id)?
102 .set_gravity([0., 0., -9.81])?
103 .set_real_time_simulation(true)?;
104
105 let colors = [
106 [1., 0., 0., 1.],
107 [0., 1., 0., 1.],
108 [0., 0., 1., 1.],
109 [1., 1., 1., 1.],
110 ];
111 let mut current_color = 0;
112
113 loop {
114 let mouse_events = client.get_mouse_events()?;
115 for event in mouse_events {
116 match event {
117 MouseEvent::Move { .. } => {}
118 MouseEvent::Button {
119 mouse_x,
120 mouse_y,
121 button_index,
122 button_state,
123 } => {
124 if button_state.was_triggered() && button_index == 0 {
125 eprintln!("Mouse click at ({}, {})", mouse_x, mouse_y);
126 let (ray_from, ray_to) = get_ray_from_to(
127 client.get_debug_visualizer_camera()?,
128 mouse_x,
129 mouse_y,
130 );
131 eprintln!("Ray from: {:?}, Ray to: {:?}", ray_from, ray_to);
132 let ray_info = client.ray_test(ray_from, ray_to, None, None)?;
133 eprintln!("{:?}", ray_info);
134 for ray in ray_info {
135 if ray.object_unique_id != plane {
136 client.change_visual_shape(
137 ray.object_unique_id,
138 ray.link_index,
139 -1,
140 &ChangeVisualShapeOptions {
141 rgba_color: Some(colors[current_color]),
142 ..Default::default()
143 },
144 )?;
145 current_color = (current_color + 1) % colors.len()
146 }
147 }
148 eprintln!("aaa")
149 }
150 }
151 }
152 }
153 if !client.is_connected() {
154 return Ok(());
155 }
156 }
157}
158
159fn get_ray_from_to(cam: DebugVisualizerCamera, mouse_x: f32, mouse_y: f32) -> ([f64; 3], [f64; 3]) {
160 let cam_pos = [
161 cam.target[0] - cam.distance * cam.camera_forward[0],
162 cam.target[1] - cam.distance * cam.camera_forward[1],
163 cam.target[2] - cam.distance * cam.camera_forward[2],
164 ];
165 let far_plane = 10000.;
166 let ray_forward = [
167 cam.target[0] - cam_pos[0],
168 cam.target[1] - cam_pos[1],
169 cam.target[2] - cam_pos[2],
170 ];
171 let inv_len = far_plane * 1.0
172 / ((ray_forward[0] * ray_forward[0]
173 + ray_forward[1] * ray_forward[1]
174 + ray_forward[2] * ray_forward[2])
175 .sqrt());
176 let ray_forward = [
177 inv_len * ray_forward[0],
178 inv_len * ray_forward[1],
179 inv_len * ray_forward[2],
180 ];
181 let ray_from = cam_pos;
182 let one_over_width = 1.0 / cam.width as f32;
183 let one_over_height = 1.0 / cam.height as f32;
184 let d_hor = [
185 cam.horizontal[0] * one_over_width,
186 cam.horizontal[1] * one_over_width,
187 cam.horizontal[2] * one_over_width,
188 ];
189 let d_ver = [
190 cam.vertical[0] * one_over_height,
191 cam.vertical[1] * one_over_height,
192 cam.vertical[2] * one_over_height,
193 ];
194 let ray_to_center = [
195 ray_from[0] + ray_forward[0],
196 ray_from[1] + ray_forward[1],
197 ray_from[2] + ray_forward[2],
198 ];
199 let ray_to = [
200 ray_to_center[0] - 0.5 * cam.horizontal[0] + 0.5 * cam.vertical[0] + mouse_x * d_hor[0]
201 - mouse_y * d_ver[0],
202 ray_to_center[1] - 0.5 * cam.horizontal[1] + 0.5 * cam.vertical[1] + mouse_x * d_hor[1]
203 - mouse_y * d_ver[1],
204 ray_to_center[2] - 0.5 * cam.horizontal[2] + 0.5 * cam.vertical[2] + mouse_x * d_hor[2]
205 - mouse_y * d_ver[2],
206 ]
207 .map(|i| i as f64);
208 (ray_from.map(|i| i as f64), ray_to)
209}