Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/hpp-fcl
  • coal-library/coal
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 9309 deletions
include/fcl/knn/nearest_neighbors_GNAT.h deleted 100644 → 0
View file @ 12194e71
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author: Mark Moll */
#ifndef FCL_KNN_NEAREST_NEIGHBORS_GNAT_H
#define FCL_KNN_NEAREST_NEIGHBORS_GNAT_H
#include "fcl/knn/nearest_neighbors.h"
#include "fcl/knn/greedy_kcenters.h"
#include "fcl/exception.h"
#include <boost/unordered_set.hpp>
#include <queue>
#include <algorithm>
namespace fcl
{
/** \brief Geometric Near-neighbor Access Tree (GNAT), a data
structure for nearest neighbor search.
See:
S. Brin, "Near neighbor search in large metric spaces," in Proc. 21st
Conf. on Very Large Databases (VLDB), pp. 574V584, 1995.
*/
template<typename _T>
class NearestNeighborsGNAT : public NearestNeighbors<_T>
{
protected:
/// \cond IGNORE
// internally, we use a priority queue for nearest neighbors, paired
// with their distance to the query point
typedef std::pair<const _T*,double> DataDist;
struct DataDistCompare
{
bool operator()(const DataDist& d0, const DataDist& d1)
{
return d0.second < d1.second;
}
};
typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
// another internal data structure is a priority queue of nodes to
// check next for possible nearest neighbors
class Node;
typedef std::pair<Node*,double> NodeDist;
struct NodeDistCompare
{
bool operator()(const NodeDist& n0, const NodeDist& n1) const
{
return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
}
};
typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
/// \endcond
public:
NearestNeighborsGNAT(unsigned int degree = 4, unsigned int minDegree = 2,
unsigned int maxDegree = 6, unsigned int maxNumPtsPerLeaf = 50,
unsigned int removedCacheSize = 50, bool rebalancing = false)
: NearestNeighbors<_T>(), tree_(NULL), degree_(degree),
minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
removedCacheSize_(removedCacheSize)
{
}
virtual ~NearestNeighborsGNAT(void)
{
if (tree_)
delete tree_;
}
/// \brief Set the distance function to use
virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
{
NearestNeighbors<_T>::setDistanceFunction(distFun);
pivotSelector_.setDistanceFunction(distFun);
if (tree_)
rebuildDataStructure();
}
virtual void clear(void)
{
if (tree_)
{
delete tree_;
tree_ = NULL;
}
size_ = 0;
removed_.clear();
if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
rebuildSize_ = maxNumPtsPerLeaf_ * degree_;
}
virtual void add(const _T &data)
{
if (tree_)
{
if (isRemoved(data))
rebuildDataStructure();
tree_->add(*this, data);
}
else
{
tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
size_ = 1;
}
}
virtual void add(const std::vector<_T> &data)
{
if (tree_)
NearestNeighbors<_T>::add(data);
else if (data.size()>0)
{
tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
for (unsigned int i=1; i<data.size(); ++i)
tree_->data_.push_back(data[i]);
if (tree_->needToSplit(*this))
tree_->split(*this);
}
size_ += data.size();
}
/// \brief Rebuild the internal data structure.
void rebuildDataStructure()
{
std::vector<_T> lst;
list(lst);
clear();
add(lst);
}
/// \brief Remove data from the tree.
/// The element won't actually be removed immediately, but just marked
/// for removal in the removed_ cache. When the cache is full, the tree
/// will be rebuilt and the elements marked for removal will actually
/// be removed.
virtual bool remove(const _T &data)
{
if (!size_) return false;
NearQueue nbhQueue;
// find data in tree
bool isPivot = nearestKInternal(data, 1, nbhQueue);
if (*nbhQueue.top().first != data)
return false;
removed_.insert(nbhQueue.top().first);
size_--;
// if we removed a pivot or if the capacity of removed elements
// has been reached, we rebuild the entire GNAT
if (isPivot || removed_.size()>=removedCacheSize_)
rebuildDataStructure();
return true;
}
virtual _T nearest(const _T &data) const
{
if (size_)
{
std::vector<_T> nbh;
nearestK(data, 1, nbh);
if (!nbh.empty()) return nbh[0];
}
throw Exception("No elements found in nearest neighbors data structure");
}
virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
{
nbh.clear();
if (k == 0) return;
if (size_)
{
NearQueue nbhQueue;
nearestKInternal(data, k, nbhQueue);
postprocessNearest(nbhQueue, nbh);
}
}
virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
{
nbh.clear();
if (size_)
{
NearQueue nbhQueue;
nearestRInternal(data, radius, nbhQueue);
postprocessNearest(nbhQueue, nbh);
}
}
virtual std::size_t size(void) const
{
return size_;
}
virtual void list(std::vector<_T> &data) const
{
data.clear();
data.reserve(size());
if (tree_)
tree_->list(*this, data);
}
/// \brief Print a GNAT structure (mostly useful for debugging purposes).
friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
{
if (gnat.tree_)
{
out << *gnat.tree_;
if (!gnat.removed_.empty())
{
out << "Elements marked for removal:\n";
for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
it != gnat.removed_.end(); it++)
out << **it << '\t';
out << std::endl;
}
}
return out;
}
// for debugging purposes
void integrityCheck()
{
std::vector<_T> lst;
boost::unordered_set<const _T*> tmp;
// get all elements, including those marked for removal
removed_.swap(tmp);
list(lst);
// check if every element marked for removal is also in the tree
for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
{
unsigned int i;
for (i=0; i<lst.size(); ++i)
if (lst[i]==**it)
break;
if (i == lst.size())
{
// an element marked for removal is not actually in the tree
std::cout << "***** FAIL!! ******\n" << *this << '\n';
for (unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<'\t';
std::cout<<std::endl;
}
assert(i != lst.size());
}
// restore
removed_.swap(tmp);
// get elements in the tree with elements marked for removal purged from the list
list(lst);
if (lst.size() != size_)
std::cout << "#########################################\n" << *this << std::endl;
assert(lst.size() == size_);
}
protected:
typedef NearestNeighborsGNAT<_T> GNAT;
/// Return true iff data has been marked for removal.
bool isRemoved(const _T& data) const
{
return !removed_.empty() && removed_.find(&data) != removed_.end();
}
/// \brief Return in nbhQueue the k nearest neighbors of data.
/// For k=1, return true if the nearest neighbor is a pivot.
/// (which is important during removal; removing pivots is a
/// special case).
bool nearestKInternal(const _T &data, std::size_t k, NearQueue& nbhQueue) const
{
bool isPivot;
double dist;
NodeDist nodeDist;
NodeQueue nodeQueue;
isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data,
NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
while (nodeQueue.size() > 0)
{
dist = nbhQueue.top().second; // note the difference with nearestRInternal
nodeDist = nodeQueue.top();
nodeQueue.pop();
if (nbhQueue.size() == k &&
(nodeDist.second > nodeDist.first->maxRadius_ + dist ||
nodeDist.second < nodeDist.first->minRadius_ - dist))
break;
nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
}
return isPivot;
}
/// \brief Return in nbhQueue the elements that are within distance radius of data.
void nearestRInternal(const _T &data, double radius, NearQueue& nbhQueue) const
{
double dist = radius; // note the difference with nearestKInternal
NodeQueue nodeQueue;
NodeDist nodeDist;
tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
while (nodeQueue.size() > 0)
{
nodeDist = nodeQueue.top();
nodeQueue.pop();
if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
nodeDist.second < nodeDist.first->minRadius_ - dist)
break;
nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
}
}
/// \brief Convert the internal data structure used for storing neighbors
/// to the vector that NearestNeighbor API requires.
void postprocessNearest(NearQueue& nbhQueue, std::vector<_T> &nbh) const
{
typename std::vector<_T>::reverse_iterator it;
nbh.resize(nbhQueue.size());
for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
*it = *nbhQueue.top().first;
}
/// The class used internally to define the GNAT.
class Node
{
public:
/// \brief Construct a node of given degree with at most
/// \e capacity data elements and wit given pivot.
Node(int degree, int capacity, const _T& pivot)
: degree_(degree), pivot_(pivot),
minRadius_(std::numeric_limits<double>::infinity()),
maxRadius_(-minRadius_), minRange_(degree, minRadius_),
maxRange_(degree, maxRadius_)
{
// The "+1" is needed because we add an element before we check whether to split
data_.reserve(capacity+1);
}
~Node()
{
for (unsigned int i=0; i<children_.size(); ++i)
delete children_[i];
}
/// \brief Update minRadius_ and maxRadius_, given that an element
/// was added with distance dist to the pivot.
void updateRadius(double dist)
{
if (minRadius_ > dist)
minRadius_ = dist;
if (maxRadius_ < dist)
maxRadius_ = dist;
}
/// \brief Update minRange_[i] and maxRange_[i], given that an
/// element was added to the i-th child of the parent that has
/// distance dist to this Node's pivot.
void updateRange(unsigned int i, double dist)
{
if (minRange_[i] > dist)
minRange_[i] = dist;
if (maxRange_[i] < dist)
maxRange_[i] = dist;
}
/// Add an element to the tree rooted at this node.
void add(GNAT& gnat, const _T& data)
{
if (children_.size()==0)
{
data_.push_back(data);
gnat.size_++;
if (needToSplit(gnat))
{
if (gnat.removed_.size() > 0)
gnat.rebuildDataStructure();
else if (gnat.size_ >= gnat.rebuildSize_)
{
gnat.rebuildSize_ <<= 1;
gnat.rebuildDataStructure();
}
else
split(gnat);
}
}
else
{
std::vector<double> dist(children_.size());
double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
int minInd = 0;
for (unsigned int i=1; i<children_.size(); ++i)
if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
{
minDist = dist[i];
minInd = i;
}
for (unsigned int i=0; i<children_.size(); ++i)
children_[i]->updateRange(minInd, dist[i]);
children_[minInd]->updateRadius(minDist);
children_[minInd]->add(gnat, data);
}
}
/// Return true iff the node needs to be split into child nodes.
bool needToSplit(const GNAT& gnat) const
{
unsigned int sz = data_.size();
return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
}
/// \brief The split operation finds pivot elements for the child
/// nodes and moves each data element of this node to the appropriate
/// child node.
void split(GNAT& gnat)
{
std::vector<std::vector<double> > dists;
std::vector<unsigned int> pivots;
children_.reserve(degree_);
gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
for(unsigned int i=0; i<pivots.size(); i++)
children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivots[i]]));
degree_ = pivots.size(); // in case fewer than degree_ pivots were found
for (unsigned int j=0; j<data_.size(); ++j)
{
unsigned int k = 0;
for (unsigned int i=1; i<degree_; ++i)
if (dists[j][i] < dists[j][k])
k = i;
Node* child = children_[k];
if (j != pivots[k])
{
child->data_.push_back(data_[j]);
child->updateRadius(dists[j][k]);
}
for (unsigned int i=0; i<degree_; ++i)
children_[i]->updateRange(k, dists[j][i]);
}
for (unsigned int i=0; i<degree_; ++i)
{
// make sure degree lies between minDegree_ and maxDegree_
children_[i]->degree_ = std::min(std::max(
degree_ * (unsigned int)(children_[i]->data_.size() / data_.size()),
gnat.minDegree_), gnat.maxDegree_);
// singleton
if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
}
// this does more than clear(); it also sets capacity to 0 and frees the memory
std::vector<_T> tmp;
data_.swap(tmp);
// check if new leaves need to be split
for (unsigned int i=0; i<degree_; ++i)
if (children_[i]->needToSplit(gnat))
children_[i]->split(gnat);
}
/// Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh.
bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const
{
if (nbh.size() < k)
{
nbh.push(std::make_pair(&data, dist));
return true;
}
else if (dist < nbh.top().second ||
(dist < std::numeric_limits<double>::epsilon() && data==key))
{
nbh.pop();
nbh.push(std::make_pair(&data, dist));
return true;
}
return false;
}
/// \brief Compute the k nearest neighbors of data in the tree.
/// For k=1, isPivot is true if the nearest neighbor is a pivot
/// (which is important during removal; removing pivots is a
/// special case). The nodeQueue, which contains other Nodes
/// that need to be checked for nearest neighbors, is updated.
void nearestK(const GNAT& gnat, const _T &data, std::size_t k,
NearQueue& nbh, NodeQueue& nodeQueue, bool& isPivot) const
{
for (unsigned int i=0; i<data_.size(); ++i)
if (!gnat.isRemoved(data_[i]))
{
if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
isPivot = false;
}
if (children_.size() > 0)
{
double dist;
Node* child;
std::vector<double> distToPivot(children_.size());
std::vector<int> permutation(children_.size());
for (unsigned int i=0; i<permutation.size(); ++i)
permutation[i] = i;
std::random_shuffle(permutation.begin(), permutation.end());
for (unsigned int i=0; i<children_.size(); ++i)
if (permutation[i] >= 0)
{
child = children_[permutation[i]];
distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
isPivot = true;
if (nbh.size()==k)
{
dist = nbh.top().second; // note difference with nearestR
for (unsigned int j=0; j<children_.size(); ++j)
if (permutation[j] >=0 && i != j &&
(distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
permutation[j] = -1;
}
}
dist = nbh.top().second;
for (unsigned int i=0; i<children_.size(); ++i)
if (permutation[i] >= 0)
{
child = children_[permutation[i]];
if (nbh.size()<k ||
(distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
distToPivot[permutation[i]] + dist >= child->minRadius_))
nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
}
}
}
/// Insert data in nbh if it is a near neighbor.
void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const
{
if (dist <= r)
nbh.push(std::make_pair(&data, dist));
}
/// \brief Return all elements that are within distance r in nbh.
/// The nodeQueue, which contains other Nodes that need to
/// be checked for nearest neighbors, is updated.
void nearestR(const GNAT& gnat, const _T &data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const
{
double dist = r; //note difference with nearestK
for (unsigned int i=0; i<data_.size(); ++i)
if (!gnat.isRemoved(data_[i]))
insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
if (children_.size() > 0)
{
Node* child;
std::vector<double> distToPivot(children_.size());
std::vector<int> permutation(children_.size());
for (unsigned int i=0; i<permutation.size(); ++i)
permutation[i] = i;
std::random_shuffle(permutation.begin(), permutation.end());
for (unsigned int i=0; i<children_.size(); ++i)
if (permutation[i] >= 0)
{
child = children_[permutation[i]];
distToPivot[i] = gnat.distFun_(data, child->pivot_);
insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
for (unsigned int j=0; j<children_.size(); ++j)
if (permutation[j] >=0 && i != j &&
(distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
distToPivot[i] + dist < child->minRange_[permutation[j]]))
permutation[j] = -1;
}
for (unsigned int i=0; i<children_.size(); ++i)
if (permutation[i] >= 0)
{
child = children_[permutation[i]];
if (distToPivot[i] - dist <= child->maxRadius_ &&
distToPivot[i] + dist >= child->minRadius_)
nodeQueue.push(std::make_pair(child, distToPivot[i]));
}
}
}
void list(const GNAT& gnat, std::vector<_T> &data) const
{
if (!gnat.isRemoved(pivot_))
data.push_back(pivot_);
for (unsigned int i=0; i<data_.size(); ++i)
if(!gnat.isRemoved(data_[i]))
data.push_back(data_[i]);
for (unsigned int i=0; i<children_.size(); ++i)
children_[i]->list(gnat, data);
}
friend std::ostream& operator<<(std::ostream& out, const Node& node)
{
out << "\ndegree:\t" << node.degree_;
out << "\nminRadius:\t" << node.minRadius_;
out << "\nmaxRadius:\t" << node.maxRadius_;
out << "\nminRange:\t";
for (unsigned int i=0; i<node.minRange_.size(); ++i)
out << node.minRange_[i] << '\t';
out << "\nmaxRange: ";
for (unsigned int i=0; i<node.maxRange_.size(); ++i)
out << node.maxRange_[i] << '\t';
out << "\npivot:\t" << node.pivot_;
out << "\ndata: ";
for (unsigned int i=0; i<node.data_.size(); ++i)
out << node.data_[i] << '\t';
out << "\nthis:\t" << &node;
out << "\nchildren:\n";
for (unsigned int i=0; i<node.children_.size(); ++i)
out << node.children_[i] << '\t';
out << '\n';
for (unsigned int i=0; i<node.children_.size(); ++i)
out << *node.children_[i] << '\n';
return out;
}
/// Number of child nodes
unsigned int degree_;
/// Data element stored in this Node
const _T pivot_;
/// Minimum distance between the pivot element and the elements stored in data_
double minRadius_;
/// Maximum distance between the pivot element and the elements stored in data_
double maxRadius_;
/// \brief The i-th element in minRange_ is the minimum distance between the
/// pivot and any data_ element in the i-th child node of this node's parent.
std::vector<double> minRange_;
/// \brief The i-th element in maxRange_ is the maximum distance between the
/// pivot and any data_ element in the i-th child node of this node's parent.
std::vector<double> maxRange_;
/// \brief The data elements stored in this node (in addition to the pivot
/// element). An internal node has no elements stored in data_.
std::vector<_T> data_;
/// \brief The child nodes of this node. By definition, only internal nodes
/// have child nodes.
std::vector<Node*> children_;
};
/// \brief The data structure containing the elements stored in this structure.
Node* tree_;
/// The desired degree of each node.
unsigned int degree_;
/// \brief After splitting a Node, each child Node has degree equal to
/// the default degree times the fraction of data elements from the
/// original node that got assigned to that child Node. However, its
/// degree can be no less than minDegree_.
unsigned int minDegree_;
/// \brief After splitting a Node, each child Node has degree equal to
/// the default degree times the fraction of data elements from the
/// original node that got assigned to that child Node. However, its
/// degree can be no larger than maxDegree_.
unsigned int maxDegree_;
/// \brief Maximum number of elements allowed to be stored in a Node before
/// it needs to be split into several nodes.
unsigned int maxNumPtsPerLeaf_;
/// \brief Number of elements stored in the tree.
std::size_t size_;
/// \brief If size_ exceeds rebuildSize_, the tree will be rebuilt (and
/// automatically rebalanced), and rebuildSize_ will be doubled.
std::size_t rebuildSize_;
/// \brief Maximum number of removed elements that can be stored in the
/// removed_ cache. If the cache is full, the tree will be rebuilt with
/// the elements in removed_ actually removed from the tree.
std::size_t removedCacheSize_;
/// \brief The data structure used to split data into subtrees.
GreedyKCenters<_T> pivotSelector_;
/// \brief Cache of removed elements.
boost::unordered_set<const _T*> removed_;
};
}
#endif
include/fcl/knn/nearest_neighbors_flann.h deleted 100644 → 0
View file @ 12194e71
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author: Mark Moll */
#ifndef FCL_KNN_NEAREST_NEIGHBORS_FLANN_H
#define FCL_KNN_NEAREST_NEIGHBORS_FLANN_H
#include "fcl/config.h"
#if FCL_HAVE_FLANN == 0
# error FLANN is not available. Please use a different NearestNeighbors data structure
#else
#include "fcl/knn/nearest_neighbors.h"
#include <flann/flann.hpp>
#include "fcl/exception.h"
namespace fcl
{
/** \brief Wrapper class to allow FLANN access to the
NearestNeighbors::distFun_ callback function
*/
template<typename _T>
class FLANNDistance
{
public:
typedef _T ElementType;
typedef double ResultType;
FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction& distFun)
: distFun_(distFun)
{
}
template <typename Iterator1, typename Iterator2>
ResultType operator()(Iterator1 a, Iterator2 b,
size_t /*size*/, ResultType /*worst_dist*/ = -1) const
{
return distFun_(*a, *b);
}
protected:
const typename NearestNeighbors<_T>::DistanceFunction& distFun_;
};
/** \brief Wrapper class for nearest neighbor data structures in the
FLANN library.
See:
M. Muja and D.G. Lowe, "Fast Approximate Nearest Neighbors with
Automatic Algorithm Configuration", in International Conference
on Computer Vision Theory and Applications (VISAPP'09), 2009.
http://people.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN
*/
template<typename _T, typename _Dist = FLANNDistance<_T> >
class NearestNeighborsFLANN : public NearestNeighbors<_T>
{
public:
NearestNeighborsFLANN(const boost::shared_ptr<flann::IndexParams> &params)
: index_(0), params_(params), searchParams_(32, 0., true), dimension_(1)
{
}
virtual ~NearestNeighborsFLANN(void)
{
if (index_)
delete index_;
}
virtual void clear(void)
{
if (index_)
{
delete index_;
index_ = NULL;
}
data_.clear();
}
virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
{
NearestNeighbors<_T>::setDistanceFunction(distFun);
rebuildIndex();
}
virtual void add(const _T &data)
{
bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
if (rebuild)
rebuildIndex(2 * data_.capacity());
data_.push_back(data);
const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
if (index_)
index_->addPoints(mat, std::numeric_limits<float>::max()/size());
else
createIndex(mat);
}
virtual void add(const std::vector<_T> &data)
{
unsigned int oldSize = data_.size();
unsigned int newSize = oldSize + data.size();
bool rebuild = index_ && (newSize > data_.capacity());
if (rebuild)
rebuildIndex(std::max(2 * oldSize, newSize));
if (index_)
{
std::copy(data.begin(), data.end(), data_.begin() + oldSize);
const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
index_->addPoints(mat, std::numeric_limits<float>::max()/size());
}
else
{
data_ = data;
const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
createIndex(mat);
}
}
virtual bool remove(const _T& data)
{
if (!index_) return false;
_T& elt = const_cast<_T&>(data);
const flann::Matrix<_T> query(&elt, 1, dimension_);
std::vector<std::vector<size_t> > indices(1);
std::vector<std::vector<double> > dists(1);
index_->knnSearch(query, indices, dists, 1, searchParams_);
if (*index_->getPoint(indices[0][0]) == data)
{
index_->removePoint(indices[0][0]);
rebuildIndex();
return true;
}
return false;
}
virtual _T nearest(const _T &data) const
{
if (size())
{
_T& elt = const_cast<_T&>(data);
const flann::Matrix<_T> query(&elt, 1, dimension_);
std::vector<std::vector<size_t> > indices(1);
std::vector<std::vector<double> > dists(1);
index_->knnSearch(query, indices, dists, 1, searchParams_);
return *index_->getPoint(indices[0][0]);
}
throw Exception("No elements found in nearest neighbors data structure");
}
virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
{
_T& elt = const_cast<_T&>(data);
const flann::Matrix<_T> query(&elt, 1, dimension_);
std::vector<std::vector<size_t> > indices;
std::vector<std::vector<double> > dists;
k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
nbh.resize(k);
for (std::size_t i = 0 ; i < k ; ++i)
nbh[i] = *index_->getPoint(indices[0][i]);
}
virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
{
_T& elt = const_cast<_T&>(data);
flann::Matrix<_T> query(&elt, 1, dimension_);
std::vector<std::vector<size_t> > indices;
std::vector<std::vector<double> > dists;
int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
nbh.resize(k);
for (int i = 0 ; i < k ; ++i)
nbh[i] = *index_->getPoint(indices[0][i]);
}
virtual std::size_t size(void) const
{
return index_ ? index_->size() : 0;
}
virtual void list(std::vector<_T> &data) const
{
std::size_t sz = size();
if (sz == 0)
{
data.resize(0);
return;
}
const _T& dummy = *index_->getPoint(0);
int checks = searchParams_.checks;
searchParams_.checks = size();
nearestK(dummy, sz, data);
searchParams_.checks = checks;
}
/// \brief Set the FLANN index parameters.
///
/// The parameters determine the type of nearest neighbor
/// data structure to be constructed.
virtual void setIndexParams(const boost::shared_ptr<flann::IndexParams> &params)
{
params_ = params;
rebuildIndex();
}
/// \brief Get the FLANN parameters used to build the current index.
virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams(void) const
{
return params_;
}
/// \brief Set the FLANN parameters to be used during nearest neighbor
/// searches
virtual void setSearchParams(const flann::SearchParams& searchParams)
{
searchParams_ = searchParams;
}
/// \brief Get the FLANN parameters used during nearest neighbor
/// searches
flann::SearchParams& getSearchParams(void)
{
return searchParams_;
}
/// \brief Get the FLANN parameters used during nearest neighbor
/// searches
const flann::SearchParams& getSearchParams(void) const
{
return searchParams_;
}
unsigned int getContainerSize(void) const
{
return dimension_;
}
protected:
/// \brief Internal function to construct nearest neighbor
/// data structure with initial elements stored in mat.
void createIndex(const flann::Matrix<_T>& mat)
{
index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
index_->buildIndex();
}
/// \brief Rebuild the nearest neighbor data structure (necessary when
/// changing the distance function or index parameters).
void rebuildIndex(unsigned int capacity = 0)
{
if (index_)
{
std::vector<_T> data;
list(data);
clear();
if (capacity)
data_.reserve(capacity);
add(data);
}
}
/// \brief vector of data stored in FLANN's index. FLANN only indexes
/// references, so we need store the original data.
std::vector<_T> data_;
/// \brief The FLANN index (the actual index type depends on params_).
flann::Index<_Dist>* index_;
/// \brief The FLANN index parameters. This contains both the type of
/// index and the parameters for that type.
boost::shared_ptr<flann::IndexParams> params_;
/// \brief The parameters used to seach for nearest neighbors.
mutable flann::SearchParams searchParams_;
/// \brief If each element has an array-like structure that is exposed
/// to FLANN, then the dimension_ needs to be set to the length of
/// this array.
unsigned int dimension_;
};
template<>
void NearestNeighborsFLANN<double, flann::L2<double> >::createIndex(
const flann::Matrix<double>& mat)
{
index_ = new flann::Index<flann::L2<double> >(mat, *params_);
index_->buildIndex();
}
template<typename _T, typename _Dist = FLANNDistance<_T> >
class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist>
{
public:
NearestNeighborsFLANNLinear()
: NearestNeighborsFLANN<_T, _Dist>(
boost::shared_ptr<flann::LinearIndexParams>(
new flann::LinearIndexParams()))
{
}
};
template<typename _T, typename _Dist = FLANNDistance<_T> >
class NearestNeighborsFLANNHierarchicalClustering : public NearestNeighborsFLANN<_T, _Dist>
{
public:
NearestNeighborsFLANNHierarchicalClustering()
: NearestNeighborsFLANN<_T, _Dist>(
boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
new flann::HierarchicalClusteringIndexParams()))
{
}
};
}
#endif
#endif
include/fcl/learning/classifier.h deleted 100644 → 0
View file @ 12194e71
#ifndef FCL_LEARNING_CLASSIFIER_H
#define FCL_LEARNING_CLASSIFIER_H
#include "fcl/math/vec_nf.h"
namespace fcl
{
template<std::size_t N>
struct Item
{
Vecnf<N> q;
bool label;
FCL_REAL w;
Item(const Vecnf<N>& q_, bool label_, FCL_REAL w_ = 1) : q(q_),
label(label_),
w(w_)
{}
Item() {}
};
template<std::size_t N>
struct Scaler
{
Vecnf<N> v_min, v_max;
Scaler()
{
// default no scale
for(std::size_t i = 0; i < N; ++i)
{
v_min[i] = 0;
v_max[i] = 1;
}
}
Scaler(const Vecnf<N>& v_min_, const Vecnf<N>& v_max_) : v_min(v_min_),
v_max(v_max_)
{}
Vecnf<N> scale(const Vecnf<N>& v) const
{
Vecnf<N> res;
for(std::size_t i = 0; i < N; ++i)
res[i] = (v[i] - v_min[i]) / (v_max[i] - v_min[i]);
return res;
}
Vecnf<N> unscale(const Vecnf<N>& v) const
{
Vecnf<N> res;
for(std::size_t i = 0; i < N; ++i)
res[i] = v[i] * (v_max[i] - v_min[i]) + v_min[i];
return res;
}
};
struct PredictResult
{
bool label;
FCL_REAL prob;
PredictResult() {}
PredictResult(bool label_, FCL_REAL prob_ = 1) : label(label_),
prob(prob_)
{}
};
template<std::size_t N>
class SVMClassifier
{
public:
virtual PredictResult predict(const Vecnf<N>& q) const = 0;
virtual std::vector<PredictResult> predict(const std::vector<Vecnf<N> >& qs) const = 0;
virtual std::vector<Item<N> > getSupportVectors() const = 0;
virtual void setScaler(const Scaler<N>& scaler) = 0;
virtual void learn(const std::vector<Item<N> >& data) = 0;
FCL_REAL error_rate(const std::vector<Item<N> >& data) const
{
std::size_t num = data.size();
std::size_t error_num = 0;
for(std::size_t i = 0; i < data.size(); ++i)
{
PredictResult res = predict(data[i].q);
if(res.label != data[i].label)
error_num++;
}
return error_num / (FCL_REAL)num;
}
};
template<std::size_t N>
Scaler<N> computeScaler(const std::vector<Item<N> >& data)
{
Vecnf<N> lower_bound, upper_bound;
for(std::size_t j = 0; j < N; ++j)
{
lower_bound[j] = std::numeric_limits<FCL_REAL>::max();
upper_bound[j] = -std::numeric_limits<FCL_REAL>::max();
}
for(std::size_t i = 0; i < data.size(); ++i)
{
for(std::size_t j = 0; j < N; ++j)
{
if(data[i].q[j] < lower_bound[j]) lower_bound[j] = data[i].q[j];
if(data[i].q[j] > upper_bound[j]) upper_bound[j] = data[i].q[j];
}
}
return Scaler<N>(lower_bound, upper_bound);
}
template<std::size_t N>
Scaler<N> computeScaler(const std::vector<Vecnf<N> >& data)
{
Vecnf<N> lower_bound, upper_bound;
for(std::size_t j = 0; j < N; ++j)
{
lower_bound[j] = std::numeric_limits<FCL_REAL>::max();
upper_bound[j] = -std::numeric_limits<FCL_REAL>::max();
}
for(std::size_t i = 0; i < data.size(); ++i)
{
for(std::size_t j = 0; j < N; ++j)
{
if(data[i][j] < lower_bound[j]) lower_bound[j] = data[i][j];
if(data[i][j] > upper_bound[j]) upper_bound[j] = data[i][j];
}
}
return Scaler<N>(lower_bound, upper_bound);
}
}
#endif
include/fcl/math/math_details.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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 FCL_MATH_DETAILS_H
#define FCL_MATH_DETAILS_H
#include <cmath>
#include <algorithm>
#include <cstring>
namespace fcl
{
namespace details
{
template <typename T>
struct Vec3Data
{
typedef T meta_type;
T vs[3];
Vec3Data() { setValue(0); }
Vec3Data(T x)
{
setValue(x);
}
Vec3Data(T* x)
{
memcpy(vs, x, sizeof(T) * 3);
}
Vec3Data(T x, T y, T z)
{
setValue(x, y, z);
}
inline void setValue(T x, T y, T z)
{
vs[0] = x; vs[1] = y; vs[2] = z;
}
inline void setValue(T x)
{
vs[0] = x; vs[1] = x; vs[2] = x;
}
inline void negate()
{
vs[0] = -vs[0]; vs[1] = -vs[1]; vs[2] = -vs[2];
}
inline Vec3Data<T>& ubound(const Vec3Data<T>& u)
{
vs[0] = std::min(vs[0], u.vs[0]);
vs[1] = std::min(vs[1], u.vs[1]);
vs[2] = std::min(vs[2], u.vs[2]);
return *this;
}
inline Vec3Data<T>& lbound(const Vec3Data<T>& l)
{
vs[0] = std::max(vs[0], l.vs[0]);
vs[1] = std::max(vs[1], l.vs[1]);
vs[2] = std::max(vs[2], l.vs[2]);
return *this;
}
T operator [] (size_t i) const { return vs[i]; }
T& operator [] (size_t i) { return vs[i]; }
inline Vec3Data<T> operator + (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] + other.vs[0], vs[1] + other.vs[1], vs[2] + other.vs[2]); }
inline Vec3Data<T> operator - (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] - other.vs[0], vs[1] - other.vs[1], vs[2] - other.vs[2]); }
inline Vec3Data<T> operator * (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] * other.vs[0], vs[1] * other.vs[1], vs[2] * other.vs[2]); }
inline Vec3Data<T> operator / (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] / other.vs[0], vs[1] / other.vs[1], vs[2] / other.vs[2]); }
inline Vec3Data<T>& operator += (const Vec3Data<T>& other) { vs[0] += other.vs[0]; vs[1] += other.vs[1]; vs[2] += other.vs[2]; return *this; }
inline Vec3Data<T>& operator -= (const Vec3Data<T>& other) { vs[0] -= other.vs[0]; vs[1] -= other.vs[1]; vs[2] -= other.vs[2]; return *this; }
inline Vec3Data<T>& operator *= (const Vec3Data<T>& other) { vs[0] *= other.vs[0]; vs[1] *= other.vs[1]; vs[2] *= other.vs[2]; return *this; }
inline Vec3Data<T>& operator /= (const Vec3Data<T>& other) { vs[0] /= other.vs[0]; vs[1] /= other.vs[1]; vs[2] /= other.vs[2]; return *this; }
inline Vec3Data<T> operator + (T t) const { return Vec3Data<T>(vs[0] + t, vs[1] + t, vs[2] + t); }
inline Vec3Data<T> operator - (T t) const { return Vec3Data<T>(vs[0] - t, vs[1] - t, vs[2] - t); }
inline Vec3Data<T> operator * (T t) const { return Vec3Data<T>(vs[0] * t, vs[1] * t, vs[2] * t); }
inline Vec3Data<T> operator / (T t) const { T inv_t = 1.0 / t; return Vec3Data<T>(vs[0] * inv_t, vs[1] * inv_t, vs[2] * inv_t); }
inline Vec3Data<T>& operator += (T t) { vs[0] += t; vs[1] += t; vs[2] += t; return *this; }
inline Vec3Data<T>& operator -= (T t) { vs[0] -= t; vs[1] -= t; vs[2] -= t; return *this; }
inline Vec3Data<T>& operator *= (T t) { vs[0] *= t; vs[1] *= t; vs[2] *= t; return *this; }
inline Vec3Data<T>& operator /= (T t) { T inv_t = 1.0 / t; vs[0] *= inv_t; vs[1] *= inv_t; vs[2] *= inv_t; return *this; }
inline Vec3Data<T> operator - () const { return Vec3Data<T>(-vs[0], -vs[1], -vs[2]); }
};
template <typename T>
static inline Vec3Data<T> cross_prod(const Vec3Data<T>& l, const Vec3Data<T>& r)
{
return Vec3Data<T>(l.vs[1] * r.vs[2] - l.vs[2] * r.vs[1],
l.vs[2] * r.vs[0] - l.vs[0] * r.vs[2],
l.vs[0] * r.vs[1] - l.vs[1] * r.vs[0]);
}
template <typename T>
static inline T dot_prod3(const Vec3Data<T>& l, const Vec3Data<T>& r)
{
return l.vs[0] * r.vs[0] + l.vs[1] * r.vs[1] + l.vs[2] * r.vs[2];
}
template <typename T>
static inline Vec3Data<T> min(const Vec3Data<T>& x, const Vec3Data<T>& y)
{
return Vec3Data<T>(std::min(x.vs[0], y.vs[0]), std::min(x.vs[1], y.vs[1]), std::min(x.vs[2], y.vs[2]));
}
template <typename T>
static inline Vec3Data<T> max(const Vec3Data<T>& x, const Vec3Data<T>& y)
{
return Vec3Data<T>(std::max(x.vs[0], y.vs[0]), std::max(x.vs[1], y.vs[1]), std::max(x.vs[2], y.vs[2]));
}
template <typename T>
static inline Vec3Data<T> abs(const Vec3Data<T>& x)
{
return Vec3Data<T>(std::abs(x.vs[0]), std::abs(x.vs[1]), std::abs(x.vs[2]));
}
template <typename T>
static inline bool equal(const Vec3Data<T>& x, const Vec3Data<T>& y, T epsilon)
{
return ((x.vs[0] - y.vs[0] < epsilon) &&
(x.vs[0] - y.vs[0] > -epsilon) &&
(x.vs[1] - y.vs[1] < epsilon) &&
(x.vs[1] - y.vs[1] > -epsilon) &&
(x.vs[2] - y.vs[2] < epsilon) &&
(x.vs[2] - y.vs[2] > -epsilon));
}
template<typename T>
struct Matrix3Data
{
typedef T meta_type;
typedef Vec3Data<T> vector_type;
Vec3Data<T> rs[3];
Matrix3Data() {};
Matrix3Data(T xx, T xy, T xz,
T yx, T yy, T yz,
T zx, T zy, T zz)
{
setValue(xx, xy, xz,
yx, yy, yz,
zx, zy, zz);
}
Matrix3Data(const Vec3Data<T>& v1, const Vec3Data<T>& v2, const Vec3Data<T>& v3)
{
rs[0] = v1;
rs[1] = v2;
rs[2] = v3;
}
Matrix3Data(const Matrix3Data<T>& other)
{
rs[0] = other.rs[0];
rs[1] = other.rs[1];
rs[2] = other.rs[2];
}
inline Vec3Data<T> getColumn(size_t i) const
{
return Vec3Data<T>(rs[0][i], rs[1][i], rs[2][i]);
}
inline const Vec3Data<T>& getRow(size_t i) const
{
return rs[i];
}
inline T operator() (size_t i, size_t j) const
{
return rs[i][j];
}
inline T& operator() (size_t i, size_t j)
{
return rs[i][j];
}
inline Vec3Data<T> operator * (const Vec3Data<T>& v) const
{
return Vec3Data<T>(dot_prod3(rs[0], v), dot_prod3(rs[1], v), dot_prod3(rs[2], v));
}
inline Matrix3Data<T> operator * (const Matrix3Data<T>& other) const
{
return Matrix3Data<T>(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]),
other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]),
other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
}
inline Matrix3Data<T> operator + (const Matrix3Data<T>& other) const
{
return Matrix3Data<T>(rs[0] + other.rs[0], rs[1] + other.rs[1], rs[2] + other.rs[2]);
}
inline Matrix3Data<T> operator - (const Matrix3Data<T>& other) const
{
return Matrix3Data<T>(rs[0] - other.rs[0], rs[1] - other.rs[1], rs[2] - other.rs[2]);
}
inline Matrix3Data<T> operator + (T c) const
{
return Matrix3Data<T>(rs[0] + c, rs[1] + c, rs[2] + c);
}
inline Matrix3Data<T> operator - (T c) const
{
return Matrix3Data<T>(rs[0] - c, rs[1] - c, rs[2] - c);
}
inline Matrix3Data<T> operator * (T c) const
{
return Matrix3Data<T>(rs[0] * c, rs[1] * c, rs[2] * c);
}
inline Matrix3Data<T> operator / (T c) const
{
return Matrix3Data<T>(rs[0] / c, rs[1] / c, rs[2] / c);
}
inline Matrix3Data<T>& operator *= (const Matrix3Data<T>& other)
{
rs[0].setValue(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]));
rs[1].setValue(other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]));
rs[2].setValue(other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
return *this;
}
inline Matrix3Data<T>& operator += (const Matrix3Data<T>& other)
{
rs[0] += other.rs[0];
rs[1] += other.rs[1];
rs[2] += other.rs[2];
return *this;
}
inline Matrix3Data<T>& operator -= (const Matrix3Data<T>& other)
{
rs[0] -= other.rs[0];
rs[1] -= other.rs[1];
rs[2] -= other.rs[2];
return *this;
}
inline Matrix3Data<T>& operator += (T c)
{
rs[0] += c;
rs[1] += c;
rs[2] += c;
return *this;
}
inline Matrix3Data<T>& operator - (T c)
{
rs[0] -= c;
rs[1] -= c;
rs[2] -= c;
return *this;
}
inline Matrix3Data<T>& operator * (T c)
{
rs[0] *= c;
rs[1] *= c;
rs[2] *= c;
return *this;
}
inline Matrix3Data<T>& operator / (T c)
{
rs[0] /= c;
rs[1] /= c;
rs[2] /= c;
return *this;
}
void setIdentity()
{
setValue((T)1, (T)0, (T)0,
(T)0, (T)1, (T)0,
(T)0, (T)0, (T)1);
}
void setZero()
{
setValue((T)0);
}
static const Matrix3Data<T>& getIdentity()
{
static const Matrix3Data<T> I((T)1, (T)0, (T)0,
(T)0, (T)1, (T)0,
(T)0, (T)0, (T)1);
return I;
}
T determinant() const
{
return dot_prod3(rs[0], cross_prod(rs[1], rs[2]));
}
Matrix3Data<T>& transpose()
{
register T tmp = rs[0][1];
rs[0][1] = rs[1][0];
rs[1][0] = tmp;
tmp = rs[0][2];
rs[0][2] = rs[2][0];
rs[2][0] = tmp;
tmp = rs[2][1];
rs[2][1] = rs[1][2];
rs[1][2] = tmp;
return *this;
}
Matrix3Data<T>& inverse()
{
T det = determinant();
register T inrsdet = 1 / det;
setValue((rs[1][1] * rs[2][2] - rs[1][2] * rs[2][1]) * inrsdet,
(rs[0][2] * rs[2][1] - rs[0][1] * rs[2][2]) * inrsdet,
(rs[0][1] * rs[1][2] - rs[0][2] * rs[1][1]) * inrsdet,
(rs[1][2] * rs[2][0] - rs[1][0] * rs[2][2]) * inrsdet,
(rs[0][0] * rs[2][2] - rs[0][2] * rs[2][0]) * inrsdet,
(rs[0][2] * rs[1][0] - rs[0][0] * rs[1][2]) * inrsdet,
(rs[1][0] * rs[2][1] - rs[1][1] * rs[2][0]) * inrsdet,
(rs[0][1] * rs[2][0] - rs[0][0] * rs[2][1]) * inrsdet,
(rs[0][0] * rs[1][1] - rs[0][1] * rs[1][0]) * inrsdet);
return *this;
}
Matrix3Data<T> transposeTimes(const Matrix3Data<T>& m) const
{
return Matrix3Data<T>(rs[0][0] * m.rs[0][0] + rs[1][0] * m.rs[1][0] + rs[2][0] * m.rs[2][0],
rs[0][0] * m.rs[0][1] + rs[1][0] * m.rs[1][1] + rs[2][0] * m.rs[2][1],
rs[0][0] * m.rs[0][2] + rs[1][0] * m.rs[1][2] + rs[2][0] * m.rs[2][2],
rs[0][1] * m.rs[0][0] + rs[1][1] * m.rs[1][0] + rs[2][1] * m.rs[2][0],
rs[0][1] * m.rs[0][1] + rs[1][1] * m.rs[1][1] + rs[2][1] * m.rs[2][1],
rs[0][1] * m.rs[0][2] + rs[1][1] * m.rs[1][2] + rs[2][1] * m.rs[2][2],
rs[0][2] * m.rs[0][0] + rs[1][2] * m.rs[1][0] + rs[2][2] * m.rs[2][0],
rs[0][2] * m.rs[0][1] + rs[1][2] * m.rs[1][1] + rs[2][2] * m.rs[2][1],
rs[0][2] * m.rs[0][2] + rs[1][2] * m.rs[1][2] + rs[2][2] * m.rs[2][2]);
}
Matrix3Data<T> timesTranspose(const Matrix3Data<T>& m) const
{
return Matrix3Data<T>(dot_prod3(rs[0], m[0]), dot_prod3(rs[0], m[1]), dot_prod3(rs[0], m[2]),
dot_prod3(rs[1], m[0]), dot_prod3(rs[1], m[1]), dot_prod3(rs[1], m[2]),
dot_prod3(rs[2], m[0]), dot_prod3(rs[2], m[1]), dot_prod3(rs[2], m[2]));
}
Vec3Data<T> transposeTimes(const Vec3Data<T>& v) const
{
return Vec3Data<T>(transposeDotX(v), transposeDotY(v), transposeDotZ(v));
}
inline T transposeDotX(const Vec3Data<T>& v) const
{
return rs[0][0] * v[0] + rs[1][0] * v[1] + rs[2][0] * v[2];
}
inline T transposeDotY(const Vec3Data<T>& v) const
{
return rs[0][1] * v[0] + rs[1][1] * v[1] + rs[2][1] * v[2];
}
inline T transposeDotZ(const Vec3Data<T>& v) const
{
return rs[0][2] * v[0] + rs[1][2] * v[1] + rs[2][2] * v[2];
}
inline T transposeDot(size_t i, const Vec3Data<T>& v) const
{
return rs[0][i] * v[0] + rs[1][i] * v[1] + rs[2][i] * v[2];
}
inline T dotX(const Vec3Data<T>& v) const
{
return rs[0][0] * v[0] + rs[0][1] * v[1] + rs[0][2] * v[2];
}
inline T dotY(const Vec3Data<T>& v) const
{
return rs[1][0] * v[0] + rs[1][1] * v[1] + rs[1][2] * v[2];
}
inline T dotZ(const Vec3Data<T>& v) const
{
return rs[2][0] * v[0] + rs[2][1] * v[1] + rs[2][2] * v[2];
}
inline T dot(size_t i, const Vec3Data<T>& v) const
{
return rs[i][0] * v[0] + rs[i][1] * v[1] + rs[i][2] * v[2];
}
inline void setValue(T xx, T xy, T xz,
T yx, T yy, T yz,
T zx, T zy, T zz)
{
rs[0].setValue(xx, xy, xz);
rs[1].setValue(yx, yy, yz);
rs[2].setValue(zx, zy, zz);
}
inline void setValue(T x)
{
rs[0].setValue(x);
rs[1].setValue(x);
rs[2].setValue(x);
}
};
template<typename T>
Matrix3Data<T> abs(const Matrix3Data<T>& m)
{
return Matrix3Data<T>(abs(m.rs[0]), abs(m.rs[1]), abs(m.rs[2]));
}
template<typename T>
Matrix3Data<T> transpose(const Matrix3Data<T>& m)
{
return Matrix3Data<T>(m.rs[0][0], m.rs[1][0], m.rs[2][0],
m.rs[0][1], m.rs[1][1], m.rs[2][1],
m.rs[0][2], m.rs[1][2], m.rs[2][2]);
}
template<typename T>
Matrix3Data<T> inverse(const Matrix3Data<T>& m)
{
T det = m.determinant();
T inrsdet = 1 / det;
return Matrix3Data<T>((m.rs[1][1] * m.rs[2][2] - m.rs[1][2] * m.rs[2][1]) * inrsdet,
(m.rs[0][2] * m.rs[2][1] - m.rs[0][1] * m.rs[2][2]) * inrsdet,
(m.rs[0][1] * m.rs[1][2] - m.rs[0][2] * m.rs[1][1]) * inrsdet,
(m.rs[1][2] * m.rs[2][0] - m.rs[1][0] * m.rs[2][2]) * inrsdet,
(m.rs[0][0] * m.rs[2][2] - m.rs[0][2] * m.rs[2][0]) * inrsdet,
(m.rs[0][2] * m.rs[1][0] - m.rs[0][0] * m.rs[1][2]) * inrsdet,
(m.rs[1][0] * m.rs[2][1] - m.rs[1][1] * m.rs[2][0]) * inrsdet,
(m.rs[0][1] * m.rs[2][0] - m.rs[0][0] * m.rs[2][1]) * inrsdet,
(m.rs[0][0] * m.rs[1][1] - m.rs[0][1] * m.rs[1][0]) * inrsdet);
}
}
}
#endif
include/fcl/math/matrix_3f.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_MATRIX_3F_H
#define FCL_MATRIX_3F_H
#include "fcl/math/vec_3f.h"
namespace fcl
{
/// @brief Matrix2 class wrapper. the core data is in the template parameter class
template<typename T>
class Matrix3fX
{
public:
typedef typename T::meta_type U;
typedef typename T::vector_type S;
T data;
Matrix3fX() {}
Matrix3fX(U xx, U xy, U xz,
U yx, U yy, U yz,
U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
{}
Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
: data(v1.data, v2.data, v3.data) {}
Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
Matrix3fX(const T& data_) : data(data_) {}
inline Vec3fX<S> getColumn(size_t i) const
{
return Vec3fX<S>(data.getColumn(i));
}
inline Vec3fX<S> getRow(size_t i) const
{
return Vec3fX<S>(data.getRow(i));
}
inline U operator () (size_t i, size_t j) const
{
return data(i, j);
}
inline U& operator () (size_t i, size_t j)
{
return data(i, j);
}
inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
{
return Vec3fX<S>(data * v.data);
}
inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
{
return Matrix3fX<T>(data * m.data);
}
inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
{
return Matrix3fX<T>(data + other.data);
}
inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
{
return Matrix3fX<T>(data - other.data);
}
inline Matrix3fX<T> operator + (U c) const
{
return Matrix3fX<T>(data + c);
}
inline Matrix3fX<T> operator - (U c) const
{
return Matrix3fX<T>(data - c);
}
inline Matrix3fX<T> operator * (U c) const
{
return Matrix3fX<T>(data * c);
}
inline Matrix3fX<T> operator / (U c) const
{
return Matrix3fX<T>(data / c);
}
inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
{
data *= other.data;
return *this;
}
inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
{
data += other.data;
return *this;
}
inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
{
data -= other.data;
return *this;
}
inline Matrix3fX<T>& operator += (U c)
{
data += c;
return *this;
}
inline Matrix3fX<T>& operator -= (U c)
{
data -= c;
return *this;
}
inline Matrix3fX<T>& operator *= (U c)
{
data *= c;
return *this;
}
inline Matrix3fX<T>& operator /= (U c)
{
data /= c;
return *this;
}
inline void setIdentity()
{
data.setIdentity();
}
inline void setZero()
{
data.setZero();
}
/// @brief Set the matrix from euler angles YPR around ZYX axes
/// @param eulerX Roll about X axis
/// @param eulerY Pitch around Y axis
/// @param eulerZ Yaw aboud Z axis
///
/// These angles are used to produce a rotation matrix. The euler
/// angles are applied in ZYX order. I.e a vector is first rotated
/// about X then Y and then Z
inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
{
FCL_REAL ci(cos(eulerX));
FCL_REAL cj(cos(eulerY));
FCL_REAL ch(cos(eulerZ));
FCL_REAL si(sin(eulerX));
FCL_REAL sj(sin(eulerY));
FCL_REAL sh(sin(eulerZ));
FCL_REAL cc = ci * ch;
FCL_REAL cs = ci * sh;
FCL_REAL sc = si * ch;
FCL_REAL ss = si * sh;
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
cj * sh, sj * ss + cc, sj * cs - sc,
-sj, cj * si, cj * ci);
}
/// @brief Set the matrix from euler angles using YPR around YXZ respectively
/// @param yaw Yaw about Y axis
/// @param pitch Pitch about X axis
/// @param roll Roll about Z axis
void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
{
setEulerZYX(roll, pitch, yaw);
}
inline U determinant() const
{
return data.determinant();
}
Matrix3fX<T>& transpose()
{
data.transpose();
return *this;
}
Matrix3fX<T>& inverse()
{
data.inverse();
return *this;
}
Matrix3fX<T>& abs()
{
data.abs();
return *this;
}
static const Matrix3fX<T>& getIdentity()
{
static const Matrix3fX<T> I((U)1, (U)0, (U)0,
(U)0, (U)1, (U)0,
(U)0, (U)0, (U)1);
return I;
}
Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
{
return Matrix3fX<T>(data.transposeTimes(other.data));
}
Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
{
return Matrix3fX<T>(data.timesTranspose(other.data));
}
Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
{
return Vec3fX<S>(data.transposeTimes(v.data));
}
Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
{
Matrix3fX<T> res(*this);
res *= m;
return res.timesTranspose(*this);
}
inline U transposeDotX(const Vec3fX<S>& v) const
{
return data.transposeDot(0, v.data);
}
inline U transposeDotY(const Vec3fX<S>& v) const
{
return data.transposeDot(1, v.data);
}
inline U transposeDotZ(const Vec3fX<S>& v) const
{
return data.transposeDot(2, v.data);
}
inline U transposeDot(size_t i, const Vec3fX<S>& v) const
{
return data.transposeDot(i, v.data);
}
inline U dotX(const Vec3fX<S>& v) const
{
return data.dot(0, v.data);
}
inline U dotY(const Vec3fX<S>& v) const
{
return data.dot(1, v.data);
}
inline U dotZ(const Vec3fX<S>& v) const
{
return data.dot(2, v.data);
}
inline U dot(size_t i, const Vec3fX<S>& v) const
{
return data.dot(i, v.data);
}
inline void setValue(U xx, U xy, U xz,
U yx, U yy, U yz,
U zx, U zy, U zz)
{
data.setValue(xx, xy, xz,
yx, yy, yz,
zx, zy, zz);
}
inline void setValue(U x)
{
data.setValue(x);
}
};
template<typename T>
void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
{
mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
}
template<typename T>
void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
{
R = R1.transposeTimes(R2);
t = R1.transposeTimes(t2 - t1);
}
/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
template<typename T>
void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
{
Matrix3fX<T> R(m);
int n = 3;
int j, iq, ip, i;
typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
int nrot;
typename T::meta_type b[3];
typename T::meta_type z[3];
typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
typename T::meta_type d[3];
for(ip = 0; ip < n; ++ip)
{
b[ip] = d[ip] = R(ip, ip);
z[ip] = 0;
}
nrot = 0;
for(i = 0; i < 50; ++i)
{
sm = 0;
for(ip = 0; ip < n; ++ip)
for(iq = ip + 1; iq < n; ++iq)
sm += std::abs(R(ip, iq));
if(sm == 0.0)
{
vout[0].setValue(v[0][0], v[0][1], v[0][2]);
vout[1].setValue(v[1][0], v[1][1], v[1][2]);
vout[2].setValue(v[2][0], v[2][1], v[2][2]);
dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
return;
}
if(i < 3) tresh = 0.2 * sm / (n * n);
else tresh = 0.0;
for(ip = 0; ip < n; ++ip)
{
for(iq= ip + 1; iq < n; ++iq)
{
g = 100.0 * std::abs(R(ip, iq));
if(i > 3 &&
std::abs(d[ip]) + g == std::abs(d[ip]) &&
std::abs(d[iq]) + g == std::abs(d[iq]))
R(ip, iq) = 0.0;
else if(std::abs(R(ip, iq)) > tresh)
{
h = d[iq] - d[ip];
if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
else
{
theta = 0.5 * h / (R(ip, iq));
t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
if(theta < 0.0) t = -t;
}
c = 1.0 / std::sqrt(1 + t * t);
s = t * c;
tau = s / (1.0 + c);
h = t * R(ip, iq);
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
R(ip, iq) = 0.0;
for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
nrot++;
}
}
}
for(ip = 0; ip < n; ++ip)
{
b[ip] += z[ip];
d[ip] = b[ip];
z[ip] = 0.0;
}
}
std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
return;
}
template<typename T>
Matrix3fX<T> abs(const Matrix3fX<T>& R)
{
return Matrix3fX<T>(abs(R.data));
}
template<typename T>
Matrix3fX<T> transpose(const Matrix3fX<T>& R)
{
return Matrix3fX<T>(transpose(R.data));
}
template<typename T>
Matrix3fX<T> inverse(const Matrix3fX<T>& R)
{
return Matrix3fX<T>(inverse(R.data));
}
template<typename T>
typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
{
return v.dot(R * v);
}
#if FCL_HAVE_SSE
typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
#else
typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
#endif
static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
{
o << "[(" << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << ")("
<< m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << ")("
<< m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << ")]";
return o;
}
/// @brief Class for variance matrix in 3d
class Variance3f
{
public:
/// @brief Variation matrix
Matrix3f Sigma;
/// @brief Variations along the eign axes
Matrix3f::U sigma[3];
/// @brief Eigen axes of the variation matrix
Vec3f axis[3];
Variance3f() {}
Variance3f(const Matrix3f& S) : Sigma(S)
{
init();
}
/// @brief init the Variance
void init()
{
eigen(Sigma, sigma, axis);
}
/// @brief Compute the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized as a square variation matrix
Variance3f& sqrt()
{
for(std::size_t i = 0; i < 3; ++i)
{
if(sigma[i] < 0) sigma[i] = 0;
sigma[i] = std::sqrt(sigma[i]);
}
Sigma.setZero();
for(std::size_t i = 0; i < 3; ++i)
{
for(std::size_t j = 0; j < 3; ++j)
{
Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j];
Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j];
Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j];
}
}
return *this;
}
};
}
#endif
include/fcl/math/sampling.h deleted 100644 → 0
View file @ 12194e71
#ifndef FCL_MATH_SAMPLING_H
#define FCL_MATH_SAMPLING_H
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/assign/list_of.hpp>
#include "fcl/math/vec_nf.h"
#include "fcl/math/transform.h"
namespace fcl
{
/// @brief Random number generation. An instance of this class
/// cannot be used by multiple threads at once (member functions
/// are not const). However, the constructor is thread safe and
/// different instances can be used safely in any number of
/// threads. It is also guaranteed that all created instances will
/// have a different random seed.
class RNG
{
public:
/// @brief Constructor. Always sets a different random seed
RNG();
/// @brief Generate a random real between 0 and 1
double uniform01()
{
return uni_();
}
/// @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound)
double uniformReal(double lower_bound, double upper_bound)
{
assert(lower_bound <= upper_bound);
return (upper_bound - lower_bound) * uni_() + lower_bound;
}
/// @brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound]
int uniformInt(int lower_bound, int upper_bound)
{
int r = (int)floor(uniformReal((double)lower_bound, (double)(upper_bound) + 1.0));
return (r > upper_bound) ? upper_bound : r;
}
/// @brief Generate a random boolean
bool uniformBool()
{
return uni_() <= 0.5;
}
/// @brief Generate a random real using a normal distribution with mean 0 and variance 1
double gaussian01()
{
return normal_();
}
/// @brief Generate a random real using a normal distribution with given mean and variance
double gaussian(double mean, double stddev)
{
return normal_() * stddev + mean;
}
/// @brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e r_min, \e r_max], but with a bias towards \e r_max. The function is implemended using a Gaussian distribution with mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max axis towards \e r_min. The variance of the distribution is (\e r_max - \e r_min) / \e focus. The higher the focus, the more probable it is that generated numbers are close to \e r_max.
double halfNormalReal(double r_min, double r_max, double focus = 3.0);
/// @brief Generate a random integer using a half-normal distribution. The value is within specified bounds ([\e r_min, \e r_max]), but with a bias towards \e r_max. The function is implemented on top of halfNormalReal()
int halfNormalInt(int r_min, int r_max, double focus = 3.0);
/// @brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w)
void quaternion(double value[4]);
/// @brief Uniform random sampling of Euler roll-pitch-yaw angles, each in the range [-pi, pi). The computed value has the order (roll, pitch, yaw) */
void eulerRPY(double value[3]);
/// @brief Uniform random sample on a disk with radius from r_min to r_max
void disk(double r_min, double r_max, double& x, double& y);
/// @brief Uniform random sample in a ball with radius from r_min to r_max
void ball(double r_min, double r_max, double& x, double& y, double& z);
/// @brief Set the seed for random number generation. Use this function to ensure the same sequence of random numbers is generated.
static void setSeed(boost::uint32_t seed);
/// @brief Get the seed used for random number generation. Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic (repeatable) behaviour. Useful for debugging.
static boost::uint32_t getSeed();
private:
boost::mt19937 generator_;
boost::uniform_real<> uniDist_;
boost::normal_distribution<> normalDist_;
boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni_;
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > normal_;
};
class SamplerBase
{
public:
mutable RNG rng;
};
template<std::size_t N>
class SamplerR : public SamplerBase
{
public:
SamplerR() {}
SamplerR(const Vecnf<N>& lower_bound_,
const Vecnf<N>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{
}
void setBound(const Vecnf<N>& lower_bound_,
const Vecnf<N>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void getBound(Vecnf<N>& lower_bound_,
Vecnf<N>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
Vecnf<N> sample() const
{
Vecnf<N> q;
for(std::size_t i = 0; i < N; ++i)
{
q[i] = rng.uniformReal(lower_bound[i], upper_bound[i]);
}
return q;
}
private:
Vecnf<N> lower_bound;
Vecnf<N> upper_bound;
};
class SamplerSE2 : public SamplerBase
{
public:
SamplerSE2() {}
SamplerSE2(const Vecnf<2>& lower_bound_,
const Vecnf<2>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{}
SamplerSE2(FCL_REAL x_min, FCL_REAL x_max,
FCL_REAL y_min, FCL_REAL y_max) : lower_bound(boost::assign::list_of<FCL_REAL>(x_min)(y_min).convert_to_container<std::vector<FCL_REAL> >()),
upper_bound(boost::assign::list_of<FCL_REAL>(x_max)(y_max).convert_to_container<std::vector<FCL_REAL> >())
{}
void setBound(const Vecnf<2>& lower_bound_,
const Vecnf<2>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void getBound(Vecnf<2>& lower_bound_,
Vecnf<2>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
Vecnf<3> sample() const
{
Vecnf<3> q;
q[0] = rng.uniformReal(lower_bound[0], lower_bound[1]);
q[1] = rng.uniformReal(lower_bound[1], lower_bound[2]);
q[2] = rng.uniformReal(-boost::math::constants::pi<FCL_REAL>(),
boost::math::constants::pi<FCL_REAL>());
return q;
}
protected:
Vecnf<2> lower_bound;
Vecnf<2> upper_bound;
};
class SamplerSE2_disk : public SamplerBase
{
public:
SamplerSE2_disk() {}
SamplerSE2_disk(FCL_REAL cx, FCL_REAL cy,
FCL_REAL r1, FCL_REAL r2,
FCL_REAL crefx, FCL_REAL crefy)
{
setBound(cx, cy, r1, r2, crefx, crefy);
}
void setBound(FCL_REAL cx, FCL_REAL cy,
FCL_REAL r1, FCL_REAL r2,
FCL_REAL crefx, FCL_REAL crefy)
{
c[0] = cx; c[1] = cy;
cref[0] = crefx; cref[1] = crefy;
r_min = r1;
r_max = r2;
}
Vecnf<3> sample() const
{
Vecnf<3> q;
FCL_REAL x, y;
rng.disk(r_min, r_max, x, y);
q[0] = x + c[0] - cref[0];
q[1] = y + c[1] - cref[1];
q[2] = rng.uniformReal(-boost::math::constants::pi<FCL_REAL>(),
boost::math::constants::pi<FCL_REAL>());
return q;
}
protected:
FCL_REAL c[2];
FCL_REAL cref[2];
FCL_REAL r_min, r_max;
};
class SamplerSE3Euler : public SamplerBase
{
public:
SamplerSE3Euler() {}
SamplerSE3Euler(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{}
SamplerSE3Euler(const Vec3f& lower_bound_,
const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >()),
upper_bound(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >())
{}
void setBound(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void setBound(const Vec3f& lower_bound_,
const Vec3f& upper_bound_)
{
lower_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
upper_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
}
void getBound(Vecnf<3>& lower_bound_,
Vecnf<3>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
void getBound(Vec3f& lower_bound_,
Vec3f& upper_bound_) const
{
lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]);
upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]);
}
Vecnf<6> sample() const
{
Vecnf<6> q;
q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]);
FCL_REAL s[4];
rng.quaternion(s);
Quaternion3f quat(s[0], s[1], s[2], s[3]);
FCL_REAL a, b, c;
quat.toEuler(a, b, c);
q[3] = a;
q[4] = b;
q[5] = c;
return q;
}
protected:
Vecnf<3> lower_bound;
Vecnf<3> upper_bound;
};
class SamplerSE3Quat : public SamplerBase
{
public:
SamplerSE3Quat() {}
SamplerSE3Quat(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_),
upper_bound(upper_bound_)
{}
SamplerSE3Quat(const Vec3f& lower_bound_,
const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >()),
upper_bound(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >())
{}
void setBound(const Vecnf<3>& lower_bound_,
const Vecnf<3>& upper_bound_)
{
lower_bound = lower_bound_;
upper_bound = upper_bound_;
}
void setBound(const Vec3f& lower_bound_,
const Vec3f& upper_bound_)
{
lower_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
upper_bound = Vecnf<3>(boost::assign::list_of<FCL_REAL>(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container<std::vector<FCL_REAL> >());
}
void getBound(Vecnf<3>& lower_bound_,
Vecnf<3>& upper_bound_) const
{
lower_bound_ = lower_bound;
upper_bound_ = upper_bound;
}
void getBound(Vec3f& lower_bound_,
Vec3f& upper_bound_) const
{
lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]);
upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]);
}
Vecnf<7> sample() const
{
Vecnf<7> q;
q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]);
q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]);
q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]);
FCL_REAL s[4];
rng.quaternion(s);
q[3] = s[0];
q[4] = s[1];
q[5] = s[2];
q[6] = s[3];
return q;
}
protected:
Vecnf<3> lower_bound;
Vecnf<3> upper_bound;
};
class SamplerSE3Euler_ball : public SamplerBase
{
public:
SamplerSE3Euler_ball() {}
SamplerSE3Euler_ball(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& c_ref)
{
setBound(c_, r1, r2, c_ref);
}
SamplerSE3Euler_ball(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
setBound(c_, r_min_, r_max_);
}
void setBound(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& cref_)
{
c = c_;
FCL_REAL cref_len = cref_.length();
r_min = std::max<FCL_REAL>(cref_len - (r1 + r2), 0);
r_max = cref_len + r1 + r2;
}
void setBound(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
c = c_;
r_min = r_min_;
r_max = r_max_;
}
void getBound(Vec3f& c_,
FCL_REAL& r_min_, FCL_REAL& r_max_) const
{
c_ = c;
r_min_ = r_min;
r_max_ = r_max;
}
Vecnf<6> sample() const
{
Vecnf<6> q;
FCL_REAL x, y, z;
rng.ball(r_min, r_max, x, y, z);
q[0] = x;
q[1] = y;
q[2] = z;
FCL_REAL s[4];
rng.quaternion(s);
Quaternion3f quat(s[0], s[1], s[2], s[3]);
FCL_REAL a, b, c;
quat.toEuler(a, b, c);
q[3] = a;
q[4] = b;
q[5] = c;
return q;
}
protected:
Vec3f c;
FCL_REAL r_min, r_max;
};
class SamplerSE3Quat_ball : public SamplerBase
{
public:
SamplerSE3Quat_ball() {}
SamplerSE3Quat_ball(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& c_ref)
{
setBound(c_, r1, r2, c_ref);
}
SamplerSE3Quat_ball(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
setBound(c_, r_min_, r_max_);
}
void setBound(const Vec3f& c_,
FCL_REAL r1, FCL_REAL r2,
const Vec3f& cref_)
{
c = c_;
FCL_REAL cref_len = cref_.length();
r_min = std::max<FCL_REAL>(cref_len - (r1 + r2), 0);
r_max = cref_len + r1 + r2;
}
void setBound(const Vec3f& c_,
FCL_REAL r_min_, FCL_REAL r_max_)
{
c = c_;
r_min = r_min_;
r_max = r_max_;
}
void getBound(Vec3f& c_,
FCL_REAL& r_min_, FCL_REAL& r_max_) const
{
c_ = c;
r_min_ = r_min;
r_max_ = r_max;
}
Vecnf<7> sample() const
{
Vecnf<7> q;
FCL_REAL x, y, z;
rng.ball(r_min, r_max, x, y, z);
q[0] = x;
q[1] = y;
q[2] = z;
FCL_REAL s[4];
rng.quaternion(s);
q[3] = s[0];
q[4] = s[1];
q[5] = s[2];
q[6] = s[3];
return q;
}
protected:
Vec3f c;
FCL_REAL r_min, r_max;
};
}
#endif
include/fcl/math/transform.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_TRANSFORM_H
#define FCL_TRANSFORM_H
#include "fcl/math/matrix_3f.h"
#include <boost/thread/mutex.hpp>
namespace fcl
{
/// @brief Quaternion used locally by InterpMotion
class Quaternion3f
{
public:
/// @brief Default quaternion is identity rotation
Quaternion3f()
{
data[0] = 1;
data[1] = 0;
data[2] = 0;
data[3] = 0;
}
Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
{
data[0] = a;
data[1] = b;
data[2] = c;
data[3] = d;
}
/// @brief Whether the rotation is identity
bool isIdentity() const
{
return (data[0] == 1) && (data[1] == 0) && (data[2] == 0) && (data[3] == 0);
}
/// @brief Matrix to quaternion
void fromRotation(const Matrix3f& R);
/// @brief Quaternion to matrix
void toRotation(Matrix3f& R) const;
/// @brief Euler to quaternion
void fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c);
/// @brief Quaternion to Euler
void toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const;
/// @brief Axes to quaternion
void fromAxes(const Vec3f axis[3]);
/// @brief Axes to matrix
void toAxes(Vec3f axis[3]) const;
/// @brief Axis and angle to quaternion
void fromAxisAngle(const Vec3f& axis, FCL_REAL angle);
/// @brief Quaternion to axis and angle
void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
/// @brief Dot product between quaternions
FCL_REAL dot(const Quaternion3f& other) const;
/// @brief addition
Quaternion3f operator + (const Quaternion3f& other) const;
const Quaternion3f& operator += (const Quaternion3f& other);
/// @brief minus
Quaternion3f operator - (const Quaternion3f& other) const;
const Quaternion3f& operator -= (const Quaternion3f& other);
/// @brief multiplication
Quaternion3f operator * (const Quaternion3f& other) const;
const Quaternion3f& operator *= (const Quaternion3f& other);
/// @brief division
Quaternion3f operator - () const;
/// @brief scalar multiplication
Quaternion3f operator * (FCL_REAL t) const;
const Quaternion3f& operator *= (FCL_REAL t);
/// @brief conjugate
Quaternion3f& conj();
/// @brief inverse
Quaternion3f& inverse();
/// @brief rotate a vector
Vec3f transform(const Vec3f& v) const;
inline const FCL_REAL& getW() const { return data[0]; }
inline const FCL_REAL& getX() const { return data[1]; }
inline const FCL_REAL& getY() const { return data[2]; }
inline const FCL_REAL& getZ() const { return data[3]; }
inline FCL_REAL& getW() { return data[0]; }
inline FCL_REAL& getX() { return data[1]; }
inline FCL_REAL& getY() { return data[2]; }
inline FCL_REAL& getZ() { return data[3]; }
Vec3f getColumn(std::size_t i) const;
Vec3f getRow(std::size_t i) const;
bool operator == (const Quaternion3f& other) const
{
for(std::size_t i = 0; i < 4; ++i)
{
if(data[i] != other[i])
return false;
}
return true;
}
bool operator != (const Quaternion3f& other) const
{
return !(*this == other);
}
FCL_REAL operator [] (std::size_t i) const
{
return data[i];
}
private:
FCL_REAL data[4];
};
/// @brief conjugate of quaternion
Quaternion3f conj(const Quaternion3f& q);
/// @brief inverse of quaternion
Quaternion3f inverse(const Quaternion3f& q);
/// @brief Simple transform class used locally by InterpMotion
class Transform3f
{
boost::mutex lock_;
/// @brief Whether matrix cache is set
mutable bool matrix_set;
/// @brief Matrix cache
mutable Matrix3f R;
/// @brief Tranlation vector
Vec3f T;
/// @brief Rotation
Quaternion3f q;
const Matrix3f& getRotationInternal() const;
public:
/// @brief Default transform is no movement
Transform3f()
{
setIdentity(); // set matrix_set true
}
/// @brief Construct transform from rotation and translation
Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
R(R_),
T(T_)
{
q.fromRotation(R_);
}
/// @brief Construct transform from rotation and translation
Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
T(T_),
q(q_)
{
}
/// @brief Construct transform from rotation
Transform3f(const Matrix3f& R_) : matrix_set(true),
R(R_)
{
q.fromRotation(R_);
}
/// @brief Construct transform from rotation
Transform3f(const Quaternion3f& q_) : matrix_set(false),
q(q_)
{
}
/// @brief Construct transform from translation
Transform3f(const Vec3f& T_) : matrix_set(true),
T(T_)
{
R.setIdentity();
}
/// @brief Construct transform from another transform
Transform3f(const Transform3f& tf) : matrix_set(tf.matrix_set),
R(tf.R),
T(tf.T),
q(tf.q)
{
}
/// @brief operator =
Transform3f& operator = (const Transform3f& tf)
{
matrix_set = tf.matrix_set;
R = tf.R;
q = tf.q;
T = tf.T;
return *this;
}
/// @brief get translation
inline const Vec3f& getTranslation() const
{
return T;
}
/// @brief get rotation
inline const Matrix3f& getRotation() const
{
if(matrix_set) return R;
return getRotationInternal();
}
/// @brief get quaternion
inline const Quaternion3f& getQuatRotation() const
{
return q;
}
/// @brief set transform from rotation and translation
inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
{
R = R_;
T = T_;
matrix_set = true;
q.fromRotation(R_);
}
/// @brief set transform from rotation and translation
inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
{
matrix_set = false;
q = q_;
T = T_;
}
/// @brief set transform from rotation
inline void setRotation(const Matrix3f& R_)
{
R = R_;
matrix_set = true;
q.fromRotation(R_);
}
/// @brief set transform from translation
inline void setTranslation(const Vec3f& T_)
{
T = T_;
}
/// @brief set transform from rotation
inline void setQuatRotation(const Quaternion3f& q_)
{
matrix_set = false;
q = q_;
}
/// @brief transform a given vector by the transform
inline Vec3f transform(const Vec3f& v) const
{
return q.transform(v) + T;
}
/// @brief inverse transform
inline Transform3f& inverse()
{
matrix_set = false;
q.conj();
T = q.transform(-T);
return *this;
}
/// @brief inverse the transform and multiply with another
inline Transform3f inverseTimes(const Transform3f& other) const
{
const Quaternion3f& q_inv = fcl::conj(q);
return Transform3f(q_inv * other.q, q_inv.transform(other.T - T));
}
/// @brief multiply with another transform
inline const Transform3f& operator *= (const Transform3f& other)
{
matrix_set = false;
T = q.transform(other.T) + T;
q *= other.q;
return *this;
}
/// @brief multiply with another transform
inline Transform3f operator * (const Transform3f& other) const
{
Quaternion3f q_new = q * other.q;
return Transform3f(q_new, q.transform(other.T) + T);
}
/// @brief check whether the transform is identity
inline bool isIdentity() const
{
return q.isIdentity() && T.isZero();
}
/// @brief set the transform to be identity transform
inline void setIdentity()
{
R.setIdentity();
T.setValue(0);
q = Quaternion3f();
matrix_set = true;
}
bool operator == (const Transform3f& other) const
{
return (q == other.getQuatRotation()) && (T == other.getTranslation());
}
bool operator != (const Transform3f& other) const
{
return !(*this == other);
}
};
/// @brief inverse the transform
Transform3f inverse(const Transform3f& tf);
/// @brief compute the relative transform between two transforms: tf2 = tf * tf1
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf);
}
#endif
include/fcl/math/vec_3f.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_VEC_3F_H
#define FCL_VEC_3F_H
#include "fcl/config.h"
#include "fcl/data_types.h"
#include "fcl/math/math_details.h"
#if FCL_HAVE_SSE
# include "fcl/simd/math_simd_details.h"
#endif
#include <cmath>
#include <iostream>
#include <limits>
namespace fcl
{
/// @brief Vector3 class wrapper. The core data is in the template parameter class.
template <typename T>
class Vec3fX
{
public:
typedef typename T::meta_type U;
/// @brief interval vector3 data
T data;
Vec3fX() {}
Vec3fX(const Vec3fX& other) : data(other.data) {}
/// @brief create Vector (x, y, z)
Vec3fX(U x, U y, U z) : data(x, y, z) {}
/// @brief create vector (x, x, x)
Vec3fX(U x) : data(x) {}
/// @brief create vector using the internal data type
Vec3fX(const T& data_) : data(data_) {}
inline U operator [] (size_t i) const { return data[i]; }
inline U& operator [] (size_t i) { return data[i]; }
inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
inline Vec3fX& operator += (U t) { data += t; return *this; }
inline Vec3fX& operator -= (U t) { data -= t; return *this; }
inline Vec3fX& operator *= (U t) { data *= t; return *this; }
inline Vec3fX& operator /= (U t) { data /= t; return *this; }
inline Vec3fX operator - () const { return Vec3fX(-data); }
inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
inline Vec3fX& normalize()
{
U sqr_length = details::dot_prod3(data, data);
if(sqr_length > 0)
*this /= (U)sqrt(sqr_length);
return *this;
}
inline Vec3fX& normalize(bool* signal)
{
U sqr_length = details::dot_prod3(data, data);
if(sqr_length > 0)
{
*this /= (U)sqrt(sqr_length);
*signal = true;
}
else
*signal = false;
return *this;
}
inline Vec3fX& abs()
{
data = abs(data);
return *this;
}
inline U length() const { return sqrt(details::dot_prod3(data, data)); }
inline U sqrLength() const { return details::dot_prod3(data, data); }
inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
inline void setValue(U x) { data.setValue(x); }
inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
inline Vec3fX<T>& negate() { data.negate(); return *this; }
bool operator == (const Vec3fX& other) const
{
return equal(other, 0);
}
bool operator != (const Vec3fX& other) const
{
return !(*this == other);
}
inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
{
data.ubound(u.data);
return *this;
}
inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
{
data.lbound(l.data);
return *this;
}
bool isZero() const
{
return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
}
};
template<typename T>
static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
{
typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
if(sqr_length > 0)
return v / (typename T::meta_type)sqrt(sqr_length);
else
return v;
}
template <typename T>
static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
{
return x.dot(y.cross(z));
}
template <typename T>
std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
{
out << x[0] << " " << x[1] << " " << x[2];
return out;
}
template <typename T>
static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
{
return Vec3fX<T>(details::min(x.data, y.data));
}
template <typename T>
static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
{
return Vec3fX<T>(details::max(x.data, y.data));
}
template <typename T>
static inline Vec3fX<T> abs(const Vec3fX<T>& x)
{
return Vec3fX<T>(details::abs(x.data));
}
template <typename T>
void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
{
typedef typename T::meta_type U;
U inv_length;
if(std::abs(w[0]) >= std::abs(w[1]))
{
inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
u[0] = -w[2] * inv_length;
u[1] = (U)0;
u[2] = w[0] * inv_length;
v[0] = w[1] * u[2];
v[1] = w[2] * u[0] - w[0] * u[2];
v[2] = -w[1] * u[0];
}
else
{
inv_length = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
u[0] = (U)0;
u[1] = w[2] * inv_length;
u[2] = -w[1] * inv_length;
v[0] = w[1] * u[2] - w[2] * u[1];
v[1] = -w[0] * u[2];
v[2] = w[0] * u[1];
}
}
#if FCL_HAVE_SSE
typedef Vec3fX<details::sse_meta_f4> Vec3f;
#else
typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
#endif
static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
{
o << "(" << v[0] << " " << v[1] << " " << v[2] << ")";
return o;
}
}
#endif
include/fcl/math/vec_nf.h deleted 100644 → 0
View file @ 12194e71
#ifndef FCL_MATH_VEC_NF_H
#define FCL_MATH_VEC_NF_H
#include <cmath>
#include <iostream>
#include <limits>
#include <vector>
#include <boost/array.hpp>
#include <cstdarg>
#include "fcl/data_types.h"
namespace fcl
{
template<typename T, std::size_t N>
class Vec_n
{
public:
Vec_n()
{
data.resize(N, 0);
}
template<std::size_t M>
Vec_n(const Vec_n<T, M>& data_)
{
std::size_t n = std::min(M, N);
for(std::size_t i = 0; i < n; ++i)
data[i] = data_[i];
}
Vec_n(const std::vector<T>& data_)
{
data.resize(N, 0);
std::size_t n = std::min(data_.size(), N);
for(std::size_t i = 0; i < n; ++i)
data[i] = data_[i];
}
bool operator == (const Vec_n<T, N>& other) const
{
for(std::size_t i = 0; i < N; ++i)
{
if(data[i] != other[i]) return false;
}
return true;
}
bool operator != (const Vec_n<T, N>& other) const
{
for(std::size_t i = 0; i < N; ++i)
{
if(data[i] != other[i]) return true;
}
return false;
}
std::size_t dim() const
{
return N;
}
void setData(const std::vector<T>& data_)
{
std::size_t n = std::min(data.size(), N);
for(std::size_t i = 0; i < n; ++i)
data[i] = data_[i];
}
T operator [] (std::size_t i) const
{
return data[i];
}
T& operator [] (std::size_t i)
{
return data[i];
}
Vec_n<T, N> operator + (const Vec_n<T, N>& other) const
{
Vec_n<T, N> result;
for(std::size_t i = 0; i < N; ++i)
result[i] = data[i] + other[i];
return result;
}
Vec_n<T, N>& operator += (const Vec_n<T, N>& other)
{
for(std::size_t i = 0; i < N; ++i)
data[i] += other[i];
return *this;
}
Vec_n<T, N> operator - (const Vec_n<T, N>& other) const
{
Vec_n<T, N> result;
for(std::size_t i = 0; i < N; ++i)
result[i] = data[i] - other[i];
return result;
}
Vec_n<T, N>& operator -= (const Vec_n<T, N>& other)
{
for(std::size_t i = 0; i < N; ++i)
data[i] -= other[i];
return *this;
}
Vec_n<T, N> operator * (T t) const
{
Vec_n<T, N> result;
for(std::size_t i = 0; i < N; ++i)
result[i] = data[i] * t;
return result;
}
Vec_n<T, N>& operator *= (T t)
{
for(std::size_t i = 0; i < N; ++i)
data[i] *= t;
return *this;
}
Vec_n<T, N> operator / (T t) const
{
Vec_n<T, N> result;
for(std::size_t i = 0; i < N; ++i)
result[i] = data[i] / 5;
return result;
}
Vec_n<T, N>& operator /= (T t)
{
for(std::size_t i = 0; i < N; ++i)
data[i] /= t;
return *this;
}
Vec_n<T, N>& setZero()
{
for(std::size_t i = 0; i < N; ++i)
data[i] = 0;
}
T dot(const Vec_n<T, N>& other) const
{
T sum = 0;
for(std::size_t i = 0; i < N; ++i)
sum += data[i] * other[i];
return sum;
}
std::vector<T> getData() const
{
return data;
}
protected:
std::vector<T> data;
};
template<typename T1, std::size_t N1,
typename T2, std::size_t N2>
void repack(const Vec_n<T1, N1>& input,
Vec_n<T2, N2>& output)
{
int n = std::min(N1, N2);
for(std::size_t i = 0; i < n; ++i)
output[i] = input[i];
}
template<typename T, std::size_t N>
Vec_n<T, N> operator * (T t, const Vec_n<T, N>& v)
{
return v * t;
}
template<typename T, std::size_t N, std::size_t M>
Vec_n<T, M+N> combine(const Vec_n<T, N>& v1, const Vec_n<T, M>& v2)
{
Vec_n<T, M+N> v;
for(std::size_t i = 0; i < N; ++i)
v[i] = v1[i];
for(std::size_t i = 0; i < M; ++i)
v[i + N] = v2[i];
return v;
}
template<typename T, std::size_t N>
std::ostream& operator << (std::ostream& o, const Vec_n<T, N>& v)
{
o << "(";
for(std::size_t i = 0; i < N; ++i)
{
if(i == N - 1)
o << v[i] << ")";
else
o << v[i] << " ";
}
return o;
}
// workaround for template alias
template<std::size_t N>
class Vecnf : public Vec_n<FCL_REAL, N>
{
public:
Vecnf(const Vec_n<FCL_REAL, N>& other) : Vec_n<FCL_REAL, N>(other)
{}
Vecnf() : Vec_n<FCL_REAL, N>()
{}
template<std::size_t M>
Vecnf(const Vec_n<FCL_REAL, M>& other) : Vec_n<FCL_REAL, N>(other)
{}
Vecnf(const std::vector<FCL_REAL>& data_) : Vec_n<FCL_REAL, N>(data_)
{}
};
}
#endif
include/fcl/narrowphase/gjk.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_GJK_H
#define FCL_GJK_H
#include "fcl/shape/geometric_shapes.h"
#include "fcl/math/transform.h"
namespace fcl
{
namespace details
{
/// @brief the support function for shape
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
/// @brief Minkowski difference class of two shapes
struct MinkowskiDiff
{
/// @brief points to two shapes
const ShapeBase* shapes[2];
/// @brief rotation from shape0 to shape1
Matrix3f toshape1;
/// @brief transform from shape1 to shape0
Transform3f toshape0;
MinkowskiDiff() { }
/// @brief support function for shape0
inline Vec3f support0(const Vec3f& d) const
{
return getSupport(shapes[0], d);
}
/// @brief support function for shape1
inline Vec3f support1(const Vec3f& d) const
{
return toshape0.transform(getSupport(shapes[1], toshape1 * d));
}
/// @brief support function for the pair of shapes
inline Vec3f support(const Vec3f& d) const
{
return support0(d) - support1(-d);
}
/// @brief support function for the d-th shape (d = 0 or 1)
inline Vec3f support(const Vec3f& d, size_t index) const
{
if(index)
return support1(d);
else
return support0(d);
}
/// @brief support function for translating shape0, which is translating at velocity v
inline Vec3f support0(const Vec3f& d, const Vec3f& v) const
{
if(d.dot(v) <= 0)
return getSupport(shapes[0], d);
else
return getSupport(shapes[0], d) + v;
}
/// @brief support function for the pair of shapes, where shape0 is translating at velocity v
inline Vec3f support(const Vec3f& d, const Vec3f& v) const
{
return support0(d, v) - support1(-d);
}
/// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v
inline Vec3f support(const Vec3f& d, const Vec3f& v, size_t index) const
{
if(index)
return support1(d);
else
return support0(d, v);
}
};
/// @brief class for GJK algorithm
struct GJK
{
struct SimplexV
{
/// @brief support direction
Vec3f d;
/// @brieg support vector (i.e., the furthest point on the shape along the support direction)
Vec3f w;
};
struct Simplex
{
/// @brief simplex vertex
SimplexV* c[4];
/// @brief weight
FCL_REAL p[4];
/// @brief size of simplex (number of vertices)
size_t rank;
Simplex() : rank(0) {}
};
enum Status {Valid, Inside, Failed};
MinkowskiDiff shape;
Vec3f ray;
FCL_REAL distance;
Simplex simplices[2];
GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_),
tolerance(tolerance_)
{
initialize();
}
void initialize();
/// @brief GJK algorithm, given the initial value guess
Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
/// @brief apply the support function along a direction, the result is return in sv
void getSupport(const Vec3f& d, SimplexV& sv) const;
/// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v
void getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const;
/// @brief discard one vertex from the simplex
void removeVertex(Simplex& simplex);
/// @brief append one vertex to the simplex
void appendVertex(Simplex& simplex, const Vec3f& v);
/// @brief whether the simplex enclose the origin
bool encloseOrigin();
/// @brief get the underlying simplex using in GJK, can be used for cache in next iteration
inline Simplex* getSimplex() const
{
return simplex;
}
/// @brief get the guess from current simplex
Vec3f getGuessFromSimplex() const;
private:
SimplexV store_v[4];
SimplexV* free_v[4];
size_t nfree;
size_t current;
Simplex* simplex;
Status status;
unsigned int max_iterations;
FCL_REAL tolerance;
};
static const size_t EPA_MAX_FACES = 128;
static const size_t EPA_MAX_VERTICES = 64;
static const FCL_REAL EPA_EPS = 0.000001;
static const size_t EPA_MAX_ITERATIONS = 255;
/// @brief class for EPA algorithm
struct EPA
{
private:
typedef GJK::SimplexV SimplexV;
struct SimplexF
{
Vec3f n;
FCL_REAL d;
SimplexV* c[3]; // a face has three vertices
SimplexF* f[3]; // a face has three adjacent faces
SimplexF* l[2]; // the pre and post faces in the list
size_t e[3];
size_t pass;
};
struct SimplexList
{
SimplexF* root;
size_t count;
SimplexList() : root(NULL), count(0) {}
void append(SimplexF* face)
{
face->l[0] = NULL;
face->l[1] = root;
if(root) root->l[0] = face;
root = face;
++count;
}
void remove(SimplexF* face)
{
if(face->l[1]) face->l[1]->l[0] = face->l[0];
if(face->l[0]) face->l[0]->l[1] = face->l[1];
if(face == root) root = face->l[1];
--count;
}
};
static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
{
fa->e[ea] = eb; fa->f[ea] = fb;
fb->e[eb] = ea; fb->f[eb] = fa;
}
struct SimplexHorizon
{
SimplexF* cf; // current face in the horizon
SimplexF* ff; // first face in the horizon
size_t nf; // number of faces in the horizon
SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
};
private:
unsigned int max_face_num;
unsigned int max_vertex_num;
unsigned int max_iterations;
FCL_REAL tolerance;
public:
enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
Status status;
GJK::Simplex result;
Vec3f normal;
FCL_REAL depth;
SimplexV* sv_store;
SimplexF* fc_store;
size_t nextsv;
SimplexList hull, stock;
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
max_vertex_num(max_vertex_num_),
max_iterations(max_iterations_),
tolerance(tolerance_)
{
initialize();
}
~EPA()
{
delete [] sv_store;
delete [] fc_store;
}
void initialize();
bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
/// @brief Find the best polytope face to split
SimplexF* findBest();
Status evaluate(GJK& gjk, const Vec3f& guess);
/// @brief the goal is to add a face connecting vertex w and face edge f[e]
bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
};
} // details
}
#endif
include/fcl/narrowphase/gjk_libccd.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_GJK_LIBCCD_H
#define FCL_GJK_LIBCCD_H
#include "fcl/shape/geometric_shapes.h"
#include "fcl/math/transform.h"
#include <ccd/ccd.h>
#include <ccd/quat.h>
namespace fcl
{
namespace details
{
/// @brief callback function used by GJK algorithm
typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
/// @brief initialize GJK stuffs
template<typename T>
class GJKInitializer
{
public:
/// @brief Get GJK support function
static GJKSupportFunction getSupportFunction() { return NULL; }
/// @brief Get GJK center function
static GJKCenterFunction getCenterFunction() { return NULL; }
/// @brief Get GJK object from a shape
/// Notice that only local transformation is applied.
/// Gloal transformation are considered later
static void* createGJKObject(const T& /* s */, const Transform3f& /*tf*/) { return NULL; }
/// @brief Delete GJK object
static void deleteGJKObject(void* o) {}
};
/// @brief initialize GJK Cylinder
template<>
class GJKInitializer<Cylinder>
{
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Cylinder& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
/// @brief initialize GJK Sphere
template<>
class GJKInitializer<Sphere>
{
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Sphere& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
/// @brief initialize GJK Box
template<>
class GJKInitializer<Box>
{
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Box& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
/// @brief initialize GJK Capsule
template<>
class GJKInitializer<Capsule>
{
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Capsule& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
/// @brief initialize GJK Cone
template<>
class GJKInitializer<Cone>
{
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Cone& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
/// @brief initialize GJK Convex
template<>
class GJKInitializer<Convex>
{
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Convex& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
/// @brief initialize GJK Triangle
GJKSupportFunction triGetSupportFunction();
GJKCenterFunction triGetCenterFunction();
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3);
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf);
void triDeleteGJKObject(void* o);
/// @brief GJK collision algorithm
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
unsigned int max_iterations, FCL_REAL tolerance,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal);
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
unsigned int max_iterations, FCL_REAL tolerance,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2);
} // details
}
#endif
include/fcl/narrowphase/narrowphase.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_NARROWPHASE_H
#define FCL_NARROWPHASE_H
#include "fcl/narrowphase/gjk.h"
#include "fcl/narrowphase/gjk_libccd.h"
namespace fcl
{
/// @brief collision and distance solver based on libccd library.
struct GJKSolver_libccd
{
/// @brief intersection checking between two shapes
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
max_collision_iterations, collision_tolerance,
contact_points, penetration_depth, normal);
details::GJKInitializer<S1>::deleteGJKObject(o1);
details::GJKInitializer<S2>::deleteGJKObject(o2);
return res;
}
/// @brief intersection checking between one shape and a triangle
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3);
bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
max_collision_iterations, collision_tolerance,
contact_points, penetration_depth, normal);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
return res;
}
//// @brief intersection checking between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
max_collision_iterations, collision_tolerance,
contact_points, penetration_depth, normal);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
return res;
}
/// @brief distance computation between two shapes
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
o2, details::GJKInitializer<S2>::getSupportFunction(),
max_distance_iterations, distance_tolerance,
dist, p1, p2);
if(p1) *p1 = inverse(tf1).transform(*p1);
if(p2) *p2 = inverse(tf2).transform(*p2);
details::GJKInitializer<S1>::deleteGJKObject(o1);
details::GJKInitializer<S2>::deleteGJKObject(o2);
return res;
}
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
FCL_REAL* dist) const
{
return shapeDistance(s1, tf1, s2, tf2, dist, NULL, NULL);
}
/// @brief distance computation between one shape and a triangle
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3);
bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
o2, details::triGetSupportFunction(),
max_distance_iterations, distance_tolerance,
dist, p1, p2);
if(p1) *p1 = inverse(tf).transform(*p1);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
return res;
}
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist)
{
return shapeTriangleDistance(s, tf, P1, P2, P3, dist, NULL, NULL);
}
/// @brief distance computation between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
o2, details::triGetSupportFunction(),
max_distance_iterations, distance_tolerance,
dist, p1, p2);
if(p1) *p1 = inverse(tf1).transform(*p1);
if(p2) *p2 = inverse(tf2).transform(*p2);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
return res;
}
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const
{
return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, NULL, NULL);
}
/// @brief default setting for GJK algorithm
GJKSolver_libccd()
{
max_collision_iterations = 500;
max_distance_iterations = 1000;
collision_tolerance = 1e-6;
distance_tolerance = 1e-6;
}
void enableCachedGuess(bool if_enable) const
{
// TODO: need change libccd to exploit spatial coherence
}
void setCachedGuess(const Vec3f& guess) const
{
// TODO: need change libccd to exploit spatial coherence
}
Vec3f getCachedGuess() const
{
return Vec3f(-1, 0, 0);
}
/// @brief maximum number of iterations used in GJK algorithm for collision
unsigned int max_collision_iterations;
/// @brief maximum number of iterations used in GJK algorithm for distance
unsigned int max_distance_iterations;
/// @brief the threshold used in GJK algorithm to stop collision iteration
FCL_REAL collision_tolerance;
/// @brief the threshold used in GJK algorithm to stop distance iteration
FCL_REAL distance_tolerance;
};
/// @brief Fast implementation for sphere-capsule collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-capsule distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver_indep
{
/// @brief intersection checking between two shapes
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.shapes[0] = &s1;
shape.shapes[1] = &s2;
shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
switch(gjk_status)
{
case details::GJK::Inside:
{
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0;
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = -epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
else return false;
}
break;
default:
;
}
return false;
}
/// @brief intersection checking between one shape and a triangle
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
TriangleP tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
shape.toshape1 = tf.getRotation();
shape.toshape0 = inverse(tf);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
switch(gjk_status)
{
case details::GJK::Inside:
{
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0;
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = -epa.normal;
if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
else return false;
}
break;
default:
;
}
return false;
}
//// @brief intersection checking between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
TriangleP tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
switch(gjk_status)
{
case details::GJK::Inside:
{
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0;
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = -epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
else return false;
}
break;
default:
;
}
return false;
}
/// @brief distance computation between two shapes
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
{
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.shapes[0] = &s1;
shape.shapes[1] = &s2;
shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
if(gjk_status == details::GJK::Valid)
{
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
if(distance) *distance = (w0 - w1).length();
if(p1) *p1 = w0;
if(p2) *p2 = shape.toshape0.transform(w1);
return true;
}
else
{
if(distance) *distance = -1;
return false;
}
}
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
FCL_REAL* distance) const
{
return shapeDistance(s1, tf1, s2, tf2, distance, NULL, NULL);
}
/// @brief distance computation between one shape and a triangle
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
{
TriangleP tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
shape.toshape1 = tf.getRotation();
shape.toshape0 = inverse(tf);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
if(gjk_status == details::GJK::Valid)
{
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
if(distance) *distance = (w0 - w1).length();
if(p1) *p1 = w0;
if(p2) *p2 = shape.toshape0.transform(w1);
return true;
}
else
{
if(distance) *distance = -1;
return false;
}
}
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* distance) const
{
return shapeTriangleDistance(s, tf, P1, P2, P3, distance, NULL, NULL);
}
/// @brief distance computation between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
{
TriangleP tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
if(gjk_status == details::GJK::Valid)
{
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
if(distance) *distance = (w0 - w1).length();
if(p1) *p1 = w0;
if(p2) *p2 = shape.toshape0.transform(w1);
return true;
}
else
{
if(distance) *distance = -1;
return false;
}
}
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* distance) const
{
return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, distance, NULL, NULL);
}
/// @brief default setting for GJK algorithm
GJKSolver_indep()
{
gjk_max_iterations = 128;
gjk_tolerance = 1e-6;
epa_max_face_num = 128;
epa_max_vertex_num = 64;
epa_max_iterations = 255;
epa_tolerance = 1e-6;
enable_cached_guess = false;
cached_guess = Vec3f(1, 0, 0);
}
void enableCachedGuess(bool if_enable) const
{
enable_cached_guess = if_enable;
}
void setCachedGuess(const Vec3f& guess) const
{
cached_guess = guess;
}
Vec3f getCachedGuess() const
{
return cached_guess;
}
/// @brief maximum number of simplex face used in EPA algorithm
unsigned int epa_max_face_num;
/// @brief maximum number of simplex vertex used in EPA algorithm
unsigned int epa_max_vertex_num;
/// @brief maximum number of iterations used for EPA iterations
unsigned int epa_max_iterations;
/// @brief the threshold used in EPA to stop iteration
FCL_REAL epa_tolerance;
/// @brief the threshold used in GJK to stop iteration
FCL_REAL gjk_tolerance;
/// @brief maximum number of iterations used for GJK iterations
FCL_REAL gjk_max_iterations;
/// @brief Whether smart guess can be provided
mutable bool enable_cached_guess;
/// @brief smart guess
mutable Vec3f cached_guess;
};
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
}
#endif
include/fcl/penetration_depth.h deleted 100644 → 0
View file @ 12194e71
#ifndef FCL_PENETRATION_DEPTH_H
#define FCL_PENETRATION_DEPTH_H
#include "fcl/math/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
#include "fcl/learning/classifier.h"
#include "fcl/knn/nearest_neighbors.h"
#include <boost/mem_fn.hpp>
#include <iostream>
namespace fcl
{
typedef double (*TransformDistance)(const Transform3f&, const Transform3f&);
class DefaultTransformDistancer
{
public:
const CollisionGeometry* o1;
const CollisionGeometry* o2;
FCL_REAL rot_x_weight, rot_y_weight, rot_z_weight;
DefaultTransformDistancer() : o1(NULL), o2(NULL)
{
rot_x_weight = rot_y_weight = rot_z_weight = 1;
}
DefaultTransformDistancer(const CollisionGeometry* o2_)
{
o2 = o2_;
init();
}
DefaultTransformDistancer(const CollisionGeometry* o1_, const CollisionGeometry* o2_)
{
o1 = o1_;
o2 = o2_;
init();
}
void init()
{
rot_x_weight = rot_y_weight = rot_z_weight = 1;
if(o2)
{
FCL_REAL V = o2->computeVolume();
Matrix3f C = o2->computeMomentofInertiaRelatedToCOM();
FCL_REAL eigen_values[3];
Vec3f eigen_vectors[3];
eigen(C, eigen_values, eigen_vectors);
rot_x_weight = eigen_values[0] * 4 / V;
rot_y_weight = eigen_values[1] * 4 / V;
rot_z_weight = eigen_values[2] * 4 / V;
// std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
}
}
void printRotWeight() const
{
std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
}
double distance(const Transform3f& tf1, const Transform3f& tf2)
{
Transform3f tf;
relativeTransform(tf1, tf2, tf);
const Quaternion3f& quat = tf.getQuatRotation();
const Vec3f& trans = tf.getTranslation();
FCL_REAL d = rot_x_weight * quat[1] * quat[1] + rot_y_weight * quat[2] * quat[2] + rot_z_weight * quat[3] * quat[3] + trans[0] * trans[0] + trans[1] * trans[1] + trans[2] * trans[2];
return d;
}
};
static DefaultTransformDistancer default_transform_distancer;
static NearestNeighbors<Transform3f>::DistanceFunction default_transform_distance_func = boost::bind(&DefaultTransformDistancer::distance, &default_transform_distancer, _1, _2);
/// @brief precompute a learning model for penetration computation
std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1,
const CollisionObject* o2,
PenetrationDepthType pd_type,
void* classifier,
std::size_t n_samples,
FCL_REAL margin,
KNNSolverType knn_solver_type,
std::size_t knn_k,
NearestNeighbors<Transform3f>::DistanceFunction distance_func = default_transform_distance_func);
/// @brief penetration depth computation between two in-collision objects
FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const PenetrationDepthRequest& request,
PenetrationDepthResult& result);
FCL_REAL penetrationDepth(const CollisionObject* o1,
const CollisionObject* o2,
const PenetrationDepthRequest& request,
PenetrationDepthResult& result);
}
#endif
include/fcl/profile.h deleted 100644 → 0
View file @ 12194e71
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/
/* Author Ioan Sucan */
#ifndef FCL_UTIL_PROFILER
#define FCL_UTIL_PROFILER
#define ENABLE_PROFILING 1
#ifndef ENABLE_PROFILING
/// The ENABLE_PROFILING macro can be set externally. If it is not, profiling is enabled by default, unless NDEBUG is defined.
# ifdef NDEBUG
# define ENABLE_PROFILING 0
# else
# define ENABLE_PROFILING 1
# endif
#endif
#if ENABLE_PROFILING
#include <map>
#include <string>
#include <iostream>
#include <boost/thread.hpp>
#include <boost/noncopyable.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
namespace fcl
{
/// @brief Namespace containing time datatypes and time operations
namespace time
{
/// @brief Representation of a point in time
typedef boost::posix_time::ptime point;
/// @brief Representation of a time duration
typedef boost::posix_time::time_duration duration;
/// @brief Get the current time point
inline point now(void)
{
return boost::posix_time::microsec_clock::universal_time();
}
/// @brief Return the time duration representing a given number of seconds
inline duration seconds(double sec)
{
long s = (long)sec;
long us = (long)((sec - s) * 1000000);
return boost::posix_time::seconds(s) + boost::posix_time::microseconds(us);
}
/// @brief Return the number of seconds that a time duration represents
inline double seconds(const duration &d)
{
return (double)d.total_microseconds() / 1000000.0;
}
}
namespace tools
{
/// @brief This is a simple thread-safe tool for counting time
/// spent in various chunks of code. This is different from
/// external profiling tools in that it allows the user to count
/// time spent in various bits of code (sub-function granularity)
/// or count how many times certain pieces of code are executed.
class Profiler : private boost::noncopyable
{
public:
/// @brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope.
class ScopedBlock
{
public:
/// @brief Start counting time for the block named \e name of the profiler \e prof
ScopedBlock(const std::string &name, Profiler &prof = Profiler::Instance()) : name_(name), prof_(prof)
{
prof_.begin(name);
}
~ScopedBlock(void)
{
prof_.end(name_);
}
private:
std::string name_;
Profiler &prof_;
};
/// @brief This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of scope.
/// If the profiler was already started, this block's constructor and destructor take no action
class ScopedStart
{
public:
/// @brief Take as argument the profiler instance to operate on (\e prof)
ScopedStart(Profiler &prof = Profiler::Instance()) : prof_(prof), wasRunning_(prof_.running())
{
if (!wasRunning_)
prof_.start();
}
~ScopedStart(void)
{
if (!wasRunning_)
prof_.stop();
}
private:
Profiler &prof_;
bool wasRunning_;
};
/// @brief Return an instance of the class
static Profiler& Instance(void);
/// @brief Constructor. It is allowed to separately instantiate this
/// class (not only as a singleton)
Profiler(bool printOnDestroy = false, bool autoStart = false) : running_(false), printOnDestroy_(printOnDestroy)
{
if (autoStart)
start();
}
/// @brief Destructor
~Profiler(void)
{
if (printOnDestroy_ && !data_.empty())
status();
}
/// @brief Start counting time
static void Start(void)
{
Instance().start();
}
/// @brief Stop counting time
static void Stop(void)
{
Instance().stop();
}
/// @brief Clear counted time and events
static void Clear(void)
{
Instance().clear();
}
/// @brief Start counting time
void start(void);
/// @brief Stop counting time
void stop(void);
/// @brief Clear counted time and events
void clear(void);
/// @brief Count a specific event for a number of times
static void Event(const std::string& name, const unsigned int times = 1)
{
Instance().event(name, times);
}
/// @brief Count a specific event for a number of times
void event(const std::string &name, const unsigned int times = 1);
/// @brief Maintain the average of a specific value
static void Average(const std::string& name, const double value)
{
Instance().average(name, value);
}
/// @brief Maintain the average of a specific value
void average(const std::string &name, const double value);
/// @brief Begin counting time for a specific chunk of code
static void Begin(const std::string &name)
{
Instance().begin(name);
}
/// @brief Stop counting time for a specific chunk of code
static void End(const std::string &name)
{
Instance().end(name);
}
/// @brief Begin counting time for a specific chunk of code
void begin(const std::string &name);
/// @brief Stop counting time for a specific chunk of code
void end(const std::string &name);
/// @brief Print the status of the profiled code chunks and
/// events. Optionally, computation done by different threads
/// can be printed separately.
static void Status(std::ostream &out = std::cout, bool merge = true)
{
Instance().status(out, merge);
}
/// @brief Print the status of the profiled code chunks and
/// events. Optionally, computation done by different threads
/// can be printed separately.
void status(std::ostream &out = std::cout, bool merge = true);
/// @brief Check if the profiler is counting time or not
bool running(void) const
{
return running_;
}
/// @brief Check if the profiler is counting time or not
static bool Running(void)
{
return Instance().running();
}
private:
/// @brief Information about time spent in a section of the code
struct TimeInfo
{
TimeInfo(void) : total(0, 0, 0, 0), shortest(boost::posix_time::pos_infin), longest(boost::posix_time::neg_infin), parts(0)
{
}
/// @brief Total time counted.
time::duration total;
/// @brief The shortest counted time interval
time::duration shortest;
/// @brief The longest counted time interval
time::duration longest;
/// @brief Number of times a chunk of time was added to this structure
unsigned long int parts;
/// @brief The point in time when counting time started
time::point start;
/// @brief Begin counting time
void set(void)
{
start = time::now();
}
/// @brief Add the counted time to the total time
void update(void)
{
const time::duration &dt = time::now() - start;
if (dt > longest)
longest = dt;
if (dt < shortest)
shortest = dt;
total = total + dt;
++parts;
}
};
/// @brief Information maintained about averaged values
struct AvgInfo
{
/// @brief The sum of the values to average
double total;
/// @brief The sub of squares of the values to average
double totalSqr;
/// @brief Number of times a value was added to this structure
unsigned long int parts;
};
/// @brief Information to be maintained for each thread
struct PerThread
{
/// @brief The stored events
std::map<std::string, unsigned long int> events;
/// @brief The stored averages
std::map<std::string, AvgInfo> avg;
/// @brief The amount of time spent in various places
std::map<std::string, TimeInfo> time;
};
void printThreadInfo(std::ostream &out, const PerThread &data);
boost::mutex lock_;
std::map<boost::thread::id, PerThread> data_;
TimeInfo tinfo_;
bool running_;
bool printOnDestroy_;
};
}
}
#else
#include <string>
#include <iostream>
/// If profiling is disabled, provide empty implementations for the public functions
namespace fcl
{
namespace tools
{
class Profiler
{
public:
class ScopedBlock
{
public:
ScopedBlock(const std::string &, Profiler & = Profiler::Instance())
{
}
~ScopedBlock(void)
{
}
};
class ScopedStart
{
public:
ScopedStart(Profiler & = Profiler::Instance())
{
}
~ScopedStart(void)
{
}
};
static Profiler& Instance(void);
Profiler(bool = true, bool = true)
{
}
~Profiler(void)
{
}
static void Start(void)
{
}
static void Stop(void)
{
}
static void Clear(void)
{
}
void start(void)
{
}
void stop(void)
{
}
void clear(void)
{
}
static void Event(const std::string&, const unsigned int = 1)
{
}
void event(const std::string &, const unsigned int = 1)
{
}
static void Average(const std::string&, const double)
{
}
void average(const std::string &, const double)
{
}
static void Begin(const std::string &)
{
}
static void End(const std::string &)
{
}
void begin(const std::string &)
{
}
void end(const std::string &)
{
}
static void Status(std::ostream & = std::cout, bool = true)
{
}
void status(std::ostream & = std::cout, bool = true)
{
}
bool running(void) const
{
return false;
}
static bool Running(void)
{
return false;
}
};
}
}
#endif
#endif
include/fcl/shape/geometric_shapes.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_GEOMETRIC_SHAPES_H
#define FCL_GEOMETRIC_SHAPES_H
#include "fcl/collision_object.h"
#include "fcl/math/vec_3f.h"
#include <string.h>
namespace fcl
{
/// @brief Base class for all basic geometric shapes
class ShapeBase : public CollisionGeometry
{
public:
ShapeBase() {}
/// @brief Get object type: a geometric shape
OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};
/// @brief Triangle stores the points instead of only indices of points
class TriangleP : public ShapeBase
{
public:
TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
{
}
/// @brief virtual function of compute AABB in local coordinate
void computeLocalAABB();
NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
Vec3f a, b, c;
};
/// @brief Center at zero point, axis aligned box
class Box : public ShapeBase
{
public:
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z)
{
}
Box(const Vec3f& side_) : ShapeBase(), side(side_)
{
}
Box() {}
/// @brief box side length
Vec3f side;
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a box
NODE_TYPE getNodeType() const { return GEOM_BOX; }
FCL_REAL computeVolume() const
{
return side[0] * side[1] * side[2];
}
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
FCL_REAL a2 = side[0] * side[0] * V;
FCL_REAL b2 = side[1] * side[1] * V;
FCL_REAL c2 = side[2] * side[2] * V;
return Matrix3f((b2 + c2) / 12, 0, 0,
0, (a2 + c2) / 12, 0,
0, 0, (a2 + b2) / 12);
}
};
/// @brief Center at zero point sphere
class Sphere : public ShapeBase
{
public:
Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
{
}
/// @brief Radius of the sphere
FCL_REAL radius;
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a sphere
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
Matrix3f computeMomentofInertia() const
{
FCL_REAL I = 0.4 * radius * radius * computeVolume();
return Matrix3f(I, 0, 0,
0, I, 0,
0, 0, I);
}
FCL_REAL computeVolume() const
{
return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius / 3;
}
};
/// @brief Center at zero point capsule
class Capsule : public ShapeBase
{
public:
Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
}
/// @brief Radius of capsule
FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a capsule
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
FCL_REAL computeVolume() const
{
return boost::math::constants::pi<FCL_REAL>() * radius * radius *(lz + radius * 4/3.0);
}
Matrix3f computeMomentofInertia() const
{
FCL_REAL v_cyl = radius * radius * lz * boost::math::constants::pi<FCL_REAL>();
FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz;
FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
return Matrix3f(ix, 0, 0,
0, ix, 0,
0, 0, iz);
}
};
/// @brief Center at zero cone
class Cone : public ShapeBase
{
public:
Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
}
/// @brief Radius of the cone
FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a cone
NODE_TYPE getNodeType() const { return GEOM_CONE; }
FCL_REAL computeVolume() const
{
return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz / 3;
}
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20);
FCL_REAL iz = 0.3 * V * radius * radius;
return Matrix3f(ix, 0, 0,
0, ix, 0,
0, 0, iz);
}
Vec3f computeCOM() const
{
return Vec3f(0, 0, -0.25 * lz);
}
};
/// @brief Center at zero cylinder
class Cylinder : public ShapeBase
{
public:
Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
}
/// @brief Radius of the cylinder
FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a cylinder
NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
FCL_REAL computeVolume() const
{
return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz;
}
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12;
FCL_REAL iz = V * radius * radius / 2;
return Matrix3f(ix, 0, 0,
0, ix, 0,
0, 0, iz);
}
};
/// @brief Convex polytope
class Convex : public ShapeBase
{
public:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
Convex(Vec3f* plane_normals_,
FCL_REAL* plane_dis_,
int num_planes_,
Vec3f* points_,
int num_points_,
int* polygons_) : ShapeBase()
{
plane_normals = plane_normals_;
plane_dis = plane_dis_;
num_planes = num_planes_;
points = points_;
polygons = polygons_;
edges = NULL;
Vec3f sum;
for(int i = 0; i < num_points; ++i)
{
sum += points[i];
}
center = sum * (FCL_REAL)(1.0 / num_points);
fillEdges();
}
/// @brief Copy constructor
Convex(const Convex& other) : ShapeBase(other)
{
plane_normals = other.plane_normals;
plane_dis = other.plane_dis;
num_planes = other.num_planes;
points = other.points;
polygons = other.polygons;
edges = new Edge[other.num_edges];
memcpy(edges, other.edges, sizeof(Edge) * num_edges);
}
~Convex()
{
delete [] edges;
}
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a conex polytope
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
Vec3f* plane_normals;
FCL_REAL* plane_dis;
/// @brief An array of indices to the points of each polygon, it should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order
int* polygons;
Vec3f* points;
int num_points;
int num_edges;
int num_planes;
struct Edge
{
int first, second;
};
Edge* edges;
/// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex)
Vec3f center;
/// based on http://number-none.com/blow/inertia/bb_inertia.doc
Matrix3f computeMomentofInertia() const
{
Matrix3f C(0, 0, 0,
0, 0, 0,
0, 0, 0);
Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0,
1/120.0, 1/60.0, 1/120.0,
1/120.0, 1/120.0, 1/60.0);
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
Vec3f plane_center;
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
plane_center += points[index[j]];
plane_center = plane_center * (1.0 / *points_in_poly);
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
const Vec3f& v3 = plane_center;
for(int j = 0; j < *points_in_poly; ++j)
{
int e_first = index[j];
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
Matrix3f A(v1, v2, v3); // this is A' in the original document
C += transpose(A) * C_canonical * A * d_six_vol; // change accordingly
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
}
FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2),
-C(1, 0), trace_C - C(1, 1), -C(1, 2),
-C(2, 0), -C(2, 1), trace_C - C(2, 2));
}
Vec3f computeCOM() const
{
Vec3f com;
FCL_REAL vol = 0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
Vec3f plane_center;
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
plane_center += points[index[j]];
plane_center = plane_center * (1.0 / *points_in_poly);
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
const Vec3f& v3 = plane_center;
for(int j = 0; j < *points_in_poly; ++j)
{
int e_first = index[j];
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
}
return com / (vol * 4); // here we choose zero as the reference
}
FCL_REAL computeVolume() const
{
FCL_REAL vol = 0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
Vec3f plane_center;
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
plane_center += points[index[j]];
plane_center = plane_center * (1.0 / *points_in_poly);
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape
const Vec3f& v3 = plane_center;
for(int j = 0; j < *points_in_poly; ++j)
{
int e_first = index[j];
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
}
return vol / 6;
}
protected:
/// @brief Get edge information
void fillEdges();
};
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
class Halfspace : public ShapeBase
{
public:
/// @brief Construct a half space with normal direction and offset
Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
{
unitNormalTest();
}
/// @brief Construct a plane with normal direction and offset
Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
{
unitNormalTest();
}
Halfspace() : ShapeBase(), n(1, 0, 0), d(0)
{
}
FCL_REAL signedDistance(const Vec3f& p) const
{
return n.dot(p) - d;
}
FCL_REAL distance(const Vec3f& p) const
{
return std::abs(n.dot(p) - d);
}
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a half space
NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
/// @brief Plane normal
Vec3f n;
/// @brief Plane offset
FCL_REAL d;
protected:
/// @brief Turn non-unit normal into unit
void unitNormalTest();
};
/// @brief Infinite plane
class Plane : public ShapeBase
{
public:
/// @brief Construct a plane with normal direction and offset
Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
{
unitNormalTest();
}
/// @brief Construct a plane with normal direction and offset
Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
{
unitNormalTest();
}
Plane() : ShapeBase(), n(1, 0, 0), d(0)
{}
FCL_REAL signedDistance(const Vec3f& p) const
{
return n.dot(p) - d;
}
FCL_REAL distance(const Vec3f& p) const
{
return std::abs(n.dot(p) - d);
}
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a plane
NODE_TYPE getNodeType() const { return GEOM_PLANE; }
/// @brief Plane normal
Vec3f n;
/// @brief Plane offset
FCL_REAL d;
protected:
/// @brief Turn non-unit normal into unit
void unitNormalTest();
};
}
#endif
include/fcl/shape/geometric_shapes_utility.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_GEOMETRIC_SHAPES_UTILITY_H
#define FCL_GEOMETRIC_SHAPES_UTILITY_H
#include <vector>
#include "fcl/shape/geometric_shapes.h"
#include "fcl/BV/BV.h"
namespace fcl
{
/// @cond IGNORE
namespace details
{
/// @brief get the vertices of some convex shape which can bound the given shape in a specific configuration
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const TriangleP& triangle, const Transform3f& tf);
}
/// @endcond
/// @brief calculate a bounding volume for a shape in a specific configuration
template<typename BV, typename S>
void computeBV(const S& s, const Transform3f& tf, BV& bv)
{
std::vector<Vec3f> convex_bound_vertices = details::getBoundVertices(s, tf);
fit(&convex_bound_vertices[0], (int)convex_bound_vertices.size(), bv);
}
template<>
void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv);
template<>
void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv);
template<>
void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv);
template<>
void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv);
template<>
void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv);
template<>
void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv);
/// @brief construct a box shape (with a configuration) from a given bounding volume
void constructBox(const AABB& bv, Box& box, Transform3f& tf);
void constructBox(const OBB& bv, Box& box, Transform3f& tf);
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf);
void constructBox(const kIOS& bv, Box& box, Transform3f& tf);
void constructBox(const RSS& bv, Box& box, Transform3f& tf);
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf);
void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf);
void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf);
void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
Halfspace transform(const Halfspace& a, const Transform3f& tf);
Plane transform(const Plane& a, const Transform3f& tf);
}
#endif
include/fcl/simd/math_simd_details.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_MATH_SIMD_DETAILS_H
#define FCL_MATH_SIMD_DETAILS_H
#include "fcl/data_types.h"
#include <xmmintrin.h>
#if defined (__SSE3__)
#include <pmmintrin.h>
#endif
#if defined (__SSE4__)
#include <smmintrin.h>
#endif
namespace fcl
{
/** \brief FCL internals. Ignore this :) unless you are God */
namespace details
{
const __m128 xmms_0 = {0.f, 0.f, 0.f, 0.f};
const __m128d xmmd_0 = {0, 0};
static inline __m128 vec_sel(__m128 a, __m128 b, __m128 mask)
{
return _mm_or_ps(_mm_and_ps(mask, b), _mm_andnot_ps(mask, a));
}
static inline __m128 vec_sel(__m128 a, __m128 b, const unsigned int* mask)
{
return vec_sel(a, b, _mm_load_ps((float*)mask));
}
static inline __m128 vec_sel(__m128 a, __m128 b, unsigned int mask)
{
return vec_sel(a, b, _mm_set1_ps(*(float*)&mask));
}
#define vec_splat(a, e) _mm_shuffle_ps((a), (a), _MM_SHUFFLE((e), (e), (e), (e)))
#define vec_splatd(a, e) _mm_shuffle_pd((a), (a), _MM_SHUFFLE2((e), (e)))
#define _mm_ror_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE(((e)+3)%4, ((e)+2)%4, ((e)+1)%4, (e)%4)) : (x))
#define _mm_rol_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE((7-(e))%4, (6-(e))%4, (5-(e))%4, (4-(e))%4)) : (x))
static inline __m128 newtonraphson_rsqrt4(const __m128 v)
{
static const union { float i[4]; __m128 m; } _half4 __attribute__ ((aligned(16))) = {{.5f, .5f, .5f, .5f}};
static const union { float i[4]; __m128 m; } _three __attribute__ ((aligned(16))) = {{3.f, 3.f, 3.f, 3.f}};
__m128 approx = _mm_rsqrt_ps(v);
__m128 muls = _mm_mul_ps(_mm_mul_ps(v, approx), approx);
return _mm_mul_ps(_mm_mul_ps(_half4.m, approx), _mm_sub_ps(_three.m, muls));
}
struct sse_meta_f4
{
typedef float meta_type;
union {float vs[4]; __m128 v; };
sse_meta_f4() : v(_mm_set1_ps(0)) {}
sse_meta_f4(float x) : v(_mm_set1_ps(x)) {}
sse_meta_f4(float* px) : v(_mm_load_ps(px)) {}
sse_meta_f4(__m128 x) : v(x) {}
sse_meta_f4(float x, float y, float z, float w = 1) : v(_mm_setr_ps(x, y, z, w)) {}
inline void setValue(float x, float y, float z, float w = 1) { v = _mm_setr_ps(x, y, z, w); }
inline void setValue(float x) { v = _mm_set1_ps(x); }
inline void setValue(__m128 x) { v = x; }
inline void negate() { v = _mm_sub_ps(xmms_0, v); }
inline sse_meta_f4& ubound(const sse_meta_f4& u)
{
v = _mm_min_ps(v, u.v);
return *this;
}
inline sse_meta_f4& lbound(const sse_meta_f4& l)
{
v = _mm_max_ps(v, l.v);
return *this;
}
inline void* operator new [] (size_t n) { return _mm_malloc(n, 16); }
inline void operator delete [] (void* x) { if(x) _mm_free(x); }
inline float operator [] (size_t i) const { return vs[i]; }
inline float& operator [] (size_t i) { return vs[i]; }
inline sse_meta_f4 operator + (const sse_meta_f4& other) const { return sse_meta_f4(_mm_add_ps(v, other.v)); }
inline sse_meta_f4 operator - (const sse_meta_f4& other) const { return sse_meta_f4(_mm_sub_ps(v, other.v)); }
inline sse_meta_f4 operator * (const sse_meta_f4& other) const { return sse_meta_f4(_mm_mul_ps(v, other.v)); }
inline sse_meta_f4 operator / (const sse_meta_f4& other) const { return sse_meta_f4(_mm_div_ps(v, other.v)); }
inline sse_meta_f4& operator += (const sse_meta_f4& other) { v = _mm_add_ps(v, other.v); return *this; }
inline sse_meta_f4& operator -= (const sse_meta_f4& other) { v = _mm_sub_ps(v, other.v); return *this; }
inline sse_meta_f4& operator *= (const sse_meta_f4& other) { v = _mm_mul_ps(v, other.v); return *this; }
inline sse_meta_f4& operator /= (const sse_meta_f4& other) { v = _mm_div_ps(v, other.v); return *this; }
inline sse_meta_f4 operator + (float t) const { return sse_meta_f4(_mm_add_ps(v, _mm_set1_ps(t))); }
inline sse_meta_f4 operator - (float t) const { return sse_meta_f4(_mm_sub_ps(v, _mm_set1_ps(t))); }
inline sse_meta_f4 operator * (float t) const { return sse_meta_f4(_mm_mul_ps(v, _mm_set1_ps(t))); }
inline sse_meta_f4 operator / (float t) const { return sse_meta_f4(_mm_div_ps(v, _mm_set1_ps(t))); }
inline sse_meta_f4& operator += (float t) { v = _mm_add_ps(v, _mm_set1_ps(t)); return *this; }
inline sse_meta_f4& operator -= (float t) { v = _mm_sub_ps(v, _mm_set1_ps(t)); return *this; }
inline sse_meta_f4& operator *= (float t) { v = _mm_mul_ps(v, _mm_set1_ps(t)); return *this; }
inline sse_meta_f4& operator /= (float t) { v = _mm_div_ps(v, _mm_set1_ps(t)); return *this; }
inline sse_meta_f4 operator - () const
{
static const union { int i[4]; __m128 m; } negativemask __attribute__ ((aligned(16))) = {{0x80000000, 0x80000000, 0x80000000, 0x80000000}};
return sse_meta_f4(_mm_xor_ps(negativemask.m, v));
}
} __attribute__ ((aligned (16)));
struct sse_meta_d4
{
typedef double meta_type;
union {double vs[4]; __m128d v[2]; };
sse_meta_d4()
{
setValue(0.0);
}
sse_meta_d4(double x)
{
setValue(x);
}
sse_meta_d4(double* px)
{
v[0] = _mm_load_pd(px);
v[1] = _mm_set_pd(0, *(px + 2));
}
sse_meta_d4(__m128d x, __m128d y)
{
v[0] = x;
v[1] = y;
}
sse_meta_d4(double x, double y, double z, double w = 0)
{
setValue(x, y, z, w);
}
inline void setValue(double x, double y, double z, double w = 0)
{
v[0] = _mm_setr_pd(x, y);
v[1] = _mm_setr_pd(z, w);
}
inline void setValue(double x)
{
v[0] = _mm_set1_pd(x);
v[1] = v[0];
}
inline void setValue(__m128d x, __m128d y)
{
v[0] = x;
v[1] = y;
}
inline void negate()
{
v[0] = _mm_sub_pd(xmmd_0, v[0]);
v[1] = _mm_sub_pd(xmmd_0, v[1]);
}
inline sse_meta_d4& ubound(const sse_meta_d4& u)
{
v[0] = _mm_min_pd(v[0], u.v[0]);
v[1] = _mm_min_pd(v[1], u.v[1]);
return *this;
}
inline sse_meta_d4& lbound(const sse_meta_d4& l)
{
v[0] = _mm_max_pd(v[0], l.v[0]);
v[1] = _mm_max_pd(v[1], l.v[1]);
return *this;
}
inline void* operator new [] (size_t n)
{
return _mm_malloc(n, 16);
}
inline void operator delete [] (void* x)
{
if(x) _mm_free(x);
}
inline double operator [] (size_t i) const { return vs[i]; }
inline double& operator [] (size_t i) { return vs[i]; }
inline sse_meta_d4 operator + (const sse_meta_d4& other) const { return sse_meta_d4(_mm_add_pd(v[0], other.v[0]), _mm_add_pd(v[1], other.v[1])); }
inline sse_meta_d4 operator - (const sse_meta_d4& other) const { return sse_meta_d4(_mm_sub_pd(v[0], other.v[0]), _mm_sub_pd(v[1], other.v[1])); }
inline sse_meta_d4 operator * (const sse_meta_d4& other) const { return sse_meta_d4(_mm_mul_pd(v[0], other.v[0]), _mm_mul_pd(v[1], other.v[1])); }
inline sse_meta_d4 operator / (const sse_meta_d4& other) const { return sse_meta_d4(_mm_div_pd(v[0], other.v[0]), _mm_div_pd(v[1], other.v[1])); }
inline sse_meta_d4& operator += (const sse_meta_d4& other) { v[0] = _mm_add_pd(v[0], other.v[0]); v[1] = _mm_add_pd(v[1], other.v[1]); return *this; }
inline sse_meta_d4& operator -= (const sse_meta_d4& other) { v[0] = _mm_sub_pd(v[0], other.v[0]); v[1] = _mm_sub_pd(v[1], other.v[1]); return *this; }
inline sse_meta_d4& operator *= (const sse_meta_d4& other) { v[0] = _mm_mul_pd(v[0], other.v[0]); v[1] = _mm_mul_pd(v[1], other.v[1]); return *this; }
inline sse_meta_d4& operator /= (const sse_meta_d4& other) { v[0] = _mm_div_pd(v[0], other.v[0]); v[1] = _mm_div_pd(v[1], other.v[1]); return *this; }
inline sse_meta_d4 operator + (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_add_pd(v[0], d), _mm_add_pd(v[1], d)); }
inline sse_meta_d4 operator - (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_sub_pd(v[0], d), _mm_sub_pd(v[1], d)); }
inline sse_meta_d4 operator * (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_mul_pd(v[0], d), _mm_mul_pd(v[1], d)); }
inline sse_meta_d4 operator / (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_div_pd(v[0], d), _mm_div_pd(v[1], d)); }
inline sse_meta_d4& operator += (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_add_pd(v[0], d); v[1] = _mm_add_pd(v[1], d); return *this; }
inline sse_meta_d4& operator -= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_sub_pd(v[0], d); v[1] = _mm_sub_pd(v[1], d); return *this; }
inline sse_meta_d4& operator *= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_mul_pd(v[0], d); v[1] = _mm_mul_pd(v[1], d); return *this; }
inline sse_meta_d4& operator /= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_div_pd(v[0], d); v[1] = _mm_div_pd(v[1], d); return *this; }
inline sse_meta_d4 operator - () const
{
static const union { FCL_INT64 i[2]; __m128d m; } negativemask __attribute__ ((aligned(16))) = {{0x8000000000000000, 0x8000000000000000}};
return sse_meta_d4(_mm_xor_pd(v[0], negativemask.m), _mm_xor_pd(v[1], negativemask.m));
}
} __attribute__ ((aligned (16)));
static inline __m128 cross_prod(__m128 x, __m128 y)
{
// set to a[1][2][0][3] , b[2][0][1][3]
// multiply
static const int s1 = _MM_SHUFFLE(3, 0, 2, 1);
static const int s2 = _MM_SHUFFLE(3, 1, 0, 2);
__m128 xa = _mm_mul_ps(_mm_shuffle_ps(x, x, s1), _mm_shuffle_ps(y, y, s2));
// set to a[2][0][1][3] , b[1][2][0][3]
// multiply
__m128 xb = _mm_mul_ps(_mm_shuffle_ps(x, x, s2), _mm_shuffle_ps(y, y, s1));
// subtract
return _mm_sub_ps(xa, xb);
}
static inline sse_meta_f4 cross_prod(const sse_meta_f4& x, const sse_meta_f4& y)
{
return sse_meta_f4(cross_prod(x.v, y.v));
}
static inline void cross_prod(__m128d x0, __m128d x1, __m128d y0, __m128d y1, __m128d* z0, __m128d* z1)
{
static const int s0 = _MM_SHUFFLE2(0, 0);
static const int s1 = _MM_SHUFFLE2(0, 1);
static const int s2 = _MM_SHUFFLE2(1, 0);
static const int s3 = _MM_SHUFFLE2(1, 1);
__m128d xa1 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s1), _mm_shuffle_pd(y1, y0, s0));
__m128d ya1 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s2), _mm_shuffle_pd(y0, y1, s3));
__m128d xa2 = _mm_mul_pd(_mm_shuffle_pd(x1, x0, s0), _mm_shuffle_pd(y0, y1, s1));
__m128d ya2 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s3), _mm_shuffle_pd(y0, y1, s2));
*z0 = _mm_sub_pd(xa1, xa2);
*z1 = _mm_sub_pd(ya1, ya2);
}
static inline sse_meta_d4 cross_prod(const sse_meta_d4& x, const sse_meta_d4& y)
{
__m128d z0, z1;
cross_prod(x.v[0], x.v[1], y.v[0], y.v[1], &z0, &z1);
return sse_meta_d4(z0, z1);
}
static inline __m128 dot_prod3(__m128 x, __m128 y)
{
register __m128 m = _mm_mul_ps(x, y);
return _mm_add_ps(_mm_shuffle_ps(m, m, _MM_SHUFFLE(0, 0, 0, 0)),
_mm_add_ps(vec_splat(m, 1), vec_splat(m, 2)));
}
static inline float dot_prod3(const sse_meta_f4& x, const sse_meta_f4& y)
{
return _mm_cvtss_f32(dot_prod3(x.v, y.v));
}
static inline __m128d dot_prod3(__m128d x0, __m128d x1, __m128d y0, __m128d y1)
{
register __m128d m1 = _mm_mul_pd(x0, y0);
register __m128d m2 = _mm_mul_pd(x1, y1);
return _mm_add_pd(_mm_add_pd(vec_splatd(m1, 0), vec_splatd(m1, 1)), vec_splatd(m2, 0));
}
static inline double dot_prod3(const sse_meta_d4& x, const sse_meta_d4& y)
{
double d;
_mm_storel_pd(&d, dot_prod3(x.v[0], x.v[1], y.v[0], y.v[1]));
return d;
}
static inline __m128 dot_prod4(__m128 x, __m128 y)
{
#if defined (__SSE4__)
return _mm_dp_ps(x, y, 0x71);
#elif defined (__SSE3__)
register __m128 t = _mm_mul_ps(x, y);
t = _mm_hadd_ps(t, t);
return _mm_hadd_ps(t, t);
#else
register __m128 s = _mm_mul_ps(x, y);
register __m128 r = _mm_add_ss(s, _mm_movehl_ps(s, s));
return _mm_add_ss(r, _mm_shuffle_ps(r, r, 1));
#endif
}
static inline float dot_prod4(const sse_meta_f4& x, const sse_meta_f4& y)
{
return _mm_cvtss_f32(dot_prod4(x.v, y.v));
}
static inline __m128d dot_prod4(__m128d x0, __m128d x1, __m128d y0, __m128d y1)
{
#if defined (__SSE4__)
register __m128d t1 = _mm_dp_pd(x0, y0, 0x31);
register __m128d t2 = _mm_dp_pd(x1, y1, 0x11);
return _mm_add_pd(t1, t2);
#elif defined (__SSE3__)
register __m128d t1 = _mm_mul_pd(x0, y0);
register __m128d t2 = _mm_mul_pd(x1, y1);
t1 = _mm_hadd_pd(t1, t1);
t2 = _mm_hadd_pd(t2, t2);
return _mm_add_pd(t1, t2);
#else
register __m128d t1 = _mm_mul_pd(x0, y0);
register __m128d t2 = _mm_mul_pd(x1, y1);
t1 = _mm_add_pd(t1, t2);
return _mm_add_pd(t1, _mm_shuffle_pd(t1, t1, 1));
#endif
}
static inline double dot_prod4(const sse_meta_d4& x, const sse_meta_d4& y)
{
double d;
_mm_storel_pd(&d, dot_prod4(x.v[0], x.v[1], y.v[0], y.v[1]));
return d;
}
static inline sse_meta_f4 min(const sse_meta_f4& x, const sse_meta_f4& y)
{
return sse_meta_f4(_mm_min_ps(x.v, y.v));
}
static inline sse_meta_d4 min(const sse_meta_d4& x, const sse_meta_d4& y)
{
return sse_meta_d4(_mm_min_pd(x.v[0], y.v[0]), _mm_min_pd(x.v[1], y.v[1]));
}
static inline sse_meta_f4 max(const sse_meta_f4& x, const sse_meta_f4& y)
{
return sse_meta_f4(_mm_max_ps(x.v, y.v));
}
static inline sse_meta_d4 max(const sse_meta_d4& x, const sse_meta_d4& y)
{
return sse_meta_d4(_mm_max_pd(x.v[0], y.v[0]), _mm_max_pd(x.v[1], y.v[1]));
}
static inline sse_meta_f4 abs(const sse_meta_f4& x)
{
static const union { int i[4]; __m128 m; } abs4mask __attribute__ ((aligned (16))) = {{0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff}};
return sse_meta_f4(_mm_and_ps(x.v, abs4mask.m));
}
static inline sse_meta_d4 abs(const sse_meta_d4& x)
{
static const union { FCL_INT64 i[2]; __m128d m; } abs2mask __attribute__ ((aligned (16))) = {{0x7fffffffffffffff, 0x7fffffffffffffff}};
return sse_meta_d4(_mm_and_pd(x.v[0], abs2mask.m), _mm_and_pd(x.v[1], abs2mask.m));
}
static inline bool equal(const sse_meta_f4& x, const sse_meta_f4& y, float epsilon)
{
register __m128 d = _mm_sub_ps(x.v, y.v);
register __m128 e = _mm_set1_ps(epsilon);
return ((_mm_movemask_ps(_mm_cmplt_ps(d, e)) & 0x7) == 0x7) && ((_mm_movemask_ps(_mm_cmpgt_ps(d, _mm_sub_ps(xmms_0, e))) & 0x7) == 0x7);
}
static inline bool equal(const sse_meta_d4& x, const sse_meta_d4& y, double epsilon)
{
register __m128d d = _mm_sub_pd(x.v[0], y.v[0]);
register __m128d e = _mm_set1_pd(epsilon);
if(_mm_movemask_pd(_mm_cmplt_pd(d, e)) != 0x3) return false;
if(_mm_movemask_pd(_mm_cmpgt_pd(d, _mm_sub_pd(xmmd_0, e))) != 0x3) return false;
d = _mm_sub_pd(x.v[1], y.v[1]);
if((_mm_movemask_pd(_mm_cmplt_pd(d, e)) & 0x1) != 0x1) return false;
if((_mm_movemask_pd(_mm_cmpgt_pd(d, _mm_sub_pd(xmmd_0, e))) & 0x1) != 0x1) return false;
return true;
}
static inline sse_meta_f4 normalize3(const sse_meta_f4& x)
{
register __m128 m = _mm_mul_ps(x.v, x.v);
__m128 r = _mm_add_ps(vec_splat(m, 0), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2)));
return sse_meta_f4(_mm_mul_ps(x.v, newtonraphson_rsqrt4(r)));
}
static inline sse_meta_f4 normalize3_approx(const sse_meta_f4& x)
{
register __m128 m = _mm_mul_ps(x.v, x.v);
__m128 r = _mm_add_ps(vec_splat(m, 0), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2)));
return sse_meta_f4(_mm_mul_ps(x.v, _mm_rsqrt_ps(r)));
}
static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128* r0, __m128* r1, __m128* r2)
{
static const union { unsigned int i[4]; __m128 m; } selectmask __attribute__ ((aligned(16))) = {{0, 0xffffffff, 0, 0}};
register __m128 t0, t1;
t0 = _mm_unpacklo_ps(c0, c2);
t1 = _mm_unpackhi_ps(c0, c2);
*r0 = _mm_unpacklo_ps(t0, c1);
*r1 = _mm_shuffle_ps(t0, t0, _MM_SHUFFLE(0, 3, 2, 2));
*r1 = vec_sel(*r1, c1, selectmask.i);
*r2 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(0, 1, 1, 0));
*r2 = vec_sel(*r2, vec_splat(c1, 2), selectmask.i);
}
static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128* i0, __m128* i1, __m128* i2)
{
__m128 t0, t1, t2, d, invd;
t2 = cross_prod(c0, c1);
t0 = cross_prod(c1, c2);
t1 = cross_prod(c2, c0);
d = dot_prod3(t2, c2);
d = vec_splat(d, 0);
invd = _mm_rcp_ps(d); // approximate inverse
transpose(t0, t1, t2, i0, i1, i2);
*i0 = _mm_mul_ps(*i0, invd);
*i1 = _mm_mul_ps(*i1, invd);
*i2 = _mm_mul_ps(*i2, invd);
}
struct sse_meta_f12
{
typedef float meta_type;
typedef sse_meta_f4 vector_type;
sse_meta_f4 c[3];
sse_meta_f12() { setZero(); }
sse_meta_f12(float xx, float xy, float xz,
float yx, float yy, float yz,
float zx, float zy, float zz)
{ setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); }
sse_meta_f12(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z)
{ setColumn(x, y, z); }
sse_meta_f12(__m128 x, __m128 y, __m128 z)
{ setColumn(x, y, z); }
inline void setValue(float xx, float xy, float xz,
float yx, float yy, float yz,
float zx, float zy, float zz)
{
c[0].setValue(xx, yx, zx, 0);
c[1].setValue(xy, yy, zy, 0);
c[2].setValue(xz, yz, zz, 0);
}
inline void setIdentity()
{
c[0].setValue(1, 0, 0, 0);
c[1].setValue(0, 1, 0, 0);
c[2].setValue(0, 0, 1, 0);
}
inline void setZero()
{
c[0].setValue(0);
c[1].setValue(0);
c[2].setValue(0);
}
inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z)
{
c[0] = x; c[1] = y; c[2] = z;
}
inline void setColumn(__m128 x, __m128 y, __m128 z)
{
c[0].setValue(x); c[1].setValue(y); c[2].setValue(z);
}
inline const sse_meta_f4& getColumn(size_t i) const
{
return c[i];
}
inline sse_meta_f4& getColumn(size_t i)
{
return c[i];
}
inline sse_meta_f4 getRow(size_t i) const
{
return sse_meta_f4(c[0][i], c[1][i], c[2][i], 0);
}
inline float operator () (size_t i, size_t j) const
{
return c[j][i];
}
inline float& operator () (size_t i, size_t j)
{
return c[j][i];
}
inline sse_meta_f4 operator * (const sse_meta_f4& v) const
{
return sse_meta_f4(_mm_add_ps(_mm_add_ps(_mm_mul_ps(c[0].v, vec_splat(v.v, 0)), _mm_mul_ps(c[1].v, vec_splat(v.v, 1))), _mm_mul_ps(c[2].v, vec_splat(v.v, 2))));
}
inline sse_meta_f12 operator * (const sse_meta_f12& mat) const
{
return sse_meta_f12((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]);
}
inline sse_meta_f12 operator + (const sse_meta_f12& mat) const
{
return sse_meta_f12(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2]);
}
inline sse_meta_f12 operator - (const sse_meta_f12& mat) const
{
return sse_meta_f12(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2]);
}
inline sse_meta_f12 operator + (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f12(c[0] + t, c[1] + t, c[2] + t);
}
inline sse_meta_f12 operator - (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f12(c[0] - t, c[1] - t, c[2] - t);
}
inline sse_meta_f12 operator * (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f12(c[0] * t, c[1] * t, c[2] * t);
}
inline sse_meta_f12 operator / (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f12(c[0] / t, c[1] / t, c[2] / t);
}
inline sse_meta_f12& operator *= (const sse_meta_f12& mat)
{
setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]);
return *this;
}
inline sse_meta_f12& operator += (const sse_meta_f12& mat)
{
c[0] += mat.c[0];
c[1] += mat.c[1];
c[2] += mat.c[2];
return *this;
}
inline sse_meta_f12& operator -= (const sse_meta_f12& mat)
{
c[0] -= mat.c[0];
c[1] -= mat.c[1];
c[2] -= mat.c[2];
return *this;
}
inline sse_meta_f12& operator += (float t_)
{
sse_meta_f4 t(t_);
c[0] += t;
c[1] += t;
c[2] += t;
return *this;
}
inline sse_meta_f12& operator -= (float t_)
{
sse_meta_f4 t(t_);
c[0] -= t;
c[1] -= t;
c[2] -= t;
return *this;
}
inline sse_meta_f12& operator *= (float t_)
{
sse_meta_f4 t(t_);
c[0] *= t;
c[1] *= t;
c[2] *= t;
return *this;
}
inline sse_meta_f12& operator /= (float t_)
{
sse_meta_f4 t(t_);
c[0] /= t;
c[1] /= t;
c[2] /= t;
return *this;
}
inline sse_meta_f12& inverse()
{
__m128 inv0, inv1, inv2;
details::inverse(c[0].v, c[1].v, c[2].v, &inv0, &inv1, &inv2);
setColumn(inv0, inv1, inv2);
return *this;
}
inline sse_meta_f12& transpose()
{
__m128 r0, r1, r2;
details::transpose(c[0].v, c[1].v, c[2].v, &r0, &r1, &r2);
setColumn(r0, r1, r2);
return *this;
}
inline sse_meta_f12& abs()
{
c[0] = details::abs(c[0]);
c[1] = details::abs(c[1]);
c[2] = details::abs(c[2]);
return *this;
}
inline float determinant() const
{
return _mm_cvtss_f32(dot_prod3(c[2].v, cross_prod(c[0].v, c[1].v)));
}
inline sse_meta_f12 transposeTimes(const sse_meta_f12& other) const
{
return sse_meta_f12(dot_prod3(c[0], other.c[0]), dot_prod3(c[0], other.c[1]), dot_prod3(c[0], other.c[2]),
dot_prod3(c[1], other.c[0]), dot_prod3(c[1], other.c[1]), dot_prod3(c[1], other.c[2]),
dot_prod3(c[2], other.c[0]), dot_prod3(c[2], other.c[1]), dot_prod3(c[2], other.c[2]));
}
inline sse_meta_f12 timesTranspose(const sse_meta_f12& m) const
{
sse_meta_f12 tmp(m);
return (*this) * tmp.transpose();
}
inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const
{
return sse_meta_f4(dot_prod3(c[0], v), dot_prod3(c[1], v), dot_prod3(c[2], v));
}
inline float transposeDot(size_t i, const sse_meta_f4& v) const
{
return dot_prod3(c[i], v);
}
inline float dot(size_t i, const sse_meta_f4& v) const
{
return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i];
}
};
static inline sse_meta_f12 abs(const sse_meta_f12& mat)
{
return sse_meta_f12(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)));
}
static inline sse_meta_f12 transpose(const sse_meta_f12& mat)
{
__m128 r0, r1, r2;
transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, &r0, &r1, &r2);
return sse_meta_f12(r0, r1, r2);
}
static inline sse_meta_f12 inverse(const sse_meta_f12& mat)
{
__m128 inv0, inv1, inv2;
inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, &inv0, &inv1, &inv2);
return sse_meta_f12(inv0, inv1, inv2);
}
static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128 c3,
__m128* r0, __m128* r1, __m128* r2, __m128* r3)
{
__m128 tmp0 = _mm_unpacklo_ps(c0, c2);
__m128 tmp1 = _mm_unpacklo_ps(c1, c3);
__m128 tmp2 = _mm_unpackhi_ps(c0, c2);
__m128 tmp3 = _mm_unpackhi_ps(c1, c3);
*r0 = _mm_unpacklo_ps(tmp0, tmp1);
*r1 = _mm_unpackhi_ps(tmp0, tmp1);
*r2 = _mm_unpacklo_ps(tmp2, tmp3);
*r3 = _mm_unpackhi_ps(tmp2, tmp3);
}
static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128 c3,
__m128* res0, __m128* res1, __m128* res2, __m128* res3)
{
__m128 Va, Vb, Vc;
__m128 r1, r2, r3, tt, tt2;
__m128 sum, Det, RDet;
__m128 trns0, trns1, trns2, trns3;
// Calculating the minterms for the first line.
tt = c3; tt2 = _mm_ror_ps(c2,1);
Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'\B7V4
Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'\B7V4"
Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'\B7V4^
r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"\B7V4^ - V3^\B7V4"
r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^\B7V4' - V3'\B7V4^
r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'\B7V4" - V3"\B7V4'
tt = c1;
Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
// Calculating the determinant.
Det = _mm_mul_ps(sum,c0);
Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
static const union { int i[4]; __m128 m; } Sign_PNPN __attribute__ ((aligned(16))) = {{0x00000000, 0x80000000, 0x00000000, 0x80000000}};
static const union { int i[4]; __m128 m; } Sign_NPNP __attribute__ ((aligned(16))) = {{0x80000000, 0x00000000, 0x80000000, 0x00000000}};
static const union { float i[4]; __m128 m; } ZERONE __attribute__ ((aligned(16))) = {{1.0f, 0.0f, 0.0f, 1.0f}};
__m128 mtL1 = _mm_xor_ps(sum,Sign_PNPN.m);
// Calculating the minterms of the second line (using previous results).
tt = _mm_ror_ps(c0,1); sum = _mm_mul_ps(tt,r1);
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
__m128 mtL2 = _mm_xor_ps(sum,Sign_NPNP.m);
// Testing the determinant.
Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
// Calculating the minterms of the third line.
tt = _mm_ror_ps(c0,1);
Va = _mm_mul_ps(tt,Vb); // V1'\B7V2"
Vb = _mm_mul_ps(tt,Vc); // V1'\B7V2^
Vc = _mm_mul_ps(tt,c1); // V1'\B7V2
r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V1"\B7V2^ - V1^\B7V2"
r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V1^\B7V2' - V1'\B7V2^
r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V1'\B7V2" - V1"\B7V2'
tt = _mm_ror_ps(c3,1); sum = _mm_mul_ps(tt,r1);
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
__m128 mtL3 = _mm_xor_ps(sum,Sign_PNPN.m);
// Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs).
RDet = _mm_div_ss(ZERONE.m, Det); // TODO: just 1.0f?
RDet = _mm_shuffle_ps(RDet,RDet,0x00);
// Devide the first 12 minterms with the determinant.
mtL1 = _mm_mul_ps(mtL1, RDet);
mtL2 = _mm_mul_ps(mtL2, RDet);
mtL3 = _mm_mul_ps(mtL3, RDet);
// Calculate the minterms of the forth line and devide by the determinant.
tt = _mm_ror_ps(c2,1); sum = _mm_mul_ps(tt,r1);
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
__m128 mtL4 = _mm_xor_ps(sum,Sign_NPNP.m);
mtL4 = _mm_mul_ps(mtL4, RDet);
// Now we just have to transpose the minterms matrix.
trns0 = _mm_unpacklo_ps(mtL1,mtL2);
trns1 = _mm_unpacklo_ps(mtL3,mtL4);
trns2 = _mm_unpackhi_ps(mtL1,mtL2);
trns3 = _mm_unpackhi_ps(mtL3,mtL4);
*res0 = _mm_movelh_ps(trns0,trns1);
*res1 = _mm_movehl_ps(trns1,trns0);
*res2 = _mm_movelh_ps(trns2,trns3);
*res3 = _mm_movehl_ps(trns3,trns2);
}
struct sse_meta_f16
{
typedef float meta_type;
typedef sse_meta_f4 vector_type;
sse_meta_f4 c[4];
sse_meta_f16() { setZero(); }
sse_meta_f16(float xx, float xy, float xz, float xw,
float yx, float yy, float yz, float yw,
float zx, float zy, float zz, float zw,
float wx, float wy, float wz, float ww)
{ setValue(xx, xy, xz, xw, yz, yy, yz, yw, zx, zy, zz, zw, wx, wy, wz, ww); }
sse_meta_f16(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z, const sse_meta_f4& w)
{ setColumn(x, y, z, w); }
sse_meta_f16(__m128 x, __m128 y, __m128 z, __m128 w)
{ setColumn(x, y, z, w); }
inline void setValue(float xx, float xy, float xz, float xw,
float yx, float yy, float yz, float yw,
float zx, float zy, float zz, float zw,
float wx, float wy, float wz, float ww)
{
c[0].setValue(xx, yx, zx, wx);
c[1].setValue(xy, yy, zy, wy);
c[2].setValue(xz, yz, zz, wz);
c[3].setValue(xw, yw, zw, ww);
}
inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z, const sse_meta_f4& w)
{
c[0] = x; c[1] = y; c[2] = z; c[3] = w;
}
inline void setColumn(__m128 x, __m128 y, __m128 z, __m128 w)
{
c[0].setValue(x); c[1].setValue(y); c[2].setValue(z); c[3].setValue(w);
}
inline void setIdentity()
{
c[0].setValue(1, 0, 0, 0);
c[1].setValue(0, 1, 0, 0);
c[2].setValue(0, 0, 1, 0);
c[3].setValue(0, 0, 0, 1);
}
inline void setZero()
{
c[0].setValue(0);
c[1].setValue(0);
c[2].setValue(0);
c[3].setValue(0);
}
inline const sse_meta_f4& getColumn(size_t i) const
{
return c[i];
}
inline sse_meta_f4& getColumn(size_t i)
{
return c[i];
}
inline sse_meta_f4 getRow(size_t i) const
{
return sse_meta_f4(c[0][i], c[1][i], c[2][i], c[3][i]);
}
inline float operator () (size_t i, size_t j) const
{
return c[j][i];
}
inline float& operator () (size_t i, size_t j)
{
return c[j][i];
}
inline sse_meta_f4 operator * (const sse_meta_f4& v) const
{
return sse_meta_f4(_mm_add_ps(_mm_add_ps(_mm_mul_ps(c[0].v, vec_splat(v.v, 0)), _mm_mul_ps(c[1].v, vec_splat(v.v, 1))),
_mm_add_ps(_mm_mul_ps(c[2].v, vec_splat(v.v, 2)), _mm_mul_ps(c[3].v, vec_splat(v.v, 3)))
));
}
inline sse_meta_f16 operator * (const sse_meta_f16& mat) const
{
return sse_meta_f16((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]);
}
inline sse_meta_f16 operator + (const sse_meta_f16& mat) const
{
return sse_meta_f16(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2], c[3] + mat.c[3]);
}
inline sse_meta_f16 operator - (const sse_meta_f16& mat) const
{
return sse_meta_f16(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2], c[3] - mat.c[3]);
}
inline sse_meta_f16 operator + (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f16(c[0] + t, c[1] + t, c[2] + t, c[3] + t);
}
inline sse_meta_f16 operator - (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f16(c[0] - t, c[1] - t, c[2] - t, c[3] - t);
}
inline sse_meta_f16 operator * (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f16(c[0] * t, c[1] * t, c[2] * t, c[3] * t);
}
inline sse_meta_f16 operator / (float t_) const
{
sse_meta_f4 t(t_);
return sse_meta_f16(c[0] / t, c[1] / t, c[2] / t, c[3] / t);
}
inline sse_meta_f16& operator *= (const sse_meta_f16& mat)
{
setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]);
return *this;
}
inline sse_meta_f16& operator += (const sse_meta_f16& mat)
{
c[0] += mat.c[0];
c[1] += mat.c[1];
c[2] += mat.c[2];
c[3] += mat.c[3];
return *this;
}
inline sse_meta_f16& operator -= (const sse_meta_f16& mat)
{
c[0] -= mat.c[0];
c[1] -= mat.c[1];
c[2] -= mat.c[2];
c[3] -= mat.c[3];
return *this;
}
inline sse_meta_f16& operator += (float t_)
{
sse_meta_f4 t(t_);
c[0] += t;
c[1] += t;
c[2] += t;
c[3] += t;
return *this;
}
inline sse_meta_f16& operator -= (float t_)
{
sse_meta_f4 t(t_);
c[0] -= t;
c[1] -= t;
c[2] -= t;
c[3] -= t;
return *this;
}
inline sse_meta_f16& operator *= (float t_)
{
sse_meta_f4 t(t_);
c[0] *= t;
c[1] *= t;
c[2] *= t;
c[3] *= t;
return *this;
}
inline sse_meta_f16& operator /= (float t_)
{
sse_meta_f4 t(t_);
c[0] /= t;
c[1] /= t;
c[2] /= t;
c[3] /= t;
return *this;
}
inline sse_meta_f16& abs()
{
c[0] = details::abs(c[0]);
c[1] = details::abs(c[1]);
c[2] = details::abs(c[2]);
c[3] = details::abs(c[3]);
return *this;
}
inline sse_meta_f16& inverse()
{
__m128 r0, r1, r2, r3;
details::inverse(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3);
setColumn(r0, r1, r2, r3);
return *this;
}
inline sse_meta_f16& transpose()
{
__m128 r0, r1, r2, r3;
details::transpose(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3);
setColumn(r0, r1, r2, r3);
return *this;
}
inline float determinant() const
{
__m128 Va, Vb, Vc;
__m128 r1, r2, r3, tt, tt2;
__m128 sum, Det;
__m128 _L1 = c[0].v;
__m128 _L2 = c[1].v;
__m128 _L3 = c[2].v;
__m128 _L4 = c[3].v;
// Calculating the minterms for the first line.
// _mm_ror_ps is just a macro using _mm_shuffle_ps().
tt = _L4; tt2 = _mm_ror_ps(_L3,1);
Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'·V4
Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'·V4"
Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'·V4^
r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"·V4^ - V3^·V4"
r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^·V4' - V3'·V4^
r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'·V4" - V3"·V4'
tt = _L2;
Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1);
Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));
// Calculating the determinant.
Det = _mm_mul_ps(sum,_L1);
Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
// Calculating the minterms of the second line (using previous results).
tt = _mm_ror_ps(_L1,1); sum = _mm_mul_ps(tt,r1);
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
// Testing the determinant.
Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
return _mm_cvtss_f32(Det);
}
inline sse_meta_f16 transposeTimes(const sse_meta_f16& other) const
{
return sse_meta_f16(dot_prod4(c[0], other.c[0]), dot_prod4(c[0], other.c[1]), dot_prod4(c[0], other.c[2]), dot_prod4(c[0], other.c[3]),
dot_prod4(c[1], other.c[0]), dot_prod4(c[1], other.c[1]), dot_prod4(c[1], other.c[2]), dot_prod4(c[1], other.c[3]),
dot_prod4(c[2], other.c[0]), dot_prod4(c[2], other.c[1]), dot_prod4(c[2], other.c[2]), dot_prod4(c[2], other.c[3]),
dot_prod4(c[3], other.c[0]), dot_prod4(c[3], other.c[1]), dot_prod4(c[3], other.c[2]), dot_prod4(c[3], other.c[3]));
}
inline sse_meta_f16 timesTranspose(const sse_meta_f16& m) const
{
sse_meta_f16 tmp(m);
return (*this) * tmp.transpose();
}
inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const
{
return sse_meta_f4(dot_prod4(c[0], v), dot_prod4(c[1], v), dot_prod4(c[2], v), dot_prod4(c[3], v));
}
inline float transposeDot(size_t i, const sse_meta_f4& v) const
{
return dot_prod4(c[i], v);
}
inline float dot(size_t i, const sse_meta_f4& v) const
{
return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i] + v[3] * c[3][i];
}
};
static inline sse_meta_f16 abs(const sse_meta_f16& mat)
{
return sse_meta_f16(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)), abs(mat.getColumn(3)));
}
static inline sse_meta_f16 transpose(const sse_meta_f16& mat)
{
__m128 r0, r1, r2, r3;
transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3);
return sse_meta_f16(r0, r1, r2, r3);
}
static inline sse_meta_f16 inverse(const sse_meta_f16& mat)
{
__m128 r0, r1, r2, r3;
inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3);
return sse_meta_f16(r0, r1, r2, r3);
}
} // details
} // fcl
#endif
include/fcl/simd/simd_intersect.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_MULTIPLE_INTERSECT
#define FCL_MULTIPLE_INTERSECT
#include "fcl/math/vec_3f.h"
#include <xmmintrin.h>
#include <pmmintrin.h>
namespace fcl
{
static inline __m128 sse_four_spheres_intersect(const Vec3f& o1, FCL_REAL r1,
const Vec3f& o2, FCL_REAL r2,
const Vec3f& o3, FCL_REAL r3,
const Vec3f& o4, FCL_REAL r4,
const Vec3f& o5, FCL_REAL r5,
const Vec3f& o6, FCL_REAL r6,
const Vec3f& o7, FCL_REAL r7,
const Vec3f& o8, FCL_REAL r8)
{
__m128 PX, PY, PZ, PR, QX, QY, QZ, QR;
PX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
PY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
PZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
PR = _mm_set_ps(r1, r2, r3, r4);
QX = _mm_set_ps(o5[0], o6[0], o7[0], o8[0]);
QY = _mm_set_ps(o5[1], o6[1], o7[1], o8[1]);
QZ = _mm_set_ps(o5[2], o6[2], o7[2], o8[2]);
QR = _mm_set_ps(r5, r6, r7, r8);
__m128 T1 = _mm_sub_ps(PX, QX);
__m128 T2 = _mm_sub_ps(PY, QY);
__m128 T3 = _mm_sub_ps(PZ, QZ);
__m128 T4 = _mm_add_ps(PR, QR);
T1 = _mm_mul_ps(T1, T1);
T2 = _mm_mul_ps(T2, T2);
T3 = _mm_mul_ps(T3, T3);
T4 = _mm_mul_ps(T4, T4);
T1 = _mm_add_ps(T1, T2);
T2 = _mm_sub_ps(R2, T3);
return _mm_cmple_ps(T1, T2);
}
static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vec3f& o1, FCL_REAL r1,
const Vec3f& o2, FCL_REAL r2,
const Vec3f& o3, FCL_REAL r3,
const Vec3f& o4, FCL_REAL r4,
const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
const Vec3f& min4, const Vec3f& max4)
{
__m128 MINX, MINY, MINZ, MAXX, MAXX, MAXY, MAXZ, SX, SY, SZ, SR;
MINX = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
MAXX = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
MINY = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
MAXY = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
MINZ = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
MAXZ = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
SX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
SY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
SZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
SR = _mm_set_ps(r1, r2, r3, r4);
__m128 TX = _mm_max_ps(SX, MINX);
__m128 TY = _mm_max_ps(SY, MINY);
__m128 TZ = _mm_max_ps(SZ, MINZ);
TX = _mm_min_ps(TX, MAXX);
TY = _mm_min_ps(TY, MAXY);
TZ = _mm_min_ps(TZ, MAXZ);
__m128 DX = _mm_sub_ps(SX, TX);
__m128 DY = _mm_sub_ps(SY, TY);
__m128 DZ = _mm_sub_ps(SZ, TZ);
__m128 R2 = _mm_mul_ps(SR, SR);
DX = _mm_mul_ps(DX, DX);
DY = _mm_mul_ps(DY, DY);
DZ = _mm_mul_ps(DZ, DZ);
__m128 T1 = _mm_add_ps(DX, DY);
__m128 T2 = _mm_sub_ps(R2, DZ);
return _mm_cmple_ps(T1, T2);
}
static inline __m128 sse_four_AABBs_intersect(const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
const Vec3f& min4, const Vec3f& max4,
const Vec3f& min5, const Vec3f& max5,
const Vec3f& min6, const Vec3f& max6,
const Vec3f& min7, const Vec3f& max7,
const Vec3f& min8, const Vec3f& max8)
{
__m128 MIN1X, MIN1Y, MIN1Z, MAX1X, MAX1Y, MAX1Z, MIN2X, MIN2Y, MIN2Z, MAX2X, MAX2Y, MAX2Z;
MIN1X = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
MAX1X = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
MIN1Y = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
MAX1Y = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
MIN1Z = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
MAX1Z = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
MIN2X = _mm_set_ps(min5[0], min6[0], min7[0], min8[0]);
MAX2X = _mm_set_ps(max5[0], max6[0], max7[0], max8[0]);
MIN2Y = _mm_set_ps(min5[1], min6[1], min7[1], min8[1]);
MAX2Y = _mm_set_ps(max5[1], max6[1], max7[1], max8[1]);
MIN2Z = _mm_set_ps(min5[2], min6[2], min7[2], min8[2]);
MAX2Z = _mm_set_ps(max5[2], max6[2], max7[2], max8[2]);
__m128 AX = _mm_max_ps(MIN1X, MIN2X);
__m128 BX = _mm_min_ps(MAX1X, MAX2X);
__m128 AY = _mm_max_ps(MIN1Y, MIN2Y);
__m128 BY = _mm_min_ps(MAX1Y, MAX2Y);
__m128 AZ = _mm_max_ps(MIN1Z, MIN2Z);
__m128 BZ = _mm_min_ps(MAX1Z, MAX2Z);
__m128 T1 = _mm_cmple_ps(AX, BX);
__m128 T2 = _mm_cmple_ps(AY, BY);
__m128 T3 = _mm_cmple_ps(AZ, BZ);
__m128 T4 = _mm_and_ps(T1, T2);
return _mm_and_ps(T3, T4);
}
static bool four_spheres_intersect_and(const Vec3f& o1, FCL_REAL r1,
const Vec3f& o2, FCL_REAL r2,
const Vec3f& o3, FCL_REAL r3,
const Vec3f& o4, FCL_REAL r4,
const Vec3f& o5, FCL_REAL r5,
const Vec3f& o6, FCL_REAL r6,
const Vec3f& o7, FCL_REAL r7,
const Vec3f& o8, FCL_REAL r8)
{
__m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
return _mm_movemask_ps(CMP) == 15.f;
}
static bool four_spheres_intersect_or(const Vec3f& o1, FCL_REAL r1,
const Vec3f& o2, FCL_REAL r2,
const Vec3f& o3, FCL_REAL r3,
const Vec3f& o4, FCL_REAL r4,
const Vec3f& o5, FCL_REAL r5,
const Vec3f& o6, FCL_REAL r6,
const Vec3f& o7, FCL_REAL r7,
const Vec3f& o8, FCL_REAL r8)
{
__m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
return __mm_movemask_ps(CMP) > 0;
}
/** \brief four spheres versus four AABBs SIMD test */
static bool four_spheres_four_AABBs_intersect_and(const Vec3f& o1, FCL_REAL r1,
const Vec3f& o2, FCL_REAL r2,
const Vec3f& o3, FCL_REAL r3,
const Vec3f& o4, FCL_REAL r4,
const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
const Vec3f& min4, const Vec3f& max4)
{
__m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
return _mm_movemask_ps(CMP) == 15.f;
}
static bool four_spheres_four_AABBs_intersect_or(const Vec3f& o1, FCL_REAL r1,
const Vec3f& o2, FCL_REAL r2,
const Vec3f& o3, FCL_REAL r3,
const Vec3f& o4, FCL_REAL r4,
const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
const Vec3f& min4, const Vec3f& max4)
{
__m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
return _mm_movemask_ps(CMP) > 0;
}
/** \brief four AABBs versus four AABBs SIMD test */
static bool four_AABBs_intersect_and(const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
const Vec3f& min4, const Vec3f& max4,
const Vec3f& min5, const Vec3f& max5,
const Vec3f& min6, const Vec3f& max6,
const Vec3f& min7, const Vec3f& max7,
const Vec3f& min8, const Vec3f& max8)
{
__m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
return _mm_movemask_ps(CMP) == 15.f;
}
static bool four_AABBs_intersect_or(const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
const Vec3f& min4, const Vec3f& max4,
const Vec3f& min5, const Vec3f& max5,
const Vec3f& min6, const Vec3f& max6,
const Vec3f& min7, const Vec3f& max7,
const Vec3f& min8, const Vec3f& max8)
{
__m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
return _mm_movemask_ps(CMP) > 0;
}
}
#endif
include/fcl/traversal/traversal_node_bvh_shape.h deleted 100644 → 0
View file @ 12194e71
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*/
/** \author Jia Pan */
#ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H
#define FCL_TRAVERSAL_NODE_MESH_SHAPE_H
#include "fcl/collision_data.h"
#include "fcl/shape/geometric_shapes.h"
#include "fcl/shape/geometric_shapes_utility.h"
#include "fcl/traversal/traversal_node_base.h"
#include "fcl/BVH/BVH_model.h"
namespace fcl
{
/// @brief Traversal node for collision between BVH and shape
template<typename BV, typename S>
class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
{
model1 = NULL;
model2 = NULL;
num_bv_tests = 0;
num_leaf_tests = 0;
query_time_seconds = 0.0;
}
/// @brief Whether the BV node in the first BVH tree is leaf
bool isFirstNodeLeaf(int b) const
{
return model1->getBV(b).isLeaf();
}
/// @brief Obtain the left child of BV node in the first BVH
int getFirstLeftChild(int b) const
{
return model1->getBV(b).leftChild();
}
/// @brief Obtain the right child of BV node in the first BVH
int getFirstRightChild(int b) const
{
return model1->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) num_bv_tests++;
return !model1->getBV(b1).bv.overlap(model2_bv);
}
const BVHModel<BV>* model1;
const S* model2;
BV model2_bv;
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
};
/// @brief Traversal node for collision between shape and BVH
template<typename S, typename BV>
class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
ShapeBVHCollisionTraversalNode() : CollisionTraversalNodeBase()
{
model1 = NULL;
model2 = NULL;
num_bv_tests = 0;
num_leaf_tests = 0;
query_time_seconds = 0.0;
}
/// @brief Alway extend the second model, which is a BVH model
bool firstOverSecond(int, int) const
{
return false;
}
/// @brief Whether the BV node in the second BVH tree is leaf
bool isSecondNodeLeaf(int b) const
{
return model2->getBV(b).isLeaf();
}
/// @brief Obtain the left child of BV node in the second BVH
int getSecondLeftChild(int b) const
{
return model2->getBV(b).leftChild();
}
/// @brief Obtain the right child of BV node in the second BVH
int getSecondRightChild(int b) const
{
return model2->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) num_bv_tests++;
return !model2->getBV(b2).bv.overlap(model1_bv);
}
const S* model1;
const BVHModel<BV>* model2;
BV model1_bv;
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
};
/// @brief Traversal node for collision between mesh and shape
template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
public:
MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode<BV, S>()
{
vertices = NULL;
tri_indices = NULL;
nsolver = NULL;
}
/// @brief Intersection testing between leaves (one triangle and one shape)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
if(this->model1->isOccupied() && this->model2->isOccupied())
{
bool is_intersect = false;
if(!this->request.enable_contact)
{
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
is_intersect = true;
if(this->request.num_max_contacts > this->result->numContacts())
this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
}
}
else
{
FCL_REAL penetration;
Vec3f normal;
Vec3f contactp;
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
{
is_intersect = true;
if(this->request.num_max_contacts > this->result->numContacts())
this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
}
}
if(is_intersect && this->request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
/// @brief Whether the traversal process can stop early
bool canStop() const
{
return this->request.isSatisfied(*(this->result));
}
Vec3f* vertices;
Triangle* tri_indices;
FCL_REAL cost_density;
const NarrowPhaseSolver* nsolver;
};
/// @cond IGNORE
namespace details
{
template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices,
const Transform3f& tf1,
const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
FCL_REAL cost_density,
int& num_leaf_tests,
const CollisionRequest& request,
CollisionResult& result)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node = model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
if(model1->isOccupied() && model2.isOccupied())
{
bool is_intersect = false;
if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
{
is_intersect = true;
if(request.num_max_contacts > result.numContacts())
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
}
}
else
{
FCL_REAL penetration;
Vec3f normal;
Vec3f contactp;
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
{
is_intersect = true;
if(request.num_max_contacts > result.numContacts())
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
}
}
if(is_intersect && request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
/* bool res = */ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
else if((!model1->isFree() || model2.isFree()) && request.enable_cost)
{
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
/* bool res = */ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
}
}
/// @endcond
/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
{
public:
MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
{
public:
MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
{
public:
MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>
{
public:
MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
}
};
/// @brief Traversal node for collision between shape and mesh
template<typename S, typename BV, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
{
public:
ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>()
{
vertices = NULL;
tri_indices = NULL;
nsolver = NULL;
}
/// @brief Intersection testing between leaves (one shape and one triangle)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model2->getBV(b2);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
if(this->model1->isOccupied() && this->model2->isOccupied())
{
bool is_intersect = false;
if(!this->request.enable_contact)
{
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
is_intersect = true;
if(this->request.num_max_contacts > this->result->numContacts())
this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
}
}
else
{
FCL_REAL penetration;
Vec3f normal;
Vec3f contactp;
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
{
is_intersect = true;
if(this->request.num_max_contacts > this->result->numContacts())
this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
}
}
if(is_intersect && this->request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
/// @brief Whether the traversal process can stop early
bool canStop() const
{
return this->request.isSatisfied(*(this->result));
}
Vec3f* vertices;
Triangle* tri_indices;
FCL_REAL cost_density;
const NarrowPhaseSolver* nsolver;
};
/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>
{
public:
ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
// may need to change the order in pairs
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>
{
public:
ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
// may need to change the order in pairs
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>
{
public:
ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
// may need to change the order in pairs
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>
{
public:
ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
{
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
// may need to change the order in pairs
}
};
/// @brief Traversal node for distance computation between BVH and shape
template<typename BV, typename S>
class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
{
model1 = NULL;
model2 = NULL;
num_bv_tests = 0;
num_leaf_tests = 0;
query_time_seconds = 0.0;
}
/// @brief Whether the BV node in the first BVH tree is leaf
bool isFirstNodeLeaf(int b) const
{
return model1->getBV(b).isLeaf();
}
/// @brief Obtain the left child of BV node in the first BVH
int getFirstLeftChild(int b) const
{
return model1->getBV(b).leftChild();
}
/// @brief Obtain the right child of BV node in the first BVH
int getFirstRightChild(int b) const
{
return model1->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
return model1->getBV(b1).bv.distance(model2_bv);
}
const BVHModel<BV>* model1;
const S* model2;
BV model2_bv;
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
};
/// @brief Traversal node for distance computation between shape and BVH
template<typename S, typename BV>
class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase()
{
model1 = NULL;
model2 = NULL;
num_bv_tests = 0;
num_leaf_tests = 0;
query_time_seconds = 0.0;
}
/// @brief Whether the BV node in the second BVH tree is leaf
bool isSecondNodeLeaf(int b) const
{
return model2->getBV(b).isLeaf();
}
/// @brief Obtain the left child of BV node in the second BVH
int getSecondLeftChild(int b) const
{
return model2->getBV(b).leftChild();
}
/// @brief Obtain the right child of BV node in the second BVH
int getSecondRightChild(int b) const
{
return model2->getBV(b).rightChild();
}
/// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
return model1_bv.distance(model2->getBV(b2).bv);
}
const S* model1;
const BVHModel<BV>* model2;
BV model1_bv;
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
};
/// @brief Traversal node for distance between mesh and shape
template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
{
public:
MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>()
{
vertices = NULL;
tri_indices = NULL;
rel_err = 0;
abs_err = 0;
nsolver = NULL;
}
/// @brief Distance testing between leaves (one triangle and one shape)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL d;
Vec3f closest_p1, closest_p2;
nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &closest_p2, &closest_p1);
this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2);
}
/// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
return true;
return false;
}
Vec3f* vertices;
Triangle* tri_indices;
FCL_REAL rel_err;
FCL_REAL abs_err;
const NarrowPhaseSolver* nsolver;
};
/// @cond IGNORE
namespace details
{
template<typename BV, typename S, typename NarrowPhaseSolver>
void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */,
const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices,
const Transform3f& tf1,
const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
int & num_leaf_tests,
const DistanceRequest& /* request */,
DistanceResult& result)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node = model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL distance;
Vec3f closest_p1, closest_p2;
nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1);
result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2);
}
template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
const S& model2, const Transform3f& tf1, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& /* request */,
DistanceResult& result)
{
const Triangle& init_tri = tri_indices[init_tri_id];
const Vec3f& p1 = vertices[init_tri[0]];
const Vec3f& p2 = vertices[init_tri[1]];
const Vec3f& p3 = vertices[init_tri[2]];
FCL_REAL distance;
Vec3f closest_p1, closest_p2;
nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1);
result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, closest_p1, closest_p2);
}
}
/// @endcond
/// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>
{
public:
MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>()
{
}
void preprocess()
{
details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
*(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
}
void postprocess()
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>
{
public:
MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>()
{
}
void preprocess()
{
details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
*(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
}
void postprocess()
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>
{
public:
MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
{
}
void preprocess()
{
details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
*(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
}
void postprocess()
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
/// @brief Traversal node for distance between shape and mesh
template<typename S, typename BV, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
{
public:
ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode<S, BV>()
{
vertices = NULL;
tri_indices = NULL;
rel_err = 0;
abs_err = 0;
nsolver = NULL;
}
/// @brief Distance testing between leaves (one shape and one triangle)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model2->getBV(b2);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& p1 = vertices[tri_id[0]];
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL distance;
Vec3f closest_p1, closest_p2;
nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance, &closest_p1, &closest_p2);
this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id, closest_p1, closest_p2);
}
/// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
return true;
return false;
}
Vec3f* vertices;
Triangle* tri_indices;
FCL_REAL rel_err;
FCL_REAL abs_err;
const NarrowPhaseSolver* nsolver;
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>
{
public:
ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
{
}
void preprocess()
{
details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
*(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result));
}
void postprocess()
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>
{
public:
ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
{
}
void preprocess()
{
details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
*(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
}
void postprocess()
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>
{
public:
ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
{
}
void preprocess()
{
details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
*(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
}
void postprocess()
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
/// @brief Traversal node for conservative advancement computation between BVH and shape
template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeConservativeAdvancementTraversalNode : public MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>
{
public:
MeshShapeConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>()
{
delta_t = 1;
toc = 0;
t_err = (FCL_REAL)0.0001;
w = w_;
motion1 = NULL;
motion2 = NULL;
}
/// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1);
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
/// @brief Conservative advancement testing between leaves (one triangle and one shape)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = this->tri_indices[primitive_id];
const Vec3f& p1 = this->vertices[tri_id[0]];
const Vec3f& p2 = this->vertices[tri_id[1]];
const Vec3f& p3 = this->vertices[tri_id[2]];
FCL_REAL d;
Vec3f P1, P2;
this->nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &P2, &P1);
if(d < this->min_distance)
{
this->min_distance = d;
closest_p1 = P1;
closest_p2 = P2;
last_tri_id = primitive_id;
}
Vec3f n = this->tf2.transform(P2) - P1; n.normalize();
// here n should be in global frame
TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n);
TBVMotionBoundVisitor<BV> mb_visitor2(this->model2_bv, -n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
FCL_REAL cur_delta_t;
if(bound <= d) cur_delta_t = 1;
else cur_delta_t = d / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
}
/// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
Vec3f n = this->tf2.transform(data.P2) - data.P1; n.normalize();
int c1 = data.c1;
TBVMotionBoundVisitor<BV> mb_visitor1(this->model1->getBV(c1).bv, n);
TBVMotionBoundVisitor<BV> mb_visitor2(this->model2_bv, -n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
FCL_REAL cur_delta_t;
if(bound < c) cur_delta_t = 1;
else cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
stack.pop_back();
return true;
}
else
{
stack.pop_back();
return false;
}
}
mutable FCL_REAL min_distance;
mutable Vec3f closest_p1, closest_p2;
mutable int last_tri_id;
/// @brief CA controlling variable: early stop for the early iterations of CA
FCL_REAL w;
/// @brief The time from beginning point
FCL_REAL toc;
FCL_REAL t_err;
/// @brief The delta_t each step
mutable FCL_REAL delta_t;
/// @brief Motions for the two objects in query
const MotionBase* motion1;
const MotionBase* motion2;
mutable std::vector<ConservativeAdvancementStackData> stack;
};
template<typename S, typename BV, typename NarrowPhaseSolver>
class ShapeMeshConservativeAdvancementTraversalNode : public ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>
{
public:
ShapeMeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>()
{
delta_t = 1;
toc = 0;
t_err = (FCL_REAL)0.0001;
w = w_;
motion1 = NULL;
motion2 = NULL;
}
/// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2);
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
/// @brief Conservative advancement testing between leaves (one triangle and one shape)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model2->getBV(b2);
int primitive_id = node.primitiveId();
const Triangle& tri_id = this->tri_indices[primitive_id];
const Vec3f& p1 = this->vertices[tri_id[0]];
const Vec3f& p2 = this->vertices[tri_id[1]];
const Vec3f& p3 = this->vertices[tri_id[2]];
FCL_REAL d;
Vec3f P1, P2;
this->nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d, &P1, &P2);
if(d < this->min_distance)
{
this->min_distance = d;
closest_p1 = P1;
closest_p2 = P2;
last_tri_id = primitive_id;
}
Vec3f n = P2 - this->tf1.transform(P1); n.normalize();
// here n should be in global frame
TBVMotionBoundVisitor<BV> mb_visitor1(this->model1_bv, n);
TriangleMotionBoundVisitor mb_visitor2(p1, p2, p3, -n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
FCL_REAL cur_delta_t;
if(bound <= d) cur_delta_t = 1;
else cur_delta_t = d / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
}
/// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
Vec3f n = data.P2 - this->tf1.transform(data.P1); n.normalize();
int c2 = data.c2;
TBVMotionBoundVisitor<BV> mb_visitor1(this->model1_bv, n);
TBVMotionBoundVisitor<BV> mb_visitor2(this->model2->getBV(c2).bv, -n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
FCL_REAL cur_delta_t;
if(bound < c) cur_delta_t = 1;
else cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
stack.pop_back();
return true;
}
else
{
stack.pop_back();
return false;
}
}
mutable FCL_REAL min_distance;
mutable Vec3f closest_p1, closest_p2;
mutable int last_tri_id;
/// @brief CA controlling variable: early stop for the early iterations of CA
FCL_REAL w;
/// @brief The time from beginning point
FCL_REAL toc;
FCL_REAL t_err;
/// @brief The delta_t each step
mutable FCL_REAL delta_t;
/// @brief Motions for the two objects in query
const MotionBase* motion1;
const MotionBase* motion2;
mutable std::vector<ConservativeAdvancementStackData> stack;
};
namespace details
{
template<typename BV, typename S, typename NarrowPhaseSolver>
void meshShapeConservativeAdvancementOrientedNodeLeafTesting(int b1, int /* b2 */,
const BVHModel<BV>* model1, const S& model2,
const BV& model2_bv,
Vec3f* vertices, Triangle* tri_indices,
const Transform3f& tf1,
const Transform3f& tf2,
const MotionBase* motion1, const MotionBase* motion2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
FCL_REAL& min_distance,
Vec3f& p1, Vec3f& p2,
int& last_tri_id,
FCL_REAL& delta_t,
int& num_leaf_tests)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node = model1->getBV(b1);
int primitive_id = node.primitiveId();
const Triangle& tri_id = tri_indices[primitive_id];
const Vec3f& t1 = vertices[tri_id[0]];
const Vec3f& t2 = vertices[tri_id[1]];
const Vec3f& t3 = vertices[tri_id[2]];
FCL_REAL distance;
Vec3f P1, P2;
nsolver->shapeTriangleDistance(model2, tf2, t1, t2, t3, tf1, &distance, &P2, &P1);
if(distance < min_distance)
{
min_distance = distance;
p1 = P1;
p2 = P2;
last_tri_id = primitive_id;
}
// n is in global frame
Vec3f n = P2 - P1; n.normalize();
TriangleMotionBoundVisitor mb_visitor1(t1, t2, t3, n);
TBVMotionBoundVisitor<BV> mb_visitor2(model2_bv, -n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
FCL_REAL cur_delta_t;
if(bound <= distance) cur_delta_t = 1;
else cur_delta_t = distance / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
}
template<typename BV, typename S>
bool meshShapeConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
FCL_REAL min_distance,
FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
const BVHModel<BV>* model1, const S& model2,
const BV& model2_bv,
const MotionBase* motion1, const MotionBase* motion2,
std::vector<ConservativeAdvancementStackData>& stack,
FCL_REAL& delta_t)
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
Vec3f n = data.P2 - data.P1; n.normalize();
int c1 = data.c1;
TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n);
TBVMotionBoundVisitor<BV> mb_visitor2(model2_bv, -n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
FCL_REAL bound = bound1 + bound2;
FCL_REAL cur_delta_t;
if(bound <= c) cur_delta_t = 1;
else cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
stack.pop_back();
return true;
}
else
{
stack.pop_back();
return false;
}
}
}
template<typename S, typename NarrowPhaseSolver>
class MeshShapeConservativeAdvancementTraversalNodeRSS : public MeshShapeConservativeAdvancementTraversalNode<RSS, S, NarrowPhaseSolver>
{
public:
MeshShapeConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode<RSS, S, NarrowPhaseSolver>(w_)
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
void leafTesting(int b1, int b2) const
{
details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2),
this->model2_bv,
this->vertices, this->tri_indices,
this->tf1, this->tf2,
this->motion1, this->motion2,
this->nsolver,
this->enable_statistics,
this->min_distance,
this->closest_p1, this->closest_p2,
this->last_tri_id,
this->delta_t,
this->num_leaf_tests);
}
bool canStop(FCL_REAL c) const
{
return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, this->w,
this->model1, *(this->model2), this->model2_bv,
this->motion1, this->motion2,
this->stack, this->delta_t);
}
};
template<typename S, typename NarrowPhaseSolver>
class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : public MeshShapeConservativeAdvancementTraversalNode<OBBRSS, S, NarrowPhaseSolver>
{
public:
MeshShapeConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode<OBBRSS, S, NarrowPhaseSolver>(w_)
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
void leafTesting(int b1, int b2) const
{
details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2),
this->model2_bv,
this->vertices, this->tri_indices,
this->tf1, this->tf2,
this->motion1, this->motion2,
this->nsolver,
this->enable_statistics,
this->min_distance,
this->closest_p1, this->closest_p2,
this->last_tri_id,
this->delta_t,
this->num_leaf_tests);
}
bool canStop(FCL_REAL c) const
{
return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, this->w,
this->model1, *(this->model2), this->model2_bv,
this->motion1, this->motion2,
this->stack, this->delta_t);
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshConservativeAdvancementTraversalNodeRSS : public ShapeMeshConservativeAdvancementTraversalNode<S, RSS, NarrowPhaseSolver>
{
public:
ShapeMeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode<S, RSS, NarrowPhaseSolver>(w_)
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
void leafTesting(int b1, int b2) const
{
details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1),
this->model1_bv,
this->vertices, this->tri_indices,
this->tf2, this->tf1,
this->motion2, this->motion1,
this->nsolver,
this->enable_statistics,
this->min_distance,
this->closest_p2, this->closest_p1,
this->last_tri_id,
this->delta_t,
this->num_leaf_tests);
}
bool canStop(FCL_REAL c) const
{
return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, this->w,
this->model2, *(this->model1), this->model1_bv,
this->motion2, this->motion1,
this->stack, this->delta_t);
}
};
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS : public ShapeMeshConservativeAdvancementTraversalNode<S, OBBRSS, NarrowPhaseSolver>
{
public:
ShapeMeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode<S, OBBRSS, NarrowPhaseSolver>(w_)
{
}
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
void leafTesting(int b1, int b2) const
{
details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1),
this->model1_bv,
this->vertices, this->tri_indices,
this->tf2, this->tf1,
this->motion2, this->motion1,
this->nsolver,
this->enable_statistics,
this->min_distance,
this->closest_p2, this->closest_p1,
this->last_tri_id,
this->delta_t,
this->num_leaf_tests);
}
bool canStop(FCL_REAL c) const
{
return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, this->w,
this->model2, *(this->model1), this->model1_bv,
this->motion2, this->motion1,
this->stack, this->delta_t);
}
};
}
#endif