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

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));
    }
}
 

@Override
public void compute(final Mesh input, final DoubleType output) {
	double total = 0;
	for (final Triangle tri : input.triangles()) {
		final Vector3D v0 = new Vector3D(tri.v0x(), tri.v0y(), tri.v0z());
		final Vector3D v1 = new Vector3D(tri.v1x(), tri.v1y(), tri.v1z());
		final Vector3D v2 = new Vector3D(tri.v2x(), tri.v2y(), tri.v2z());

		final Vector3D cross = v0.subtract(v1).crossProduct(v2.subtract(v0));
		final double norm = cross.getNorm();
		if (norm > 0) total += norm * 0.5;
	}
	output.set(total);
}
 

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;
}
 

@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());
    }
}
 

/**
 * Compute the area of this facet.
 */
private void computeArea() {
	Vector3D cross = vertices.get(0).subtract(vertices.get(1))
			.crossProduct(vertices.get(2).subtract(vertices.get(0)));
	area = cross.getNorm() * 0.5;
}