Merge pull request #10 from IMRCLab/issue9

Issue9
IMRCLab · Jun 6, 2023 · fe24ec9 · fe24ec9
2 parents 6b3806d + e46d7f5
commit fe24ec9
Showing 1 changed file with 22 additions and 2 deletions.
diff --git a/src/vicon.cpp b/src/vicon.cpp
@@ -69,8 +69,28 @@ namespace libmotioncapture {
     rigidBodies_.clear();
     size_t count = pImpl->client.GetSubjectCount().SubjectCount;
     for (size_t i = 0; i < count; ++i) {
-      const std::string name = pImpl->client.GetSubjectName(i).SubjectName;
-      rigidBodies_.emplace(name, rigidBodyByName(name));
+      auto const name = pImpl->client.GetSubjectName(i).SubjectName;
+      auto const translation = pImpl->client.GetSegmentGlobalTranslation(name, name);
+      auto const quaternion = pImpl->client.GetSegmentGlobalRotationQuaternion(name, name);
+      if (   translation.Result == Result::Success
+          && quaternion.Result == Result::Success
+          && !translation.Occluded
+          && !quaternion.Occluded) {
+
+        Eigen::Vector3f position(
+          translation.Translation[0] / 1000.0,
+          translation.Translation[1] / 1000.0,
+          translation.Translation[2] / 1000.0);
+
+        Eigen::Quaternionf rotation(
+          quaternion.Rotation[3], // w
+          quaternion.Rotation[0], // x
+          quaternion.Rotation[1], // y
+          quaternion.Rotation[2]  // z
+          );
+
+        rigidBodies_.emplace(name, RigidBody(name, position, rotation));
+      }
     }
     return rigidBodies_;
   }