georegression.geometry.UtilVector3D_F32 Maven / Gradle / Ivy

Show more of this group Show more artifacts with this name
Show all versions of georegression Show documentation
GeoRegression is a free Java based geometry library for scientific computing in fields such as robotics and computer vision with a focus on 2D/3D space.
There is a newer version: 0.27.1
Show newest version
/*
 * Copyright (C) 2020, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.geometry;

import javax.annotation.Generated;
import georegression.struct.GeoTuple3D_F32;
import georegression.struct.point.Vector3D_F32;
import org.ejml.data.FMatrixRMaj;
import org.jetbrains.annotations.Nullable;

import java.util.Random;


/**
 * @author Peter Abeles
 */

// TODO create a UtilGeoTuple3D class for this and point 3d?
@Generated("georegression.geometry.UtilVector3D_F64")
public class UtilVector3D_F32 {

	/**
	 * Creates a random vector where each axis is selected from a uniform distribution.
	 *
	 * @param min  minimum value
	 * @param max  maximum value
	 * @param rand random number generator
	 * @return the new random vector
	 */
	public static Vector3D_F32 createRandom( float min, float max, Random rand ) {
		float range = max - min;

		Vector3D_F32 a = new Vector3D_F32();

		a.x = range * rand.nextFloat() + min;
		a.y = range * rand.nextFloat() + min;
		a.z = range * rand.nextFloat() + min;

		return a;
	}

	/**
	 * Selects a vector which will be perpendicular.
	 *
	 * @param A (Input) A vector
	 * @param output (Optional) storage for output
	 * @return Perpendicular vector
	 */
	public static Vector3D_F32 perpendicularCanonical( Vector3D_F32 A , @Nullable Vector3D_F32 output ) {
		if( output == null )
			output = new Vector3D_F32();

		// normalize for scaling
		float scale = (float)Math.abs(A.x)+Math.abs(A.y)+Math.abs(A.z);

		if( scale == 0 ) {
			output.setTo(0,0,0);
		} else {
			float x = A.x / scale;
			float y = A.y / scale;
			float z = A.z / scale;

			// For numerical stability ensure that the largest variable is swapped
			if (Math.abs(x) > (float)Math.abs(y)) {
				output.setTo(z, 0, -x);
			} else {
				output.setTo(0, z,-y);
			}
		}

		return output;
	}

	/**
	 * Checks to see if the two vectors are identical to within tolerance. Each axis is checked
	 * individually.
	 *
	 * @param a   First vector.
	 * @param b   Second vector.
	 * @param tol Tolerance for equality.
	 * @return true if identical and false if not.
	 */
	public static boolean isIdentical( Vector3D_F32 a, Vector3D_F32 b, float tol ) {
		if( (float)Math.abs( a.x - b.x ) > tol )
			return false;

		if( (float)Math.abs( a.y - b.y ) > tol )
			return false;

		return (float)Math.abs( a.z - b.z ) <= tol;

	}

	/**
	 * Rescales the vector such that its normal is equal to one.
	 *
	 * @param v Vector being normalized.
	 */
	public static void normalize( Vector3D_F32 v ) {
		float a = v.norm();

		v.x /= a;
		v.y /= a;
		v.z /= a;
	}

	/**
	 * Creates a matrix from the set of column vectors.  Each vector is a column in the new matrix.
	 *
	 * @param v Set of vectors. Not modified.
	 * @param R If not null the vectors are stored here.
	 * @return Matrix.
	 */
	public static FMatrixRMaj createMatrix( @Nullable FMatrixRMaj R, Vector3D_F32... v ) {
		if( R == null ) {
			R = new FMatrixRMaj( 3, v.length );
		}

		for( int i = 0; i < v.length; i++ ) {
			R.set( 0, i, v[i].x );
			R.set( 1, i, v[i].y );
			R.set( 2, i, v[i].z );
		}

		return R;
	}

	/**
	 * Converts matrices into vectors.  All matrices must be vectors with 3 elements.
	 *
	 * @param m A 3x1 or 1x3 matrix
	 * @return Equivalent 3D vector
	 */
	public static Vector3D_F32 convert( FMatrixRMaj m ) {

		Vector3D_F32 v = new Vector3D_F32();
		v.x = (float) m.data[0];
		v.y = (float) m.data[1];
		v.z = (float) m.data[2];

		return v;
	}

	/**
	 * Returns the acute angle between the two vectors.  Computed using the dot product.
	 * @param a vector
	 * @param b vector
	 * @return acute angle
	 */
	public static float acute(GeoTuple3D_F32 a , GeoTuple3D_F32 b ) {
		float dot = a.x*b.x + a.y*b.y + a.z*b.z;

		float value = dot/(a.norm()*b.norm());
		if( value > 1.0f )
			value = 1.0f;
		else if( value < -1.0f )
			value = -1.0f;

		return (float)Math.acos( value );
	}

	/**
	 * Finds which axis in `v` has the largest magnitude
	 */
	public static int axisMaxMag( GeoTuple3D_F32 v ) {
		int bestAxis = -1;
		float best = -1;
		for (int i = 0; i < 3; i++) {
			float m = (float)Math.abs(v.getIdx(i));
			if( m > best ) {
				best = m;
				bestAxis = i;
			}
		}
		return bestAxis;
	}
}