/**
 **
 ** IntersceneLmaFloat - Primitive float pose projection/Jacobian oracle
 **
 ** Copyright (C) 2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  IntersceneLmaFloat.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */

package com.elphel.imagej.tileprocessor;

import java.util.Arrays;

import Jama.Matrix;

/**
 * Primitive single-precision oracle for the CUAS three-angle pose Jacobian.
 *
 * <p>This is deliberately not a second general-purpose LMA implementation. It
 * clones only the useful {@code IntersceneLma.getFxDerivs()} boundary for the
 * current rotation-only pose fit: one independent projection/Jacobian result
 * per selected tile, flattened exactly as the CUDA implementation will consume
 * it. The legacy double implementation remains the behavioral oracle.</p>
 */
public final class IntersceneLmaFloat {
	public static final int NUM_PARAMS = 3;
	public static final int NUM_COMPONENTS = 2;
	public static final int LMA_PRODUCTS = NUM_PARAMS * NUM_PARAMS + NUM_PARAMS;
	public static final int LMA_RESULT_FLOATS = 1 + LMA_PRODUCTS + 2 * NUM_PARAMS;
	public static final int LMA_RESIDENT_RESULT_FLOATS = LMA_RESULT_FLOATS + 6;
	public static final int PREP_RESULT_FLOATS = 8; // rung C1 resident prepare compact result // By Claude on 07/17/2026
	public static final float INFINITY_DISPARITY = 0.01f;

	static final int ERS_XYZ = 0;
	static final int ERS_XYZ_DT = 3;
	static final int ERS_QUAT = 6;
	static final int ERS_QUAT_DT = 10;
	static final int ERS_LINE_STRIDE = 14;

	private static final float SCENE_UNITS_SCALE = 0.001f;
	private static final float LINE_ERR = 0.001f;
	private static final float PROJECTION_EPS = 1.0e-10f;
	private static final int PROJECTION_MAX_STEPS = 100;

	private IntersceneLmaFloat() {}

	/** Immutable primitive camera state captured after ERS setup. */
	public static final class Camera {
		public final int width;
		public final int height;
		public final float focalLength;
		public final float pixelSize;
		public final float distortionRadius;
		public final float disparityRadius;
		public final float lineTime;
		public final float stepR;
		public final float [] radial;
		public final float [] rByRDist;
		public final float [] ers;

		private Camera(final ErsCorrection source) {
			width = source.pixelCorrectionWidth;
			height = source.pixelCorrectionHeight;
			focalLength = (float) source.focalLength;
			pixelSize = (float) source.pixelSize;
			distortionRadius = (float) source.distortionRadius;
			disparityRadius = (float) source.disparityRadius;
			lineTime = (float) source.line_time;
			stepR = (float) source.getStepR();
			radial = new float [] {
					(float) source.distortionC,
					(float) source.distortionB,
					(float) source.distortionA,
					(float) source.distortionA5,
					(float) source.distortionA6,
					(float) source.distortionA7,
					(float) source.distortionA8};
			double [] table = source.getRByRDist();
			if (table == null) {
				source.getRByRDist(0.0, false); // lazily build the legacy table
				table = source.getRByRDist();
			}
			rByRDist = new float [table.length];
			for (int i = 0; i < table.length; i++) rByRDist[i] = (float) table[i];
			ers = source.copyErsPoseStateFloat();
		}

		public static Camera capture(final ErsCorrection source) {
			if (source == null) throw new IllegalArgumentException("null ErsCorrection");
			return new Camera(source);
		}
	}

	/** Flattened output: fx[2*n], optional parameter-major jt[3][2*n], and tile validity. */
	public static final class Result {
		public final int numTiles;
		public final float [] fx;
		public final float [] jt;
		public final byte [] valid;
		public final int numValid;

		public Result(
				final int numTiles,
				final float [] fx,
				final float [] jt,
				final byte [] valid,
				final int numValid) {
			this.numTiles = numTiles;
			this.fx = fx;
			this.jt = jt;
			this.valid = valid;
			this.numValid = numValid;
		}
	}

	/** Statistics for comparing two flattened float implementations. */
	public static final class ResultComparison {
		public int compared;
		public int actualOnlyValid;
		public int expectedOnlyValid;
		public float maxAbsFx;
		public float maxAbsJt;

		public String format(final String actualLabel, final String expectedLabel) {
			return String.format(
					"compared=%d, %s-only=%d, %s-only=%d, max|fx|=%g, max|J|=%g",
					compared, actualLabel, actualOnlyValid, expectedLabel, expectedOnlyValid,
					maxAbsFx, maxAbsJt);
		}
	}

	/** Statistics for the optional float-vs-current-double comparison gate. */
	public static final class Comparison {
		public int compared;
		public int floatOnlyValid;
		public int doubleOnlyValid;
		public float maxAbsFx;
		public float maxAbsJt;

		@Override
		public String toString() {
			return String.format(
					"float getFxDerivs: compared=%d, float-only=%d, double-only=%d, max|fx|=%g, max|J|=%g",
					compared, floatOnlyValid, doubleOnlyValid, maxAbsFx, maxAbsJt);
		}
	}

	/** One fixed three-angle LMA candidate: raw H/b, delta and updated parameter vector. */
	public static final class LmaStepResult {
		public final boolean valid;
		public final float [] products; // row-major H[3][3], then b[3], before damping
		public final float [] delta;
		public final float [] candidate;

		public LmaStepResult(
				final boolean valid,
				final float [] products,
				final float [] delta,
				final float [] candidate) {
			this.valid = valid;
			this.products = products;
			this.delta = delta;
			this.candidate = candidate;
		}
	}

	/** Statistics for checking the one-thread CUDA step against the Java float clone. */
	public static final class LmaStepComparison {
		public boolean validityMatch;
		public boolean bitExact;
		public float maxAbsProducts;
		public float maxAbsDelta;
		public float maxAbsCandidate;

		public String format() {
			return String.format(
					"valid-match=%s, bit-exact=%s, max|H,b|=%g, max|delta|=%g, max|candidate|=%g",
					validityMatch, bitExact, maxAbsProducts, maxAbsDelta, maxAbsCandidate);
		}
	}

	/** Float eigen/residual preparation immediately downstream of raw resident fx/J. */
	public static final class PreparedLma {
		public final float [] jt;
		public final float [] ymfxWeighted;
		public final float rms;
		public final float pureRms;

		public PreparedLma(
				final float [] jt,
				final float [] ymfxWeighted,
				final float rms,
				final float pureRms) {
			this.jt = jt;
			this.ymfxWeighted = ymfxWeighted;
			this.rms = rms;
			this.pureRms = pureRms;
		}
	}

