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/SoftBodyNode.hpp"
34
35 #include <map>
36 #include <string>
37 #include <vector>
38
39 #include "dart/common/Console.hpp"
40 #include "dart/dynamics/Joint.hpp"
41 #include "dart/dynamics/Shape.hpp"
42 #include "dart/dynamics/Skeleton.hpp"
43 #include "dart/math/Helpers.hpp"
44
45 #include "dart/dynamics/PointMass.hpp"
46 #include "dart/dynamics/SoftMeshShape.hpp"
47
48 namespace dart {
49 namespace dynamics {
50
51 namespace detail {
52
53 //==============================================================================
SoftBodyNodeUniqueProperties(double _Kv,double _Ke,double _DampCoeff,const std::vector<PointMass::Properties> & _points,const std::vector<Eigen::Vector3i> & _faces)54 SoftBodyNodeUniqueProperties::SoftBodyNodeUniqueProperties(
55 double _Kv,
56 double _Ke,
57 double _DampCoeff,
58 const std::vector<PointMass::Properties>& _points,
59 const std::vector<Eigen::Vector3i>& _faces)
60 : mKv(_Kv),
61 mKe(_Ke),
62 mDampCoeff(_DampCoeff),
63 mPointProps(_points),
64 mFaces(_faces)
65 {
66 // Do nothing
67 }
68
69 //==============================================================================
addPointMass(const PointMass::Properties & _properties)70 void SoftBodyNodeUniqueProperties::addPointMass(
71 const PointMass::Properties& _properties)
72 {
73 mPointProps.push_back(_properties);
74 }
75
76 //==============================================================================
connectPointMasses(std::size_t i1,std::size_t i2)77 bool SoftBodyNodeUniqueProperties::connectPointMasses(
78 std::size_t i1, std::size_t i2)
79 {
80 if (i1 >= mPointProps.size() || i2 >= mPointProps.size())
81 {
82 if (mPointProps.size() == 0)
83 dtwarn << "[SoftBodyNode::Properties::addConnection] Attempting to "
84 << "add a connection between indices " << i1 << " and " << i2
85 << ", but there are currently no entries in mPointProps!\n";
86 else
87 dtwarn << "[SoftBodyNode::Properties::addConnection] Attempting to "
88 << "add a connection between indices " << i1 << " and " << i2
89 << ", but the entries in mPointProps only go up to "
90 << mPointProps.size() - 1 << "!\n";
91 return false;
92 }
93
94 mPointProps[i1].mConnectedPointMassIndices.push_back(i2);
95 mPointProps[i2].mConnectedPointMassIndices.push_back(i1);
96
97 return true;
98 }
99
100 //==============================================================================
addFace(const Eigen::Vector3i & _newFace)101 void SoftBodyNodeUniqueProperties::addFace(const Eigen::Vector3i& _newFace)
102 {
103 assert(_newFace[0] != _newFace[1]);
104 assert(_newFace[1] != _newFace[2]);
105 assert(_newFace[2] != _newFace[0]);
106 assert(
107 0 <= _newFace[0]
108 && static_cast<std::size_t>(_newFace[0]) < mPointProps.size());
109 assert(
110 0 <= _newFace[1]
111 && static_cast<std::size_t>(_newFace[1]) < mPointProps.size());
112 assert(
113 0 <= _newFace[2]
114 && static_cast<std::size_t>(_newFace[2]) < mPointProps.size());
115 mFaces.push_back(_newFace);
116 }
117
118 //==============================================================================
SoftBodyNodeProperties(const BodyNode::Properties & _bodyProperties,const SoftBodyNodeUniqueProperties & _softProperties)119 SoftBodyNodeProperties::SoftBodyNodeProperties(
120 const BodyNode::Properties& _bodyProperties,
121 const SoftBodyNodeUniqueProperties& _softProperties)
122 : BodyNode::Properties(_bodyProperties),
123 SoftBodyNodeUniqueProperties(_softProperties)
124 {
125 // Do nothing
126 }
127
128 } // namespace detail
129
130 //==============================================================================
~SoftBodyNode()131 SoftBodyNode::~SoftBodyNode()
132 {
133 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
134 delete mPointMasses[i];
135
136 delete mNotifier;
137 }
138
139 //==============================================================================
asSoftBodyNode()140 SoftBodyNode* SoftBodyNode::asSoftBodyNode()
141 {
142 return this;
143 }
144
145 //==============================================================================
asSoftBodyNode() const146 const SoftBodyNode* SoftBodyNode::asSoftBodyNode() const
147 {
148 return this;
149 }
150
151 //==============================================================================
setProperties(const Properties & _properties)152 void SoftBodyNode::setProperties(const Properties& _properties)
153 {
154 BodyNode::setProperties(
155 static_cast<const BodyNode::Properties&>(_properties));
156
157 setProperties(static_cast<const UniqueProperties&>(_properties));
158 }
159
160 //==============================================================================
setProperties(const UniqueProperties & _properties)161 void SoftBodyNode::setProperties(const UniqueProperties& _properties)
162 {
163 setAspectProperties(_properties);
164 }
165
166 //==============================================================================
setAspectState(const AspectState & state)167 void SoftBodyNode::setAspectState(const AspectState& state)
168 {
169 if (mAspectState.mPointStates == state.mPointStates)
170 return;
171
172 mAspectState = state;
173 mNotifier->dirtyTransform();
174 }
175
176 //==============================================================================
setAspectProperties(const AspectProperties & properties)177 void SoftBodyNode::setAspectProperties(const AspectProperties& properties)
178 {
179 setVertexSpringStiffness(properties.mKv);
180 setEdgeSpringStiffness(properties.mKe);
181 setDampingCoefficient(properties.mDampCoeff);
182
183 if (properties.mPointProps != mAspectProperties.mPointProps
184 || properties.mFaces != mAspectProperties.mFaces)
185 {
186 mAspectProperties.mPointProps = properties.mPointProps;
187 mAspectProperties.mFaces = properties.mFaces;
188 configurePointMasses(mSoftShapeNode.lock());
189 }
190 }
191
192 //==============================================================================
getSoftBodyNodeProperties() const193 SoftBodyNode::Properties SoftBodyNode::getSoftBodyNodeProperties() const
194 {
195 return Properties(getBodyNodeProperties(), mAspectProperties);
196 }
197
198 //==============================================================================
copy(const SoftBodyNode & _otherSoftBodyNode)199 void SoftBodyNode::copy(const SoftBodyNode& _otherSoftBodyNode)
200 {
201 if (this == &_otherSoftBodyNode)
202 return;
203
204 setProperties(_otherSoftBodyNode.getSoftBodyNodeProperties());
205 }
206
207 //==============================================================================
copy(const SoftBodyNode * _otherSoftBodyNode)208 void SoftBodyNode::copy(const SoftBodyNode* _otherSoftBodyNode)
209 {
210 if (nullptr == _otherSoftBodyNode)
211 return;
212
213 copy(*_otherSoftBodyNode);
214 }
215
216 //==============================================================================
operator =(const SoftBodyNode & _otherSoftBodyNode)217 SoftBodyNode& SoftBodyNode::operator=(const SoftBodyNode& _otherSoftBodyNode)
218 {
219 copy(_otherSoftBodyNode);
220 return *this;
221 }
222
223 //==============================================================================
getNumPointMasses() const224 std::size_t SoftBodyNode::getNumPointMasses() const
225 {
226 return mPointMasses.size();
227 }
228
229 //==============================================================================
getPointMass(std::size_t _idx)230 PointMass* SoftBodyNode::getPointMass(std::size_t _idx)
231 {
232 assert(_idx < mPointMasses.size());
233 if (_idx < mPointMasses.size())
234 return mPointMasses[_idx];
235
236 return nullptr;
237 }
238
239 //==============================================================================
getPointMass(std::size_t _idx) const240 const PointMass* SoftBodyNode::getPointMass(std::size_t _idx) const
241 {
242 return const_cast<SoftBodyNode*>(this)->getPointMass(_idx);
243 }
244
245 //==============================================================================
getPointMasses() const246 const std::vector<PointMass*>& SoftBodyNode::getPointMasses() const
247 {
248 return mPointMasses;
249 }
250
251 //==============================================================================
SoftBodyNode(BodyNode * _parentBodyNode,Joint * _parentJoint,const Properties & _properties)252 SoftBodyNode::SoftBodyNode(
253 BodyNode* _parentBodyNode,
254 Joint* _parentJoint,
255 const Properties& _properties)
256 : Entity(Frame::World(), false),
257 Frame(Frame::World()),
258 Base(std::make_tuple(_parentBodyNode, _parentJoint, _properties)),
259 mSoftShapeNode(nullptr)
260 {
261 createSoftBodyAspect();
262 mNotifier = new PointMassNotifier(this, getName() + "_PointMassNotifier");
263 ShapeNode* softNode
264 = createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
265 std::make_shared<SoftMeshShape>(this), getName() + "_SoftMeshShape");
266 mSoftShapeNode = softNode;
267
268 // Dev's Note: We do this workaround (instead of just using setProperties(~))
269 // because mSoftShapeNode cannot be used until init(SkeletonPtr) has been
270 // called on this BodyNode, but that happens after construction is finished.
271 mAspectProperties = _properties;
272 configurePointMasses(softNode);
273 mNotifier->dirtyTransform();
274 }
275
276 //==============================================================================
clone(BodyNode * _parentBodyNode,Joint * _parentJoint,bool cloneNodes) const277 BodyNode* SoftBodyNode::clone(
278 BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const
279 {
280 SoftBodyNode* clonedBn = new SoftBodyNode(
281 _parentBodyNode, _parentJoint, getSoftBodyNodeProperties());
282
283 clonedBn->matchAspects(this);
284
285 if (cloneNodes)
286 clonedBn->matchNodes(this);
287
288 return clonedBn;
289 }
290
291 //==============================================================================
configurePointMasses(ShapeNode * softNode)292 void SoftBodyNode::configurePointMasses(ShapeNode* softNode)
293 {
294 const UniqueProperties& softProperties = mAspectProperties;
295
296 std::size_t newCount = softProperties.mPointProps.size();
297 std::size_t oldCount = mPointMasses.size();
298
299 if (newCount == oldCount)
300 return;
301
302 // Adjust the number of PointMass objects since that has changed
303 if (newCount < oldCount)
304 {
305 for (std::size_t i = newCount; i < oldCount; ++i)
306 delete mPointMasses[i];
307 mPointMasses.resize(newCount);
308 }
309 else if (oldCount < newCount)
310 {
311 mPointMasses.resize(newCount);
312 for (std::size_t i = oldCount; i < newCount; ++i)
313 {
314 mPointMasses[i] = new PointMass(this);
315 mPointMasses[i]->mIndex = i;
316 mPointMasses[i]->init();
317 }
318 }
319
320 // Resize the number of States in the Aspect
321 mAspectState.mPointStates.resize(
322 softProperties.mPointProps.size(), PointMass::State());
323
324 // Access the SoftMeshShape and reallocate its meshes
325 if (softNode)
326 {
327 std::shared_ptr<SoftMeshShape> softShape
328 = std::dynamic_pointer_cast<SoftMeshShape>(softNode->getShape());
329
330 if (softShape)
331 softShape->_buildMesh();
332 }
333 else
334 {
335 dtwarn << "[SoftBodyNode::configurePointMasses] The ShapeNode containing "
336 << "the SoftMeshShape for the SoftBodyNode named [" << getName()
337 << "] (" << this << ") has been removed. The soft body features for "
338 << "this SoftBodyNode cannot be used unless you recreate the "
339 << "SoftMeshShape.\n";
340
341 std::cout << "ShapeNodes: " << std::endl;
342 for (std::size_t i = 0; i < getNumShapeNodes(); ++i)
343 {
344 std::cout << "- " << i << ") " << getShapeNode(i)->getName() << std::endl;
345 }
346 }
347
348 incrementVersion();
349 mNotifier->dirtyTransform();
350 }
351
352 //==============================================================================
init(const SkeletonPtr & _skeleton)353 void SoftBodyNode::init(const SkeletonPtr& _skeleton)
354 {
355 BodyNode::init(_skeleton);
356
357 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
358 mPointMasses[i]->init();
359
360 // //----------------------------------------------------------------------------
361 // // Visualization shape
362 // //----------------------------------------------------------------------------
363 // assert(mSoftVisualShape == nullptr);
364 // mSoftVisualShape = new SoftMeshShape(this);
365 // BodyNode::addVisualizationShape(mSoftVisualShape);
366
367 // //----------------------------------------------------------------------------
368 // // Collision shape
369 // //----------------------------------------------------------------------------
370 // assert(mSoftCollShape == nullptr);
371 // mSoftCollShape = new SoftMeshShape(this);
372 // BodyNode::addCollisionShape(mSoftCollShape);
373 }
374
375 //==============================================================================
376 // void SoftBodyNode::aggregateGenCoords(std::vector<GenCoord*>* _genCoords)
377 //{
378 // BodyNode::aggregateGenCoords(_genCoords);
379 // aggregatePointMassGenCoords(_genCoords);
380 //}
381
382 //==============================================================================
383 // void SoftBodyNode::aggregatePointMassGenCoords(
384 // std::vector<GenCoord*>* _genCoords)
385 //{
386 // for (std::size_t i = 0; i < getNumPointMasses(); ++i)
387 // {
388 // PointMass* pointMass = getPointMass(i);
389 // for (int j = 0; j < pointMass->getNumDofs(); ++j)
390 // {
391 // GenCoord* genCoord = pointMass->getGenCoord(j);
392 // genCoord->setSkeletonIndex(_genCoords->size());
393 // _genCoords->push_back(genCoord);
394 // }
395 // }
396 //}
397
398 //==============================================================================
getNotifier()399 PointMassNotifier* SoftBodyNode::getNotifier()
400 {
401 return mNotifier;
402 }
403
404 //==============================================================================
getNotifier() const405 const PointMassNotifier* SoftBodyNode::getNotifier() const
406 {
407 return mNotifier;
408 }
409
410 //==============================================================================
getMass() const411 double SoftBodyNode::getMass() const
412 {
413 double totalMass = BodyNode::getMass();
414
415 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
416 totalMass += mPointMasses.at(i)->getMass();
417
418 return totalMass;
419 }
420
421 //==============================================================================
setVertexSpringStiffness(double _kv)422 void SoftBodyNode::setVertexSpringStiffness(double _kv)
423 {
424 assert(0.0 <= _kv);
425
426 if (_kv == mAspectProperties.mKv)
427 return;
428
429 mAspectProperties.mKv = _kv;
430 incrementVersion();
431 }
432
433 //==============================================================================
getVertexSpringStiffness() const434 double SoftBodyNode::getVertexSpringStiffness() const
435 {
436 return mAspectProperties.mKv;
437 }
438
439 //==============================================================================
setEdgeSpringStiffness(double _ke)440 void SoftBodyNode::setEdgeSpringStiffness(double _ke)
441 {
442 assert(0.0 <= _ke);
443
444 if (_ke == mAspectProperties.mKe)
445 return;
446
447 mAspectProperties.mKe = _ke;
448 incrementVersion();
449 }
450
451 //==============================================================================
getEdgeSpringStiffness() const452 double SoftBodyNode::getEdgeSpringStiffness() const
453 {
454 return mAspectProperties.mKe;
455 }
456
457 //==============================================================================
setDampingCoefficient(double _damp)458 void SoftBodyNode::setDampingCoefficient(double _damp)
459 {
460 assert(_damp >= 0.0);
461
462 if (_damp == mAspectProperties.mDampCoeff)
463 return;
464
465 mAspectProperties.mDampCoeff = _damp;
466 incrementVersion();
467 }
468
469 //==============================================================================
getDampingCoefficient() const470 double SoftBodyNode::getDampingCoefficient() const
471 {
472 return mAspectProperties.mDampCoeff;
473 }
474
475 //==============================================================================
removeAllPointMasses()476 void SoftBodyNode::removeAllPointMasses()
477 {
478 mPointMasses.clear();
479 mAspectProperties.mPointProps.clear();
480 mAspectProperties.mFaces.clear();
481 configurePointMasses(mSoftShapeNode.lock());
482 }
483
484 //==============================================================================
addPointMass(const PointMass::Properties & _properties)485 PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties)
486 {
487 mPointMasses.push_back(new PointMass(this));
488 mPointMasses.back()->mIndex = mPointMasses.size() - 1;
489 mAspectProperties.mPointProps.push_back(_properties);
490 configurePointMasses(mSoftShapeNode.lock());
491
492 return mPointMasses.back();
493 }
494
495 //==============================================================================
connectPointMasses(std::size_t _idx1,std::size_t _idx2)496 void SoftBodyNode::connectPointMasses(std::size_t _idx1, std::size_t _idx2)
497 {
498 assert(_idx1 != _idx2);
499 assert(_idx1 < mPointMasses.size());
500 assert(_idx2 < mPointMasses.size());
501 mPointMasses[_idx1]->addConnectedPointMass(mPointMasses[_idx2]);
502 mPointMasses[_idx2]->addConnectedPointMass(mPointMasses[_idx1]);
503 // Version incremented in addConnectedPointMass
504 }
505
506 //==============================================================================
addFace(const Eigen::Vector3i & _face)507 void SoftBodyNode::addFace(const Eigen::Vector3i& _face)
508 {
509 mAspectProperties.addFace(_face);
510 configurePointMasses(mSoftShapeNode.lock());
511 }
512
513 //==============================================================================
getFace(std::size_t _idx) const514 const Eigen::Vector3i& SoftBodyNode::getFace(std::size_t _idx) const
515 {
516 assert(_idx < mAspectProperties.mFaces.size());
517 return mAspectProperties.mFaces[_idx];
518 }
519
520 //==============================================================================
getNumFaces() const521 std::size_t SoftBodyNode::getNumFaces() const
522 {
523 return mAspectProperties.mFaces.size();
524 }
525
526 //==============================================================================
clearConstraintImpulse()527 void SoftBodyNode::clearConstraintImpulse()
528 {
529 BodyNode::clearConstraintImpulse();
530
531 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
532 mPointMasses.at(i)->clearConstraintImpulse();
533 }
534
535 //==============================================================================
checkArticulatedInertiaUpdate() const536 void SoftBodyNode::checkArticulatedInertiaUpdate() const
537 {
538 ConstSkeletonPtr skel = getSkeleton();
539 if (skel && skel->mTreeCache[mTreeIndex].mDirty.mArticulatedInertia)
540 skel->updateArticulatedInertia(mTreeIndex);
541 }
542
543 //==============================================================================
updateTransform()544 void SoftBodyNode::updateTransform()
545 {
546 BodyNode::updateTransform();
547
548 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
549 mPointMasses.at(i)->updateTransform();
550
551 mNotifier->clearTransformNotice();
552 }
553
554 //==============================================================================
updateVelocity()555 void SoftBodyNode::updateVelocity()
556 {
557 BodyNode::updateVelocity();
558
559 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
560 mPointMasses.at(i)->updateVelocity();
561
562 mNotifier->clearVelocityNotice();
563 }
564
565 //==============================================================================
updatePartialAcceleration() const566 void SoftBodyNode::updatePartialAcceleration() const
567 {
568 BodyNode::updatePartialAcceleration();
569
570 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
571 mPointMasses.at(i)->updatePartialAcceleration();
572
573 mNotifier->clearPartialAccelerationNotice();
574 }
575
576 //==============================================================================
updateAccelerationID()577 void SoftBodyNode::updateAccelerationID()
578 {
579 BodyNode::updateAccelerationID();
580
581 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
582 mPointMasses.at(i)->updateAccelerationID();
583
584 mNotifier->clearAccelerationNotice();
585 }
586
587 //==============================================================================
updateTransmittedForceID(const Eigen::Vector3d & _gravity,bool _withExternalForces)588 void SoftBodyNode::updateTransmittedForceID(
589 const Eigen::Vector3d& _gravity, bool _withExternalForces)
590 {
591 const Eigen::Matrix6d& mI
592 = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
593 for (auto& pointMass : mPointMasses)
594 pointMass->updateTransmittedForceID(_gravity, _withExternalForces);
595
596 // Gravity force
597 if (BodyNode::mAspectProperties.mGravityMode == true)
598 mFgravity.noalias()
599 = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
600 else
601 mFgravity.setZero();
602
603 // Inertial force
604 mF.noalias() = mI * getSpatialAcceleration();
605
606 // External force
607 if (_withExternalForces)
608 mF -= BodyNode::mAspectState.mFext;
609
610 // Verification
611 assert(!math::isNan(mF));
612
613 // Gravity force
614 mF -= mFgravity;
615
616 // Coriolis force
617 const Eigen::Vector6d& V = getSpatialVelocity();
618 mF -= math::dad(V, mI * V);
619
620 //
621 for (const auto& childBodyNode : mChildBodyNodes)
622 {
623 Joint* childJoint = childBodyNode->getParentJoint();
624 assert(childJoint != nullptr);
625
626 mF += math::dAdInvT(
627 childJoint->getRelativeTransform(), childBodyNode->getBodyForce());
628 }
629 for (auto& pointMass : mPointMasses)
630 {
631 mF.head<3>() += pointMass->getLocalPosition().cross(pointMass->mF);
632 mF.tail<3>() += pointMass->mF;
633 }
634
635 // Verification
636 assert(!math::isNan(mF));
637 }
638
639 //==============================================================================
updateJointForceID(double _timeStep,bool _withDampingForces,bool _withSpringForces)640 void SoftBodyNode::updateJointForceID(
641 double _timeStep, bool _withDampingForces, bool _withSpringForces)
642 {
643 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
644 mPointMasses.at(i)->updateJointForceID(
645 _timeStep, _withDampingForces, _withSpringForces);
646
647 BodyNode::updateJointForceID(
648 _timeStep, _withDampingForces, _withSpringForces);
649 }
650
651 //==============================================================================
updateJointForceFD(double _timeStep,bool _withDampingForces,bool _withSpringForces)652 void SoftBodyNode::updateJointForceFD(
653 double _timeStep, bool _withDampingForces, bool _withSpringForces)
654 {
655 BodyNode::updateJointForceFD(
656 _timeStep, _withDampingForces, _withSpringForces);
657 }
658
659 //==============================================================================
updateJointImpulseFD()660 void SoftBodyNode::updateJointImpulseFD()
661 {
662 BodyNode::updateJointImpulseFD();
663 }
664
665 //==============================================================================
updateArtInertia(double _timeStep) const666 void SoftBodyNode::updateArtInertia(double _timeStep) const
667 {
668 const Eigen::Matrix6d& mI
669 = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
670 for (auto& pointMass : mPointMasses)
671 pointMass->updateArtInertiaFD(_timeStep);
672
673 assert(mParentJoint != nullptr);
674
675 // Set spatial inertia to the articulated body inertia
676 mArtInertia = mI;
677 mArtInertiaImplicit = mI;
678
679 // and add child articulated body inertia
680 for (const auto& child : mChildBodyNodes)
681 {
682 Joint* childJoint = child->getParentJoint();
683
684 childJoint->addChildArtInertiaTo(mArtInertia, child->mArtInertia);
685 childJoint->addChildArtInertiaImplicitTo(
686 mArtInertiaImplicit, child->mArtInertiaImplicit);
687 }
688
689 //
690 for (const auto& pointMass : mPointMasses)
691 {
692 _addPiToArtInertia(pointMass->getLocalPosition(), pointMass->mPi);
693 _addPiToArtInertiaImplicit(
694 pointMass->getLocalPosition(), pointMass->mImplicitPi);
695 }
696
697 // Verification
698 assert(!math::isNan(mArtInertia));
699 assert(!math::isNan(mArtInertiaImplicit));
700
701 // Update parent joint's inverse of projected articulated body inertia
702 mParentJoint->updateInvProjArtInertia(mArtInertia);
703 mParentJoint->updateInvProjArtInertiaImplicit(mArtInertiaImplicit, _timeStep);
704
705 // Verification
706 assert(!math::isNan(mArtInertia));
707 assert(!math::isNan(mArtInertiaImplicit));
708 }
709
710 //==============================================================================
updateBiasForce(const Eigen::Vector3d & _gravity,double _timeStep)711 void SoftBodyNode::updateBiasForce(
712 const Eigen::Vector3d& _gravity, double _timeStep)
713 {
714 const Eigen::Matrix6d& mI
715 = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
716 for (PointMass* pointMass : mPointMasses)
717 {
718 // Reset internal forces of point masses before used.
719 //
720 // Once control force for point mass is introduced, assign it to the
721 // internal force instead of always resetting the internal forces to zero.
722 pointMass->resetForces();
723
724 pointMass->updateBiasForceFD(_timeStep, _gravity);
725 }
726
727 // Gravity force
728 if (BodyNode::mAspectProperties.mGravityMode == true)
729 mFgravity.noalias()
730 = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
731 else
732 mFgravity.setZero();
733
734 // Set bias force
735 const Eigen::Vector6d& V = getSpatialVelocity();
736 mBiasForce = -math::dad(V, mI * V) - BodyNode::mAspectState.mFext - mFgravity;
737
738 // Verifycation
739 assert(!math::isNan(mBiasForce));
740
741 // And add child bias force
742 for (const auto& childBodyNode : mChildBodyNodes)
743 {
744 Joint* childJoint = childBodyNode->getParentJoint();
745
746 childJoint->addChildBiasForceTo(
747 mBiasForce,
748 childBodyNode->getArticulatedInertiaImplicit(),
749 childBodyNode->mBiasForce,
750 childBodyNode->getPartialAcceleration());
751 }
752
753 //
754 for (const auto& pointMass : mPointMasses)
755 {
756 mBiasForce.head<3>()
757 += pointMass->getLocalPosition().cross(pointMass->mBeta);
758 mBiasForce.tail<3>() += pointMass->mBeta;
759 }
760
761 // Verifycation
762 assert(!math::isNan(mBiasForce));
763
764 // Update parent joint's total force with implicit joint damping and spring
765 // forces
766 mParentJoint->updateTotalForce(
767 getArticulatedInertiaImplicit() * getPartialAcceleration() + mBiasForce,
768 _timeStep);
769 }
770
771 //==============================================================================
updateAccelerationFD()772 void SoftBodyNode::updateAccelerationFD()
773 {
774 BodyNode::updateAccelerationFD();
775
776 for (auto& pointMass : mPointMasses)
777 pointMass->updateAccelerationFD();
778
779 mNotifier->clearAccelerationNotice();
780 }
781
782 //==============================================================================
updateTransmittedForceFD()783 void SoftBodyNode::updateTransmittedForceFD()
784 {
785 BodyNode::updateTransmittedForceFD();
786
787 for (auto& pointMass : mPointMasses)
788 pointMass->updateTransmittedForce();
789 }
790
791 //==============================================================================
updateBiasImpulse()792 void SoftBodyNode::updateBiasImpulse()
793 {
794 for (auto& pointMass : mPointMasses)
795 pointMass->updateBiasImpulseFD();
796
797 // Update impulsive bias force
798 mBiasImpulse = -mConstraintImpulse;
799
800 // And add child bias impulse
801 for (auto& childBodyNode : mChildBodyNodes)
802 {
803 Joint* childJoint = childBodyNode->getParentJoint();
804
805 childJoint->addChildBiasImpulseTo(
806 mBiasImpulse,
807 childBodyNode->getArticulatedInertia(),
808 childBodyNode->mBiasImpulse);
809 }
810
811 for (auto& pointMass : mPointMasses)
812 {
813 mBiasImpulse.head<3>()
814 += pointMass->getLocalPosition().cross(pointMass->mImpBeta);
815 mBiasImpulse.tail<3>() += pointMass->mImpBeta;
816 }
817
818 // Verification
819 assert(!math::isNan(mBiasImpulse));
820
821 // Update parent joint's total force
822 mParentJoint->updateTotalImpulse(mBiasImpulse);
823 }
824
825 //==============================================================================
updateVelocityChangeFD()826 void SoftBodyNode::updateVelocityChangeFD()
827 {
828 BodyNode::updateVelocityChangeFD();
829
830 for (auto& pointMass : mPointMasses)
831 pointMass->updateVelocityChangeFD();
832 }
833
834 //==============================================================================
updateTransmittedImpulse()835 void SoftBodyNode::updateTransmittedImpulse()
836 {
837 BodyNode::updateTransmittedImpulse();
838
839 for (auto& pointMass : mPointMasses)
840 pointMass->updateTransmittedImpulse();
841 }
842
843 //==============================================================================
updateConstrainedTerms(double _timeStep)844 void SoftBodyNode::updateConstrainedTerms(double _timeStep)
845 {
846 BodyNode::updateConstrainedTerms(_timeStep);
847
848 for (auto& pointMass : mPointMasses)
849 pointMass->updateConstrainedTermsFD(_timeStep);
850 }
851
852 //==============================================================================
updateMassMatrix()853 void SoftBodyNode::updateMassMatrix()
854 {
855 BodyNode::updateMassMatrix();
856
857 // for (std::size_t i = 0; i < mPointMasses.size(); ++i)
858 // mPointMasses.at(i)->updateMassMatrix();
859 }
860
861 //==============================================================================
aggregateMassMatrix(Eigen::MatrixXd & _MCol,std::size_t _col)862 void SoftBodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col)
863 {
864 BodyNode::aggregateMassMatrix(_MCol, _col);
865 // //------------------------ PointMass Part
866 // ------------------------------------ for (std::size_t i = 0; i <
867 // mPointMasses.size(); ++i)
868 // mPointMasses.at(i)->aggregateMassMatrix(_MCol, _col);
869
870 // //----------------------- SoftBodyNode Part
871 // ----------------------------------
872 // //
873 // mM_F.noalias() = mI * mM_dV;
874
875 // // Verification
876 // assert(!math::isNan(mM_F));
877
878 // //
879 // for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
880 // it != mChildBodyNodes.end(); ++it)
881 // {
882 // mM_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(),
883 // (*it)->mM_F);
884 // }
885
886 // //
887 // for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
888 // it != mPointMasses.end(); ++it)
889 // {
890 // mM_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mM_F);
891 // mM_F.tail<3>() += (*it)->mM_F;
892 // }
893
894 // // Verification
895 // assert(!math::isNan(mM_F));
896
897 // //
898 // int dof = mParentJoint->getNumDofs();
899 // if (dof > 0)
900 // {
901 // int iStart = mParentJoint->getIndexInTree(0);
902 // _MCol->block(iStart, _col, dof, 1).noalias()
903 // = mParentJoint->getRelativeJacobian().transpose() * mM_F;
904 // }
905 }
906
907 //==============================================================================
aggregateAugMassMatrix(Eigen::MatrixXd & _MCol,std::size_t _col,double _timeStep)908 void SoftBodyNode::aggregateAugMassMatrix(
909 Eigen::MatrixXd& _MCol, std::size_t _col, double _timeStep)
910 {
911 // TODO(JS): Need to be reimplemented
912
913 const Eigen::Matrix6d& mI
914 = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
915 //------------------------ PointMass Part ------------------------------------
916 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
917 mPointMasses.at(i)->aggregateAugMassMatrix(_MCol, _col, _timeStep);
918
919 //----------------------- SoftBodyNode Part ----------------------------------
920 mM_F.noalias() = mI * mM_dV;
921 assert(!math::isNan(mM_F));
922
923 for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
924 it != mChildBodyNodes.end();
925 ++it)
926 {
927 mM_F += math::dAdInvT(
928 (*it)->getParentJoint()->getRelativeTransform(), (*it)->mM_F);
929 }
930 for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
931 it != mPointMasses.end();
932 ++it)
933 {
934 mM_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mM_F);
935 mM_F.tail<3>() += (*it)->mM_F;
936 }
937 assert(!math::isNan(mM_F));
938
939 std::size_t dof = mParentJoint->getNumDofs();
940 if (dof > 0)
941 {
942 Eigen::MatrixXd K = Eigen::MatrixXd::Zero(dof, dof);
943 Eigen::MatrixXd D = Eigen::MatrixXd::Zero(dof, dof);
944 for (std::size_t i = 0; i < dof; ++i)
945 {
946 K(i, i) = mParentJoint->getSpringStiffness(i);
947 D(i, i) = mParentJoint->getDampingCoefficient(i);
948 }
949 int iStart = mParentJoint->getIndexInTree(0);
950
951 // TODO(JS): Not recommended to use Joint::getAccelerations
952 _MCol.block(iStart, _col, dof, 1).noalias()
953 = mParentJoint->getRelativeJacobian().transpose() * mM_F
954 + D * (_timeStep * mParentJoint->getAccelerations())
955 + K * (_timeStep * _timeStep * mParentJoint->getAccelerations());
956 }
957 }
958
959 //==============================================================================
updateInvMassMatrix()960 void SoftBodyNode::updateInvMassMatrix()
961 {
962 //------------------------ PointMass Part ------------------------------------
963 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
964 mPointMasses.at(i)->updateInvMassMatrix();
965
966 //----------------------- SoftBodyNode Part ----------------------------------
967 //
968 mInvM_c.setZero();
969
970 //
971 for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
972 it != mChildBodyNodes.end();
973 ++it)
974 {
975 (*it)->getParentJoint()->addChildBiasForceForInvMassMatrix(
976 mInvM_c, (*it)->getArticulatedInertia(), (*it)->mInvM_c);
977 }
978
979 //
980 for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
981 it != mPointMasses.end();
982 ++it)
983 {
984 mInvM_c.head<3>()
985 += (*it)->getLocalPosition().cross((*it)->mBiasForceForInvMeta);
986 mInvM_c.tail<3>() += (*it)->mBiasForceForInvMeta;
987 }
988
989 // Verification
990 assert(!math::isNan(mInvM_c));
991
992 // Update parent joint's total force for inverse mass matrix
993 mParentJoint->updateTotalForceForInvMassMatrix(mInvM_c);
994 }
995
996 //==============================================================================
updateInvAugMassMatrix()997 void SoftBodyNode::updateInvAugMassMatrix()
998 {
999 BodyNode::updateInvAugMassMatrix();
1000
1001 // //------------------------ PointMass Part
1002 // ------------------------------------
1003 //// for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1004 //// mPointMasses.at(i)->updateInvAugMassMatrix();
1005
1006 // //----------------------- SoftBodyNode Part
1007 // ----------------------------------
1008 // //
1009 // mInvM_c.setZero();
1010
1011 // //
1012 // for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
1013 // it != mChildBodyNodes.end(); ++it)
1014 // {
1015 // (*it)->getParentJoint()->addChildBiasForceForInvAugMassMatrix(
1016 // mInvM_c, (*it)->getArticulatedInertiaImplicit(), (*it)->mInvM_c);
1017 // }
1018
1019 // //
1020 //// for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
1021 //// it != mPointMasses.end(); ++it)
1022 //// {
1023 //// mInvM_c.head<3>() +=
1024 ///(*it)->getLocalPosition().cross((*it)->mBiasForceForInvMeta); /
1025 /// mInvM_c.tail<3>() += (*it)->mBiasForceForInvMeta; / }
1026
1027 // // Verification
1028 // assert(!math::isNan(mInvM_c));
1029
1030 // // Update parent joint's total force for inverse mass matrix
1031 // mParentJoint->updateTotalForceForInvMassMatrix(mInvM_c);
1032 }
1033
1034 //==============================================================================
aggregateInvMassMatrix(Eigen::MatrixXd & _InvMCol,std::size_t _col)1035 void SoftBodyNode::aggregateInvMassMatrix(
1036 Eigen::MatrixXd& _InvMCol, std::size_t _col)
1037 {
1038 if (mParentBodyNode)
1039 {
1040 //
1041 mParentJoint->getInvMassMatrixSegment(
1042 _InvMCol, _col, getArticulatedInertia(), mParentBodyNode->mInvM_U);
1043
1044 //
1045 mInvM_U = math::AdInvT(
1046 mParentJoint->getRelativeTransform(), mParentBodyNode->mInvM_U);
1047 }
1048 else
1049 {
1050 //
1051 mParentJoint->getInvMassMatrixSegment(
1052 _InvMCol, _col, getArticulatedInertia(), Eigen::Vector6d::Zero());
1053
1054 //
1055 mInvM_U.setZero();
1056 }
1057
1058 //
1059 mParentJoint->addInvMassMatrixSegmentTo(mInvM_U);
1060
1061 //
1062 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1063 mPointMasses.at(i)->aggregateInvMassMatrix(_InvMCol, _col);
1064 }
1065
1066 //==============================================================================
aggregateInvAugMassMatrix(Eigen::MatrixXd & _InvMCol,std::size_t _col,double _timeStep)1067 void SoftBodyNode::aggregateInvAugMassMatrix(
1068 Eigen::MatrixXd& _InvMCol, std::size_t _col, double _timeStep)
1069 {
1070 BodyNode::aggregateInvAugMassMatrix(_InvMCol, _col, _timeStep);
1071
1072 // if (mParentBodyNode)
1073 // {
1074 // //
1075 // mParentJoint->getInvAugMassMatrixSegment(
1076 // *_InvMCol, _col, getArticulatedInertiaImplicit(),
1077 // mParentBodyNode->mInvM_U);
1078
1079 // //
1080 // mInvM_U = math::AdInvT(mParentJoint->mT, mParentBodyNode->mInvM_U);
1081 // }
1082 // else
1083 // {
1084 // //
1085 // mParentJoint->getInvAugMassMatrixSegment(
1086 // *_InvMCol, _col, getArticulatedInertiaImplicit(),
1087 // Eigen::Vector6d::Zero());
1088
1089 // //
1090 // mInvM_U.setZero();
1091 // }
1092
1093 // //
1094 // mParentJoint->addInvMassMatrixSegmentTo(mInvM_U);
1095
1096 // //
1097 //// for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1098 //// mPointMasses.at(i)->aggregateInvAugMassMatrix(_InvMCol, _col,
1099 ///_timeStep);
1100 }
1101
1102 //==============================================================================
aggregateCoriolisForceVector(Eigen::VectorXd & _C)1103 void SoftBodyNode::aggregateCoriolisForceVector(Eigen::VectorXd& _C)
1104 {
1105 BodyNode::aggregateCoriolisForceVector(_C);
1106 }
1107
1108 //==============================================================================
aggregateGravityForceVector(Eigen::VectorXd & _g,const Eigen::Vector3d & _gravity)1109 void SoftBodyNode::aggregateGravityForceVector(
1110 Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity)
1111 {
1112 const Eigen::Matrix6d& mI
1113 = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
1114 //------------------------ PointMass Part ------------------------------------
1115 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1116 mPointMasses.at(i)->aggregateGravityForceVector(_g, _gravity);
1117
1118 //----------------------- SoftBodyNode Part ----------------------------------
1119 if (BodyNode::mAspectProperties.mGravityMode == true)
1120 mG_F = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
1121 else
1122 mG_F.setZero();
1123
1124 for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
1125 it != mChildBodyNodes.end();
1126 ++it)
1127 {
1128 mG_F += math::dAdInvT(
1129 (*it)->mParentJoint->getRelativeTransform(), (*it)->mG_F);
1130 }
1131
1132 for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
1133 it != mPointMasses.end();
1134 ++it)
1135 {
1136 mG_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mG_F);
1137 mG_F.tail<3>() += (*it)->mG_F;
1138 }
1139
1140 int nGenCoords = mParentJoint->getNumDofs();
1141 if (nGenCoords > 0)
1142 {
1143 Eigen::VectorXd g
1144 = -(mParentJoint->getRelativeJacobian().transpose() * mG_F);
1145 int iStart = mParentJoint->getIndexInTree(0);
1146 _g.segment(iStart, nGenCoords) = g;
1147 }
1148 }
1149
1150 //==============================================================================
updateCombinedVector()1151 void SoftBodyNode::updateCombinedVector()
1152 {
1153 BodyNode::updateCombinedVector();
1154
1155 // for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1156 // mPointMasses.at(i)->updateCombinedVector();
1157 }
1158
1159 //==============================================================================
aggregateCombinedVector(Eigen::VectorXd & _Cg,const Eigen::Vector3d & _gravity)1160 void SoftBodyNode::aggregateCombinedVector(
1161 Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity)
1162 {
1163 BodyNode::aggregateCombinedVector(_Cg, _gravity);
1164 // //------------------------ PointMass Part
1165 // ------------------------------------ for (std::size_t i = 0; i <
1166 // mPointMasses.size(); ++i)
1167 // mPointMasses.at(i)->aggregateCombinedVector(_Cg, _gravity);
1168
1169 // //----------------------- SoftBodyNode Part
1170 // ----------------------------------
1171 // // H(i) = I(i) * W(i) -
1172 // // dad{V}(I(i) * V(i)) + sum(k \in children)
1173 // dAd_{T(i,j)^{-1}}(H(k)) if (mGravityMode == true)
1174 // mFgravity = mI * math::AdInvRLinear(mW, _gravity);
1175 // else
1176 // mFgravity.setZero();
1177
1178 // mCg_F = mI * mCg_dV;
1179 // mCg_F -= mFgravity;
1180 // mCg_F -= math::dad(mV, mI * mV);
1181
1182 // for (std::vector<BodyNode*>::iterator it = mChildBodyNodes.begin();
1183 // it != mChildBodyNodes.end(); ++it)
1184 // {
1185 // mCg_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(),
1186 // (*it)->mCg_F);
1187 // }
1188
1189 // for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
1190 // it != mPointMasses.end(); ++it)
1191 // {
1192 // mCg_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mCg_F);
1193 // mCg_F.tail<3>() += (*it)->mCg_F;
1194 // }
1195
1196 // int nGenCoords = mParentJoint->getNumDofs();
1197 // if (nGenCoords > 0)
1198 // {
1199 // Eigen::VectorXd Cg = mParentJoint->getRelativeJacobian().transpose() *
1200 // mCg_F; int iStart = mParentJoint->getIndexInTree(0);
1201 // _Cg->segment(iStart, nGenCoords) = Cg;
1202 // }
1203 }
1204
1205 //==============================================================================
aggregateExternalForces(Eigen::VectorXd & _Fext)1206 void SoftBodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext)
1207 {
1208 //------------------------ PointMass Part ------------------------------------
1209 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1210 mPointMasses.at(i)->aggregateExternalForces(_Fext);
1211
1212 //----------------------- SoftBodyNode Part ----------------------------------
1213 mFext_F = BodyNode::mAspectState.mFext;
1214
1215 for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
1216 it != mChildBodyNodes.end();
1217 ++it)
1218 {
1219 mFext_F += math::dAdInvT(
1220 (*it)->mParentJoint->getRelativeTransform(), (*it)->mFext_F);
1221 }
1222
1223 for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
1224 it != mPointMasses.end();
1225 ++it)
1226 {
1227 mFext_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mFext);
1228 mFext_F.tail<3>() += (*it)->mFext;
1229 }
1230
1231 int nGenCoords = mParentJoint->getNumDofs();
1232 if (nGenCoords > 0)
1233 {
1234 Eigen::VectorXd Fext
1235 = mParentJoint->getRelativeJacobian().transpose() * mFext_F;
1236 int iStart = mParentJoint->getIndexInTree(0);
1237 _Fext.segment(iStart, nGenCoords) = Fext;
1238 }
1239 }
1240
1241 //==============================================================================
clearExternalForces()1242 void SoftBodyNode::clearExternalForces()
1243 {
1244 BodyNode::clearExternalForces();
1245
1246 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1247 mPointMasses.at(i)->clearExtForce();
1248 }
1249
1250 //==============================================================================
clearInternalForces()1251 void SoftBodyNode::clearInternalForces()
1252 {
1253 BodyNode::clearInternalForces();
1254
1255 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1256 mPointMasses[i]->resetForces();
1257 }
1258
1259 //==============================================================================
_addPiToArtInertia(const Eigen::Vector3d & _p,double _Pi) const1260 void SoftBodyNode::_addPiToArtInertia(
1261 const Eigen::Vector3d& _p, double _Pi) const
1262 {
1263 Eigen::Matrix3d tmp = math::makeSkewSymmetric(_p);
1264
1265 mArtInertia.topLeftCorner<3, 3>() -= _Pi * tmp * tmp;
1266 mArtInertia.topRightCorner<3, 3>() += _Pi * tmp;
1267 mArtInertia.bottomLeftCorner<3, 3>() -= _Pi * tmp;
1268
1269 mArtInertia(3, 3) += _Pi;
1270 mArtInertia(4, 4) += _Pi;
1271 mArtInertia(5, 5) += _Pi;
1272 }
1273
1274 //==============================================================================
_addPiToArtInertiaImplicit(const Eigen::Vector3d & _p,double _ImplicitPi) const1275 void SoftBodyNode::_addPiToArtInertiaImplicit(
1276 const Eigen::Vector3d& _p, double _ImplicitPi) const
1277 {
1278 Eigen::Matrix3d tmp = math::makeSkewSymmetric(_p);
1279
1280 mArtInertiaImplicit.topLeftCorner<3, 3>() -= _ImplicitPi * tmp * tmp;
1281 mArtInertiaImplicit.topRightCorner<3, 3>() += _ImplicitPi * tmp;
1282 mArtInertiaImplicit.bottomLeftCorner<3, 3>() -= _ImplicitPi * tmp;
1283
1284 mArtInertiaImplicit(3, 3) += _ImplicitPi;
1285 mArtInertiaImplicit(4, 4) += _ImplicitPi;
1286 mArtInertiaImplicit(5, 5) += _ImplicitPi;
1287 }
1288
1289 //==============================================================================
updateInertiaWithPointMass()1290 void SoftBodyNode::updateInertiaWithPointMass()
1291 {
1292 // TODO(JS): Not implemented
1293
1294 const Eigen::Matrix6d& mI
1295 = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
1296 mI2 = mI;
1297
1298 for (std::size_t i = 0; i < mPointMasses.size(); ++i)
1299 {
1300 }
1301 }
1302
1303 //==============================================================================
makeBoxProperties(const Eigen::Vector3d & _size,const Eigen::Isometry3d & _localTransform,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)1304 SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeBoxProperties(
1305 const Eigen::Vector3d& _size,
1306 const Eigen::Isometry3d& _localTransform,
1307 double _totalMass,
1308 double _vertexStiffness,
1309 double _edgeStiffness,
1310 double _dampingCoeff)
1311 {
1312 SoftBodyNode::UniqueProperties properties(
1313 _vertexStiffness, _edgeStiffness, _dampingCoeff);
1314
1315 //----------------------------------------------------------------------------
1316 // Point masses
1317 //----------------------------------------------------------------------------
1318 // Number of point masses
1319 std::size_t nPointMasses = 8;
1320 properties.mPointProps.resize(8);
1321
1322 // Mass per vertices
1323 double mass = _totalMass / nPointMasses;
1324
1325 // Resting positions for each point mass
1326 std::vector<Eigen::Vector3d> restingPos(
1327 nPointMasses, Eigen::Vector3d::Zero());
1328 restingPos[0] = _size.cwiseProduct(Eigen::Vector3d(-1.0, -1.0, -1.0)) * 0.5;
1329 restingPos[1] = _size.cwiseProduct(Eigen::Vector3d(+1.0, -1.0, -1.0)) * 0.5;
1330 restingPos[2] = _size.cwiseProduct(Eigen::Vector3d(-1.0, +1.0, -1.0)) * 0.5;
1331 restingPos[3] = _size.cwiseProduct(Eigen::Vector3d(+1.0, +1.0, -1.0)) * 0.5;
1332 restingPos[4] = _size.cwiseProduct(Eigen::Vector3d(-1.0, -1.0, +1.0)) * 0.5;
1333 restingPos[5] = _size.cwiseProduct(Eigen::Vector3d(+1.0, -1.0, +1.0)) * 0.5;
1334 restingPos[6] = _size.cwiseProduct(Eigen::Vector3d(-1.0, +1.0, +1.0)) * 0.5;
1335 restingPos[7] = _size.cwiseProduct(Eigen::Vector3d(+1.0, +1.0, +1.0)) * 0.5;
1336
1337 // Point masses
1338 for (std::size_t i = 0; i < nPointMasses; ++i)
1339 {
1340 properties.mPointProps[i].mX0 = _localTransform * restingPos[i];
1341 properties.mPointProps[i].mMass = mass;
1342 }
1343
1344 //----------------------------------------------------------------------------
1345 // Edges
1346 //----------------------------------------------------------------------------
1347 // -- Bottoms
1348 properties.connectPointMasses(0, 1);
1349 properties.connectPointMasses(1, 3);
1350 properties.connectPointMasses(3, 2);
1351 properties.connectPointMasses(2, 0);
1352
1353 // -- Tops
1354 properties.connectPointMasses(4, 5);
1355 properties.connectPointMasses(5, 7);
1356 properties.connectPointMasses(7, 6);
1357 properties.connectPointMasses(6, 4);
1358
1359 // -- Sides
1360 properties.connectPointMasses(0, 4);
1361 properties.connectPointMasses(1, 5);
1362 properties.connectPointMasses(2, 6);
1363 properties.connectPointMasses(3, 7);
1364
1365 //----------------------------------------------------------------------------
1366 // Faces
1367 //----------------------------------------------------------------------------
1368 // -- -Z
1369 properties.addFace(Eigen::Vector3i(1, 0, 2)); // 0
1370 properties.addFace(Eigen::Vector3i(1, 2, 3)); // 1
1371
1372 // -- +Z
1373 properties.addFace(Eigen::Vector3i(5, 6, 4)); // 2
1374 properties.addFace(Eigen::Vector3i(5, 7, 6)); // 3
1375
1376 // -- -Y
1377 properties.addFace(Eigen::Vector3i(0, 5, 4)); // 4
1378 properties.addFace(Eigen::Vector3i(0, 1, 5)); // 5
1379
1380 // -- +Y
1381 properties.addFace(Eigen::Vector3i(1, 3, 7)); // 6
1382 properties.addFace(Eigen::Vector3i(1, 7, 5)); // 7
1383
1384 // -- -X
1385 properties.addFace(Eigen::Vector3i(3, 2, 6)); // 8
1386 properties.addFace(Eigen::Vector3i(3, 6, 7)); // 9
1387
1388 // -- +X
1389 properties.addFace(Eigen::Vector3i(2, 0, 4)); // 10
1390 properties.addFace(Eigen::Vector3i(2, 4, 6)); // 11
1391
1392 return properties;
1393 }
1394
1395 //==============================================================================
setBox(SoftBodyNode * _softBodyNode,const Eigen::Vector3d & _size,const Eigen::Isometry3d & _localTransform,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)1396 void SoftBodyNodeHelper::setBox(
1397 SoftBodyNode* _softBodyNode,
1398 const Eigen::Vector3d& _size,
1399 const Eigen::Isometry3d& _localTransform,
1400 double _totalMass,
1401 double _vertexStiffness,
1402 double _edgeStiffness,
1403 double _dampingCoeff)
1404 {
1405 assert(_softBodyNode != nullptr);
1406 _softBodyNode->setProperties(makeBoxProperties(
1407 _size,
1408 _localTransform,
1409 _totalMass,
1410 _vertexStiffness,
1411 _edgeStiffness,
1412 _dampingCoeff));
1413 }
1414
1415 //==============================================================================
makeBoxProperties(const Eigen::Vector3d & _size,const Eigen::Isometry3d & _localTransform,const Eigen::Vector3i & _frags,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)1416 SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeBoxProperties(
1417 const Eigen::Vector3d& _size,
1418 const Eigen::Isometry3d& _localTransform,
1419 const Eigen::Vector3i& _frags,
1420 double _totalMass,
1421 double _vertexStiffness,
1422 double _edgeStiffness,
1423 double _dampingCoeff)
1424 {
1425 Eigen::Vector3i frags = _frags;
1426
1427 for (int i = 0; i < 3; ++i)
1428 {
1429 if (frags[i] <= 2)
1430 {
1431 dtwarn << "[SoftBodyNodeHelper::makeBoxProperties] Invalid argument for "
1432 << "_frags. The number of vertices assigned to soft box edge #"
1433 << i << " is " << frags[i] << ", but it must be greater than or "
1434 << "equal to 3. We will set it to 3.\n";
1435 frags[i] = 3;
1436 }
1437 }
1438
1439 std::size_t id = 0;
1440
1441 SoftBodyNode::UniqueProperties properties(
1442 _vertexStiffness, _edgeStiffness, _dampingCoeff);
1443
1444 //----------------------------------------------------------------------------
1445 // Point masses
1446 //----------------------------------------------------------------------------
1447 // Number of point masses
1448 assert(frags[0] > 1 && frags[1] > 1 && frags[2] > 1);
1449
1450 std::size_t nCorners = 8;
1451 std::size_t nVerticesAtEdgeX = frags[0] - 2;
1452 std::size_t nVerticesAtEdgeY = frags[1] - 2;
1453 std::size_t nVerticesAtEdgeZ = frags[2] - 2;
1454 std::size_t nVerticesAtSideX = nVerticesAtEdgeY * nVerticesAtEdgeZ;
1455 std::size_t nVerticesAtSideY = nVerticesAtEdgeZ * nVerticesAtEdgeX;
1456 std::size_t nVerticesAtSideZ = nVerticesAtEdgeX * nVerticesAtEdgeY;
1457 std::size_t nVertices
1458 = nCorners + 4 * (nVerticesAtEdgeX + nVerticesAtEdgeY + nVerticesAtEdgeZ)
1459 + 2 * (nVerticesAtSideX + nVerticesAtSideY + nVerticesAtSideZ);
1460
1461 // Mass per vertices
1462 double mass = _totalMass / nVertices;
1463
1464 Eigen::Vector3d segLength(
1465 _size[0] / (frags[0] - 1),
1466 _size[1] / (frags[1] - 1),
1467 _size[2] / (frags[2] - 1));
1468
1469 typedef std::pair<PointMass::Properties, std::size_t> PointPair;
1470
1471 std::vector<PointPair> corners(nCorners);
1472
1473 std::vector<std::vector<PointPair> > edgeX(
1474 4, std::vector<PointPair>(nVerticesAtEdgeX));
1475 std::vector<std::vector<PointPair> > edgeY(
1476 4, std::vector<PointPair>(nVerticesAtEdgeY));
1477 std::vector<std::vector<PointPair> > edgeZ(
1478 4, std::vector<PointPair>(nVerticesAtEdgeZ));
1479
1480 std::vector<std::vector<PointPair> > sideXNeg(
1481 nVerticesAtEdgeY, std::vector<PointPair>(nVerticesAtEdgeZ));
1482 std::vector<std::vector<PointPair> > sideXPos(
1483 nVerticesAtEdgeY, std::vector<PointPair>(nVerticesAtEdgeZ));
1484
1485 std::vector<std::vector<PointPair> > sideYNeg(
1486 nVerticesAtEdgeZ, std::vector<PointPair>(nVerticesAtEdgeX));
1487 std::vector<std::vector<PointPair> > sideYPos(
1488 nVerticesAtEdgeZ, std::vector<PointPair>(nVerticesAtEdgeX));
1489
1490 std::vector<std::vector<PointPair> > sideZNeg(
1491 nVerticesAtEdgeX, std::vector<PointPair>(nVerticesAtEdgeY));
1492 std::vector<std::vector<PointPair> > sideZPos(
1493 nVerticesAtEdgeX, std::vector<PointPair>(nVerticesAtEdgeY));
1494
1495 Eigen::Vector3d x0y0z0
1496 = _size.cwiseProduct(Eigen::Vector3d(-1.0, -1.0, -1.0)) * 0.5;
1497 Eigen::Vector3d x1y0z0
1498 = _size.cwiseProduct(Eigen::Vector3d(+1.0, -1.0, -1.0)) * 0.5;
1499 Eigen::Vector3d x1y1z0
1500 = _size.cwiseProduct(Eigen::Vector3d(+1.0, +1.0, -1.0)) * 0.5;
1501 Eigen::Vector3d x0y1z0
1502 = _size.cwiseProduct(Eigen::Vector3d(-1.0, +1.0, -1.0)) * 0.5;
1503 Eigen::Vector3d x0y0z1
1504 = _size.cwiseProduct(Eigen::Vector3d(-1.0, -1.0, +1.0)) * 0.5;
1505 Eigen::Vector3d x1y0z1
1506 = _size.cwiseProduct(Eigen::Vector3d(+1.0, -1.0, +1.0)) * 0.5;
1507 Eigen::Vector3d x1y1z1
1508 = _size.cwiseProduct(Eigen::Vector3d(+1.0, +1.0, +1.0)) * 0.5;
1509 Eigen::Vector3d x0y1z1
1510 = _size.cwiseProduct(Eigen::Vector3d(-1.0, +1.0, +1.0)) * 0.5;
1511
1512 std::vector<Eigen::Vector3d> beginPts;
1513 Eigen::Vector3d restPos;
1514
1515 // Corners
1516 beginPts.resize(nCorners);
1517 beginPts[0] = x0y0z0;
1518 beginPts[1] = x1y0z0;
1519 beginPts[2] = x1y1z0;
1520 beginPts[3] = x0y1z0;
1521 beginPts[4] = x0y0z1;
1522 beginPts[5] = x1y0z1;
1523 beginPts[6] = x1y1z1;
1524 beginPts[7] = x0y1z1;
1525
1526 for (std::size_t i = 0; i < nCorners; ++i)
1527 {
1528 corners[i].first.setRestingPosition(_localTransform * beginPts[i]);
1529 corners[i].first.setMass(mass);
1530 properties.addPointMass(corners[i].first);
1531
1532 corners[i].second = id++;
1533 }
1534
1535 // Edges (along X-axis)
1536 beginPts.resize(4);
1537 beginPts[0] = x0y0z0;
1538 beginPts[1] = x0y1z0;
1539 beginPts[2] = x0y1z1;
1540 beginPts[3] = x0y0z1;
1541
1542 for (std::size_t i = 0; i < 4; ++i)
1543 {
1544 restPos = beginPts[i];
1545
1546 for (std::size_t j = 0; j < nVerticesAtEdgeX; ++j)
1547 {
1548 restPos[0] += segLength[0];
1549
1550 edgeX[i][j].first.setRestingPosition(_localTransform * restPos);
1551 edgeX[i][j].first.setMass(mass);
1552 properties.addPointMass(edgeX[i][j].first);
1553
1554 edgeX[i][j].second = id++;
1555 }
1556 }
1557
1558 // Edges (along Y-axis)
1559 beginPts[0] = x0y0z0;
1560 beginPts[1] = x0y0z1;
1561 beginPts[2] = x1y0z1;
1562 beginPts[3] = x1y0z0;
1563
1564 for (std::size_t i = 0; i < 4; ++i)
1565 {
1566 restPos = beginPts[i];
1567
1568 for (std::size_t j = 0; j < nVerticesAtEdgeY; ++j)
1569 {
1570 restPos[1] += segLength[1];
1571
1572 edgeY[i][j].first.setRestingPosition(_localTransform * restPos);
1573 edgeY[i][j].first.setMass(mass);
1574 properties.addPointMass(edgeY[i][j].first);
1575
1576 edgeY[i][j].second = id++;
1577 }
1578 }
1579
1580 // Edges (along Z-axis)
1581 beginPts[0] = x0y0z0;
1582 beginPts[1] = x1y0z0;
1583 beginPts[2] = x1y1z0;
1584 beginPts[3] = x0y1z0;
1585
1586 for (std::size_t i = 0; i < 4; ++i)
1587 {
1588 restPos = beginPts[i];
1589
1590 for (std::size_t j = 0; j < nVerticesAtEdgeZ; ++j)
1591 {
1592 restPos[2] += segLength[2];
1593
1594 edgeZ[i][j].first.setRestingPosition(_localTransform * restPos);
1595 edgeZ[i][j].first.setMass(mass);
1596 properties.addPointMass(edgeZ[i][j].first);
1597
1598 edgeZ[i][j].second = id++;
1599 }
1600 }
1601
1602 // Negative X side
1603 restPos = x0y0z0;
1604
1605 for (std::size_t i = 0; i < nVerticesAtEdgeY; ++i)
1606 {
1607 restPos[2] = x0y0z0[2];
1608 restPos[1] += segLength[1];
1609
1610 for (std::size_t j = 0; j < nVerticesAtEdgeZ; ++j)
1611 {
1612 restPos[2] += segLength[2];
1613
1614 sideXNeg[i][j].first.setRestingPosition(_localTransform * restPos);
1615 sideXNeg[i][j].first.setMass(mass);
1616 properties.addPointMass(sideXNeg[i][j].first);
1617
1618 sideXNeg[i][j].second = id++;
1619 }
1620 }
1621
1622 // Positive X side
1623 restPos = x1y0z0;
1624
1625 for (std::size_t i = 0; i < nVerticesAtEdgeY; ++i)
1626 {
1627 restPos[2] = x1y0z0[2];
1628 restPos[1] += segLength[1];
1629
1630 for (std::size_t j = 0; j < nVerticesAtEdgeZ; ++j)
1631 {
1632 restPos[2] += segLength[2];
1633
1634 sideXPos[i][j].first.setRestingPosition(_localTransform * restPos);
1635 sideXPos[i][j].first.setMass(mass);
1636 properties.addPointMass(sideXPos[i][j].first);
1637
1638 sideXPos[i][j].second = id++;
1639 }
1640 }
1641
1642 // Negative Y side
1643 restPos = x0y0z0;
1644
1645 for (std::size_t i = 0; i < nVerticesAtEdgeZ; ++i)
1646 {
1647 restPos[0] = x0y0z0[0];
1648 restPos[2] += segLength[2];
1649
1650 for (std::size_t j = 0; j < nVerticesAtEdgeX; ++j)
1651 {
1652 restPos[0] += segLength[0];
1653
1654 sideYNeg[i][j].first.setRestingPosition(_localTransform * restPos);
1655 sideYNeg[i][j].first.setMass(mass);
1656 properties.addPointMass(sideYNeg[i][j].first);
1657
1658 sideYNeg[i][j].second = id++;
1659 }
1660 }
1661
1662 // Positive Y side
1663 restPos = x0y1z0;
1664
1665 for (std::size_t i = 0; i < nVerticesAtEdgeZ; ++i)
1666 {
1667 restPos[0] = x0y1z0[0];
1668 restPos[2] += segLength[2];
1669
1670 for (std::size_t j = 0; j < nVerticesAtEdgeX; ++j)
1671 {
1672 restPos[0] += segLength[0];
1673
1674 sideYPos[i][j].first.setRestingPosition(_localTransform * restPos);
1675 sideYPos[i][j].first.setMass(mass);
1676 properties.addPointMass(sideYPos[i][j].first);
1677
1678 sideYPos[i][j].second = id++;
1679 }
1680 }
1681
1682 // Negative Z side
1683 restPos = x0y0z0;
1684
1685 for (std::size_t i = 0; i < nVerticesAtEdgeX; ++i)
1686 {
1687 restPos[1] = x0y0z0[1];
1688 restPos[0] += segLength[0];
1689
1690 for (std::size_t j = 0; j < nVerticesAtEdgeY; ++j)
1691 {
1692 restPos[1] += segLength[1];
1693
1694 sideZNeg[i][j].first.setRestingPosition(_localTransform * restPos);
1695 sideZNeg[i][j].first.setMass(mass);
1696 properties.addPointMass(sideZNeg[i][j].first);
1697
1698 sideZNeg[i][j].second = id++;
1699 }
1700 }
1701
1702 // Positive Z side
1703 restPos = x0y0z1;
1704
1705 for (std::size_t i = 0; i < nVerticesAtEdgeX; ++i)
1706 {
1707 restPos[1] = x0y0z1[1];
1708 restPos[0] += segLength[0];
1709
1710 for (std::size_t j = 0; j < nVerticesAtEdgeY; ++j)
1711 {
1712 restPos[1] += segLength[1];
1713
1714 sideZPos[i][j].first.setRestingPosition(_localTransform * restPos);
1715 sideZPos[i][j].first.setMass(mass);
1716 properties.addPointMass(sideZPos[i][j].first);
1717
1718 sideZPos[i][j].second = id++;
1719 }
1720 }
1721
1722 //----------------------------------------------------------------------------
1723 // Faces
1724 //----------------------------------------------------------------------------
1725 std::size_t nFacesX = 2 * (frags[1] - 1) * (frags[2] - 1);
1726 std::size_t nFacesY = 2 * (frags[2] - 1) * (frags[0] - 1);
1727 std::size_t nFacesZ = 2 * (frags[0] - 1) * (frags[1] - 1);
1728 std::size_t nFaces = 2 * nFacesX + 2 * nFacesY + 2 * nFacesZ;
1729
1730 std::vector<Eigen::Vector3i> faces(nFaces, Eigen::Vector3i::Zero());
1731
1732 int fIdx = 0;
1733
1734 // Corners[0] faces
1735 faces[fIdx][0] = corners[0].second;
1736 faces[fIdx][1] = edgeZ[0][0].second;
1737 faces[fIdx][2] = edgeY[0][0].second;
1738 fIdx++;
1739
1740 faces[fIdx][0] = sideXNeg[0][0].second;
1741 faces[fIdx][1] = edgeY[0][0].second;
1742 faces[fIdx][2] = edgeZ[0][0].second;
1743 fIdx++;
1744
1745 faces[fIdx][0] = corners[0].second;
1746 faces[fIdx][1] = edgeX[0][0].second;
1747 faces[fIdx][2] = edgeZ[0][0].second;
1748 fIdx++;
1749
1750 faces[fIdx][0] = sideYNeg[0][0].second;
1751 faces[fIdx][1] = edgeZ[0][0].second;
1752 faces[fIdx][2] = edgeX[0][0].second;
1753 fIdx++;
1754
1755 faces[fIdx][0] = corners[0].second;
1756 faces[fIdx][1] = edgeY[0][0].second;
1757 faces[fIdx][2] = edgeX[0][0].second;
1758 fIdx++;
1759
1760 faces[fIdx][0] = sideZNeg[0][0].second;
1761 faces[fIdx][1] = edgeX[0][0].second;
1762 faces[fIdx][2] = edgeY[0][0].second;
1763 fIdx++;
1764
1765 properties.addFace(faces[0]);
1766 properties.addFace(faces[1]);
1767 properties.addFace(faces[2]);
1768 properties.addFace(faces[3]);
1769 properties.addFace(faces[4]);
1770 properties.addFace(faces[5]);
1771
1772 // Corners[1] faces
1773 faces[fIdx][0] = corners[1].second;
1774 faces[fIdx][1] = edgeY[3][0].second;
1775 faces[fIdx][2] = edgeZ[1][0].second;
1776 fIdx++;
1777
1778 faces[fIdx][0] = sideXPos[0][0].second;
1779 faces[fIdx][1] = edgeZ[1][0].second;
1780 faces[fIdx][2] = edgeY[3][0].second;
1781 fIdx++;
1782
1783 faces[fIdx][0] = corners[1].second;
1784 faces[fIdx][1] = edgeZ[1][0].second;
1785 faces[fIdx][2] = edgeX[0].back().second;
1786 fIdx++;
1787
1788 faces[fIdx][0] = sideYNeg[0].back().second;
1789 faces[fIdx][1] = edgeX[0].back().second;
1790 faces[fIdx][2] = edgeZ[1][0].second;
1791 fIdx++;
1792
1793 faces[fIdx][0] = corners[1].second;
1794 faces[fIdx][1] = edgeX[0].back().second;
1795 faces[fIdx][2] = edgeY[3][0].second;
1796 fIdx++;
1797
1798 faces[fIdx][0] = sideZNeg.back()[0].second;
1799 faces[fIdx][1] = edgeY[3][0].second;
1800 faces[fIdx][2] = edgeX[0].back().second;
1801 fIdx++;
1802
1803 properties.addFace(faces[6]);
1804 properties.addFace(faces[7]);
1805 properties.addFace(faces[8]);
1806 properties.addFace(faces[9]);
1807 properties.addFace(faces[10]);
1808 properties.addFace(faces[11]);
1809
1810 // Corners[2] faces
1811 faces[fIdx][0] = corners[2].second;
1812 faces[fIdx][1] = edgeZ[2][0].second;
1813 faces[fIdx][2] = edgeY[3].back().second;
1814 fIdx++;
1815
1816 faces[fIdx][0] = sideXPos.back()[0].second;
1817 faces[fIdx][1] = edgeY[3].back().second;
1818 faces[fIdx][2] = edgeZ[2][0].second;
1819 fIdx++;
1820
1821 faces[fIdx][0] = corners[2].second;
1822 faces[fIdx][1] = edgeX[1].back().second;
1823 faces[fIdx][2] = edgeZ[2][0].second;
1824 fIdx++;
1825
1826 faces[fIdx][0] = sideYPos[0].back().second;
1827 faces[fIdx][1] = edgeZ[2][0].second;
1828 faces[fIdx][2] = edgeX[1].back().second;
1829 fIdx++;
1830
1831 faces[fIdx][0] = corners[2].second;
1832 faces[fIdx][1] = edgeY[3].back().second;
1833 faces[fIdx][2] = edgeX[1].back().second;
1834 fIdx++;
1835
1836 faces[fIdx][0] = sideZNeg.back().back().second;
1837 faces[fIdx][1] = edgeX[1].back().second;
1838 faces[fIdx][2] = edgeY[3].back().second;
1839 fIdx++;
1840
1841 properties.addFace(faces[12]);
1842 properties.addFace(faces[13]);
1843 properties.addFace(faces[14]);
1844 properties.addFace(faces[15]);
1845 properties.addFace(faces[16]);
1846 properties.addFace(faces[17]);
1847
1848 // Corners[3] faces
1849 faces[fIdx][0] = corners[3].second;
1850 faces[fIdx][1] = edgeY[0].back().second;
1851 faces[fIdx][2] = edgeZ[3][0].second;
1852 fIdx++;
1853
1854 faces[fIdx][0] = sideXNeg.back()[0].second;
1855 faces[fIdx][1] = edgeZ[3][0].second;
1856 faces[fIdx][2] = edgeY[0].back().second;
1857 fIdx++;
1858
1859 faces[fIdx][0] = corners[3].second;
1860 faces[fIdx][1] = edgeZ[3][0].second;
1861 faces[fIdx][2] = edgeX[1][0].second;
1862 fIdx++;
1863
1864 faces[fIdx][0] = sideYPos[0][0].second;
1865 faces[fIdx][1] = edgeX[1][0].second;
1866 faces[fIdx][2] = edgeZ[3][0].second;
1867 fIdx++;
1868
1869 faces[fIdx][0] = corners[3].second;
1870 faces[fIdx][1] = edgeX[1][0].second;
1871 faces[fIdx][2] = edgeY[0].back().second;
1872 fIdx++;
1873
1874 faces[fIdx][0] = sideZNeg[0].back().second;
1875 faces[fIdx][1] = edgeY[0].back().second;
1876 faces[fIdx][2] = edgeX[1][0].second;
1877 fIdx++;
1878
1879 properties.addFace(faces[18]);
1880 properties.addFace(faces[19]);
1881 properties.addFace(faces[20]);
1882 properties.addFace(faces[21]);
1883 properties.addFace(faces[22]);
1884 properties.addFace(faces[23]);
1885
1886 // Corners[4] faces
1887 faces[fIdx][0] = corners[4].second;
1888 faces[fIdx][1] = edgeY[1][0].second;
1889 faces[fIdx][2] = edgeZ[0].back().second;
1890 fIdx++;
1891
1892 faces[fIdx][0] = sideXNeg[0].back().second;
1893 faces[fIdx][1] = edgeY[1][0].second;
1894 faces[fIdx][2] = edgeZ[0].back().second;
1895 fIdx++;
1896
1897 faces[fIdx][0] = corners[4].second;
1898 faces[fIdx][1] = edgeZ[0].back().second;
1899 faces[fIdx][2] = edgeX[3][0].second;
1900 fIdx++;
1901
1902 faces[fIdx][0] = sideYNeg.back()[0].second;
1903 faces[fIdx][1] = edgeZ[0].back().second;
1904 faces[fIdx][2] = edgeX[3][0].second;
1905 fIdx++;
1906
1907 faces[fIdx][0] = corners[4].second;
1908 faces[fIdx][1] = edgeX[3][0].second;
1909 faces[fIdx][2] = edgeY[1][0].second;
1910 fIdx++;
1911
1912 faces[fIdx][0] = sideZPos[0][0].second;
1913 faces[fIdx][1] = edgeX[3][0].second;
1914 faces[fIdx][2] = edgeY[1][0].second;
1915 fIdx++;
1916
1917 properties.addFace(faces[24]);
1918 properties.addFace(faces[25]);
1919 properties.addFace(faces[26]);
1920 properties.addFace(faces[27]);
1921 properties.addFace(faces[28]);
1922 properties.addFace(faces[29]);
1923
1924 // Corners[5] faces
1925 faces[fIdx][0] = corners[5].second;
1926 faces[fIdx][1] = edgeZ[1].back().second;
1927 faces[fIdx][2] = edgeY[2][0].second;
1928 fIdx++;
1929
1930 faces[fIdx][0] = sideXPos[0].back().second;
1931 faces[fIdx][1] = edgeY[2][0].second;
1932 faces[fIdx][2] = edgeZ[1].back().second;
1933 fIdx++;
1934
1935 faces[fIdx][0] = corners[5].second;
1936 faces[fIdx][1] = edgeX[3].back().second;
1937 faces[fIdx][2] = edgeZ[1].back().second;
1938 fIdx++;
1939
1940 faces[fIdx][0] = sideYNeg.back().back().second;
1941 faces[fIdx][1] = edgeZ[1].back().second;
1942 faces[fIdx][2] = edgeX[3].back().second;
1943 fIdx++;
1944
1945 faces[fIdx][0] = corners[5].second;
1946 faces[fIdx][1] = edgeY[2][0].second;
1947 faces[fIdx][2] = edgeX[3].back().second;
1948 fIdx++;
1949
1950 faces[fIdx][0] = sideZPos.back()[0].second;
1951 faces[fIdx][1] = edgeX[3].back().second;
1952 faces[fIdx][2] = edgeY[2][0].second;
1953 fIdx++;
1954
1955 properties.addFace(faces[30]);
1956 properties.addFace(faces[31]);
1957 properties.addFace(faces[32]);
1958 properties.addFace(faces[33]);
1959 properties.addFace(faces[34]);
1960 properties.addFace(faces[35]);
1961
1962 // Corners[6] faces
1963 faces[fIdx][0] = corners[6].second;
1964 faces[fIdx][1] = edgeY[2].back().second;
1965 faces[fIdx][2] = edgeZ[2].back().second;
1966 fIdx++;
1967
1968 faces[fIdx][0] = sideXPos.back().back().second;
1969 faces[fIdx][1] = edgeZ[2].back().second;
1970 faces[fIdx][2] = edgeY[2].back().second;
1971 fIdx++;
1972
1973 faces[fIdx][0] = corners[6].second;
1974 faces[fIdx][1] = edgeZ[2].back().second;
1975 faces[fIdx][2] = edgeX[2].back().second;
1976 fIdx++;
1977
1978 faces[fIdx][0] = sideYPos.back().back().second;
1979 faces[fIdx][1] = edgeX[2].back().second;
1980 faces[fIdx][2] = edgeZ[2].back().second;
1981 fIdx++;
1982
1983 faces[fIdx][0] = corners[6].second;
1984 faces[fIdx][1] = edgeX[2].back().second;
1985 faces[fIdx][2] = edgeY[2].back().second;
1986 fIdx++;
1987
1988 faces[fIdx][0] = sideZPos.back().back().second;
1989 faces[fIdx][1] = edgeY[2].back().second;
1990 faces[fIdx][2] = edgeX[2].back().second;
1991 fIdx++;
1992
1993 properties.addFace(faces[36]);
1994 properties.addFace(faces[37]);
1995 properties.addFace(faces[38]);
1996 properties.addFace(faces[39]);
1997 properties.addFace(faces[40]);
1998 properties.addFace(faces[41]);
1999
2000 // Corners[7] faces
2001 faces[fIdx][0] = corners[7].second;
2002 faces[fIdx][1] = edgeZ[3].back().second;
2003 faces[fIdx][2] = edgeY[1].back().second;
2004 fIdx++;
2005
2006 faces[fIdx][0] = sideXNeg.back().back().second;
2007 faces[fIdx][1] = edgeY[1].back().second;
2008 faces[fIdx][2] = edgeZ[3].back().second;
2009 fIdx++;
2010
2011 faces[fIdx][0] = corners[7].second;
2012 faces[fIdx][1] = edgeX[2][0].second;
2013 faces[fIdx][2] = edgeZ[3].back().second;
2014 fIdx++;
2015
2016 faces[fIdx][0] = sideYPos.back()[0].second;
2017 faces[fIdx][1] = edgeZ[3].back().second;
2018 faces[fIdx][2] = edgeX[2][0].second;
2019 fIdx++;
2020
2021 faces[fIdx][0] = corners[7].second;
2022 faces[fIdx][1] = edgeY[1].back().second;
2023 faces[fIdx][2] = edgeX[2][0].second;
2024 fIdx++;
2025
2026 faces[fIdx][0] = sideZPos[0].back().second;
2027 faces[fIdx][1] = edgeX[2][0].second;
2028 faces[fIdx][2] = edgeY[1].back().second;
2029 fIdx++;
2030
2031 properties.addFace(faces[42]);
2032 properties.addFace(faces[43]);
2033 properties.addFace(faces[44]);
2034 properties.addFace(faces[45]);
2035 properties.addFace(faces[46]);
2036 properties.addFace(faces[47]);
2037
2038 // EdgeX[0]
2039 for (std::size_t i = 0; i < edgeX[0].size() - 1; ++i)
2040 {
2041 faces[fIdx][0] = edgeX[0][i].second;
2042 faces[fIdx][1] = edgeX[0][i + 1].second;
2043 faces[fIdx][2] = sideYNeg[0][i].second;
2044 properties.addFace(faces[fIdx]);
2045 fIdx++;
2046
2047 faces[fIdx][0] = sideYNeg[0][i + 1].second;
2048 faces[fIdx][1] = sideYNeg[0][i].second;
2049 faces[fIdx][2] = edgeX[0][i + 1].second;
2050 properties.addFace(faces[fIdx]);
2051 fIdx++;
2052
2053 faces[fIdx][0] = edgeX[0][i].second;
2054 faces[fIdx][1] = sideZNeg[i][0].second;
2055 faces[fIdx][2] = edgeX[0][i + 1].second;
2056 properties.addFace(faces[fIdx]);
2057 fIdx++;
2058
2059 faces[fIdx][0] = sideZNeg[i + 1][0].second;
2060 faces[fIdx][1] = edgeX[0][i + 1].second;
2061 faces[fIdx][2] = sideZNeg[i][0].second;
2062 properties.addFace(faces[fIdx]);
2063 fIdx++;
2064 }
2065
2066 // EdgeX[1]
2067 for (std::size_t i = 0; i < edgeX[1].size() - 1; ++i)
2068 {
2069 faces[fIdx][0] = edgeX[1][i].second;
2070 faces[fIdx][1] = sideYPos[0][i].second;
2071 faces[fIdx][2] = edgeX[1][i + 1].second;
2072 properties.addFace(faces[fIdx]);
2073 fIdx++;
2074
2075 faces[fIdx][0] = sideYPos[0][i + 1].second;
2076 faces[fIdx][1] = edgeX[1][i + 1].second;
2077 faces[fIdx][2] = sideYPos[0][i].second;
2078 properties.addFace(faces[fIdx]);
2079 fIdx++;
2080
2081 faces[fIdx][0] = edgeX[1][i].second;
2082 faces[fIdx][1] = edgeX[1][i + 1].second;
2083 faces[fIdx][2] = sideZNeg[i].back().second;
2084 properties.addFace(faces[fIdx]);
2085 fIdx++;
2086
2087 faces[fIdx][0] = sideZNeg[i + 1].back().second;
2088 faces[fIdx][1] = sideZNeg[i].back().second;
2089 faces[fIdx][2] = edgeX[1][i + 1].second;
2090 properties.addFace(faces[fIdx]);
2091 fIdx++;
2092 }
2093
2094 // EdgeX[2]
2095 for (std::size_t i = 0; i < edgeX[2].size() - 1; ++i)
2096 {
2097 faces[fIdx][0] = edgeX[2][i + 1].second;
2098 faces[fIdx][1] = sideYPos.back()[i + 1].second;
2099 faces[fIdx][2] = edgeX[2][i].second;
2100 properties.addFace(faces[fIdx]);
2101 fIdx++;
2102
2103 faces[fIdx][0] = sideYPos.back()[i].second;
2104 faces[fIdx][1] = edgeX[2][i].second;
2105 faces[fIdx][2] = sideYPos.back()[i + 1].second;
2106 properties.addFace(faces[fIdx]);
2107 fIdx++;
2108
2109 faces[fIdx][0] = edgeX[2][i + 1].second;
2110 faces[fIdx][1] = edgeX[2][i].second;
2111 faces[fIdx][2] = sideZPos[i + 1].back().second;
2112 properties.addFace(faces[fIdx]);
2113 fIdx++;
2114
2115 faces[fIdx][0] = sideZPos[i].back().second;
2116 faces[fIdx][1] = sideZPos[i + 1].back().second;
2117 faces[fIdx][2] = edgeX[2][i].second;
2118 properties.addFace(faces[fIdx]);
2119 fIdx++;
2120 }
2121
2122 // EdgeX[3]
2123 for (std::size_t i = 0; i < edgeX[3].size() - 1; ++i)
2124 {
2125 faces[fIdx][0] = edgeX[3][i].second;
2126 faces[fIdx][1] = sideYNeg.back()[i].second;
2127 faces[fIdx][2] = edgeX[3][i + 1].second;
2128 properties.addFace(faces[fIdx]);
2129 fIdx++;
2130
2131 faces[fIdx][0] = sideYNeg.back()[i + 1].second;
2132 faces[fIdx][1] = edgeX[3][i + 1].second;
2133 faces[fIdx][2] = sideYNeg.back()[i].second;
2134 properties.addFace(faces[fIdx]);
2135 fIdx++;
2136
2137 faces[fIdx][0] = edgeX[3][i].second;
2138 faces[fIdx][1] = edgeX[3][i + 1].second;
2139 faces[fIdx][2] = sideZPos[i][0].second;
2140 properties.addFace(faces[fIdx]);
2141 fIdx++;
2142
2143 faces[fIdx][0] = sideZPos[i + 1][0].second;
2144 faces[fIdx][1] = sideZPos[i][0].second;
2145 faces[fIdx][2] = edgeX[3][i + 1].second;
2146 properties.addFace(faces[fIdx]);
2147 fIdx++;
2148 }
2149
2150 // edgeY[0]
2151 for (std::size_t i = 0; i < edgeY[0].size() - 1; ++i)
2152 {
2153 faces[fIdx][0] = edgeY[0][i + 1].second;
2154 faces[fIdx][1] = sideZNeg[0][i + 1].second;
2155 faces[fIdx][2] = edgeY[0][i].second;
2156 properties.addFace(faces[fIdx]);
2157 fIdx++;
2158
2159 faces[fIdx][0] = sideZNeg[0][i].second;
2160 faces[fIdx][1] = edgeY[0][i].second;
2161 faces[fIdx][2] = sideZNeg[0][i + 1].second;
2162 properties.addFace(faces[fIdx]);
2163 fIdx++;
2164
2165 faces[fIdx][0] = edgeY[0][i].second;
2166 faces[fIdx][1] = sideXNeg[i][0].second;
2167 faces[fIdx][2] = edgeY[0][i + 1].second;
2168 properties.addFace(faces[fIdx]);
2169 fIdx++;
2170
2171 faces[fIdx][0] = sideXNeg[i + 1][0].second;
2172 faces[fIdx][1] = edgeY[0][i + 1].second;
2173 faces[fIdx][2] = sideXNeg[i][0].second;
2174 properties.addFace(faces[fIdx]);
2175 fIdx++;
2176 }
2177
2178 // edgeY[1]
2179 for (std::size_t i = 0; i < edgeY[1].size() - 1; ++i)
2180 {
2181 faces[fIdx][0] = edgeY[1][i].second;
2182 faces[fIdx][1] = sideZPos[0][i].second;
2183 faces[fIdx][2] = edgeY[1][i + 1].second;
2184 properties.addFace(faces[fIdx]);
2185 fIdx++;
2186
2187 faces[fIdx][0] = sideZPos[0][i + 1].second;
2188 faces[fIdx][1] = edgeY[1][i + 1].second;
2189 faces[fIdx][2] = sideZPos[0][i].second;
2190 properties.addFace(faces[fIdx]);
2191 fIdx++;
2192
2193 faces[fIdx][0] = edgeY[1][i].second;
2194 faces[fIdx][1] = edgeY[1][i + 1].second;
2195 faces[fIdx][2] = sideXNeg[i].back().second;
2196 properties.addFace(faces[fIdx]);
2197 fIdx++;
2198
2199 faces[fIdx][0] = sideXNeg[i + 1].back().second;
2200 faces[fIdx][1] = sideXNeg[i].back().second;
2201 faces[fIdx][2] = edgeY[1][i + 1].second;
2202 properties.addFace(faces[fIdx]);
2203 fIdx++;
2204 }
2205
2206 // edgeY[2]
2207 for (std::size_t i = 0; i < edgeY[2].size() - 1; ++i)
2208 {
2209 faces[fIdx][0] = edgeY[2][i + 1].second;
2210 faces[fIdx][1] = sideZPos.back()[i + 1].second;
2211 faces[fIdx][2] = edgeY[2][i].second;
2212 properties.addFace(faces[fIdx]);
2213 fIdx++;
2214
2215 faces[fIdx][0] = sideZPos.back()[i].second;
2216 faces[fIdx][1] = edgeY[2][i].second;
2217 faces[fIdx][2] = sideZPos.back()[i + 1].second;
2218 properties.addFace(faces[fIdx]);
2219 fIdx++;
2220
2221 faces[fIdx][0] = edgeY[2][i + 1].second;
2222 faces[fIdx][1] = edgeY[2][i].second;
2223 faces[fIdx][2] = sideXPos[i + 1].back().second;
2224 properties.addFace(faces[fIdx]);
2225 fIdx++;
2226
2227 faces[fIdx][0] = sideXPos[i].back().second;
2228 faces[fIdx][1] = sideXPos[i + 1].back().second;
2229 faces[fIdx][2] = edgeY[2][i].second;
2230 properties.addFace(faces[fIdx]);
2231 fIdx++;
2232 }
2233
2234 // edgeY[3]
2235 for (std::size_t i = 0; i < edgeY[3].size() - 1; ++i)
2236 {
2237 faces[fIdx][0] = edgeY[3][i].second;
2238 faces[fIdx][1] = sideZNeg.back()[i].second;
2239 faces[fIdx][2] = edgeY[3][i + 1].second;
2240 properties.addFace(faces[fIdx]);
2241 fIdx++;
2242
2243 faces[fIdx][0] = sideZNeg.back()[i + 1].second;
2244 faces[fIdx][1] = edgeY[3][i + 1].second;
2245 faces[fIdx][2] = sideZNeg.back()[i].second;
2246 properties.addFace(faces[fIdx]);
2247 fIdx++;
2248
2249 faces[fIdx][0] = edgeY[3][i].second;
2250 faces[fIdx][1] = edgeY[3][i + 1].second;
2251 faces[fIdx][2] = sideXPos[i][0].second;
2252 properties.addFace(faces[fIdx]);
2253 fIdx++;
2254
2255 faces[fIdx][0] = sideXPos[i + 1][0].second;
2256 faces[fIdx][1] = sideXPos[i][0].second;
2257 faces[fIdx][2] = edgeY[3][i + 1].second;
2258 properties.addFace(faces[fIdx]);
2259 fIdx++;
2260 }
2261
2262 // edgeZ[0]
2263 for (std::size_t i = 0; i < edgeZ[0].size() - 1; ++i)
2264 {
2265 faces[fIdx][0] = edgeZ[0][i + 1].second;
2266 faces[fIdx][1] = sideXNeg[0][i + 1].second;
2267 faces[fIdx][2] = edgeZ[0][i].second;
2268 properties.addFace(faces[fIdx]);
2269 fIdx++;
2270
2271 faces[fIdx][0] = sideXNeg[0][i].second;
2272 faces[fIdx][1] = edgeZ[0][i].second;
2273 faces[fIdx][2] = sideXNeg[0][i + 1].second;
2274 properties.addFace(faces[fIdx]);
2275 fIdx++;
2276
2277 faces[fIdx][0] = edgeZ[0][i].second;
2278 faces[fIdx][1] = sideYNeg[i][0].second;
2279 faces[fIdx][2] = edgeZ[0][i + 1].second;
2280 properties.addFace(faces[fIdx]);
2281 fIdx++;
2282
2283 faces[fIdx][0] = sideYNeg[i + 1][0].second;
2284 faces[fIdx][1] = edgeZ[0][i + 1].second;
2285 faces[fIdx][2] = sideYNeg[i][0].second;
2286 properties.addFace(faces[fIdx]);
2287 fIdx++;
2288 }
2289
2290 // edgeZ[1]
2291 for (std::size_t i = 0; i < edgeZ[1].size() - 1; ++i)
2292 {
2293 faces[fIdx][0] = edgeZ[1][i].second;
2294 faces[fIdx][1] = sideXPos[0][i].second;
2295 faces[fIdx][2] = edgeZ[1][i + 1].second;
2296 properties.addFace(faces[fIdx]);
2297 fIdx++;
2298
2299 faces[fIdx][0] = sideXPos[0][i + 1].second;
2300 faces[fIdx][1] = edgeZ[1][i + 1].second;
2301 faces[fIdx][2] = sideXPos[0][i].second;
2302 properties.addFace(faces[fIdx]);
2303 fIdx++;
2304
2305 faces[fIdx][0] = edgeZ[1][i].second;
2306 faces[fIdx][1] = edgeZ[1][i + 1].second;
2307 faces[fIdx][2] = sideYNeg[i].back().second;
2308 properties.addFace(faces[fIdx]);
2309 fIdx++;
2310
2311 faces[fIdx][0] = sideYNeg[i + 1].back().second;
2312 faces[fIdx][1] = sideYNeg[i].back().second;
2313 faces[fIdx][2] = edgeZ[1][i + 1].second;
2314 properties.addFace(faces[fIdx]);
2315 fIdx++;
2316 }
2317
2318 // edgeZ[2]
2319 for (std::size_t i = 0; i < edgeZ[2].size() - 1; ++i)
2320 {
2321 faces[fIdx][0] = edgeZ[2][i + 1].second;
2322 faces[fIdx][1] = sideXPos.back()[i + 1].second;
2323 faces[fIdx][2] = edgeZ[2][i].second;
2324 properties.addFace(faces[fIdx]);
2325 fIdx++;
2326
2327 faces[fIdx][0] = sideXPos.back()[i].second;
2328 faces[fIdx][1] = edgeZ[2][i].second;
2329 faces[fIdx][2] = sideXPos.back()[i + 1].second;
2330 properties.addFace(faces[fIdx]);
2331 fIdx++;
2332
2333 faces[fIdx][0] = edgeZ[2][i + 1].second;
2334 faces[fIdx][1] = edgeZ[2][i].second;
2335 faces[fIdx][2] = sideYPos[i + 1].back().second;
2336 properties.addFace(faces[fIdx]);
2337 fIdx++;
2338
2339 faces[fIdx][0] = sideYPos[i].back().second;
2340 faces[fIdx][1] = sideYPos[i + 1].back().second;
2341 faces[fIdx][2] = edgeZ[2][i].second;
2342 properties.addFace(faces[fIdx]);
2343 fIdx++;
2344 }
2345
2346 // edgeZ[3]
2347 for (std::size_t i = 0; i < edgeZ[3].size() - 1; ++i)
2348 {
2349 faces[fIdx][0] = edgeZ[3][i].second;
2350 faces[fIdx][1] = sideXNeg.back()[i].second;
2351 faces[fIdx][2] = edgeZ[3][i + 1].second;
2352 properties.addFace(faces[fIdx]);
2353 fIdx++;
2354
2355 faces[fIdx][0] = sideXNeg.back()[i + 1].second;
2356 faces[fIdx][1] = edgeZ[3][i + 1].second;
2357 faces[fIdx][2] = sideXNeg.back()[i].second;
2358 properties.addFace(faces[fIdx]);
2359 fIdx++;
2360
2361 faces[fIdx][0] = edgeZ[3][i].second;
2362 faces[fIdx][1] = edgeZ[3][i + 1].second;
2363 faces[fIdx][2] = sideYPos[i][0].second;
2364 properties.addFace(faces[fIdx]);
2365 fIdx++;
2366
2367 faces[fIdx][0] = sideYPos[i + 1][0].second;
2368 faces[fIdx][1] = sideYPos[i][0].second;
2369 faces[fIdx][2] = edgeZ[3][i + 1].second;
2370 properties.addFace(faces[fIdx]);
2371 fIdx++;
2372 }
2373
2374 // -X side
2375 for (std::size_t i = 0; i < sideXNeg.size() - 1; ++i)
2376 {
2377 for (std::size_t j = 0; j < sideXNeg[i].size() - 1; ++j)
2378 {
2379 faces[fIdx][0] = sideXNeg[i + 0][j + 0].second;
2380 faces[fIdx][1] = sideXNeg[i + 0][j + 1].second;
2381 faces[fIdx][2] = sideXNeg[i + 1][j + 0].second;
2382 properties.addFace(faces[fIdx]);
2383 fIdx++;
2384
2385 faces[fIdx][0] = sideXNeg[i + 1][j + 1].second;
2386 faces[fIdx][1] = sideXNeg[i + 1][j + 0].second;
2387 faces[fIdx][2] = sideXNeg[i + 0][j + 1].second;
2388 properties.addFace(faces[fIdx]);
2389 fIdx++;
2390 }
2391 }
2392
2393 // +X side
2394 for (std::size_t i = 0; i < sideXPos.size() - 1; ++i)
2395 {
2396 for (std::size_t j = 0; j < sideXPos[i].size() - 1; ++j)
2397 {
2398 faces[fIdx][0] = sideXPos[i + 0][j + 0].second;
2399 faces[fIdx][1] = sideXPos[i + 1][j + 0].second;
2400 faces[fIdx][2] = sideXPos[i + 0][j + 1].second;
2401 properties.addFace(faces[fIdx]);
2402 fIdx++;
2403
2404 faces[fIdx][0] = sideXPos[i + 1][j + 1].second;
2405 faces[fIdx][1] = sideXPos[i + 0][j + 1].second;
2406 faces[fIdx][2] = sideXPos[i + 1][j + 0].second;
2407 properties.addFace(faces[fIdx]);
2408 fIdx++;
2409 }
2410 }
2411
2412 // -Y side
2413 for (std::size_t i = 0; i < sideYNeg.size() - 1; ++i)
2414 {
2415 for (std::size_t j = 0; j < sideYNeg[i].size() - 1; ++j)
2416 {
2417 faces[fIdx][0] = sideYNeg[i + 0][j + 0].second;
2418 faces[fIdx][1] = sideYNeg[i + 0][j + 1].second;
2419 faces[fIdx][2] = sideYNeg[i + 1][j + 0].second;
2420 properties.addFace(faces[fIdx]);
2421 fIdx++;
2422
2423 faces[fIdx][0] = sideYNeg[i + 1][j + 1].second;
2424 faces[fIdx][1] = sideYNeg[i + 1][j + 0].second;
2425 faces[fIdx][2] = sideYNeg[i + 0][j + 1].second;
2426 properties.addFace(faces[fIdx]);
2427 fIdx++;
2428 }
2429 }
2430
2431 // +Y side
2432 for (std::size_t i = 0; i < sideYPos.size() - 1; ++i)
2433 {
2434 for (std::size_t j = 0; j < sideYPos[i].size() - 1; ++j)
2435 {
2436 faces[fIdx][0] = sideYPos[i + 0][j + 0].second;
2437 faces[fIdx][1] = sideYPos[i + 1][j + 0].second;
2438 faces[fIdx][2] = sideYPos[i + 0][j + 1].second;
2439 properties.addFace(faces[fIdx]);
2440 fIdx++;
2441
2442 faces[fIdx][0] = sideYPos[i + 1][j + 1].second;
2443 faces[fIdx][1] = sideYPos[i + 0][j + 1].second;
2444 faces[fIdx][2] = sideYPos[i + 1][j + 0].second;
2445 properties.addFace(faces[fIdx]);
2446 fIdx++;
2447 }
2448 }
2449
2450 // -Z side
2451 for (std::size_t i = 0; i < sideZNeg.size() - 1; ++i)
2452 {
2453 for (std::size_t j = 0; j < sideZNeg[i].size() - 1; ++j)
2454 {
2455 faces[fIdx][0] = sideZNeg[i + 0][j + 0].second;
2456 faces[fIdx][1] = sideZNeg[i + 0][j + 1].second;
2457 faces[fIdx][2] = sideZNeg[i + 1][j + 0].second;
2458 properties.addFace(faces[fIdx]);
2459 fIdx++;
2460
2461 faces[fIdx][0] = sideZNeg[i + 1][j + 1].second;
2462 faces[fIdx][1] = sideZNeg[i + 1][j + 0].second;
2463 faces[fIdx][2] = sideZNeg[i + 0][j + 1].second;
2464 properties.addFace(faces[fIdx]);
2465 fIdx++;
2466 }
2467 }
2468
2469 // +Z side
2470 for (std::size_t i = 0; i < sideZPos.size() - 1; ++i)
2471 {
2472 for (std::size_t j = 0; j < sideZPos[i].size() - 1; ++j)
2473 {
2474 faces[fIdx][0] = sideZPos[i + 0][j + 0].second;
2475 faces[fIdx][1] = sideZPos[i + 1][j + 0].second;
2476 faces[fIdx][2] = sideZPos[i + 0][j + 1].second;
2477 properties.addFace(faces[fIdx]);
2478 fIdx++;
2479
2480 faces[fIdx][0] = sideZPos[i + 1][j + 1].second;
2481 faces[fIdx][1] = sideZPos[i + 0][j + 1].second;
2482 faces[fIdx][2] = sideZPos[i + 1][j + 0].second;
2483 properties.addFace(faces[fIdx]);
2484 fIdx++;
2485 }
2486 }
2487
2488 return properties;
2489 }
2490
2491 //==============================================================================
setBox(SoftBodyNode * _softBodyNode,const Eigen::Vector3d & _size,const Eigen::Isometry3d & _localTransform,const Eigen::Vector3i & _frags,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2492 void SoftBodyNodeHelper::setBox(
2493 SoftBodyNode* _softBodyNode,
2494 const Eigen::Vector3d& _size,
2495 const Eigen::Isometry3d& _localTransform,
2496 const Eigen::Vector3i& _frags,
2497 double _totalMass,
2498 double _vertexStiffness,
2499 double _edgeStiffness,
2500 double _dampingCoeff)
2501 {
2502 assert(_softBodyNode != nullptr);
2503 _softBodyNode->setProperties(makeBoxProperties(
2504 _size,
2505 _localTransform,
2506 _frags,
2507 _totalMass,
2508 _vertexStiffness,
2509 _edgeStiffness,
2510 _dampingCoeff));
2511 }
2512
2513 //==============================================================================
2514 SoftBodyNode::UniqueProperties
makeSinglePointMassProperties(double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2515 SoftBodyNodeHelper::makeSinglePointMassProperties(
2516 double _totalMass,
2517 double _vertexStiffness,
2518 double _edgeStiffness,
2519 double _dampingCoeff)
2520 {
2521 SoftBodyNode::UniqueProperties properties(
2522 _vertexStiffness, _edgeStiffness, _dampingCoeff);
2523
2524 //----------------------------------------------------------------------------
2525 // Point masses
2526 //----------------------------------------------------------------------------
2527 // Number of point masses
2528 std::size_t nPointMasses = 1;
2529
2530 // Mass per vertices
2531 double mass = _totalMass / nPointMasses;
2532
2533 // Resting positions for each point mass
2534 std::vector<Eigen::Vector3d> restingPos(
2535 nPointMasses, Eigen::Vector3d::Zero());
2536 restingPos[0] = Eigen::Vector3d(+0.1, +0.1, +0.1);
2537
2538 // Point masses
2539 for (std::size_t i = 0; i < nPointMasses; ++i)
2540 {
2541 PointMass::Properties point(restingPos[i], mass);
2542 properties.addPointMass(point);
2543 }
2544
2545 return properties;
2546 }
2547
2548 //==============================================================================
setSinglePointMass(SoftBodyNode * _softBodyNode,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2549 void SoftBodyNodeHelper::setSinglePointMass(
2550 SoftBodyNode* _softBodyNode,
2551 double _totalMass,
2552 double _vertexStiffness,
2553 double _edgeStiffness,
2554 double _dampingCoeff)
2555 {
2556 assert(_softBodyNode != nullptr);
2557 _softBodyNode->setProperties(makeSinglePointMassProperties(
2558 _totalMass, _vertexStiffness, _edgeStiffness, _dampingCoeff));
2559 }
2560
2561 //==============================================================================
makeSphereProperties(double _radius,std::size_t _nSlices,std::size_t _nStacks,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2562 SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeSphereProperties(
2563 double _radius,
2564 std::size_t _nSlices,
2565 std::size_t _nStacks,
2566 double _totalMass,
2567 double _vertexStiffness,
2568 double _edgeStiffness,
2569 double _dampingCoeff)
2570 {
2571 return makeEllipsoidProperties(
2572 Eigen::Vector3d::Constant(_radius * 2.0),
2573 _nSlices,
2574 _nStacks,
2575 _totalMass,
2576 _vertexStiffness,
2577 _edgeStiffness,
2578 _dampingCoeff);
2579 }
2580
2581 //==============================================================================
makeEllipsoidProperties(const Eigen::Vector3d & _size,std::size_t _nSlices,std::size_t _nStacks,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2582 SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeEllipsoidProperties(
2583 const Eigen::Vector3d& _size,
2584 std::size_t _nSlices,
2585 std::size_t _nStacks,
2586 double _totalMass,
2587 double _vertexStiffness,
2588 double _edgeStiffness,
2589 double _dampingCoeff)
2590 {
2591 using namespace dart::math::suffixes;
2592
2593 SoftBodyNode::UniqueProperties properties(
2594 _vertexStiffness, _edgeStiffness, _dampingCoeff);
2595
2596 //----------------------------------------------------------------------------
2597 // Point masses
2598 //----------------------------------------------------------------------------
2599 // Number of point masses
2600 int nPointMasses = (_nStacks - 1) * _nSlices + 2;
2601
2602 // Mass per vertices
2603 double mass = _totalMass / nPointMasses;
2604
2605 // Resting positions for each point mass
2606 // -- top
2607 properties.addPointMass(
2608 PointMass::Properties(Eigen::Vector3d(0.0, 0.0, 0.5 * _size(2)), mass));
2609
2610 // middle
2611 float drho = 1_pi / _nStacks;
2612 float dtheta = 2_pi / _nSlices;
2613 for (std::size_t i = 1; i < _nStacks; i++)
2614 {
2615 float rho = i * drho;
2616 float srho = (sin(rho));
2617 float crho = (cos(rho));
2618
2619 for (std::size_t j = 0; j < _nSlices; j++)
2620 {
2621 float theta = (j == _nSlices) ? 0.0f : j * dtheta;
2622 float stheta = (-sin(theta));
2623 float ctheta = (cos(theta));
2624
2625 float x = 0.5 * srho * stheta;
2626 float y = 0.5 * srho * ctheta;
2627 float z = 0.5 * crho;
2628
2629 properties.addPointMass(PointMass::Properties(
2630 Eigen::Vector3d(x * _size(0), y * _size(1), z * _size(2)), mass));
2631 }
2632 }
2633 // bottom
2634 properties.addPointMass(
2635 PointMass::Properties(Eigen::Vector3d(0.0, 0.0, -0.5 * _size(2)), mass));
2636
2637 //----------------------------------------------------------------------------
2638 // Edges
2639 //----------------------------------------------------------------------------
2640 // a) longitudinal
2641 // -- top
2642 for (std::size_t i = 0; i < _nSlices; i++)
2643 properties.connectPointMasses(0, i + 1);
2644 // -- middle
2645 for (std::size_t i = 0; i < _nStacks - 2; i++)
2646 for (std::size_t j = 0; j < _nSlices; j++)
2647 properties.connectPointMasses(
2648 i * _nSlices + j + 1, (i + 1) * _nSlices + j + 1);
2649 // -- bottom
2650 for (std::size_t i = 0; i < _nSlices; i++)
2651 properties.connectPointMasses(
2652 (_nStacks - 1) * _nSlices + 1, (_nStacks - 2) * _nSlices + i + 1);
2653
2654 // b) latitudinal
2655 for (std::size_t i = 0; i < _nStacks - 1; i++)
2656 {
2657 for (std::size_t j = 0; j < _nSlices - 1; j++)
2658 {
2659 properties.connectPointMasses(i * _nSlices + j + 1, i * _nSlices + j + 2);
2660 }
2661 properties.connectPointMasses((i + 1) * _nSlices, i * _nSlices + 1);
2662 }
2663
2664 // c) cross (shear)
2665 for (std::size_t i = 0; i < _nStacks - 2; i++)
2666 {
2667 for (std::size_t j = 0; j < _nSlices - 1; j++)
2668 {
2669 properties.connectPointMasses(
2670 i * _nSlices + j + 1, (i + 1) * _nSlices + j + 2);
2671 properties.connectPointMasses(
2672 i * _nSlices + j + 2, (i + 1) * _nSlices + j + 1);
2673 }
2674 properties.connectPointMasses((i + 1) * _nSlices, (i + 1) * _nSlices + 1);
2675 properties.connectPointMasses(i * _nSlices + 1, (i + 2) * _nSlices);
2676 }
2677
2678 //----------------------------------------------------------------------------
2679 // Faces
2680 //----------------------------------------------------------------------------
2681 int meshIdx1 = 0;
2682 int meshIdx2 = 0;
2683 int meshIdx3 = 0;
2684
2685 // top
2686 meshIdx1 = 0;
2687 for (std::size_t i = 0; i < _nSlices - 1; i++)
2688 {
2689 meshIdx2 = i + 1;
2690 meshIdx3 = i + 2;
2691 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2692 }
2693 meshIdx2 = _nSlices;
2694 meshIdx3 = 1;
2695 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2696
2697 // middle
2698 for (std::size_t i = 0; i < _nStacks - 2; i++)
2699 {
2700 for (std::size_t j = 0; j < _nSlices - 1; j++)
2701 {
2702 meshIdx1 = i * _nSlices + j + 1;
2703 meshIdx2 = (i + 1) * _nSlices + j + 1;
2704 meshIdx3 = i * _nSlices + j + 2;
2705 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2706
2707 meshIdx1 = i * _nSlices + j + 2;
2708 meshIdx2 = (i + 1) * _nSlices + j + 1;
2709 meshIdx3 = (i + 1) * _nSlices + j + 2;
2710 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2711 }
2712
2713 meshIdx1 = (i + 1) * _nSlices;
2714 meshIdx2 = (i + 2) * _nSlices;
2715 meshIdx3 = i * _nSlices + 1;
2716 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2717
2718 meshIdx1 = i * _nSlices + 1;
2719 meshIdx2 = (i + 2) * _nSlices;
2720 meshIdx3 = (i + 2) * _nSlices + 1;
2721 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2722 }
2723
2724 // bottom
2725 meshIdx1 = (_nStacks - 1) * _nSlices + 1;
2726 for (std::size_t i = 0; i < _nSlices - 1; i++)
2727 {
2728 meshIdx2 = (_nStacks - 2) * _nSlices + i + 2;
2729 meshIdx3 = (_nStacks - 2) * _nSlices + i + 1;
2730 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2731 }
2732 meshIdx2 = (_nStacks - 2) * _nSlices + 2;
2733 meshIdx3 = (_nStacks - 1) * _nSlices;
2734 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2735
2736 return properties;
2737 }
2738
2739 //==============================================================================
setEllipsoid(SoftBodyNode * _softBodyNode,const Eigen::Vector3d & _size,std::size_t _nSlices,std::size_t _nStacks,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2740 void SoftBodyNodeHelper::setEllipsoid(
2741 SoftBodyNode* _softBodyNode,
2742 const Eigen::Vector3d& _size,
2743 std::size_t _nSlices,
2744 std::size_t _nStacks,
2745 double _totalMass,
2746 double _vertexStiffness,
2747 double _edgeStiffness,
2748 double _dampingCoeff)
2749 {
2750 assert(_softBodyNode != nullptr);
2751 _softBodyNode->setProperties(makeEllipsoidProperties(
2752 _size,
2753 _nSlices,
2754 _nStacks,
2755 _totalMass,
2756 _vertexStiffness,
2757 _edgeStiffness,
2758 _dampingCoeff));
2759 }
2760
2761 //==============================================================================
makeCylinderProperties(double _radius,double _height,std::size_t _nSlices,std::size_t _nStacks,std::size_t _nRings,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)2762 SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeCylinderProperties(
2763 double _radius,
2764 double _height,
2765 std::size_t _nSlices,
2766 std::size_t _nStacks,
2767 std::size_t _nRings,
2768 double _totalMass,
2769 double _vertexStiffness,
2770 double _edgeStiffness,
2771 double _dampingCoeff)
2772 {
2773 using namespace dart::math::suffixes;
2774
2775 SoftBodyNode::UniqueProperties properties(
2776 _vertexStiffness, _edgeStiffness, _dampingCoeff);
2777
2778 //----------------------------------------------------------------------------
2779 // Point masses
2780 //----------------------------------------------------------------------------
2781 // Number of point masses
2782 std::size_t nTopPointMasses = _nSlices * (_nRings - 1) + 1;
2783 std::size_t nDrumPointMasses = (_nStacks + 1) * _nSlices;
2784 std::size_t nTotalMasses = nDrumPointMasses + 2 * nTopPointMasses;
2785
2786 // Mass per vertices
2787 double mass = _totalMass / nTotalMasses;
2788
2789 // Resting positions for each point mass
2790 float dradius = _radius / static_cast<float>(_nRings);
2791 float dtheta = 2_pi / static_cast<float>(_nSlices);
2792
2793 // -- top
2794 properties.addPointMass(
2795 PointMass::Properties(Eigen::Vector3d(0.0, 0.0, 0.5 * _height), mass));
2796
2797 for (std::size_t i = 1; i < _nRings; ++i)
2798 {
2799 float z = 0.5;
2800 float radius = i * dradius;
2801
2802 for (std::size_t j = 0; j < _nSlices; j++)
2803 {
2804 float theta = (j == _nSlices) ? 0.0f : j * dtheta;
2805 float stheta = (-sin(theta));
2806 float ctheta = (cos(theta));
2807
2808 float x = stheta;
2809 float y = ctheta;
2810
2811 properties.addPointMass(PointMass::Properties(
2812 Eigen::Vector3d(x * radius, y * radius, z * _height), mass));
2813 }
2814 }
2815
2816 // -- middle
2817 float dz = -1.0 / static_cast<float>(_nStacks);
2818 for (std::size_t i = 0; i < _nStacks + 1; i++)
2819 {
2820 float z = 0.5 + i * dz;
2821
2822 for (std::size_t j = 0; j < _nSlices; j++)
2823 {
2824 float theta = (j == _nSlices) ? 0.0f : j * dtheta;
2825 float stheta = (-sin(theta));
2826 float ctheta = (cos(theta));
2827
2828 float x = stheta;
2829 float y = ctheta;
2830
2831 properties.addPointMass(PointMass::Properties(
2832 Eigen::Vector3d(x * _radius, y * _radius, z * _height), mass));
2833 }
2834 }
2835
2836 // -- bottom
2837 for (std::size_t i = 1; i < _nRings; ++i)
2838 {
2839 float z = -0.5;
2840 float radius = _radius - i * dradius;
2841
2842 for (std::size_t j = 0; j < _nSlices; j++)
2843 {
2844 float theta = (j == _nSlices) ? 0.0f : j * dtheta;
2845 float stheta = (-sin(theta));
2846 float ctheta = (cos(theta));
2847
2848 float x = stheta;
2849 float y = ctheta;
2850
2851 properties.addPointMass(PointMass::Properties(
2852 Eigen::Vector3d(x * radius, y * radius, z * _height), mass));
2853 }
2854 }
2855
2856 properties.addPointMass(
2857 PointMass::Properties(Eigen::Vector3d(0.0, 0.0, -0.5 * _height), mass));
2858
2859 //----------------------------------------------------------------------------
2860 // Edges
2861 //----------------------------------------------------------------------------
2862 // A. Drum part
2863
2864 // a) longitudinal
2865 // -- top
2866 for (std::size_t i = 0; i < _nSlices; i++)
2867 properties.connectPointMasses(0, i + 1);
2868 for (std::size_t i = 0; i < _nRings - 1; i++)
2869 {
2870 for (std::size_t j = 0; j < _nSlices; j++)
2871 {
2872 properties.connectPointMasses(
2873 _nSlices + 1 + (i + 0) * _nSlices + j,
2874 _nSlices + 1 + (i + 1) * _nSlices + j);
2875 }
2876 }
2877 // -- middle
2878 for (std::size_t i = 0; i < _nStacks - 1; i++)
2879 {
2880 for (std::size_t j = 0; j < _nSlices; j++)
2881 {
2882 properties.connectPointMasses(
2883 nTopPointMasses + (i + 0) * _nSlices + j,
2884 nTopPointMasses + (i + 1) * _nSlices + j);
2885 }
2886 }
2887 // -- bottom
2888 for (std::size_t i = 0; i < _nRings - 1; i++)
2889 {
2890 for (std::size_t j = 0; j < _nSlices; j++)
2891 {
2892 properties.connectPointMasses(
2893 nTopPointMasses + (nDrumPointMasses - _nSlices) + (i + 0) * _nSlices
2894 + j,
2895 nTopPointMasses + (nDrumPointMasses - _nSlices) + (i + 1) * _nSlices
2896 + j);
2897 }
2898 }
2899 for (std::size_t i = 1; i < _nSlices; i++)
2900 properties.connectPointMasses(nTotalMasses - 1 - i, nTotalMasses - 1);
2901
2902 // b) latitudinal
2903 for (std::size_t i = 0; i < _nStacks; i++)
2904 {
2905 for (std::size_t j = 0; j < _nSlices - 1; j++)
2906 {
2907 properties.connectPointMasses(
2908 nTopPointMasses + i * _nSlices + j + 0,
2909 nTopPointMasses + i * _nSlices + j + 1);
2910 }
2911
2912 properties.connectPointMasses(
2913 nTopPointMasses + (i + 0) * _nSlices + _nSlices - 1,
2914 nTopPointMasses + (i + 0) * _nSlices);
2915 }
2916 // -- disk parts
2917 // TODO(JS): No latitudinal connections for top and bottom disks
2918
2919 // c) cross (shear)
2920 // -- drum part
2921 for (std::size_t i = 0; i < _nStacks - 2; i++)
2922 {
2923 for (std::size_t j = 0; j < _nSlices - 1; j++)
2924 {
2925 properties.connectPointMasses(
2926 nTopPointMasses + (i + 0) * _nSlices + j + 0,
2927 nTopPointMasses + (i + 1) * _nSlices + j + 1);
2928 properties.connectPointMasses(
2929 nTopPointMasses + (i + 0) * _nSlices + j + 1,
2930 nTopPointMasses + (i + 1) * _nSlices + j + 0);
2931 }
2932
2933 properties.connectPointMasses(
2934 nTopPointMasses + (i + 0) * _nSlices + _nSlices - 1,
2935 nTopPointMasses + (i + 1) * _nSlices);
2936 properties.connectPointMasses(
2937 nTopPointMasses + (i + 0) * _nSlices,
2938 nTopPointMasses + (i + 1) * _nSlices + _nSlices - 1);
2939 }
2940 // -- disk parts
2941 // TODO(JS): No cross connections for top and bottom disks
2942
2943 //----------------------------------------------------------------------------
2944 // Faces
2945 //----------------------------------------------------------------------------
2946 int meshIdx1 = 0;
2947 int meshIdx2 = 0;
2948 int meshIdx3 = 0;
2949
2950 // top
2951 std::size_t nConePointMass = 1;
2952 meshIdx1 = 0;
2953 for (std::size_t i = 0; i < _nSlices - 1; i++)
2954 {
2955 meshIdx2 = i + 1;
2956 meshIdx3 = i + 2;
2957 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2958 }
2959 meshIdx2 = _nSlices;
2960 meshIdx3 = 1;
2961 properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
2962 for (std::size_t i = 0; i < _nRings - 1; ++i)
2963 {
2964 for (std::size_t j = 0; j < _nSlices - 1; ++j)
2965 {
2966 meshIdx1 = (i + 0) * _nSlices + j;
2967 meshIdx2 = (i + 1) * _nSlices + j;
2968 meshIdx3 = (i + 0) * _nSlices + j + 1;
2969 properties.addFace(Eigen::Vector3i(
2970 nConePointMass + meshIdx1,
2971 nConePointMass + meshIdx2,
2972 nConePointMass + meshIdx3));
2973
2974 meshIdx1 = (i + 0) * _nSlices + j + 1;
2975 meshIdx2 = (i + 1) * _nSlices + j;
2976 meshIdx3 = (i + 1) * _nSlices + j + 1;
2977 properties.addFace(Eigen::Vector3i(
2978 nConePointMass + meshIdx1,
2979 nConePointMass + meshIdx2,
2980 nConePointMass + meshIdx3));
2981 }
2982
2983 meshIdx1 = (i + 0) * _nSlices + _nSlices - 1;
2984 meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
2985 meshIdx3 = (i + 0) * _nSlices + 0;
2986 properties.addFace(Eigen::Vector3i(
2987 nConePointMass + meshIdx1,
2988 nConePointMass + meshIdx2,
2989 nConePointMass + meshIdx3));
2990
2991 meshIdx1 = (i + 0) * _nSlices + 0;
2992 meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
2993 meshIdx3 = (i + 1) * _nSlices + 0;
2994 properties.addFace(Eigen::Vector3i(
2995 nConePointMass + meshIdx1,
2996 nConePointMass + meshIdx2,
2997 nConePointMass + meshIdx3));
2998 }
2999
3000 // middle
3001 for (std::size_t i = 0; i < _nStacks; i++)
3002 {
3003 for (std::size_t j = 0; j < _nSlices - 1; j++)
3004 {
3005 meshIdx1 = (i + 0) * _nSlices + j;
3006 meshIdx2 = (i + 1) * _nSlices + j;
3007 meshIdx3 = (i + 0) * _nSlices + j + 1;
3008 properties.addFace(Eigen::Vector3i(
3009 nTopPointMasses + meshIdx1,
3010 nTopPointMasses + meshIdx2,
3011 nTopPointMasses + meshIdx3));
3012
3013 meshIdx1 = (i + 0) * _nSlices + j + 1;
3014 meshIdx2 = (i + 1) * _nSlices + j;
3015 meshIdx3 = (i + 1) * _nSlices + j + 1;
3016 properties.addFace(Eigen::Vector3i(
3017 nTopPointMasses + meshIdx1,
3018 nTopPointMasses + meshIdx2,
3019 nTopPointMasses + meshIdx3));
3020 }
3021
3022 meshIdx1 = (i + 0) * _nSlices;
3023 meshIdx2 = (i + 0) * _nSlices + _nSlices - 1;
3024 meshIdx3 = (i + 1) * _nSlices;
3025 properties.addFace(Eigen::Vector3i(
3026 nTopPointMasses + meshIdx1,
3027 nTopPointMasses + meshIdx2,
3028 nTopPointMasses + meshIdx3));
3029
3030 meshIdx1 = (i + 0) * _nSlices + _nSlices - 1;
3031 meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
3032 meshIdx3 = (i + 1) * _nSlices + 0;
3033 properties.addFace(Eigen::Vector3i(
3034 nTopPointMasses + meshIdx1,
3035 nTopPointMasses + meshIdx2,
3036 nTopPointMasses + meshIdx3));
3037 }
3038
3039 // bottom
3040 for (std::size_t i = 0; i < _nRings - 1; ++i)
3041 {
3042 for (std::size_t j = 0; j < _nSlices - 1; ++j)
3043 {
3044 meshIdx1 = (i + 0) * _nSlices + j;
3045 meshIdx2 = (i + 1) * _nSlices + j;
3046 meshIdx3 = (i + 0) * _nSlices + j + 1;
3047 properties.addFace(Eigen::Vector3i(
3048 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
3049 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
3050 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
3051
3052 meshIdx1 = (i + 0) * _nSlices + j + 1;
3053 meshIdx2 = (i + 1) * _nSlices + j;
3054 meshIdx3 = (i + 1) * _nSlices + j + 1;
3055 properties.addFace(Eigen::Vector3i(
3056 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
3057 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
3058 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
3059 }
3060
3061 meshIdx1 = (i + 0) * _nSlices + _nSlices - 1;
3062 meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
3063 meshIdx3 = (i + 0) * _nSlices + 0;
3064 properties.addFace(Eigen::Vector3i(
3065 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
3066 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
3067 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
3068
3069 meshIdx1 = (i + 0) * _nSlices + 0;
3070 meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
3071 meshIdx3 = (i + 1) * _nSlices + 0;
3072 properties.addFace(Eigen::Vector3i(
3073 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
3074 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
3075 nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
3076 }
3077 meshIdx1 = 1;
3078 for (std::size_t i = 0; i < _nSlices - 1; i++)
3079 {
3080 meshIdx2 = i + 2;
3081 meshIdx3 = i + 3;
3082 properties.addFace(Eigen::Vector3i(
3083 nTotalMasses - meshIdx1,
3084 nTotalMasses - meshIdx2,
3085 nTotalMasses - meshIdx3));
3086 }
3087 meshIdx2 = _nSlices + 1;
3088 meshIdx3 = 2;
3089 properties.addFace(Eigen::Vector3i(
3090 nTotalMasses - meshIdx1,
3091 nTotalMasses - meshIdx2,
3092 nTotalMasses - meshIdx3));
3093
3094 return properties;
3095 }
3096
3097 //==============================================================================
setCylinder(SoftBodyNode * _softBodyNode,double _radius,double _height,std::size_t _nSlices,std::size_t _nStacks,std::size_t _nRings,double _totalMass,double _vertexStiffness,double _edgeStiffness,double _dampingCoeff)3098 void SoftBodyNodeHelper::setCylinder(
3099 SoftBodyNode* _softBodyNode,
3100 double _radius,
3101 double _height,
3102 std::size_t _nSlices,
3103 std::size_t _nStacks,
3104 std::size_t _nRings,
3105 double _totalMass,
3106 double _vertexStiffness,
3107 double _edgeStiffness,
3108 double _dampingCoeff)
3109 {
3110 assert(_softBodyNode != nullptr);
3111 _softBodyNode->setProperties(makeCylinderProperties(
3112 _radius,
3113 _height,
3114 _nSlices,
3115 _nStacks,
3116 _nRings,
3117 _totalMass,
3118 _vertexStiffness,
3119 _edgeStiffness,
3120 _dampingCoeff));
3121 }
3122
3123 } // namespace dynamics
3124 } // namespace dart
3125