File indexing completed on 2023-09-24 04:01:44
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 "motor.h" 0008 #include "rigidbody.h" 0009 #include "particle.h" 0010 #include <cmath> 0011 #include <Eigen/Geometry> 0012 0013 namespace StepCore 0014 { 0015 0016 STEPCORE_META_OBJECT(LinearMotor, QT_TRANSLATE_NOOP("ObjectClass", "LinearMotor"), QT_TRANSLATE_NOOP("ObjectDescription", "Linear motor: applies a constant force to a given position of the body"), 0, 0017 STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Force), 0018 STEPCORE_PROPERTY_RW(Object*, body, QT_TRANSLATE_NOOP("PropertyName", "body"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body"), body, setBody) 0019 STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition, QT_TRANSLATE_NOOP("PropertyName", "localPosition"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position of the motor on a body"), localPosition, setLocalPosition) 0020 STEPCORE_PROPERTY_RW(StepCore::Vector2d, forceValue, QT_TRANSLATE_NOOP("PropertyName", "forceValue"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "Value of the force, acting on the body"), forceValue, setForceValue) 0021 STEPCORE_PROPERTY_RW(bool, rigidlyFixed, QT_TRANSLATE_NOOP("PropertyName", "rigidlyFixed"), QT_TRANSLATE_NOOP("Units", ""), QT_TRANSLATE_NOOP("PropertyDescription", "Rotate the force vector in sync with body rotation"), isRigidlyFixed, setRigidlyFixed)) 0022 0023 STEPCORE_META_OBJECT(CircularMotor, QT_TRANSLATE_NOOP("ObjectClass", "CircularMotor"), QT_TRANSLATE_NOOP("ObjectDescription", "Circular motor: applies a constant torque to the body"), 0, 0024 STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Force), 0025 STEPCORE_PROPERTY_RW(Object*, body, QT_TRANSLATE_NOOP("PropertyName", "body"), STEPCORE_UNITS_NULL, QT_TRANSLATE_NOOP("PropertyDescription", "Body"), body, setBody) 0026 STEPCORE_PROPERTY_RW(StepCore::Vector2d, localPosition, QT_TRANSLATE_NOOP("PropertyName", "localPosition"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position of the motor on a body"), localPosition, setLocalPosition) 0027 STEPCORE_PROPERTY_RW(double, torqueValue, QT_TRANSLATE_NOOP("PropertyName", "torqueValue"), QT_TRANSLATE_NOOP("Units", "N m"), QT_TRANSLATE_NOOP("PropertyDescription", "Value of the torque, acting on the body"), torqueValue, setTorqueValue)) 0028 0029 0030 LinearMotor::LinearMotor(Object* body, const Vector2d& localPosition, const Vector2d &forceValue) 0031 : Force() 0032 , _localPosition(localPosition) 0033 , _forceValue(forceValue) 0034 { 0035 setBody(body); 0036 setColor(0xff0000ff); 0037 } 0038 0039 void LinearMotor::calcForce(bool /*calcVariances*/) 0040 { 0041 if(_p) { 0042 _p->applyForce(_forceValue); 0043 } else if(_r) { 0044 double angleDelta = _r->angle() - _lastBodyAngle; 0045 _lastBodyAngle = _r->angle(); 0046 if (_rigidlyFixed) { 0047 Eigen::Rotation2Dd rot(angleDelta); 0048 _forceValue = rot * _forceValue; 0049 } 0050 _r->applyForce(_forceValue, 0051 _r->pointLocalToWorld(_localPosition)); 0052 } 0053 } 0054 0055 void LinearMotor::setBody(Object* body) 0056 { 0057 if(body) { 0058 if(body->metaObject()->inherits<Particle>()) { 0059 _body = body; 0060 _p = static_cast<Particle*>(body); 0061 _r = nullptr; 0062 _lastBodyAngle = 0; 0063 return; 0064 } else if(body->metaObject()->inherits<RigidBody>()) { 0065 _body = body; 0066 _p = nullptr; 0067 _r = static_cast<RigidBody*>(body); 0068 _lastBodyAngle = _r->angle(); 0069 return; 0070 } 0071 } 0072 _body = nullptr; 0073 _p = nullptr; 0074 _r = nullptr; 0075 _lastBodyAngle = 0; 0076 } 0077 0078 Vector2d LinearMotor::position() const 0079 { 0080 if(_p) return _p->position() + _localPosition; 0081 else if(_r) return _r->pointLocalToWorld(_localPosition); 0082 return _localPosition; 0083 } 0084 0085 /* 0086 void LinearMotor::worldItemRemoved(Item* item) 0087 { 0088 if(item == NULL) return; 0089 if(item == _body) setBody(NULL); 0090 } 0091 0092 void LinearMotor::setWorld(World* world) 0093 { 0094 if(world == NULL) { 0095 setBody(NULL); 0096 } else if(this->world() != NULL) { 0097 if(_body != NULL) setBody(world->item(body()->name())); 0098 } 0099 Item::setWorld(world); 0100 } 0101 */ 0102 0103 ////////////////////////////////////////////////////////////////////////// 0104 CircularMotor::CircularMotor(Object* body, const Vector2d& localPosition, double torqueValue) 0105 : Force() 0106 , _localPosition(localPosition) 0107 , _torqueValue(torqueValue) 0108 { 0109 setBody(body); 0110 setColor(0xff0000ff); 0111 } 0112 0113 void CircularMotor::calcForce(bool /*calcVariances*/) 0114 { 0115 if(_r) _r->applyTorque(_torqueValue); 0116 } 0117 0118 void CircularMotor::setBody(Object* body) 0119 { 0120 if(body) { 0121 if(body->metaObject()->inherits<RigidBody>()) { 0122 _body = body; 0123 _r = static_cast<RigidBody*>(body); 0124 return; 0125 } 0126 } 0127 _body = nullptr; 0128 _r = nullptr; 0129 } 0130 0131 Vector2d CircularMotor::localPosition() const 0132 { 0133 if(_r) return Vector2d::Zero(); 0134 else return _localPosition; 0135 } 0136 0137 Vector2d CircularMotor::position() const 0138 { 0139 if(_r) return _r->position(); 0140 return _localPosition; 0141 } 0142 0143 /* 0144 void CircularMotor::worldItemRemoved(Item* item) 0145 { 0146 if(item == NULL) return; 0147 if(item == _body) setBody(NULL); 0148 } 0149 0150 void CircularMotor::setWorld(World* world) 0151 { 0152 if(world == NULL) { 0153 setBody(NULL); 0154 } else if(this->world() != NULL) { 0155 if(_body != NULL) setBody(world->item(body()->name())); 0156 } 0157 Item::setWorld(world); 0158 } 0159 */ 0160 0161 } // namespace StepCore 0162