geometric_shapes.cpp 5.23 KB
Newer Older
sachinc's avatar
sachinc committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
sachinc's avatar
sachinc committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  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.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
 *     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 */


39
40
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
sachinc's avatar
sachinc committed
41

42
43
#include <set>

44
45
namespace hpp
{
sachinc's avatar
sachinc committed
46
47
48
namespace fcl
{

49
void Convex::initialize()
sachinc's avatar
sachinc committed
50
{
51
52
53
54
  center.setZero();
  for(int i = 0; i < num_points; ++i)
    center += points[i];
  center /= num_points;
sachinc's avatar
sachinc committed
55

56
57
  int *points_in_poly = polygons;
  neighbors = new Neighbors[num_points];
sachinc's avatar
sachinc committed
58

59
60
  std::vector<std::set<unsigned int> > nneighbors (num_points);
  unsigned int c_nneighbors = 0;
sachinc's avatar
sachinc committed
61

62
  for (int l = 0; l < num_polygons; ++l)
sachinc's avatar
sachinc committed
63
  {
64
65
66
67
68
69
70
71
72
73
74
75
    int n = *points_in_poly;

    for (int j = 0; j < n; ++j) {
      int i = (j==0  ) ? n-1 : j-1;
      int k = (j==n-1) ? 0   : j+1;
      int pi = points_in_poly[i+1];
      int pj = points_in_poly[j+1];
      int pk = points_in_poly[k+1];
      // Update neighbors of pj;
      if (nneighbors[pj].count(pi) == 0) {
        c_nneighbors++;
        nneighbors[pj].insert(pi);
sachinc's avatar
sachinc committed
76
      }
77
78
79
      if (nneighbors[pj].count(pk) == 0) {
        c_nneighbors++;
        nneighbors[pj].insert(pk);
sachinc's avatar
sachinc committed
80
81
82
      }
    }

83
    points_in_poly += n+1;
sachinc's avatar
sachinc committed
84
85
  }

86
87
88
89
90
91
92
93
94
  nneighbors_ = new unsigned int[c_nneighbors];
  unsigned int* p_nneighbors = nneighbors_;
  for (int i = 0; i < num_points; ++i) {
    Neighbors& n = neighbors[i];
    if (nneighbors[i].size() >= std::numeric_limits<unsigned char>::max())
      throw std::logic_error ("Too many neighbors.");
    n.count_ = (unsigned char)nneighbors[i].size();
    n.n_     = p_nneighbors;
    p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
sachinc's avatar
sachinc committed
95
  }
96
  assert (p_nneighbors == nneighbors_ + c_nneighbors);
sachinc's avatar
sachinc committed
97
98
}

99
100
void Halfspace::unitNormalTest()
{
101
  FCL_REAL l = n.norm();
102
103
104
105
106
107
108
109
  if(l > 0)
  {
    FCL_REAL inv_l = 1.0 / l;
    n *= inv_l;
    d *= inv_l;
  }
  else
  {
110
    n << 1, 0, 0;
111
112
113
114
    d = 0;
  }  
}

sachinc's avatar
sachinc committed
115
116
void Plane::unitNormalTest()
{
117
  FCL_REAL l = n.norm();
sachinc's avatar
sachinc committed
118
119
  if(l > 0)
  {
120
    FCL_REAL inv_l = 1.0 / l;
121
    n *= inv_l;
sachinc's avatar
sachinc committed
122
123
124
125
    d *= inv_l;
  }
  else
  {
126
    n << 1, 0, 0;
sachinc's avatar
sachinc committed
127
128
129
130
    d = 0;
  }
}

131
132
133

void Box::computeLocalAABB()
{
134
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
135
  aabb_center = aabb_local.center();
136
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
137
138
139
140
}

void Sphere::computeLocalAABB()
{
141
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
142
  aabb_center = aabb_local.center();
143
144
145
146
147
  aabb_radius = radius;
}

void Capsule::computeLocalAABB()
{
148
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
149
  aabb_center = aabb_local.center();
150
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
151
152
153
154
}

void Cone::computeLocalAABB()
{
155
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
156
  aabb_center = aabb_local.center();
157
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
158
159
160
161
}

void Cylinder::computeLocalAABB()
{
162
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
163
  aabb_center = aabb_local.center();
164
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
165
166
167
168
}

void Convex::computeLocalAABB()
{
169
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
170
  aabb_center = aabb_local.center();
171
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
172
173
}

174
175
176
177
void Halfspace::computeLocalAABB()
{
  computeBV<AABB>(*this, Transform3f(), aabb_local);
  aabb_center = aabb_local.center();
178
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
179
180
}

181
182
void Plane::computeLocalAABB()
{
183
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
184
  aabb_center = aabb_local.center();
185
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
186
187
}

188
void TriangleP::computeLocalAABB()
189
{
190
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
191
  aabb_center = aabb_local.center();
192
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
193
194
195
}


sachinc's avatar
sachinc committed
196
}
197
198

} // namespace hpp