geometric_shapes.h 14.9 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 */


Joseph Mirabel's avatar
Joseph Mirabel committed
39
40
#ifndef HPP_FCL_GEOMETRIC_SHAPES_H
#define HPP_FCL_GEOMETRIC_SHAPES_H
sachinc's avatar
sachinc committed
41

42
43
#include <boost/math/constants/constants.hpp>

44
#include <hpp/fcl/collision_object.h>
45
#include <hpp/fcl/data_types.h>
sachinc's avatar
sachinc committed
46
47
#include <string.h>

48
49
namespace hpp
{
sachinc's avatar
sachinc committed
50
51
52
namespace fcl
{

53
/// @brief Base class for all basic geometric shapes
54
class ShapeBase : public CollisionGeometry
sachinc's avatar
sachinc committed
55
56
{
public:
57
  ShapeBase() {}
sachinc's avatar
sachinc committed
58

59
60
  virtual ~ShapeBase () {};

61
  /// @brief Get object type: a geometric shape
sachinc's avatar
sachinc committed
62
63
64
  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};

65
/// @brief Triangle stores the points instead of only indices of points
66
class TriangleP : public ShapeBase
67
68
{
public:
69
  TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
jpan's avatar
jpan committed
70
71
  {
  }
72

73
  /// @brief virtual function of compute AABB in local coordinate
74
75
76
77
78
79
  void computeLocalAABB();
  
  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }

  Vec3f a, b, c;
};
sachinc's avatar
sachinc committed
80

81
/// @brief Center at zero point, axis aligned box
sachinc's avatar
sachinc committed
82
83
84
class Box : public ShapeBase
{
public:
85
  Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2)
jpan's avatar
jpan committed
86
87
  {
  }
sachinc's avatar
sachinc committed
88

89
  Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2) 
jpan's avatar
jpan committed
90
91
92
  {
  }

93
94
  Box() {}

95
96
  /// @brief box side half-length
  Vec3f halfSide;
sachinc's avatar
sachinc committed
97

98
  /// @brief Compute AABB
99
  void computeLocalAABB();
sachinc's avatar
sachinc committed
100

101
  /// @brief Get node type: a box
sachinc's avatar
sachinc committed
102
  NODE_TYPE getNodeType() const { return GEOM_BOX; }
panjia1983's avatar
panjia1983 committed
103
104
105

  FCL_REAL computeVolume() const
  {
106
    return 8*halfSide.prod();
panjia1983's avatar
panjia1983 committed
107
108
109
110
111
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
112
113
    Vec3f s (halfSide.cwiseAbs2() * V);
    return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
panjia1983's avatar
panjia1983 committed
114
  }
sachinc's avatar
sachinc committed
115
116
};

117
/// @brief Center at zero point sphere
sachinc's avatar
sachinc committed
118
119
120
class Sphere : public ShapeBase
{
public:
jpan's avatar
jpan committed
121
122
123
  Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
  {
  }
jpan's avatar
   
jpan committed
124
  
125
  /// @brief Radius of the sphere 
126
  FCL_REAL radius;
sachinc's avatar
sachinc committed
127

128
  /// @brief Compute AABB 
129
  void computeLocalAABB();
sachinc's avatar
sachinc committed
130

131
  /// @brief Get node type: a sphere 
sachinc's avatar
sachinc committed
132
  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
panjia1983's avatar
panjia1983 committed
133
134
135
136

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL I = 0.4 * radius * radius * computeVolume();
137
    return I * Matrix3f::Identity();
panjia1983's avatar
panjia1983 committed
138
139
140
141
142
143
  }

  FCL_REAL computeVolume() const
  {
    return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius / 3;
  }
sachinc's avatar
sachinc committed
144
145
};

