summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--logic/pipeline.cpp55
-rw-r--r--logic/pipeline.hpp5
2 files changed, 36 insertions, 24 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 5dfe3ccb..c5dfe2d7 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -65,12 +65,13 @@ euler_t reltrans::rotate(const rmat& R, const euler_t& in, vec3_bool disable) co
return output;
}
-Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec6_bool& disable)
+Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
+ const vec6_bool& disable, bool neck_enable, int neck_z)
{
+ euler_t rel(static_cast<const double*>(value));
+
if (state != reltrans_disabled)
{
- euler_t rel { &value[0] };
-
{
bool tcomp_in_zone_ = progn(
if (state == reltrans_non_center)
@@ -100,6 +101,15 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec
value(Roll) * d2r * !disable(Roll)));
rel = rotate(R, rel, &disable[TX]);
+
+ // dynamic neck
+ if (neck_enable)
+ {
+ const euler_t neck = apply_neck(value, -neck_z);
+
+ for (unsigned k = 0; k < 3; k++)
+ rel(k) += neck(k);
+ }
}
if (cur)
@@ -130,31 +140,35 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec
{
interp_pos = rel;
}
-
- return { rel(0), rel(1), rel(2), value(Yaw), value(Pitch), value(Roll) };
}
else
{
cur = false;
in_zone = false;
- return value;
+ // dynamic neck
+ if (neck_enable)
+ {
+ const euler_t neck = apply_neck(value, -neck_z);
+
+ for (unsigned k = 0; k < 3; k++)
+ rel(k) += neck(k);
+ }
}
+
+ return {
+ rel(TX), rel(TY), rel(TZ),
+ value(Yaw), value(Pitch), value(Roll),
+ };
}
-euler_t reltrans::apply_neck(const Pose& value, bool enable, int nz) const
+euler_t reltrans::apply_neck(const Pose& value, int nz) const
{
- if (!enable)
- return {};
-
euler_t neck;
- if (nz != 0)
- {
- const rmat R = euler_to_rmat(euler_t(&value[Yaw]) * d2r);
- neck = rotate(R, { 0, 0, nz }, vec3_bool());
- neck(TZ) = neck(TZ) - nz;
- }
+ const rmat R = euler_to_rmat(euler_t(&value[Yaw]) * d2r);
+ neck = rotate(R, { 0, 0, nz }, vec3_bool());
+ neck(TZ) = neck(TZ) - nz;
return neck;
}
@@ -386,18 +400,15 @@ Pose pipeline::apply_zero_pos(Pose value) const
Pose pipeline::apply_reltrans(Pose value, vec6_bool disabled)
{
- const euler_t neck = rel.apply_neck(value, s.neck_enable, -s.neck_z);
-
value = rel.apply_pipeline(s.reltrans_mode, value,
{ !!s.reltrans_disable_src_yaw,
!!s.reltrans_disable_src_pitch,
!!s.reltrans_disable_src_roll,
!!s.reltrans_disable_tx,
!!s.reltrans_disable_ty,
- !!s.reltrans_disable_tz });
-
- for (int i = 0; i < 3; i++)
- value(i) += neck(i);
+ !!s.reltrans_disable_tz },
+ s.neck_enable,
+ s.neck_z);
// reltrans will move it
for (unsigned k = 0; k < 6; k++)
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index d51655b3..7f618ea1 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -52,10 +52,11 @@ public:
euler_t rotate(const rmat& rmat, const euler_t& in, vec3_bool disable) const;
warn_result_unused
- Pose apply_pipeline(reltrans_state cur, const Pose& value, const vec6_bool& disable);
+ Pose apply_pipeline(reltrans_state state, const Pose& value,
+ const vec6_bool& disable, bool neck_enable, int neck_z);
warn_result_unused
- euler_t apply_neck(const Pose& value, bool enable, int nz) const;
+ euler_t apply_neck(const Pose& value, int nz) const;
};
using namespace time_units;