git.maemo.org Git - blok/blob - Box2D/Source/Dynamics/Joints/b2PulleyJoint.cpp

   1 /*
   2 * Copyright (c) 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 "b2PulleyJoint.h"
  20 #include "../b2Body.h"
  21 #include "../b2World.h"
  22
  23 // Pulley:
  24 // length1 = norm(p1 - s1)
  25 // length2 = norm(p2 - s2)
  26 // C0 = (length1 + ratio * length2)_initial
  27 // C = C0 - (length1 + ratio * length2) >= 0
  28 // u1 = (p1 - s1) / norm(p1 - s1)
  29 // u2 = (p2 - s2) / norm(p2 - s2)
  30 // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
  31 // J = -[u1 cross(r1, u1) ratio * u2  ratio * cross(r2, u2)]
  32 // K = J * invM * JT
  33 //   = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
  34 //
  35 // Limit:
  36 // C = maxLength - length
  37 // u = (p - s) / norm(p - s)
  38 // Cdot = -dot(u, v + cross(w, r))
  39 // K = invMass + invI * cross(r, u)^2
  40 // 0 <= impulse
  41
  42 void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
  43                                 const b2Vec2& ga1, const b2Vec2& ga2,
  44                                 const b2Vec2& anchor1, const b2Vec2& anchor2,
  45                                 float32 r)
  46 {
  47         body1 = b1;
  48         body2 = b2;
  49         groundAnchor1 = ga1;
  50         groundAnchor2 = ga2;
  51         localAnchor1 = body1->GetLocalPoint(anchor1);
  52         localAnchor2 = body2->GetLocalPoint(anchor2);
  53         b2Vec2 d1 = anchor1 - ga1;
  54         length1 = d1.Length();
  55         b2Vec2 d2 = anchor2 - ga2;
  56         length2 = d2.Length();
  57         ratio = r;
  58         b2Assert(ratio > B2_FLT_EPSILON);
  59         float32 C = length1 + ratio * length2;
  60         maxLength1 = C - ratio * b2_minPulleyLength;
  61         maxLength2 = (C - b2_minPulleyLength) / ratio;
  62 }
  63
  64 b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
  65 : b2Joint(def)
  66 {
  67         m_ground = m_body1->GetWorld()->GetGroundBody();
  68         m_groundAnchor1 = def->groundAnchor1 - m_ground->GetXForm().position;
  69         m_groundAnchor2 = def->groundAnchor2 - m_ground->GetXForm().position;
  70         m_localAnchor1 = def->localAnchor1;
  71         m_localAnchor2 = def->localAnchor2;
  72
  73         b2Assert(def->ratio != 0.0f);
  74         m_ratio = def->ratio;
  75
  76         m_constant = def->length1 + m_ratio * def->length2;
  77
  78         m_maxLength1 = b2Min(def->maxLength1, m_constant - m_ratio * b2_minPulleyLength);
  79         m_maxLength2 = b2Min(def->maxLength2, (m_constant - b2_minPulleyLength) / m_ratio);
  80
  81         m_force = 0.0f;
  82         m_limitForce1 = 0.0f;
  83         m_limitForce2 = 0.0f;
  84 }
  85
  86 void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
  87 {
  88         b2Body* b1 = m_body1;
  89         b2Body* b2 = m_body2;
  90
  91         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
  92         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
  93
  94         b2Vec2 p1 = b1->m_sweep.c + r1;
  95         b2Vec2 p2 = b2->m_sweep.c + r2;
  96
  97         b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
  98         b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
  99
 100         // Get the pulley axes.
 101         m_u1 = p1 - s1;
 102         m_u2 = p2 - s2;
 103
 104         float32 length1 = m_u1.Length();
 105         float32 length2 = m_u2.Length();
 106
 107         if (length1 > b2_linearSlop)
 108         {
 109                 m_u1 *= 1.0f / length1;
 110         }
 111         else
 112         {
 113                 m_u1.SetZero();
 114         }
 115
 116         if (length2 > b2_linearSlop)
 117         {
 118                 m_u2 *= 1.0f / length2;
 119         }
 120         else
 121         {
 122                 m_u2.SetZero();
 123         }
 124
 125         float32 C = m_constant - length1 - m_ratio * length2;
 126         if (C > 0.0f)
 127         {
 128                 m_state = e_inactiveLimit;
 129                 m_force = 0.0f;
 130         }
 131         else
 132         {
 133                 m_state = e_atUpperLimit;
 134                 m_positionImpulse = 0.0f;
 135         }
 136
 137         if (length1 < m_maxLength1)
 138         {
 139                 m_limitState1 = e_inactiveLimit;
 140                 m_limitForce1 = 0.0f;
 141         }
 142         else
 143         {
 144                 m_limitState1 = e_atUpperLimit;
 145                 m_limitPositionImpulse1 = 0.0f;
 146         }
 147
 148         if (length2 < m_maxLength2)
 149         {
 150                 m_limitState2 = e_inactiveLimit;
 151                 m_limitForce2 = 0.0f;
 152         }
 153         else
 154         {
 155                 m_limitState2 = e_atUpperLimit;
 156                 m_limitPositionImpulse2 = 0.0f;
 157         }
 158
 159         // Compute effective mass.
 160         float32 cr1u1 = b2Cross(r1, m_u1);
 161         float32 cr2u2 = b2Cross(r2, m_u2);
 162
 163         m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
 164         m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
 165         m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
 166         b2Assert(m_limitMass1 > B2_FLT_EPSILON);
 167         b2Assert(m_limitMass2 > B2_FLT_EPSILON);
 168         b2Assert(m_pulleyMass > B2_FLT_EPSILON);
 169         m_limitMass1 = 1.0f / m_limitMass1;
 170         m_limitMass2 = 1.0f / m_limitMass2;
 171         m_pulleyMass = 1.0f / m_pulleyMass;
 172
 173         if (step.warmStarting)
 174         {
 175                 // Warm starting.
 176                 b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
 177                 b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
 178                 b1->m_linearVelocity += b1->m_invMass * P1;
 179                 b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
 180                 b2->m_linearVelocity += b2->m_invMass * P2;
 181                 b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
 182         }
 183         else
 184         {
 185                 m_force = 0.0f;
 186                 m_limitForce1 = 0.0f;
 187                 m_limitForce2 = 0.0f;
 188         }
 189 }
 190
 191 void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
 192 {
 193         b2Body* b1 = m_body1;
 194         b2Body* b2 = m_body2;
 195
 196         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 197         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 198
 199         if (m_state == e_atUpperLimit)
 200         {
 201                 b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
 202                 b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
 203
 204                 float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
 205                 float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_pulleyMass * Cdot;
 206                 float32 oldForce = m_force;
 207                 m_force = b2Max(0.0f, m_force + force);
 208                 force = m_force - oldForce;
 209
 210                 b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
 211                 b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * m_ratio * force * m_u2;
 212                 b1->m_linearVelocity += b1->m_invMass * P1;
 213                 b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
 214                 b2->m_linearVelocity += b2->m_invMass * P2;
 215                 b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
 216         }
 217
 218         if (m_limitState1 == e_atUpperLimit)
 219         {
 220                 b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
 221
 222                 float32 Cdot = -b2Dot(m_u1, v1);
 223                 float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass1 * Cdot;
 224                 float32 oldForce = m_limitForce1;
 225                 m_limitForce1 = b2Max(0.0f, m_limitForce1 + force);
 226                 force = m_limitForce1 - oldForce;
 227
 228                 b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
 229                 b1->m_linearVelocity += b1->m_invMass * P1;
 230                 b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
 231         }
 232
 233         if (m_limitState2 == e_atUpperLimit)
 234         {
 235                 b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
 236
 237                 float32 Cdot = -b2Dot(m_u2, v2);
 238                 float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass2 * Cdot;
 239                 float32 oldForce = m_limitForce2;
 240                 m_limitForce2 = b2Max(0.0f, m_limitForce2 + force);
 241                 force = m_limitForce2 - oldForce;
 242
 243                 b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * force * m_u2;
 244                 b2->m_linearVelocity += b2->m_invMass * P2;
 245                 b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
 246         }
 247 }
 248
 249 bool b2PulleyJoint::SolvePositionConstraints()
 250 {
 251         b2Body* b1 = m_body1;
 252         b2Body* b2 = m_body2;
 253
 254         b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
 255         b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
 256
 257         float32 linearError = 0.0f;
 258
 259         if (m_state == e_atUpperLimit)
 260         {
 261                 b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 262                 b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 263
 264                 b2Vec2 p1 = b1->m_sweep.c + r1;
 265                 b2Vec2 p2 = b2->m_sweep.c + r2;
 266
 267                 // Get the pulley axes.
 268                 m_u1 = p1 - s1;
 269                 m_u2 = p2 - s2;
 270
 271                 float32 length1 = m_u1.Length();
 272                 float32 length2 = m_u2.Length();
 273
 274                 if (length1 > b2_linearSlop)
 275                 {
 276                         m_u1 *= 1.0f / length1;
 277                 }
 278                 else
 279                 {
 280                         m_u1.SetZero();
 281                 }
 282
 283                 if (length2 > b2_linearSlop)
 284                 {
 285                         m_u2 *= 1.0f / length2;
 286                 }
 287                 else
 288                 {
 289                         m_u2.SetZero();
 290                 }
 291
 292                 float32 C = m_constant - length1 - m_ratio * length2;
 293                 linearError = b2Max(linearError, -C);
 294
 295                 C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 296                 float32 impulse = -m_pulleyMass * C;
 297                 float32 oldImpulse = m_positionImpulse;
 298                 m_positionImpulse = b2Max(0.0f, m_positionImpulse + impulse);
 299                 impulse = m_positionImpulse - oldImpulse;
 300
 301                 b2Vec2 P1 = -impulse * m_u1;
 302                 b2Vec2 P2 = -m_ratio * impulse * m_u2;
 303
 304                 b1->m_sweep.c += b1->m_invMass * P1;
 305                 b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
 306                 b2->m_sweep.c += b2->m_invMass * P2;
 307                 b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
 308
 309                 b1->SynchronizeTransform();
 310                 b2->SynchronizeTransform();
 311         }
 312
 313         if (m_limitState1 == e_atUpperLimit)
 314         {
 315                 b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 316                 b2Vec2 p1 = b1->m_sweep.c + r1;
 317
 318                 m_u1 = p1 - s1;
 319                 float32 length1 = m_u1.Length();
 320
 321                 if (length1 > b2_linearSlop)
 322                 {
 323                         m_u1 *= 1.0f / length1;
 324                 }
 325                 else
 326                 {
 327                         m_u1.SetZero();
 328                 }
 329
 330                 float32 C = m_maxLength1 - length1;
 331                 linearError = b2Max(linearError, -C);
 332                 C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 333                 float32 impulse = -m_limitMass1 * C;
 334                 float32 oldLimitPositionImpulse = m_limitPositionImpulse1;
 335                 m_limitPositionImpulse1 = b2Max(0.0f, m_limitPositionImpulse1 + impulse);
 336                 impulse = m_limitPositionImpulse1 - oldLimitPositionImpulse;
 337
 338                 b2Vec2 P1 = -impulse * m_u1;
 339                 b1->m_sweep.c += b1->m_invMass * P1;
 340                 b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
 341
 342                 b1->SynchronizeTransform();
 343         }
 344
 345         if (m_limitState2 == e_atUpperLimit)
 346         {
 347                 b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 348                 b2Vec2 p2 = b2->m_sweep.c + r2;
 349
 350                 m_u2 = p2 - s2;
 351                 float32 length2 = m_u2.Length();
 352
 353                 if (length2 > b2_linearSlop)
 354                 {
 355                         m_u2 *= 1.0f / length2;
 356                 }
 357                 else
 358                 {
 359                         m_u2.SetZero();
 360                 }
 361
 362                 float32 C = m_maxLength2 - length2;
 363                 linearError = b2Max(linearError, -C);
 364                 C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 365                 float32 impulse = -m_limitMass2 * C;
 366                 float32 oldLimitPositionImpulse = m_limitPositionImpulse2;
 367                 m_limitPositionImpulse2 = b2Max(0.0f, m_limitPositionImpulse2 + impulse);
 368                 impulse = m_limitPositionImpulse2 - oldLimitPositionImpulse;
 369
 370                 b2Vec2 P2 = -impulse * m_u2;
 371                 b2->m_sweep.c += b2->m_invMass * P2;
 372                 b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
 373
 374                 b2->SynchronizeTransform();
 375         }
 376
 377         return linearError < b2_linearSlop;
 378 }
 379
 380 b2Vec2 b2PulleyJoint::GetAnchor1() const
 381 {
 382         return m_body1->GetWorldPoint(m_localAnchor1);
 383 }
 384
 385 b2Vec2 b2PulleyJoint::GetAnchor2() const
 386 {
 387         return m_body2->GetWorldPoint(m_localAnchor2);
 388 }
 389
 390 b2Vec2 b2PulleyJoint::GetReactionForce() const
 391 {
 392         b2Vec2 F = B2FORCE_SCALE(m_force) * m_u2;
 393         return F;
 394 }
 395
 396 float32 b2PulleyJoint::GetReactionTorque() const
 397 {
 398         return 0.0f;
 399 }
 400
 401 b2Vec2 b2PulleyJoint::GetGroundAnchor1() const
 402 {
 403         return m_ground->GetXForm().position + m_groundAnchor1;
 404 }
 405
 406 b2Vec2 b2PulleyJoint::GetGroundAnchor2() const
 407 {
 408         return m_ground->GetXForm().position + m_groundAnchor2;
 409 }
 410
 411 float32 b2PulleyJoint::GetLength1() const
 412 {
 413         b2Vec2 p = m_body1->GetWorldPoint(m_localAnchor1);
 414         b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor1;
 415         b2Vec2 d = p - s;
 416         return d.Length();
 417 }
 418
 419 float32 b2PulleyJoint::GetLength2() const
 420 {
 421         b2Vec2 p = m_body2->GetWorldPoint(m_localAnchor2);
 422         b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor2;
 423         b2Vec2 d = p - s;
 424         return d.Length();
 425 }
 426
 427 float32 b2PulleyJoint::GetRatio() const
 428 {
 429         return m_ratio;
 430 }