146
/// @brief Center at zero point capsule 
sachinc's avatar
sachinc committed
147
148
149
class Capsule : public ShapeBase
{
public:
jpan's avatar
jpan committed
150
151
  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
  {
Lucile Remigy's avatar
Lucile Remigy committed
152
153
    lz = 0;
    HalfLength = lz/2;
jpan's avatar
jpan committed
154
  }
jpan's avatar
   
jpan committed
155

Lucile Remigy's avatar
Lucile Remigy committed
156
 // Capsule::Capsule() : HalfLength(lz/2), lz(0){} 
Lucile Remigy's avatar
Lucile Remigy committed
157

158
  /// @brief Radius of capsule 
159
  FCL_REAL radius;
sachinc's avatar
sachinc committed
160

161
  /// @brief Length along z axis 
162
  FCL_REAL lz;
jpan's avatar
   
jpan committed
163

Lucile Remigy's avatar
Lucile Remigy committed
164
165
166
  /// @brief Half Length along z axis 
  FCL_REAL HalfLength;

167
  /// @brief Compute AABB 
168
  void computeLocalAABB();
sachinc's avatar
sachinc committed
169

170
  /// @brief Get node type: a capsule 
sachinc's avatar
sachinc committed
171
  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
panjia1983's avatar
panjia1983 committed
172
173
174
175
176
177
178
179
180
181
182
183
184
185

  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;

186
187
188
    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
panjia1983's avatar
panjia1983 committed
189
190
  }
  
sachinc's avatar
sachinc committed
191
192
};

193
/// @brief Center at zero cone 
sachinc's avatar
sachinc committed
194
195
196
class Cone : public ShapeBase
{
public:
jpan's avatar
jpan committed
197
198
  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
  {
Lucile Remigy's avatar
Lucile Remigy committed
199
200
    lz = 0;
    HalfLength = lz/2;
jpan's avatar
jpan committed
201
202
  }

Lucile Remigy's avatar
Lucile Remigy committed
203
  //Cone::Cone() : HalfLength(lz/2), lz(0){} 
Lucile Remigy's avatar
Lucile Remigy committed
204

205
  /// @brief Radius of the cone 
206
  FCL_REAL radius;
sachinc's avatar
sachinc committed
207

208
  /// @brief Length along z axis 
209
  FCL_REAL lz;
jpan's avatar
   
jpan committed
210

Lucile Remigy's avatar
Lucile Remigy committed
211
212
213
  /// @brief Half Length along z axis 
  FCL_REAL HalfLength;

214
  /// @brief Compute AABB 
215
  void computeLocalAABB();
sachinc's avatar
sachinc committed
216

217
  /// @brief Get node type: a cone 
sachinc's avatar
sachinc committed
218
  NODE_TYPE getNodeType() const { return GEOM_CONE; }
panjia1983's avatar
panjia1983 committed
219
220
221
222
223
224
225
226
227
228
229
230

  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;

231
232
233
    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
panjia1983's avatar
panjia1983 committed
234
235
236
237
238
239
  }

  Vec3f computeCOM() const
  {
    return Vec3f(0, 0, -0.25 * lz);
  }
sachinc's avatar
sachinc committed
240
241
};

242
/// @brief Center at zero cylinder 
sachinc's avatar
sachinc committed
243
244
245
class Cylinder : public ShapeBase
{
public:
jpan's avatar
jpan committed
246
247
  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
  {
Lucile Remigy's avatar
Lucile Remigy committed
248
249
    lz = 0;
    HalfLength = lz/2;
jpan's avatar
jpan committed
250
251
  }

Lucile Remigy's avatar
Lucile Remigy committed
252
 // Cylinder::Cylinder() : HalfLength(lz/2), lz(0){} 
jpan's avatar
   
jpan committed
253
  
254
  /// @brief Radius of the cylinder 
255
  FCL_REAL radius;
sachinc's avatar
sachinc committed
256

257
  /// @brief Length along z axis 
258
  FCL_REAL lz;
jpan's avatar
   
jpan committed
259

Lucile Remigy's avatar
Lucile Remigy committed
260
261
262
  /// @brief Half Length along z axis 
  FCL_REAL HalfLength;

263
  /// @brief Compute AABB 
264
  void computeLocalAABB();
sachinc's avatar
sachinc committed
265

266
  /// @brief Get node type: a cylinder 
sachinc's avatar
sachinc committed
267
  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
panjia1983's avatar
panjia1983 committed
268
269
270
271
272
273
274
275
276
277
278

  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;
279
280
281
    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
panjia1983's avatar
panjia1983 committed
282
  }