	/** Compact resident step plus optional prepared arrays for a one-shot oracle gate. */
	/** Rung C1 (design 2026-07-16): compact result of the resident prepareLMA.
	 *  capturedY/Weights/Eigen are non-null only on the one-shot oracle call.
	 *  By Claude on 07/17/2026. */
	public static final class PrepareResidentResult {
		public final int numTiles;
		public final boolean valid;
		public final int numMeas;
		public final float sumWRaw;
		public final float pureWeight;
		public final float fullWeight;
		public final float [] regWeights;
		public final float [] capturedY;
		public final float [] capturedWeights;
		public final float [] capturedEigen;

		public PrepareResidentResult(
				final int numTiles,
				final boolean valid,
				final int numMeas,
				final float sumWRaw,
				final float pureWeight,
				final float fullWeight,
				final float [] regWeights,
				final float [] capturedY,
				final float [] capturedWeights,
				final float [] capturedEigen) {
			this.numTiles = numTiles;
			this.valid = valid;
			this.numMeas = numMeas;
			this.sumWRaw = sumWRaw;
			this.pureWeight = pureWeight;
			this.fullWeight = fullWeight;
			this.regWeights = regWeights;
			this.capturedY = capturedY;
			this.capturedWeights = capturedWeights;
			this.capturedEigen = capturedEigen;
		}
	}

	/** Serial float clone of the GPU resident prepare - the bit-exact oracle for
	 *  the captured buffers (assemble scatter is order-free; both tails are
	 *  serial in ascending order on the GPU by design). By Claude on 07/17/2026. */
	public static final class PrepareResidentOracle {
		public final float [] y;
		public final float [] weights;
		public final float [] eigen;
		public final int numMeas;
		public final float sumWRaw;
		public final float pureWeight;
		public final float fullWeight;
		public final float [] regWeights;

		PrepareResidentOracle(final float [] y, final float [] weights, final float [] eigen,
				final int numMeas, final float sumWRaw, final float pureWeight,
				final float fullWeight, final float [] regWeights) {
			this.y = y;
			this.weights = weights;
			this.eigen = eigen;
			this.numMeas = numMeas;
			this.sumWRaw = sumWRaw;
			this.pureWeight = pureWeight;
			this.fullWeight = fullWeight;
			this.regWeights = regWeights;
		}
	}

	public static PrepareResidentOracle prepareResidentOracle(
			final double [][] vectorXYSDS, // exact float widenings of the peak dx/dy/str
			final double [][] eigenSrc,    // {e0x,e0y,l0,l1} per tile, or null = identity mode
			final Result raw,              // raw float fx/J/valid at the current pose (readback)
			final float eigMinSqrt,
			final float eigMaxSqrt,
			final float minConfidence,
			final boolean sameWeights,
			final float [] pullTargets,
			final float [] paramRegweights) {
		final int numTiles = raw.numTiles;
		final int ns = NUM_COMPONENTS * numTiles;
		final int numValues = ns + NUM_PARAMS;
		final float [] y = new float [numValues];
		final float [] weights = new float [numValues];
		final float [] eigen = new float [4 * numTiles];
		// assemble (per-tile, order-free scatter on the GPU)
		final boolean useEigen = (eigenSrc != null);
		for (int tile = 0; tile < numTiles; tile++) {
			if (vectorXYSDS[tile] == null) continue; // unmeasured / kernel-rejected
			final float str = (float) vectorXYSDS[tile][2];
			if (!(str >= minConfidence)) continue;   // NaN fails here too
			float e0x = 0f, e0y = 0f, l0 = 0f, l1 = 0f;
			if (useEigen) {
				if (eigenSrc[tile] == null) continue; // null transform skips the tile
				e0x = (float) eigenSrc[tile][0];
				e0y = (float) eigenSrc[tile][1];
				l0 =  (float) eigenSrc[tile][2];
				l1 =  (float) eigenSrc[tile][3];
				if (!Float.isFinite(e0x) || !Float.isFinite(e0y) ||
						!Float.isFinite(l0) || !Float.isFinite(l1)) continue;
			}
			if (!(str > 0.0f)) continue;
			final float w = sameWeights ? 1.0f : str;
			final int vi = NUM_COMPONENTS * tile;
			weights[vi] = w;
			y[vi] =     (float) vectorXYSDS[tile][0];
			y[vi + 1] = (float) vectorXYSDS[tile][1];
			final int ei = 4 * tile;
			if (useEigen) {
				final float k0inv = 1.0f / eigMaxSqrt;
				final float k0 = Math.max(0.0f,
						1.0f / Math.max(eigMinSqrt, (float) Math.sqrt(l0)) - k0inv);
				final float k1 = Math.max(0.0f,
						1.0f / Math.max(eigMinSqrt, (float) Math.sqrt(l1)) - k0inv);
				eigen[ei]     = k0 * e0x;
				eigen[ei + 1] = k0 * e0y;
				eigen[ei + 2] = k1 * e0y;
				eigen[ei + 3] = k1 * -e0x;
			} else {
				eigen[ei] = 1.0f;
				eigen[ei + 3] = 1.0f;
			}
		}
		// tail 1: sample-weight normalization (serial ascending, GPU order)
		float sum = 0.0f;
		int numMeas = 0;
		for (int tile = 0; tile < numTiles; tile++) {
			final float w = weights[NUM_COMPONENTS * tile];
			sum += w + w;
			if (w > 0.0f) numMeas++;
		}
		final float sNorm = 1.0f / sum;
		for (int tile = 0; tile < numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			final float w = weights[vi] * sNorm;
			weights[vi] = w;
			weights[vi + 1] = w;
		}
		for (int par = 0; par < NUM_PARAMS; par++) {
			y[ns + par] = pullTargets[par];
		}
		// tail 2: y += fx (measured+valid), H diagonal, reg weights, full normalize
		for (int tile = 0; tile < numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			if ((weights[vi] > 0.0f) && (raw.valid[tile] != 0)) {
				y[vi] = y[vi] + raw.fx[vi];
				y[vi + 1] = y[vi + 1] + raw.fx[vi + 1];
			}
		}
		final float [] h = new float [NUM_PARAMS];
		for (int tile = 0; tile < numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			final float w = weights[vi];
			if (!(w > 0.0f)) continue;
			final boolean haveRaw = raw.valid[tile] != 0;
			final int ei = 4 * tile;
			for (int par = 0; par < NUM_PARAMS; par++) {
				final int rawI = par * ns + vi;
				final float jx = haveRaw ? raw.jt[rawI] : 0.0f;
				final float jy = haveRaw ? raw.jt[rawI + 1] : 0.0f;
				final float tj0 = eigen[ei] * jx + eigen[ei + 1] * jy;
				final float tj1 = eigen[ei + 2] * jx + eigen[ei + 3] * jy;
				h[par] = h[par] + w * (tj0 * tj0);
				h[par] = h[par] + w * (tj1 * tj1);
			}
		}
		final float [] regWeights = new float [NUM_PARAMS];
		for (int par = 0; par < NUM_PARAMS; par++) {
			regWeights[par] = (h[par] > 0.0f) ?
					(paramRegweights[par] / (float) Math.sqrt(h[par])) : 0.0f;
			weights[ns + par] = regWeights[par];
		}
		float pure = 0.0f;
		for (int i = 0; i < ns; i++) pure += weights[i];
		float full = pure;
		for (int par = 0; par < NUM_PARAMS; par++) full += weights[ns + par];
		final float scale = 1.0f / full;
		for (int i = 0; i < numValues; i++) weights[i] *= scale;
		return new PrepareResidentOracle(y, weights, eigen,
				numMeas, sum, pure / full, full, regWeights);
	}

