()
| 332 | } |
| 333 | |
| 334 | func (solver *B2ContactSolver) SolveVelocityConstraints() { |
| 335 | for i := 0; i < solver.M_count; i++ { |
| 336 | vc := &solver.M_velocityConstraints[i] |
| 337 | |
| 338 | indexA := vc.IndexA |
| 339 | indexB := vc.IndexB |
| 340 | mA := vc.InvMassA |
| 341 | iA := vc.InvIA |
| 342 | mB := vc.InvMassB |
| 343 | iB := vc.InvIB |
| 344 | pointCount := vc.PointCount |
| 345 | |
| 346 | vA := solver.M_velocities[indexA].V |
| 347 | wA := solver.M_velocities[indexA].W |
| 348 | vB := solver.M_velocities[indexB].V |
| 349 | wB := solver.M_velocities[indexB].W |
| 350 | |
| 351 | normal := vc.Normal |
| 352 | tangent := B2Vec2CrossVectorScalar(normal, 1.0) |
| 353 | friction := vc.Friction |
| 354 | |
| 355 | B2Assert(pointCount == 1 || pointCount == 2) |
| 356 | |
| 357 | // Solve tangent constraints first because non-penetration is more important |
| 358 | // than friction. |
| 359 | for j := 0; j < pointCount; j++ { |
| 360 | vcp := &vc.Points[j] |
| 361 | |
| 362 | // Relative velocity at contact |
| 363 | dv := B2Vec2Add( |
| 364 | vB, |
| 365 | B2Vec2Sub( |
| 366 | B2Vec2Sub( |
| 367 | B2Vec2CrossScalarVector(wB, vcp.RB), |
| 368 | vA, |
| 369 | ), |
| 370 | B2Vec2CrossScalarVector(wA, vcp.RA), |
| 371 | ), |
| 372 | ) |
| 373 | |
| 374 | // Compute tangent force |
| 375 | vt := B2Vec2Dot(dv, tangent) - vc.TangentSpeed |
| 376 | lambda := vcp.TangentMass * (-vt) |
| 377 | |
| 378 | // b2Clamp the accumulated force |
| 379 | maxFriction := friction * vcp.NormalImpulse |
| 380 | newImpulse := B2FloatClamp(vcp.TangentImpulse+lambda, -maxFriction, maxFriction) |
| 381 | lambda = newImpulse - vcp.TangentImpulse |
| 382 | vcp.TangentImpulse = newImpulse |
| 383 | |
| 384 | // Apply contact impulse |
| 385 | P := B2Vec2MulScalar(lambda, tangent) |
| 386 | |
| 387 | vA.OperatorMinusInplace(B2Vec2MulScalar(mA, P)) |
| 388 | wA -= iA * B2Vec2Cross(vcp.RA, P) |
| 389 | |
| 390 | vB.OperatorPlusInplace(B2Vec2MulScalar(mB, P)) |
| 391 | wB += iB * B2Vec2Cross(vcp.RB, P) |
nothing calls this directly
no test coverage detected