Ejemplo n.º 1
0
  /**
   * Draws the triangle defined through the given values on the renderPanel.
   *
   * @param positions
   * @param colors
   * @param normals
   */
  private void rasterizeTriangle(
      Point4f[] positions, Color3f[] colors, Point2f[] texCoords, Material material) {
    Matrix3f alphabetagamma = new Matrix3f();
    // start with smallest "possible" bounding rectangle
    Point topLeft = new Point(width - 1, height - 1);
    Point botRight = new Point(0, 0);
    float[] oneOverWarray = new float[3];
    for (int i = 0; i < 3; i++) {
      minimizeBoundingRectangle(topLeft, botRight, positions[i]);
      float[] row = {positions[i].x, positions[i].y, positions[i].w};
      alphabetagamma.setRow(i, row);
      oneOverWarray[i] = 1 / positions[i].w;
    }
    alphabetagamma.invert();

    validifyBoundingRectangle(topLeft, botRight);

    for (int y = topLeft.y; y <= botRight.y; y++) {
      for (int x = topLeft.x; x <= botRight.x; x++) {
        Vector3f vertexWeights = getVertexWeights(x, y, alphabetagamma);
        if (vertexWeights != null) {
          float zvalue = new Vector3f(oneOverWarray).dot(vertexWeights);
          if (betterZvalue(x, y, zvalue)) {
            zBuffer[x][y] = zvalue;
            int c;
            if (material != null && material.getTexture() != null)
              c = interpolateColorFromTexture(vertexWeights, texCoords, material.getTexture());
            else c = interpolateColor(vertexWeights, colors).getRGB();
            drawPointAt(x, y, c);
          }
        }
      }
    }
  }
Ejemplo n.º 2
0
 public Vector3f velocityLocal() {
   Vector3f v = new Vector3f();
   body.getLinearVelocity(v);
   Matrix3f invOri = orientation();
   invOri.invert();
   return invOri.mult(v);
 }
Ejemplo n.º 3
0
 @Override
 public void setModelViewMatrix(Matrix4f value) {
   gl.uniformMatrix(getUniformLocation("matWorldView"), value);
   value.get(normalMatrix);
   normalMatrix.invert();
   normalMatrix.transpose();
   gl.uniformMatrix(getUniformLocation("uNormalMatrix"), normalMatrix);
 }
Ejemplo n.º 4
0
 public void alignAxisToVec(String axisName, Vector3f vec) {
   Vector3f alignAxis = axis(axisName);
   Vector3f rotAxis = new Vector3f();
   rotAxis.cross(alignAxis, vec);
   if (rotAxis.length() == 0) rotAxis = axis(("XYZ".indexOf(axisName) + 1) % 3);
   Matrix3f rotMatrix = Matrix3f.rotation(rotAxis, alignAxis.angle(vec));
   Matrix3f ori = orientation();
   rotMatrix.mul(ori);
   orientation(rotMatrix);
 }
Ejemplo n.º 5
0
  public void rotateLocal(float x, float y, float z) {
    Matrix3f ori = orientation();

    Matrix3f rot = new Matrix3f();
    MatrixUtil.setEulerZYX(rot, x, y, z);

    ori.mul(rot);

    orientation(ori);
  }
