Exemplo n.º 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;
  }
Exemplo n.º 2
0
  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);
  }