vcs.maemo.org Git - blok/blob - Box2D/Source/Dynamics/Joints/b2RevoluteJoint.cpp

   1 /*
   2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * arising from the use of this software.
   7 * Permission is granted to anyone to use this software for any purpose,
   8 * including commercial applications, and to alter it and redistribute it
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2RevoluteJoint.h"
  20 #include "../b2Body.h"
  21 #include "../b2World.h"
  22
  23 #include "../b2Island.h"
  24
  25 // Point-to-point constraint
  26 // C = p2 - p1
  27 // Cdot = v2 - v1
  28 //      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
  29 // J = [-I -r1_skew I r2_skew ]
  30 // Identity used:
  31 // w k % (rx i + ry j) = w * (-ry i + rx j)
  32
  33 // Motor constraint
  34 // Cdot = w2 - w1
  35 // J = [0 0 -1 0 0 1]
  36 // K = invI1 + invI2
  37
  38 void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
  39 {
  40         body1 = b1;
  41         body2 = b2;
  42         localAnchor1 = body1->GetLocalPoint(anchor);
  43         localAnchor2 = body2->GetLocalPoint(anchor);
  44         referenceAngle = body2->GetAngle() - body1->GetAngle();
  45 }
  46
  47 b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
  48 : b2Joint(def)
  49 {
  50         m_localAnchor1 = def->localAnchor1;
  51         m_localAnchor2 = def->localAnchor2;
  52         m_referenceAngle = def->referenceAngle;
  53
  54         m_pivotForce.Set(0.0f, 0.0f);
  55         m_motorForce = 0.0f;
  56         m_limitForce = 0.0f;
  57         m_limitPositionImpulse = 0.0f;
  58
  59         m_lowerAngle = def->lowerAngle;
  60         m_upperAngle = def->upperAngle;
  61         m_maxMotorTorque = def->maxMotorTorque;
  62         m_motorSpeed = def->motorSpeed;
  63         m_enableLimit = def->enableLimit;
  64         m_enableMotor = def->enableMotor;
  65 }
  66
  67 void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
  68 {
  69         b2Body* b1 = m_body1;
  70         b2Body* b2 = m_body2;
  71
  72         // Compute the effective mass matrix.
  73         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
  74         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
  75
  76         // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
  77         //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
  78         //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
  79         float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
  80         float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
  81
  82         b2Mat22 K1;
  83         K1.col1.x = invMass1 + invMass2;        K1.col2.x = 0.0f;
  84         K1.col1.y = 0.0f;                                       K1.col2.y = invMass1 + invMass2;
  85
  86         b2Mat22 K2;
  87         K2.col1.x =  invI1 * r1.y * r1.y;       K2.col2.x = -invI1 * r1.x * r1.y;
  88         K2.col1.y = -invI1 * r1.x * r1.y;       K2.col2.y =  invI1 * r1.x * r1.x;
  89
  90         b2Mat22 K3;
  91         K3.col1.x =  invI2 * r2.y * r2.y;       K3.col2.x = -invI2 * r2.x * r2.y;
  92         K3.col1.y = -invI2 * r2.x * r2.y;       K3.col2.y =  invI2 * r2.x * r2.x;
  93
  94         b2Mat22 K = K1 + K2 + K3;
  95         m_pivotMass = K.Invert();
  96
  97         m_motorMass = 1.0f / (invI1 + invI2);
  98
  99         if (m_enableMotor == false)
 100         {
 101                 m_motorForce = 0.0f;
 102         }
 103
 104         if (m_enableLimit)
 105         {
 106                 float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
 107                 if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
 108                 {
 109                         m_limitState = e_equalLimits;
 110                 }
 111                 else if (jointAngle <= m_lowerAngle)
 112                 {
 113                         if (m_limitState != e_atLowerLimit)
 114                         {
 115                                 m_limitForce = 0.0f;
 116                         }
 117                         m_limitState = e_atLowerLimit;
 118                 }
 119                 else if (jointAngle >= m_upperAngle)
 120                 {
 121                         if (m_limitState != e_atUpperLimit)
 122                         {
 123                                 m_limitForce = 0.0f;
 124                         }
 125                         m_limitState = e_atUpperLimit;
 126                 }
 127                 else
 128                 {
 129                         m_limitState = e_inactiveLimit;
 130                         m_limitForce = 0.0f;
 131                 }
 132         }
 133         else
 134         {
 135                 m_limitForce = 0.0f;
 136         }
 137
 138         if (step.warmStarting)
 139         {
 140                 b1->m_linearVelocity -= B2FORCE_SCALE(step.dt) * invMass1 * m_pivotForce;
 141                 b1->m_angularVelocity -= B2FORCE_SCALE(step.dt) * invI1 * (b2Cross(r1, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
 142
 143                 b2->m_linearVelocity += B2FORCE_SCALE(step.dt) * invMass2 * m_pivotForce;
 144                 b2->m_angularVelocity += B2FORCE_SCALE(step.dt) * invI2 * (b2Cross(r2, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
 145         }
 146         else
 147         {
 148                 m_pivotForce.SetZero();
 149                 m_motorForce = 0.0f;
 150                 m_limitForce = 0.0f;
 151         }
 152
 153         m_limitPositionImpulse = 0.0f;
 154 }
 155
 156 void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
 157 {
 158         b2Body* b1 = m_body1;
 159         b2Body* b2 = m_body2;
 160
 161         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 162         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 163
 164         // Solve point-to-point constraint
 165         b2Vec2 pivotCdot = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1);
 166         b2Vec2 pivotForce = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_pivotMass, pivotCdot);
 167         m_pivotForce += pivotForce;
 168
 169         b2Vec2 P = B2FORCE_SCALE(step.dt) * pivotForce;
 170         b1->m_linearVelocity -= b1->m_invMass * P;
 171         b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
 172
 173         b2->m_linearVelocity += b2->m_invMass * P;
 174         b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
 175
 176         if (m_enableMotor && m_limitState != e_equalLimits)
 177         {
 178                 float32 motorCdot = b2->m_angularVelocity - b1->m_angularVelocity - m_motorSpeed;
 179                 float32 motorForce = -step.inv_dt * m_motorMass * motorCdot;
 180                 float32 oldMotorForce = m_motorForce;
 181                 m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
 182                 motorForce = m_motorForce - oldMotorForce;
 183
 184                 float32 P = step.dt * motorForce;
 185                 b1->m_angularVelocity -= b1->m_invI * P;
 186                 b2->m_angularVelocity += b2->m_invI * P;
 187         }
 188
 189         if (m_enableLimit && m_limitState != e_inactiveLimit)
 190         {
 191                 float32 limitCdot = b2->m_angularVelocity - b1->m_angularVelocity;
 192                 float32 limitForce = -step.inv_dt * m_motorMass * limitCdot;
 193
 194                 if (m_limitState == e_equalLimits)
 195                 {
 196                         m_limitForce += limitForce;
 197                 }
 198                 else if (m_limitState == e_atLowerLimit)
 199                 {
 200                         float32 oldLimitForce = m_limitForce;
 201                         m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
 202                         limitForce = m_limitForce - oldLimitForce;
 203                 }
 204                 else if (m_limitState == e_atUpperLimit)
 205                 {
 206                         float32 oldLimitForce = m_limitForce;
 207                         m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
 208                         limitForce = m_limitForce - oldLimitForce;
 209                 }
 210
 211                 float32 P = step.dt * limitForce;
 212                 b1->m_angularVelocity -= b1->m_invI * P;
 213                 b2->m_angularVelocity += b2->m_invI * P;
 214         }
 215 }
 216
 217 bool b2RevoluteJoint::SolvePositionConstraints()
 218 {
 219         b2Body* b1 = m_body1;
 220         b2Body* b2 = m_body2;
 221
 222         float32 positionError = 0.0f;
 223
 224         // Solve point-to-point position error.
 225         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 226         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 227
 228         b2Vec2 p1 = b1->m_sweep.c + r1;
 229         b2Vec2 p2 = b2->m_sweep.c + r2;
 230         b2Vec2 ptpC = p2 - p1;
 231
 232         positionError = ptpC.Length();
 233
 234         // Prevent overly large corrections.
 235         //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
 236         //ptpC = b2Clamp(ptpC, -dpMax, dpMax);
 237
 238         float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
 239         float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
 240
 241         b2Mat22 K1;
 242         K1.col1.x = invMass1 + invMass2;        K1.col2.x = 0.0f;
 243         K1.col1.y = 0.0f;                                       K1.col2.y = invMass1 + invMass2;
 244
 245         b2Mat22 K2;
 246         K2.col1.x =  invI1 * r1.y * r1.y;       K2.col2.x = -invI1 * r1.x * r1.y;
 247         K2.col1.y = -invI1 * r1.x * r1.y;       K2.col2.y =  invI1 * r1.x * r1.x;
 248
 249         b2Mat22 K3;
 250         K3.col1.x =  invI2 * r2.y * r2.y;       K3.col2.x = -invI2 * r2.x * r2.y;
 251         K3.col1.y = -invI2 * r2.x * r2.y;       K3.col2.y =  invI2 * r2.x * r2.x;
 252
 253         b2Mat22 K = K1 + K2 + K3;
 254         b2Vec2 impulse = K.Solve(-ptpC);
 255
 256         b1->m_sweep.c -= b1->m_invMass * impulse;
 257         b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
 258
 259         b2->m_sweep.c += b2->m_invMass * impulse;
 260         b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
 261
 262         b1->SynchronizeTransform();
 263         b2->SynchronizeTransform();
 264
 265         // Handle limits.
 266         float32 angularError = 0.0f;
 267
 268         if (m_enableLimit && m_limitState != e_inactiveLimit)
 269         {
 270                 float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
 271                 float32 limitImpulse = 0.0f;
 272
 273                 if (m_limitState == e_equalLimits)
 274                 {
 275                         // Prevent large angular corrections
 276                         float32 limitC = b2Clamp(angle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
 277                         limitImpulse = -m_motorMass * limitC;
 278                         angularError = b2Abs(limitC);
 279                 }
 280                 else if (m_limitState == e_atLowerLimit)
 281                 {
 282                         float32 limitC = angle - m_lowerAngle;
 283                         angularError = b2Max(0.0f, -limitC);
 284
 285                         // Prevent large angular corrections and allow some slop.
 286                         limitC = b2Clamp(limitC + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
 287                         limitImpulse = -m_motorMass * limitC;
 288                         float32 oldLimitImpulse = m_limitPositionImpulse;
 289                         m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
 290                         limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
 291                 }
 292                 else if (m_limitState == e_atUpperLimit)
 293                 {
 294                         float32 limitC = angle - m_upperAngle;
 295                         angularError = b2Max(0.0f, limitC);
 296
 297                         // Prevent large angular corrections and allow some slop.
 298                         limitC = b2Clamp(limitC - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
 299                         limitImpulse = -m_motorMass * limitC;
 300                         float32 oldLimitImpulse = m_limitPositionImpulse;
 301                         m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
 302                         limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
 303                 }
 304
 305                 b1->m_sweep.a -= b1->m_invI * limitImpulse;
 306                 b2->m_sweep.a += b2->m_invI * limitImpulse;
 307
 308                 b1->SynchronizeTransform();
 309                 b2->SynchronizeTransform();
 310         }
 311
 312         return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
 313 }
 314
 315 b2Vec2 b2RevoluteJoint::GetAnchor1() const
 316 {
 317         return m_body1->GetWorldPoint(m_localAnchor1);
 318 }
 319
 320 b2Vec2 b2RevoluteJoint::GetAnchor2() const
 321 {
 322         return m_body2->GetWorldPoint(m_localAnchor2);
 323 }
 324
 325 b2Vec2 b2RevoluteJoint::GetReactionForce() const
 326 {
 327         return B2FORCE_SCALE(float32(1.0))*m_pivotForce;
 328 }
 329
 330 float32 b2RevoluteJoint::GetReactionTorque() const
 331 {
 332         return m_limitForce;
 333 }
 334
 335 float32 b2RevoluteJoint::GetJointAngle() const
 336 {
 337         b2Body* b1 = m_body1;
 338         b2Body* b2 = m_body2;
 339         return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
 340 }
 341
 342 float32 b2RevoluteJoint::GetJointSpeed() const
 343 {
 344         b2Body* b1 = m_body1;
 345         b2Body* b2 = m_body2;
 346         return b2->m_angularVelocity - b1->m_angularVelocity;
 347 }
 348
 349 bool b2RevoluteJoint::IsMotorEnabled() const
 350 {
 351         return m_enableMotor;
 352 }
 353
 354 void b2RevoluteJoint::EnableMotor(bool flag)
 355 {
 356         m_enableMotor = flag;
 357 }
 358
 359 float32 b2RevoluteJoint::GetMotorTorque() const
 360 {
 361         return m_motorForce;
 362 }
 363
 364 void b2RevoluteJoint::SetMotorSpeed(float32 speed)
 365 {
 366         m_motorSpeed = speed;
 367 }
 368
 369 void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
 370 {
 371         m_maxMotorTorque = torque;
 372 }
 373
 374 bool b2RevoluteJoint::IsLimitEnabled() const
 375 {
 376         return m_enableLimit;
 377 }
 378
 379 void b2RevoluteJoint::EnableLimit(bool flag)
 380 {
 381         m_enableLimit = flag;
 382 }
 383
 384 float32 b2RevoluteJoint::GetLowerLimit() const
 385 {
 386         return m_lowerAngle;
 387 }
 388
 389 float32 b2RevoluteJoint::GetUpperLimit() const
 390 {
 391         return m_upperAngle;
 392 }
 393
 394 void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
 395 {
 396         b2Assert(lower <= upper);
 397         m_lowerAngle = lower;
 398         m_upperAngle = upper;
 399 }