feat: arm now checks all joints for collisions

MotionCore/Arm.cpp

@@ -76,15 +76,16 @@ bool Arm::isMoving() const noexcept
 	return false;
 }
 
-bool Arm::isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept
+bool Arm::checkJointCollisions(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept
 {
-	// Otherwise: check for endeffector position
-	const Vector3d ef = joints.back().getTargetedGlobalEndpoint();
-
+	// TODO: Perform this check within the FK loop to prevent redundant operations
 	// If any deadzone intersects with the EF pos, fai
 	for (const AxisAlignedBoundingBox& aabb : deadzones) {
-		if (aabb.doesIntersect(ef)) {
-			return false;
+
+		for (auto& joint : joints) {
+			if (aabb.doesIntersect(joint.getTargetedGlobalEndpoint())) {
+				return false;
+			}
 		}
 	}
 

MotionCore/Arm.h

@@ -23,12 +23,13 @@ public:
 	void rollbackPose() noexcept;
 	bool isMoving() const noexcept;
 
-	// Will test if the end effectors current target (or staged) position is not within a collider
-	bool isEFPositionValid(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept;
+	// Will test if the joints target position is not within a collider
+	bool checkJointCollisions(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept;
 	// Will roll back individual joints if their angles exceed their limits. Returns true if no rollbacks were made.
 	bool rollbackJointsBasedOnValidRange() noexcept;
 
 	const std::array<ArmSegment, NUM_JOINTS>& getJoints() const { return joints; };
+	std::array<ArmSegment, NUM_JOINTS>& getJoints() { return joints; };
 	const ArmSegment& getEndEffectorJoint() const { return joints.back(); };
 
 	void update(double frametime);

MotionCore/Scene_JointSpaceControl.cpp

@@ -43,7 +43,7 @@ void Scene_JointSpaceControl::update(double frametime)
 
     // Stage the new pose and keep it if it is good
     arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos });
-    if (arm.isEFPositionValid(deadzones)) {
+    if (arm.checkJointCollisions(deadzones)) {
         // Roll back individual joints if their angles are out of range
         arm.rollbackJointsBasedOnValidRange();