FCL  0.6.0
Flexible Collision Library
collision_object-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_COLLISION_OBJECT_INL_H
39 #define FCL_COLLISION_OBJECT_INL_H
40 
41 #include "fcl/narrowphase/collision_object.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class CollisionObject<double>;
49 
50 //==============================================================================
51 template <typename S>
52 CollisionObject<S>::CollisionObject(
53  const std::shared_ptr<CollisionGeometry<S>>& cgeom_)
54  : cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3<S>::Identity())
55 {
56  if (cgeom)
57  {
58  cgeom->computeLocalAABB();
59  computeAABB();
60  }
61 }
62 
63 //==============================================================================
64 template <typename S>
65 CollisionObject<S>::CollisionObject(
66  const std::shared_ptr<CollisionGeometry<S>>& cgeom_,
67  const Transform3<S>& tf)
68  : cgeom(cgeom_), cgeom_const(cgeom_), t(tf)
69 {
70  cgeom->computeLocalAABB();
71  computeAABB();
72 }
73 
74 //==============================================================================
75 template <typename S>
76 CollisionObject<S>::CollisionObject(
77  const std::shared_ptr<CollisionGeometry<S>>& cgeom_,
78  const Matrix3<S>& R,
79  const Vector3<S>& T)
80  : cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3<S>::Identity())
81 {
82  t.linear() = R;
83  t.translation() = T;
84  cgeom->computeLocalAABB();
85  computeAABB();
86 }
87 
88 //==============================================================================
89 template <typename S>
90 CollisionObject<S>::~CollisionObject()
91 {
92  // Do nothing
93 }
94 
95 //==============================================================================
96 template <typename S>
98 {
99  return cgeom->getObjectType();
100 }
101 
102 //==============================================================================
103 template <typename S>
105 {
106  return cgeom->getNodeType();
107 }
108 
109 //==============================================================================
110 template <typename S>
112 {
113  return aabb;
114 }
115 
116 //==============================================================================
117 template <typename S>
119 {
120  if(t.linear().isIdentity())
121  {
122  aabb = translate(cgeom->aabb_local, t.translation());
123  }
124  else
125  {
126  Vector3<S> center = t * cgeom->aabb_center;
127  Vector3<S> delta = Vector3<S>::Constant(cgeom->aabb_radius);
128  aabb.min_ = center - delta;
129  aabb.max_ = center + delta;
130  }
131 }
132 
133 //==============================================================================
134 template <typename S>
136 {
137  return user_data;
138 }
139 
140 //==============================================================================
141 template <typename S>
143 {
144  user_data = data;
145 }
146 
147 //==============================================================================
148 template <typename S>
149 const Vector3<S> CollisionObject<S>::getTranslation() const
150 {
151  return t.translation();
152 }
153 
154 //==============================================================================
155 template <typename S>
156 const Matrix3<S> CollisionObject<S>::getRotation() const
157 {
158  return t.linear();
159 }
160 
161 //==============================================================================
162 template <typename S>
163 const Quaternion<S> CollisionObject<S>::getQuatRotation() const
164 {
165  return Quaternion<S>(t.linear());
166 }
167 
168 //==============================================================================
169 template <typename S>
170 const Transform3<S>& CollisionObject<S>::getTransform() const
171 {
172  return t;
173 }
174 
175 //==============================================================================
176 template <typename S>
177 void CollisionObject<S>::setRotation(const Matrix3<S>& R)
178 {
179  t.linear() = R;
180 }
181 
182 //==============================================================================
183 template <typename S>
184 void CollisionObject<S>::setTranslation(const Vector3<S>& T)
185 {
186  t.translation() = T;
187 }
188 
189 //==============================================================================
190 template <typename S>
191 void CollisionObject<S>::setQuatRotation(const Quaternion<S>& q)
192 {
193  t.linear() = q.toRotationMatrix();
194 }
195 
196 //==============================================================================
197 template <typename S>
198 void CollisionObject<S>::setTransform(const Matrix3<S>& R, const Vector3<S>& T)
199 {
200  setRotation(R);
201  setTranslation(T);
202 }
203 
204 //==============================================================================
205 template <typename S>
206 void CollisionObject<S>::setTransform(const Quaternion<S>& q, const Vector3<S>& T)
207 {
208  setQuatRotation(q);
209  setTranslation(T);
210 }
211 
212 //==============================================================================
213 template <typename S>
214 void CollisionObject<S>::setTransform(const Transform3<S>& tf)
215 {
216  t = tf;
217 }
218 
219 //==============================================================================
220 template <typename S>
222 {
223  return t.matrix().isIdentity();
224 }
225 
226 //==============================================================================
227 template <typename S>
229 {
230  t.setIdentity();
231 }
232 
233 //==============================================================================
234 template <typename S>
236 {
237  return cgeom.get();
238 }
239 
240 //==============================================================================
241 template <typename S>
242 const std::shared_ptr<const CollisionGeometry<S>>&
244 {
245  return cgeom_const;
246 }
247 
248 //==============================================================================
249 template <typename S>
251 {
252  return cgeom->cost_density;
253 }
254 
255 //==============================================================================
256 template <typename S>
258 {
259  cgeom->cost_density = c;
260 }
261 
262 //==============================================================================
263 template <typename S>
265 {
266  return cgeom->isOccupied();
267 }
268 
269 //==============================================================================
270 template <typename S>
272 {
273  return cgeom->isFree();
274 }
275 
276 //==============================================================================
277 template <typename S>
279 {
280  return cgeom->isUncertain();
281 }
282 
283 } // namespace fcl
284 
285 #endif
NODE_TYPE getNodeType() const
get the node type
Definition: collision_object-inl.h:104
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, halfspace, triangle), and octree
Definition: collision_geometry.h:54
bool isFree() const
whether the object is completely free
Definition: collision_object-inl.h:271
void setRotation(const Matrix3< S > &R)
set object&#39;s rotation matrix
Definition: collision_object-inl.h:177
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object-inl.h:221
S getCostDensity() const
get object&#39;s cost density
Definition: collision_object-inl.h:250
void setQuatRotation(const Quaternion< S > &q)
set object&#39;s quatenrion rotation
Definition: collision_object-inl.h:191
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
const Matrix3< S > getRotation() const
get matrix rotation of the object
Definition: collision_object-inl.h:156
void setCostDensity(S c)
set object&#39;s cost density
Definition: collision_object-inl.h:257
void setTranslation(const Vector3< S > &T)
set object&#39;s translation
Definition: collision_object-inl.h:184
void setTransform(const Matrix3< S > &R, const Vector3< S > &T)
set object&#39;s transform
Definition: collision_object-inl.h:198
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:59
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object-inl.h:228
void computeAABB()
compute the AABB in world space
Definition: collision_object-inl.h:118
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_geometry.h:51
const Transform3< S > & getTransform() const
get object&#39;s transform
Definition: collision_object-inl.h:170
const std::shared_ptr< const CollisionGeometry< S > > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object-inl.h:243
const AABB< S > & getAABB() const
get the AABB in world space
Definition: collision_object-inl.h:111
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object-inl.h:264
bool isUncertain() const
whether the object is uncertain
Definition: collision_object-inl.h:278
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object-inl.h:97
const Quaternion< S > getQuatRotation() const
get quaternion rotation of the object
Definition: collision_object-inl.h:163
const Vector3< S > getTranslation() const
get translation of the object
Definition: collision_object-inl.h:149
void * getUserData() const
get user data in object
Definition: collision_object-inl.h:135
void setUserData(void *data)
set user data in object
Definition: collision_object-inl.h:142
FCL_DEPRECATED const CollisionGeometry< S > * getCollisionGeometry() const
get geometry from the object instance
Definition: collision_object-inl.h:235