Package eas.simulation.spatial.sim2D.physicalSimulation.physicsEngine.net.phys2d.math

Examples of eas.simulation.spatial.sim2D.physicalSimulation.physicsEngine.net.phys2d.math.Vector2f


   * @param intersection The intection to set the contact information for
   * @param vertsA The vertices of polygon A
   * @param vertsB The vertices of polygon B
   */
  public void setContact(Contact contact, Intersection intersection, Vector2f[] vertsA, Vector2f[] vertsB) {   
    Vector2f startA = vertsA[intersection.edgeA];
    Vector2f endA = vertsA[(intersection.edgeA + 1) % vertsA.length];
    Vector2f startB = vertsB[intersection.edgeB];
    Vector2f endB = vertsB[(intersection.edgeB + 1) % vertsB.length];
   
    Vector2f normal = MathUtil.getNormal(startA, endA);
    normal.sub(MathUtil.getNormal(startB, endB));
    normal.normalise();
   
    contact.setNormal(normal);
    contact.setSeparation(0);
    contact.setFeature(new FeaturePair(intersection.edgeA, intersection.edgeB, 0, 0));
    contact.setPosition(intersection.position);
View Full Code Here


    Matrix2f rot1 = new Matrix2f(body1.getRotation());
    Matrix2f rot2 = new Matrix2f(body2.getRotation());
    Matrix2f rot1T = rot1.transpose();
    Matrix2f rot2T = rot2.transpose();

    Vector2f a1 = new Vector2f(anchor);
    a1.sub(body1.getPosition());
    localAnchor1 = MathUtil.mul(rot1T,a1);
    Vector2f a2 = new Vector2f(anchor);
    a2.sub(body2.getPosition());
    localAnchor2 = MathUtil.mul(rot2T,a2);

    accumulatedImpulse.set(0.0f, 0.0f);
    relaxation = 1.0f;
  }
View Full Code Here

      Contact contact2,
      Intersection in,
      Intersection out,
      Vector2f[] vertsA,
      Vector2f[] vertsB) {
    Vector2f entryPoint = in.position;
    Vector2f exitPoint = out.position;
   
    Vector2f normal = MathUtil.getNormal(entryPoint, exitPoint);
   
    FeaturePair feature = new FeaturePair(in.edgeA, in.edgeB, out.edgeA, out.edgeB);
   
    float separation = -PenetrationSweep.getPenetrationDepth(in, out, normal, vertsA, vertsB);
    // divided by 2 because there are two contact points
View Full Code Here

   * the edge between vertsA[r[x][0]] and vertsA[r[x][0] + 1]
   * overlaps with vertsB[r[x][1]] and vertsB[r[x][1] + 1].
   */
    public int[][] getCollisionCandidates(Vector2f[] vertsA, Vector2f[] vertsB,
            Vector2f sweepDirStart, Vector2f sweepDirEnd) {
        Vector2f sweepDir = new Vector2f(sweepDirEnd);
        sweepDir.sub(sweepDirStart);

        return getCollisionCandidates(new EdgeSweep(sweepDir), vertsA, vertsB);
    }
View Full Code Here

    K3.col1.y = -body2.getInvI() * r2.x * r2.y;    K3.col2.y =  body2.getInvI() * r2.x * r2.x;

    Matrix2f K = MathUtil.add(MathUtil.add(K1,K2),K3);
    M = K.invert();

    Vector2f p1 = new Vector2f(body1.getPosition());
    p1.add(r1);
    Vector2f p2 = new Vector2f(body2.getPosition());
    p2.add(r2);
    Vector2f dp = new Vector2f(p2);
    dp.sub(p1);
   
    bias = new Vector2f(dp);
    bias.scale(-0.1f);
    bias.scale(invDT);

    // Apply accumulated impulse.
    accumulatedImpulse.scale(relaxation);
   
    if (!body1.isStatic()) {
      Vector2f accum1 = new Vector2f(accumulatedImpulse);
      accum1.scale(-body1.getInvMass());
      body1.adjustVelocity(accum1);
      body1.adjustAngularVelocity(-(body1.getInvI() * MathUtil.cross(r1, accumulatedImpulse)));
    }

    if (!body2.isStatic()) {
      Vector2f accum2 = new Vector2f(accumulatedImpulse);
      accum2.scale(body2.getInvMass());
      body2.adjustVelocity(accum2);
      body2.adjustAngularVelocity(body2.getInvI() * MathUtil.cross(r2, accumulatedImpulse));
    }
  }