Ejemplo n.º 6
0
  protected void doGroundMovement() {
    if (!isOnGround) return;

    // ground movement is as follows, the arrow key dictate the target velocity, and accelleration
    // is
    // added to achieve that. If opposing keys are held the target velocity is set to zero.

    // This is the velocity in the normal plane in the player space
    Vector3f targetVelocity = new Vector3f();
    if (forward) targetVelocity.z += 1;
    if (backward) targetVelocity.z -= 1;
    if (left) targetVelocity.x -= 1;
    if (right) targetVelocity.x += 1;

    if (forward != backward || left != right) {
      targetVelocity.normalize();
      targetVelocity.scale(MAX_SPEED);
    }

    forward = false;
    backward = false;
    left = false;
    right = false;

    Vector3f forward = new Vector3f(0, 0, 1);
    QuaternionUtil.quatRotate(owner.getOrientation(), forward, forward);

    // right = normal x forward
    // forward = right x normal
    Vector3f right = new Vector3f();
    right.cross(surfaceNormal, forward);
    forward.cross(right, surfaceNormal);
    right.normalize();
    forward.normalize();

    Matrix3f transform = new Matrix3f();
    transform.setColumn(0, right);
    transform.setColumn(1, surfaceNormal);
    transform.setColumn(2, forward);

    transform.transform(targetVelocity);

    Vector3f delta = new Vector3f();
    owner.getRigidBody().getLinearVelocity(delta);
    delta.sub(targetVelocity, delta);

    // feet can't pull and don't need to push for now
    // Ds = D - N(N.D)
    Vector3f parComp = new Vector3f(surfaceNormal);
    parComp.scale(surfaceNormal.dot(delta));
    delta.sub(delta, parComp);

    owner.getRigidBody().applyCentralForce(scaleForce(delta));
  }
Ejemplo n.º 7
0
 /**
  * Returns a vector of the edge coefficients or null if one of them is smaller than 0
  *
  * @param x
  * @param y
  * @param alphabetagamma
  * @return
  */
 private Vector3f getVertexWeights(int x, int y, Matrix3f alphabetagamma) {
   Vector3f abgVector = new Vector3f();
   float[] coeffs = new float[3];
   for (int i = 0; i < 3; i++) {
     alphabetagamma.getColumn(i, abgVector);
     float coeff = abgVector.dot(new Vector3f(x, y, 1));
     if (coeff < 0) return null;
     else coeffs[i] = coeff;
   }
   return new Vector3f(coeffs);
 }
Ejemplo n.º 8
0
    public Vertex bake(Mesh mesh, Function<Node<?>, Matrix4f> animator) {
      // geometry
      Float totalWeight = 0f;
      Matrix4f t = new Matrix4f();
      if (mesh.getWeightMap().get(this).isEmpty()) {
        t.setIdentity();
      } else {
        for (Pair<Float, Node<Bone>> bone : mesh.getWeightMap().get(this)) {
          totalWeight += bone.getLeft();
          Matrix4f bm = animator.apply(bone.getRight());
          bm.mul(bone.getLeft());
          t.add(bm);
        }
        if (Math.abs(totalWeight) > 1e-4) t.mul(1f / totalWeight);
        else t.setIdentity();
      }

      // pos
      Vector4f pos = new Vector4f(this.pos), newPos = new Vector4f();
      pos.w = 1;
      t.transform(pos, newPos);
      Vector3f rPos = new Vector3f(newPos.x / newPos.w, newPos.y / newPos.w, newPos.z / newPos.w);

      // normal
      Vector3f rNormal = null;

      if (this.normal != null) {
        Matrix3f tm = new Matrix3f();
        t.getRotationScale(tm);
        tm.invert();
        tm.transpose();
        Vector3f normal = new Vector3f(this.normal);
        rNormal = new Vector3f();
        tm.transform(normal, rNormal);
        rNormal.normalize();
      }

      // texCoords TODO
      return new Vertex(rPos, rNormal, color, texCoords);
    }
Ejemplo n.º 9
0
 public void setTransform(
     float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3) {
   if (matrixRotate != null || !doSetOrientation) return;
   matrixRotate = new Matrix3f();
   Vector3f v = new Vector3f();
   // rows in Sygress/CAChe and Spartan become columns here
   v.set(x1, y1, z1);
   v.normalize();
   matrixRotate.setColumn(0, v);
   v.set(x2, y2, z2);
   v.normalize();
   matrixRotate.setColumn(1, v);
   v.set(x3, y3, z3);
   v.normalize();
   matrixRotate.setColumn(2, v);
   atomSetCollection.setAtomSetCollectionAuxiliaryInfo(
       "defaultOrientationMatrix", new Matrix3f(matrixRotate));
   // first two matrix column vectors define quaternion X and XY plane
   Quaternion q = new Quaternion(matrixRotate);
   atomSetCollection.setAtomSetCollectionAuxiliaryInfo("defaultOrientationQuaternion", q);
   Logger.info("defaultOrientationMatrix = " + matrixRotate);
 }