	/** One-shot comparison of the captured GPU prepare buffers vs the serial
	 *  float clone: exact float mismatch counts (0 expected). By Claude on 07/17/2026. */
	public static String comparePrepareResident(
			final PrepareResidentResult gpu,
			final PrepareResidentOracle oracle) {
		int badY = 0, badW = 0, badE = 0;
		for (int i = 0; i < oracle.y.length; i++) {
			if (Float.floatToIntBits(gpu.capturedY[i]) != Float.floatToIntBits(oracle.y[i])) badY++;
		}
		for (int i = 0; i < oracle.weights.length; i++) {
			if (Float.floatToIntBits(gpu.capturedWeights[i]) != Float.floatToIntBits(oracle.weights[i])) badW++;
		}
		for (int i = 0; i < oracle.eigen.length; i++) {
			if (Float.floatToIntBits(gpu.capturedEigen[i]) != Float.floatToIntBits(oracle.eigen[i])) badE++;
		}
		return String.format(
				"y mismatches=%d, weight mismatches=%d, eigen mismatches=%d; "+
				"num_meas %d/%d, pure_weight %g/%g, full_weight %g/%g",
				badY, badW, badE, gpu.numMeas, oracle.numMeas,
				gpu.pureWeight, oracle.pureWeight, gpu.fullWeight, oracle.fullWeight);
	}

	public static final class ResidentLmaStepResult {
		public final LmaStepResult step;
		public final PreparedLma prepared;
		public final float currentRms;
		public final float currentPureRms;
		public final float candidateRms;
		public final float candidatePureRms;
		public final boolean accepted;
		public final boolean stop;

		public ResidentLmaStepResult(
				final LmaStepResult step,
				final PreparedLma prepared,
				final float currentRms,
				final float currentPureRms,
				final float candidateRms,
				final float candidatePureRms,
				final boolean accepted,
				final boolean stop) {
			this.step = step;
			this.prepared = prepared;
			this.currentRms = currentRms;
			this.currentPureRms = currentPureRms;
			this.candidateRms = candidateRms;
			this.candidatePureRms = candidatePureRms;
			this.accepted = accepted;
			this.stop = stop;
		}
	}

	/** Statistics for the resident preparation arrays and current-point RMS. */
	public static final class PreparationComparison {
		public boolean bitExact;
		public float maxAbsJt;
		public float maxAbsYmfx;
		public float absRms;
		public float absPureRms;

		public String format() {
			return String.format(
					"bit-exact=%s, max|J|=%g, max|W(y-f)|=%g, |RMS|=%g, |pure RMS|=%g",
					bitExact, maxAbsJt, maxAbsYmfx, absRms, absPureRms);
		}
	}

	/**
	 * Primitive-float clone of the fixed three-angle candidate calculation in
	 * {@code IntersceneLma.lmaStep()}. It deliberately keeps all reductions and
	 * the explicit 3x3 solve serial so the matching CUDA kernel can use one
	 * thread; parallel tile preparation/reduction is a later residency layer.
	 */
	public static LmaStepResult lmaStep(
			final float lambda,
			final float [] weights,
			final float [] jt,
			final float [] ymfxWeighted,
			final float [] vector) {
		if ((weights == null) || (weights.length == 0) ||
				(jt == null) || (jt.length != NUM_PARAMS * weights.length) ||
				(ymfxWeighted == null) || (ymfxWeighted.length != weights.length) ||
				(vector == null) || (vector.length != NUM_PARAMS)) {
			throw new IllegalArgumentException("inconsistent fixed pose LMA arrays");
		}
		final int numValues = weights.length;
		final float [] products = new float [LMA_PRODUCTS];
		for (int row = 0; row < NUM_PARAMS; row++) {
			final int ri = row * numValues;
			for (int column = row; column < NUM_PARAMS; column++) {
				final int ci = column * numValues;
				float sum = 0.0f;
				for (int value = 0; value < numValues; value++) {
					sum += (weights[value] * jt[ri + value]) * jt[ci + value];
				}
				products[row * NUM_PARAMS + column] = sum;
				products[column * NUM_PARAMS + row] = sum;
			}
			float sum = 0.0f;
			for (int value = 0; value < numValues; value++) {
				sum += jt[ri + value] * ymfxWeighted[value];
			}
			products[NUM_PARAMS * NUM_PARAMS + row] = sum;
		}

		final float [] damped = Arrays.copyOf(products, NUM_PARAMS * NUM_PARAMS);
		for (int i = 0; i < NUM_PARAMS; i++) {
			final int diagonal = i * NUM_PARAMS + i;
			damped[diagonal] += damped[diagonal] * lambda;
		}
		final float [] inverse = inverse3x3(damped);
		final float [] delta = new float [] {Float.NaN, Float.NaN, Float.NaN};
		final float [] candidate = vector.clone();
		if (inverse == null) return new LmaStepResult(false, products, delta, candidate);
		final int bi = NUM_PARAMS * NUM_PARAMS;
		for (int row = 0; row < NUM_PARAMS; row++) {
			delta[row] = inverse[row * NUM_PARAMS] * products[bi] +
					inverse[row * NUM_PARAMS + 1] * products[bi + 1] +
					inverse[row * NUM_PARAMS + 2] * products[bi + 2];
			candidate[row] += delta[row];
		}
		return new LmaStepResult(allFinite(delta) && allFinite(candidate), products, delta, candidate);
	}

