1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
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 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include "dart/dynamics/PlaneShape.hpp"
34
35 namespace dart {
36 namespace dynamics {
37
38 //==============================================================================
PlaneShape(const Eigen::Vector3d & _normal,double _offset)39 PlaneShape::PlaneShape(const Eigen::Vector3d& _normal, double _offset)
40 : Shape(PLANE), mNormal(_normal.normalized()), mOffset(_offset)
41 {
42 }
43
44 //==============================================================================
PlaneShape(const Eigen::Vector3d & _normal,const Eigen::Vector3d & _point)45 PlaneShape::PlaneShape(
46 const Eigen::Vector3d& _normal, const Eigen::Vector3d& _point)
47 : Shape(), mNormal(_normal.normalized()), mOffset(mNormal.dot(_point))
48 {
49 }
50
51 //==============================================================================
getType() const52 const std::string& PlaneShape::getType() const
53 {
54 return getStaticType();
55 }
56
57 //==============================================================================
getStaticType()58 const std::string& PlaneShape::getStaticType()
59 {
60 static const std::string type("PlaneShape");
61 return type;
62 }
63
64 //==============================================================================
computeInertia(double) const65 Eigen::Matrix3d PlaneShape::computeInertia(double /*mass*/) const
66 {
67 return Eigen::Matrix3d::Zero();
68 }
69
70 //==============================================================================
setNormal(const Eigen::Vector3d & _normal)71 void PlaneShape::setNormal(const Eigen::Vector3d& _normal)
72 {
73 mNormal = _normal.normalized();
74
75 incrementVersion();
76 }
77
78 //==============================================================================
getNormal() const79 const Eigen::Vector3d& PlaneShape::getNormal() const
80 {
81 return mNormal;
82 }
83
84 //==============================================================================
setOffset(double _offset)85 void PlaneShape::setOffset(double _offset)
86 {
87 mOffset = _offset;
88
89 incrementVersion();
90 }
91
92 //==============================================================================
getOffset() const93 double PlaneShape::getOffset() const
94 {
95 return mOffset;
96 }
97
98 //==============================================================================
setNormalAndOffset(const Eigen::Vector3d & _normal,double _offset)99 void PlaneShape::setNormalAndOffset(
100 const Eigen::Vector3d& _normal, double _offset)
101 {
102 setNormal(_normal);
103 setOffset(_offset);
104 }
105
106 //==============================================================================
setNormalAndPoint(const Eigen::Vector3d & _normal,const Eigen::Vector3d & _point)107 void PlaneShape::setNormalAndPoint(
108 const Eigen::Vector3d& _normal, const Eigen::Vector3d& _point)
109 {
110 setNormal(_normal);
111 setOffset(mNormal.dot(_point));
112 }
113
114 //==============================================================================
computeDistance(const Eigen::Vector3d & _point) const115 double PlaneShape::computeDistance(const Eigen::Vector3d& _point) const
116 {
117 return std::abs(computeSignedDistance(_point));
118 }
119
120 //==============================================================================
computeSignedDistance(const Eigen::Vector3d & _point) const121 double PlaneShape::computeSignedDistance(const Eigen::Vector3d& _point) const
122 {
123 return mNormal.dot(_point) - mOffset;
124 }
125
126 //==============================================================================
updateBoundingBox() const127 void PlaneShape::updateBoundingBox() const
128 {
129 mBoundingBox.setMin(
130 Eigen::Vector3d::Constant(-std::numeric_limits<double>::infinity()));
131 mBoundingBox.setMax(
132 Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity()));
133
134 mIsBoundingBoxDirty = false;
135 }
136
137 //==============================================================================
updateVolume() const138 void PlaneShape::updateVolume() const
139 {
140 mVolume = 0.0;
141 mIsVolumeDirty = false;
142 }
143
144 } // namespace dynamics
145 } // namespace dart
146