Ejemplo n.º 10
0
  /**
   * Adds a new physics block to be rendered as a rigid body. Translucent blocks are ignored.
   *
   * @param position The position
   * @param type The block type
   * @param impulse An impulse
   * @param size The size of the block
   * @return The created rigid body (if any)
   */
  public synchronized BlockRigidBody addBlock(
      Vector3f position, byte type, Vector3f impulse, BLOCK_SIZE size, boolean temporary) {
    if (temporary && _blocks.size() > MAX_TEMP_BLOCKS) return null;

    BoxShape shape = _blockShape;
    Block block = BlockManager.getInstance().getBlock(type);

    if (block.isTranslucent()) return null;

    if (size == BLOCK_SIZE.HALF_SIZE) shape = _blockShapeHalf;
    else if (size == BLOCK_SIZE.QUARTER_SIZE) shape = _blockShapeQuarter;

    Matrix3f rot = new Matrix3f();
    rot.setIdentity();

    DefaultMotionState blockMotionState =
        new DefaultMotionState(new Transform(new Matrix4f(rot, position, 1.0f)));

    Vector3f fallInertia = new Vector3f();
    shape.calculateLocalInertia(block.getMass(), fallInertia);

    RigidBodyConstructionInfo blockCI =
        new RigidBodyConstructionInfo(block.getMass(), blockMotionState, shape, fallInertia);

    BlockRigidBody rigidBlock = new BlockRigidBody(blockCI, type);
    rigidBlock.setRestitution(0.0f);
    rigidBlock.setAngularFactor(0.5f);
    rigidBlock.setFriction(0.5f);
    rigidBlock._temporary = temporary;

    // Apply impulse
    rigidBlock.applyImpulse(impulse, new Vector3f(0.0f, 0.0f, 0.0f));

    _insertionQueue.add(rigidBlock);

    return rigidBlock;
  }
Ejemplo n.º 11
0
  RigidBody createBody(
      float mass,
      Vector3f position,
      CollisionShape shape,
      Float initialVelocity,
      Matrix3f orientation) {

    // Create Dynamic Objects
    Transform startTransform = new Transform();
    startTransform.setIdentity();
    // rigidbody is dynamic if and only if mass is non zero,
    // otherwise static
    boolean isDynamic = (mass != 0f);
    Vector3f localInertia = new Vector3f(0, 0, 0);
    if (isDynamic) {
      shape.calculateLocalInertia(mass, localInertia);
    }
    if (orientation != null) {
      startTransform.set(orientation);
    }
    startTransform.origin.set(position);
    // using motionstate is recommended, it provides
    // interpolation capabilities, and only synchronizes
    // 'active' objects
    DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
    RigidBodyConstructionInfo rbInfo =
        new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);

    RigidBody rb = new RigidBody(rbInfo);
    rb.setFriction(0.4f);
    // rb.setDamping(0.7f, 0.7f);
    if (initialVelocity != 0f && orientation != null) {
      synchronized (initialVelocityVector) {
        initialVelocityVector.set(0f, 0f, initialVelocity);
        orientation.transform(initialVelocityVector);
        rb.setLinearVelocity(initialVelocityVector);
      }
    }
    return rb;
  }