	/**
	 * Primitive-float oracle for the resident per-tile preparation kernel. Raw
	 * projection/Jacobian values are eigen-transformed, weighted residuals are
	 * formed, and the three pull rows are appended without any general matrix API.
	 */
	public static PreparedLma prepareLma(
			final Result raw,
			final float [] y,
			final float [] weights,
			final float [] eigen,
			final float [] vector,
			final float pureWeight) {
		if ((raw == null) || (raw.numTiles <= 0) || (raw.fx == null) ||
				(raw.fx.length != NUM_COMPONENTS * raw.numTiles) || (raw.jt == null) ||
				(raw.jt.length != NUM_PARAMS * NUM_COMPONENTS * raw.numTiles) ||
				(raw.valid == null) || (raw.valid.length != raw.numTiles) ||
				(y == null) || (weights == null) ||
				(y.length != NUM_COMPONENTS * raw.numTiles + NUM_PARAMS) ||
				(weights.length != y.length) || (eigen == null) ||
				(eigen.length != 4 * raw.numTiles) || (vector == null) ||
				(vector.length != NUM_PARAMS) || !finite(pureWeight) || !(pureWeight > 0.0f)) {
			throw new IllegalArgumentException("inconsistent resident pose LMA preparation arrays");
		}
		final int numValues = y.length;
		final float [] transformedJt = new float [NUM_PARAMS * numValues];
		final float [] ymfx = new float [numValues];
		float pureL2 = 0.0f;
		for (int tile = 0; tile < raw.numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			final int ei = 4 * tile;
			final boolean useTile = weights[vi] > 0.0f;
			final boolean haveRaw = useTile && (raw.valid[tile] != 0);
			final float rawX = haveRaw ? raw.fx[vi] : 0.0f;
			final float rawY = haveRaw ? raw.fx[vi + 1] : 0.0f;
			float residual0 = 0.0f;
			float residual1 = 0.0f;
			if (useTile) {
				final float dx = y[vi] - rawX;
				final float dy = y[vi + 1] - rawY;
				residual0 = eigen[ei] * dx + eigen[ei + 1] * dy;
				residual1 = eigen[ei + 2] * dx + eigen[ei + 3] * dy;
			}
			ymfx[vi] = residual0 * weights[vi];
			ymfx[vi + 1] = residual1 * weights[vi + 1];
			pureL2 += residual0 * ymfx[vi];
			pureL2 += residual1 * ymfx[vi + 1];
			for (int par = 0; par < NUM_PARAMS; par++) {
				final int rawIndex = par * NUM_COMPONENTS * raw.numTiles + vi;
				final float jx = haveRaw ? raw.jt[rawIndex] : 0.0f;
				final float jy = haveRaw ? raw.jt[rawIndex + 1] : 0.0f;
				final int outIndex = par * numValues + vi;
				transformedJt[outIndex] = eigen[ei] * jx + eigen[ei + 1] * jy;
				transformedJt[outIndex + 1] = eigen[ei + 2] * jx + eigen[ei + 3] * jy;
			}
		}
		float fullL2 = pureL2;
		for (int par = 0; par < NUM_PARAMS; par++) {
			final int value = NUM_COMPONENTS * raw.numTiles + par;
			transformedJt[par * numValues + value] = 1.0f;
			final float residual = y[value] - vector[par];
			ymfx[value] = residual * weights[value];
			fullL2 += residual * ymfx[value];
		}
		return new PreparedLma(
				transformedJt, ymfx,
				(float) Math.sqrt(fullL2),
				(float) Math.sqrt(pureL2 / pureWeight));
	}

	public static LmaStepComparison compareLmaSteps(
			final LmaStepResult actual,
			final LmaStepResult expected) {
		if ((actual == null) || (expected == null) ||
				(actual.products == null) || (expected.products == null) ||
				(actual.products.length != expected.products.length) ||
				(actual.delta == null) || (expected.delta == null) ||
				(actual.delta.length != expected.delta.length) ||
				(actual.candidate == null) || (expected.candidate == null) ||
				(actual.candidate.length != expected.candidate.length)) {
			throw new IllegalArgumentException("incompatible float LMA step results");
		}
		final LmaStepComparison comparison = new LmaStepComparison();
		comparison.validityMatch = actual.valid == expected.valid;
		comparison.bitExact = comparison.validityMatch;
		comparison.maxAbsProducts = compareFloatArrays(actual.products, expected.products, comparison);
		comparison.maxAbsDelta = compareFloatArrays(actual.delta, expected.delta, comparison);
		comparison.maxAbsCandidate = compareFloatArrays(actual.candidate, expected.candidate, comparison);
		return comparison;
	}

	public static PreparationComparison comparePreparation(
			final PreparedLma actual,
			final PreparedLma expected) {
		if ((actual == null) || (expected == null) || (actual.jt == null) ||
				(expected.jt == null) || (actual.jt.length != expected.jt.length) ||
				(actual.ymfxWeighted == null) || (expected.ymfxWeighted == null) ||
				(actual.ymfxWeighted.length != expected.ymfxWeighted.length)) {
			throw new IllegalArgumentException("incompatible resident pose LMA preparation results");
		}
		final PreparationComparison comparison = new PreparationComparison();
		comparison.bitExact = true;
		comparison.maxAbsJt = comparePreparedArrays(actual.jt, expected.jt, comparison);
		comparison.maxAbsYmfx = comparePreparedArrays(
				actual.ymfxWeighted, expected.ymfxWeighted, comparison);
		comparison.absRms = Math.abs(actual.rms - expected.rms);
		comparison.absPureRms = Math.abs(actual.pureRms - expected.pureRms);
		return comparison;
	}

	private static float comparePreparedArrays(
			final float [] actual,
			final float [] expected,
			final PreparationComparison comparison) {
		float maximum = 0.0f;
		for (int i = 0; i < actual.length; i++) {
			final boolean sameBits = Float.floatToRawIntBits(actual[i]) ==
					Float.floatToRawIntBits(expected[i]);
			if (!sameBits) comparison.bitExact = false;
			if (finite(actual[i]) && finite(expected[i])) {
				maximum = Math.max(maximum, Math.abs(actual[i] - expected[i]));
			} else if (!sameBits) {
				maximum = Float.POSITIVE_INFINITY;
			}
		}
		return maximum;
	}

	private static float compareFloatArrays(
			final float [] actual,
			final float [] expected,
			final LmaStepComparison comparison) {
		float maximum = 0.0f;
		for (int i = 0; i < actual.length; i++) {
			final boolean sameBits = Float.floatToRawIntBits(actual[i]) ==
					Float.floatToRawIntBits(expected[i]);
			if (!sameBits) comparison.bitExact = false;
			if (finite(actual[i]) && finite(expected[i])) {
				maximum = Math.max(maximum, Math.abs(actual[i] - expected[i]));
			} else if (!sameBits) {
				maximum = Float.POSITIVE_INFINITY;
			}
		}
		return maximum;
	}

	/** Convert the existing sparse center representation to CUDA-style xyz triples. */
	public static float [] flattenCenters(final double [][] centers) {
		final float [] flat = new float [3 * centers.length];
		Arrays.fill(flat, Float.NaN);
		for (int tile = 0; tile < centers.length; tile++) {
			if ((centers[tile] == null) || (centers[tile].length < 3)) continue;
			for (int i = 0; i < 3; i++) flat[3 * tile + i] = (float) centers[tile][i];
		}
		return flat;
	}

	public static float [] toFloat3(final double [] vector) {
		if ((vector == null) || (vector.length < 3)) throw new IllegalArgumentException("expected 3-vector");
		return new float [] {(float) vector[0], (float) vector[1], (float) vector[2]};
	}

