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/SoftMeshShape.hpp"
34
35 #include "dart/common/Console.hpp"
36
37 #include "dart/dynamics/PointMass.hpp"
38 #include "dart/dynamics/SoftBodyNode.hpp"
39
40 namespace dart {
41 namespace dynamics {
42
SoftMeshShape(SoftBodyNode * _softBodyNode)43 SoftMeshShape::SoftMeshShape(SoftBodyNode* _softBodyNode)
44 : Shape(SOFT_MESH), mSoftBodyNode(_softBodyNode), mAssimpMesh(nullptr)
45 {
46 assert(_softBodyNode != nullptr);
47 // Build mesh here using soft body node
48 // TODO(JS): Not implemented.
49 _buildMesh();
50 mVariance = DYNAMIC_VERTICES;
51 }
52
~SoftMeshShape()53 SoftMeshShape::~SoftMeshShape()
54 {
55 // Do nothing
56 }
57
58 //==============================================================================
getType() const59 const std::string& SoftMeshShape::getType() const
60 {
61 return getStaticType();
62 }
63
64 //==============================================================================
getStaticType()65 const std::string& SoftMeshShape::getStaticType()
66 {
67 static const std::string type("SoftMeshShape");
68 return type;
69 }
70
getAssimpMesh() const71 const aiMesh* SoftMeshShape::getAssimpMesh() const
72 {
73 return mAssimpMesh.get();
74 }
75
getSoftBodyNode() const76 const SoftBodyNode* SoftMeshShape::getSoftBodyNode() const
77 {
78 return mSoftBodyNode;
79 }
80
computeInertia(double) const81 Eigen::Matrix3d SoftMeshShape::computeInertia(double /*mass*/) const
82 {
83 dtwarn << "[SoftMeshShape::computeInertia] Not implemented yet.\n";
84 // TODO(JS): Not implemented.
85
86 return Eigen::Matrix3d::Zero();
87 }
88
89 //==============================================================================
updateBoundingBox() const90 void SoftMeshShape::updateBoundingBox() const
91 {
92 // TODO(JS): Not implemented.
93 mIsBoundingBoxDirty = false;
94 }
95
96 //==============================================================================
updateVolume() const97 void SoftMeshShape::updateVolume() const
98 {
99 // TODO(JS): Not implemented.
100 mIsVolumeDirty = false;
101 }
102
_buildMesh()103 void SoftMeshShape::_buildMesh()
104 {
105 // Get number of vertices and faces from soft body node
106 int nVertices = mSoftBodyNode->getNumPointMasses();
107 int nFaces = mSoftBodyNode->getNumFaces();
108
109 // Create new aiMesh
110 mAssimpMesh = std::make_unique<aiMesh>();
111
112 // Set vertices and normals
113 mAssimpMesh->mNumVertices = nVertices;
114 mAssimpMesh->mVertices = new aiVector3D[nVertices];
115 mAssimpMesh->mNormals = new aiVector3D[nVertices];
116 aiVector3D itAIVector3d;
117 for (int i = 0; i < nVertices; ++i)
118 {
119 PointMass* itPointMass = mSoftBodyNode->getPointMass(i);
120 const Eigen::Vector3d& vertex = itPointMass->getRestingPosition();
121 itAIVector3d.Set(vertex[0], vertex[1], vertex[2]);
122 mAssimpMesh->mVertices[i] = itAIVector3d;
123 mAssimpMesh->mNormals[i] = itAIVector3d;
124 }
125
126 // Set faces
127 mAssimpMesh->mNumFaces = nFaces;
128 mAssimpMesh->mFaces = new aiFace[nFaces];
129 for (int i = 0; i < nFaces; ++i)
130 {
131 Eigen::Vector3i itFace = mSoftBodyNode->getFace(i);
132 aiFace* itAIFace = &mAssimpMesh->mFaces[i];
133 itAIFace->mNumIndices = 3;
134 itAIFace->mIndices = new unsigned int[3];
135 itAIFace->mIndices[0] = itFace[0];
136 itAIFace->mIndices[1] = itFace[1];
137 itAIFace->mIndices[2] = itFace[2];
138 }
139 }
140
update()141 void SoftMeshShape::update()
142 {
143 std::size_t nVertices = mSoftBodyNode->getNumPointMasses();
144
145 aiVector3D itAIVector3d;
146 for (std::size_t i = 0; i < nVertices; ++i)
147 {
148 PointMass* itPointMass = mSoftBodyNode->getPointMass(i);
149 const Eigen::Vector3d& vertex = itPointMass->getLocalPosition();
150 itAIVector3d.Set(vertex[0], vertex[1], vertex[2]);
151 mAssimpMesh->mVertices[i] = itAIVector3d;
152 }
153 }
154
155 } // namespace dynamics
156 } // namespace dart
157