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
36 /** @author Jia Pan */
37
38 #ifndef FCL_SHAPE_BOX_INL_H
39 #define FCL_SHAPE_BOX_INL_H
40
41 #include "fcl/geometry/shape/box.h"
42
43 namespace fcl
44 {
45
46 //==============================================================================
47 extern template
48 class FCL_EXPORT Box<double>;
49
50 //==============================================================================
51 template <typename S>
Box(S x,S y,S z)52 Box<S>::Box(S x, S y, S z)
53 : ShapeBase<S>(), side(x, y, z)
54 {
55 // Do nothing
56 }
57
58 //==============================================================================
59 template <typename S>
Box(const Vector3<S> & side_)60 Box<S>::Box(const Vector3<S>& side_)
61 : ShapeBase<S>(), side(side_)
62 {
63 // Do nothing
64 }
65
66 //==============================================================================
67 template <typename S>
Box()68 Box<S>::Box()
69 : ShapeBase<S>(), side(Vector3<S>::Zero())
70 {
71 // Do nothing
72 }
73
74 //==============================================================================
75 template <typename S>
computeLocalAABB()76 void Box<S>::computeLocalAABB()
77 {
78 const Vector3<S> v_delta = 0.5 * side;
79 this->aabb_local.max_ = v_delta;
80 this->aabb_local.min_ = -v_delta;
81
82 this->aabb_center = this->aabb_local.center();
83 this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm();
84 }
85
86 //==============================================================================
87 template <typename S>
getNodeType()88 NODE_TYPE Box<S>::getNodeType() const
89 {
90 return GEOM_BOX;
91 }
92
93 //==============================================================================
94 template <typename S>
computeVolume()95 S Box<S>::computeVolume() const
96 {
97 return side.prod();
98 }
99
100 //==============================================================================
101 template <typename S>
computeMomentofInertia()102 Matrix3<S> Box<S>::computeMomentofInertia() const
103 {
104 S V = computeVolume();
105
106 S a2 = side[0] * side[0] * V;
107 S b2 = side[1] * side[1] * V;
108 S c2 = side[2] * side[2] * V;
109
110 Vector3<S> I((b2 + c2) / 12, (a2 + c2) / 12, (a2 + b2) / 12);
111
112 return I.asDiagonal();
113 }
114
115 //==============================================================================
116 template <typename S>
getBoundVertices(const Transform3<S> & tf)117 std::vector<Vector3<S>> Box<S>::getBoundVertices(
118 const Transform3<S>& tf) const
119 {
120 std::vector<Vector3<S>> result(8);
121 auto a = side[0] / 2;
122 auto b = side[1] / 2;
123 auto c = side[2] / 2;
124 result[0] = tf * Vector3<S>(a, b, c);
125 result[1] = tf * Vector3<S>(a, b, -c);
126 result[2] = tf * Vector3<S>(a, -b, c);
127 result[3] = tf * Vector3<S>(a, -b, -c);
128 result[4] = tf * Vector3<S>(-a, b, c);
129 result[5] = tf * Vector3<S>(-a, b, -c);
130 result[6] = tf * Vector3<S>(-a, -b, c);
131 result[7] = tf * Vector3<S>(-a, -b, -c);
132
133 return result;
134 }
135
136 } // namespace fcl
137
138 #endif
139