step/stepcore/joints.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 "joints.h"
0008 #include "particle.h"
0009 #include "rigidbody.h"
0010 #include <cstring>
0011
0012 namespace StepCore {
0013
0014 STEPCORE_META_OBJECT(Anchor, QT_TRANSLATE_NOOP("ObjectClass", "Anchor"), QT_TRANSLATE_NOOP("ObjectDescription", "Anchor: fixes position of the body"), 0,
0015     STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Joint),
0016     STEPCORE_PROPERTY_RW(Object*, body, QT_TRANSLATE_NOOP("PropertyName", "body"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body"), body, setBody)
0017     STEPCORE_PROPERTY_RW(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position"), position, setPosition)
0018     STEPCORE_PROPERTY_RW(double, angle, QT_TRANSLATE_NOOP("PropertyName", "angle"), QT_TRANSLATE_NOOP("Units", "rad"), QT_TRANSLATE_NOOP("PropertyDescription", "Angle"), angle, setAngle))
0019
0020 STEPCORE_META_OBJECT(Pin, QT_TRANSLATE_NOOP("ObjectClass", "Pin"), QT_TRANSLATE_NOOP("ObjectDescription", "Pin: fixes position of a given point on the body"), 0,
0021     STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Joint),
0022     STEPCORE_PROPERTY_RW(Object*, body, QT_TRANSLATE_NOOP("PropertyName", "body"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body"), body, setBody)
0023     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition, QT_TRANSLATE_NOOP("PropertyName", "localPosition"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position on the body"), localPosition, setLocalPosition)
0024     STEPCORE_PROPERTY_RW(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position in the world"), position, setPosition))
0025
0026 STEPCORE_META_OBJECT(Stick, QT_TRANSLATE_NOOP("ObjectClass", "Stick"), QT_TRANSLATE_NOOP("ObjectDescription", "Massless stick which can be connected to bodies"), 0,
0027     STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Joint),
0028     STEPCORE_PROPERTY_RW(double, restLength, QT_TRANSLATE_NOOP("PropertyName", "restLength"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Rest length of the stick"), restLength, setRestLength)
0029     STEPCORE_PROPERTY_RW(Object*, body1, QT_TRANSLATE_NOOP("PropertyName", "body1"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body1"), body1, setBody1)
0030     STEPCORE_PROPERTY_RW(Object*, body2, QT_TRANSLATE_NOOP("PropertyName", "body2"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body2"), body2, setBody2)
0031     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition1, QT_TRANSLATE_NOOP("PropertyName", "localPosition1"), QT_TRANSLATE_NOOP("Units", "m"),
0032                     QT_TRANSLATE_NOOP("PropertyDescription", "Local position 1"), localPosition1, setLocalPosition1)
0033     STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition2, QT_TRANSLATE_NOOP("PropertyName", "localPosition2"), QT_TRANSLATE_NOOP("Units", "m"),
0034                     QT_TRANSLATE_NOOP("PropertyDescription", "Local position 2"), localPosition2, setLocalPosition2)
0035     STEPCORE_PROPERTY_R_D(StepCore::Vector2d, position1, QT_TRANSLATE_NOOP("PropertyName", "position1"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position1"), position1)
0036     STEPCORE_PROPERTY_R_D(StepCore::Vector2d, position2, QT_TRANSLATE_NOOP("PropertyName", "position2"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position2"), position2)
0037     )
0038
0039 STEPCORE_META_OBJECT(Rope, QT_TRANSLATE_NOOP("ObjectClass", "Rope"), QT_TRANSLATE_NOOP("ObjectDescription", "Massless rope which can be connected to bodies"), 0,
0040     STEPCORE_SUPER_CLASS(Stick),)
0041
0042 Anchor::Anchor(Object* body, const Vector2d& position, double angle)
0043     : _position(position), _angle(angle)
0044 {
0045     setBody(body);
0046     setColor(0xffff0000);
0047 }
0048
0049 void Anchor::setBody(Object* body)
0050 {
0051     if(body) {
0052         if(body->metaObject()->inherits<Particle>()) {
0053             _body = body;
0054             _p = static_cast<Particle*>(body);
0055             _r = nullptr;
0056             return;
0057         } else if(body->metaObject()->inherits<RigidBody>()) {
0058             _body = body;
0059             _p = nullptr;
0060             _r = static_cast<RigidBody*>(body);
0061             return;
0062         }
0063     }
0064     _body = nullptr;
0065     _p = nullptr;
0066     _r = nullptr;
0067 }
0068
0069 int Anchor::constraintsCount()
0070 {
0071     if(_p) return 2;
0072     else if(_r) return 3;
0073     else return 0;
0074 }
0075
0076 void Anchor::getConstraintsInfo(ConstraintsInfo* info, int offset)
0077 {
0078     if(_p) {
0079         info->value[offset  ] = _p->position()[0] - _position[0];
0080         info->value[offset+1] = _p->position()[1] - _position[1];
0081
0082         info->derivative[offset  ] = _p->velocity()[0];
0083         info->derivative[offset+1] = _p->velocity()[1];
0084
0085         info->jacobian.coeffRef(offset, _p->variablesOffset()+Particle::PositionOffset) = 1;
0086         info->jacobian.coeffRef(offset+1, _p->variablesOffset()+Particle::PositionOffset+1) = 1;
0087
0088     } else if(_r) {
0089         info->value[offset  ] = _r->position()[0] - _position[0];
0090         info->value[offset+1] = _r->position()[1] - _position[1];
0091         info->value[offset+2] = _r->angle() - _angle;
0092
0093         info->derivative[offset  ] = _r->velocity()[0];
0094         info->derivative[offset+1] = _r->velocity()[1];
0095         info->derivative[offset+2] = _r->angularVelocity();
0096
0097         info->jacobian.coeffRef(offset, _r->variablesOffset()+RigidBody::PositionOffset) = 1;
0098         info->jacobian.coeffRef(offset+1, _r->variablesOffset()+RigidBody::PositionOffset+1) = 1;
0099         info->jacobian.coeffRef(offset+2, _r->variablesOffset()+RigidBody::AngleOffset) = 1;
0100     }
0101 }
0102
0103 Pin::Pin(Object* body, const Vector2d& localPosition, const Vector2d& position)
0104     : _localPosition(localPosition), _position(position)
0105 {
0106     setBody(body);
0107     setColor(0xffff0000);
0108 }
0109
0110 void Pin::setBody(Object* body)
0111 {
0112     if(body) {
0113         if(body->metaObject()->inherits<Particle>()) {
0114             _body = body;
0115             _p = static_cast<Particle*>(body);
0116             _r = nullptr;
0117             return;
0118         } else if(body->metaObject()->inherits<RigidBody>()) {
0119             _body = body;
0120             _p = nullptr;
0121             _r = static_cast<RigidBody*>(body);
0122             return;
0123         }
0124     }
0125     _body = nullptr;
0126     _p = nullptr;
0127     _r = nullptr;
0128 }
0129
0130 int Pin::constraintsCount()
0131 {
0132     if(_p) {
0133         if(_localPosition.squaredNorm() != 0) return 1; // XXX: add some epsilon here
0134         else return 2;
0135     } else if(_r) return 2;
0136     else return 0;
0137 }
0138
0139 void Pin::getConstraintsInfo(ConstraintsInfo* info, int offset)
0140 {
0141     if(_p) {
0142         Vector2d r = _p->position() - _position;
0143         double lnorm2 = _localPosition.squaredNorm();
0144
0145         if(lnorm2 != 0) { // XXX: add some epsilon here
0146             info->value[offset] = (r.squaredNorm() - lnorm2)*0.5;
0147             info->derivative[offset] = _p->velocity().dot(r);
0148
0149             info->jacobian.coeffRef(offset, _p->variablesOffset()+Particle::PositionOffset) = r[0];
0150             info->jacobian.coeffRef(offset, _p->variablesOffset()+Particle::PositionOffset+1) = r[1];
0151
0152             info->jacobianDerivative.coeffRef(offset,
0153                                 _p->variablesOffset()+Particle::PositionOffset) = _p->velocity()[0];
0154             info->jacobianDerivative.coeffRef(offset,
0155                                 _p->variablesOffset()+Particle::PositionOffset+1) = _p->velocity()[1];
0156         } else {
0157             info->value[offset  ]  = r[0];
0158             info->value[offset+1]  = r[1];
0159             info->derivative[offset  ] = _p->velocity()[0];
0160             info->derivative[offset+1] = _p->velocity()[1];
0161
0162             info->jacobian.coeffRef(offset, _p->variablesOffset()+Particle::PositionOffset) = 1;
0163             info->jacobian.coeffRef(offset+1, _p->variablesOffset()+Particle::PositionOffset+1) = 1;
0164         }
0165     } else if(_r) {
0166         Vector2d r1 = _r->vectorLocalToWorld(_localPosition);
0167         Vector2d p1 = _r->position() + r1;
0168         Vector2d v1 = _r->velocityWorld(p1);
0169         double   av = _r->angularVelocity();
0170
0171         info->value[offset  ] = p1[0] - _position[0];
0172         info->value[offset+1] = p1[1] - _position[1];
0173         info->derivative[offset  ] = v1[0];
0174         info->derivative[offset+1] = v1[1];
0175
0176         info->jacobian.coeffRef(offset  , _r->variablesOffset()+RigidBody::PositionOffset) = 1;
0177         info->jacobian.coeffRef(offset  , _r->variablesOffset()+RigidBody::AngleOffset) = -r1[1];
0178         info->jacobianDerivative.coeffRef(offset  , _r->variablesOffset()+RigidBody::AngleOffset) = -av*r1[0];
0179         info->jacobian.coeffRef(offset+1, _r->variablesOffset()+RigidBody::PositionOffset+1) = 1;
0180         info->jacobian.coeffRef(offset+1, _r->variablesOffset()+RigidBody::AngleOffset) =  r1[0];
0181         info->jacobianDerivative.coeffRef(offset+1, _r->variablesOffset()+RigidBody::AngleOffset) = -av*r1[1];
0182     }
0183
0184 }
0185
0186 Stick::Stick(double restLength, Object* body1, Object* body2,
0187            const Vector2d& localPosition1, const Vector2d& localPosition2)
0188     : _restLength(restLength), _localPosition1(localPosition1), _localPosition2(localPosition2)
0189 {
0190     setColor(0xffff0000);
0191     setBody1(body1);
0192     setBody2(body2);
0193 }
0194
0195 void Stick::setBody1(Object* body1)
0196 {
0197     if(body1) {
0198         if(body1->metaObject()->inherits<Particle>()) {
0199             _body1 = body1;
0200             _p1 = static_cast<Particle*>(body1);
0201             _r1 = nullptr;
0202             return;
0203         } else if(body1->metaObject()->inherits<RigidBody>()) {
0204             _body1 = body1;
0205             _p1 = nullptr;
0206             _r1 = static_cast<RigidBody*>(body1);
0207             return;
0208         }
0209     }
0210     _body1 = nullptr;
0211     _p1 = nullptr;
0212     _r1 = nullptr;
0213 }
0214
0215 void Stick::setBody2(Object* body2)
0216 {
0217     if(body2) {
0218         if(body2->metaObject()->inherits<Particle>()) {
0219             _body2 = body2;
0220             _p2 = static_cast<Particle*>(body2);
0221             _r2 = nullptr;
0222             return;
0223         } else if(body2->metaObject()->inherits<RigidBody>()) {
0224             _body2 = body2;
0225             _p2 = nullptr;
0226             _r2 = static_cast<RigidBody*>(body2);
0227             return;
0228         }
0229     }
0230     _body2 = nullptr;
0231     _p2 = nullptr;
0232     _r2 = nullptr;
0233 }
0234
0235 Vector2d Stick::position1() const
0236 {
0237     if(_p1) return _p1->position() + _localPosition1;
0238     else if(_r1) return _r1->pointLocalToWorld(_localPosition1);
0239     else return _localPosition1;
0240 }
0241
0242 Vector2d Stick::position2() const
0243 {
0244     if(_p2) return _p2->position() + _localPosition2;
0245     else if(_r2) return _r2->pointLocalToWorld(_localPosition2);
0246     else return _localPosition2;
0247 }
0248
0249 Vector2d Stick::velocity1() const
0250 {
0251     if(_p1) return _p1->velocity();
0252     else if(_r1) return _r1->velocityLocal(_localPosition1);
0253     else return Vector2d::Zero();
0254 }
0255
0256 Vector2d Stick::velocity2() const
0257 {
0258     if(_p2) return _p2->velocity();
0259     else if(_r2) return _r2->velocityLocal(_localPosition2);
0260     else return Vector2d::Zero();
0261 }
0262
0263 int Stick::constraintsCount()
0264 {
0265     if(!_body1 && !_body2) return 0;
0266
0267     if(_restLength != 0) return 1; // XXX: add some epsilon here
0268     else return 2;
0269 }
0270
0271 void Stick::getConstraintsInfo(ConstraintsInfo* info, int offset)
0272 {
0273     if(!_body1 && !_body2) return;
0274
0275     Vector2d p = position2() - position1();
0276     Vector2d v = velocity2() - velocity1();
0277
0278     //qDebug("_restLength=%f", _restLength);
0279     if(_restLength != 0) {
0280         info->value[offset] = (p.squaredNorm() - _restLength*_restLength)*0.5;
0281         info->derivative[offset] = v.dot(p);
0282
0283         if(p[0] == 0 && p[1] == 0) p[0] = 0.1; //XXX: add epsilon
0284
0285         if(_p1) {
0286             info->jacobian.coeffRef(offset, _p1->variablesOffset() + Particle::PositionOffset) = (  -p[0]);
0287             info->jacobian.coeffRef(offset, _p1->variablesOffset() + Particle::PositionOffset+1) =( -p[1]);
0288
0289             info->jacobianDerivative.coeffRef(offset, _p1->variablesOffset() + Particle::PositionOffset) =(   -v[0]);
0290             info->jacobianDerivative.coeffRef(offset, _p1->variablesOffset() + Particle::PositionOffset+1) =( -v[1]);
0291
0292         } else if(_r1) {
0293             Vector2d r1 = _r1->vectorLocalToWorld(_localPosition1);
0294
0295             info->jacobian.coeffRef(offset, _r1->variablesOffset() + RigidBody::PositionOffset) =(   -p[0]);
0296             info->jacobian.coeffRef(offset, _r1->variablesOffset() + RigidBody::PositionOffset+1) =( -p[1]);
0297             info->jacobian.coeffRef(offset, _r1->variablesOffset() + RigidBody::AngleOffset) =( +p[0]*r1[1] - p[1]*r1[0]);
0298
0299             info->jacobianDerivative.coeffRef(offset, _r1->variablesOffset() + RigidBody::PositionOffset) =(   -v[0]);
0300             info->jacobianDerivative.coeffRef(offset, _r1->variablesOffset() + RigidBody::PositionOffset+1) =( -v[1]);
0301             info->jacobianDerivative.coeffRef(offset, _r1->variablesOffset() + RigidBody::AngleOffset) =(
0302                                 + v[0]*r1[1] - v[1]*r1[0] + _r1->angularVelocity()*r1.dot(p));
0303         }
0304
0305         if(_p2) {
0306             info->jacobian.coeffRef(offset, _p2->variablesOffset() + Particle::PositionOffset) =(   p[0]);
0307             info->jacobian.coeffRef(offset, _p2->variablesOffset() + Particle::PositionOffset+1) =( p[1]);
0308
0309             info->jacobianDerivative.coeffRef(offset, _p2->variablesOffset() + Particle::PositionOffset) =(   v[0]);
0310             info->jacobianDerivative.coeffRef(offset, _p2->variablesOffset() + Particle::PositionOffset+1) =( v[1]);
0311
0312         } else if(_r2) {
0313             Vector2d r2 = _r2->vectorLocalToWorld(_localPosition2);
0314
0315             info->jacobian.coeffRef(offset, _r2->variablesOffset() + RigidBody::PositionOffset) =(   p[0]);
0316             info->jacobian.coeffRef(offset, _r2->variablesOffset() + RigidBody::PositionOffset+1) =( p[1]);
0317             info->jacobian.coeffRef(offset, _r2->variablesOffset() + RigidBody::AngleOffset) =( -p[0]*r2[1] + p[1]*r2[0]);
0318
0319             info->jacobianDerivative.coeffRef(offset, _r2->variablesOffset() + RigidBody::PositionOffset) =(   v[0]);
0320             info->jacobianDerivative.coeffRef(offset, _r2->variablesOffset() + RigidBody::PositionOffset+1) =( v[1]);
0321             info->jacobianDerivative.coeffRef(offset, _r2->variablesOffset() + RigidBody::AngleOffset) =(
0322                                 - v[0]*r2[1] + v[1]*r2[0] - _r2->angularVelocity()*r2.dot(p));
0323         }
0324
0325     } else {
0326         info->value[offset  ] = p[0];
0327         info->value[offset+1] = p[1];
0328
0329         info->derivative[offset  ] = v[0];
0330         info->derivative[offset+1] = v[1];
0331
0332         if(_p1) {
0333             info->jacobian.coeffRef(offset  , _p1->variablesOffset() + Particle::PositionOffset)   = -1;
0334             info->jacobian.coeffRef(offset+1, _p1->variablesOffset() + Particle::PositionOffset+1) = -1;
0335
0336         } else if(_r1) {
0337             Vector2d r1 = _r1->vectorLocalToWorld(_localPosition1);
0338             double   av = _r1->angularVelocity();
0339
0340             info->jacobian.coeffRef(offset  , _r1->variablesOffset() + Particle::PositionOffset) =(   -1);
0341             info->jacobian.coeffRef(offset+1, _r1->variablesOffset() + Particle::PositionOffset+1) =( -1);
0342
0343             info->jacobian.coeffRef(offset  , _r1->variablesOffset()+RigidBody::AngleOffset) =( +r1[1]);
0344             info->jacobian.coeffRef(offset+1, _r1->variablesOffset()+RigidBody::AngleOffset) =( -r1[0]);
0345             info->jacobianDerivative.coeffRef(offset  , _r1->variablesOffset()+RigidBody::AngleOffset) =( +av*r1[0]);
0346             info->jacobianDerivative.coeffRef(offset+1, _r1->variablesOffset()+RigidBody::AngleOffset) =( +av*r1[1]);
0347         }
0348
0349         if(_p2) {
0350             info->jacobian.coeffRef(offset  , _p2->variablesOffset() + Particle::PositionOffset) =(   1);
0351             info->jacobian.coeffRef(offset+1, _p2->variablesOffset() + Particle::PositionOffset+1) =( 1);
0352
0353         } else if(_r2) {
0354             Vector2d r2 = _r2->vectorLocalToWorld(_localPosition2);
0355             double   av = _r2->angularVelocity();
0356
0357             info->jacobian.coeffRef(offset  , _r2->variablesOffset() + Particle::PositionOffset) =(   1);
0358             info->jacobian.coeffRef(offset+1, _r2->variablesOffset() + Particle::PositionOffset+1) =( 1);
0359
0360             info->jacobian.coeffRef(offset  , _r2->variablesOffset()+RigidBody::AngleOffset) =( -r2[1]);
0361             info->jacobian.coeffRef(offset+1, _r2->variablesOffset()+RigidBody::AngleOffset) =( +r2[0]);
0362             info->jacobianDerivative.coeffRef(offset  , _r2->variablesOffset()+RigidBody::AngleOffset) =( -av*r2[0]);
0363             info->jacobianDerivative.coeffRef(offset+1, _r2->variablesOffset()+RigidBody::AngleOffset) =( -av*r2[1]);
0364         }
0365     }
0366 }
0367
0368 void Rope::getConstraintsInfo(ConstraintsInfo* info, int offset)
0369 {
0370     if(!_body1 && !_body2) return;
0371
0372     Vector2d p = position2() - position1();
0373     Vector2d v = velocity2() - velocity1();
0374
0375     if(_restLength != 0) {
0376         if(p.norm() >= _restLength) { // rope is in tension
0377             Stick::getConstraintsInfo(info, offset);
0378             info->forceMax[offset] = 0;
0379         } else { // rope is free
0380             info->value[offset] = 0;
0381             info->derivative[offset] = 0;
0382         }
0383     } else {
0384         Stick::getConstraintsInfo(info, offset);
0385     }
0386 }
0387
0388 } // namespace StepCore
0389
0390