View Full Code Here

  /**
   * Apply the impulse caused by the joint to the bodies attached.
   */
  @Override
    public void applyImpulse() {
    Vector2f dv = new Vector2f(body2.getVelocity());
    dv.add(MathUtil.cross(body2.getAngularVelocity(),r2));
    dv.sub(body1.getVelocity());
    dv.sub(MathUtil.cross(body1.getAngularVelocity(),r1));
      dv.scale(-1);
      dv.add(bias); // TODO: is this baumgarte stabilization?
     
      if (dv.lengthSquared() == 0) {
        return;
      }
     
    Vector2f impulse = MathUtil.mul(M, dv);

    if (!body1.isStatic()) {
      Vector2f delta1 = new Vector2f(impulse);
      delta1.scale(-body1.getInvMass());
      body1.adjustVelocity(delta1);
      body1.adjustAngularVelocity(-body1.getInvI() * MathUtil.cross(r1,impulse));
    }

    if (!body2.isStatic()) {
      Vector2f delta2 = new Vector2f(impulse);
      delta2.scale(body2.getInvMass());
      body2.adjustVelocity(delta2);
      body2.adjustAngularVelocity(body2.getInvI() * MathUtil.cross(r2,impulse));
    }
   
    accumulatedImpulse.add(impulse);
 
View Full Code Here

                  ROVector2f pos,
                  Matrix2f rot, Vector2f normal) {
    // The normal is from the reference box. Convert it
    // to the incident boxe's frame and flip sign.
    Matrix2f rotT = rot.transpose();
    Vector2f n = MathUtil.scale(MathUtil.mul(rotT,normal),-1);
    Vector2f nAbs = MathUtil.abs(n);

    if (nAbs.x > nAbs.y)
    {
      if (MathUtil.sign(n.x) > 0.0f)
      {
View Full Code Here

//    Vector2f a1 = rotA.col1;
//    Vector2f a2 = rotA.col2;
//    Vector2f b1 = rotB.col1;
//    Vector2f b2 = rotB.col2;

    Vector2f dp = MathUtil.sub(posB,posA);
    Vector2f dA = MathUtil.mul(RotAT,dp);
    Vector2f dB = MathUtil.mul(RotBT,dp);

    Matrix2f C = MathUtil.mul(RotAT,rotB);
    Matrix2f absC = MathUtil.abs(C);
    Matrix2f absCT = absC.transpose();

    // Box A faces
    Vector2f faceA = MathUtil.abs(dA);
    faceA.sub(hA);
    faceA.sub(MathUtil.mul(absC,hB));
   
    if (faceA.x > 0.0f || faceA.y > 0.0f) {
      return 0;
    }

    // Box B faces
    Vector2f faceB = MathUtil.abs(dB);
    faceB.sub(MathUtil.mul(absCT,hA));
    faceB.sub(hB);
    //MathUtil.sub(MathUtil.sub(MathUtil.abs(dB),MathUtil.mul(absCT,hA)),hB);
    if (faceB.x > 0.0f || faceB.y > 0.0f) {
      return 0;
    }

    // Find best axis
    int axis;
    float separation;
    Vector2f normal;

    // Box A faces
    axis = FACE_A_X;
    separation = faceA.x;
    normal = dA.x > 0.0f ? rotA.col1 : MathUtil.scale(rotA.col1,-1);

    if (faceA.y > 1.05f * separation + 0.01f * hA.y)
    {
      axis = FACE_A_Y;
      separation = faceA.y;
      normal = dA.y > 0.0f ? rotA.col2 : MathUtil.scale(rotA.col2,-1);
    }

    // Box B faces
    if (faceB.x > 1.05f * separation + 0.01f * hB.x)
    {
      axis = FACE_B_X;
      separation = faceB.x;
      normal = dB.x > 0.0f ? rotB.col1 : MathUtil.scale(rotB.col1,-1);
    }

    if (faceB.y > 1.05f * separation + 0.01f * hB.y)
    {
      axis = FACE_B_Y;
      separation = faceB.y;
      normal = dB.y > 0.0f ? rotB.col2 : MathUtil.scale(rotB.col2,-1);
    }

    // Setup clipping plane data based on the separating axis
    Vector2f frontNormal, sideNormal;
    ClipVertex[] incidentEdge = new ClipVertex[] {new ClipVertex(), new ClipVertex()};
    float front, negSide, posSide;
    char negEdge, posEdge;

    // Compute the clipping lines and the line segment to be clipped.
    switch (axis)
    {
    case FACE_A_X:
      {
        frontNormal = normal;
        front = posA.dot(frontNormal) + hA.x;
        sideNormal = rotA.col2;
        float side = posA.dot(sideNormal);
        negSide = -side + hA.y;
        posSide =  side + hA.y;
        negEdge = EDGE3;
        posEdge = EDGE1;
        computeIncidentEdge(incidentEdge, hB, posB, rotB, frontNormal);
      }
      break;

    case FACE_A_Y:
      {
        frontNormal = normal;
        front = posA.dot(frontNormal) + hA.y;
        sideNormal = rotA.col1;
        float side = posA.dot(sideNormal);
        negSide = -side + hA.x;
        posSide =  side + hA.x;
        negEdge = EDGE2;
        posEdge = EDGE4;
        computeIncidentEdge(incidentEdge, hB, posB, rotB, frontNormal);
      }
      break;

    case FACE_B_X:
      {
        frontNormal = MathUtil.scale(normal,-1);
        front = posB.dot(frontNormal) + hB.x;
        sideNormal = rotB.col2;
        float side = posB.dot(sideNormal);
        negSide = -side + hB.y;
        posSide =  side + hB.y;
        negEdge = EDGE3;
        posEdge = EDGE1;
        computeIncidentEdge(incidentEdge, hA, posA, rotA, frontNormal);
      }
      break;

    case FACE_B_Y:
      {
        frontNormal = MathUtil.scale(normal,-1);
        front = posB.dot(frontNormal) + hB.y;
        sideNormal = rotB.col1;
        float side = posB.dot(sideNormal);
        negSide = -side + hB.x;
        posSide =  side + hB.x;
        negEdge = EDGE2;
        posEdge = EDGE4;
        computeIncidentEdge(incidentEdge, hA, posA, rotA, frontNormal);
      }
      break;
    default:
      throw new RuntimeException("Unknown face!");
    }

    // clip other face with 5 box planes (1 face plane, 4 edge planes)

    ClipVertex[] clipPoints1 = new ClipVertex[] {new ClipVertex(), new ClipVertex()};
    ClipVertex[] clipPoints2 = new ClipVertex[] {new ClipVertex(), new ClipVertex()};
    int np;

    // Clip to box side 1
    np = clipSegmentToLine(clipPoints1, incidentEdge, MathUtil.scale(sideNormal,-1), negSide, negEdge);

    if (np < 2)
      return 0;

    // Clip to negative box side 1
    np = clipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, posSide, posEdge);

    if (np < 2)
      return 0;

    // Now clipPoints2 contains the clipping points.
    // Due to roundoff, it is possible that clipping removes all points.

    int numContacts = 0;
    for (int i = 0; i < 2; ++i)
    {
      float separation2 = frontNormal.dot(clipPoints2[i].v) - front;

      if (separation2 <= 0)
      {
        contacts[numContacts].setSeparation(separation2);
        contacts[numContacts].setNormal(normal);
View Full Code Here

  /**
   * @see eas.simulation.spatial.sim2D.physicalSimulation.physicsEngine.net.phys2d.raw.Joint#applyImpulse()
   */
  @Override
    public void applyImpulse() {
    Vector2f dv = new Vector2f(body2.getVelocity());
    dv.add(MathUtil.cross(body2.getAngularVelocity(), r2));
    dv.sub(body1.getVelocity());
    dv.sub(MathUtil.cross(body1.getAngularVelocity(), r1));

    float ju = -dv.dot(dp) + bias;
    float p = ju / sc;

    Vector2f impulse = new Vector2f(dp);
    impulse.scale(p);

    if (!body1.isStatic()) {
      Vector2f accum1 = new Vector2f(impulse);
      accum1.scale(-body1.getInvMass());
      body1.adjustVelocity(accum1);
      body1.adjustAngularVelocity(-(body1.getInvI() * MathUtil.cross(r1,
          impulse)));
    }

    if (!body2.isStatic()) {
      Vector2f accum2 = new Vector2f(impulse);
      accum2.scale(body2.getInvMass());
      body2.adjustVelocity(accum2);
      body2.adjustAngularVelocity(body2.getInvI()
          * MathUtil.cross(r2, impulse));
    }

View Full Code Here

    K3.col1.y = -body2.getInvI() * r2.x * r2.y;
    K3.col2.y = body2.getInvI() * r2.x * r2.x;

    Matrix2f K = MathUtil.add(MathUtil.add(K1, K2), K3);

    Vector2f p1 = new Vector2f(body1.getPosition());
    p1.add(r1);
    Vector2f p2 = new Vector2f(body2.getPosition());
    p2.add(r2);
    dp = new Vector2f(p2);
    dp.sub(p1);

    float biasFactor = 0.3f;
    bias = biasFactor * (-dp.lengthSquared() + distant);

    dp.normalise();

    sc = MathUtil.mul(K, dp).dot(dp);

    Vector2f impulse = new Vector2f(dp);
    impulse.scale(accumulatedImpulse);

    if (!body1.isStatic()) {
      Vector2f accum1 = new Vector2f(impulse);
      accum1.scale(-body1.getInvMass());
      body1.adjustVelocity(accum1);
      body1.adjustAngularVelocity(-(body1.getInvI() * MathUtil.cross(r1,
          impulse)));
    }

    if (!body2.isStatic()) {
      Vector2f accum2 = new Vector2f(impulse);
      accum2.scale(body2.getInvMass());
      body2.adjustVelocity(accum2);
      body2.adjustAngularVelocity(body2.getInvI()
          * MathUtil.cross(r2, impulse));
    }
  }
View Full Code Here

TOP

Related Classes of eas.simulation.spatial.sim2D.physicalSimulation.physicsEngine.net.phys2d.math.Vector2f

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.