Java Code Examples for org.apache.commons.math3.geometry.euclidean.threed.Vector3D#dotProduct()

The following examples show how to use org.apache.commons.math3.geometry.euclidean.threed.Vector3D#dotProduct() . 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: Car.java    From log-synth with Apache License 2.0 6 votes vote down vote up 
Local(GeoPoint start, GeoPoint end, Random rand) {
    Vector3D dr = end.as3D().subtract(start.as3D());
    double distance = dr.getNorm();

    double step = Math.abs((rand.nextGaussian() + 2) / Constants.EARTH_RADIUS_KM);

    Vector3D east = start.east();
    double eastWest = dr.dotProduct(east);
    double p = eastWest / distance;
    if (rand.nextDouble() < Math.abs(p * p)) {

        // go east/west
        if (step > Math.abs(eastWest)) {
            // don't overshoot
            step = Math.abs(eastWest);
        }
        super.end = new GeoPoint(start.r.add(step * Math.signum(eastWest), east));
    } else {
        Vector3D north = start.north(east);
        double northSouth = dr.dotProduct(north);
        if (step > Math.abs(northSouth)) {
            step = Math.abs(northSouth);
        }
        super.end = new GeoPoint(start.r.add(step * Math.signum(northSouth), north));
    }
}
Example 2
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionQuaternion() {

    // Case : analytic test case

    final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
    final Quaternion qB = new Quaternion(6, 2, 1, -9);
    final Quaternion qResult = Quaternion.multiply(qA, qB);

    Assert.assertEquals(44, qResult.getQ0(), EPS);
    Assert.assertEquals(28, qResult.getQ1(), EPS);
    Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
    Assert.assertEquals(21.5, qResult.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^
    // vectorB)

    final Vector3D vectorA = new Vector3D(qA.getVectorPart());
    final Vector3D vectorB = new Vector3D(qB.getVectorPart());
    final Vector3D vectorResult = new Vector3D(qResult.getVectorPart());

    final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB);

    Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS);

    final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA
            .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB));
    final double norm = (vectorResult.subtract(vectorPartRef)).getNorm();

    Assert.assertEquals(0, norm, EPS);

    // Conjugate of the product of two quaternions and product of their conjugates :
    // Conj(qA * qB) = Conj(qB) * Conj(qA)

    final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
    final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();

    Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
}
Example 3
Source File: DriveTest.java    From log-synth with Apache License 2.0 4 votes vote down vote up 
@SuppressWarnings("WeakerAccess")
static Vector3D project(Vector3D east, Vector3D north, Vector3D step) {
    return new Vector3D(step.dotProduct(east) * Constants.EARTH_RADIUS_KM, step.dotProduct(north) * Constants.EARTH_RADIUS_KM, 0);
}
Example 4
Source File: Producer.java    From log-synth with Apache License 2.0 4 votes vote down vote up 
static Vector3D project(Vector3D east, Vector3D north, Vector3D step) {
    return new Vector3D(step.dotProduct(east) * Constants.EARTH_RADIUS_KM, step.dotProduct(north) * Constants.EARTH_RADIUS_KM, 0);
}
Example 5
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionVector() {

    // Case : Product between a vector and a quaternion : QxV

    final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
    final double[] vector = {2.0, 1.0, 3.0};
    final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));

    Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
    Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
    Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
    Assert.assertEquals(21, qResultQxV.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector)

    final double[] vectorQ = quaternion.getVectorPart();
    final double[] vectorResultQxV = qResultQxV.getVectorPart();

    final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);

    final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vectorQ), new Vector3D(vector)));
    final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm();
    Assert.assertEquals(0, normQxV, EPS);

    // Case : Product between a vector and a quaternion : VxQ

    final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);

    Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
    Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
    Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
    Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);

    final double[] vectorResultVxQ = qResultVxQ.getVectorPart();

    // comparison with the result given by the formula :
    // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ)

    final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);

    final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vector), new Vector3D(vectorQ)));
    final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm();
    Assert.assertEquals(0, normVxQ, EPS);
}
Example 6
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionQuaternion() {

    // Case : analytic test case

    final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
    final Quaternion qB = new Quaternion(6, 2, 1, -9);
    final Quaternion qResult = Quaternion.multiply(qA, qB);

    Assert.assertEquals(44, qResult.getQ0(), EPS);
    Assert.assertEquals(28, qResult.getQ1(), EPS);
    Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
    Assert.assertEquals(21.5, qResult.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^
    // vectorB)

    final Vector3D vectorA = new Vector3D(qA.getVectorPart());
    final Vector3D vectorB = new Vector3D(qB.getVectorPart());
    final Vector3D vectorResult = new Vector3D(qResult.getVectorPart());

    final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB);

    Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS);

    final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA
            .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB));
    final double norm = (vectorResult.subtract(vectorPartRef)).getNorm();

    Assert.assertEquals(0, norm, EPS);

    // Conjugate of the product of two quaternions and product of their conjugates :
    // Conj(qA * qB) = Conj(qB) * Conj(qA)

    final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
    final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();

    Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
}
Example 7
Source File: Circle.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
/** {@inheritDoc} */
public boolean sameOrientationAs(final Hyperplane<Sphere2D> other) {
    final Circle otherC = (Circle) other;
    return Vector3D.dotProduct(pole, otherC.pole) >= 0.0;
}
Example 8
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionVector() {

    // Case : Product between a vector and a quaternion : QxV

    final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
    final double[] vector = {2.0, 1.0, 3.0};
    final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));

    Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
    Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
    Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
    Assert.assertEquals(21, qResultQxV.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector)

    final double[] vectorQ = quaternion.getVectorPart();
    final double[] vectorResultQxV = qResultQxV.getVectorPart();

    final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);

    final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vectorQ), new Vector3D(vector)));
    final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm();
    Assert.assertEquals(0, normQxV, EPS);

    // Case : Product between a vector and a quaternion : VxQ

    final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);

    Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
    Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
    Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
    Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);

    final double[] vectorResultVxQ = qResultVxQ.getVectorPart();

    // comparison with the result given by the formula :
    // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ)

    final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);

    final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vector), new Vector3D(vectorQ)));
    final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm();
    Assert.assertEquals(0, normVxQ, EPS);
}
Example 9
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionQuaternion() {

    // Case : analytic test case

    final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
    final Quaternion qB = new Quaternion(6, 2, 1, -9);
    final Quaternion qResult = Quaternion.multiply(qA, qB);

    Assert.assertEquals(44, qResult.getQ0(), EPS);
    Assert.assertEquals(28, qResult.getQ1(), EPS);
    Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
    Assert.assertEquals(21.5, qResult.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^
    // vectorB)

    final Vector3D vectorA = new Vector3D(qA.getVectorPart());
    final Vector3D vectorB = new Vector3D(qB.getVectorPart());
    final Vector3D vectorResult = new Vector3D(qResult.getVectorPart());

    final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB);

    Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS);

    final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA
            .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB));
    final double norm = (vectorResult.subtract(vectorPartRef)).getNorm();

    Assert.assertEquals(0, norm, EPS);

    // Conjugate of the product of two quaternions and product of their conjugates :
    // Conj(qA * qB) = Conj(qB) * Conj(qA)

    final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
    final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();

    Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
}
Example 10
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionVector() {

    // Case : Product between a vector and a quaternion : QxV

    final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
    final double[] vector = {2.0, 1.0, 3.0};
    final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));

    Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
    Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
    Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
    Assert.assertEquals(21, qResultQxV.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector)

    final double[] vectorQ = quaternion.getVectorPart();
    final double[] vectorResultQxV = qResultQxV.getVectorPart();

    final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);

    final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vectorQ), new Vector3D(vector)));
    final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm();
    Assert.assertEquals(0, normQxV, EPS);

    // Case : Product between a vector and a quaternion : VxQ

    final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);

    Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
    Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
    Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
    Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);

    final double[] vectorResultVxQ = qResultVxQ.getVectorPart();

    // comparison with the result given by the formula :
    // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ)

    final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);

    final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vector), new Vector3D(vectorQ)));
    final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm();
    Assert.assertEquals(0, normVxQ, EPS);
}
Example 11
Source File: World.java    From notreal2d with MIT License 4 votes vote down vote up 
@SuppressWarnings("Duplicates")
private void resolveImpact(
        @Nonnull Body bodyA, @Nonnull Body bodyB, @Nonnull Vector3D collisionNormalB,
        @Nonnull Vector3D vectorAC, @Nonnull Vector3D vectorBC, @Nonnull Vector3D relativeVelocityC) {
    Double momentumTransferFactor;

    if (momentumTransferFactorProvider == null
            || (momentumTransferFactor = momentumTransferFactorProvider.getFactor(bodyA, bodyB)) == null) {
        momentumTransferFactor = bodyA.getMomentumTransferFactor() * bodyB.getMomentumTransferFactor();
    }

    Vector3D denominatorPartA = vectorAC.crossProduct(collisionNormalB)
            .scalarMultiply(bodyA.getInvertedAngularMass()).crossProduct(vectorAC);
    Vector3D denominatorPartB = vectorBC.crossProduct(collisionNormalB)
            .scalarMultiply(bodyB.getInvertedAngularMass()).crossProduct(vectorBC);

    double denominator = bodyA.getInvertedMass() + bodyB.getInvertedMass()
            + collisionNormalB.dotProduct(denominatorPartA.add(denominatorPartB));

    double impulseChange = -1.0D * (1.0D + momentumTransferFactor) * relativeVelocityC.dotProduct(collisionNormalB)
            / denominator;

    if (abs(impulseChange) < epsilon) {
        return;
    }

    if (!bodyA.isStatic()) {
        Vector3D velocityChangeA = collisionNormalB.scalarMultiply(impulseChange * bodyA.getInvertedMass());
        Vector3D newVelocityA = toVector3D(bodyA.getVelocity()).add(velocityChangeA);
        bodyA.setVelocity(newVelocityA.getX(), newVelocityA.getY());

        Vector3D angularVelocityChangeA = vectorAC.crossProduct(collisionNormalB.scalarMultiply(impulseChange))
                .scalarMultiply(bodyA.getInvertedAngularMass());
        Vector3D newAngularVelocityA = toVector3DZ(bodyA.getAngularVelocity()).add(angularVelocityChangeA);
        bodyA.setAngularVelocity(newAngularVelocityA.getZ());
    }

    if (!bodyB.isStatic()) {
        Vector3D velocityChangeB = collisionNormalB.scalarMultiply(impulseChange * bodyB.getInvertedMass());
        Vector3D newVelocityB = toVector3D(bodyB.getVelocity()).subtract(velocityChangeB);
        bodyB.setVelocity(newVelocityB.getX(), newVelocityB.getY());

        Vector3D angularVelocityChangeB = vectorBC.crossProduct(collisionNormalB.scalarMultiply(impulseChange))
                .scalarMultiply(bodyB.getInvertedAngularMass());
        Vector3D newAngularVelocityB = toVector3DZ(bodyB.getAngularVelocity()).subtract(angularVelocityChangeB);
        bodyB.setAngularVelocity(newAngularVelocityB.getZ());
    }
}
Example 12
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionVector() {

    // Case : Product between a vector and a quaternion : QxV

    final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
    final double[] vector = {2.0, 1.0, 3.0};
    final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));

    Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
    Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
    Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
    Assert.assertEquals(21, qResultQxV.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector)

    final double[] vectorQ = quaternion.getVectorPart();
    final double[] vectorResultQxV = qResultQxV.getVectorPart();

    final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);

    final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vectorQ), new Vector3D(vector)));
    final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm();
    Assert.assertEquals(0, normQxV, EPS);

    // Case : Product between a vector and a quaternion : VxQ

    final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);

    Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
    Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
    Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
    Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);

    final double[] vectorResultVxQ = qResultVxQ.getVectorPart();

    // comparison with the result given by the formula :
    // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ)

    final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);

    final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vector), new Vector3D(vectorQ)));
    final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm();
    Assert.assertEquals(0, normVxQ, EPS);
}
Example 13
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionQuaternion() {

    // Case : analytic test case

    final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
    final Quaternion qB = new Quaternion(6, 2, 1, -9);
    final Quaternion qResult = Quaternion.multiply(qA, qB);

    Assert.assertEquals(44, qResult.getQ0(), EPS);
    Assert.assertEquals(28, qResult.getQ1(), EPS);
    Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
    Assert.assertEquals(21.5, qResult.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^
    // vectorB)

    final Vector3D vectorA = new Vector3D(qA.getVectorPart());
    final Vector3D vectorB = new Vector3D(qB.getVectorPart());
    final Vector3D vectorResult = new Vector3D(qResult.getVectorPart());

    final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB);

    Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS);

    final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA
            .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB));
    final double norm = (vectorResult.subtract(vectorPartRef)).getNorm();

    Assert.assertEquals(0, norm, EPS);

    // Conjugate of the product of two quaternions and product of their conjugates :
    // Conj(qA * qB) = Conj(qB) * Conj(qA)

    final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
    final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();

    Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
}
Example 14
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionVector() {

    // Case : Product between a vector and a quaternion : QxV

    final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
    final double[] vector = {2.0, 1.0, 3.0};
    final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));

    Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
    Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
    Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
    Assert.assertEquals(21, qResultQxV.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector)

    final double[] vectorQ = quaternion.getVectorPart();
    final double[] vectorResultQxV = qResultQxV.getVectorPart();

    final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);

    final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vectorQ), new Vector3D(vector)));
    final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm();
    Assert.assertEquals(0, normQxV, EPS);

    // Case : Product between a vector and a quaternion : VxQ

    final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);

    Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
    Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
    Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
    Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);

    final double[] vectorResultVxQ = qResultVxQ.getVectorPart();

    // comparison with the result given by the formula :
    // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ)

    final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);

    final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vector), new Vector3D(vectorQ)));
    final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm();
    Assert.assertEquals(0, normVxQ, EPS);
}
Example 15
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionQuaternion() {

    // Case : analytic test case

    final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
    final Quaternion qB = new Quaternion(6, 2, 1, -9);
    final Quaternion qResult = Quaternion.multiply(qA, qB);

    Assert.assertEquals(44, qResult.getQ0(), EPS);
    Assert.assertEquals(28, qResult.getQ1(), EPS);
    Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
    Assert.assertEquals(21.5, qResult.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^
    // vectorB)

    final Vector3D vectorA = new Vector3D(qA.getVectorPart());
    final Vector3D vectorB = new Vector3D(qB.getVectorPart());
    final Vector3D vectorResult = new Vector3D(qResult.getVectorPart());

    final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB);

    Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS);

    final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA
            .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB));
    final double norm = (vectorResult.subtract(vectorPartRef)).getNorm();

    Assert.assertEquals(0, norm, EPS);

    // Conjugate of the product of two quaternions and product of their conjugates :
    // Conj(qA * qB) = Conj(qB) * Conj(qA)

    final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
    final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();

    Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
}
Example 16
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionVector() {

    // Case : Product between a vector and a quaternion : QxV

    final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
    final double[] vector = {2.0, 1.0, 3.0};
    final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));

    Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
    Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
    Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
    Assert.assertEquals(21, qResultQxV.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (- vectorQ . vector) + (scalarQ * vector + vectorQ ^ vector)

    final double[] vectorQ = quaternion.getVectorPart();
    final double[] vectorResultQxV = qResultQxV.getVectorPart();

    final double scalarPartRefQxV = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);

    final Vector3D vectorPartRefQxV = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vectorQ), new Vector3D(vector)));
    final double normQxV = (new Vector3D(vectorResultQxV).subtract(vectorPartRefQxV)).getNorm();
    Assert.assertEquals(0, normQxV, EPS);

    // Case : Product between a vector and a quaternion : VxQ

    final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);

    Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
    Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
    Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
    Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);

    final double[] vectorResultVxQ = qResultVxQ.getVectorPart();

    // comparison with the result given by the formula :
    // qResult = (- vector . vectorQ) + (scalarQ * vector + vector ^ vectorQ)

    final double scalarPartRefVxQ = -Vector3D.dotProduct(new Vector3D(vectorQ), new Vector3D(vector));
    Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);

    final Vector3D vectorPartRefVxQ = (new Vector3D(vector).scalarMultiply(quaternion.getScalarPart())).add(Vector3D
            .crossProduct(new Vector3D(vector), new Vector3D(vectorQ)));
    final double normVxQ = (new Vector3D(vectorResultVxQ).subtract(vectorPartRefVxQ)).getNorm();
    Assert.assertEquals(0, normVxQ, EPS);
}
Example 17
Source File: QuaternionTest.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
@Test
public final void testProductQuaternionQuaternion() {

    // Case : analytic test case

    final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
    final Quaternion qB = new Quaternion(6, 2, 1, -9);
    final Quaternion qResult = Quaternion.multiply(qA, qB);

    Assert.assertEquals(44, qResult.getQ0(), EPS);
    Assert.assertEquals(28, qResult.getQ1(), EPS);
    Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
    Assert.assertEquals(21.5, qResult.getQ3(), EPS);

    // comparison with the result given by the formula :
    // qResult = (scalarA * scalarB - vectorA . vectorB) + (scalarA * vectorB + scalarB * vectorA + vectorA ^
    // vectorB)

    final Vector3D vectorA = new Vector3D(qA.getVectorPart());
    final Vector3D vectorB = new Vector3D(qB.getVectorPart());
    final Vector3D vectorResult = new Vector3D(qResult.getVectorPart());

    final double scalarPartRef = qA.getScalarPart() * qB.getScalarPart() - Vector3D.dotProduct(vectorA, vectorB);

    Assert.assertEquals(scalarPartRef, qResult.getScalarPart(), EPS);

    final Vector3D vectorPartRef = ((vectorA.scalarMultiply(qB.getScalarPart())).add(vectorB.scalarMultiply(qA
            .getScalarPart()))).add(Vector3D.crossProduct(vectorA, vectorB));
    final double norm = (vectorResult.subtract(vectorPartRef)).getNorm();

    Assert.assertEquals(0, norm, EPS);

    // Conjugate of the product of two quaternions and product of their conjugates :
    // Conj(qA * qB) = Conj(qB) * Conj(qA)

    final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
    final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();

    Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
    Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
}
Example 18
Source File: Circle.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
/** {@inheritDoc} */
public boolean sameOrientationAs(final Hyperplane<Sphere2D> other) {
    final Circle otherC = (Circle) other;
    return Vector3D.dotProduct(pole, otherC.pole) >= 0.0;
}
Example 19
Source File: World.java    From notreal2d with MIT License 4 votes vote down vote up 
@SuppressWarnings("Duplicates")
private void resolveSurfaceFriction(
        @Nonnull Body bodyA, @Nonnull Body bodyB, @Nonnull Vector3D collisionNormalB,
        @Nonnull Vector3D vectorAC, @Nonnull Vector3D vectorBC, @Nonnull Vector3D relativeVelocityC) {
    Vector3D tangent = relativeVelocityC
            .subtract(collisionNormalB.scalarMultiply(relativeVelocityC.dotProduct(collisionNormalB)));

    if (tangent.getNormSq() < squaredEpsilon) {
        return;
    }

    tangent = tangent.normalize();

    double surfaceFriction = sqrt(bodyA.getSurfaceFrictionFactor() * bodyB.getSurfaceFrictionFactor())
            * SQRT_2 * abs(relativeVelocityC.dotProduct(collisionNormalB)) / relativeVelocityC.getNorm();

    if (surfaceFriction < epsilon) {
        return;
    }

    Vector3D denominatorPartA = vectorAC.crossProduct(tangent)
            .scalarMultiply(bodyA.getInvertedAngularMass()).crossProduct(vectorAC);
    Vector3D denominatorPartB = vectorBC.crossProduct(tangent)
            .scalarMultiply(bodyB.getInvertedAngularMass()).crossProduct(vectorBC);

    double denominator = bodyA.getInvertedMass() + bodyB.getInvertedMass()
            + tangent.dotProduct(denominatorPartA.add(denominatorPartB));

    double impulseChange = -1.0D * surfaceFriction * relativeVelocityC.dotProduct(tangent)
            / denominator;

    if (abs(impulseChange) < epsilon) {
        return;
    }

    if (!bodyA.isStatic()) {
        Vector3D velocityChangeA = tangent.scalarMultiply(impulseChange * bodyA.getInvertedMass());
        Vector3D newVelocityA = toVector3D(bodyA.getVelocity()).add(velocityChangeA);
        bodyA.setVelocity(newVelocityA.getX(), newVelocityA.getY());

        Vector3D angularVelocityChangeA = vectorAC.crossProduct(tangent.scalarMultiply(impulseChange))
                .scalarMultiply(bodyA.getInvertedAngularMass());
        Vector3D newAngularVelocityA = toVector3DZ(bodyA.getAngularVelocity()).add(angularVelocityChangeA);
        bodyA.setAngularVelocity(newAngularVelocityA.getZ());
    }

    if (!bodyB.isStatic()) {
        Vector3D velocityChangeB = tangent.scalarMultiply(impulseChange * bodyB.getInvertedMass());
        Vector3D newVelocityB = toVector3D(bodyB.getVelocity()).subtract(velocityChangeB);
        bodyB.setVelocity(newVelocityB.getX(), newVelocityB.getY());

        Vector3D angularVelocityChangeB = vectorBC.crossProduct(tangent.scalarMultiply(impulseChange))
                .scalarMultiply(bodyB.getInvertedAngularMass());
        Vector3D newAngularVelocityB = toVector3DZ(bodyB.getAngularVelocity()).subtract(angularVelocityChangeB);
        bodyB.setAngularVelocity(newAngularVelocityB.getZ());
    }
}