mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ui/update_dmonitoring: Improve readability of r_xyz matrix initialization (#32359)

This commit is contained in:
Dean Lee 2024-05-22 08:27:25 +08:00 committed by GitHub
parent 9f327aeb48
commit d234085496
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
selfdrive/ui/ui.cc
View File

@ -125,25 +125,27 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers
scene.driver_pose_coss[i] = cosf(scene.driver_pose_vals[i]*(1.0-dm_fade_state));
}
auto [sin_y, sin_x, sin_z] = scene.driver_pose_sins;
auto [cos_y, cos_x, cos_z] = scene.driver_pose_coss;
const mat3 r_xyz = (mat3){{
scene.driver_pose_coss[1]*scene.driver_pose_coss[2],
scene.driver_pose_coss[1]*scene.driver_pose_sins[2],
-scene.driver_pose_sins[1],
cos_x * cos_z,
cos_x * sin_z,
-sin_x,
-scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_coss[0]*scene.driver_pose_sins[2],
-scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_coss[0]*scene.driver_pose_coss[2],
-scene.driver_pose_sins[0]*scene.driver_pose_coss[1],
-sin_y * sin_x * cos_z - cos_y * sin_z,
-sin_y * sin_x * sin_z + cos_y * cos_z,
-sin_y * cos_x,
scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_sins[0]*scene.driver_pose_sins[2],
scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_sins[0]*scene.driver_pose_coss[2],
scene.driver_pose_coss[0]*scene.driver_pose_coss[1],
cos_y * sin_x * cos_z - sin_y * sin_z,
cos_y * sin_x * sin_z + sin_y * cos_z,
cos_y * cos_x,
}};
// transform vertices
for (int kpi = 0; kpi < std::size(default_face_kpts_3d); kpi++) {
vec3 kpt_this = default_face_kpts_3d[kpi];
kpt_this = matvecmul3(r_xyz, kpt_this);
scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}};
vec3 kpt_this = matvecmul3(r_xyz, default_face_kpts_3d[kpi]);
scene.face_kpts_draw[kpi] = (vec3){{kpt_this.v[0], kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}};
}
}