Java Code Examples for org.apache.commons.math3.exception.util.LocalizedFormats#ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR
The following examples show how to use
org.apache.commons.math3.exception.util.LocalizedFormats#ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR .
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example 1
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public FieldRotation(final FieldVector3D<T> u, final FieldVector3D<T> v) throws MathArithmeticException {
final T normProduct = u.getNorm().multiply(v.getNorm());
if (normProduct.getReal() == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
final T dot = FieldVector3D.dotProduct(u, v);
if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
final FieldVector3D<T> w = u.orthogonal();
q0 = normProduct.getField().getZero();
q1 = w.getX().negate();
q2 = w.getY().negate();
q3 = w.getZ().negate();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt();
final T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal();
final FieldVector3D<T> q = FieldVector3D.crossProduct(v, u);
q1 = coeff.multiply(q.getX());
q2 = coeff.multiply(q.getY());
q3 = coeff.multiply(q.getZ());
}
}
Example 2
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 3
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 4
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathIllegalArgumentException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 5
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public FieldRotation(final FieldVector3D<T> u, final FieldVector3D<T> v) throws MathArithmeticException {
final T normProduct = u.getNorm().multiply(v.getNorm());
if (normProduct.getReal() == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
final T dot = FieldVector3D.dotProduct(u, v);
if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
final FieldVector3D<T> w = u.orthogonal();
q0 = normProduct.getField().getZero();
q1 = w.getX().negate();
q2 = w.getY().negate();
q3 = w.getZ().negate();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt();
final T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal();
final FieldVector3D<T> q = FieldVector3D.crossProduct(v, u);
q1 = coeff.multiply(q.getX());
q2 = coeff.multiply(q.getY());
q3 = coeff.multiply(q.getZ());
}
}
Example 6
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 7
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathIllegalArgumentException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 8
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public FieldRotation(final FieldVector3D<T> u, final FieldVector3D<T> v) throws MathArithmeticException {
final T normProduct = u.getNorm().multiply(v.getNorm());
if (normProduct.getReal() == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
final T dot = FieldVector3D.dotProduct(u, v);
if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
final FieldVector3D<T> w = u.orthogonal();
q0 = normProduct.getField().getZero();
q1 = w.getX().negate();
q2 = w.getY().negate();
q3 = w.getZ().negate();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt();
final T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal();
final FieldVector3D<T> q = FieldVector3D.crossProduct(v, u);
q1 = coeff.multiply(q.getX());
q2 = coeff.multiply(q.getY());
q3 = coeff.multiply(q.getZ());
}
}
Example 9
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 10
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public FieldRotation(final FieldVector3D<T> u, final FieldVector3D<T> v) throws MathArithmeticException {
final T normProduct = u.getNorm().multiply(v.getNorm());
if (normProduct.getReal() == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
final T dot = FieldVector3D.dotProduct(u, v);
if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
final FieldVector3D<T> w = u.orthogonal();
q0 = normProduct.getField().getZero();
q1 = w.getX().negate();
q2 = w.getY().negate();
q3 = w.getZ().negate();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt();
final T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal();
final FieldVector3D<T> q = FieldVector3D.crossProduct(v, u);
q1 = coeff.multiply(q.getX());
q2 = coeff.multiply(q.getY());
q3 = coeff.multiply(q.getZ());
}
}
Example 11
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 12
| Source File: FieldRotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public FieldRotation(final FieldVector3D<T> u, final FieldVector3D<T> v) throws MathArithmeticException {
final T normProduct = u.getNorm().multiply(v.getNorm());
if (normProduct.getReal() == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
final T dot = FieldVector3D.dotProduct(u, v);
if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
final FieldVector3D<T> w = u.orthogonal();
q0 = normProduct.getField().getZero();
q1 = w.getX().negate();
q2 = w.getY().negate();
q3 = w.getZ().negate();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt();
final T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal();
final FieldVector3D<T> q = FieldVector3D.crossProduct(v, u);
q1 = coeff.multiply(q.getX());
q2 = coeff.multiply(q.getY());
q3 = coeff.multiply(q.getZ());
}
}
Example 13
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 5 votes |
|
/** Build one of the rotations that transform one vector into another one.
* <p>Except for a possible scale factor, if the instance were
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
* @param v desired image of u by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException {
double normProduct = u.getNorm() * v.getNorm();
if (normProduct == 0) {
throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
// an arbitrary vector orthogonal to u
Vector3D w = u.orthogonal();
q0 = 0.0;
q1 = -w.getX();
q2 = -w.getY();
q3 = -w.getZ();
} else {
// general case: (u, v) defines a plane, we select
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
Vector3D q = v.crossProduct(u);
q1 = coeff * q.getX();
q2 = coeff * q.getY();
q3 = coeff * q.getZ();
}
}
Example 14
| Source File: Rotation.java From astor with GNU General Public License v2.0 | 4 votes |
|
/** Build the rotation that transforms a pair of vector into another pair.
* <p>Except for possible scale factors, if the instance were applied to
* the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair
* (v<sub>1</sub>, v<sub>2</sub>).</p>
* <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is
* not the same as the angular separation between v<sub>1</sub> and
* v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than
* v<sub>2</sub>, the corrected vector will be in the (v<sub>1</sub>,
* v<sub>2</sub>) plane.</p>
* @param u1 first vector of the origin pair
* @param u2 second vector of the origin pair
* @param v1 desired image of u1 by the rotation
* @param v2 desired image of u2 by the rotation
* @exception MathIllegalArgumentException if the norm of one of the vectors is zero
*/
public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) {
// norms computation
double u1u1 = u1.getNormSq();
double u2u2 = u2.getNormSq();
double v1v1 = v1.getNormSq();
double v2v2 = v2.getNormSq();
if ((u1u1 == 0) || (u2u2 == 0) || (v1v1 == 0) || (v2v2 == 0)) {
throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
// normalize v1 in order to have (v1'|v1') = (u1|u1)
v1 = new Vector3D(FastMath.sqrt(u1u1 / v1v1), v1);
// adjust v2 in order to have (u1|u2) = (v1'|v2') and (v2'|v2') = (u2|u2)
double u1u2 = u1.dotProduct(u2);
double v1v2 = v1.dotProduct(v2);
double coeffU = u1u2 / u1u1;
double coeffV = v1v2 / u1u1;
double beta = FastMath.sqrt((u2u2 - u1u2 * coeffU) / (v2v2 - v1v2 * coeffV));
double alpha = coeffU - beta * coeffV;
v2 = new Vector3D(alpha, v1, beta, v2);
// preliminary computation
Vector3D uRef = u1;
Vector3D vRef = v1;
Vector3D v1Su1 = v1.subtract(u1);
Vector3D v2Su2 = v2.subtract(u2);
Vector3D k = v1Su1.crossProduct(v2Su2);
Vector3D u3 = u1.crossProduct(u2);
double c = k.dotProduct(u3);
final double inPlaneThreshold = 0.001;
if (c <= inPlaneThreshold * k.getNorm() * u3.getNorm()) {
// the (q1, q2, q3) vector is close to the (u1, u2) plane
// we try other vectors
Vector3D v3 = Vector3D.crossProduct(v1, v2);
Vector3D v3Su3 = v3.subtract(u3);
k = v1Su1.crossProduct(v3Su3);
Vector3D u2Prime = u1.crossProduct(u3);
c = k.dotProduct(u2Prime);
if (c <= inPlaneThreshold * k.getNorm() * u2Prime.getNorm()) {
// the (q1, q2, q3) vector is also close to the (u1, u3) plane,
// it is almost aligned with u1: we try (u2, u3) and (v2, v3)
k = v2Su2.crossProduct(v3Su3);
c = k.dotProduct(u2.crossProduct(u3));
if (c <= 0) {
// the (q1, q2, q3) vector is aligned with everything
// this is really the identity rotation
q0 = 1.0;
q1 = 0.0;
q2 = 0.0;
q3 = 0.0;
return;
}
// we will have to use u2 and v2 to compute the scalar part
uRef = u2;
vRef = v2;
}
}
// compute the vectorial part
c = FastMath.sqrt(c);
double inv = 1.0 / (c + c);
q1 = inv * k.getX();
q2 = inv * k.getY();
q3 = inv * k.getZ();
// compute the scalar part
k = new Vector3D(uRef.getY() * q3 - uRef.getZ() * q2,
uRef.getZ() * q1 - uRef.getX() * q3,
uRef.getX() * q2 - uRef.getY() * q1);
q0 = vRef.dotProduct(k) / (2 * k.getNormSq());
}
