convex-shape.hh

Go to the documentation of this file.

// Copyright (c) 2015, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_HH
#define HPP_CONSTRAINTS_CONVEX_SHAPE_HH
 
#include <coal/shape/geometric_shapes.h>
 
#include <vector>
 
// Only for specialization of vector3_t. This is a bad design of Pinocchio.
#include <hpp/constraints/config.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/pinocchio/joint.hh>
#include <pinocchio/multibody/model.hpp>
 
namespace hpp {
namespace constraints {
inline void closestPointToSegment(const vector3_t& P, const vector3_t& A,
                                  const vector3_t& v, vector3_t& B) {
  vector3_t w = A - P;
  value_type c1, c2;
  c1 = v.dot(w);
  c2 = v.dot(v);
  if (c1 <= 0)
    B = A;
  else if (c2 <= c1)
    B = A + v;
  else
    B = A + c1 / c2 * v;
}
 
inline vector3_t linePlaneIntersection(const vector3_t& A, const vector3_t& u,
                                       const vector3_t& P, const vector3_t& n) {
  assert(std::abs(n.dot(u)) > 1e-8);
  return A + u * (n.dot(P - A)) / n.dot(u);
}
 
class HPP_CONSTRAINTS_DLLAPI ConvexShape {
 public:
  ConvexShape(const std::vector<vector3_t>& pts,
              JointPtr_t joint = JointPtr_t())
      : Pts_(pts), joint_(joint) {
    init();
  }
 
  ConvexShape(const coal::TriangleP& t, const JointPtr_t& joint = JointPtr_t())
      : Pts_(triangleToPoints(t)), joint_(joint) {
    init();
  }
 
  ConvexShape(const vector3_t& p0, const vector3_t& p1, const vector3_t& p2,
              const JointPtr_t& joint = JointPtr_t())
      : Pts_(points(p0, p1, p2)), joint_(joint) {
    init();
  }
 
  // Copy constructor
  ConvexShape(const ConvexShape& t) : Pts_(t.Pts_), joint_(t.joint_) { init(); }
 
  void reverse() {
    std::reverse(Pts_.begin(), Pts_.end());
    init();
  }
 
  inline vector3_t intersectionLocal(const vector3_t& A,
                                     const vector3_t& u) const {
    return linePlaneIntersection(A, u, C_, N_);
  }
 
  inline bool isInsideLocal(const vector3_t& Ap) const {
    assert(shapeDimension_ > 2);
    for (std::size_t i = 0; i < shapeDimension_; ++i) {
      if (Ns_[i].dot(Ap - Pts_[i]) > 0) return false;
    }
    return true;
  }
 
  inline value_type distanceLocal(const vector3_t& a) const {
    assert(shapeDimension_ > 1);
    const value_type inf = std::numeric_limits<value_type>::infinity();
    value_type minPosDist = inf, maxNegDist = -inf;
    bool outside = false;
    for (std::size_t i = 0; i < shapeDimension_; ++i) {
      value_type d = dist(a - Pts_[i], Ls_[i], Us_[i], Ns_[i]);
      if (d > 0) {
        outside = true;
        if (d < minPosDist) minPosDist = d;
      }
      if (d <= 0 && d > maxNegDist) maxNegDist = d;
    }
    if (outside) return minPosDist;
    return maxNegDist;
  }
 
  inline const vector3_t& planeXaxis() const {
    assert(shapeDimension_ > 2);
    return Ns_[0];
  }
  inline const vector3_t& planeYaxis() const {
    assert(shapeDimension_ > 2);
    return Us_[0];
  }
 
  inline const Transform3s& positionInJoint() const { return MinJoint_; }
 
  bool operator==(ConvexShape const& other) const {
    if (Pts_ != other.Pts_) return false;
    if (shapeDimension_ != other.shapeDimension_) return false;
    if (C_ != other.C_) return false;
    if (N_ != other.N_) return false;
    if (Ns_ != other.Ns_) return false;
    if (Us_ != other.Us_) return false;
    if (Ls_ != other.Ls_) return false;
    if (MinJoint_ != other.MinJoint_) return false;
    if (joint_ != other.joint_) return false;
    return true;
  }
  bool operator!=(ConvexShape const& other) const {
    return !(this->operator==(other));
  }
 
  std::vector<vector3_t> Pts_;
  size_t shapeDimension_;
  vector3_t C_;
  vector3_t N_;
  std::vector<vector3_t> Ns_, Us_;
  vector_t Ls_;
  Transform3s MinJoint_;
  JointPtr_t joint_;
 
 private:
  inline value_type dist(const vector3_t& w, const value_type& c2,
                         const vector3_t& v, const vector3_t& u) const {
    value_type c1;
    c1 = v.dot(w);
    if (c1 <= 0) return (u.dot(w) > 0) ? (w.norm()) : (-w.norm());
    if (c2 <= c1)
      // TODO: (w - c2 * v).norm() == sqrt((u.dot(w)**2 + (c1 - c2)**2)
      // second should be cheaper.
      return (u.dot(w) > 0) ? ((w - c2 * v).norm()) : (-(w - c2 * v).norm());
    return u.dot(w);
  }
 
  static std::vector<vector3_t> triangleToPoints(const coal::TriangleP& t) {
    // TODO
    // return points (t.a, t.b, t.c);
    std::vector<vector3_t> ret(3);
    ret[0] = t.a;
    ret[1] = t.b;
    ret[2] = t.c;
    return ret;
  }
  static std::vector<vector3_t> points(const vector3_t& p0, const vector3_t& p1,
                                       const vector3_t& p2) {
    std::vector<vector3_t> ret(3);
    ret[0] = p0;
    ret[1] = p1;
    ret[2] = p2;
    return ret;
  }
 