	/**
	 * Calculate scene pixel coordinates and derivatives by scene azimuth, tilt
	 * and roll. Centers are packed xyz triples; a null selection means all finite
	 * centers. Optional eigen transforms are packed per tile as 2x2 row-major.
	 */
	public static Result getFxDerivs(
			final Camera reference,
			final Camera scene,
			final float [] referenceXyz,
			final float [] referenceAtr,
			final float [] sceneXyz,
			final float [] sceneAtr,
			final float [] centers,
			final boolean [] selection,
			final float [] eigenTransforms) {
		return getFxDerivs(reference, scene, referenceXyz, referenceAtr, sceneXyz, sceneAtr,
				centers, selection, eigenTransforms, true);
	}

	/**
	 * Shared projection path with optional Jacobian output, matching the nullable
	 * {@code jt} contract of the legacy method. The pose-global matrices and all
	 * per-tile projection work are common; {@code calculateJacobian=false} stops
	 * after fx and validity have been produced.
	 */
	public static Result getFxDerivs(
			final Camera reference,
			final Camera scene,
			final float [] referenceXyz,
			final float [] referenceAtr,
			final float [] sceneXyz,
			final float [] sceneAtr,
			final float [] centers,
			final boolean [] selection,
			final float [] eigenTransforms,
			final boolean calculateJacobian) {
		if ((centers == null) || ((centers.length % 3) != 0)) {
			throw new IllegalArgumentException("centers should contain packed xyz triples");
		}
		final int numTiles = centers.length / 3;
		if ((selection != null) && (selection.length != numTiles)) {
			throw new IllegalArgumentException("selection length");
		}
		if ((eigenTransforms != null) && (eigenTransforms.length != 4 * numTiles)) {
			throw new IllegalArgumentException("eigenTransforms length");
		}
		checkVector3(referenceXyz, "referenceXyz");
		checkVector3(referenceAtr, "referenceAtr");
		checkVector3(sceneXyz, "sceneXyz");
		checkVector3(sceneAtr, "sceneAtr");

		final float [] refInverse = rotationMatrices(referenceAtr, true);
		final float [] sceneInverse = rotationMatrices(sceneAtr, true);
		final float [] sceneDirect = rotationMatrices(sceneAtr, false);
		final float [] fx = new float [NUM_COMPONENTS * numTiles];
		final float [] jt = calculateJacobian ?
				new float [NUM_PARAMS * NUM_COMPONENTS * numTiles] : null;
		final byte [] valid = new byte [numTiles];
		Arrays.fill(fx, Float.NaN);
		if (jt != null) Arrays.fill(jt, Float.NaN);
		int numValid = 0;
		for (int tile = 0; tile < numTiles; tile++) {
			if ((selection != null) && !selection[tile]) continue;
			final int ci = 3 * tile;
			final float px = centers[ci];
			final float py = centers[ci + 1];
			final float disparity = centers[ci + 2];
			if (!finite(px) || !finite(py) || !finite(disparity)) continue;
			final boolean infinity = disparity < INFINITY_DISPARITY;
			final WorldDerivs referenceWorld = worldFromPixel(
					reference, px, py, disparity, infinity, referenceXyz, refInverse);
			if (referenceWorld == null) continue;
			final float [] scenePixel = pixelFromWorld(
					scene, referenceWorld.world, infinity, sceneXyz, sceneDirect);
			if (scenePixel == null) continue;
			final int vi = NUM_COMPONENTS * tile;
			fx[vi] = scenePixel[0];
			fx[vi + 1] = scenePixel[1];
			if (!calculateJacobian) {
				valid[tile] = 1;
				numValid++;
				continue;
			}
			final WorldDerivs sceneWorld = worldFromPixel(
					scene, scenePixel[0], scenePixel[1], scenePixel[2], infinity, sceneXyz, sceneInverse);
			if (sceneWorld == null) continue;

			final float [] dWorldDPixel = sceneWorld.dPixel.clone();
			final float [] dWorldDAtr = sceneWorld.dAtr.clone();
			if (infinity && !normalizeInfinityDerivatives(sceneWorld.world, dWorldDPixel, dWorldDAtr)) continue;
			final float [] dPixelDWorld = inverse3x3(columnsToRowMajor(dWorldDPixel));
			if (dPixelDWorld == null) continue;

			for (int par = 0; par < NUM_PARAMS; par++) {
				final int di = 3 * par;
				final float rawX = -(dPixelDWorld[0] * dWorldDAtr[di] +
						dPixelDWorld[1] * dWorldDAtr[di + 1] + dPixelDWorld[2] * dWorldDAtr[di + 2]);
				final float rawY = -(dPixelDWorld[3] * dWorldDAtr[di] +
						dPixelDWorld[4] * dWorldDAtr[di + 1] + dPixelDWorld[5] * dWorldDAtr[di + 2]);
				final int ji = par * NUM_COMPONENTS * numTiles + vi;
				if (eigenTransforms == null) {
					jt[ji] = rawX;
					jt[ji + 1] = rawY;
				} else {
					final int ei = 4 * tile;
					jt[ji] = eigenTransforms[ei] * rawX + eigenTransforms[ei + 1] * rawY;
					jt[ji + 1] = eigenTransforms[ei + 2] * rawX + eigenTransforms[ei + 3] * rawY;
				}
			}
			valid[tile] = 1;
			numValid++;
		}
		return new Result(numTiles, fx, jt, valid, numValid);
	}

