git.maemo.org Git - blok/blob - Box2D/Source/Dynamics/Joints/b2PrismaticJoint.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 "b2PrismaticJoint.h"
  20 #include "../b2Body.h"
  21 #include "../b2World.h"
  22
  23 // Linear constraint (point-to-line)
  24 // d = p2 - p1 = x2 + r2 - x1 - r1
  25 // C = dot(ay1, d)
  26 // Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
  27 //      = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
  28 // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
  29 //
  30 // Angular constraint
  31 // C = a2 - a1 + a_initial
  32 // Cdot = w2 - w1
  33 // J = [0 0 -1 0 0 1]
  34
  35 // Motor/Limit linear constraint
  36 // C = dot(ax1, d)
  37 // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
  38 // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
  39
  40 void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
  41 {
  42         body1 = b1;
  43         body2 = b2;
  44         localAnchor1 = body1->GetLocalPoint(anchor);
  45         localAnchor2 = body2->GetLocalPoint(anchor);
  46         localAxis1 = body1->GetLocalVector(axis);
  47         referenceAngle = body2->GetAngle() - body1->GetAngle();
  48 }
  49
  50 b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
  51 : b2Joint(def)
  52 {
  53         m_localAnchor1 = def->localAnchor1;
  54         m_localAnchor2 = def->localAnchor2;
  55         m_localXAxis1 = def->localAxis1;
  56         m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
  57         m_refAngle = def->referenceAngle;
  58
  59         m_linearJacobian.SetZero();
  60         m_linearMass = 0.0f;
  61         m_force = 0.0f;
  62
  63         m_angularMass = 0.0f;
  64         m_torque = 0.0f;
  65
  66         m_motorJacobian.SetZero();
  67         m_motorMass = 0.0;
  68         m_motorForce = 0.0f;
  69         m_limitForce = 0.0f;
  70         m_limitPositionImpulse = 0.0f;
  71
  72         m_lowerTranslation = def->lowerTranslation;
  73         m_upperTranslation = def->upperTranslation;
  74         m_maxMotorForce = B2FORCE_INV_SCALE(def->maxMotorForce);
  75         m_motorSpeed = def->motorSpeed;
  76         m_enableLimit = def->enableLimit;
  77         m_enableMotor = def->enableMotor;
  78 }
  79
  80 void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
  81 {
  82         b2Body* b1 = m_body1;
  83         b2Body* b2 = m_body2;
  84
  85         // Compute the effective masses.
  86         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
  87         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
  88
  89         float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
  90         float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
  91
  92         // Compute point to line constraint effective mass.
  93         // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
  94         b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
  95         b2Vec2 e = b2->m_sweep.c + r2 - b1->m_sweep.c;  // e = d + r1
  96
  97         m_linearJacobian.Set(-ay1, -b2Cross(e, ay1), ay1, b2Cross(r2, ay1));
  98         m_linearMass =  invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 +
  99                                         invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
 100         b2Assert(m_linearMass > B2_FLT_EPSILON);
 101         m_linearMass = 1.0f / m_linearMass;
 102
 103         // Compute angular constraint effective mass.
 104         m_angularMass = invI1 + invI2;
 105         if (m_angularMass > B2_FLT_EPSILON)
 106         {
 107                 m_angularMass = 1.0f / m_angularMass;
 108         }
 109
 110         // Compute motor and limit terms.
 111         if (m_enableLimit || m_enableMotor)
 112         {
 113                 // The motor and limit share a Jacobian and effective mass.
 114                 b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
 115                 m_motorJacobian.Set(-ax1, -b2Cross(e, ax1), ax1, b2Cross(r2, ax1));
 116                 m_motorMass =   invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 +
 117                                                 invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
 118                 b2Assert(m_motorMass > B2_FLT_EPSILON);
 119                 m_motorMass = 1.0f / m_motorMass;
 120
 121                 if (m_enableLimit)
 122                 {
 123                         b2Vec2 d = e - r1;      // p2 - p1
 124                         float32 jointTranslation = b2Dot(ax1, d);
 125                         if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
 126                         {
 127                                 m_limitState = e_equalLimits;
 128                         }
 129                         else if (jointTranslation <= m_lowerTranslation)
 130                         {
 131                                 if (m_limitState != e_atLowerLimit)
 132                                 {
 133                                         m_limitForce = 0.0f;
 134                                 }
 135                                 m_limitState = e_atLowerLimit;
 136                         }
 137                         else if (jointTranslation >= m_upperTranslation)
 138                         {
 139                                 if (m_limitState != e_atUpperLimit)
 140                                 {
 141                                         m_limitForce = 0.0f;
 142                                 }
 143                                 m_limitState = e_atUpperLimit;
 144                         }
 145                         else
 146                         {
 147                                 m_limitState = e_inactiveLimit;
 148                                 m_limitForce = 0.0f;
 149                         }
 150                 }
 151         }
 152
 153         if (m_enableMotor == false)
 154         {
 155                 m_motorForce = 0.0f;
 156         }
 157
 158         if (m_enableLimit == false)
 159         {
 160                 m_limitForce = 0.0f;
 161         }
 162
 163         if (step.warmStarting)
 164         {
 165                 b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear1 + (m_motorForce + m_limitForce) * m_motorJacobian.linear1);
 166                 b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear2 + (m_motorForce + m_limitForce) * m_motorJacobian.linear2);
 167                 float32 L1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
 168                 float32 L2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
 169
 170                 b1->m_linearVelocity += invMass1 * P1;
 171                 b1->m_angularVelocity += invI1 * L1;
 172
 173                 b2->m_linearVelocity += invMass2 * P2;
 174                 b2->m_angularVelocity += invI2 * L2;
 175         }
 176         else
 177         {
 178                 m_force = 0.0f;
 179                 m_torque = 0.0f;
 180                 m_limitForce = 0.0f;
 181                 m_motorForce = 0.0f;
 182         }
 183
 184         m_limitPositionImpulse = 0.0f;
 185 }
 186
 187 void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
 188 {
 189         b2Body* b1 = m_body1;
 190         b2Body* b2 = m_body2;
 191
 192         float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
 193         float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
 194
 195         // Solve linear constraint.
 196         float32 linearCdot = m_linearJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
 197         float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_linearMass * linearCdot;
 198         m_force += force;
 199
 200         float32 P = B2FORCE_SCALE(step.dt) * force;
 201         b1->m_linearVelocity += (invMass1 * P) * m_linearJacobian.linear1;
 202         b1->m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
 203
 204         b2->m_linearVelocity += (invMass2 * P) * m_linearJacobian.linear2;
 205         b2->m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
 206
 207         // Solve angular constraint.
 208         float32 angularCdot = b2->m_angularVelocity - b1->m_angularVelocity;
 209         float32 torque = -B2FORCE_INV_SCALE(step.inv_dt) * m_angularMass * angularCdot;
 210         m_torque += torque;
 211
 212         float32 L = B2FORCE_SCALE(step.dt) * torque;
 213         b1->m_angularVelocity -= invI1 * L;
 214         b2->m_angularVelocity += invI2 * L;
 215
 216         // Solve linear motor constraint.
 217         if (m_enableMotor && m_limitState != e_equalLimits)
 218         {
 219                 float32 motorCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity) - m_motorSpeed;
 220                 float32 motorForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * motorCdot;
 221                 float32 oldMotorForce = m_motorForce;
 222                 m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
 223                 motorForce = m_motorForce - oldMotorForce;
 224
 225                 float32 P = B2FORCE_SCALE(step.dt) * motorForce;
 226                 b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
 227                 b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
 228
 229                 b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
 230                 b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
 231         }
 232
 233         // Solve linear limit constraint.
 234         if (m_enableLimit && m_limitState != e_inactiveLimit)
 235         {
 236                 float32 limitCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
 237                 float32 limitForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * limitCdot;
 238
 239                 if (m_limitState == e_equalLimits)
 240                 {
 241                         m_limitForce += limitForce;
 242                 }
 243                 else if (m_limitState == e_atLowerLimit)
 244                 {
 245                         float32 oldLimitForce = m_limitForce;
 246                         m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
 247                         limitForce = m_limitForce - oldLimitForce;
 248                 }
 249                 else if (m_limitState == e_atUpperLimit)
 250                 {
 251                         float32 oldLimitForce = m_limitForce;
 252                         m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
 253                         limitForce = m_limitForce - oldLimitForce;
 254                 }
 255
 256                 float32 P = B2FORCE_SCALE(step.dt) * limitForce;
 257
 258                 b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
 259                 b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
 260
 261                 b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
 262                 b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
 263         }
 264 }
 265
 266 bool b2PrismaticJoint::SolvePositionConstraints()
 267 {
 268         b2Body* b1 = m_body1;
 269         b2Body* b2 = m_body2;
 270
 271         float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
 272         float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
 273
 274         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 275         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 276         b2Vec2 p1 = b1->m_sweep.c + r1;
 277         b2Vec2 p2 = b2->m_sweep.c + r2;
 278         b2Vec2 d = p2 - p1;
 279         b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
 280
 281         // Solve linear (point-to-line) constraint.
 282         float32 linearC = b2Dot(ay1, d);
 283         // Prevent overly large corrections.
 284         linearC = b2Clamp(linearC, -b2_maxLinearCorrection, b2_maxLinearCorrection);
 285         float32 linearImpulse = -m_linearMass * linearC;
 286
 287         b1->m_sweep.c += (invMass1 * linearImpulse) * m_linearJacobian.linear1;
 288         b1->m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
 289         //b1->SynchronizeTransform(); // updated by angular constraint
 290         b2->m_sweep.c += (invMass2 * linearImpulse) * m_linearJacobian.linear2;
 291         b2->m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
 292         //b2->SynchronizeTransform(); // updated by angular constraint
 293
 294         float32 positionError = b2Abs(linearC);
 295
 296         // Solve angular constraint.
 297         float32 angularC = b2->m_sweep.a - b1->m_sweep.a - m_refAngle;
 298         // Prevent overly large corrections.
 299         angularC = b2Clamp(angularC, -b2_maxAngularCorrection, b2_maxAngularCorrection);
 300         float32 angularImpulse = -m_angularMass * angularC;
 301
 302         b1->m_sweep.a -= b1->m_invI * angularImpulse;
 303         b2->m_sweep.a += b2->m_invI * angularImpulse;
 304
 305         b1->SynchronizeTransform();
 306         b2->SynchronizeTransform();
 307
 308         float32 angularError = b2Abs(angularC);
 309
 310         // Solve linear limit constraint.
 311         if (m_enableLimit && m_limitState != e_inactiveLimit)
 312         {
 313                 b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 314                 b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 315                 b2Vec2 p1 = b1->m_sweep.c + r1;
 316                 b2Vec2 p2 = b2->m_sweep.c + r2;
 317                 b2Vec2 d = p2 - p1;
 318                 b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
 319
 320                 float32 translation = b2Dot(ax1, d);
 321                 float32 limitImpulse = 0.0f;
 322
 323                 if (m_limitState == e_equalLimits)
 324                 {
 325                         // Prevent large angular corrections
 326                         float32 limitC = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
 327                         limitImpulse = -m_motorMass * limitC;
 328                         positionError = b2Max(positionError, b2Abs(angularC));
 329                 }
 330                 else if (m_limitState == e_atLowerLimit)
 331                 {
 332                         float32 limitC = translation - m_lowerTranslation;
 333                         positionError = b2Max(positionError, -limitC);
 334
 335                         // Prevent large linear corrections and allow some slop.
 336                         limitC = b2Clamp(limitC + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 337                         limitImpulse = -m_motorMass * limitC;
 338                         float32 oldLimitImpulse = m_limitPositionImpulse;
 339                         m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
 340                         limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
 341                 }
 342                 else if (m_limitState == e_atUpperLimit)
 343                 {
 344                         float32 limitC = translation - m_upperTranslation;
 345                         positionError = b2Max(positionError, limitC);
 346
 347                         // Prevent large linear corrections and allow some slop.
 348                         limitC = b2Clamp(limitC - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
 349                         limitImpulse = -m_motorMass * limitC;
 350                         float32 oldLimitImpulse = m_limitPositionImpulse;
 351                         m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
 352                         limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
 353                 }
 354
 355                 b1->m_sweep.c += (invMass1 * limitImpulse) * m_motorJacobian.linear1;
 356                 b1->m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
 357                 b2->m_sweep.c += (invMass2 * limitImpulse) * m_motorJacobian.linear2;
 358                 b2->m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
 359
 360                 b1->SynchronizeTransform();
 361                 b2->SynchronizeTransform();
 362         }
 363
 364         return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
 365 }
 366
 367 b2Vec2 b2PrismaticJoint::GetAnchor1() const
 368 {
 369         return m_body1->GetWorldPoint(m_localAnchor1);
 370 }
 371
 372 b2Vec2 b2PrismaticJoint::GetAnchor2() const
 373 {
 374         return m_body2->GetWorldPoint(m_localAnchor2);
 375 }
 376
 377 b2Vec2 b2PrismaticJoint::GetReactionForce() const
 378 {
 379         b2Vec2 ax1 = b2Mul(m_body1->GetXForm().R, m_localXAxis1);
 380         b2Vec2 ay1 = b2Mul(m_body1->GetXForm().R, m_localYAxis1);
 381
 382         return B2FORCE_SCALE(float32(1.0))*(m_limitForce * ax1 + m_force * ay1);
 383 }
 384
 385 float32 b2PrismaticJoint::GetReactionTorque() const
 386 {
 387         return B2FORCE_SCALE(m_torque);
 388 }
 389
 390 float32 b2PrismaticJoint::GetJointTranslation() const
 391 {
 392         b2Body* b1 = m_body1;
 393         b2Body* b2 = m_body2;
 394
 395         b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
 396         b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
 397         b2Vec2 d = p2 - p1;
 398         b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
 399
 400         float32 translation = b2Dot(d, axis);
 401         return translation;
 402 }
 403
 404 float32 b2PrismaticJoint::GetJointSpeed() const
 405 {
 406         b2Body* b1 = m_body1;
 407         b2Body* b2 = m_body2;
 408
 409         b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 410         b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 411         b2Vec2 p1 = b1->m_sweep.c + r1;
 412         b2Vec2 p2 = b2->m_sweep.c + r2;
 413         b2Vec2 d = p2 - p1;
 414         b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
 415
 416         b2Vec2 v1 = b1->m_linearVelocity;
 417         b2Vec2 v2 = b2->m_linearVelocity;
 418         float32 w1 = b1->m_angularVelocity;
 419         float32 w2 = b2->m_angularVelocity;
 420
 421         float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
 422         return speed;
 423 }
 424
 425 bool b2PrismaticJoint::IsLimitEnabled() const
 426 {
 427         return m_enableLimit;
 428 }
 429
 430 void b2PrismaticJoint::EnableLimit(bool flag)
 431 {
 432         m_enableLimit = flag;
 433 }
 434
 435 float32 b2PrismaticJoint::GetLowerLimit() const
 436 {
 437         return m_lowerTranslation;
 438 }
 439
 440 float32 b2PrismaticJoint::GetUpperLimit() const
 441 {
 442         return m_upperTranslation;
 443 }
 444
 445 void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
 446 {
 447         b2Assert(lower <= upper);
 448         m_lowerTranslation = lower;
 449         m_upperTranslation = upper;
 450 }
 451
 452 bool b2PrismaticJoint::IsMotorEnabled() const
 453 {
 454         return m_enableMotor;
 455 }
 456
 457 void b2PrismaticJoint::EnableMotor(bool flag)
 458 {
 459         m_enableMotor = flag;
 460 }
 461
 462 void b2PrismaticJoint::SetMotorSpeed(float32 speed)
 463 {
 464         m_motorSpeed = speed;
 465 }
 466
 467 void b2PrismaticJoint::SetMaxMotorForce(float32 force)
 468 {
 469         m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
 470 }
 471
 472 float32 b2PrismaticJoint::GetMotorForce() const
 473 {
 474         return m_motorForce;
 475 }
 476
 477
 478