sachinc's avatar
sachinc committed
283
284
};

285
/// @brief Convex polytope 
sachinc's avatar
sachinc committed
286
287
288
class Convex : public ShapeBase
{
public:
289
  /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information 
Joseph Mirabel's avatar
Joseph Mirabel committed
290
291
292
293
294
295
  /// \param points_ list of 3D points
  /// \param num_points_ number of 3D points
  /// \param polygons_ \copydoc Convex::polygons
  /// \param num_polygons_ the number of polygons.
  /// \note num_polygons_ is not the allocated size of polygons_.
  Convex(Vec3f* points_,
Joseph Mirabel's avatar
Joseph Mirabel committed
296
         int num_points_,
Joseph Mirabel's avatar
Joseph Mirabel committed
297
298
         int* polygons_,
         int num_polygons_) : ShapeBase()
sachinc's avatar
sachinc committed
299
  {
Joseph Mirabel's avatar
Joseph Mirabel committed
300
    num_polygons = num_polygons_;
sachinc's avatar
sachinc committed
301
    points = points_;
Joseph Mirabel's avatar
Joseph Mirabel committed
302
    num_points = num_points_;
sachinc's avatar
sachinc committed
303
304
305
    polygons = polygons_;
    edges = NULL;

Joseph Mirabel's avatar
Joseph Mirabel committed
306
    Vec3f sum (0,0,0);
sachinc's avatar
sachinc committed
307
308
309
310
311
    for(int i = 0; i < num_points; ++i)
    {
      sum += points[i];
    }

312
    center = sum * (FCL_REAL)(1.0 / num_points);
sachinc's avatar
sachinc committed
313
314
315
316

    fillEdges();
  }

317
  /// @brief Copy constructor 
sachinc's avatar
sachinc committed
318
319
  Convex(const Convex& other) : ShapeBase(other)
  {
Joseph Mirabel's avatar
Joseph Mirabel committed
320
    num_polygons = other.num_polygons;
sachinc's avatar
sachinc committed
321
    points = other.points;
Joseph Mirabel's avatar
Joseph Mirabel committed
322
    num_points = other.num_points;
sachinc's avatar
sachinc committed
323
    polygons = other.polygons;
Joseph Mirabel's avatar
Joseph Mirabel committed
324
325
    num_edges = other.num_edges;
    edges = new Edge[num_edges];
sachinc's avatar
sachinc committed
326
327
328
329
330
331
332
333
    memcpy(edges, other.edges, sizeof(Edge) * num_edges);
  }

  ~Convex()
  {
    delete [] edges;
  }

334
  /// @brief Compute AABB 
335
  void computeLocalAABB();
sachinc's avatar
sachinc committed
336

337
  /// @brief Get node type: a conex polytope 
sachinc's avatar
sachinc committed
338
339
  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }

340
341
  /// @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
sachinc's avatar
sachinc committed
342
343
344
345
346
  int* polygons;

  Vec3f* points;
  int num_points;
  int num_edges;
Joseph Mirabel's avatar
Joseph Mirabel committed
347
  int num_polygons;
sachinc's avatar
sachinc committed
348
349
350
351
352
353
354
355

  struct Edge
  {
    int first, second;
  };

  Edge* edges;

356
  /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) 
sachinc's avatar
sachinc committed
357
358
  Vec3f center;

