File indexing completed on 2024-04-21 03:51:22
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