()
| 295 | } |
| 296 | |
| 297 | func (solver *B2ContactSolver) WarmStart() { |
| 298 | // Warm start. |
| 299 | for i := 0; i < solver.M_count; i++ { |
| 300 | vc := &solver.M_velocityConstraints[i] |
| 301 | |
| 302 | indexA := vc.IndexA |
| 303 | indexB := vc.IndexB |
| 304 | mA := vc.InvMassA |
| 305 | iA := vc.InvIA |
| 306 | mB := vc.InvMassB |
| 307 | iB := vc.InvIB |
| 308 | pointCount := vc.PointCount |
| 309 | |
| 310 | vA := solver.M_velocities[indexA].V |
| 311 | wA := solver.M_velocities[indexA].W |
| 312 | vB := solver.M_velocities[indexB].V |
| 313 | wB := solver.M_velocities[indexB].W |
| 314 | |
| 315 | normal := vc.Normal |
| 316 | tangent := B2Vec2CrossVectorScalar(normal, 1.0) |
| 317 | |
| 318 | for j := 0; j < pointCount; j++ { |
| 319 | vcp := &vc.Points[j] |
| 320 | P := B2Vec2Add(B2Vec2MulScalar(vcp.NormalImpulse, normal), B2Vec2MulScalar(vcp.TangentImpulse, tangent)) |
| 321 | wA -= iA * B2Vec2Cross(vcp.RA, P) |
| 322 | vA.OperatorMinusInplace(B2Vec2MulScalar(mA, P)) |
| 323 | wB += iB * B2Vec2Cross(vcp.RB, P) |
| 324 | vB.OperatorPlusInplace(B2Vec2MulScalar(mB, P)) |
| 325 | } |
| 326 | |
| 327 | solver.M_velocities[indexA].V = vA |
| 328 | solver.M_velocities[indexA].W = wA |
| 329 | solver.M_velocities[indexB].V = vB |
| 330 | solver.M_velocities[indexB].W = wB |
| 331 | } |
| 332 | } |
| 333 | |
| 334 | func (solver *B2ContactSolver) SolveVelocityConstraints() { |
| 335 | for i := 0; i < solver.M_count; i++ { |
no test coverage detected