Sequential position solver for position constraints.
(toiIndexA int, toiIndexB int)
| 816 | |
| 817 | // Sequential position solver for position constraints. |
| 818 | func (solver *B2ContactSolver) SolveTOIPositionConstraints(toiIndexA int, toiIndexB int) bool { |
| 819 | |
| 820 | minSeparation := 0.0 |
| 821 | |
| 822 | for i := 0; i < solver.M_count; i++ { |
| 823 | pc := &solver.M_positionConstraints[i] |
| 824 | |
| 825 | indexA := pc.IndexA |
| 826 | indexB := pc.IndexB |
| 827 | localCenterA := pc.LocalCenterA |
| 828 | localCenterB := pc.LocalCenterB |
| 829 | pointCount := pc.PointCount |
| 830 | |
| 831 | mA := 0.0 |
| 832 | iA := 0.0 |
| 833 | if indexA == toiIndexA || indexA == toiIndexB { |
| 834 | mA = pc.InvMassA |
| 835 | iA = pc.InvIA |
| 836 | } |
| 837 | |
| 838 | mB := 0.0 |
| 839 | iB := 0.0 |
| 840 | if indexB == toiIndexA || indexB == toiIndexB { |
| 841 | mB = pc.InvMassB |
| 842 | iB = pc.InvIB |
| 843 | } |
| 844 | |
| 845 | cA := solver.M_positions[indexA].C |
| 846 | aA := solver.M_positions[indexA].A |
| 847 | |
| 848 | cB := solver.M_positions[indexB].C |
| 849 | aB := solver.M_positions[indexB].A |
| 850 | |
| 851 | // Solve normal constraints |
| 852 | for j := 0; j < pointCount; j++ { |
| 853 | xfA := MakeB2Transform() |
| 854 | xfB := MakeB2Transform() |
| 855 | |
| 856 | xfA.Q.Set(aA) |
| 857 | xfB.Q.Set(aB) |
| 858 | xfB.P = B2Vec2Sub(cB, B2RotVec2Mul(xfB.Q, localCenterB)) |
| 859 | xfA.P = B2Vec2Sub(cA, B2RotVec2Mul(xfA.Q, localCenterA)) |
| 860 | |
| 861 | psm := MakeB2PositionSolverManifold() |
| 862 | psm.Initialize(pc, xfA, xfB, j) |
| 863 | normal := psm.Normal |
| 864 | |
| 865 | point := psm.Point |
| 866 | separation := psm.Separation |
| 867 | |
| 868 | rA := B2Vec2Sub(point, cA) |
| 869 | rB := B2Vec2Sub(point, cB) |
| 870 | |
| 871 | // Track max constraint error. |
| 872 | minSeparation = math.Min(minSeparation, separation) |
| 873 | |
| 874 | // Prevent large corrections and allow slop. |
| 875 | C := B2FloatClamp(B2_toiBaugarte*(separation+B2_linearSlop), -B2_maxLinearCorrection, 0.0) |
no test coverage detected