Пример #1
0
  private double[] springPenalty(Vector3d xi, Vector3d xj, double dist) {
    double[] F = new double[6];
    // collision normal
    Vector3d n = xi.minus(xj);
    n = n.dot(1 / n.norm());

    F[0] = -(k * (n.x()) * (-2 * proximity + dist));
    F[1] = -(k * (n.y()) * (-2 * proximity + dist));
    F[2] = -(k * (n.z()) * (-2 * proximity + dist));
    F[3] = -F[0];
    F[4] = -F[1];
    F[5] = -F[2];

    return F;
  }
Пример #2
0
  private double[] calculatePenaltyForces(double[] X, double[] V, HashSet<Collision> cSet) {
    double[] penalty = new double[X.length];

    // for each collision, apply the penalties
    for (Collision col : cSet) {
      // Collision col = col_rec[i];
      // vertex-face collision
      if (col.getType() == Collision.VERTEX_FACE) {
        //	System.out.println("VERTEX-FACE");
        // get indicies
        int[] parts = col.getParticles();
        int p = parts[0];
        int a = parts[1];
        int b = parts[2];
        int c = parts[3];

        // get barycentric coords
        double[] bc = col.getBaryCoords();
        double u = bc[0];
        double v = bc[1];
        double w = bc[2];

        // get collision point and collision velocities
        Vector3d xa = new Vector3d(); // was Vector3d xa, xb, va, vb;
        Vector3d xb = new Vector3d();
        Vector3d va = new Vector3d();
        Vector3d vb = new Vector3d();

        xa.set(X[3 * p], X[3 * p + 1], X[3 * p + 2]);
        xb.set(
            u * X[3 * a] + v * X[3 * b] + w * X[3 * c],
            u * X[3 * a + 1] + v * X[3 * b + 1] + w * X[3 * c + 1],
            u * X[3 * a + 2] + v * X[3 * b + 2] + w * X[3 * c + 2]);
        va.set(V[3 * p], V[3 * p + 1], V[3 * p + 2]);
        vb.set(
            u * V[3 * a] + v * V[3 * b] + w * V[3 * c],
            u * V[3 * a + 1] + v * V[3 * b + 1] + w * V[3 * c + 1],
            u * V[3 * a + 2] + v * V[3 * b + 2] + w * V[3 * c + 2]);

        // System.out.println("REL VEL "+xa.minus(xb).dot(va.minus(vb))+" "+va+" "+vb+" "+xa+"
        // "+xb);
        // System.out.println("REL VEL2 "+xb.minus(xa).dot(vb.minus(va)));
        /*if the objects have do not have
        a seperating velocity, apply the
        local spring penalty */
        if (xa.minus(xb).dot(va.minus(vb)) < 0) {
          double[] local_penalty = springPenalty(xa, xb, col.getDistance());
          // apply to vertex
          penalty[3 * p] += local_penalty[0];
          penalty[3 * p + 1] += local_penalty[1];
          penalty[3 * p + 2] += local_penalty[2];

          // apply to triangle coordinates
          penalty[3 * a] += bc[0] * local_penalty[3];
          penalty[3 * a + 1] += bc[0] * local_penalty[4];
          penalty[3 * a + 2] += bc[0] * local_penalty[5];
          penalty[3 * b] += bc[1] * local_penalty[3];
          penalty[3 * b + 1] += bc[1] * local_penalty[4];
          penalty[3 * b + 2] += bc[1] * local_penalty[5];
          penalty[3 * c] += bc[2] * local_penalty[3];
          penalty[3 * c + 1] += bc[2] * local_penalty[4];
          penalty[3 * c + 2] += bc[2] * local_penalty[5];
        }
      }

      // edge-edge collision
      if (col.getType() == Collision.EDGE_EDGE) {
        // get indicies
        int[] parts = col.getParticles();
        int p1 = parts[0];
        int q1 = parts[1];
        int p2 = parts[2];
        int q2 = parts[3];

        // get barycentric coords
        double[] coords = col.getBaryCoords();
        double s = coords[0];
        double t = coords[1];

        Vector3d xa = new Vector3d(); // was Vector3d xa, xb, va, vb;
        Vector3d xb = new Vector3d();
        Vector3d va = new Vector3d();
        Vector3d vb = new Vector3d();

        // edge points
        xa.set(
            s * X[3 * p1] + (1 - s) * X[3 * q1],
            s * X[3 * p1 + 1] + (1 - s) * X[3 * q1 + 1],
            s * X[3 * p1 + 2] + (1 - s) * X[3 * q1 + 2]);
        xb.set(
            t * X[3 * p2] + (1 - t) * X[3 * q2],
            t * X[3 * p2 + 1] + (1 - t) * X[3 * q2 + 1],
            t * X[3 * p2 + 2] + (1 - t) * X[3 * q2 + 2]);

        // velocities
        va.set(
            s * V[3 * p1] + (1 - s) * V[3 * q1],
            s * V[3 * p1 + 1] + (1 - s) * V[3 * q1 + 1],
            s * V[3 * p1 + 2] + (1 - s) * V[3 * q1 + 2]);
        vb.set(
            t * V[3 * p2] + (1 - t) * V[3 * q2],
            t * V[3 * p2 + 1] + (1 - t) * V[3 * q2 + 1],
            t * V[3 * p2 + 2] + (1 - t) * V[3 * q2 + 2]);

        /*if the objects have do not have
        a seperating velocity, apply the
        local spring penalty */
        if (xa.minus(xb).dot(va.minus(vb)) < 0) {
          double[] local_penalty = springPenalty(xa, xb, col.getDistance());
          // apply penalty to first edge
          penalty[3 * p1] += s * local_penalty[0];
          penalty[3 * p1 + 1] += s * local_penalty[1];
          penalty[3 * p1 + 2] += s * local_penalty[2];
          penalty[3 * q1] += (1 - s) * local_penalty[0];
          penalty[3 * q1 + 1] += (1 - s) * local_penalty[1];
          penalty[3 * q1 + 2] += (1 - s) * local_penalty[2];
          // apply penalty to second edge
          penalty[3 * p2] += t * local_penalty[3];
          penalty[3 * p2 + 1] += t * local_penalty[4];
          penalty[3 * p2 + 2] += t * local_penalty[5];
          penalty[3 * q2] += (1 - t) * local_penalty[3];
          penalty[3 * q2 + 1] += (1 - t) * local_penalty[4];
          penalty[3 * q2 + 2] += (1 - t) * local_penalty[5];
        }
      }
    }

    return penalty;
  }