	/** Compare a float result with the exact legacy double geometry call. */
	public static Comparison compareToDouble(
			final Result result,
			final ErsCorrection reference,
			final ErsCorrection scene,
			final double [] referenceXyz,
			final double [] referenceAtr,
			final double [] sceneXyz,
			final double [] sceneAtr,
			final double [][] centers,
			final boolean [] selection,
			final float [] eigenTransforms,
			final int debugLevel) {
		if (result.numTiles != centers.length) throw new IllegalArgumentException("result/centers length");
		reference.setupERS();
		scene.setupERS();
		final Matrix [] refInverse = ErsCorrection.getInterRotDeriveMatrices(referenceAtr, true);
		final Matrix [] sceneInverse = ErsCorrection.getInterRotDeriveMatrices(sceneAtr, true);
		final Matrix sceneDirect = ErsCorrection.getInterRotDeriveMatrices(sceneAtr, false)[0];
		final Comparison comparison = new Comparison();
		for (int tile = 0; tile < centers.length; tile++) {
			if ((selection != null) && !selection[tile]) continue;
			double [][] expected = null;
			if ((centers[tile] != null) && (centers[tile].length >= 3)) {
				expected = reference.getDPxSceneDParameters(
						scene, true, centers[tile][2] < INFINITY_DISPARITY, centers[tile],
						referenceXyz, sceneXyz, refInverse, sceneInverse, sceneDirect, debugLevel);
			}
			final boolean doubleValid = expected != null;
			final boolean floatValid = result.valid[tile] != 0;
			if (floatValid && !doubleValid) {
				comparison.floatOnlyValid++;
				continue;
			}
			if (!floatValid && doubleValid) {
				comparison.doubleOnlyValid++;
				continue;
			}
			if (!floatValid) continue;
			comparison.compared++;
			final int vi = NUM_COMPONENTS * tile;
			comparison.maxAbsFx = Math.max(comparison.maxAbsFx,
					Math.max(Math.abs(result.fx[vi] - (float) expected[0][0]),
							Math.abs(result.fx[vi + 1] - (float) expected[0][1])));
			if (result.jt == null) continue;
			for (int par = 0; par < NUM_PARAMS; par++) {
				float ex = (float) expected[ErsCorrection.DP_DSAZ + par + 1][0];
				float ey = (float) expected[ErsCorrection.DP_DSAZ + par + 1][1];
				if (eigenTransforms != null) {
					final int ei = 4 * tile;
					final float tx = eigenTransforms[ei] * ex + eigenTransforms[ei + 1] * ey;
					final float ty = eigenTransforms[ei + 2] * ex + eigenTransforms[ei + 3] * ey;
					ex = tx;
					ey = ty;
				}
				final int ji = par * NUM_COMPONENTS * result.numTiles + vi;
				comparison.maxAbsJt = Math.max(comparison.maxAbsJt,
						Math.max(Math.abs(result.jt[ji] - ex), Math.abs(result.jt[ji + 1] - ey)));
			}
		}
		return comparison;
	}

	/** Compare raw fx/J and validity from two float implementations. */
	public static ResultComparison compareResults(final Result actual, final Result expected) {
		if ((actual == null) || (expected == null) || (actual.numTiles != expected.numTiles) ||
				(actual.fx.length != expected.fx.length) || (actual.valid.length != expected.valid.length) ||
				((actual.jt == null) != (expected.jt == null)) ||
				((actual.jt != null) && (actual.jt.length != expected.jt.length))) {
			throw new IllegalArgumentException("incompatible float getFxDerivs results");
		}
		final ResultComparison comparison = new ResultComparison();
		for (int tile = 0; tile < actual.numTiles; tile++) {
			final boolean actualValid = actual.valid[tile] != 0;
			final boolean expectedValid = expected.valid[tile] != 0;
			if (actualValid && !expectedValid) {
				comparison.actualOnlyValid++;
				continue;
			}
			if (!actualValid && expectedValid) {
				comparison.expectedOnlyValid++;
				continue;
			}
			if (!actualValid) continue;
			comparison.compared++;
			final int vi = NUM_COMPONENTS * tile;
			for (int component = 0; component < NUM_COMPONENTS; component++) {
				comparison.maxAbsFx = Math.max(comparison.maxAbsFx,
						Math.abs(actual.fx[vi + component] - expected.fx[vi + component]));
			}
			if (actual.jt == null) continue;
			for (int par = 0; par < NUM_PARAMS; par++) {
				final int ji = par * NUM_COMPONENTS * actual.numTiles + vi;
				for (int component = 0; component < NUM_COMPONENTS; component++) {
					comparison.maxAbsJt = Math.max(comparison.maxAbsJt,
							Math.abs(actual.jt[ji + component] - expected.jt[ji + component]));
				}
			}
		}
		return comparison;
	}

	private static final class WorldDerivs {
		final float [] world = new float [3];
		final float [] dPixel = new float [9]; // columns: d/d px, py, disparity
		final float [] dAtr = new float [9];   // columns: d/d azimuth, tilt, roll
	}

	private static WorldDerivs worldFromPixel(
			final Camera camera,
			final float px,
			final float py,
			final float disparity,
			final boolean infinity,
			final float [] cameraXyz,
			final float [] inverseMatrices) {
		if (!finite(disparity)) return null;
		int line = Math.round(py);
		if (line < 0) line = 0;
		else if (line >= camera.height) line = camera.height - 1;
		final float pXcd = px - 0.5f * camera.width;
		final float pYcd = py - 0.5f * camera.height;
		final float rScale = 0.001f * camera.pixelSize / camera.distortionRadius;
		final float rDn = (float) Math.sqrt(pXcd * pXcd + pYcd * pYcd) * rScale;
		final float rND2RD = inverseDistortion(camera, rDn);
		if (!finite(rND2RD)) return null;
		final float pXc = pXcd * rND2RD;
		final float pYc = pYcd * rND2RD;
		final float rn = rDn * rND2RD;

		float derivative = 0.0f;
		float rr = 1.0f;
		for (int j = 0; j < camera.radial.length; j++) {
			derivative += camera.radial[j] * (j + 1) * rr;
			rr *= rn;
		}
		final float rD2RND = 1.0f / rND2RD;
		final float denominator = (rD2RND * rD2RND) * (derivative * rn + rD2RND);
		final float dInvDr = -derivative / denominator;
		final float dRdx;
		final float dRdy;
		if (rDn == 0.0f) {
			dRdx = 0.0f;
			dRdy = 0.0f;
		} else {
			dRdx = pXcd * rScale * rScale / rDn;
			dRdy = pYcd * rScale * rScale / rDn;
		}
		final float dPxcDx = rND2RD + pXcd * dInvDr * dRdx;
		final float dPxcDy = pXcd * dInvDr * dRdy;
		final float dPycDx = pYcd * dInvDr * dRdx;
		final float dPycDy = rND2RD + pYcd * dInvDr * dRdy;

		final float zh = -SCENE_UNITS_SCALE * camera.focalLength * camera.disparityRadius /
				(0.001f * camera.pixelSize);
		final float xh = SCENE_UNITS_SCALE * pXc * camera.disparityRadius;
		final float yh = -SCENE_UNITS_SCALE * pYc * camera.disparityRadius;
		final float dXhDx = SCENE_UNITS_SCALE * dPxcDx * camera.disparityRadius;
		final float dXhDy = SCENE_UNITS_SCALE * dPxcDy * camera.disparityRadius;
		final float dYhDx = -SCENE_UNITS_SCALE * dPycDx * camera.disparityRadius;
		final float dYhDy = -SCENE_UNITS_SCALE * dPycDy * camera.disparityRadius;
		final float [] xyz = new float [] {xh, yh, zh};
		final float [] dpx = new float [] {dXhDx, dYhDx, 0.0f};
		final float [] dpy = new float [] {dXhDy, dYhDy, 0.0f};
		final float [] dd = new float [3];
		if (!infinity) {
			if (disparity == 0.0f) return null;
			for (int i = 0; i < 3; i++) xyz[i] /= disparity;
			for (int i = 0; i < 2; i++) {
				dpx[i] /= disparity;
				dpy[i] /= disparity;
			}
			for (int i = 0; i < 3; i++) dd[i] = -xyz[i] / disparity;
		}

		final int eb = line * ERS_LINE_STRIDE;
		rotate(camera.ers, eb + ERS_QUAT, true, dpx, dpx);
		rotate(camera.ers, eb + ERS_QUAT, true, dpy, dpy);
		rotate(camera.ers, eb + ERS_QUAT, true, dd, dd);
		final float [] cameraNow = new float [3];
		rotate(camera.ers, eb + ERS_QUAT, true, xyz, cameraNow);
		final float ox = camera.ers[eb + ERS_QUAT_DT + 1];
		final float oy = camera.ers[eb + ERS_QUAT_DT + 2];
		final float oz = camera.ers[eb + ERS_QUAT_DT + 3];
		final float [] dwDt = new float [] {
				oy * cameraNow[2] - oz * cameraNow[1],
				oz * cameraNow[0] - ox * cameraNow[2],
				ox * cameraNow[1] - oy * cameraNow[0]};
		for (int i = 0; i < 3; i++) {
			dpy[i] += camera.lineTime * dwDt[i];
			if (!infinity) dpy[i] += camera.lineTime * camera.ers[eb + ERS_XYZ_DT + i];
			if (!infinity) cameraNow[i] += camera.ers[eb + ERS_XYZ + i];
		}

		final WorldDerivs result = new WorldDerivs();
		matVec(inverseMatrices, 0, cameraNow, result.world);
		matVec(inverseMatrices, 0, dpx, result.dPixel, 0);
		matVec(inverseMatrices, 0, dpy, result.dPixel, 3);
		matVec(inverseMatrices, 0, dd, result.dPixel, 6);
		if (!infinity) {
			for (int i = 0; i < 3; i++) result.world[i] += cameraXyz[i];
		}
		for (int par = 0; par < NUM_PARAMS; par++) {
			matVec(inverseMatrices, 9 * (par + 1), cameraNow, result.dAtr, 3 * par);
		}
		return allFinite(result.world) && allFinite(result.dPixel) && allFinite(result.dAtr) ? result : null;
	}

