diff --git a/sample/addons/many_bone_ik/qcp.gd b/sample/addons/many_bone_ik/qcp.gd index 6a59e242..6ee8a109 100644 --- a/sample/addons/many_bone_ik/qcp.gd +++ b/sample/addons/many_bone_ik/qcp.gd @@ -37,8 +37,8 @@ extends RefCounted var eigenvector_precision: float -var target = PackedVector3Array() var moved = PackedVector3Array() +var target = PackedVector3Array() var weight = PackedFloat64Array() var transformation_calculated = false var inner_product_calculated = false @@ -66,22 +66,33 @@ var sum_xy_minus_yx: float = 0 var sum_xx_plus_yy: float = 0 var sum_xx_minus_yy: float = 0 +func weighted_superpose( + p_moved: PackedVector3Array, + p_target: PackedVector3Array, + p_weights: PackedFloat64Array, + translate: bool +) -> Quaternion: + _set_qcp(p_moved, p_target, p_weights, translate) + return get_rotation() + -func _init(p_evec_prec: float): - self.eigenvector_precision = p_evec_prec +func get_translation() -> Vector3: + return target_center - moved_center func get_rotation() -> Quaternion: - var result: Quaternion if not transformation_calculated: if not inner_product_calculated: - inner_product(target, moved) - result = calculate_rotation() + _inner_product(target, moved) transformation_calculated = true - return result + return _calculate_rotation() + +func _init(p_evec_prec: float): + self.eigenvector_precision = p_evec_prec -func calculate_rotation() -> Quaternion: + +func _calculate_rotation() -> Quaternion: var result: Quaternion if moved.size() == 1: @@ -157,16 +168,12 @@ func calculate_rotation() -> Quaternion: return result -func translate(translation_vector: Vector3, x: PackedVector3Array) -> void: +func _translate(translation_vector: Vector3, x: PackedVector3Array) -> void: for i in range(x.size()): x[i] += translation_vector -func get_translation() -> Vector3: - return target_center - moved_center - - -func move_to_weighted_center(to_center: PackedVector3Array, weights: PackedFloat64Array) -> Vector3: +func _move_to_weighted_center(to_center: PackedVector3Array, weights: PackedFloat64Array) -> Vector3: var center: Vector3 = Vector3.ZERO var total_weight: float = 0 var size = to_center.size() @@ -188,7 +195,7 @@ func move_to_weighted_center(to_center: PackedVector3Array, weights: PackedFloat return center -func inner_product(coords1: PackedVector3Array, coords2: PackedVector3Array) -> void: +func _inner_product(coords1: PackedVector3Array, coords2: PackedVector3Array) -> void: var weighted_coord1: Vector3 var weighted_coord2: Vector3 var sum_of_squares1: float = 0.0 @@ -248,17 +255,7 @@ func inner_product(coords1: PackedVector3Array, coords2: PackedVector3Array) -> inner_product_calculated = true -func weighted_superpose( - p_moved: PackedVector3Array, - p_target: PackedVector3Array, - p_weights: PackedFloat64Array, - translate: bool -) -> Quaternion: - set_qcp(p_moved, p_target, p_weights, translate) - return get_rotation() - - -func set_qcp( +func _set_qcp( p_moved: PackedVector3Array, p_target: PackedVector3Array, p_weights: PackedFloat64Array, @@ -267,16 +264,16 @@ func set_qcp( transformation_calculated = false inner_product_calculated = false - moved = p_moved - target = p_target - weight = p_weights + moved = p_moved.duplicate() + target = p_target.duplicate() + weight = p_weights.duplicate() if p_translate: - moved_center = move_to_weighted_center(moved, weight) - target_center = move_to_weighted_center(target, weight) + moved_center = _move_to_weighted_center(moved, weight) + target_center = _move_to_weighted_center(target, weight) w_sum = 0 # Initialize to 0 so we don't double up - translate(-moved_center, moved) - translate(-target_center, target) + _translate(-moved_center, moved) + _translate(-target_center, target) else: if weight.size() > 0: w_sum = 0 diff --git a/sample/addons/many_bone_ik/tests/test_qcp.gd b/sample/addons/many_bone_ik/tests/test_qcp.gd index 70df6b4b..aa1afda2 100644 --- a/sample/addons/many_bone_ik/tests/test_qcp.gd +++ b/sample/addons/many_bone_ik/tests/test_qcp.gd @@ -58,41 +58,48 @@ func test_weighted_superpose_with_translation(): assert_almost_eq(translation_result.z, translation_vector.z, epsilon) -func rotate_target_headings_basis(tipHeadings: PackedVector3Array, targetHeadings: PackedVector3Array, basis: Basis): +func rotate_target_headings_basis( + tipHeadings: PackedVector3Array, targetHeadings: PackedVector3Array, basis: Basis +): for i in range(tipHeadings.size()): targetHeadings[i] = basis * tipHeadings[i] -func test_rotation(): +func test_get_rotation(): var qcp := qcp_const.new(epsilon) - var localizedTipHeadings := PackedVector3Array([ - Vector3(-14.739, -18.673, 15.040), - Vector3(-12.473, -15.810, 16.074), - Vector3(-14.802, -13.307, 14.408), - Vector3(-17.782, -14.852, 16.171), - Vector3(-16.124, -14.617, 19.584), - Vector3(-15.029, -11.037, 18.902), - Vector3(-18.577, -10.001, 17.996) - ]) - - var localizedTargetHeadings: PackedVector3Array = localizedTipHeadings.duplicate() - var originalTargetHeadings: PackedVector3Array = localizedTargetHeadings.duplicate() - - var basis_x := Basis(Quaternion(Vector3(1.0, 0.0, 0.0), PI / 2.0)) - var basis_y := Basis(Quaternion(Vector3(0.0, 1., 0.0), PI / 2.0)) - var basis_z := Basis(Quaternion(Vector3(0.0, 0.0, 1.), PI / 2.0)) - - gut.p("Before rotation around x-axis: %s" % localizedTargetHeadings) - rotate_target_headings_basis(localizedTipHeadings, localizedTargetHeadings, basis_x) - gut.p("After rotation around x-axis: %s" % localizedTargetHeadings) - assert_ne_deep(localizedTargetHeadings, originalTargetHeadings) - - gut.p("Before rotation around y-axis: %s" % localizedTargetHeadings) - rotate_target_headings_basis(localizedTipHeadings, localizedTargetHeadings, basis_y) - gut.p("After rotation around y-axis: %s" % localizedTargetHeadings) - assert_ne_deep(localizedTargetHeadings, originalTargetHeadings) - - gut.p("Before rotation around z-axis: %s" % localizedTargetHeadings) - rotate_target_headings_basis(localizedTipHeadings, localizedTargetHeadings, basis_z) - gut.p("After rotation around z-axis: %s" % localizedTargetHeadings) - assert_ne_deep(localizedTargetHeadings, originalTargetHeadings) + # Arrange + var moved = PackedVector3Array([Vector3(0, 1, 0)]) + var expected_rotation = Quaternion(0.3826834323650898, 0, 0, 0.9238795325112867) + var target = PackedVector3Array([expected_rotation * Vector3(1, 0, 0)]) + gut.p("Expected Rotation: %s" % str(expected_rotation)) + gut.p("Target: %s" % target[0]) + qcp.weighted_superpose(moved, target, PackedFloat64Array([1, 1, 1]), false) + + # Act + var result: Quaternion = qcp.get_rotation() + + # Log the result + gut.p("Result of calculate_rotation: %s" % result) + + # Assert + assert_eq(result, expected_rotation) + +func test_get_rotation_xform(): + # Arrange + var moved = [Vector3(0, 1, 0)] + var expected_rotation = Quaternion(0.3826834323650898, 0, 0, 0.9238795325112867) + var expected_vector = Vector3(1, 0, 0) + var target = expected_rotation * expected_vector + gut.p("Expected Rotation: %s" % str(expected_rotation)) + gut.p("Target: %s" % target[0]) + + # Act + var result = expected_rotation.inverse() * target + + # Log the result + gut.p("Result of calculate_rotation: %s" % result) + + # Assert + assert_eq(result, expected_vector) + +