Package org.jbox2d.dynamics.contacts.ContactVelocityConstraint

Examples of org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint


      pc.type = manifold.type;

      // System.out.println("contact point count: " + pointCount);
      for (int j = 0; j < pointCount; j++) {
        ManifoldPoint cp = manifold.points[j];
        VelocityConstraintPoint vcp = vc.points[j];

        if (m_step.warmStarting) {
          // assert(cp.normalImpulse == 0);
          // System.out.println("contact normal impulse: " + cp.normalImpulse);
          vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
 
View Full Code Here


      Vec2 normal = vc.normal;
      float tangentx = 1.0f * normal.y;
      float tangenty = -1.0f * normal.x;

      for (int j = 0; j < pointCount; ++j) {
        VelocityConstraintPoint vcp = vc.points[j];
        float Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse;
        float Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse;

        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
        vA.x -= Px * mA;
 
View Full Code Here

      vc.normal.set(worldManifold.normal);

      int pointCount = vc.pointCount;
      for (int j = 0; j < pointCount; ++j) {
        VelocityConstraintPoint vcp = vc.points[j];

        vcp.rA.set(worldManifold.points[j]).subLocal(cA);
        vcp.rB.set(worldManifold.points[j]).subLocal(cB);

        float rnA = vcp.rA.x * vc.normal.y - vcp.rA.y * vc.normal.x;
        float rnB = vcp.rB.x * vc.normal.y - vcp.rB.y * vc.normal.x;

        float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

        vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;

        float tangentx = 1.0f * vc.normal.y;
        float tangenty = -1.0f * vc.normal.x;

        float rtA = vcp.rA.x * tangenty - vcp.rA.y * tangentx;
        float rtB = vcp.rB.x * tangenty - vcp.rB.y * tangentx;

        float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;

        vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;

        // Setup a velocity bias for restitution.
        vcp.velocityBias = 0.0f;
        float tempx = vB.x + -wB * vcp.rB.y - vA.x - (-wA * vcp.rA.y);
        float tempy = vB.y + wB * vcp.rB.x - vA.y - (wA * vcp.rA.x);
        float vRel = vc.normal.x * tempx + vc.normal.y * tempy;
        if (vRel < -Settings.velocityThreshold) {
          vcp.velocityBias = -vc.restitution * vRel;
        }
      }

      // If we have two points, then prepare the block solver.
      if (vc.pointCount == 2) {
        VelocityConstraintPoint vcp1 = vc.points[0];
        VelocityConstraintPoint vcp2 = vc.points[1];

        float rn1A = Vec2.cross(vcp1.rA, vc.normal);
        float rn1B = Vec2.cross(vcp1.rB, vc.normal);
        float rn2A = Vec2.cross(vcp2.rA, vc.normal);
        float rn2B = Vec2.cross(vcp2.rB, vc.normal);
View Full Code Here

      assert (pointCount == 1 || pointCount == 2);

      // Solve tangent constraints
      for (int j = 0; j < pointCount; ++j) {
        final VelocityConstraintPoint vcp = vc.points[j];
        final Vec2 a = vcp.rA;
        float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * a.y;
        float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * a.x;

        // Compute tangent force
        final float vt = dvx * tangent.x + dvy * tangent.y - vc.tangentSpeed;
        float lambda = vcp.tangentMass * (-vt);

        // Clamp the accumulated force
        final float maxFriction = friction * vcp.normalImpulse;
        final float newImpulse =
            MathUtils.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
        lambda = newImpulse - vcp.tangentImpulse;
        vcp.tangentImpulse = newImpulse;

        // Apply contact impulse
        // Vec2 P = lambda * tangent;

        final float Px = tangent.x * lambda;
        final float Py = tangent.y * lambda;

        // vA -= invMassA * P;
        vA.x -= Px * mA;
        vA.y -= Py * mA;
        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);

        // vB += invMassB * P;
        vB.x += Px * mB;
        vB.y += Py * mB;
        wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
      }

      // Solve normal constraints
      if (vc.pointCount == 1) {
        final VelocityConstraintPoint vcp = vc.points[0];

        // Relative velocity at contact
        // Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);

        float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * vcp.rA.y;
        float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * vcp.rA.x;

        // Compute normal impulse
        final float vn = dvx * normal.x + dvy * normal.y;
        float lambda = -vcp.normalMass * (vn - vcp.velocityBias);

        // Clamp the accumulated impulse
        float a = vcp.normalImpulse + lambda;
        final float newImpulse = (a > 0.0f ? a : 0.0f);
        lambda = newImpulse - vcp.normalImpulse;
        vcp.normalImpulse = newImpulse;

        // Apply contact impulse
        float Px = normal.x * lambda;
        float Py = normal.y * lambda;

        // vA -= invMassA * P;
        vA.x -= Px * mA;
        vA.y -= Py * mA;
        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);

        // vB += invMassB * P;
        vB.x += Px * mB;
        vB.y += Py * mB;
        wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
      } else {
        // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on
        // Box2D_Lite).
        // Build the mini LCP for this contact patch
        //
        // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
        //
        // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
        // b = vn_0 - velocityBias
        //
        // The system is solved using the "Total enumeration method" (s. Murty). The complementary
        // constraint vn_i * x_i
        // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D
        // contact problem the cases
        // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be
        // tested. The first valid
        // solution that satisfies the problem is chosen.
        //
        // In order to account of the accumulated impulse 'a' (because of the iterative nature of
        // the solver which only requires
        // that the accumulated impulse is clamped and not the incremental impulse) we change the
        // impulse variable (x_i).
        //
        // Substitute:
        //
        // x = a + d
        //
        // a := old total impulse
        // x := new total impulse
        // d := incremental impulse
        //
        // For the current iteration we extend the formula for the incremental impulse
        // to compute the new total impulse:
        //
        // vn = A * d + b
        // = A * (x - a) + b
        // = A * x + b - A * a
        // = A * x + b'
        // b' = b - A * a;

        final VelocityConstraintPoint cp1 = vc.points[0];
        final VelocityConstraintPoint cp2 = vc.points[1];
        a.x = cp1.normalImpulse;
        a.y = cp2.normalImpulse;

        assert (a.x >= 0.0f && a.y >= 0.0f);
        // Relative velocity at contact
View Full Code Here

      pc.type = manifold.type;

      // System.out.println("contact point count: " + pointCount);
      for (int j = 0; j < pointCount; j++) {
        ManifoldPoint cp = manifold.points[j];
        VelocityConstraintPoint vcp = vc.points[j];

        if (m_step.warmStarting) {
          // assert(cp.normalImpulse == 0);
          // System.out.println("contact normal impulse: " + cp.normalImpulse);
          vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
 
View Full Code Here

      Vec2 normal = vc.normal;
      float tangentx = 1.0f * normal.y;
      float tangenty = -1.0f * normal.x;

      for (int j = 0; j < pointCount; ++j) {
        VelocityConstraintPoint vcp = vc.points[j];
        float Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse;
        float Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse;

        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
        vA.x -= Px * mA;
 
View Full Code Here

      vcnormal.x = worldManifold.normal.x;
      vcnormal.y = worldManifold.normal.y;

      int pointCount = vc.pointCount;
      for (int j = 0; j < pointCount; ++j) {
        VelocityConstraintPoint vcp = vc.points[j];
        Vec2 wmPj = worldManifold.points[j];
        final Vec2 vcprA = vcp.rA;
        final Vec2 vcprB = vcp.rB;
        vcprA.x = wmPj.x - cA.x;
        vcprA.y = wmPj.y - cA.y;
        vcprB.x = wmPj.x - cB.x;
        vcprB.y = wmPj.y - cB.y;

        float rnA = vcprA.x * vcnormal.y - vcprA.y * vcnormal.x;
        float rnB = vcprB.x * vcnormal.y - vcprB.y * vcnormal.x;

        float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

        vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;

        float tangentx = 1.0f * vcnormal.y;
        float tangenty = -1.0f * vcnormal.x;

        float rtA = vcprA.x * tangenty - vcprA.y * tangentx;
        float rtB = vcprB.x * tangenty - vcprB.y * tangentx;

        float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;

        vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;

        // Setup a velocity bias for restitution.
        vcp.velocityBias = 0.0f;
        float tempx = vB.x + -wB * vcprB.y - vA.x - (-wA * vcprA.y);
        float tempy = vB.y + wB * vcprB.x - vA.y - (wA * vcprA.x);
        float vRel = vcnormal.x * tempx + vcnormal.y * tempy;
        if (vRel < -Settings.velocityThreshold) {
          vcp.velocityBias = -vc.restitution * vRel;
        }
      }

      // If we have two points, then prepare the block solver.
      if (vc.pointCount == 2) {
        VelocityConstraintPoint vcp1 = vc.points[0];
        VelocityConstraintPoint vcp2 = vc.points[1];
        float rn1A = vcp1.rA.x * vcnormal.y - vcp1.rA.y * vcnormal.x;
        float rn1B = vcp1.rB.x * vcnormal.y - vcp1.rB.y * vcnormal.x;
        float rn2A = vcp2.rA.x * vcnormal.y - vcp2.rA.y * vcnormal.x;
        float rn2B = vcp2.rB.x * vcnormal.y - vcp2.rB.y * vcnormal.x;

 
View Full Code Here

      assert (pointCount == 1 || pointCount == 2);

      // Solve tangent constraints
      for (int j = 0; j < pointCount; ++j) {
        final VelocityConstraintPoint vcp = vc.points[j];
        final Vec2 a = vcp.rA;
        float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * a.y;
        float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * a.x;

        // Compute tangent force
        final float vt = dvx * tangentx + dvy * tangenty - vc.tangentSpeed;
        float lambda = vcp.tangentMass * (-vt);

        // Clamp the accumulated force
        final float maxFriction = friction * vcp.normalImpulse;
        final float newImpulse =
            MathUtils.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
        lambda = newImpulse - vcp.tangentImpulse;
        vcp.tangentImpulse = newImpulse;

        // Apply contact impulse
        // Vec2 P = lambda * tangent;

        final float Px = tangentx * lambda;
        final float Py = tangenty * lambda;

        // vA -= invMassA * P;
        vA.x -= Px * mA;
        vA.y -= Py * mA;
        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);

        // vB += invMassB * P;
        vB.x += Px * mB;
        vB.y += Py * mB;
        wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
      }

      // Solve normal constraints
      if (vc.pointCount == 1) {
        final VelocityConstraintPoint vcp = vc.points[0];

        // Relative velocity at contact
        // Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);

        float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * vcp.rA.y;
        float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * vcp.rA.x;

        // Compute normal impulse
        final float vn = dvx * normalx + dvy * normaly;
        float lambda = -vcp.normalMass * (vn - vcp.velocityBias);

        // Clamp the accumulated impulse
        float a = vcp.normalImpulse + lambda;
        final float newImpulse = (a > 0.0f ? a : 0.0f);
        lambda = newImpulse - vcp.normalImpulse;
        vcp.normalImpulse = newImpulse;

        // Apply contact impulse
        float Px = normalx * lambda;
        float Py = normaly * lambda;

        // vA -= invMassA * P;
        vA.x -= Px * mA;
        vA.y -= Py * mA;
        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);

        // vB += invMassB * P;
        vB.x += Px * mB;
        vB.y += Py * mB;
        wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
      } else {
        // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on
        // Box2D_Lite).
        // Build the mini LCP for this contact patch
        //
        // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
        //
        // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
        // b = vn_0 - velocityBias
        //
        // The system is solved using the "Total enumeration method" (s. Murty). The complementary
        // constraint vn_i * x_i
        // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D
        // contact problem the cases
        // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be
        // tested. The first valid
        // solution that satisfies the problem is chosen.
        //
        // In order to account of the accumulated impulse 'a' (because of the iterative nature of
        // the solver which only requires
        // that the accumulated impulse is clamped and not the incremental impulse) we change the
        // impulse variable (x_i).
        //
        // Substitute:
        //
        // x = a + d
        //
        // a := old total impulse
        // x := new total impulse
        // d := incremental impulse
        //
        // For the current iteration we extend the formula for the incremental impulse
        // to compute the new total impulse:
        //
        // vn = A * d + b
        // = A * (x - a) + b
        // = A * x + b - A * a
        // = A * x + b'
        // b' = b - A * a;

        final VelocityConstraintPoint cp1 = vc.points[0];
        final VelocityConstraintPoint cp2 = vc.points[1];
        final Vec2 cp1rA = cp1.rA;
        final Vec2 cp1rB = cp1.rB;
        final Vec2 cp2rA = cp2.rA;
        final Vec2 cp2rB = cp2.rB;
        float ax = cp1.normalImpulse;
View Full Code Here

      pc.type = manifold.type;

      // System.out.println("contact point count: " + pointCount);
      for (int j = 0; j < pointCount; j++) {
        ManifoldPoint cp = manifold.points[j];
        VelocityConstraintPoint vcp = vc.points[j];

        if (m_step.warmStarting) {
          // assert(cp.normalImpulse == 0);
          // System.out.println("contact normal impulse: " + cp.normalImpulse);
          vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
 
View Full Code Here

      Vec2 normal = vc.normal;
      float tangentx = 1.0f * normal.y;
      float tangenty = -1.0f * normal.x;

      for (int j = 0; j < pointCount; ++j) {
        VelocityConstraintPoint vcp = vc.points[j];
        float Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse;
        float Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse;

        wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
        vA.x -= Px * mA;
 
View Full Code Here

TOP

Related Classes of org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint

Copyright © 2018 www.massapicom. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.