diff --git a/kindred_solver/constraints.py b/kindred_solver/constraints.py index b9093c5..9c56875 100644 --- a/kindred_solver/constraints.py +++ b/kindred_solver/constraints.py @@ -196,8 +196,8 @@ class PointOnLineConstraint(ConstraintBase): p_i = self.body_i.world_point(*self.marker_i_pos) p_j = self.body_j.world_point(*self.marker_j_pos) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy = point_line_perp_components(p_i, p_j, z_j) - return [cx, cy] + cx, cy, cz = point_line_perp_components(p_i, p_j, z_j) + return [cx, cy, cz] class PointInPlaneConstraint(ConstraintBase): @@ -244,8 +244,9 @@ class ParallelConstraint(ConstraintBase): """Parallel axes — 2 DOF removed. marker Z-axes are parallel: z_i x z_j = 0. - 2 residuals from the cross product (only 2 of 3 components are - independent for unit vectors). + 3 cross-product residuals (rank 2 at the solution). Using all 3 + avoids a singularity when the axes lie in the XY plane, where + dropping cz would leave the constraint blind to yaw rotations. """ def __init__( @@ -264,7 +265,7 @@ class ParallelConstraint(ConstraintBase): z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) cx, cy, cz = cross3(z_i, z_j) - return [cx, cy] + return [cx, cy, cz] class PerpendicularConstraint(ConstraintBase): @@ -350,17 +351,17 @@ class ConcentricConstraint(ConstraintBase): self.distance = distance def residuals(self) -> List[Expr]: - # Parallel axes (2 residuals) + # Parallel axes (3 cross-product residuals, rank 2 at solution) z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy, _cz = cross3(z_i, z_j) + cx, cy, cz = cross3(z_i, z_j) # Point-on-line: marker_i origin on line through marker_j along z_j p_i = self.body_i.world_point(*self.marker_i_pos) p_j = self.body_j.world_point(*self.marker_j_pos) - lx, ly = point_line_perp_components(p_i, p_j, z_j) + lx, ly, lz = point_line_perp_components(p_i, p_j, z_j) - return [cx, cy, lx, ly] + return [cx, cy, cz, lx, ly, lz] class TangentConstraint(ConstraintBase): @@ -417,10 +418,10 @@ class PlanarConstraint(ConstraintBase): self.offset = offset def residuals(self) -> List[Expr]: - # Parallel normals + # Parallel normals (3 cross-product residuals, rank 2 at solution) z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy, _cz = cross3(z_i, z_j) + cx, cy, cz = cross3(z_i, z_j) # Point-in-plane p_i = self.body_i.world_point(*self.marker_i_pos) @@ -429,7 +430,7 @@ class PlanarConstraint(ConstraintBase): if self.offset != 0.0: d = d - Const(self.offset) - return [cx, cy, d] + return [cx, cy, cz, d] class LineInPlaneConstraint(ConstraintBase): @@ -527,12 +528,12 @@ class RevoluteConstraint(ConstraintBase): p_j = self.body_j.world_point(*self.marker_j_pos) pos = [p_i[0] - p_j[0], p_i[1] - p_j[1], p_i[2] - p_j[2]] - # Parallel Z-axes + # Parallel Z-axes (3 cross-product residuals, rank 2 at solution) z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy, _cz = cross3(z_i, z_j) + cx, cy, cz = cross3(z_i, z_j) - return pos + [cx, cy] + return pos + [cx, cy, cz] class CylindricalConstraint(ConstraintBase): @@ -559,17 +560,17 @@ class CylindricalConstraint(ConstraintBase): self.marker_j_quat = marker_j_quat def residuals(self) -> List[Expr]: - # Parallel Z-axes + # Parallel Z-axes (3 cross-product residuals, rank 2 at solution) z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy, _cz = cross3(z_i, z_j) + cx, cy, cz = cross3(z_i, z_j) # Point-on-line p_i = self.body_i.world_point(*self.marker_i_pos) p_j = self.body_j.world_point(*self.marker_j_pos) - lx, ly = point_line_perp_components(p_i, p_j, z_j) + lx, ly, lz = point_line_perp_components(p_i, p_j, z_j) - return [cx, cy, lx, ly] + return [cx, cy, cz, lx, ly, lz] class SliderConstraint(ConstraintBase): @@ -598,22 +599,22 @@ class SliderConstraint(ConstraintBase): self.marker_j_quat = marker_j_quat def residuals(self) -> List[Expr]: - # Parallel Z-axes + # Parallel Z-axes (3 cross-product residuals, rank 2 at solution) z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy, _cz = cross3(z_i, z_j) + cx, cy, cz = cross3(z_i, z_j) # Point-on-line p_i = self.body_i.world_point(*self.marker_i_pos) p_j = self.body_j.world_point(*self.marker_j_pos) - lx, ly = point_line_perp_components(p_i, p_j, z_j) + lx, ly, lz = point_line_perp_components(p_i, p_j, z_j) # Rotation lock: x_i . y_j = 0 x_i = marker_x_axis(self.body_i, self.marker_i_quat) y_j = marker_y_axis(self.body_j, self.marker_j_quat) twist = dot3(x_i, y_j) - return [cx, cy, lx, ly, twist] + return [cx, cy, cz, lx, ly, lz, twist] class ScrewConstraint(ConstraintBase): @@ -648,14 +649,14 @@ class ScrewConstraint(ConstraintBase): self.pitch = pitch def residuals(self) -> List[Expr]: - # Cylindrical residuals (4) + # Cylindrical residuals (5: 3 parallel + 2 point-on-line) z_i = marker_z_axis(self.body_i, self.marker_i_quat) z_j = marker_z_axis(self.body_j, self.marker_j_quat) - cx, cy, _cz = cross3(z_i, z_j) + cx, cy, cz = cross3(z_i, z_j) p_i = self.body_i.world_point(*self.marker_i_pos) p_j = self.body_j.world_point(*self.marker_j_pos) - lx, ly = point_line_perp_components(p_i, p_j, z_j) + lx, ly, lz = point_line_perp_components(p_i, p_j, z_j) # Pitch coupling: axial_disp = pitch * angle / (2*pi) # Axial displacement @@ -684,7 +685,7 @@ class ScrewConstraint(ConstraintBase): # = axial - pitch * qz / pi coupling = axial - Const(self.pitch / math.pi) * rel[3] - return [cx, cy, lx, ly, coupling] + return [cx, cy, cz, lx, ly, lz, coupling] class UniversalConstraint(ConstraintBase): diff --git a/kindred_solver/geometry.py b/kindred_solver/geometry.py index 0365921..c82dd43 100644 --- a/kindred_solver/geometry.py +++ b/kindred_solver/geometry.py @@ -117,15 +117,15 @@ def point_line_perp_components( point: Vec3, line_origin: Vec3, line_dir: Vec3, -) -> tuple[Expr, Expr]: - """Two independent perpendicular-distance components from point to line. +) -> tuple[Expr, Expr, Expr]: + """Three perpendicular-distance components from point to line. The line passes through line_origin along line_dir. - Returns the x and y components of (point - line_origin) x line_dir, - which are zero when the point lies on the line. + Returns all 3 components of (point - line_origin) x line_dir, + which are zero when the point lies on the line. Only 2 of 3 are + independent, but using all 3 avoids a singularity when the line + direction aligns with a coordinate axis. """ d = sub3(point, line_origin) cx, cy, cz = cross3(d, line_dir) - # All three components of d x line_dir are zero when d is parallel - # to line_dir, but only 2 are independent. We return x and y. - return cx, cy + return cx, cy, cz diff --git a/tests/test_constraints_phase2.py b/tests/test_constraints_phase2.py index 210df3b..f48a039 100644 --- a/tests/test_constraints_phase2.py +++ b/tests/test_constraints_phase2.py @@ -65,7 +65,7 @@ class TestPointOnLine: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = PointOnLineConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) - assert len(c.residuals()) == 2 + assert len(c.residuals()) == 3 class TestPointInPlane: @@ -135,7 +135,7 @@ class TestParallel: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = ParallelConstraint(b1, ID_QUAT, b2, ID_QUAT) - assert len(c.residuals()) == 2 + assert len(c.residuals()) == 3 class TestPerpendicular: @@ -222,7 +222,7 @@ class TestConcentric: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = ConcentricConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) - assert len(c.residuals()) == 4 + assert len(c.residuals()) == 6 class TestTangent: @@ -279,7 +279,7 @@ class TestPlanar: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = PlanarConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) - assert len(c.residuals()) == 3 + assert len(c.residuals()) == 4 class TestLineInPlane: @@ -334,7 +334,7 @@ class TestRevolute: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = RevoluteConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) - assert len(c.residuals()) == 5 + assert len(c.residuals()) == 6 class TestCylindrical: @@ -353,7 +353,7 @@ class TestCylindrical: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = CylindricalConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) - assert len(c.residuals()) == 4 + assert len(c.residuals()) == 6 class TestSlider: @@ -375,15 +375,15 @@ class TestSlider: c = SliderConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) env = pt.get_env() vals = [r.eval(env) for r in c.residuals()] - # First 4 should be ~0 (parallel + on-line), but twist residual should be ~1 - assert abs(vals[4]) > 0.5 + # First 6 should be ~0 (parallel + on-line), but twist residual should be ~1 + assert abs(vals[6]) > 0.5 def test_residual_count(self): pt = ParamTable() b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = SliderConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT) - assert len(c.residuals()) == 5 + assert len(c.residuals()) == 7 class TestUniversal: @@ -411,7 +411,7 @@ class TestScrew: b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0)) b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0)) c = ScrewConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT, pitch=10.0) - assert len(c.residuals()) == 5 + assert len(c.residuals()) == 7 def test_zero_displacement_zero_rotation(self): """Both at origin with identity rotation → all residuals 0.""" diff --git a/tests/test_geometry.py b/tests/test_geometry.py index 45b90ba..fae6a71 100644 --- a/tests/test_geometry.py +++ b/tests/test_geometry.py @@ -173,15 +173,17 @@ class TestPointLinePerp: pt = (Const(0.0), Const(0.0), Const(5.0)) origin = (Const(0.0), Const(0.0), Const(0.0)) direction = (Const(0.0), Const(0.0), Const(1.0)) - cx, cy = point_line_perp_components(pt, origin, direction) + cx, cy, cz = point_line_perp_components(pt, origin, direction) assert abs(cx.eval({})) < 1e-10 assert abs(cy.eval({})) < 1e-10 + assert abs(cz.eval({})) < 1e-10 def test_off_line(self): pt = (Const(3.0), Const(0.0), Const(0.0)) origin = (Const(0.0), Const(0.0), Const(0.0)) direction = (Const(0.0), Const(0.0), Const(1.0)) - cx, cy = point_line_perp_components(pt, origin, direction) + cx, cy, cz = point_line_perp_components(pt, origin, direction) # d = (3,0,0), dir = (0,0,1), d x dir = (0*1-0*0, 0*0-3*1, 3*0-0*0) = (0,-3,0) assert abs(cx.eval({})) < 1e-10 assert abs(cy.eval({}) - (-3.0)) < 1e-10 + assert abs(cz.eval({})) < 1e-10 diff --git a/tests/test_joints.py b/tests/test_joints.py index 9b96757..8c5cb44 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -421,8 +421,10 @@ class TestSliderCrank: bodies = [ground, crank, rod, piston] dof = _dof(pt, bodies, constraints) - # 3D slider-crank: planar motion + out-of-plane fold modes - assert dof == 3 + # With full 3-component cross products, the redundant constraint rows + # eliminate the out-of-plane fold modes, giving the correct 1 DOF + # (crank rotation only). + assert dof == 1 def test_slider_crank_solves(self): """Slider-crank converges from displaced state."""