georegression.geometry.GeometryMath_F32 Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of georegression Show documentation
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.
/*
* Copyright (C) 2011-2017, 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 georegression.struct.GeoTuple2D_F32;
import georegression.struct.GeoTuple3D_F32;
import org.ejml.data.FMatrixRMaj;
import org.ejml.dense.row.mult.VectorVectorMult_FDRM;
/**
* Math operations that can be applied to geometric primitives.
*
* @author Peter Abeles
*/
// TODO rename to PerspectiveMath?
// todo separate off fucntions that are in homogeneous coordinates into their own class?
// alternatively indicate by the function name?
// todo make sure all functions have unit tests
@SuppressWarnings({"unchecked", "RedundantCast"})
public class GeometryMath_F32 {
/**
* Creates a skew symmetric cross product matrix from the provided tuple.
*
* @param x0 Element 0.
* @param x1 Element 1.
* @param x2 Element 2.
* @param ret If not null the results are stored here, otherwise a new matrix is created.
* @return Skew symmetric cross product matrix.
*/
public static FMatrixRMaj crossMatrix( float x0, float x1, float x2, FMatrixRMaj ret ) {
if( ret == null ) {
ret = new FMatrixRMaj( 3, 3 );
} else {
ret.zero();
}
ret.set( 0, 1, -x2 );
ret.set( 0, 2, x1 );
ret.set( 1, 0, x2 );
ret.set( 1, 2, -x0 );
ret.set( 2, 0, -x1 );
ret.set( 2, 1, x0 );
return ret;
}
/**
* Creates a skew symmetric cross product matrix from the provided tuple.
*
* @param v Tuple. Not modified.
* @param ret If not null the results are stored here, otherwise a new matrix is created.
* @return Skew symmetric cross product matrix.
*/
public static FMatrixRMaj crossMatrix( GeoTuple3D_F32 v, FMatrixRMaj ret ) {
if( ret == null ) {
ret = new FMatrixRMaj( 3, 3 );
} else {
ret.zero();
}
float x = v.getX();
float y = v.getY();
float z = v.getZ();
ret.set( 0, 1, -z );
ret.set( 0, 2, y );
ret.set( 1, 0, z );
ret.set( 1, 2, -x );
ret.set( 2, 0, -y );
ret.set( 2, 1, x );
return ret;
}
/**
*
* Computes the cross product:
*
* c = a x b
*
*
* @param a Not modified.
* @param b Not modified.
* @param c Modified.
*/
public static void cross( GeoTuple3D_F32 a, GeoTuple3D_F32 b, GeoTuple3D_F32 c ) {
c.x = a.y * b.z - a.z * b.y;
c.y = a.z * b.x - a.x * b.z;
c.z = a.x * b.y - a.y * b.x;
}
/**
*
* Computes the cross product:
*
* c = a x b
*
*
* @param a_x x-coordinate of a
* @param a_y y-coordinate of a
* @param a_z z-coordinate of a
* @param b_x x-coordinate of b
* @param b_y y-coordinate of b
* @param b_z z-coordinate of b
* @param c Modified.
*/
public static void cross( float a_x, float a_y , float a_z ,
float b_x, float b_y , float b_z,
GeoTuple3D_F32 c ) {
c.x = a_y * b_z - a_z * b_y;
c.y = a_z * b_x - a_x * b_z;
c.z = a_x * b_y - a_y * b_x;
}
/**
*
* Computes the cross product:
*
* c = a x b
* where 'a' is in homogeneous coordinates.
*
*
* @param a Homogeneous coordinates, z = 1 assumed. Not modified.
* @param b Not modified.
* @param c Modified.
*/
public static void cross( GeoTuple2D_F32 a, GeoTuple3D_F32 b, GeoTuple3D_F32 c ) {
c.x = a.y * b.z - b.y;
c.y = b.x - a.x * b.z;
c.z = a.x * b.y - a.y * b.x;
}
/**
*
* Computes the cross product:
*
* c = a x b
*
*
* @param a Homogeneous coordinates, z = 1 assumed. Not modified.
* @param b Homogeneous coordinates, z = 1 assumed. Not modified.
* @param c Modified.
*/
public static void cross( GeoTuple2D_F32 a, GeoTuple2D_F32 b, GeoTuple3D_F32 c ) {
c.x = a.y * 1 - b.y;
c.y = b.x - a.x;
c.z = a.x * b.y - a.y * b.x;
}
/**
*
* Adds two points together.
*
* c = a + b
*
*
*
* Point 'c' can be the same instance as 'a' or 'b'.
*
*
* @param a A point. Not modified.
* @param b A point. Not modified.
* @param c Where the results are stored. Modified.
*/
public static void add( GeoTuple3D_F32 a, GeoTuple3D_F32 b, GeoTuple3D_F32 c ) {
c.x = a.x + b.x;
c.y = a.y + b.y;
c.z = a.z + b.z;
}
/**
*
* Adds two points together while scaling them.
*
* pt2 = a0 pt0 + a1 pt1
*
*
*
* Point 'c' can be the same instance as 'a' or 'b'.
*
*
* @param a0 Scaling factor for pt0.
* @param pt0 A point. Not modified.
* @param a1 Scaling factor for pt1.
* @param pt1 A point. Not modified.
* @param pt2 Where the results are stored. Modified.
*/
public static void add( float a0, GeoTuple3D_F32 pt0, float a1, GeoTuple3D_F32 pt1, GeoTuple3D_F32 pt2 ) {
pt2.x = a0 * pt0.x + a1 * pt1.x;
pt2.y = a0 * pt0.y + a1 * pt1.y;
pt2.z = a0 * pt0.z + a1 * pt1.z;
}
/**
* ret = p0 + M*p1
*
* @param p0
* @param M
* @param p1
* @param ret
*/
public static T addMult( T p0, FMatrixRMaj M, T p1, T ret ) {
ret = mult( M, p1, ret );
ret.x += p0.x;
ret.y += p0.y;
ret.z += p0.z;
return ret;
}
/**
*
* Substracts two points from each other.
*
* c = a - b
*
*
*
* Point 'c' can be the same instance as 'a' or 'b'.
*
*
* @param a A point. Not modified.
* @param b A point. Not modified.
* @param c Where the results are stored. Modified.
*/
public static void sub( GeoTuple3D_F32 a, GeoTuple3D_F32 b, GeoTuple3D_F32 c ) {
c.x = a.x - b.x;
c.y = a.y - b.y;
c.z = a.z - b.z;
}
/**
* Rotates a 2D point by the specified angle.
*
* @param theta
* @param pt
* @param solution where the solution is written to. Can be the same point as 'pt'.
*/
public static void rotate( float theta, GeoTuple2D_F32 pt, GeoTuple2D_F32 solution ) {
float c = (float)Math.cos( theta );
float s = (float)Math.sin( theta );
float x = pt.x;
float y = pt.y;
solution.x = c * x - s * y;
solution.y = s * x + c * y;
}
/**
* Rotates a 2D point by the specified angle.
*
* @param c Cosine of theta
* @param s Sine of theta
* @param pt
* @param solution where the solution is written to. Can be the same point as 'pt'.
*/
public static void rotate( float c , float s, GeoTuple2D_F32 pt, GeoTuple2D_F32 solution ) {
float x = pt.x;
float y = pt.y;
solution.x = c * x - s * y;
solution.y = s * x + c * y;
}
/**
* mod = M*pt
*
* pt and mod can be the same reference.
*
*
* @param M
* @param pt
* @param result Storage for output. Can be the same instance as param 'pt'. Modified.
*/
public static T mult( FMatrixRMaj M, T pt, T result ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );
if( result == null ) {
result = (T) pt.createNewInstance();
}
float x = pt.x;
float y = pt.y;
float z = pt.z;
result.x = (float) ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) * z );
result.y = (float) ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) * z );
result.z = (float) ( M.unsafe_get(2, 0) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) * z );
return (T) result;
}
/**
*
* mod = M*pt
* where mod is a 2D point that has an implicit z=1.
*
*
*
* Multiplies the 3x3 matrix against the 3D point, and normalizes the 2D point output
* by dividing the x and y values by the found z.
*
*/
public static T mult( FMatrixRMaj M, GeoTuple3D_F32 pt, T mod ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );
float x = pt.x;
float y = pt.y;
float z = pt.z;
mod.x = (float) ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) * z );
mod.y = (float) ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) * z );
z = (float) ( M.unsafe_get(2, 0) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) * z );
mod.x /= z;
mod.y /= z;
return mod;
}
/**
*
* mod = M*pt
* where pt has z=1 implicitly.
*
*
*
* Multiplies the 3x3 matrix against the 2D point, which has an implicit z=1 value, and the output is
* a 3d point.
*
*/
public static void mult( FMatrixRMaj M, GeoTuple2D_F32 pt, GeoTuple3D_F32 mod ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );
float x = pt.x;
float y = pt.y;
mod.x = (float) ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) );
mod.y = (float) ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) );
mod.z = (float) ( M.unsafe_get(2, 0) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) );
}
/**
*
* Computes mod = M*pt, where both pt and mod are in homogeneous coordinates with z assumed to be
* equal to 1, and M is a 3x3 matrix.
*
*
* 'pt' and 'mod' can be the same point.
*
* @param M 3x3 matrix
* @param pt Homogeneous point with z=1
* @param mod Storage for the computation. If null a new point is declared. Can be same instance as pt.
* @return Result of computation.
*/
public static T mult( FMatrixRMaj M, T pt, T mod ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );
if( mod == null ) {
throw new IllegalArgumentException( "Must provide an instance in mod" );
}
float x = pt.x;
float y = pt.y;
float modz = (float) ( M.unsafe_get( 2, 0 ) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) );
mod.x = (float) ( ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) ) / modz );
mod.y = (float) ( ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) ) / modz );
return mod;
}
/**
*
* Computes the following:
* result = cross(A)*M
* where M and result are 3x3 matrices, cross(A) is the cross product matrix of A.
*
*
* @param A 2D homogenous coordinate (implicit z = 1) that is internally converted into cross product matrix.
* @param M 3x3 matrix.
* @param result Storage for results. Can be null.
* @return Results.
*/
public static FMatrixRMaj multCrossA( GeoTuple2D_F32 A , FMatrixRMaj M, FMatrixRMaj result ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );
if( result == null ) {
result = new FMatrixRMaj(3,3);
}
float x = A.x;
float y = A.y;
float a11 = M.data[0]; float a12 = M.data[1]; float a13 = M.data[2];
float a21 = M.data[3]; float a22 = M.data[4]; float a23 = M.data[5];
float a31 = M.data[6]; float a32 = M.data[7]; float a33 = M.data[8];
result.data[0] = -a21 + a31*y;
result.data[1] = -a22 + a32*y;
result.data[2] = -a23 + a33*y;
result.data[3] = a11 - a31*x;
result.data[4] = a12 - a32*x;
result.data[5] = a13 - a33*x;
result.data[6] = -a11*y + a21*x;
result.data[7] = -a12*y + a22*x;
result.data[8] = -a13*y + a23*x;
return result;
}
/**
*
* Computes the following:
* result = cross(A)*M
* where M and result are 3x3 matrices, cross(A) is the cross product matrix of A.
*
*
* @param A 3D coordinate that is internally converted into cross product matrix.
* @param M 3x3 matrix.
* @param result Storage for results. Can be null.
* @return Results.
*/
public static FMatrixRMaj multCrossA( GeoTuple3D_F32 A , FMatrixRMaj M, FMatrixRMaj result ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );
if( result == null ) {
result = new FMatrixRMaj(3,3);
}
float x = A.x;
float y = A.y;
float z = A.z;
float a11 = M.data[0]; float a12 = M.data[1]; float a13 = M.data[2];
float a21 = M.data[3]; float a22 = M.data[4]; float a23 = M.data[5];
float a31 = M.data[6]; float a32 = M.data[7]; float a33 = M.data[8];
result.data[0] = -a21*z + a31*y;
result.data[1] = -a22*z + a32*y;
result.data[2] = -a23*z + a33*y;
result.data[3] = a11*z - a31*x;
result.data[4] = a12*z - a32*x;
result.data[5] = a13*z - a33*x;
result.data[6] = -a11*y + a21*x;
result.data[7] = -a12*y + a22*x;
result.data[8] = -a13*y + a23*x;
return result;
}
/**
* mod = MT*pt. Both pt and mod can be the same instance.
*
*
*/
public static T multTran( FMatrixRMaj M, T pt, T mod ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Rotation matrices are 3 by 3." );
if( mod == null ) {
mod = (T) pt.createNewInstance();
}
float x = pt.x;
float y = pt.y;
float z = pt.z;
mod.x = (float) ( M.unsafe_get( 0, 0 ) * x + M.unsafe_get( 1, 0 ) * y + M.unsafe_get( 2, 0 ) * z );
mod.y = (float) ( M.unsafe_get( 0, 1 ) * x + M.unsafe_get( 1, 1 ) * y + M.unsafe_get( 2, 1 ) * z );
mod.z = (float) ( M.unsafe_get( 0, 2 ) * x + M.unsafe_get( 1, 2 ) * y + M.unsafe_get( 2, 2 ) * z );
return (T) mod;
}
/**
* mod = MT*pt
* where pt.z = 1 implicitly.
*
* @param M 3 by 3 matrix.
* @param pt 2D point in homogeneous coordinates. Implicit z = 1
* @param mod 2D point in homogeneous coordinates. Implicit z = 1
* @return 2D point in homogeneous coordinates. Implicit z = 1
*/
public static T multTran( FMatrixRMaj M, GeoTuple2D_F32 pt, T mod ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Rotation matrices are 3 by 3." );
if( mod == null ) {
throw new IllegalArgumentException( "Must provide an instance in mod" );
}
float x = pt.x;
float y = pt.y;
mod.x = (float) ( M.unsafe_get( 0, 0 ) * x + M.unsafe_get( 1, 0 ) * y + M.unsafe_get( 2, 0 ) );
mod.y = (float) ( M.unsafe_get( 0, 1 ) * x + M.unsafe_get( 1, 1 ) * y + M.unsafe_get( 2, 1 ) );
mod.z = (float) ( M.unsafe_get( 0, 2 ) * x + M.unsafe_get( 1, 2 ) * y + M.unsafe_get( 2, 2 ) );
return mod;
}
/**
* mod = MT*pt
* where pt.z = 1 implicitly.
*
* @param M 3 by 3 matrix.
* @param pt 2D point in homogeneous coordinates. Implicit z = 1
* @param mod 2D point in homogeneous coordinates. Implicit z = 1
* @return 2D point in homogeneous coordinates. Implicit z = 1
*/
public static T multTran( FMatrixRMaj M, GeoTuple2D_F32 pt, T mod ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "Rotation matrices are 3 by 3." );
if( mod == null ) {
throw new IllegalArgumentException( "Must provide an instance in mod" );
}
float x = pt.x;
float y = pt.y;
float modZ = (float) ( M.unsafe_get( 0, 2 ) * x + M.unsafe_get( 1, 2 ) * y + M.unsafe_get( 2, 2 ) );
mod.x = (float) ( M.unsafe_get( 0, 0 ) * x + M.unsafe_get( 1, 0 ) * y + M.unsafe_get( 2, 0 ) )/modZ;
mod.y = (float) ( M.unsafe_get( 0, 1 ) * x + M.unsafe_get( 1, 1 ) * y + M.unsafe_get( 2, 1 ) )/modZ;
return mod;
}
/**
*
* ret = aT*M*b
*
*
* @param a 3D point.
* @param M 3 by 3 matrix.
* @param b 3D point.
* @return scalar number
*/
public static float innerProd( GeoTuple3D_F32 a, FMatrixRMaj M, GeoTuple3D_F32 b ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "M must be 3 by 3." );
FMatrixRMaj m1 = new FMatrixRMaj( 3, 1, true, a.x, a.y, a.z );
FMatrixRMaj m2 = new FMatrixRMaj( 3, 1, true, b.x, b.y, b.z );
return (float) ( VectorVectorMult_FDRM.innerProdA( m1, M, m2 ) );
}
/**
*
* Computes the inner matrix product:
* ret = xTATy
*
*
* @param a 3D point.
* @param M 3 by 3 matrix.
* @param b 3D point.
* @return scalar number
*/
public static float innerProdTranM( GeoTuple3D_F32 a, FMatrixRMaj M, GeoTuple3D_F32 b ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "M must be 3 by 3." );
FMatrixRMaj m1 = new FMatrixRMaj( 3, 1, true, a.x, a.y, a.z );
FMatrixRMaj m2 = new FMatrixRMaj( 3, 1, true, b.x, b.y, b.z );
return (float) ( VectorVectorMult_FDRM.innerProdTranA( m1, M, m2 ) );
}
/**
* Computes the outer product of two vectors:
* O = a*bT
*
* @param a 3D vector
* @param b 3D vector
* @param ret 3 x 3 matrix or null.
* @return outer product of two 3d vectors
*/
public static FMatrixRMaj outerProd(GeoTuple3D_F32 a, GeoTuple3D_F32 b, FMatrixRMaj ret) {
if( ret == null )
ret = new FMatrixRMaj(3,3);
ret.data[0] = a.x*b.x;
ret.data[1] = a.x*b.y;
ret.data[2] = a.x*b.z;
ret.data[3] = a.y*b.x;
ret.data[4] = a.y*b.y;
ret.data[5] = a.y*b.z;
ret.data[6] = a.z*b.x;
ret.data[7] = a.z*b.y;
ret.data[8] = a.z*b.z;
return ret;
}
/**
* Adds the outer product of two vectors onto a matrix:
* ret = A + scalar*a*bT
*
* @param A 3x3 matrix
* @param b 3D vector
* @param c 3D vector
* @param ret 3 x 3 matrix or null.
* @return outer product of two 3d vectors
*/
public static FMatrixRMaj addOuterProd(FMatrixRMaj A , float scalar , GeoTuple3D_F32 b, GeoTuple3D_F32 c, FMatrixRMaj ret) {
if( ret == null )
ret = new FMatrixRMaj(3,3);
ret.data[0] = A.data[0] + scalar*b.x*c.x;
ret.data[1] = A.data[1] + scalar*b.x*c.y;
ret.data[2] = A.data[2] + scalar*b.x*c.z;
ret.data[3] = A.data[3] + scalar*b.y*c.x;
ret.data[4] = A.data[4] + scalar*b.y*c.y;
ret.data[5] = A.data[5] + scalar*b.y*c.z;
ret.data[6] = A.data[6] + scalar*b.z*c.x;
ret.data[7] = A.data[7] + scalar*b.z*c.y;
ret.data[8] = A.data[8] + scalar*b.z*c.z;
return ret;
}
/**
*
* Computes the inner matrix product: ret = a'*M*b
* where ret is a scalar number. 'a' and 'b' are automatically converted into homogeneous
* coordinates.
*
*
* @param a 2D point.
* @param M 3 by 3 matrix.
* @param b 2D point.
* @return scalar number,
*/
public static float innerProd( GeoTuple2D_F32 a, FMatrixRMaj M, GeoTuple2D_F32 b ) {
if( M.numRows != 3 || M.numCols != 3 )
throw new IllegalArgumentException( "M must be 3 by 3." );
FMatrixRMaj m1 = new FMatrixRMaj( 3, 1, true, a.x, a.y, 1 );
FMatrixRMaj m2 = new FMatrixRMaj( 3, 1, true, b.x, b.y, 1 );
return (float) ( VectorVectorMult_FDRM.innerProdA( m1, M, m2 ) );
}
/**
*
* Dot product: ret = aTb
*
*
* @param a A tuple.
* @param b A tuple.
* @return scalar
*/
public static float dot( GeoTuple3D_F32 a, GeoTuple3D_F32 b ) {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/**
*
* Multiplies each element in the tuple by 'v'.
* pi=pi*v
*
*
* @param p tuple.
* @param v scaling factor.
*/
public static void scale( GeoTuple3D_F32 p, float v ) {
p.x *= v;
p.y *= v;
p.z *= v;
}
/**
* Divides each element by 'v'
*
* @param p tuple
* @param v divisor
*/
public static void divide( GeoTuple3D_F32 p , float v ) {
p.x /= v;
p.y /= v;
p.z /= v;
}
/**
*
* Changes the sign of the vector:
*
* T = -T
*
*
* @param t Vector whose sign is being changed. Modified.
*/
public static void changeSign( GeoTuple3D_F32 t ) {
t.x = -t.x;
t.y = -t.y;
t.z = -t.z;
}
/**
* Converts a GeoTuple3D_F32 into FMatrixRMaj
*
* @param in Input vector
* @param out Output matrix. If null a new matrix will be declared
* @return Converted matrix
*/
public static FMatrixRMaj toMatrix(GeoTuple3D_F32 in, FMatrixRMaj out) {
if( out == null )
out = new FMatrixRMaj(3,1);
else if( out.getNumElements() != 3 )
throw new IllegalArgumentException("Vector with 3 elements expected");
out.data[0] = in.x;
out.data[1] = in.y;
out.data[2] = in.z;
return out;
}
/**
* Converts a FMatrixRMaj into GeoTuple3D_F32
*
* @param in Input matrix
* @param out Output vector.
*/
public static void toTuple3D(FMatrixRMaj in, GeoTuple3D_F32 out) {
out.x = (float)in.get(0);
out.y = (float)in.get(1);
out.z = (float)in.get(2);
}
}