git.maemo.org Git - blok/blob - Box2D/Source/Dynamics/Contacts/b2ContactSolver.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 "b2ContactSolver.h"
  20 #include "b2Contact.h"
  21 #include "../b2Body.h"
  22 #include "../b2World.h"
  23 #include "../../Common/b2StackAllocator.h"
  24
  25 b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
  26 {
  27         m_step = step;
  28         m_allocator = allocator;
  29
  30         m_constraintCount = 0;
  31         for (int32 i = 0; i < contactCount; ++i)
  32         {
  33                 b2Assert(contacts[i]->IsSolid());
  34                 m_constraintCount += contacts[i]->GetManifoldCount();
  35         }
  36
  37         m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
  38
  39         int32 count = 0;
  40         for (int32 i = 0; i < contactCount; ++i)
  41         {
  42                 b2Contact* contact = contacts[i];
  43
  44                 b2Body* b1 = contact->m_shape1->GetBody();
  45                 b2Body* b2 = contact->m_shape2->GetBody();
  46                 int32 manifoldCount = contact->GetManifoldCount();
  47                 b2Manifold* manifolds = contact->GetManifolds();
  48                 float32 friction = contact->m_friction;
  49                 float32 restitution = contact->m_restitution;
  50
  51                 b2Vec2 v1 = b1->m_linearVelocity;
  52                 b2Vec2 v2 = b2->m_linearVelocity;
  53                 float32 w1 = b1->m_angularVelocity;
  54                 float32 w2 = b2->m_angularVelocity;
  55
  56                 for (int32 j = 0; j < manifoldCount; ++j)
  57                 {
  58                         b2Manifold* manifold = manifolds + j;
  59
  60                         b2Assert(manifold->pointCount > 0);
  61
  62                         const b2Vec2 normal = manifold->normal;
  63
  64                         b2Assert(count < m_constraintCount);
  65                         b2ContactConstraint* c = m_constraints + count;
  66                         c->body1 = b1;
  67                         c->body2 = b2;
  68                         c->manifold = manifold;
  69                         c->normal = normal;
  70                         c->pointCount = manifold->pointCount;
  71                         c->friction = friction;
  72                         c->restitution = restitution;
  73
  74                         for (int32 k = 0; k < c->pointCount; ++k)
  75                         {
  76                                 b2ManifoldPoint* cp = manifold->points + k;
  77                                 b2ContactConstraintPoint* ccp = c->points + k;
  78
  79                                 ccp->normalImpulse = cp->normalImpulse;
  80                                 ccp->tangentImpulse = cp->tangentImpulse;
  81                                 ccp->separation = cp->separation;
  82                                 ccp->positionImpulse = 0.0f;
  83
  84                                 ccp->localAnchor1 = cp->localPoint1;
  85                                 ccp->localAnchor2 = cp->localPoint2;
  86                                 ccp->r1 = b2Mul(b1->GetXForm().R, cp->localPoint1 - b1->GetLocalCenter());
  87                                 ccp->r2 = b2Mul(b2->GetXForm().R, cp->localPoint2 - b2->GetLocalCenter());
  88
  89                                 float32 r1Sqr = b2Dot(ccp->r1, ccp->r1);
  90                                 float32 r2Sqr = b2Dot(ccp->r2, ccp->r2);
  91                                 float32 rn1 = b2Dot(ccp->r1, normal);
  92                                 float32 rn2 = b2Dot(ccp->r2, normal);
  93
  94                                 float32 kNormal = b1->m_invMass + b2->m_invMass;
  95                                 kNormal += b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_invI * (r2Sqr - rn2 * rn2);
  96
  97                                 b2Assert(kNormal > B2_FLT_EPSILON);
  98                                 ccp->normalMass = 1.0f / kNormal;
  99
 100                                 float32 kEqualized = b1->m_mass * b1->m_invMass + b2->m_mass * b2->m_invMass;
 101                                 kEqualized += b1->m_mass * b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_mass * b2->m_invI * (r2Sqr - rn2 * rn2);
 102
 103                                 b2Assert(kEqualized > B2_FLT_EPSILON);
 104                                 ccp->equalizedMass = 1.0f / kEqualized;
 105
 106                                 b2Vec2 tangent = b2Cross(normal, 1.0f);
 107
 108                                 float32 rt1 = b2Dot(ccp->r1, tangent);
 109                                 float32 rt2 = b2Dot(ccp->r2, tangent);
 110                                 float32 kTangent = b1->m_invMass + b2->m_invMass;
 111                                 kTangent += b1->m_invI * (r1Sqr - rt1 * rt1) + b2->m_invI * (r2Sqr - rt2 * rt2);
 112
 113                                 b2Assert(kTangent > B2_FLT_EPSILON);
 114                                 ccp->tangentMass = 1.0f /  kTangent;
 115
 116                                 // Setup a velocity bias for restitution.
 117                                 ccp->velocityBias = 0.0f;
 118                                 if (ccp->separation > 0.0f)
 119                                 {
 120                                         ccp->velocityBias = -60.0f * ccp->separation; // TODO_ERIN b2TimeStep
 121                                 }
 122
 123                                 float32 vRel = b2Dot(c->normal, v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1));
 124                                 if (vRel < -b2_velocityThreshold)
 125                                 {
 126                                         ccp->velocityBias += -c->restitution * vRel;
 127                                 }
 128                         }
 129
 130                         ++count;
 131                 }
 132         }
 133
 134         b2Assert(count == m_constraintCount);
 135 }
 136
 137 b2ContactSolver::~b2ContactSolver()
 138 {
 139         m_allocator->Free(m_constraints);
 140 }
 141
 142 void b2ContactSolver::InitVelocityConstraints(const b2TimeStep& step)
 143 {
 144         // Warm start.
 145         for (int32 i = 0; i < m_constraintCount; ++i)
 146         {
 147                 b2ContactConstraint* c = m_constraints + i;
 148
 149                 b2Body* b1 = c->body1;
 150                 b2Body* b2 = c->body2;
 151                 float32 invMass1 = b1->m_invMass;
 152                 float32 invI1 = b1->m_invI;
 153                 float32 invMass2 = b2->m_invMass;
 154                 float32 invI2 = b2->m_invI;
 155                 b2Vec2 normal = c->normal;
 156                 b2Vec2 tangent = b2Cross(normal, 1.0f);
 157
 158                 if (step.warmStarting)
 159                 {
 160                         for (int32 j = 0; j < c->pointCount; ++j)
 161                         {
 162                                 b2ContactConstraintPoint* ccp = c->points + j;
 163                                 ccp->normalImpulse *= step.dtRatio;
 164                                 ccp->tangentImpulse *= step.dtRatio;
 165                                 b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
 166                                 b1->m_angularVelocity -= invI1 * b2Cross(ccp->r1, P);
 167                                 b1->m_linearVelocity -= invMass1 * P;
 168                                 b2->m_angularVelocity += invI2 * b2Cross(ccp->r2, P);
 169                                 b2->m_linearVelocity += invMass2 * P;
 170                         }
 171                 }
 172                 else
 173                 {
 174                         for (int32 j = 0; j < c->pointCount; ++j)
 175                         {
 176                                 b2ContactConstraintPoint* ccp = c->points + j;
 177                                 ccp->normalImpulse = 0.0f;
 178                                 ccp->tangentImpulse = 0.0f;
 179                         }
 180                 }
 181         }
 182 }
 183
 184 void b2ContactSolver::SolveVelocityConstraints()
 185 {
 186         for (int32 i = 0; i < m_constraintCount; ++i)
 187         {
 188                 b2ContactConstraint* c = m_constraints + i;
 189                 b2Body* b1 = c->body1;
 190                 b2Body* b2 = c->body2;
 191                 float32 w1 = b1->m_angularVelocity;
 192                 float32 w2 = b2->m_angularVelocity;
 193                 b2Vec2 v1 = b1->m_linearVelocity;
 194                 b2Vec2 v2 = b2->m_linearVelocity;
 195                 float32 invMass1 = b1->m_invMass;
 196                 float32 invI1 = b1->m_invI;
 197                 float32 invMass2 = b2->m_invMass;
 198                 float32 invI2 = b2->m_invI;
 199                 b2Vec2 normal = c->normal;
 200                 b2Vec2 tangent = b2Cross(normal, 1.0f);
 201                 float32 friction = c->friction;
 202 //#define DEFERRED_UPDATE
 203 #ifdef DEFERRED_UPDATE
 204                 b2Vec2 b1_linearVelocity = b1->m_linearVelocity;
 205                 float32 b1_angularVelocity = b1->m_angularVelocity;
 206                 b2Vec2 b2_linearVelocity = b2->m_linearVelocity;
 207                 float32 b2_angularVelocity = b2->m_angularVelocity;
 208 #endif
 209                 // Solve normal constraints
 210                 for (int32 j = 0; j < c->pointCount; ++j)
 211                 {
 212                         b2ContactConstraintPoint* ccp = c->points + j;
 213
 214                         // Relative velocity at contact
 215                         b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
 216
 217                         // Compute normal impulse
 218                         float32 vn = b2Dot(dv, normal);
 219                         float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
 220
 221                         // b2Clamp the accumulated impulse
 222                         float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
 223                         lambda = newImpulse - ccp->normalImpulse;
 224
 225                         // Apply contact impulse
 226                         b2Vec2 P = lambda * normal;
 227 #ifdef DEFERRED_UPDATE
 228                         b1_linearVelocity -= invMass1 * P;
 229                         b1_angularVelocity -= invI1 * b2Cross(r1, P);
 230
 231                         b2_linearVelocity += invMass2 * P;
 232                         b2_angularVelocity += invI2 * b2Cross(r2, P);
 233 #else
 234                         v1 -= invMass1 * P;
 235                         w1 -= invI1 * b2Cross(ccp->r1, P);
 236
 237                         v2 += invMass2 * P;
 238                         w2 += invI2 * b2Cross(ccp->r2, P);
 239 #endif
 240                         ccp->normalImpulse = newImpulse;
 241                 }
 242
 243 #ifdef DEFERRED_UPDATE
 244                 b1->m_linearVelocity = b1_linearVelocity;
 245                 b1->m_angularVelocity = b1_angularVelocity;
 246                 b2->m_linearVelocity = b2_linearVelocity;
 247                 b2->m_angularVelocity = b2_angularVelocity;
 248 #endif
 249                 // Solve tangent constraints
 250                 for (int32 j = 0; j < c->pointCount; ++j)
 251                 {
 252                         b2ContactConstraintPoint* ccp = c->points + j;
 253
 254                         // Relative velocity at contact
 255                         b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
 256
 257                         // Compute tangent force
 258                         float32 vt = b2Dot(dv, tangent);
 259                         float32 lambda = ccp->tangentMass * (-vt);
 260
 261                         // b2Clamp the accumulated force
 262                         float32 maxFriction = friction * ccp->normalImpulse;
 263                         float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
 264                         lambda = newImpulse - ccp->tangentImpulse;
 265
 266                         // Apply contact impulse
 267                         b2Vec2 P = lambda * tangent;
 268
 269                         v1 -= invMass1 * P;
 270                         w1 -= invI1 * b2Cross(ccp->r1, P);
 271
 272                         v2 += invMass2 * P;
 273                         w2 += invI2 * b2Cross(ccp->r2, P);
 274
 275                         ccp->tangentImpulse = newImpulse;
 276                 }
 277
 278                 b1->m_linearVelocity = v1;
 279                 b1->m_angularVelocity = w1;
 280                 b2->m_linearVelocity = v2;
 281                 b2->m_angularVelocity = w2;
 282         }
 283 }
 284
 285 void b2ContactSolver::FinalizeVelocityConstraints()
 286 {
 287         for (int32 i = 0; i < m_constraintCount; ++i)
 288         {
 289                 b2ContactConstraint* c = m_constraints + i;
 290                 b2Manifold* m = c->manifold;
 291
 292                 for (int32 j = 0; j < c->pointCount; ++j)
 293                 {
 294                         m->points[j].normalImpulse = c->points[j].normalImpulse;
 295                         m->points[j].tangentImpulse = c->points[j].tangentImpulse;
 296                 }
 297         }
 298 }
 299
 300 bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
 301 {
 302         float32 minSeparation = 0.0f;
 303
 304         for (int32 i = 0; i < m_constraintCount; ++i)
 305         {
 306                 b2ContactConstraint* c = m_constraints + i;
 307                 b2Body* b1 = c->body1;
 308                 b2Body* b2 = c->body2;
 309                 float32 invMass1 = b1->m_mass * b1->m_invMass;
 310                 float32 invI1 = b1->m_mass * b1->m_invI;
 311                 float32 invMass2 = b2->m_mass * b2->m_invMass;
 312                 float32 invI2 = b2->m_mass * b2->m_invI;
 313
 314                 b2Vec2 normal = c->normal;
 315
 316                 // Solver normal constraints
 317                 for (int32 j = 0; j < c->pointCount; ++j)
 318                 {
 319                         b2ContactConstraintPoint* ccp = c->points + j;
 320
 321                         b2Vec2 r1 = b2Mul(b1->GetXForm().R, ccp->localAnchor1 - b1->GetLocalCenter());
 322                         b2Vec2 r2 = b2Mul(b2->GetXForm().R, ccp->localAnchor2 - b2->GetLocalCenter());
 323
 324                         b2Vec2 p1 = b1->m_sweep.c + r1;
 325                         b2Vec2 p2 = b2->m_sweep.c + r2;
 326                         b2Vec2 dp = p2 - p1;
 327
 328                         // Approximate the current separation.
 329                         float32 separation = b2Dot(dp, normal) + ccp->separation;
 330
 331                         // Track max constraint error.
 332                         minSeparation = b2Min(minSeparation, separation);
 333
 334                         // Prevent large corrections and allow slop.
 335                         float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 336
 337                         // Compute normal impulse
 338                         float32 dImpulse = -ccp->equalizedMass * C;
 339
 340                         // b2Clamp the accumulated impulse
 341                         float32 impulse0 = ccp->positionImpulse;
 342                         ccp->positionImpulse = b2Max(impulse0 + dImpulse, 0.0f);
 343                         dImpulse = ccp->positionImpulse - impulse0;
 344
 345                         b2Vec2 impulse = dImpulse * normal;
 346
 347                         b1->m_sweep.c -= invMass1 * impulse;
 348                         b1->m_sweep.a -= invI1 * b2Cross(r1, impulse);
 349                         b1->SynchronizeTransform();
 350
 351                         b2->m_sweep.c += invMass2 * impulse;
 352                         b2->m_sweep.a += invI2 * b2Cross(r2, impulse);
 353                         b2->SynchronizeTransform();
 354                 }
 355         }
 356
 357         // We can't expect minSpeparation >= -b2_linearSlop because we don't
 358         // push the separation above -b2_linearSlop.
 359         return minSeparation >= -1.5f * b2_linearSlop;
 360 }