panjia1983's avatar
panjia1983 committed
359
360
361
362
  /// based on http://number-none.com/blow/inertia/bb_inertia.doc
  Matrix3f computeMomentofInertia() const
  {
    
363
    Matrix3f C = Matrix3f::Zero();
panjia1983's avatar
panjia1983 committed
364

365
366
367
368
    Matrix3f C_canonical;
    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;
panjia1983's avatar
panjia1983 committed
369
370
371

    int* points_in_poly = polygons;
    int* index = polygons + 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
372
    for(int i = 0; i < num_polygons; ++i)
panjia1983's avatar
panjia1983 committed
373
    {
374
      Vec3f plane_center(0,0,0);
panjia1983's avatar
panjia1983 committed
375
376
377
378
379
380
381
382
383
384
385
386
387
388

      // 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];
389
        Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document
390
        C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
panjia1983's avatar
panjia1983 committed
391
392
393
394
395
396
      }
      
      points_in_poly += (*points_in_poly + 1);
      index = points_in_poly + 1;
    }

397
    return C.trace() * Matrix3f::Identity() - C;
panjia1983's avatar
panjia1983 committed
398
399
400
401
  }

  Vec3f computeCOM() const
  {
402
    Vec3f com(0,0,0);
panjia1983's avatar
panjia1983 committed
403
404
405
    FCL_REAL vol = 0;
    int* points_in_poly = polygons;
    int* index = polygons + 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
406
    for(int i = 0; i < num_polygons; ++i)
panjia1983's avatar
panjia1983 committed
407
    {
408
      Vec3f plane_center(0,0,0);
panjia1983's avatar
panjia1983 committed
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439

      // 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;
Joseph Mirabel's avatar
Joseph Mirabel committed
440
    for(int i = 0; i < num_polygons; ++i)
panjia1983's avatar
panjia1983 committed
441
    {
442
      Vec3f plane_center(0,0,0);
panjia1983's avatar
panjia1983 committed
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469

      // 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;
  }

  

sachinc's avatar
sachinc committed
470
protected:
471
  /// @brief Get edge information 
sachinc's avatar
sachinc committed
472
473
474
  void fillEdges();
};

475
476
477
478
479
480
481
482
483
484
485
486
487
488

/// @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
489
  Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
490
491
492
493
  {
    unitNormalTest();
  }

494
495
496
497
498
499
500
501
502
503
504
505
506
507
  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);
  }

508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
  /// @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();
};

526
/// @brief Infinite plane 
sachinc's avatar
sachinc committed
527
528
529
class Plane : public ShapeBase
{
public:
530
  /// @brief Construct a plane with normal direction and offset 
jpan's avatar
jpan committed
531
532
533
534
  Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) 
  { 
    unitNormalTest(); 
  }
jpan's avatar
   
jpan committed
535
  
536
  /// @brief Construct a plane with normal direction and offset 
537
  Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
sachinc's avatar
sachinc committed
538
539
540
541
  {
    unitNormalTest();
  }

542
543
544
545
546
547
548
549
550
551
552
553
554
  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);
  }

555
  /// @brief Compute AABB 
556
  void computeLocalAABB();
sachinc's avatar
sachinc committed
557

558
  /// @brief Get node type: a plane 
sachinc's avatar
sachinc committed
559
560
  NODE_TYPE getNodeType() const { return GEOM_PLANE; }

561
  /// @brief Plane normal 
sachinc's avatar
sachinc committed
562
  Vec3f n;
jpan's avatar
   
jpan committed
563

564
  /// @brief Plane offset 
565
  FCL_REAL d;
sachinc's avatar
sachinc committed
566
567

protected:
jpan's avatar
   
jpan committed
568
  
569
  /// @brief Turn non-unit normal into unit 
sachinc's avatar
sachinc committed
570
571
572
573
574
575
  void unitNormalTest();
};


}

576
577
} // namespace hpp

sachinc's avatar
sachinc committed
578
#endif