step/stepcore/softbody.cc

0001 /*
0002     SPDX-FileCopyrightText: 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
0003
0004     SPDX-License-Identifier: GPL-2.0-or-later
0005 */
0006
0007 #include "softbody.h"
0008 #include "types.h"
0009 #include <algorithm>
0010 #include <cstdlib>
0011 #include <QtGlobal>
0012
0013 // XXX
0014 #include <QStringList>
0015
0016 namespace StepCore
0017 {
0018
0019 STEPCORE_META_OBJECT(SoftBodyParticle, QT_TRANSLATE_NOOP("ObjectClass", "SoftBodyParticle"), QT_TRANSLATE_NOOP("ObjectDescription", "SoftBody particle"), 0, STEPCORE_SUPER_CLASS(Particle),)
0020 STEPCORE_META_OBJECT(SoftBodySpring, QT_TRANSLATE_NOOP("ObjectClass", "SoftBodySpring"), QT_TRANSLATE_NOOP("ObjectDescription", "SoftBody spring"), 0, STEPCORE_SUPER_CLASS(Spring),)
0021 STEPCORE_META_OBJECT(SoftBody, QT_TRANSLATE_NOOP("ObjectClass", "SoftBody"), QT_TRANSLATE_NOOP("ObjectDescription", "Deformable SoftBody"), 0, STEPCORE_SUPER_CLASS(ItemGroup),
0022         STEPCORE_PROPERTY_RW(bool, showInternalItems, QT_TRANSLATE_NOOP("PropertyName", "showInternalItems"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Show internal items"),
0023                                             showInternalItems, setShowInternalItems)
0024         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position of the center of mass"), position, setPosition)
0025
0026         STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocity, QT_TRANSLATE_NOOP("PropertyName", "velocity"), QT_TRANSLATE_NOOP("Units", "m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "Velocity of the center of mass"), velocity, setVelocity)
0027         STEPCORE_PROPERTY_RW_D(double, angularVelocity, QT_TRANSLATE_NOOP("PropertyName", "angularVelocity"), QT_TRANSLATE_NOOP("Units", "rad/s"), QT_TRANSLATE_NOOP("PropertyDescription", "Angular velocity of the body"), angularVelocity, setAngularVelocity)
0028         STEPCORE_PROPERTY_RW_D(double, angularMomentum, QT_TRANSLATE_NOOP("PropertyName", "angularMomentum"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m²/s")),
0029                                 QT_TRANSLATE_NOOP("PropertyDescription", "Angular momentum of the body"), angularMomentum, setAngularMomentum)
0030
0031         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, acceleration, QT_TRANSLATE_NOOP("PropertyName", "acceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
0032                                             QT_TRANSLATE_NOOP("PropertyDescription", "Acceleration of the center of mass"), acceleration)
0033         STEPCORE_PROPERTY_R_D(double, angularAcceleration, QT_TRANSLATE_NOOP("PropertyName", "angularAcceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "rad/s²")),
0034                                             QT_TRANSLATE_NOOP("PropertyDescription", "Angular acceleration of the body"), angularAcceleration)
0035
0036         STEPCORE_PROPERTY_R_D(StepCore::Vector2d, force, QT_TRANSLATE_NOOP("PropertyName", "force"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "Force that acts upon the body"), force)
0037         STEPCORE_PROPERTY_R_D(double, torque, QT_TRANSLATE_NOOP("PropertyName", "torque"), QT_TRANSLATE_NOOP("Units", "N m"), QT_TRANSLATE_NOOP("PropertyDescription", "Torque that acts upon the body"), torque)
0038         STEPCORE_PROPERTY_R_D(double, mass, QT_TRANSLATE_NOOP("PropertyName", "mass"), QT_TRANSLATE_NOOP("Units", "kg"), QT_TRANSLATE_NOOP("PropertyDescription", "Total mass of the body"), mass)
0039         STEPCORE_PROPERTY_R_D(double, inertia, QT_TRANSLATE_NOOP("PropertyName", "inertia"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m²")),
0040                                 QT_TRANSLATE_NOOP("PropertyDescription", "Inertia \"tensor\" of the body"), inertia)
0041         STEPCORE_PROPERTY_RW(QString, borderParticleNames, QT_TRANSLATE_NOOP("PropertyName", "borderParticleNames"), STEPCORE_UNITS_NULL,
0042                                 QT_TRANSLATE_NOOP("PropertyDescription", "Border particle names (temporal property)"), borderParticleNames, setBorderParticleNames)
0043         )
0044
0045
0046 ItemList SoftBody::createSoftBodyItems(const Vector2d& position, const Vector2d& size, const Vector2i& split,
0047                     double bodyMass, double youngModulus, double bodyDamping)
0048 {
0049     ItemList items;
0050     _borderParticles.clear();
0051
0052     if((split[0] < 1 || split[1] < 1) || (split[0] == 1 && split[1] == 1)) {
0053         return items;
0054     }
0055
0056     Vector2d vel = Vector2d::Zero(); //to be changed
0057     Vector2d pos;
0058
0059     double mass = bodyMass/(split[0]*split[1]);
0060     double stiffnes;
0061     double damping;
0062     double h0;
0063     double h1;
0064     double h;
0065
0066     if(split[0] == 1) {
0067         _borderParticles.resize(split[1]);
0068         stiffnes = youngModulus/(split[1]-1);
0069         damping = bodyDamping/(split[1]-1);
0070         h1 = size[1]/(split[1]-1); h0 = 0;
0071     } else if(split[1] == 1) {
0072         _borderParticles.resize(split[0]);
0073         stiffnes = youngModulus/(split[0]-1);
0074         damping = bodyDamping/(split[0]-1);
0075         h0 = size[0]/(split[0]-1); h1 = 0;
0076     } else {
0077         _borderParticles.resize(2*split[0] + 2*split[1] - 4);
0078         stiffnes = youngModulus*(size[0]/size[1])*(split[0]-1)/(2*split[1]-1);
0079         damping  = bodyDamping* (size[0]/size[1])*(split[0]-1)/(2*split[1]-1);
0080         h0 = size[0]/(split[0]-1);
0081         h1 = size[1]/(split[1]-1);
0082     }
0083
0084     // particles
0085     pos[1] = position[1] - (split[1]>1 ? 0.5*size[1] : 0);
0086     for(int j=0; j < split[1]; ++j) {
0087         pos[0] = position[0] - (split[0]>1 ? 0.5*size[0] : 0);
0088         for(int i=0; i < split[0]; ++i) {
0089             SoftBodyParticle* item = new SoftBodyParticle(pos, vel, mass);
0090             items.push_back(item);
0091
0092             if(j == 0) _borderParticles[i] = item;
0093             else if(i == split[0]-1) _borderParticles[split[0]-1+j] = item;
0094             else if(j == split[1]-1) _borderParticles[split[1]+2*split[0]-3-i] = item;
0095             else if(i == 0) _borderParticles[2*split[0]+2*split[1]-4-j] = item;
0096             pos[0] += h0;
0097         }
0098         pos[1] += h1;
0099     }
0100
0101     // horisontal springs
0102     for(int i=0; i<split[1]; i++) {
0103         for(int j=0; j<split[0]-1; j++) {
0104             SoftBodySpring* item = new SoftBodySpring(h0, stiffnes, damping,
0105                                     items[split[0]*i+j], items[split[0]*i+j+1]);
0106             items.push_back(item);
0107         }
0108     }
0109
0110     // vertical springs
0111     for(int i=0; i<split[1]-1; i++) {
0112         for(int j=0; j<split[0]; j++) {
0113             SoftBodySpring* item = new SoftBodySpring(h1, stiffnes, damping,
0114                                     items[split[0]*i+j], items[split[0]*(i+1)+j]);
0115             items.push_back(item);
0116         }
0117     }
0118
0119     // diagonal springs
0120     h = std::sqrt(h0*h0 + h1*h1);
0121     stiffnes /= M_SQRT2;//XXX
0122     damping /= M_SQRT2;
0123     for(int i=0; i<split[1]-1; i++){
0124         for(int j=0; j<split[0]-1; j++){
0125             SoftBodySpring* item1 = new SoftBodySpring(h, stiffnes, damping,
0126                                     items[split[0]*i+j], items[split[0]*(i+1)+j+1]);
0127             SoftBodySpring* item2 = new SoftBodySpring(h, stiffnes, damping,
0128                                     items[split[0]*i+j+1], items[split[0]*(i+1)+j]);
0129             items.push_back(item1);
0130             items.push_back(item2);
0131         }
0132     }
0133
0134     return items;
0135 }
0136
0137 void SoftBody::addItems(const ItemList& items)
0138 {
0139     const ItemList::const_iterator end = items.end();
0140     for(ItemList::const_iterator it = items.begin(); it != end; ++it) {
0141         addItem(*it);
0142     }
0143 }
0144
0145 double SoftBody::mass() const
0146 {
0147     double totMass = 0;
0148
0149     const ItemList::const_iterator end = items().end();
0150     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0151         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0152         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0153         totMass += p1->mass();
0154     }
0155
0156     return totMass;
0157 }
0158
0159 Vector2d SoftBody::position() const
0160 {
0161     Vector2d cmPosition = Vector2d::Zero();
0162
0163     const ItemList::const_iterator end = items().end();
0164     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0165         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0166         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0167         cmPosition += p1->mass() * p1->position();
0168     }
0169     cmPosition = cmPosition/mass();
0170     return cmPosition;
0171 }
0172
0173 void SoftBody::setPosition(const Vector2d &position)
0174 {
0175     Vector2d delta = position - this->position();
0176
0177     const ItemList::const_iterator end = items().end();
0178     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0179         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0180         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0181         p1->setPosition(p1->position() + delta);
0182     }
0183 }
0184
0185 Vector2d SoftBody::velocity() const
0186 {
0187     Vector2d cmVelocity = Vector2d::Zero();
0188
0189     const ItemList::const_iterator end = items().end();
0190     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0191         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0192         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0193         cmVelocity += p1->mass() * p1->velocity();
0194     }
0195
0196     cmVelocity = cmVelocity/mass();
0197     return cmVelocity;
0198 }
0199
0200 void SoftBody::setVelocity(const Vector2d &velocity)
0201 {
0202     Vector2d delta = velocity - this->velocity();
0203
0204     const ItemList::const_iterator end = items().end();
0205     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0206         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0207         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0208         p1->setVelocity(p1->velocity() + delta);
0209     }
0210 }
0211
0212 double SoftBody::inertia() const
0213 {
0214     double inertia = 0;
0215     Vector2d position = this->position();
0216
0217     const ItemList::const_iterator end = items().end();
0218     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0219         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0220         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0221         inertia += p1->mass() * (p1->position() - position).squaredNorm();
0222     }
0223
0224     return inertia;
0225 }
0226
0227 double SoftBody::angularMomentum() const
0228 {
0229     double angMomentum = 0;
0230     Vector2d pos = position();
0231     Vector2d vel = velocity();
0232
0233     const ItemList::const_iterator end = items().end();
0234     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0235         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0236         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0237         angMomentum += p1->mass() * ((p1->position() - pos)[0] * (p1->velocity() - vel)[1]
0238                                    - (p1->position() - pos)[1] * (p1->velocity() - vel)[0]) ;
0239     }
0240
0241     return angMomentum;
0242 }
0243
0244 double SoftBody::angularVelocity() const
0245 {
0246     return angularMomentum()/inertia();
0247 }
0248
0249 void SoftBody::setAngularVelocity(double angularVelocity)
0250 {
0251     Vector2d pos = position();
0252     Vector2d vel = velocity();
0253
0254     const ItemList::const_iterator end = items().end();
0255     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0256         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0257         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0258         Vector2d r = p1->position() - pos;
0259         Vector2d n(-r[1], r[0]);
0260         double vn = n.dot(p1->velocity() - vel);
0261         p1->setVelocity(p1->velocity() + (angularVelocity - vn/r.squaredNorm())*n);
0262     }
0263 }
0264
0265 void SoftBody::setAngularMomentum(double angularMomentum)
0266 {
0267     setAngularVelocity(angularMomentum/inertia());
0268 }
0269
0270 Vector2d SoftBody::force() const
0271 {
0272     Vector2d force = Vector2d::Zero();
0273
0274     const ItemList::const_iterator end = items().end();
0275     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0276         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0277         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0278         force += p1->force();
0279     }
0280
0281     return force;
0282 }
0283
0284 double SoftBody::torque() const
0285 {
0286     double torque = 0;
0287     Vector2d pos = position();
0288     const ItemList::const_iterator end = items().end();
0289     for(ItemList::const_iterator i1 = items().begin(); i1 != end; ++i1) {
0290         if(!(*i1)->metaObject()->inherits<SoftBodyParticle>()) continue;
0291         SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(*i1);
0292         Vector2d r = p1->position() - pos;
0293         torque += r[0] * p1->force()[1] - r[1] * p1->force()[0];
0294     }
0295
0296     return torque;
0297 }
0298
0299 const SoftBodyParticleList& SoftBody::borderParticles()
0300 {
0301     if(_borderParticles.empty() && !_borderParticleNames.isEmpty() && world()) {
0302         const QStringList list = _borderParticleNames.split(',');
0303         QStringList::const_iterator end = list.constEnd();
0304         for(QStringList::const_iterator it = list.constBegin(); it != end; ++it) {
0305             Object* obj = world()->object(*it);
0306             if(!obj->metaObject()->inherits<SoftBodyParticle>()) continue;
0307             SoftBodyParticle* p1 = static_cast<SoftBodyParticle*>(obj);
0308             _borderParticles.push_back(p1);
0309         }
0310         _borderParticleNames.clear();
0311     }
0312     return _borderParticles;
0313 }
0314
0315 QString SoftBody::borderParticleNames() const
0316 {
0317     QString list;
0318     SoftBodyParticleList::const_iterator end = _borderParticles.end();
0319     for(SoftBodyParticleList::const_iterator it = _borderParticles.begin(); it != end; ++it) {
0320         if(!list.isEmpty()) list.append(",");
0321         list.append((*it)->name());
0322     }
0323     return list;
0324 }
0325
0326 void SoftBody::setBorderParticleNames(const QString& borderParticleNames)
0327 {
0328     if(_borderParticles.empty() && _borderParticleNames.isEmpty())
0329         _borderParticleNames = borderParticleNames;
0330 }
0331
0332 void SoftBody::worldItemRemoved(Item* item)
0333 {
0334     if(!item) return;
0335
0336     if(!item->metaObject()->inherits<SoftBodyParticle>()) return;
0337     SoftBodyParticle* p = static_cast<SoftBodyParticle*>(item);
0338
0339     SoftBodyParticleList::iterator i =
0340             std::find(_borderParticles.begin(), _borderParticles.end(), p);
0341     if(i != _borderParticles.end()) _borderParticles.erase(i);
0342 }
0343
0344 void SoftBody::setWorld(World* world)
0345 {
0346     if(world == nullptr) {
0347         _borderParticles.clear();
0348     } else if(this->world() != nullptr) {
0349         const SoftBodyParticleList::iterator end = _borderParticles.end();
0350         for(SoftBodyParticleList::iterator i = _borderParticles.begin(); i != end; ++i) {
0351             *i = static_cast<SoftBodyParticle*>(world->object((*i)->name()));
0352         }
0353     }
0354     ItemGroup::setWorld(world);
0355 }
0356
0357 }
0358