/*
* (C) Copyright 2015-2017 Kai Burjack
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
package org.joml;
import java.io.Externalizable;
import java.io.IOException;
import java.io.ObjectInput;
import java.io.ObjectOutput;
import java.text.DecimalFormat;
import java.text.NumberFormat;
/**
* Represents a 3D rotation of a given radians about an axis represented as an
* unit 3D vector.
* <p>
* This class uses double-precision components.
*
* @author Kai Burjack
*/
public class AxisAngle4d implements Externalizable {
private static final long serialVersionUID = 1L;
/**
* The angle in radians.
*/
public double angle;
/**
* The x-component of the rotation axis.
*/
public double x;
/**
* The y-component of the rotation axis.
*/
public double y;
/**
* The z-component of the rotation axis.
*/
public double z;
/**
* Create a new {@link AxisAngle4d} with zero rotation about <tt>(0, 0, 1)</tt>.
*/
public AxisAngle4d() {
z = 1.0;
}
/**
* Create a new {@link AxisAngle4d} with the same values of <code>a</code>.
*
* @param a
* the AngleAxis4d to copy the values from
*/
public AxisAngle4d(AxisAngle4d a) {
x = a.x;
y = a.y;
z = a.z;
angle = (a.angle < 0.0 ? Math.PI + Math.PI + a.angle % (Math.PI + Math.PI) : a.angle) % (Math.PI + Math.PI);
}
/**
* Create a new {@link AxisAngle4d} with the same values of <code>a</code>.
*
* @param a
* the AngleAxis4f to copy the values from
*/
public AxisAngle4d(AxisAngle4f a) {
x = a.x;
y = a.y;
z = a.z;
angle = (a.angle < 0.0 ? Math.PI + Math.PI + a.angle % (Math.PI + Math.PI) : a.angle) % (Math.PI + Math.PI);
}
/**
* Create a new {@link AxisAngle4d} from the given {@link Quaternionfc}.
* <p>
* Reference: <a href=
* "http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/"
* >http://www.euclideanspace.com</a>
*
* @param q
* the quaternion from which to create the new AngleAxis4f
*/
public AxisAngle4d(Quaternionfc q) {
double acos = safeAcos(q.w());
double invSqrt = 1.0 / Math.sqrt(1.0 - q.w() * q.w());
x = q.x() * invSqrt;
y = q.y() * invSqrt;
z = q.z() * invSqrt;
angle = acos + acos;
}
/**
* Create a new {@link AxisAngle4d} from the given {@link Quaterniondc}.
* <p>
* Reference: <a href=
* "http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/"
* >http://www.euclideanspace.com</a>
*
* @param q
* the quaternion from which to create the new AngleAxis4d
*/
public AxisAngle4d(Quaterniondc q) {
double acos = safeAcos(q.w());
double invSqrt = 1.0 / Math.sqrt(1.0 - q.w() * q.w());
x = q.x() * invSqrt;
y = q.y() * invSqrt;
z = q.z() * invSqrt;
angle = acos + acos;
}
/**
* Create a new {@link AxisAngle4d} with the given values.
*
* @param angle
* the angle in radians
* @param x
* the x-coordinate of the rotation axis
* @param y
* the y-coordinate of the rotation axis
* @param z
* the z-coordinate of the rotation axis
*/
public AxisAngle4d(double angle, double x, double y, double z) {
this.x = x;
this.y = y;
this.z = z;
this.angle = (angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI);
}
/**
* Create a new {@link AxisAngle4d} with the given values.
*
* @param angle the angle in radians
* @param v the rotation axis as a {@link Vector3dc}
*/
public AxisAngle4d(double angle, Vector3dc v) {
this(angle, v.x(), v.y(), v.z());
}
/**
* Create a new {@link AxisAngle4d} with the given values.
*
* @param angle the angle in radians
* @param v the rotation axis as a {@link Vector3f}
*/
public AxisAngle4d(double angle, Vector3f v) {
this(angle, v.x, v.y, v.z);
}
/**
* Set this {@link AxisAngle4d} to the values of <code>a</code>.
*
* @param a
* the AngleAxis4f to copy the values from
* @return this
*/
public AxisAngle4d set(AxisAngle4d a) {
x = a.x;
y = a.y;
z = a.z;
angle = (a.angle < 0.0 ? Math.PI + Math.PI + a.angle % (Math.PI + Math.PI) : a.angle) % (Math.PI + Math.PI);
return this;
}
/**
* Set this {@link AxisAngle4d} to the values of <code>a</code>.
*
* @param a
* the AngleAxis4f to copy the values from
* @return this
*/
public AxisAngle4d set(AxisAngle4f a) {
x = a.x;
y = a.y;
z = a.z;
angle = (a.angle < 0.0 ? Math.PI + Math.PI + a.angle % (Math.PI + Math.PI) : a.angle) % (Math.PI + Math.PI);
return this;
}
/**
* Set this {@link AxisAngle4d} to the given values.
*
* @param angle
* the angle in radians
* @param x
* the x-coordinate of the rotation axis
* @param y
* the y-coordinate of the rotation axis
* @param z
* the z-coordinate of the rotation axis
* @return this
*/
public AxisAngle4d set(double angle, double x, double y, double z) {
this.x = x;
this.y = y;
this.z = z;
this.angle = (angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI);
return this;
}
/**
* Set this {@link AxisAngle4d} to the given values.
*
* @param angle
* the angle in radians
* @param v
* the rotation axis as a {@link Vector3dc}
* @return this
*/
public AxisAngle4d set(double angle, Vector3dc v) {
return set(angle, v.x(), v.y(), v.z());
}
/**
* Set this {@link AxisAngle4d} to the given values.
*
* @param angle
* the angle in radians
* @param v
* the rotation axis as a {@link Vector3f}
* @return this
*/
public AxisAngle4d set(double angle, Vector3f v) {
return set(angle, v.x, v.y, v.z);
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the given
* {@link Quaternionfc}.
*
* @param q
* the quaternion to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Quaternionfc q) {
double acos = safeAcos(q.w());
double invSqrt = 1.0 / Math.sqrt(1.0 - q.w() * q.w());
this.x = q.x() * invSqrt;
this.y = q.y() * invSqrt;
this.z = q.z() * invSqrt;
this.angle = acos + acos;
return this;
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the given
* {@link Quaterniondc}.
*
* @param q
* the quaternion to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Quaterniondc q) {
double acos = safeAcos(q.w());
double invSqrt = 1.0 / Math.sqrt(1.0 - q.w() * q.w());
this.x = q.x() * invSqrt;
this.y = q.y() * invSqrt;
this.z = q.z() * invSqrt;
this.angle = acos + acos;
return this;
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the rotation
* of the given {@link Matrix3fc}.
* <p>
* Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
*
* @param m
* the Matrix3fc to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Matrix3fc m) {
double nm00 = m.m00(), nm01 = m.m01(), nm02 = m.m02();
double nm10 = m.m10(), nm11 = m.m11(), nm12 = m.m12();
double nm20 = m.m20(), nm21 = m.m21(), nm22 = m.m22();
double lenX = 1.0 / Math.sqrt(m.m00() * m.m00() + m.m01() * m.m01() + m.m02() * m.m02());
double lenY = 1.0 / Math.sqrt(m.m10() * m.m10() + m.m11() * m.m11() + m.m12() * m.m12());
double lenZ = 1.0 / Math.sqrt(m.m20() * m.m20() + m.m21() * m.m21() + m.m22() * m.m22());
nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
double epsilon = 1E-4;
if ((Math.abs(nm10 - nm01) < epsilon) && (Math.abs(nm20 - nm02) < epsilon) && (Math.abs(nm21 - nm12) < epsilon)) {
angle = Math.PI;
double xx = (nm00 + 1) / 2;
double yy = (nm11 + 1) / 2;
double zz = (nm22 + 1) / 2;
double xy = (nm10 + nm01) / 4;
double xz = (nm20 + nm02) / 4;
double yz = (nm21 + nm12) / 4;
if ((xx > yy) && (xx > zz)) {
x = Math.sqrt(xx);
y = xy / x;
z = xz / x;
} else if (yy > zz) {
y = Math.sqrt(yy);
x = xy / y;
z = yz / y;
} else {
z = Math.sqrt(zz);
x = xz / z;
y = yz / z;
}
return this;
}
double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
angle = safeAcos((nm00 + nm11 + nm22 - 1) / 2);
x = (nm12 - nm21) / s;
y = (nm20 - nm02) / s;
z = (nm01 - nm10) / s;
return this;
}
private static double safeAcos(double v) {
if (v < -1.0)
return Math.PI;
else if (v > +1.0)
return 0.0;
else
return Math.acos(v);
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the rotation
* of the given {@link Matrix3dc}.
* <p>
* Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
*
* @param m
* the Matrix3dc to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Matrix3dc m) {
double nm00 = m.m00(), nm01 = m.m01(), nm02 = m.m02();
double nm10 = m.m10(), nm11 = m.m11(), nm12 = m.m12();
double nm20 = m.m20(), nm21 = m.m21(), nm22 = m.m22();
double lenX = 1.0 / Math.sqrt(m.m00() * m.m00() + m.m01() * m.m01() + m.m02() * m.m02());
double lenY = 1.0 / Math.sqrt(m.m10() * m.m10() + m.m11() * m.m11() + m.m12() * m.m12());
double lenZ = 1.0 / Math.sqrt(m.m20() * m.m20() + m.m21() * m.m21() + m.m22() * m.m22());
nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
double epsilon = 1E-4;
if ((Math.abs(nm10 - nm01) < epsilon) && (Math.abs(nm20 - nm02) < epsilon) && (Math.abs(nm21 - nm12) < epsilon)) {
angle = Math.PI;
double xx = (nm00 + 1) / 2;
double yy = (nm11 + 1) / 2;
double zz = (nm22 + 1) / 2;
double xy = (nm10 + nm01) / 4;
double xz = (nm20 + nm02) / 4;
double yz = (nm21 + nm12) / 4;
if ((xx > yy) && (xx > zz)) {
x = Math.sqrt(xx);
y = xy / x;
z = xz / x;
} else if (yy > zz) {
y = Math.sqrt(yy);
x = xy / y;
z = yz / y;
} else {
z = Math.sqrt(zz);
x = xz / z;
y = yz / z;
}
return this;
}
double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
angle = safeAcos((nm00 + nm11 + nm22 - 1) / 2);
x = (nm12 - nm21) / s;
y = (nm20 - nm02) / s;
z = (nm01 - nm10) / s;
return this;
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the rotational component
* of the given {@link Matrix4fc}.
* <p>
* Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
*
* @param m
* the Matrix4fc to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Matrix4fc m) {
double nm00 = m.m00(), nm01 = m.m01(), nm02 = m.m02();
double nm10 = m.m10(), nm11 = m.m11(), nm12 = m.m12();
double nm20 = m.m20(), nm21 = m.m21(), nm22 = m.m22();
double lenX = 1.0 / Math.sqrt(m.m00() * m.m00() + m.m01() * m.m01() + m.m02() * m.m02());
double lenY = 1.0 / Math.sqrt(m.m10() * m.m10() + m.m11() * m.m11() + m.m12() * m.m12());
double lenZ = 1.0 / Math.sqrt(m.m20() * m.m20() + m.m21() * m.m21() + m.m22() * m.m22());
nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
double epsilon = 1E-4;
if ((Math.abs(nm10 - nm01) < epsilon) && (Math.abs(nm20 - nm02) < epsilon) && (Math.abs(nm21 - nm12) < epsilon)) {
angle = Math.PI;
double xx = (nm00 + 1) / 2;
double yy = (nm11 + 1) / 2;
double zz = (nm22 + 1) / 2;
double xy = (nm10 + nm01) / 4;
double xz = (nm20 + nm02) / 4;
double yz = (nm21 + nm12) / 4;
if ((xx > yy) && (xx > zz)) {
x = Math.sqrt(xx);
y = xy / x;
z = xz / x;
} else if (yy > zz) {
y = Math.sqrt(yy);
x = xy / y;
z = yz / y;
} else {
z = Math.sqrt(zz);
x = xz / z;
y = yz / z;
}
return this;
}
double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
angle = safeAcos((nm00 + nm11 + nm22 - 1) / 2);
x = (nm12 - nm21) / s;
y = (nm20 - nm02) / s;
z = (nm01 - nm10) / s;
return this;
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the rotational component
* of the given {@link Matrix4x3fc}.
* <p>
* Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
*
* @param m
* the Matrix4x3fc to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Matrix4x3fc m) {
double nm00 = m.m00(), nm01 = m.m01(), nm02 = m.m02();
double nm10 = m.m10(), nm11 = m.m11(), nm12 = m.m12();
double nm20 = m.m20(), nm21 = m.m21(), nm22 = m.m22();
double lenX = 1.0 / Math.sqrt(m.m00() * m.m00() + m.m01() * m.m01() + m.m02() * m.m02());
double lenY = 1.0 / Math.sqrt(m.m10() * m.m10() + m.m11() * m.m11() + m.m12() * m.m12());
double lenZ = 1.0 / Math.sqrt(m.m20() * m.m20() + m.m21() * m.m21() + m.m22() * m.m22());
nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
double epsilon = 1E-4;
if ((Math.abs(nm10 - nm01) < epsilon) && (Math.abs(nm20 - nm02) < epsilon) && (Math.abs(nm21 - nm12) < epsilon)) {
angle = Math.PI;
double xx = (nm00 + 1) / 2;
double yy = (nm11 + 1) / 2;
double zz = (nm22 + 1) / 2;
double xy = (nm10 + nm01) / 4;
double xz = (nm20 + nm02) / 4;
double yz = (nm21 + nm12) / 4;
if ((xx > yy) && (xx > zz)) {
x = Math.sqrt(xx);
y = xy / x;
z = xz / x;
} else if (yy > zz) {
y = Math.sqrt(yy);
x = xy / y;
z = yz / y;
} else {
z = Math.sqrt(zz);
x = xz / z;
y = yz / z;
}
return this;
}
double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
angle = safeAcos((nm00 + nm11 + nm22 - 1) / 2);
x = (nm12 - nm21) / s;
y = (nm20 - nm02) / s;
z = (nm01 - nm10) / s;
return this;
}
/**
* Set this {@link AxisAngle4d} to be equivalent to the rotational component
* of the given {@link Matrix4dc}.
* <p>
* Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
*
* @param m
* the Matrix4dc to set this AngleAxis4d from
* @return this
*/
public AxisAngle4d set(Matrix4dc m) {
double nm00 = m.m00(), nm01 = m.m01(), nm02 = m.m02();
double nm10 = m.m10(), nm11 = m.m11(), nm12 = m.m12();
double nm20 = m.m20(), nm21 = m.m21(), nm22 = m.m22();
double lenX = 1.0 / Math.sqrt(m.m00() * m.m00() + m.m01() * m.m01() + m.m02() * m.m02());
double lenY = 1.0 / Math.sqrt(m.m10() * m.m10() + m.m11() * m.m11() + m.m12() * m.m12());
double lenZ = 1.0 / Math.sqrt(m.m20() * m.m20() + m.m21() * m.m21() + m.m22() * m.m22());
nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
double epsilon = 1E-4;
if ((Math.abs(nm10 - nm01) < epsilon) && (Math.abs(nm20 - nm02) < epsilon) && (Math.abs(nm21 - nm12) < epsilon)) {
angle = Math.PI;
double xx = (nm00 + 1) / 2;
double yy = (nm11 + 1) / 2;
double zz = (nm22 + 1) / 2;
double xy = (nm10 + nm01) / 4;
double xz = (nm20 + nm02) / 4;
double yz = (nm21 + nm12) / 4;
if ((xx > yy) && (xx > zz)) {
x = Math.sqrt(xx);
y = xy / x;
z = xz / x;
} else if (yy > zz) {
y = Math.sqrt(yy);
x = xy / y;
z = yz / y;
} else {
z = Math.sqrt(zz);
x = xz / z;
y = yz / z;
}
return this;
}
double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
angle = safeAcos((nm00 + nm11 + nm22 - 1) / 2);
x = (nm12 - nm21) / s;
y = (nm20 - nm02) / s;
z = (nm01 - nm10) / s;
return this;
}
/**
* Set the given {@link Quaternionf} to be equivalent to this {@link AxisAngle4d} rotation.
*
* @see Quaternionf#set(AxisAngle4d)
*
* @param q
* the quaternion to set
* @return q
*/
public Quaternionf get(Quaternionf q) {
return q.set(this);
}
/**
* Set the given {@link Quaterniond} to be equivalent to this {@link AxisAngle4d} rotation.
*
* @see Quaterniond#set(AxisAngle4d)
*
* @param q
* the quaternion to set
* @return q
*/
public Quaterniond get(Quaterniond q) {
return q.set(this);
}
/**
* Set the given {@link Matrix4f} to a rotation transformation equivalent to this {@link AxisAngle4d}.
*
* @see Matrix4f#set(AxisAngle4d)
*
* @param m
* the matrix to set
* @return m
*/
public Matrix4f get(Matrix4f m) {
return m.set(this);
}
/**
* Set the given {@link Matrix3f} to a rotation transformation equivalent to this {@link AxisAngle4d}.
*
* @see Matrix3f#set(AxisAngle4d)
*
* @param m
* the matrix to set
* @return m
*/
public Matrix3f get(Matrix3f m) {
return m.set(this);
}
/**
* Set the given {@link Matrix4d} to a rotation transformation equivalent to this {@link AxisAngle4d}.
*
* @see Matrix4f#set(AxisAngle4d)
*
* @param m
* the matrix to set
* @return m
*/
public Matrix4d get(Matrix4d m) {
return m.set(this);
}
/**
* Set the given {@link Matrix3d} to a rotation transformation equivalent to this {@link AxisAngle4d}.
*
* @see Matrix3f#set(AxisAngle4d)
*
* @param m
* the matrix to set
* @return m
*/
public Matrix3d get(Matrix3d m) {
return m.set(this);
}
public void writeExternal(ObjectOutput out) throws IOException {
out.writeDouble(angle);
out.writeDouble(x);
out.writeDouble(y);
out.writeDouble(z);
}
public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException {
angle = in.readDouble();
x = in.readDouble();
y = in.readDouble();
z = in.readDouble();
}
/**
* Normalize the axis vector.
*
* @return this
*/
public AxisAngle4d normalize() {
double invLength = 1.0 / Math.sqrt(x * x + y * y + z * z);
x *= invLength;
y *= invLength;
z *= invLength;
return this;
}
/**
* Increase the rotation angle by the given amount.
* <p>
* This method also takes care of wrapping around.
*
* @param ang
* the angle increase
* @return this
*/
public AxisAngle4d rotate(double ang) {
angle += ang;
angle = (angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI);
return this;
}
/**
* Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}.
*
* @param v
* the vector to transform
* @return v
*/
public Vector3d transform(Vector3d v) {
return transform(v, v);
}
/**
* Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}
* and store the result in <code>dest</code>.
*
* @param v
* the vector to transform
* @param dest
* will hold the result
* @return dest
*/
public Vector3d transform(Vector3dc v, Vector3d dest) {
double sin = Math.sin(angle);
double cos = Math.cosFromSin(sin, angle);
double dot = x * v.x() + y * v.y() + z * v.z();
dest.set(v.x() * cos + sin * (y * v.z() - z * v.y()) + (1.0 - cos) * dot * x,
v.y() * cos + sin * (z * v.x() - x * v.z()) + (1.0 - cos) * dot * y,
v.z() * cos + sin * (x * v.y() - y * v.x()) + (1.0 - cos) * dot * z);
return dest;
}
/**
* Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}.
*
* @param v
* the vector to transform
* @return v
*/
public Vector3f transform(Vector3f v) {
return transform(v, v);
}
/**
* Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}
* and store the result in <code>dest</code>.
*
* @param v
* the vector to transform
* @param dest
* will hold the result
* @return dest
*/
public Vector3f transform(Vector3fc v, Vector3f dest) {
double sin = Math.sin(angle);
double cos = Math.cosFromSin(sin, angle);
double dot = x * v.x() + y * v.y() + z * v.z();
dest.set((float) (v.x() * cos + sin * (y * v.z() - z * v.y()) + (1.0 - cos) * dot * x),
(float) (v.y() * cos + sin * (z * v.x() - x * v.z()) + (1.0 - cos) * dot * y),
(float) (v.z() * cos + sin * (x * v.y() - y * v.x()) + (1.0 - cos) * dot * z));
return dest;
}
/**
* Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}.
*
* @param v
* the vector to transform
* @return v
*/
public Vector4d transform(Vector4d v) {
return transform(v, v);
}
/**
* Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}
* and store the result in <code>dest</code>.
*
* @param v
* the vector to transform
* @param dest
* will hold the result
* @return dest
*/
public Vector4d transform(Vector4dc v, Vector4d dest) {
double sin = Math.sin(angle);
double cos = Math.cosFromSin(sin, angle);
double dot = x * v.x() + y * v.y() + z * v.z();
dest.set(v.x() * cos + sin * (y * v.z() - z * v.y()) + (1.0 - cos) * dot * x,
v.y() * cos + sin * (z * v.x() - x * v.z()) + (1.0 - cos) * dot * y,
v.z() * cos + sin * (x * v.y() - y * v.x()) + (1.0 - cos) * dot * z,
dest.w);
return dest;
}
/**
* Return a string representation of this {@link AxisAngle4d}.
* <p>
* This method creates a new {@link DecimalFormat} on every invocation with the format string "<tt> 0.000E0;-</tt>".
*
* @return the string representation
*/
public String toString() {
DecimalFormat formatter = new DecimalFormat(" 0.000E0;-");
String str = toString(formatter);
StringBuffer res = new StringBuffer();
int eIndex = Integer.MIN_VALUE;
for (int i = 0; i < str.length(); i++) {
char c = str.charAt(i);
if (c == 'E') {
eIndex = i;
} else if (c == ' ' && eIndex == i - 1) {
// workaround Java 1.4 DecimalFormat bug
res.append('+');
continue;
} else if (Character.isDigit(c) && eIndex == i - 1) {
res.append('+');
}
res.append(c);
}
return res.toString();
}
/**
* Return a string representation of this {@link AxisAngle4d} by formatting the components with the given {@link NumberFormat}.
*
* @param formatter
* the {@link NumberFormat} used to format the vector components with
* @return the string representation
*/
public String toString(NumberFormat formatter) {
return "(" + formatter.format(x) + formatter.format(y) + formatter.format(z) + " <|" + formatter.format(angle) + " )"; //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
}
public int hashCode() {
final int prime = 31;
int result = 1;
long temp;
temp = Double.doubleToLongBits((angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI));
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(x);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(y);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(z);
result = prime * result + (int) (temp ^ (temp >>> 32));
return result;
}
public boolean equals(Object obj) {
if (this == obj)
return true;
if (obj == null)
return false;
if (getClass() != obj.getClass())
return false;
AxisAngle4d other = (AxisAngle4d) obj;
if (Double.doubleToLongBits((angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI)) !=
Double.doubleToLongBits((other.angle < 0.0 ? Math.PI + Math.PI + other.angle % (Math.PI + Math.PI) : other.angle) % (Math.PI + Math.PI)))
return false;
if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x))
return false;
if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y))
return false;
if (Double.doubleToLongBits(z) != Double.doubleToLongBits(other.z))
return false;
return true;
}
}