pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
triangular-matrix.hpp
1//
2// Copyright (c) 2022-2023 INRIA
3//
4
5#ifndef __pinocchio_math_triangular_matrix_hpp__
6#define __pinocchio_math_triangular_matrix_hpp__
7
8#include "pinocchio/macros.hpp"
9
10#include <Eigen/Dense>
11
12namespace pinocchio
13{
14
15 namespace internal
16 {
17 template<
18 Eigen::UpLoType info,
19 typename RhsMatrix,
20 typename Scalar = typename RhsMatrix::Scalar,
21 bool is_vector_at_compile_time = RhsMatrix::IsVectorAtCompileTime>
22 struct TriangularMatrixMatrixProduct
23 {
24 template<typename LhsMatrix, typename ResMat>
25 static void run(
26 const Eigen::MatrixBase<LhsMatrix> & lhs_mat,
27 const Eigen::MatrixBase<RhsMatrix> & rhs_vec,
28 const Eigen::MatrixBase<ResMat> & res)
29 {
30 res.const_cast_derived().col(0).noalias() =
31 lhs_mat.derived().template triangularView<info>() * rhs_vec.derived();
32 }
33 };
34
35 template<Eigen::UpLoType info, typename RhsMatrix, typename Scalar>
36 struct TriangularMatrixMatrixProduct<info, RhsMatrix, Scalar, false>
37 {
38 template<typename LhsMatrix, typename ResMat>
39 static void run(
40 const Eigen::MatrixBase<LhsMatrix> & lhs_mat,
41 const Eigen::MatrixBase<RhsMatrix> & rhs_mat,
42 const Eigen::MatrixBase<ResMat> & res)
43 {
44 res.const_cast_derived().noalias() =
45 lhs_mat.derived().template triangularView<info>() * rhs_mat.derived();
46 }
47 };
48 } // namespace internal
49
58 template<Eigen::UpLoType info, typename LhsMatrix, typename RhsMatrix, typename ResMat>
60 const Eigen::MatrixBase<LhsMatrix> & lhs_mat,
61 const Eigen::MatrixBase<RhsMatrix> & rhs_mat,
62 const Eigen::MatrixBase<ResMat> & res)
63 {
64 internal::TriangularMatrixMatrixProduct<info, RhsMatrix>::run(
65 lhs_mat.derived(), rhs_mat.derived(), res.const_cast_derived());
66 }
67
68} // namespace pinocchio
69
70#endif // #ifndef __pinocchio_math_triangular_matrix_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
void triangularMatrixMatrixProduct(const Eigen::MatrixBase< LhsMatrix > &lhs_mat, const Eigen::MatrixBase< RhsMatrix > &rhs_mat, const Eigen::MatrixBase< ResMat > &res)
Evaluate the product of a triangular matrix times a matrix. Eigen showing a bug at this level,...