/*******************************************************************************
* Copyright 2010 Simon Mieth
*
* 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 org.kabeja.math;
/**
* @author <a href="mailto:simon.mieth@gmx.de>Simon Mieth</a>
*
*/
public class MathUtils {
public final static double DISTANCE_DELTA = 0.00001;
/**
* Calculate the scalar product of vector a and vector b
*
* @param a
* @param b
* @return the result of the scalar product
*/
public static double scalarProduct(Vector a, Vector b) {
double r = (a.getX() * b.getX()) + (a.getY() * b.getY()) +
(a.getZ() * b.getZ());
return r;
}
/**
* Returns the absalute value (or length) of the vector
*
* @param v
* @return
*/
public static double absoluteValue(Vector v) {
double r = Math.sqrt(Math.pow(v.getX(), 2) + Math.pow(v.getY(), 2) +
Math.pow(v.getZ(), 2));
return r;
}
/**
* Calculate the cross product of 2 vectors.
*
* @param a
* @param b
* @return a new vector as result of the cross product of a and b
*/
public static Vector crossProduct(Vector a, Vector b) {
Vector r = new Vector();
r.setX((a.getY() * b.getZ()) - (a.getZ() * b.getY()));
r.setY((a.getZ() * b.getX()) - (a.getX() * b.getZ()));
r.setZ((a.getX() * b.getY()) - (a.getY() * b.getX()));
return r;
}
/**
* Scale a vector with the given value
*
* @param a
* the vector
* @param scale
* the value to scale
* @return a new vector scaled with the given
*/
public static Vector scaleVector(Vector a, double scale) {
Vector v = new Vector();
v.setX(a.getX() * scale);
v.setY(a.getY() * scale);
v.setZ(a.getZ() * scale);
return v;
}
/**
* Calculate a point of a straigt line.
*
* @param a
* the startpoint of the straight line
* @param direction
* the direction vector of the straight line
* @param parameter
* the parameter
* @return a new point
*/
public static Point3D getPointOfStraightLine(Point3D a, Vector direction,
double parameter) {
// p = a + v*scale
Point3D p = new Point3D();
Vector v = scaleVector(direction, parameter);
p.setX(a.getX() + v.getX());
p.setY(a.getY() + v.getY());
p.setZ(a.getZ() + v.getZ());
return p;
}
/**
* Calculate the vector from point a to point b
*
* @param a
* @param b
* @return the vector from a to b
*/
public static Vector getVector(Point3D a, Point3D b) {
Vector v = new Vector();
v.setX(b.getX() - a.getX());
v.setY(b.getY() - a.getY());
v.setZ(b.getZ() - a.getZ());
return v;
}
public static Point3D getIntersection(Point3D a, Vector u, Point3D b, Vector v) {
//u = normalize(u);
//v = normalize(v);
Vector n = crossProduct(u, v);
Vector m = crossProduct(getVector(a, b), v);
double s = 0;
if (n.getZ() != 0.0) {
s = m.getZ() / n.getZ();
} else if (n.getY() != 0.0) {
s = m.getY() / n.getY();
} else if (n.getX() != 0.0) {
s = m.getX() / n.getX();
}
Point3D p = getPointOfStraightLine(a, u, s);
return p;
}
public static double distance(Point3D start, Point3D end) {
double length = Math.sqrt(Math.pow((end.getX() - start.getX()), 2) +
Math.pow((end.getY() - start.getY()), 2) +
Math.pow((end.getZ() - start.getZ()), 2));
return length;
}
/**
* Calculate the angle between vector a vector b
*
* @param a
* @param b
* @return the angle in radian
*/
public static double getAngle(Vector a, Vector b) {
double cos = scalarProduct(a, b) / (absoluteValue(a) * absoluteValue(b));
return Math.acos(cos);
}
/**
* Rotate the given point around centerpoint with the given angle in X-Y
* plane.
*
* @param p
* the point to rotate
* @param center
* the centerpoint
* @param angle
* in radian
* @return the rotated point
*/
public static Point3D rotatePointXY(Point3D p, Point3D center, double angle) {
Point3D r = new Point3D();
r.setX((center.getX() + (Math.cos(angle) * (p.getX() - center.getX()))) -
((p.getY() - center.getY()) * Math.sin(angle)));
r.setY((center.getY() + (Math.cos(angle) * (p.getY() - center.getY()))) -
((p.getX() - center.getX()) * Math.sin(angle)));
r.setZ(p.getZ());
return r;
}
public static Vector normalize(Vector v) {
double l = absoluteValue(v);
return scaleVector(v, (1 / l));
}
/**
* Returns the qaudrant:<br/>
* 0,1,2 or 3
* @param p
* @param center
* @return
*/
public static int getQuadrant(Point3D p, Point3D center) {
if (p.getX() < center.getX()) {
if (p.getY() >= center.getY()) {
return 1;
} else {
return 2;
}
} else {
if (p.getY() >= center.getY()) {
return 0;
} else {
return 3;
}
}
}
/**
* Returns the qaudrant for the given angle:<br/>
* 0,1,2 or 3
* @param angle in degree
* @return the quadrant
*/
public static int getQuadrant(double angle) {
if (angle > 360) {
angle = angle - (Math.floor(angle / 360) * 360);
}
if ((angle >= 0) && (angle < 90)) {
return 0;
} else if ((angle >= 90) && (angle < 180)) {
return 1;
} else if ((angle >= 180) && (angle < 270)) {
return 2;
} else {
return 3;
}
}
/**
* Returns the angle of the vector again the x-axis
* @param v the vector
* @return the angle in degree
*/
public static double getDirectionAngle(Vector v) {
double l = absoluteValue(v);
if ((v.getX() < 0) && (v.getY() < 0)) {
return (Math.toDegrees(Math.acos(Math.abs(v.getX() / l))) + 180);
} else if ((v.getX() == 0.0) || (v.getY() < 0)) {
return Math.toDegrees(Math.asin(v.getY() / l));
} else {
return Math.toDegrees(Math.acos(v.getX() / l));
}
}
/**
* Invert the direction of the given vector
* @param the vector to invert
* @return new inverted vector
*/
public static Vector invertDirection(Vector v) {
return scaleVector(v, -1);
}
public static double[][] multiplyMatrixByMatrix(double[][] a, double[][] b)
throws IllegalArgumentException {
if (a[0].length != b.length) {
throw new IllegalArgumentException(
"Cannot multiply a with b, columns of a != rows of b. ");
}
double[][] c = new double[a.length][b[0].length];
for (int i = 0; i < a.length; i++) {
for (int x = 0; x < b.length; x++) {
for (int y = 0; y < a[i].length; y++) {
c[i][x] += (a[i][y] * b[y][x]);
}
}
}
return c;
}
public static double[] multiplyMatrixByVector(double[][] a, double[] v)
throws IllegalArgumentException {
if (a[0].length != v.length) {
throw new IllegalArgumentException(
"Cannot multiply a with v, columns of a != rows of v. ");
}
double[] r = new double[a.length];
for (int i = 0; i < a.length; i++) {
for (int x = 0; x < v.length; x++) {
r[i] += (a[i][x] * v[x]);
}
}
return r;
}
/**
* Substracts a vector from other vector.
* @param a the minuend
* @param b the subtrahend
* @return the difference as vector
*/
public static Vector subtractVectorByVector(Vector a, Vector b) {
Vector result = new Vector();
result.setX(a.getX() - b.getX());
result.setY(a.getY() - b.getY());
result.setZ(a.getZ() - b.getZ());
return result;
}
public static Vector addVectorToVector(Vector a, Vector b) {
Vector result = new Vector();
result.setX(a.getX() + b.getX());
result.setY(a.getY() + b.getY());
result.setZ(a.getZ() + b.getZ());
return result;
}
}