	private static float [] pixelFromWorld(
			final Camera camera,
			final float [] world,
			final boolean infinity,
			final float [] cameraXyz,
			final float [] directMatrices) {
		final float [] centered = world.clone();
		if (!infinity) {
			for (int i = 0; i < 3; i++) centered[i] -= cameraXyz[i];
		}
		final float [] centerLocal = new float [3];
		matVec(directMatrices, 0, centered, centerLocal);
		float line = 0.5f * camera.height;
		float error = line;
		float [] result = null;
		for (int step = 0; (step < PROJECTION_MAX_STEPS) && (error > LINE_ERR); step++) {
			int line0 = (int) Math.floor(line);
			if (line0 < 0) line0 = 0;
			else if (line0 >= camera.height) line0 = camera.height - 1;
			final int line1 = Math.min(line0 + 1, camera.height - 1);
			final float fraction = line - line0;
			final int e0 = line0 * ERS_LINE_STRIDE;
			final int e1 = line1 * ERS_LINE_STRIDE;
			final float [] now = centerLocal.clone();
			if (!infinity) {
				for (int i = 0; i < 3; i++) {
					final float motion = (1.0f - fraction) * camera.ers[e0 + ERS_XYZ + i] +
							fraction * camera.ers[e1 + ERS_XYZ + i];
					now[i] -= motion;
				}
			}
			final float [] quaternion = new float [4];
			for (int i = 0; i < 4; i++) {
				quaternion[i] = (1.0f - fraction) * camera.ers[e0 + ERS_QUAT + i] +
						fraction * camera.ers[e1 + ERS_QUAT + i];
			}
			final float [] local = new float [3];
			rotate(quaternion, 0, false, now, local);
			if (Math.abs(local[2]) < PROJECTION_EPS) return null;
			final float focalPixels = 1000.0f * camera.focalLength / camera.pixelSize;
			final float pXc = -focalPixels * local[0] / local[2];
			final float pYc = focalPixels * local[1] / local[2];
			final float disparity = infinity ? 0.0f :
					-focalPixels / local[2] * SCENE_UNITS_SCALE * camera.disparityRadius;
			final float rND = (float) Math.sqrt(pXc * pXc + pYc * pYc) * 0.001f * camera.pixelSize;
			final float distortion = forwardDistortion(camera, rND / camera.distortionRadius);
			final float px = pXc * distortion + 0.5f * camera.width;
			final float py = pYc * distortion + 0.5f * camera.height;
			if (!finite(px) || !finite(py) || !finite(disparity)) return null;
			result = new float [] {px, py, disparity};
			final float nextLine = Math.max(0.0f, Math.min(camera.height - 1.0f, py));
			error = Math.abs(nextLine - line);
			line = nextLine;
		}
		return result;
	}

	private static boolean normalizeInfinityDerivatives(
			final float [] world,
			final float [] dPixel,
			final float [] dAtr) {
		final float x = world[0];
		final float y = world[1];
		final float z = world[2];
		final float z2 = z * z;
		if (!finite(z2) || (z2 == 0.0f)) return false;
		for (int column = 0; column < 2; column++) {
			final int i = 3 * column;
			final float dx = dPixel[i];
			final float dy = dPixel[i + 1];
			final float dz = dPixel[i + 2];
			dPixel[i] = dx / z - x * dz / z2;
			dPixel[i + 1] = dy / z - y * dz / z2;
			dPixel[i + 2] = 0.0f;
		}
		dPixel[6] = 0.0f;
		dPixel[7] = 0.0f;
		dPixel[8] = 1.0f;
		for (int column = 0; column < NUM_PARAMS; column++) {
			final int i = 3 * column;
			final float dx = dAtr[i];
			final float dy = dAtr[i + 1];
			final float dz = dAtr[i + 2];
			dAtr[i] = dx / z - x * dz / z2;
			dAtr[i + 1] = dy / z - y * dz / z2;
			dAtr[i + 2] = 0.0f;
		}
		return allFinite(dPixel) && allFinite(dAtr);
	}