  void init() {
    shapeDimension_ = Pts_.size();
 
    switch (shapeDimension_) {
      case 0:
        throw std::logic_error("Cannot represent an empty shape.");
        break;
      case 1:
        C_ = Pts_[0];
        // The transformation will be (N_, Ns_[0], Us_[0])
        // Fill vectors so as to be consistent
        N_ = vector3_t(1, 0, 0);
        Ns_.push_back(vector3_t(0, 1, 0));
        Us_.push_back(vector3_t(0, 0, 1));
        break;
      case 2:
        Ls_ = vector_t(1);
        C_ = (Pts_[0] + Pts_[1]) / 2;
        // The transformation will be (N_, Ns_[0], Us_[0])
        // Fill vectors so as to be consistent
        Us_.push_back(Pts_[1] - Pts_[0]);
        Ls_[0] = Us_[0].norm();
        Us_[0].normalize();
        if (Us_[0][0] != 0)
          N_ = vector3_t(-Us_[0][1], Us_[0][0], 0);
        else
          N_ = vector3_t(0, -Us_[0][2], Us_[0][1]);
        N_.normalize();
        Ns_.push_back(Us_[0].cross(N_));
        Ns_[0].normalize();  // Should be unnecessary
        break;
      default:
        Ls_ = vector_t(shapeDimension_);
        C_.setZero();
        for (std::size_t i = 0; i < shapeDimension_; ++i) C_ += Pts_[i];
        // TODO This is very ugly. Why Eigen does not have the operator/=(int)
        // ...
        C_ /= (value_type)Pts_.size();
        N_ = (Pts_[1] - Pts_[0]).cross(Pts_[2] - Pts_[1]);
        assert(!N_.isZero());
        N_.normalize();
 
        Us_.resize(Pts_.size());
        Ns_.resize(Pts_.size());
        for (std::size_t i = 0; i < shapeDimension_; ++i) {
          Us_[i] = Pts_[(i + 1) % shapeDimension_] - Pts_[i];
          Ls_[i] = Us_[i].norm();
          Us_[i].normalize();
          Ns_[i] = Us_[i].cross(N_);
          Ns_[i].normalize();
        }
        for (std::size_t i = 0; i < shapeDimension_; ++i) {
          assert(Us_[(i + 1) % shapeDimension_].dot(Ns_[i]) < 0 &&
                 "The sequence does not define a convex surface");
        }
        break;
    }
 
    MinJoint_.translation() = C_;
    MinJoint_.rotation().col(0) = N_;
    MinJoint_.rotation().col(1) = Ns_[0];
    MinJoint_.rotation().col(2) = Us_[0];
  }
};
 
struct HPP_CONSTRAINTS_DLLAPI ConvexShapeData {
  // normal in the world frame
  vector3_t normal_;
  // center in the world frame
  vector3_t center_;
  // Current joint position
  Transform3s oMj_;
 
  inline void updateToCurrentTransform(const ConvexShape& cs) {
    if (cs.joint_ == NULL) {
      oMj_.setIdentity();
      _recompute<true>(cs);
    } else {
      oMj_ = cs.joint_->currentTransformation();
      _recompute<false>(cs);
    }
  }
 
  inline void updateToCurrentTransform(const ConvexShape& cs,
                                       const pinocchio::DeviceData& d) {
    if (cs.joint_ == NULL) {
      oMj_.setIdentity();
      _recompute<true>(cs);
    } else {
      oMj_ = cs.joint_->currentTransformation(d);
      _recompute<false>(cs);
    }
  }
 
  template <bool WorldFrame>
  inline void _recompute(const ConvexShape& cs) {
    if (WorldFrame) {
      center_ = cs.C_;
      normal_ = cs.N_;
    } else {
      center_ = oMj_.act(cs.C_);
      normal_ = oMj_.rotation() * cs.N_;
    }
  }
 
  inline vector3_t intersection(const vector3_t& A, const vector3_t& u) const {
    return linePlaneIntersection(A, u, center_, normal_);
  }
 
  inline bool isInside(const ConvexShape& cs, const vector3_t& A,
                       const vector3_t& u) const {
    return isInside(cs, intersection(A, u));
  }
  inline bool isInside(const ConvexShape& cs, const vector3_t& Ap) const {
    if (cs.joint_ == NULL) return cs.isInsideLocal(Ap);
    vector3_t Ap_loc = oMj_.actInv(Ap);
    return cs.isInsideLocal(Ap_loc);
  }
 
  inline Transform3s alignedPositionInJoint(const ConvexShape& cs,
                                            vector3_t yaxis) const {
    assert(cs.shapeDimension_ > 2);
    // Project vector onto the plane
    yaxis = oMj_.actInv(yaxis);
    vector3_t yproj = yaxis - yaxis.dot(cs.N_) * cs.N_;
    if (yproj.isZero())
      return cs.MinJoint_;
    else {
      Transform3s M;
      M.translation() = cs.C_;
      M.rotation().col(0) = cs.N_;
      M.rotation().col(1) = yaxis;
      M.rotation().col(2) = cs.N_.cross(yaxis);
      return M;
    }
  }
 
  inline value_type distance(const ConvexShape& cs, vector3_t a) const {
    if (cs.joint_ != NULL) a = oMj_.actInv(a);
    return cs.distanceLocal(a);
  }
};
}  // namespace constraints
}  // namespace hpp
 
#endif  //  HPP_CONSTRAINTS_CONVEX_SHAPE_HH