gram-schmidt-orthonormalisation.hpp

//
// Copyright (c) 2024 INRIA
//

#ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__
#define __pinocchio_math_gram_schmidt_orthonormalisation_hpp__

#include "pinocchio/math/fwd.hpp"
#include <Eigen/Core>

namespace pinocchio
{
 template<typename MatrixType, typename VectorType>
 void orthonormalisation(
 const Eigen::MatrixBase<MatrixType> & basis, const Eigen::MatrixBase<VectorType> & vec_)
 {
 typedef typename VectorType::Scalar Scalar;
 VectorType & vec = vec_.const_cast_derived();

 PINOCCHIO_CHECK_ARGUMENT_SIZE(basis.rows(), vec.size());
 assert((basis.transpose() * basis).isIdentity() && "The input basis is not orthonormal.");

 for (Eigen::DenseIndex col_id = 0; col_id < basis.cols(); ++col_id)
 {
 const auto col = basis.col(col_id);
 const Scalar alpha = col.dot(vec);
 vec -= alpha * col;
 }

 assert((basis.transpose() * vec).isZero());
 }
} // namespace pinocchio

#endif // ifndef __pinocchio_math_gram_schmidt_orthonormalisation_hpp__