Optimize plane collision contacts for retargeting

cmake/build_variables.bzl

@@ -286,6 +286,7 @@ character_solver_public_headers = [
     "character_solver/model_parameters_error_function.h",
     "character_solver/normal_error_function.h",
     "character_solver/orientation_error_function.h",
+    "character_solver/plane_collision_error_function.h",
     "character_solver/plane_error_function.h",
     "character_solver/point_triangle_vertex_error_function.h",
     "character_solver/pose_prior_error_function.h",
@@ -332,6 +333,7 @@ character_solver_sources = [
     "character_solver/model_parameters_error_function.cpp",
     "character_solver/normal_error_function.cpp",
     "character_solver/orientation_error_function.cpp",
+    "character_solver/plane_collision_error_function.cpp",
     "character_solver/plane_error_function.cpp",
     "character_solver/point_triangle_vertex_error_function.cpp",
     "character_solver/pose_prior_error_function.cpp",
@@ -374,6 +376,7 @@ character_solver_test_sources = [
     "test/character_solver/limit_error_function_test.cpp",
     "test/character_solver/normal_error_function_test.cpp",
     "test/character_solver/orientation_error_function_test.cpp",
+    "test/character_solver/plane_collision_error_function_test.cpp",
     "test/character_solver/plane_error_function_test.cpp",
     "test/character_solver/point_triangle_vertex_error_function_test.cpp",
     "test/character_solver/position_error_function_test.cpp",

momentum/character/collision_geometry_state.cpp

@@ -13,10 +13,7 @@
 namespace momentum {
 
 template <typename T>
-void CollisionGeometryStateT<T>::update(
-    const SkeletonStateT<T>& skeletonState,
-    const CollisionGeometry& collisionGeometry) {
-  const size_t numElements = collisionGeometry.size();
+void CollisionGeometryStateT<T>::resize(const size_t numElements) {
   origin.resize(numElements);
   direction.resize(numElements);
   radius.resize(numElements);
@@ -25,44 +22,62 @@ void CollisionGeometryStateT<T>::update(
   orientation.resize(numElements);
   ellipsoidRadii.resize(numElements);
   boxHalfExtents.resize(numElements);
+}
 
-  // TODO: This per-primitive loop is independent across iterations and could be parallelized.
-  for (size_t i = 0; i < numElements; ++i) {
-    const auto& primitive = collisionGeometry[i];
-    const TransformT<T> parentTransform = (primitive.parent == kInvalidIndex)
-        ? TransformT<T>()
-        : skeletonState.jointState[primitive.parent].transform;
-    const TransformT<T> transform = parentTransform * primitive.transformation.cast<T>();
-    type[i] = primitive.type;
-    origin[i] = transform.translation;
-    orientation[i] = transform.rotation;
+template <typename T>
+void CollisionGeometryStateT<T>::updatePrimitive(
+    const SkeletonStateT<T>& skeletonState,
+    const CollisionGeometry& collisionGeometry,
+    const size_t primitiveIndex) {
+  const auto& primitive = collisionGeometry[primitiveIndex];
+  const TransformT<T> parentTransform = (primitive.parent == kInvalidIndex)
+      ? TransformT<T>()
+      : skeletonState.jointState[primitive.parent].transform;
+  const TransformT<T> transform = parentTransform * primitive.transformation.cast<T>();
+  type[primitiveIndex] = primitive.type;
+  origin[primitiveIndex] = transform.translation;
+  orientation[primitiveIndex] = transform.rotation;
+
+  if (primitive.type == CollisionPrimitiveType::TaperedCapsule) {
+    direction[primitiveIndex].noalias() = transform.getAxisX() * transform.scale * primitive.length;
+    radius[primitiveIndex].noalias() = primitive.radius.cast<T>() * parentTransform.scale;
+    delta[primitiveIndex] = radius[primitiveIndex][1] - radius[primitiveIndex][0];
+    ellipsoidRadii[primitiveIndex].setZero();
+    boxHalfExtents[primitiveIndex].setZero();
+    return;
+  }
 
-    if (primitive.type == CollisionPrimitiveType::TaperedCapsule) {
-      direction[i].noalias() = transform.getAxisX() * transform.scale * primitive.length;
-      radius[i].noalias() = primitive.radius.cast<T>() * parentTransform.scale;
-      delta[i] = radius[i][1] - radius[i][0];
-      ellipsoidRadii[i].setZero();
-      boxHalfExtents[i].setZero();
-      continue;
-    }
+  if (primitive.type == CollisionPrimitiveType::Ellipsoid) {
+    direction[primitiveIndex].setZero();
+    radius[primitiveIndex].setZero();
+    delta[primitiveIndex] = T(0);
+    ellipsoidRadii[primitiveIndex].noalias() =
+        primitive.ellipsoidRadii.cast<T>() * parentTransform.scale;
+    boxHalfExtents[primitiveIndex].setZero();
+    return;
+  }
 
-    if (primitive.type == CollisionPrimitiveType::Ellipsoid) {
-      direction[i].setZero();
-      radius[i].setZero();
-      delta[i] = T(0);
-      ellipsoidRadii[i].noalias() = primitive.ellipsoidRadii.cast<T>() * parentTransform.scale;
-      boxHalfExtents[i].setZero();
-      continue;
-    }
+  if (primitive.type == CollisionPrimitiveType::Box) {
+    direction[primitiveIndex].setZero();
+    radius[primitiveIndex].setZero();
+    delta[primitiveIndex] = T(0);
+    ellipsoidRadii[primitiveIndex].setZero();
+    boxHalfExtents[primitiveIndex].noalias() =
+        primitive.boxHalfExtents.cast<T>() * parentTransform.scale;
+    return;
+  }
+}
 
-    if (primitive.type == CollisionPrimitiveType::Box) {
-      direction[i].setZero();
-      radius[i].setZero();
-      delta[i] = T(0);
-      ellipsoidRadii[i].setZero();
-      boxHalfExtents[i].noalias() = primitive.boxHalfExtents.cast<T>() * parentTransform.scale;
-      continue;
-    }
+template <typename T>
+void CollisionGeometryStateT<T>::update(
+    const SkeletonStateT<T>& skeletonState,
+    const CollisionGeometry& collisionGeometry) {
+  const size_t numElements = collisionGeometry.size();
+  resize(numElements);
+
+  // TODO: This per-primitive loop is independent across iterations and could be parallelized.
+  for (size_t i = 0; i < numElements; ++i) {
+    updatePrimitive(skeletonState, collisionGeometry, i);
   }
 }
 

momentum/character/collision_geometry_state.h

@@ -60,6 +60,15 @@ struct CollisionGeometryStateT {
   /// Scaled box half extents in global coordinates.
   std::vector<Vector3<T>> boxHalfExtents;
 
+  /// Resizes all per-primitive state arrays to hold @p numElements entries.
+  void resize(size_t numElements);
+
+  /// Updates one primitive state from the given skeleton state and collision geometry.
+  void updatePrimitive(
+      const SkeletonStateT<T>& skeletonState,
+      const CollisionGeometry& collisionGeometry,
+      size_t primitiveIndex);
+
   /// Updates the state based on a given skeleton state and collision geometry.
   void update(const SkeletonStateT<T>& skeletonState, const CollisionGeometry& collisionGeometry);
 };

momentum/character_solver/fwd.h

@@ -826,6 +826,25 @@ using OrientationRotDiffErrorFunctiond_const_u =
 using OrientationRotDiffErrorFunctiond_const_w =
     ::std::weak_ptr<const OrientationRotDiffErrorFunctiond>;
 
+template <typename T>
+class PlaneCollisionErrorFunctionT;
+using PlaneCollisionErrorFunction = PlaneCollisionErrorFunctionT<float>;
+using PlaneCollisionErrorFunctiond = PlaneCollisionErrorFunctionT<double>;
+
+using PlaneCollisionErrorFunction_p = ::std::shared_ptr<PlaneCollisionErrorFunction>;
+using PlaneCollisionErrorFunction_u = ::std::unique_ptr<PlaneCollisionErrorFunction>;
+using PlaneCollisionErrorFunction_w = ::std::weak_ptr<PlaneCollisionErrorFunction>;
+using PlaneCollisionErrorFunction_const_p = ::std::shared_ptr<const PlaneCollisionErrorFunction>;
+using PlaneCollisionErrorFunction_const_u = ::std::unique_ptr<const PlaneCollisionErrorFunction>;
+using PlaneCollisionErrorFunction_const_w = ::std::weak_ptr<const PlaneCollisionErrorFunction>;
+
+using PlaneCollisionErrorFunctiond_p = ::std::shared_ptr<PlaneCollisionErrorFunctiond>;
+using PlaneCollisionErrorFunctiond_u = ::std::unique_ptr<PlaneCollisionErrorFunctiond>;
+using PlaneCollisionErrorFunctiond_w = ::std::weak_ptr<PlaneCollisionErrorFunctiond>;
+using PlaneCollisionErrorFunctiond_const_p = ::std::shared_ptr<const PlaneCollisionErrorFunctiond>;
+using PlaneCollisionErrorFunctiond_const_u = ::std::unique_ptr<const PlaneCollisionErrorFunctiond>;
+using PlaneCollisionErrorFunctiond_const_w = ::std::weak_ptr<const PlaneCollisionErrorFunctiond>;
+
 template <typename T>
 class PlaneErrorFunctionT;
 using PlaneErrorFunction = PlaneErrorFunctionT<float>;