CollisionFunction.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tvm/Convex.h>
 
#include <tvm/function/abstract/Function.h>
 
#include <sch/CD/CD_Pair.h>
 
#include <RBDyn/Jacobian.h>
 
namespace mc_tvm
{
 
class MC_TVM_DLLAPI CollisionFunction : public tvm::function::abstract::Function
{
public:
  SET_UPDATES(CollisionFunction, Value, Velocity, Jacobian, NormalAcceleration)
 
  
  CollisionFunction(Convex & c1,
                    Convex & c2,
                    const Eigen::VectorXd & r1Selector,
                    const Eigen::VectorXd & r2Selector,
                    double dt);
 
  void tick();
 
  inline double distance() const noexcept { return this->value()(0); }
 
  inline const Convex & c1() const noexcept { return *c1_; }
 
  inline const Eigen::Vector3d & p1() const noexcept { return p1_; }
 
  inline const Convex & c2() const noexcept { return *c2_; }
 
  inline const Eigen::Vector3d & p2() const noexcept { return p2_; }
 
protected:
  /* Update functions */
  void updateValue();
  void updateVelocity();
  void updateJacobian();
  void updateNormalAcceleration();
 
  uint64_t iter_ = 0;
  uint64_t prevIter_ = 0;
 
  Convex * c1_;
  Convex * c2_;
  double dt_;
 
  Eigen::Vector3d p1_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d p2_ = Eigen::Vector3d::Zero();
 
  sch::CD_Pair pair_;
 
  struct ObjectData
  {
    Eigen::Vector3d nearestPoint_;
    rbd::Jacobian jac_;
    Eigen::VectorXd selector_;
  };
  std::vector<ObjectData> data_;
 
  Eigen::Vector3d normVecDist_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d prevNormVecDist_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d speedVec_ = Eigen::Vector3d::Zero();
 
  Eigen::MatrixXd fullJac_;
  Eigen::MatrixXd distJac_;
};
 
using CollisionFunctionPtr = std::shared_ptr<CollisionFunction>;
 
} // namespace mc_tvm