|
|
@@ -1,10 +1,10 @@
|
|
|
-#include "render/humanoid/pose_controller.h"
|
|
|
#include "render/humanoid/humanoid_math.h"
|
|
|
#include "render/humanoid/humanoid_specs.h"
|
|
|
+#include "render/humanoid/pose_controller.h"
|
|
|
#include "render/humanoid/rig.h"
|
|
|
#include <QVector3D>
|
|
|
-#include <gtest/gtest.h>
|
|
|
#include <cmath>
|
|
|
+#include <gtest/gtest.h>
|
|
|
|
|
|
using namespace Render::GL;
|
|
|
|
|
|
@@ -46,7 +46,7 @@ protected:
|
|
|
|
|
|
TEST_F(PoseControllerCompatibilityTest, ElbowIKMatchesLegacyFunction) {
|
|
|
// Test that controller's solveElbowIK produces same result as elbowBendTorso
|
|
|
-
|
|
|
+
|
|
|
QVector3D const shoulder(0.21F, 1.45F, 0.0F);
|
|
|
QVector3D const hand(0.35F, 1.15F, 0.75F);
|
|
|
QVector3D const outward_dir(1.0F, 0.0F, 0.0F);
|
|
|
@@ -54,18 +54,18 @@ TEST_F(PoseControllerCompatibilityTest, ElbowIKMatchesLegacyFunction) {
|
|
|
float const lateral_offset = 0.12F;
|
|
|
float const y_bias = 0.02F;
|
|
|
float const outward_sign = 1.0F;
|
|
|
-
|
|
|
+
|
|
|
// Legacy approach
|
|
|
- QVector3D const legacy_elbow = elbowBendTorso(shoulder, hand, outward_dir,
|
|
|
- along_frac, lateral_offset,
|
|
|
- y_bias, outward_sign);
|
|
|
-
|
|
|
+ QVector3D const legacy_elbow =
|
|
|
+ elbowBendTorso(shoulder, hand, outward_dir, along_frac, lateral_offset,
|
|
|
+ y_bias, outward_sign);
|
|
|
+
|
|
|
// New controller approach
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
- QVector3D const controller_elbow = controller.solveElbowIK(
|
|
|
- false, shoulder, hand, outward_dir, along_frac, lateral_offset, y_bias,
|
|
|
- outward_sign);
|
|
|
-
|
|
|
+ QVector3D const controller_elbow =
|
|
|
+ controller.solveElbowIK(false, shoulder, hand, outward_dir, along_frac,
|
|
|
+ lateral_offset, y_bias, outward_sign);
|
|
|
+
|
|
|
// Should be identical
|
|
|
EXPECT_TRUE(approxEqual(legacy_elbow, controller_elbow, 0.001F))
|
|
|
<< "Legacy: " << legacy_elbow.x() << ", " << legacy_elbow.y() << ", "
|
|
|
@@ -76,12 +76,12 @@ TEST_F(PoseControllerCompatibilityTest, ElbowIKMatchesLegacyFunction) {
|
|
|
|
|
|
TEST_F(PoseControllerCompatibilityTest, PlaceHandAtUsesCorrectElbowIK) {
|
|
|
// Verify that placeHandAt uses the same IK as direct manipulation
|
|
|
-
|
|
|
+
|
|
|
// Create a copy for legacy approach
|
|
|
HumanoidPose legacy_pose = pose;
|
|
|
-
|
|
|
+
|
|
|
QVector3D const target_hand(0.30F, 1.20F, 0.80F);
|
|
|
-
|
|
|
+
|
|
|
// Legacy approach: manual IK
|
|
|
legacy_pose.hand_r = target_hand;
|
|
|
QVector3D right_axis = legacy_pose.shoulderR - legacy_pose.shoulderL;
|
|
|
@@ -90,15 +90,16 @@ TEST_F(PoseControllerCompatibilityTest, PlaceHandAtUsesCorrectElbowIK) {
|
|
|
QVector3D const outward_r = right_axis;
|
|
|
legacy_pose.elbowR = elbowBendTorso(legacy_pose.shoulderR, target_hand,
|
|
|
outward_r, 0.48F, 0.12F, 0.02F, 1.0F);
|
|
|
-
|
|
|
+
|
|
|
// New controller approach
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
controller.placeHandAt(false, target_hand);
|
|
|
-
|
|
|
+
|
|
|
// Hand should be at target
|
|
|
EXPECT_TRUE(approxEqual(pose.hand_r, target_hand, 0.001F));
|
|
|
-
|
|
|
- // Elbow should be very similar (minor differences due to internal calculations)
|
|
|
+
|
|
|
+ // Elbow should be very similar (minor differences due to internal
|
|
|
+ // calculations)
|
|
|
EXPECT_TRUE(approxEqual(pose.elbowR, legacy_pose.elbowR, 0.05F))
|
|
|
<< "Legacy elbow: " << legacy_pose.elbowR.x() << ", "
|
|
|
<< legacy_pose.elbowR.y() << ", " << legacy_pose.elbowR.z() << "\n"
|
|
|
@@ -108,16 +109,16 @@ TEST_F(PoseControllerCompatibilityTest, PlaceHandAtUsesCorrectElbowIK) {
|
|
|
|
|
|
TEST_F(PoseControllerCompatibilityTest, KneeIKHandlesExtremeCases) {
|
|
|
// Test knee IK with extreme cases to verify robustness
|
|
|
-
|
|
|
+
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
-
|
|
|
+
|
|
|
// Very short distance (hip very close to foot)
|
|
|
QVector3D const hip1(0.0F, 0.50F, 0.0F);
|
|
|
QVector3D const foot1(0.05F, 0.45F, 0.05F);
|
|
|
QVector3D const knee1 = controller.solveKneeIK(true, hip1, foot1, 1.0F);
|
|
|
EXPECT_GE(knee1.y(), HumanProportions::GROUND_Y);
|
|
|
EXPECT_LE(knee1.y(), hip1.y());
|
|
|
-
|
|
|
+
|
|
|
// Maximum reach (foot very far from hip)
|
|
|
QVector3D const hip2(0.0F, 1.00F, 0.0F);
|
|
|
QVector3D const foot2(0.80F, 0.0F, 0.80F);
|
|
|
@@ -126,11 +127,13 @@ TEST_F(PoseControllerCompatibilityTest, KneeIKHandlesExtremeCases) {
|
|
|
EXPECT_LE(knee2.y(), hip2.y());
|
|
|
}
|
|
|
|
|
|
-TEST_F(PoseControllerCompatibilityTest, KneelProducesSimilarPoseToExistingCode) {
|
|
|
+TEST_F(PoseControllerCompatibilityTest,
|
|
|
+ KneelProducesSimilarPoseToExistingCode) {
|
|
|
// Compare kneel() result with typical hand-coded kneeling pose
|
|
|
using HP = HumanProportions;
|
|
|
-
|
|
|
- // Create a reference pose with manual kneeling (similar to archer_renderer.cpp)
|
|
|
+
|
|
|
+ // Create a reference pose with manual kneeling (similar to
|
|
|
+ // archer_renderer.cpp)
|
|
|
HumanoidPose reference_pose = pose;
|
|
|
float const kneel_depth = 0.45F;
|
|
|
float const pelvis_y = HP::WAIST_Y - kneel_depth;
|
|
|
@@ -138,42 +141,45 @@ TEST_F(PoseControllerCompatibilityTest, KneelProducesSimilarPoseToExistingCode)
|
|
|
reference_pose.shoulderL.setY(HP::SHOULDER_Y - kneel_depth);
|
|
|
reference_pose.shoulderR.setY(HP::SHOULDER_Y - kneel_depth);
|
|
|
reference_pose.neck_base.setY(HP::NECK_BASE_Y - kneel_depth);
|
|
|
- reference_pose.headPos.setY((HP::HEAD_TOP_Y + HP::CHIN_Y) * 0.5F - kneel_depth);
|
|
|
-
|
|
|
+ reference_pose.headPos.setY((HP::HEAD_TOP_Y + HP::CHIN_Y) * 0.5F -
|
|
|
+ kneel_depth);
|
|
|
+
|
|
|
// Use controller to kneel
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
controller.kneel(1.0F); // Full kneel
|
|
|
-
|
|
|
+
|
|
|
// Should be similar (allowing for controller's specific implementation)
|
|
|
EXPECT_NEAR(pose.pelvisPos.y(), reference_pose.pelvisPos.y(), 0.10F);
|
|
|
EXPECT_LT(pose.shoulderL.y(), HP::SHOULDER_Y); // Shoulders lowered
|
|
|
EXPECT_LT(pose.shoulderR.y(), HP::SHOULDER_Y);
|
|
|
}
|
|
|
|
|
|
-TEST_F(PoseControllerCompatibilityTest, LeanProducesReasonableUpperBodyDisplacement) {
|
|
|
+TEST_F(PoseControllerCompatibilityTest,
|
|
|
+ LeanProducesReasonableUpperBodyDisplacement) {
|
|
|
// Test that lean produces sensible displacement
|
|
|
using HP = HumanProportions;
|
|
|
-
|
|
|
+
|
|
|
QVector3D const original_shoulder_l = pose.shoulderL;
|
|
|
QVector3D const original_shoulder_r = pose.shoulderR;
|
|
|
QVector3D const original_head = pose.headPos;
|
|
|
-
|
|
|
+
|
|
|
QVector3D const lean_dir(0.0F, 0.0F, 1.0F); // Forward
|
|
|
float const lean_amount = 0.8F;
|
|
|
-
|
|
|
+
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
controller.lean(lean_dir, lean_amount);
|
|
|
-
|
|
|
+
|
|
|
// Shoulders should move forward
|
|
|
EXPECT_GT(pose.shoulderL.z(), original_shoulder_l.z());
|
|
|
EXPECT_GT(pose.shoulderR.z(), original_shoulder_r.z());
|
|
|
-
|
|
|
+
|
|
|
// Head should move forward but less than shoulders
|
|
|
EXPECT_GT(pose.headPos.z(), original_head.z());
|
|
|
- float const shoulder_displacement = pose.shoulderL.z() - original_shoulder_l.z();
|
|
|
+ float const shoulder_displacement =
|
|
|
+ pose.shoulderL.z() - original_shoulder_l.z();
|
|
|
float const head_displacement = pose.headPos.z() - original_head.z();
|
|
|
EXPECT_LT(head_displacement, shoulder_displacement);
|
|
|
-
|
|
|
+
|
|
|
// Displacement should be proportional to lean amount
|
|
|
float const expected_magnitude = 0.12F * lean_amount;
|
|
|
EXPECT_NEAR(shoulder_displacement, expected_magnitude, 0.02F);
|
|
|
@@ -182,43 +188,46 @@ TEST_F(PoseControllerCompatibilityTest, LeanProducesReasonableUpperBodyDisplacem
|
|
|
TEST_F(PoseControllerCompatibilityTest, CanRecreateBowAimingPose) {
|
|
|
// Recreate a typical bow aiming pose using the controller
|
|
|
using HP = HumanProportions;
|
|
|
-
|
|
|
+
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
-
|
|
|
+
|
|
|
// Archer kneel and aim
|
|
|
controller.kneel(1.0F);
|
|
|
controller.lean(QVector3D(0.0F, 0.0F, 1.0F), 0.2F); // Slight forward lean
|
|
|
-
|
|
|
+
|
|
|
// Position hands for bow
|
|
|
float const lowered_shoulder_y = pose.shoulderL.y();
|
|
|
- controller.placeHandAt(true, QVector3D(-0.15F, lowered_shoulder_y + 0.30F, 0.55F));
|
|
|
- controller.placeHandAt(false, QVector3D(0.12F, pose.shoulderR.y() + 0.15F, 0.10F));
|
|
|
-
|
|
|
+ controller.placeHandAt(true,
|
|
|
+ QVector3D(-0.15F, lowered_shoulder_y + 0.30F, 0.55F));
|
|
|
+ controller.placeHandAt(false,
|
|
|
+ QVector3D(0.12F, pose.shoulderR.y() + 0.15F, 0.10F));
|
|
|
+
|
|
|
// Verify pose is in a reasonable configuration
|
|
|
- EXPECT_LT(pose.pelvisPos.y(), HP::WAIST_Y); // Kneeling
|
|
|
+ EXPECT_LT(pose.pelvisPos.y(), HP::WAIST_Y); // Kneeling
|
|
|
EXPECT_GT(pose.handL.y(), pose.shoulderL.y()); // Left hand raised
|
|
|
- EXPECT_GT(pose.handL.z(), 0.0F); // Left hand forward
|
|
|
+ EXPECT_GT(pose.handL.z(), 0.0F); // Left hand forward
|
|
|
EXPECT_LT(pose.hand_r.z(), pose.handL.z()); // Right hand back (drawing bow)
|
|
|
}
|
|
|
|
|
|
TEST_F(PoseControllerCompatibilityTest, CanRecreateMeleeAttackPose) {
|
|
|
// Recreate a typical melee attack pose using the controller
|
|
|
using HP = HumanProportions;
|
|
|
-
|
|
|
+
|
|
|
HumanoidPoseController controller(pose, anim_ctx);
|
|
|
-
|
|
|
+
|
|
|
// Spearman thrust pose
|
|
|
controller.lean(QVector3D(0.0F, 0.0F, 1.0F), 0.5F); // Forward lean
|
|
|
-
|
|
|
+
|
|
|
// Thrust position
|
|
|
QVector3D const thrust_hand(0.32F, HP::SHOULDER_Y + 0.10F, 0.90F);
|
|
|
controller.placeHandAt(false, thrust_hand);
|
|
|
-
|
|
|
+
|
|
|
// Support hand
|
|
|
- controller.placeHandAt(true, QVector3D(-0.05F, HP::SHOULDER_Y + 0.03F, 0.53F));
|
|
|
-
|
|
|
+ controller.placeHandAt(true,
|
|
|
+ QVector3D(-0.05F, HP::SHOULDER_Y + 0.03F, 0.53F));
|
|
|
+
|
|
|
// Verify thrust pose characteristics
|
|
|
- EXPECT_GT(pose.hand_r.z(), 0.80F); // Hand extended forward
|
|
|
- EXPECT_GT(pose.shoulderL.z(), 0.0F); // Body leaning forward
|
|
|
+ EXPECT_GT(pose.hand_r.z(), 0.80F); // Hand extended forward
|
|
|
+ EXPECT_GT(pose.shoulderL.z(), 0.0F); // Body leaning forward
|
|
|
EXPECT_GT(pose.elbowR.z(), pose.shoulderR.z()); // Elbow extended
|
|
|
}
|