forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
101 lines
4.2 KiB
101 lines
4.2 KiB
#pragma once
|
|
|
|
#include <functional>
|
|
#include <vector>
|
|
|
|
#include "drake/common/eigen_types.h"
|
|
#include "drake/multibody/triangle_quadrature/triangle_quadrature_rule.h"
|
|
|
|
namespace drake {
|
|
namespace multibody {
|
|
|
|
/// A class for integrating a function using numerical quadrature over
|
|
/// triangular domains.
|
|
///
|
|
/// @tparam NumericReturnType the output type of the function being integrated.
|
|
/// Commonly will be a IEEE floating point scalar (e.g., `double`), but
|
|
/// could be an Eigen::VectorXd, a multibody::SpatialForce, or any
|
|
/// other numeric type that supports both scalar multiplication
|
|
/// (i.e., operator*(const NumericReturnType&, double) and addition with
|
|
/// another of the same type (i.e.,
|
|
/// operator+(const NumericReturnType&, const NumericReturnType&)).
|
|
/// @tparam T the scalar type of the function being integrated over. Supported
|
|
/// types are currently only IEEE floating point scalars.
|
|
template <typename NumericReturnType, typename T>
|
|
class TriangleQuadrature {
|
|
public:
|
|
/// Numerically integrates the function f over a triangle using the given
|
|
/// quadrature rule and the initial value.
|
|
/// @param f(p) a function that returns a numerical value for point p in the
|
|
/// domain of the triangle, specified in barycentric coordinates.
|
|
/// The barycentric coordinates are given by
|
|
/// (p[0], p[1], 1 - p[0] - p[1]).
|
|
/// @param area the area of the triangle.
|
|
static NumericReturnType Integrate(
|
|
const std::function<NumericReturnType(const Vector2<T>&)>& f,
|
|
const TriangleQuadratureRule& rule,
|
|
const T& area);
|
|
|
|
/// Alternative signature for Integrate() that uses three-dimensional
|
|
/// barycentric coordinates.
|
|
/// @param f(p) a function that returns a numerical value for point p in the
|
|
/// domain of the triangle, specified in barycentric coordinates.
|
|
/// The barycentric coordinates are given by
|
|
/// (p[0], p[1], p[2]).
|
|
/// @param area the area of the triangle.
|
|
static NumericReturnType Integrate(
|
|
const std::function<NumericReturnType(const Vector3<T>&)>& f,
|
|
const TriangleQuadratureRule& rule,
|
|
const T& area);
|
|
};
|
|
|
|
template <typename NumericReturnType, typename T>
|
|
NumericReturnType TriangleQuadrature<NumericReturnType, T>::Integrate(
|
|
const std::function<NumericReturnType(const Vector2<T>&)>& f,
|
|
const TriangleQuadratureRule& rule,
|
|
const T& area) {
|
|
// Get the quadrature points and weights.
|
|
const std::vector<Eigen::Vector2d>& barycentric_coordinates =
|
|
rule.quadrature_points();
|
|
const std::vector<double>& weights = rule.weights();
|
|
DRAKE_DEMAND(barycentric_coordinates.size() == weights.size());
|
|
DRAKE_DEMAND(weights.size() >= 1);
|
|
|
|
// Sum the weighted function evaluated at the transformed quadrature points.
|
|
// The looping is done in this particular way so that the return type can
|
|
// be either a traditional scalar (e.g., `double`), an Eigen Vector, or
|
|
// some other numeric type, without having to worry about the numerous
|
|
// possible ways to initialize to zero (e.g., `double integral = 0.0` vs.
|
|
// `Eigen::VectorXd = Eigen::VectorXd::Zero())` or having to know the
|
|
// dimension of the NumericReturnType (i.e., scalar or vector dimension).
|
|
NumericReturnType integral = f(barycentric_coordinates[0]) * weights[0];
|
|
for (int i = 1; i < static_cast<int>(weights.size()); ++i)
|
|
integral += f(barycentric_coordinates[i]) * weights[i];
|
|
|
|
return integral * area;
|
|
}
|
|
|
|
template <typename NumericReturnType, typename T>
|
|
NumericReturnType TriangleQuadrature<NumericReturnType, T>::Integrate(
|
|
const std::function<NumericReturnType(const Vector3<T>&)>& f,
|
|
const TriangleQuadratureRule& rule,
|
|
const T& area) {
|
|
// TODO(edrumwri) These composite functions might be slowing computation.
|
|
// Consider optimizing this.
|
|
|
|
// Create a function fprime accepting a 2D barycentric representation but
|
|
// using it to call the given 3D function f.
|
|
std::function<NumericReturnType(const Vector2<T>&)> fprime = [&f](
|
|
const Vector2<T>& barycentric_2D) {
|
|
return f(Vector3<T>(
|
|
barycentric_2D[0],
|
|
barycentric_2D[1],
|
|
T(1.0) - barycentric_2D[0] - barycentric_2D[1]));
|
|
};
|
|
|
|
return Integrate(fprime, rule, area);
|
|
}
|
|
|
|
} // namespace multibody
|
|
} // namespace drake
|