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

The following examples show how to use org.apache.commons.math3.geometry.euclidean.threed.Vector3D#normalize() . 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: GeoPoint.java    From log-synth with Apache License 2.0 5 votes vote down vote up 
public Vector3D east(Vector3D r) {
    Vector3D ux = r.crossProduct(Z);
    if (ux.getNorm() < 1e-4) {
        // near the poles (i.e. < 640 meters from them), the definition of east is difficult
        ux = this.r.crossProduct(X);
    }
    ux = ux.normalize();
    return ux;
}
Example 2
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());
    }
}
Example 3
Source File: Circle.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
/** Reset the instance as if built from a pole.
 * <p>The circle is oriented in the trigonometric direction around pole.</p>
 * @param newPole circle pole
 */
public void reset(final Vector3D newPole) {
    this.pole = newPole.normalize();
    this.x    = newPole.orthogonal();
    this.y    = Vector3D.crossProduct(newPole, x).normalize();
}
Example 4
Source File: Circle.java    From astor with GNU General Public License v2.0 4 votes vote down vote up 
/** Reset the instance as if built from a pole.
 * <p>The circle is oriented in the trigonometric direction around pole.</p>
 * @param newPole circle pole
 */
public void reset(final Vector3D newPole) {
    this.pole = newPole.normalize();
    this.x    = newPole.orthogonal();
    this.y    = Vector3D.crossProduct(newPole, x).normalize();
}
Example 5
Source File: GeoPoint.java    From log-synth with Apache License 2.0 4 votes vote down vote up 
@SuppressWarnings("WeakerAccess")
public GeoPoint(Vector3D r) {
    this.r = r.normalize();
}
Example 6
Source File: GeoPoint.java    From log-synth with Apache License 2.0 4 votes vote down vote up 
public void setPosition(Vector3D position) {
    this.r = position.normalize();
}
Example 7
Source File: S2Point.java    From astor with GNU General Public License v2.0 2 votes vote down vote up 
/** Simple constructor.
 * Build a vector from its underlying 3D vector
 * @param vector 3D vector
 * @exception MathArithmeticException if vector norm is zero
 */
public S2Point(final Vector3D vector) throws MathArithmeticException {
    this(FastMath.atan2(vector.getY(), vector.getX()), Vector3D.angle(Vector3D.PLUS_K, vector),
         vector.normalize());
}
Example 8
Source File: S2Point.java    From astor with GNU General Public License v2.0 2 votes vote down vote up 
/** Simple constructor.
 * Build a vector from its underlying 3D vector
 * @param vector 3D vector
 * @exception MathArithmeticException if vector norm is zero
 */
public S2Point(final Vector3D vector) throws MathArithmeticException {
    this(FastMath.atan2(vector.getY(), vector.getX()), Vector3D.angle(Vector3D.PLUS_K, vector),
         vector.normalize());
}