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

// clang-format off
#include <pinocchio/multibody/model.hpp>
// clang-format on

#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>

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