예제 #1
0
  /**
   * <code>toRotationMatrix</code> converts this quaternion to a rotational matrix. The result is
   * stored in result.
   *
   * @param result The Matrix3f to store the result in.
   * @return the rotation matrix representation of this quaternion.
   */
  public Matrix3f toRotationMatrix(Matrix3f result) {

    float norm = norm();
    // we explicitly test norm against one here, saving a division
    // at the cost of a test and branch.  Is it worth it?
    float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0;

    // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
    // will be used 2-4 times each.
    float xs = x * s;
    float ys = y * s;
    float zs = z * s;
    float xx = x * xs;
    float xy = x * ys;
    float xz = x * zs;
    float xw = w * xs;
    float yy = y * ys;
    float yz = y * zs;
    float yw = w * ys;
    float zz = z * zs;
    float zw = w * zs;

    // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
    result.m00 = 1 - (yy + zz);
    result.m01 = (xy - zw);
    result.m02 = (xz + yw);
    result.m10 = (xy + zw);
    result.m11 = 1 - (xx + zz);
    result.m12 = (yz - xw);
    result.m20 = (xz - yw);
    result.m21 = (yz + xw);
    result.m22 = 1 - (xx + yy);

    return result;
  }
예제 #2
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));
  }
예제 #3
0
파일: Line.java 프로젝트: jmecn/learnJME3
  public void orthogonalLineFit(FloatBuffer points) {
    if (points == null) {
      return;
    }

    points.rewind();

    // compute average of points
    int length = points.remaining() / 3;

    BufferUtils.populateFromBuffer(origin, points, 0);
    for (int i = 1; i < length; i++) {
      BufferUtils.populateFromBuffer(compVec1, points, i);
      origin.addLocal(compVec1);
    }

    origin.multLocal(1f / (float) length);

    // compute sums of products
    float sumXX = 0.0f, sumXY = 0.0f, sumXZ = 0.0f;
    float sumYY = 0.0f, sumYZ = 0.0f, sumZZ = 0.0f;

    points.rewind();
    for (int i = 0; i < length; i++) {
      BufferUtils.populateFromBuffer(compVec1, points, i);
      compVec1.subtract(origin, compVec2);
      sumXX += compVec2.x * compVec2.x;
      sumXY += compVec2.x * compVec2.y;
      sumXZ += compVec2.x * compVec2.z;
      sumYY += compVec2.y * compVec2.y;
      sumYZ += compVec2.y * compVec2.z;
      sumZZ += compVec2.z * compVec2.z;
    }

    // find the smallest eigen vector for the direction vector
    compMat1.m00 = sumYY + sumZZ;
    compMat1.m01 = -sumXY;
    compMat1.m02 = -sumXZ;
    compMat1.m10 = -sumXY;
    compMat1.m11 = sumXX + sumZZ;
    compMat1.m12 = -sumYZ;
    compMat1.m20 = -sumXZ;
    compMat1.m21 = -sumYZ;
    compMat1.m22 = sumXX + sumYY;

    compEigen1.calculateEigen(compMat1);
    direction = compEigen1.getEigenVector(0);
  }
예제 #4
0
 /**
  * <code>toAxes</code> takes in an array of three vectors. Each vector corresponds to an axis of
  * the coordinate system defined by the quaternion rotation.
  *
  * @param axis the array of vectors to be filled.
  */
 public void toAxes(Vector3f axis[]) {
   Matrix3f tempMat = toRotationMatrix();
   axis[0] = tempMat.getColumn(0, axis[0]);
   axis[1] = tempMat.getColumn(1, axis[1]);
   axis[2] = tempMat.getColumn(2, axis[2]);
 }