Ejemplo n.º 12
0
  @SuppressWarnings("unchecked")
  public static String toJSON(String infoType, Object info) {

    // Logger.debug(infoType+" -- "+info);

    StringBuilder sb = new StringBuilder();
    String sep = "";
    if (info == null) return packageJSON(infoType, (String) null);
    if (info instanceof Integer || info instanceof Float || info instanceof Double)
      return packageJSON(infoType, info.toString());
    if (info instanceof String) return packageJSON(infoType, fixString((String) info));
    if (info instanceof String[]) {
      sb.append("[");
      int imax = ((String[]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(fixString(((String[]) info)[i]));
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof int[]) {
      sb.append("[");
      int imax = ((int[]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(((int[]) info)[i]);
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof float[]) {
      sb.append("[");
      int imax = ((float[]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(((float[]) info)[i]);
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof double[]) {
      sb.append("[");
      int imax = ((double[]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(((double[]) info)[i]);
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof Point3f[]) {
      sb.append("[");
      int imax = ((Point3f[]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep);
        addJsonTuple(sb, ((Point3f[]) info)[i]);
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof String[][]) {
      sb.append("[");
      int imax = ((String[][]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(toJSON(null, ((String[][]) info)[i]));
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof int[][]) {
      sb.append("[");
      int imax = ((int[][]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(toJSON(null, ((int[][]) info)[i]));
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof float[][]) {
      sb.append("[");
      int imax = ((float[][]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(toJSON(null, ((float[][]) info)[i]));
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof float[][][]) {
      sb.append("[");
      int imax = ((float[][][]) info).length;
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(toJSON(null, ((float[][][]) info)[i]));
        sep = ",";
      }
      sb.append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof List) {
      sb.append("[ ");
      int imax = ((List<?>) info).size();
      for (int i = 0; i < imax; i++) {
        sb.append(sep).append(toJSON(null, ((List<?>) info).get(i)));
        sep = ",";
      }
      sb.append(" ]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof Matrix4f) {
      float[] x = new float[4];
      Matrix4f m4 = (Matrix4f) info;
      sb.append('[');
      for (int i = 0; i < 4; i++) {
        if (i > 0) sb.append(',');
        m4.getRow(i, x);
        sb.append(toJSON(null, x));
      }
      sb.append(']');
      return packageJSON(infoType, sb);
    }
    if (info instanceof Matrix3f) {
      float[] x = new float[3];
      Matrix3f m3 = (Matrix3f) info;
      sb.append('[');
      for (int i = 0; i < 3; i++) {
        if (i > 0) sb.append(',');
        m3.getRow(i, x);
        sb.append(toJSON(null, x));
      }
      sb.append(']');
      return packageJSON(infoType, sb);
    }
    if (info instanceof Tuple3f) {
      addJsonTuple(sb, (Tuple3f) info);
      return packageJSON(infoType, sb);
    }
    if (info instanceof AxisAngle4f) {
      sb.append("[")
          .append(((AxisAngle4f) info).x)
          .append(",")
          .append(((AxisAngle4f) info).y)
          .append(",")
          .append(((AxisAngle4f) info).z)
          .append(",")
          .append((float) (((AxisAngle4f) info).angle * 180d / Math.PI))
          .append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof Point4f) {
      sb.append("[")
          .append(((Point4f) info).x)
          .append(",")
          .append(((Point4f) info).y)
          .append(",")
          .append(((Point4f) info).z)
          .append(",")
          .append(((Point4f) info).w)
          .append("]");
      return packageJSON(infoType, sb);
    }
    if (info instanceof Map) {
      sb.append("{ ");
      Iterator<String> e = ((Map<String, ?>) info).keySet().iterator();
      while (e.hasNext()) {
        String key = e.next();
        sb.append(sep).append(packageJSON(key, toJSON(null, ((Map<?, ?>) info).get(key))));
        sep = ",";
      }
      sb.append(" }");
      return packageJSON(infoType, sb);
    }
    return packageJSON(infoType, fixString(info.toString()));
  }
Ejemplo n.º 13
0
  @Override
  public void buildJacobian() {
    Vector3f tmp = Stack.alloc(Vector3f.class);
    Vector3f tmp1 = Stack.alloc(Vector3f.class);
    Vector3f tmp2 = Stack.alloc(Vector3f.class);

    Transform tmpTrans = Stack.alloc(Transform.class);

    appliedImpulse = 0f;

    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;

    if (!angularOnly) {
      Vector3f pivotAInW = Stack.alloc(rbAFrame.origin);
      rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);

      Vector3f pivotBInW = Stack.alloc(rbBFrame.origin);
      rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);

      Vector3f relPos = Stack.alloc(Vector3f.class);
      relPos.sub(pivotBInW, pivotAInW);

      // TODO: stack
      Vector3f[] normal /*[3]*/ =
          new Vector3f[] {
            Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class)
          };
      if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
        normal[0].normalize(relPos);
      } else {
        normal[0].set(1f, 0f, 0f);
      }

      TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);

      for (int i = 0; i < 3; i++) {
        Matrix3f mat1 = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
        mat1.transpose();

        Matrix3f mat2 = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
        mat2.transpose();

        tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
        tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));

        jac[i].init(
            mat1,
            mat2,
            tmp1,
            tmp2,
            normal[i],
            rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
            rbA.getInvMass(),
            rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
            rbB.getInvMass());
      }
    }

    Vector3f b1Axis1 = Stack.alloc(Vector3f.class),
        b1Axis2 = Stack.alloc(Vector3f.class),
        b1Axis3 = Stack.alloc(Vector3f.class);
    Vector3f b2Axis1 = Stack.alloc(Vector3f.class), b2Axis2 = Stack.alloc(Vector3f.class);

    rbAFrame.basis.getColumn(0, b1Axis1);
    getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);

    rbBFrame.basis.getColumn(0, b2Axis1);
    getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);

    float swing1 = 0f, swing2 = 0f;

    float swx = 0f, swy = 0f;
    float thresh = 10f;
    float fact;

    // Get Frame into world space
    if (swingSpan1 >= 0.05f) {
      rbAFrame.basis.getColumn(1, b1Axis2);
      getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
      //			swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
      swx = b2Axis1.dot(b1Axis1);
      swy = b2Axis1.dot(b1Axis2);
      swing1 = ScalarUtil.atan2Fast(swy, swx);
      fact = (swy * swy + swx * swx) * thresh * thresh;
      fact = fact / (fact + 1f);
      swing1 *= fact;
    }

    if (swingSpan2 >= 0.05f) {
      rbAFrame.basis.getColumn(2, b1Axis3);
      getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
      //			swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
      swx = b2Axis1.dot(b1Axis1);
      swy = b2Axis1.dot(b1Axis3);
      swing2 = ScalarUtil.atan2Fast(swy, swx);
      fact = (swy * swy + swx * swx) * thresh * thresh;
      fact = fact / (fact + 1f);
      swing2 *= fact;
    }

    float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
    float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
    float EllipseAngle =
        Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;

    if (EllipseAngle > 1.0f) {
      swingCorrection = EllipseAngle - 1.0f;
      solveSwingLimit = true;

      // Calculate necessary axis & factors
      tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
      tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
      tmp.add(tmp1, tmp2);
      swingAxis.cross(b2Axis1, tmp);
      swingAxis.normalize();

      float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
      swingAxis.scale(swingAxisSign);

      kSwing =
          1f
              / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis)
                  + getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
    }

    // Twist limits
    if (twistSpan >= 0f) {
      // Vector3f b2Axis2 = Stack.alloc(Vector3f.class);
      rbBFrame.basis.getColumn(1, b2Axis2);
      getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);

      Quat4f rotationArc =
          QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, Stack.alloc(Quat4f.class));
      Vector3f TwistRef =
          QuaternionUtil.quatRotate(rotationArc, b2Axis2, Stack.alloc(Vector3f.class));
      float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));

      float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
      if (twist <= -twistSpan * lockedFreeFactor) {
        twistCorrection = -(twist + twistSpan);
        solveTwistLimit = true;

        twistAxis.add(b2Axis1, b1Axis1);
        twistAxis.scale(0.5f);
        twistAxis.normalize();
        twistAxis.scale(-1.0f);

        kTwist =
            1f
                / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis)
                    + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));

      } else if (twist > twistSpan * lockedFreeFactor) {
        twistCorrection = (twist - twistSpan);
        solveTwistLimit = true;

        twistAxis.add(b2Axis1, b1Axis1);
        twistAxis.scale(0.5f);
        twistAxis.normalize();

        kTwist =
            1f
                / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis)
                    + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
      }
    }
  }