From 73d07eaa496045ee1fd02c5f83530fc8fc497a8e Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Wed, 23 May 2018 00:59:59 +0200
Subject: logic/pipeline: disable neck Z if reltrans Z disabled
---
logic/pipeline.cpp | 9 ++++++---
1 file changed, 6 insertions(+), 3 deletions(-)
(limited to 'logic/pipeline.cpp')
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 105e672a..4756db3e 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -114,7 +114,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
// dynamic neck
if (neck_enable)
{
- const euler_t neck = apply_neck(value, -neck_z);
+ const euler_t neck = apply_neck(value, -neck_z, disable(TZ));
for (unsigned k = 0; k < 3; k++)
rel(k) += neck(k);
@@ -169,7 +169,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
// dynamic neck
if (neck_enable)
{
- const euler_t neck = apply_neck(value, -neck_z);
+ const euler_t neck = apply_neck(value, -neck_z, disable(TZ));
for (unsigned k = 0; k < 3; k++)
rel(k) += neck(k);
@@ -182,7 +182,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value,
};
}
-euler_t reltrans::apply_neck(const Pose& value, int nz) const
+euler_t reltrans::apply_neck(const Pose& value, int nz, bool disable_tz) const
{
euler_t neck;
@@ -190,6 +190,9 @@ euler_t reltrans::apply_neck(const Pose& value, int nz) const
neck = rotate(R, { 0, 0, nz }, vec3_bool());
neck(TZ) = neck(TZ) - nz;
+ if (disable_tz)
+ neck(TZ) = 0;
+
return neck;
}
--
cgit v1.2.3