/* (non-Javadoc)
@see org.joml.Quaterniondc#nlerpIterative(org.joml.Quaterniondc, double, double, org.joml.Quaterniond)
509059
public Quaterniond nlerpIterative(Quaterniondc q, double alpha, double dotThreshold, Quaterniond dest) {
        double q1x = x, q1y = y, q1z = z, q1w = w;
        double q2x = q.x(), q2y = q.y(), q2z = q.z(), q2w = q.w();
        double dot = q1x * q2x + q1y * q2y + q1z * q2z + q1w * q2w;
        double absDot = Math.abs(dot);
        if (1.0 - 1E-6 < absDot) {
            return dest.set(this);
        }
        double alphaN = alpha;
        while (absDot < dotThreshold) {
            double scale0 = 0.5;
            double scale1 = dot >= 0.0 ? 0.5 : -0.5;
            if (alphaN < 0.5) {
                q2x = scale0 * q2x + scale1 * q1x;
                q2y = scale0 * q2y + scale1 * q1y;
                q2z = scale0 * q2z + scale1 * q1z;
                q2w = scale0 * q2w + scale1 * q1w;
                double s = 1.0 / Math.sqrt(q2x * q2x + q2y * q2y + q2z * q2z + q2w * q2w);
                q2x *= s;
                q2y *= s;
                q2z *= s;
                q2w *= s;
                alphaN = alphaN + alphaN;
            } else {
                q1x = scale0 * q1x + scale1 * q2x;
                q1y = scale0 * q1y + scale1 * q2y;
                q1z = scale0 * q1z + scale1 * q2z;
                q1w = scale0 * q1w + scale1 * q2w;
                double s = 1.0 / Math.sqrt(q1x * q1x + q1y * q1y + q1z * q1z + q1w * q1w);
                q1x *= s;
                q1y *= s;
                q1z *= s;
                q1w *= s;
                alphaN = alphaN + alphaN - 1.0;
            }
            dot = q1x * q2x + q1y * q2y + q1z * q2z + q1w * q2w;
            absDot = Math.abs(dot);
        }
        double scale0 = 1.0 - alphaN;
        double scale1 = dot >= 0.0 ? alphaN : -alphaN;
        double destX = scale0 * q1x + scale1 * q2x;
        double destY = scale0 * q1y + scale1 * q2y;
        double destZ = scale0 * q1z + scale1 * q2z;
        double destW = scale0 * q1w + scale1 * q2w;
        double s = 1.0 / Math.sqrt(destX * destX + destY * destY + destZ * destZ + destW * destW);
        dest.x *= s;
        dest.y *= s;
        dest.z *= s;
        dest.w *= s;
        return dest;
    }
310744
private static int winding(final Point2d[] coords, final int[] winding) {
        int cw = 0, ccw = 0;

        Point2d prev = coords[coords.length - 1];
        for (int i = 0; i < coords.length; i++) {
            Point2d curr = coords[i];
            Point2d next = coords[(i + 1) % coords.length];
            winding[i] = winding(prev, curr, next);

            if (winding[i] < 0)
                cw++;
            else if (winding[i] > 0)
                ccw++;
            else
                return 0;

            prev = curr;
        }

        if (cw == ccw)
            return 0;

        return cw > ccw ? CW : CCW;
    }
562377
public Quaterniond nlerp(Quaterniondc q, double factor, Quaterniond dest) {
        double cosom = x * q.x() + y * q.y() + z * q.z() + w * q.w();
        double scale0 = 1.0 - factor;
        double scale1 = (cosom >= 0.0) ? factor : -factor;
        dest.x = scale0 * x + scale1 * q.x();
        dest.y = scale0 * y + scale1 * q.y();
        dest.z = scale0 * z + scale1 * q.z();
        dest.w = scale0 * w + scale1 * q.w();
        double s = 1.0 / Math.sqrt(dest.x * dest.x + dest.y * dest.y + dest.z * dest.z + dest.w * dest.w);
        dest.x *= s;
        dest.y *= s;
        dest.z *= s;
        dest.w *= s;
        return dest;
    }
763870
public static double angleSparseDense(SparseNumberVector v1, NumberVector v2) {
    // TODO: exploit precomputed length, when available.
    final int dim2 = v2.getDimensionality();
    double l1 = 0., l2 = 0., cross = 0.;
    int i1 = v1.iter(), d2 = 0;
    while(v1.iterValid(i1)) {
      final int d1 = v1.iterDim(i1);
      while(d2 < d1 && d2 < dim2) {
        final double val = v2.doubleValue(d2);
        l2 += val * val;
        ++d2;
      }
      if(d2 < dim2) {
        assert (d1 == d2) : "Dimensions not ordered";
        final double val1 = v1.iterDoubleValue(i1);
        final double val2 = v2.doubleValue(d2);
        l1 += val1 * val1;
        l2 += val2 * val2;
        cross += val1 * val2;
        i1 = v1.iterAdvance(i1);
        ++d2;
      }
      else {
        final double val = v1.iterDoubleValue(i1);
        l1 += val * val;
        i1 = v1.iterAdvance(i1);
      }
    }
    while(d2 < dim2) {
      final double val = v2.doubleValue(d2);
      l2 += val * val;
      ++d2;
    }
    final double a = (cross == 0.) ? 0. : //
        (l1 == 0. || l2 == 0.) ? 1. : //
            FastMath.sqrt((cross / l1) * (cross / l2));
    return (a < 1.) ? a : 1.;
  }
578197
public void normalize() {

        double angle = this.getAngle();
        if (Utils.within(angle, 0, 1) || Utils.within(angle, 180, 1)) { // almost horizontal
            this.setLine(this.x1, this.y1, this.x2, this.y1);
        }
        else if (Utils.within(angle, 90, 1) || Utils.within(angle, 270, 1)) { // almost vertical
            this.setLine(this.x1, this.y1, this.x1, this.y2);
        }
//        else {
//            System.out.println("oblique: " + this + " ("+ this.getAngle() + ")");
//        }
    }
678236
@Override
	public double integrate(DoubleUnaryOperator integrand) {
		double	lowerBound			= getLowerBound();
		double	upperBound			= getUpperBound();
		double	range				= upperBound-lowerBound;
		int		numberOfIntervalls	= (int) ((numberOfEvaluationPoints-1) / 2.0);

		double fullIntervall = range / numberOfIntervalls;
		double halfIntervall = 0.5 * fullIntervall;

		IntStream intervals = IntStream.range(1, numberOfIntervalls);
		if(useParallelEvaluation) {
			intervals = intervals.parallel();
		}

		double sum = intervals.mapToDouble(
				new IntToDoubleFunction() {
					@Override
					public double applyAsDouble(int i) {
						return 2 * integrand.applyAsDouble(lowerBound + i * fullIntervall + halfIntervall) + integrand.applyAsDouble(lowerBound + i * fullIntervall);
					}
				}
				).sum();

		sum += 2.0 * integrand.applyAsDouble(lowerBound + halfIntervall);

		return (integrand.applyAsDouble(lowerBound) + integrand.applyAsDouble(upperBound) + 2.0 * sum) / 6.0 * fullIntervall;
	}
135886
protected PolynomialApproximation knnDistanceApproximation() {
    int p_max = 0;
    double[] b = null;
    for(int i = 0; i < getNumEntries(); i++) {
      MkAppEntry entry = getEntry(i);
      PolynomialApproximation approximation = entry.getKnnDistanceApproximation();
      if(b == null) {
        p_max = approximation.getPolynomialOrder();
        b = new double[p_max];
      }
      for(int p = 0; p < p_max; p++) {
        b[p] += approximation.getB(p);
      }
    }

    for(int p = 0; p < p_max; p++) {
      b[p] /= p_max;
    }

    if(LoggingConfiguration.DEBUG) {
      StringBuilder msg = new StringBuilder();
      msg.append("b " + FormatUtil.format(b, FormatUtil.NF4));
      Logger.getLogger(this.getClass().getName()).fine(msg.toString());
    }

    return new PolynomialApproximation(b);
  }
785859
private void tql2 (double[][] V) {

   //  This is derived from the Algol procedures tql2, by
   //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
   //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
   //  Fortran subroutine in EISPACK.
   
      for (int i = 1; i < n; i++) {
         e[i-1] = e[i];
      }
      e[n-1] = 0.0;
   
      double f = 0.0;
      double tst1 = 0.0;
      double eps = Math.pow(2.0,-52.0);
      for (int l = 0; l < n; l++) {

         // Find small subdiagonal element
   
         tst1 = Math.max(tst1,Math.abs(d[l]) + Math.abs(e[l]));
         int m = l;
         while (m < n) {
            if (Math.abs(e[m]) <= eps*tst1) {
               break;
            }
            m++;
         }
   
         // If m == l, d[l] is an eigenvalue,
         // otherwise, iterate.
   
         if (m > l) {
            int iter = 0;
            do {
               iter = iter + 1;  // (Could check iteration count here.)
   
               // Compute implicit shift
   
               double g = d[l];
               double p = (d[l+1] - g) / (2.0 * e[l]);
               double r = Math.hypot(p,1.0);
               if (p < 0) {
                  r = -r;
               }
               d[l] = e[l] / (p + r);
               d[l+1] = e[l] * (p + r);
               double dl1 = d[l+1];
               double h = g - d[l];
               for (int i = l+2; i < n; i++) {
                  d[i] -= h;
               }
               f = f + h;
   
               // Implicit QL transformation.
   
               p = d[m];
               double c = 1.0;
               double c2 = c;
               double c3 = c;
               double el1 = e[l+1];
               double s = 0.0;
               double s2 = 0.0;
               for (int i = m-1; i >= l; i--) {
                  c3 = c2;
                  c2 = c;
                  s2 = s;
                  g = c * e[i];
                  h = c * p;
                  r = Math.hypot(p,e[i]);
                  e[i+1] = s * r;
                  s = e[i] / r;
                  c = p / r;
                  p = c * d[i] - s * g;
                  d[i+1] = h + s * (c * g + s * d[i]);
   
                  // Accumulate transformation.
   
                  for (int k = 0; k < n; k++) {
                     h = V[k][i+1];
                     V[k][i+1] = s * V[k][i] + c * h;
                     V[k][i] = c * V[k][i] - s * h;
                  }
               }
               p = -s * s2 * c3 * el1 * e[l] / dl1;
               e[l] = s * p;
               d[l] = c * p;
   
               // Check for convergence.
   
            } while (Math.abs(e[l]) > eps*tst1);
         }
         d[l] = d[l] + f;
         e[l] = 0.0;
      }
     
      // Sort eigenvalues and corresponding vectors.
   
      for (int i = 0; i < n-1; i++) {
         int k = i;
         double p = d[i];
         for (int j = i+1; j < n; j++) {
            if (d[j] < p) {
               k = j;
               p = d[j];
            }
         }
         if (k != i) {
            d[k] = d[i];
            d[i] = p;
            for (int j = 0; j < n; j++) {
               p = V[j][i];
               V[j][i] = V[j][k];
               V[j][k] = p;
            }
         }
      }
   }
530258
@Override
    public void scal(long N, double alpha, INDArray X) {
        switch (X.data().dataType()) {
            case DOUBLE:
                dscal(N, alpha, X, 1);
                break;
            case FLOAT:
                sscal(N, alpha, X, 1);
                break;
            case HALF:
                hscal(N, alpha, X, 1);
                break;
            default:
                throw new UnsupportedOperationException();
        }

    }
57043
private void orthes() {
    // FIXME: does this fail on NaN/inf values?

    // This is derived from the Algol procedures orthes and ortran,
    // by Martin and Wilkinson, Handbook for Auto. Comp.,
    // Vol.ii-Linear Algebra, and the corresponding
    // Fortran subroutines in EISPACK.

    final int low = 0, high = n - 1;
    for(int m = low + 1; m <= high - 1; m++) {
      // Scale column.
      double scale = 0.0;
      for(int i = m; i <= high; i++) {
        scale += abs(H[i][m - 1]);
      }
      if(scale > 0.0 || scale < 0.0) {
        // Compute Householder transformation.
        double h = 0.0;
        for(int i = high; i >= m; i--) {
          double oi = ort[i] = H[i][m - 1] / scale;
          h += oi * oi;
        }
        double g = FastMath.sqrt(h), om = ort[m];
        g = (om > 0) ? -g : g;
        h -= om * g;
        ort[m] = om - g;

        // Apply Householder similarity transformation
        // H = (I-u*u'/h)*H*(I-u*u')/h)

        for(int j = m; j < n; j++) {
          double f = 0.0;
          for(int i = high; i >= m; i--) {
            f += ort[i] * H[i][j];
          }
          f /= h;
          for(int i = m; i <= high; i++) {
            H[i][j] -= f * ort[i];
          }
        }

        for(int i = 0; i <= high; i++) {
          final double[] Hi = H[i];
          double f = 0.0;
          for(int j = high; j >= m; j--) {
            f += ort[j] * Hi[j];
          }
          f /= h;
          for(int j = m; j <= high; j++) {
            Hi[j] -= f * ort[j];
          }
        }
        ort[m] *= scale;
        H[m][m - 1] = scale * g;
      }
    }

    // Accumulate transformations (Algol's ortran).
    for(int i = 0; i < n; i++) {
      Arrays.fill(V[i], 0.);
      V[i][i] = 1.;
    }

    for(int m = high - 1; m >= low + 1; m--) {
      final double[] H_m = H[m];
      if(H_m[m - 1] != 0.0) {
        for(int i = m + 1; i <= high; i++) {
          ort[i] = H[i][m - 1];
        }
        final double ort_m = ort[m];
        for(int j = m; j <= high; j++) {
          double g = 0.0;
          for(int i = m; i <= high; i++) {
            g += ort[i] * V[i][j];
          }
          // Double division avoids possible underflow
          g = (g / ort_m) / H_m[m - 1];
          for(int i = m; i <= high; i++) {
            V[i][j] += g * ort[i];
          }
        }
      }
    }
  }
518071
@Override
	public void compute(float x, float y, Point2D_F32 out) {
		narrowToNorm.compute(x,y, norm);

		// Convert from 2D homogenous to 3D
		unit.set( norm.x , norm.y , 1.0f);

		// Rotate then make it a unit vector
		GeometryMath_F32.mult(rotateWideToNarrow,unit,unit);
		float n = unit.norm();
		unit.x /= n;
		unit.y /= n;
		unit.z /= n;

		unitToWide.compute(unit.x,unit.y,unit.z,out);
	}
1234492
@Override
	protected void doTransform(Position<T> transformable, float comp)
	{
		int fromX = reversed ? this.toX : this.fromX;
		int toX = reversed ? this.fromX : this.toX;
		int fromY = reversed ? this.toY : this.fromY;
		int toY = reversed ? this.fromY : this.toY;

		int x = Math.round(fromX + (toX - fromX) * comp);
		int y = Math.round(fromY + (toY - fromY) * comp);

		transformable.setPosition(x, y);
	}
1240826
public void rotate(Quaternion rotation) {
		Transform3D m = new Transform3D();
		m.setRotation(rotation);
		m.transform(this.getSegmentVector());
		this.getSegmentVector().normalize();
	}
1006392
@Override
    public Quaternionf wrapQuaternion(ByteBuffer buffer, int offset) {
    	float w = buffer.getFloat(offset);
    	float x = buffer.getFloat(offset + 4);
    	float y = buffer.getFloat(offset + 8);
    	float z = buffer.getFloat(offset + 12);
        return new Quaternionf(x, y, z, w);
    }
67608
@Override // from IMatrix4
    public Quaternion extractRotation (Quaternion result) throws SingularMatrixException {
        // start with the contents of the upper 3x3 portion of the matrix
        float n00 = this.m00, n10 = this.m10, n20 = this.m20;
        float n01 = this.m01, n11 = this.m11, n21 = this.m21;
        float n02 = this.m02, n12 = this.m12, n22 = this.m22;
        for (int ii = 0; ii < 10; ii++) {
            // store the results of the previous iteration
            float o00 = n00, o10 = n10, o20 = n20;
            float o01 = n01, o11 = n11, o21 = n21;
            float o02 = n02, o12 = n12, o22 = n22;

            // compute average of the matrix with its inverse transpose
            float sd00 = o11*o22 - o21*o12;
            float sd10 = o01*o22 - o21*o02;
            float sd20 = o01*o12 - o11*o02;
            float det = o00*sd00 + o20*sd20 - o10*sd10;
            if (Math.abs(det) == 0f) {
                // determinant is zero; matrix is not invertible
                throw new SingularMatrixException(this.toString());
            }
            float hrdet = 0.5f / det;
            n00 = +sd00 * hrdet + o00*0.5f;
            n10 = -sd10 * hrdet + o10*0.5f;
            n20 = +sd20 * hrdet + o20*0.5f;

            n01 = -(o10*o22 - o20*o12) * hrdet + o01*0.5f;
            n11 = +(o00*o22 - o20*o02) * hrdet + o11*0.5f;
            n21 = -(o00*o12 - o10*o02) * hrdet + o21*0.5f;

            n02 = +(o10*o21 - o20*o11) * hrdet + o02*0.5f;
            n12 = -(o00*o21 - o20*o01) * hrdet + o12*0.5f;
            n22 = +(o00*o11 - o10*o01) * hrdet + o22*0.5f;

            // compute the difference; if it's small enough, we're done
            float d00 = n00 - o00, d10 = n10 - o10, d20 = n20 - o20;
            float d01 = n01 - o01, d11 = n11 - o11, d21 = n21 - o21;
            float d02 = n02 - o02, d12 = n12 - o12, d22 = n22 - o22;
            if (d00*d00 + d10*d10 + d20*d20 + d01*d01 + d11*d11 + d21*d21 +
                d02*d02 + d12*d12 + d22*d22 < MathUtil.EPSILON) {
                break;
            }
        }
        // now that we have a nice orthogonal matrix, we can extract the rotation quaternion
        // using the method described in http://en.wikipedia.org/wiki/Rotation_matrix#Conversions
        float x2 = Math.abs(1f + n00 - n11 - n22);
        float y2 = Math.abs(1f - n00 + n11 - n22);
        float z2 = Math.abs(1f - n00 - n11 + n22);
        float w2 = Math.abs(1f + n00 + n11 + n22);
        result.set(
            0.5f * FloatMath.sqrt(x2) * (n12 >= n21 ? +1f : -1f),
            0.5f * FloatMath.sqrt(y2) * (n20 >= n02 ? +1f : -1f),
            0.5f * FloatMath.sqrt(z2) * (n01 >= n10 ? +1f : -1f),
            0.5f * FloatMath.sqrt(w2));
        return result;
    }
44192
@Override
	public void compute(float x, float y, Point2D_F32 out) {

		float[] radial = params.radial;
		float t1 = params.t1;
		float t2 = params.t2;

		float r2 = x*x + y*y;
		float ri2 = r2;

		float sum = 0;
		for( int i = 0; i < radial.length; i++ ) {
			sum += radial[i]*ri2;
			ri2 *= r2;
		}

		out.x = x*(1 + sum) + 2*t1*x*y + t2*(r2 + 2*x*x);
		out.y = y*(1 + sum) + t1*(r2 + 2*y*y) + 2*t2*x*y;
	}
537914
@Override
	public void compute(double x, double y, Point2D_F64 out) {
		narrowToNorm.compute(x,y, norm);

		// Convert from 2D homogenous to 3D
		unit.set( norm.x , norm.y , 1.0);

		// Rotate then make it a unit vector
		GeometryMath_F64.mult(rotateWideToNarrow,unit,unit);
		double n = unit.norm();
		unit.x /= n;
		unit.y /= n;
		unit.z /= n;

		unitToWide.compute(unit.x,unit.y,unit.z,out);
	}