Ejemplo n.º 1
0
		public bool SolvePositionConstraints(){
			float minSeparation = 0.0f;

			for (int i = 0; i < m_contacts.Count(); ++i) {
				ContactPositionConstraint pc = m_positionConstraints[i];

				int indexA = pc.indexA;
				int indexB = pc.indexB;
				Vec2 localCenterA = pc.localCenterA;
				float mA = pc.invMassA;
				float iA = pc.invIA;
				Vec2 localCenterB = pc.localCenterB;
				float mB = pc.invMassB;
				float iB = pc.invIB;
				int pointCount = pc.pointCount;

				Vec2 cA = m_positions[indexA].c;
				float aA = m_positions[indexA].a;

				Vec2 cB = m_positions[indexB].c;
				float aB = m_positions[indexB].a;

				// Solve normal constraints
				for (int j = 0; j < pointCount; ++j) {
					Transform xfA = new Transform();
					Transform xfB = new Transform();
					xfA.q.Set(aA);
					xfB.q.Set(aB);
					xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
					xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);

					PositionSolverManifold psm = new PositionSolverManifold();
					psm.Initialize(pc, xfA, xfB, j);
					Vec2 normal = psm.normal;

					Vec2 point = psm.point;
					float separation = psm.separation;

					Vec2 rA = point - cA;
					Vec2 rB = point - cB;

					// Track max constraint error.
					minSeparation = Math.Min(minSeparation, separation);

					// Prevent large corrections and allow slop.
					float C = Utilities.Clamp(Settings._baumgarte * (separation + Settings._linearSlop), -Settings._maxLinearCorrection, 0.0f);

					// Compute the effective mass.
					float rnA = Utilities.Cross(rA, normal);
					float rnB = Utilities.Cross(rB, normal);
					float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

					// Compute normal impulse
					float impulse = K > 0.0f ? -C / K : 0.0f;

					Vec2 P = impulse * normal;

					cA -= mA * P;
					aA -= iA * Utilities.Cross(rA, P);

					cB += mB * P;
					aB += iB * Utilities.Cross(rB, P);
				}

				m_positions[indexA].c = cA;
				m_positions[indexA].a = aA;

				m_positions[indexB].c = cB;
				m_positions[indexB].a = aB;
			}

			// We can't expect minSpeparation >= -_linearSlop because we don't
			// push the separation above -_linearSlop.
			return minSeparation >= -3.0f * Settings._linearSlop;
		}
Ejemplo n.º 2
0
        public bool SolvePositionConstraints(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];

                Body bodyA = c.bodyA;
                Body bodyB = c.bodyB;

                float invMassA = bodyA._mass * bodyA._invMass;
                float invIA    = bodyA._mass * bodyA._invI;
                float invMassB = bodyB._mass * bodyB._invMass;
                float invIB    = bodyB._mass * bodyB._invI;

                // Solve normal constraints
                for (int j = 0; j < c.pointCount; ++j)
                {
                    PositionSolverManifold psm = new PositionSolverManifold(ref c, j);
                    Vector2 normal             = psm._normal;

                    Vector2 point      = psm._point;
                    float   separation = psm._separation;

                    Vector2 rA = point - bodyA._sweep.c;
                    Vector2 rB = point - bodyB._sweep.c;

                    // Track max constraint error.
                    minSeparation = Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = MathUtils.Cross(rA, normal);
                    float rnB = MathUtils.Cross(rB, normal);
                    float K   = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

#if MATH_OVERLOADS
                    Vector2 P = impulse * normal;

                    bodyA._sweep.c -= invMassA * P;
                    bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);

                    bodyB._sweep.c += invMassB * P;
                    bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
#else
                    Vector2 P = new Vector2(impulse * normal.x, impulse * normal.y);

                    bodyA._sweep.c.x -= invMassA * P.x;
                    bodyA._sweep.c.y -= invMassA * P.y;
                    bodyA._sweep.a   -= invIA * (rA.x * P.y - rA.y * P.x);

                    bodyB._sweep.c.x += invMassB * P.x;
                    bodyB._sweep.c.y += invMassB * P.y;
                    bodyB._sweep.a   += invIB * (rB.x * P.y - rB.y * P.x);
#endif
                    bodyA.SynchronizeTransform();
                    bodyB.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
            // push the separation above -Settings.b2_linearSlop.
            return(minSeparation >= -1.5f * Settings.b2_linearSlop);
        }
Ejemplo n.º 3
0
        public bool SolvePositionConstraints()
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < m_contacts.Count(); ++i)
            {
                ContactPositionConstraint pc = m_positionConstraints[i];

                int   indexA       = pc.indexA;
                int   indexB       = pc.indexB;
                Vec2  localCenterA = pc.localCenterA;
                float mA           = pc.invMassA;
                float iA           = pc.invIA;
                Vec2  localCenterB = pc.localCenterB;
                float mB           = pc.invMassB;
                float iB           = pc.invIB;
                int   pointCount   = pc.pointCount;

                Vec2  cA = m_positions[indexA].c;
                float aA = m_positions[indexA].a;

                Vec2  cB = m_positions[indexB].c;
                float aB = m_positions[indexB].a;

                // Solve normal constraints
                for (int j = 0; j < pointCount; ++j)
                {
                    Transform xfA = new Transform();
                    Transform xfB = new Transform();
                    xfA.q.Set(aA);
                    xfB.q.Set(aB);
                    xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
                    xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);

                    PositionSolverManifold psm = new PositionSolverManifold();
                    psm.Initialize(pc, xfA, xfB, j);
                    Vec2 normal = psm.normal;

                    Vec2  point      = psm.point;
                    float separation = psm.separation;

                    Vec2 rA = point - cA;
                    Vec2 rB = point - cB;

                    // Track max constraint error.
                    minSeparation = Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = Utilities.Clamp(Settings._baumgarte * (separation + Settings._linearSlop), -Settings._maxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = Utilities.Cross(rA, normal);
                    float rnB = Utilities.Cross(rB, normal);
                    float K   = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

                    Vec2 P = impulse * normal;

                    cA -= mA * P;
                    aA -= iA * Utilities.Cross(rA, P);

                    cB += mB * P;
                    aB += iB * Utilities.Cross(rB, P);
                }

                m_positions[indexA].c = cA;
                m_positions[indexA].a = aA;

                m_positions[indexB].c = cB;
                m_positions[indexB].a = aB;
            }

            // We can't expect minSpeparation >= -_linearSlop because we don't
            // push the separation above -_linearSlop.
            return(minSeparation >= -3.0f * Settings._linearSlop);
        }