	private static float [] rotationMatrices(final float [] atr, final boolean invert) {
		final float ca = (float) Math.cos(atr[0]);
		final float sa = (float) Math.sin(atr[0]);
		final float ct = (float) Math.cos(atr[1]);
		final float st = (float) Math.sin(atr[1]);
		final float cr = (float) Math.cos(atr[2]);
		final float sr = (float) Math.sin(atr[2]);
		final float azSign = invert ? 1.0f : -1.0f;
		final float tiltSign = invert ? -1.0f : 1.0f;
		final float rollSign = invert ? -1.0f : 1.0f;
		final float [] az = new float [] {ca,0,sa*azSign, 0,1,0, -sa*azSign,0,ca};
		final float [] tilt = new float [] {1,0,0, 0,ct,st*tiltSign, 0,-st*tiltSign,ct};
		final float [] roll = new float [] {cr,sr*rollSign,0, -sr*rollSign,cr,0, 0,0,1};
		final float [] daz = new float [] {-sa,0,ca*azSign, 0,0,0, -ca*azSign,0,-sa};
		final float [] dtilt = new float [] {0,0,0, 0,-st,ct*tiltSign, 0,-ct*tiltSign,-st};
		final float [] droll = new float [] {-sr,cr*rollSign,0, -cr*rollSign,-sr,0, 0,0,0};
		final float [] result = new float [36];
		if (invert) {
			putMatrix(result, 0, multiply(az, multiply(tilt, roll)));
			putMatrix(result, 9, multiply(daz, multiply(tilt, roll)));
			putMatrix(result, 18, multiply(az, multiply(dtilt, roll)));
			putMatrix(result, 27, multiply(az, multiply(tilt, droll)));
		} else {
			putMatrix(result, 0, multiply(roll, multiply(tilt, az)));
			putMatrix(result, 9, multiply(roll, multiply(tilt, daz)));
			putMatrix(result, 18, multiply(roll, multiply(dtilt, az)));
			putMatrix(result, 27, multiply(droll, multiply(tilt, az)));
		}
		return result;
	}

	private static float [] inverse3x3(final float [] a) {
		final float c00 = a[4] * a[8] - a[5] * a[7];
		final float c01 = a[2] * a[7] - a[1] * a[8];
		final float c02 = a[1] * a[5] - a[2] * a[4];
		final float c10 = a[5] * a[6] - a[3] * a[8];
		final float c11 = a[0] * a[8] - a[2] * a[6];
		final float c12 = a[2] * a[3] - a[0] * a[5];
		final float c20 = a[3] * a[7] - a[4] * a[6];
		final float c21 = a[1] * a[6] - a[0] * a[7];
		final float c22 = a[0] * a[4] - a[1] * a[3];
		final float determinant = a[0] * c00 + a[1] * c10 + a[2] * c20;
		if (!finite(determinant) || (determinant == 0.0f)) return null;
		final float scale = 1.0f / determinant;
		return new float [] {c00*scale,c01*scale,c02*scale, c10*scale,c11*scale,c12*scale,
				c20*scale,c21*scale,c22*scale};
	}

	/** Convert three packed column vectors to the row-major matrix they form. */
	private static float [] columnsToRowMajor(final float [] columns) {
		return new float [] {
				columns[0], columns[3], columns[6],
				columns[1], columns[4], columns[7],
				columns[2], columns[5], columns[8]};
	}

	private static float inverseDistortion(final Camera camera, final float radius) {
		if (!finite(radius)) return Float.NaN;
		final float indexFloat = Math.max(0.0f, radius) / camera.stepR;
		final int index = (int) Math.floor(indexFloat);
		if (index >= camera.rByRDist.length - 2) return camera.rByRDist.length - 1;
		final float mu = indexFloat - index;
		final float mu2 = mu * mu;
		final float y0 = (index > 0) ? camera.rByRDist[index - 1] :
				2.0f * camera.rByRDist[index] - camera.rByRDist[index + 1];
		final float a0 = -0.5f*y0 + 1.5f*camera.rByRDist[index] -
				1.5f*camera.rByRDist[index + 1] + 0.5f*camera.rByRDist[index + 2];
		final float a1 = y0 - 2.5f*camera.rByRDist[index] +
				2.0f*camera.rByRDist[index + 1] - 0.5f*camera.rByRDist[index + 2];
		final float a2 = -0.5f*y0 + 0.5f*camera.rByRDist[index + 1];
		return a0*mu*mu2 + a1*mu2 + a2*mu + camera.rByRDist[index];
	}

	private static float forwardDistortion(final Camera camera, final float radius) {
		float constant = 1.0f;
		for (float coefficient : camera.radial) constant -= coefficient;
		float value = camera.radial[6];
		for (int i = 5; i >= 0; i--) value = value * radius + camera.radial[i];
		return value * radius + constant;
	}

	private static void rotate(
			final float [] quaternion,
			final int offset,
			final boolean inverse,
			final float [] vector,
			final float [] out) {
		float w = quaternion[offset];
		float x = quaternion[offset + 1];
		float y = quaternion[offset + 2];
		float z = quaternion[offset + 3];
		final float norm = (float) Math.sqrt(w*w + x*x + y*y + z*z);
		if (norm == 0.0f) {
			Arrays.fill(out, Float.NaN);
			return;
		}
		w /= norm; x /= norm; y /= norm; z /= norm;
		// Commons Math Rotation stores/applies the vector part with the opposite
		// convention from the textbook q*v*q^-1 formula used below.
		if (!inverse) { x = -x; y = -y; z = -z; }
		final float vx = vector[0];
		final float vy = vector[1];
		final float vz = vector[2];
		final float tx = 2.0f * (y * vz - z * vy);
		final float ty = 2.0f * (z * vx - x * vz);
		final float tz = 2.0f * (x * vy - y * vx);
		out[0] = vx + w * tx + (y * tz - z * ty);
		out[1] = vy + w * ty + (z * tx - x * tz);
		out[2] = vz + w * tz + (x * ty - y * tx);
	}

	private static float [] multiply(final float [] left, final float [] right) {
		final float [] result = new float [9];
		for (int row = 0; row < 3; row++) {
			for (int column = 0; column < 3; column++) {
				result[3 * row + column] = left[3 * row] * right[column] +
						left[3 * row + 1] * right[3 + column] + left[3 * row + 2] * right[6 + column];
			}
		}
		return result;
	}

	private static void putMatrix(final float [] destination, final int offset, final float [] matrix) {
		System.arraycopy(matrix, 0, destination, offset, 9);
	}

	private static void matVec(final float [] matrix, final int offset, final float [] vector, final float [] out) {
		matVec(matrix, offset, vector, out, 0);
	}

	private static void matVec(
			final float [] matrix,
			final int offset,
			final float [] vector,
			final float [] out,
			final int outOffset) {
		final float x = vector[0];
		final float y = vector[1];
		final float z = vector[2];
		out[outOffset] = matrix[offset] * x + matrix[offset + 1] * y + matrix[offset + 2] * z;
		out[outOffset + 1] = matrix[offset + 3] * x + matrix[offset + 4] * y + matrix[offset + 5] * z;
		out[outOffset + 2] = matrix[offset + 6] * x + matrix[offset + 7] * y + matrix[offset + 8] * z;
	}

	private static void checkVector3(final float [] vector, final String name) {
		if ((vector == null) || (vector.length < 3)) throw new IllegalArgumentException(name);
	}

	private static boolean allFinite(final float [] values) {
		for (float value : values) if (!finite(value)) return false;
		return true;
	}

	private static boolean finite(final float value) {
		return !Float.isNaN(value) && !Float.isInfinite(value);
	}
}
