georegression.metric.UtilAngle 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.metric;
import georegression.misc.GrlConstants;
import static java.lang.Math.PI;
/**
* Utility functions relating to angles. Unless otherwise state all angles
* are in radians and have a domain of -pi to pi
*/
public class UtilAngle {
public static double radianToDegree( double angleRad ) {
return 180.0 * angleRad / Math.PI;
}
public static double degreeToRadian( double angleDegree ) {
return Math.PI * angleDegree / 180.0;
}
public static float radianToDegree( float angleRad ) {
return 180.0f * angleRad / GrlConstants.F_PI;
}
public static float degreeToRadian( float angleDegree ) {
return GrlConstants.F_PI * angleDegree / 180.0f;
}
/**
* Converts radians into degrees
* @param angleDegree angle in degrees
* @return angle in radians
*/
public static double radian( double angleDegree ) {
return degreeToRadian(angleDegree);
}
/**
* Converts degrees into radians
* @param angleRad angle in radians
* @return angle in degrees
*/
public static double degree( double angleRad ) {
return radianToDegree(angleRad);
}
/**
* Converts radians into degrees
* @param angleDegree angle in degrees
* @return angle in radians
*/
public static float radian( float angleDegree ) {
return degreeToRadian(angleDegree);
}
/**
* Converts degrees into radians
* @param angleRad angle in radians
* @return angle in degrees
*/
public static float degree( float angleRad ) {
return radianToDegree(angleRad);
}
public static double atanSafe( double y , double x ) {
if( x == 0.0 ) {
if( y >= 0.0)
return Math.PI / 2;
else
return -Math.PI / 2;
}
return Math.atan(y/x);
}
public static float atanSafe( float y , float x ) {
if( x == 0.0 )
if( y >= 0.0)
return GrlConstants.F_PId2;
else
return -GrlConstants.F_PId2;
return (float)Math.atan(y/x);
}
/**
* Converts an angle which is (-pi to pi) into a half circle angle (-pi/2 to pi/2).
*
* @param angle Angle between -pi and pi.
* @return -pi/2 to pi/2
*/
public static double toHalfCircle( double angle ) {
if( angle < 0 )
angle += Math.PI;
if( angle > Math.PI/2.0 )
angle -= Math.PI;
return angle;
}
/**
* Converts an angle which is (-pi to pi) into a half circle angle (0 to pi).
*
* @param angle Angle between -pi and pi.
* @return 0 to pi
*/
public static float toHalfCircle( float angle ) {
if( angle >= 0 )
return angle;
return GrlConstants.F_PI+angle;
}
/**
* Checks to see if it is between -π and π.
*
* @param ang Angle being tested
* @return true if it is between -π and π
*/
public static boolean isStandardDomain( double ang ) {
return ( ang <= Math.PI && ang >= -Math.PI );
}
/**
* Checks to see if it is between -π/2 and π/2.
*
* @param ang Angle being tested
* @return true if it is between -π/2 and π/2;
*/
public static boolean isHalfDomain( double ang ) {
return ( ang <= Math.PI/2 && ang >= -Math.PI/2 );
}
/**
* Returns an angle which is equivalent to the one provided, but between (inclusive) -π and π.
*/
public static double bound( double ang ) {
ang %= GrlConstants.PI2;
if( ang > PI ) {
return ang - GrlConstants.PI2;
} else if( ang < -PI ) {
return ang + GrlConstants.PI2;
}
return ang;
}
/**
* Returns an angle which is equivalent to the one provided, but between (inclusive) -π and π.
*/
public static float bound( float ang ) {
ang %= GrlConstants.F_PI2;
if( ang > GrlConstants.F_PI ) {
return ang - GrlConstants.F_PI2;
} else if( ang < -GrlConstants.F_PI ) {
return ang + GrlConstants.F_PI2;
}
return ang;
}
/**
* Bounds the angle between -π/2 and &pi/2
* @param angle angle in radians
* @return bounded angle
*/
public static double boundHalf( double angle ) {
angle = bound(angle);
if( angle > GrlConstants.PId2 ) {
angle -= Math.PI;
} else if( angle < -GrlConstants.PId2 ){
angle += Math.PI;
}
return angle;
}
/**
* Bounds the angle between -π/2 and &pi/2
* @param angle angle in radians
* @return bounded angle
*/
public static float boundHalf( float angle ) {
angle = bound(angle);
if( angle > GrlConstants.F_PId2 ) {
angle -= GrlConstants.F_PI;
} else if( angle < -GrlConstants.F_PId2 ){
angle += GrlConstants.F_PI;
}
return angle;
}
/**
* Angular distance in radians to go from angA to angB in counter clock-wise direction.
* The resulting angle will be from 0 to 2π.
*
* @param angA First angle. -pi to pi
* @param angB Second angle -pi to pi
* @return An angle from 0 to 2 π
*/
public static double distanceCCW( double angA, double angB ) {
if( angB >= angA )
return angB-angA;
else
return GrlConstants.PI2 - (angA-angB);
}
/**
* Angular distance in radians to go from angA to angB in counter clock-wise direction.
* The resulting angle will be from 0 to 2π.
*
* @param angA First angle. -pi to pi
* @param angB Second angle -pi to pi
* @return An angle from 0 to 2 π
*/
public static float distanceCCW( float angA, float angB ) {
if( angB >= angA )
return angB-angA;
else
return GrlConstants.F_PI2 - (angA-angB);
}
/**
* Angular distance in radians to go from angA to angB in clock-wise direction.
* The resulting angle will be from 0 to 2π.
*
* @param angA First angle. -pi to pi
* @param angB Second angle -pi to pi
* @return An angle from 0 to 2 π
*/
public static double distanceCW( double angA, double angB ) {
if( angA >= angB )
return angA-angB;
else
return GrlConstants.PI2-(angB-angA);
}
/**
* Angular distance in radians to go from angA to angB in clock-wise direction.
* The resulting angle will be from 0 to 2π.
*
* @param angA First angle. -pi to pi
* @param angB Second angle -pi to pi
* @return An angle from 0 to 2 π
*/
public static float distanceCW( float angA, float angB ) {
if( angA >= angB )
return angA-angB;
else
return GrlConstants.F_PI2-(angB-angA);
}
/**
*
* Returns the difference between two angles and bounds the result between -pi and pi:
* result = angA - angB
* and takes in account boundary conditions.
*
*
* @param angA first angle. Must be between -pi and pi.
* @param angB second angle. Must be between -pi and pi.
* @return an angle between -pi and pi
*/
public static double minus( double angA, double angB ) {
double diff = angA - angB;
if( diff > Math.PI ) {
return GrlConstants.PI2 - diff;
} else if( diff < -Math.PI )
return -GrlConstants.PI2 - diff;
return diff;
}
/**
*
* Returns the difference between two angles and bounds the result between -pi and pi:
* result = angA - angB
* and takes in account boundary conditions.
*
*
* @param angA first angle. Must be between -pi and pi.
* @param angB second angle. Must be between -pi and pi.
* @return an angle between -pi and pi
*/
public static float minus( float angA, float angB ) {
float diff = angA - angB;
if( diff > GrlConstants.F_PI ) {
return GrlConstants.F_PI2 - diff;
} else if( diff < -GrlConstants.F_PI )
return -GrlConstants.F_PI2 - diff;
return diff;
}
/**
*
* Returns the number of radians two angles are apart. This is equivalent to
* Math.abs(UtilAngle.minus(angA,angB)).
*
*
* @param angA first angle. Must be between -pi and pi.
* @param angB second angle. Must be between -pi and pi.
* @return an angle between 0 and pi
*/
public static double dist( double angA, double angB ) {
return Math.abs(minus(angA,angB));
}
/**
*
* Returns the number of radians two angles are apart. This is equivalent to
* Math.abs(UtilAngle.minus(angA,angB)).
*
*
* @param angA first angle. Must be between -pi and pi.
* @param angB second angle. Must be between -pi and pi.
* @return an angle between 0 and pi
*/
public static float dist( float angA, float angB ) {
return Math.abs(minus(angA,angB));
}
/**
* Angular distance between two half circle angles.
*
* @param angA Angle between -pi/2 and pi/2.
* @param angB Angle between -pi/2 and pi/2.
* @return Acute angle between the two input angles.
*/
public static double distHalf( double angA , double angB ) {
double a = Math.abs(angA-angB);
if( a <= Math.PI/2 )
return a;
else
return Math.PI-a;
}
/**
* Converts an angle from -pi to pi into 0 to 2*pi domain
* @param angle angle from -pi to pi radians
* @return angle from 0 to 2*pi radians
*/
public static double domain2PI( double angle ) {
if( angle < 0 ) {
return angle + 2*Math.PI;
} else {
return angle;
}
}
/**
* Ensures a wrapping circular bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is exclusive.
* Examples:
* 1.5 = 0.5
* -0.25 = 0.75
* 0 = 0
* 1 = 0
* 0.999 = 0.999
* 2 = 0
* -1 = 0
*
*
* @return A value from 0 to 1. [0,1)
*/
public static double wrapZeroToOne(double value ) {
if( value >= 0 )
return value % 1.0;
else {
return (1.0+(value%1.0))%1.0; // last bit is to ensure that 1.0 is returned as 0.0
}
}
/**
* Ensures a wrapping circular bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is exclusive.
* Examples:
* 1.5 = 0.5
* -0.25 = 0.75
* 0 = 0
* 1 = 0
* 0.999 = 0.999
* 2 = 0
* -1 = 0
*
*
* @return A value from 0 to 1. [0,1)
*/
public static float wrapZeroToOne(float value ) {
if( value >= 0 )
return value % 1.0f;
else {
return (1.0f+(value%1.0f))%1.0f; // last bit is to ensure that 1.0 is returned as 0.0
}
}
/**
* Ensures a reflective bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is inclusive.
* Examples:
* 1.5 = 0.5
* -0.25 = 0.25
* -0.75 = 0.75
* 0 = 0
* 1 = 1
* 0.999 = 0.999
* 2 = 0
* -1 = 1
*
*
* @return A value from 0 to 1. [0,1]
*/
public static double reflectZeroToOne(double value ) {
if( value < 0 )
value = -value;
value = value%2.0;
if( value > 1.0 )
return 2.0 - value;
return value;
}
/**
* Ensures a reflective bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is inclusive.
* Examples:
* 1.5 = 0.5
* -0.25 = 0.25
* -0.75 = 0.75
* 0 = 0
* 1 = 1
* 0.999 = 0.999
* 2 = 0
* -1 = 1
*
*
* @return A value from 0 to 1. [0,1]
*/
public static float reflectZeroToOne(float value ) {
if( value < 0 )
value = -value;
value = value%2.0f;
if( value > 1.0f )
return 2.0f - value;
return value;
}
}