geometric_shapes.cpp 4.94 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
#include <hpp/fcl/shape/geometric_shapes.h>
Lucile Remigy's avatar
Lucile Remigy committed
40
#include <hpp/fcl/shape/geometric_shapes_utility.h>
sachinc's avatar
sachinc committed
41

42
43
namespace hpp
{
sachinc's avatar
sachinc committed
44
45
46
namespace fcl
{

47
ConvexBase::ConvexBase(bool own_storage, Vec3f* points_, int num_points_) :
48
49
  ShapeBase(),
  points       (points_),
50
51
  num_points   (num_points_),
  own_storage_ (own_storage)
sachinc's avatar
sachinc committed
52
{
53
54
  computeCenter();
}
sachinc's avatar
sachinc committed
55

56
57
58
59
ConvexBase::ConvexBase(const ConvexBase& other) :
  ShapeBase    (other),
  points       (other.points),
  num_points   (other.num_points),
60
61
  center       (other.center),
  own_storage_ (other.own_storage_)
62
{
63
64
65
66
67
  if (own_storage_) {
    points = new Vec3f[num_points];
    memcpy(points, other.points, sizeof(Vec3f) * num_points);
  }

68
  neighbors = new Neighbors[num_points];
69
  memcpy(neighbors, other.neighbors, sizeof(Neighbors) * num_points);
sachinc's avatar
sachinc committed
70

71
72
73
74
75
  int c_nneighbors = 0;
  for (int i = 0; i < num_points; ++i) c_nneighbors += neighbors[i].count();
  nneighbors_ = new unsigned int[c_nneighbors];
  memcpy(nneighbors_, other.nneighbors_, sizeof(unsigned int) * c_nneighbors);
}
sachinc's avatar
sachinc committed
76

77
78
79
80
ConvexBase::~ConvexBase ()
{
  delete [] neighbors;
  delete [] nneighbors_;
81
  if (own_storage_) delete [] points;
82
}
sachinc's avatar
sachinc committed
83

84
85
86
87
88
89
void ConvexBase::computeCenter()
{
  center.setZero();
  for(int i = 0; i < num_points; ++i)
    center += points[i];
  center /= num_points;
sachinc's avatar
sachinc committed
90
91
}

92
93
void Halfspace::unitNormalTest()
{
94
  FCL_REAL l = n.norm();
95
96
97
98
99
100
101
102
  if(l > 0)
  {
    FCL_REAL inv_l = 1.0 / l;
    n *= inv_l;
    d *= inv_l;
  }
  else
  {
103
    n << 1, 0, 0;
104
105
106
107
    d = 0;
  }  
}

sachinc's avatar
sachinc committed
108
109
void Plane::unitNormalTest()
{
110
  FCL_REAL l = n.norm();
sachinc's avatar
sachinc committed
111
112
  if(l > 0)
  {
113
    FCL_REAL inv_l = 1.0 / l;
114
    n *= inv_l;
sachinc's avatar
sachinc committed
115
116
117
118
    d *= inv_l;
  }
  else
  {
119
    n << 1, 0, 0;
sachinc's avatar
sachinc committed
120
121
122
123
    d = 0;
  }
}

124
125
void Box::computeLocalAABB()
{
126
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
127
  aabb_center = aabb_local.center();
128
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
129
130
131
132
}

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

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

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

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

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

166
167
168
169
void Halfspace::computeLocalAABB()
{
  computeBV<AABB>(*this, Transform3f(), aabb_local);
  aabb_center = aabb_local.center();
170
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
171
172
}

173
174
void Plane::computeLocalAABB()
{
175
  computeBV<AABB>(*this, Transform3f(), aabb_local);
jpan's avatar
jpan committed
176
  aabb_center = aabb_local.center();
177
  aabb_radius = (aabb_local.min_ - aabb_center).norm();
178
179
}

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


sachinc's avatar
sachinc committed
188
}
189
190

} // namespace hpp