Java Code Examples for javax.vecmath.Matrix3d#set()

The following examples show how to use javax.vecmath.Matrix3d#set() . 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: Model.java    From Robot-Overlord-App with GNU General Public License v2.0 6 votes vote down vote up 
/**
 * Rotate all the vertexes by a given amount
 * @param arg0 amount in degrees to rotate around X,Y, and then Z. 
 */
public void adjustRotation(Vector3d arg0) {
	// generate the pose matrix
	Matrix3d pose = new Matrix3d();
	Matrix3d rotX = new Matrix3d();
	Matrix3d rotY = new Matrix3d();
	Matrix3d rotZ = new Matrix3d();
	rotX.rotX((float)Math.toRadians(arg0.x));
	rotY.rotY((float)Math.toRadians(arg0.y));
	rotZ.rotZ((float)Math.toRadians(arg0.z));
	pose.set(rotX);
	pose.mul(rotY);
	pose.mul(rotZ);
	adjust.set(pose);
	isDirty=true;
}
Example 2
Source File: Prism.java    From biojava with GNU Lesser General Public License v2.1 6 votes vote down vote up 
@Override
public Matrix3d getViewMatrix(int index) {
	Matrix3d m = new Matrix3d();
	switch (index) {
	case 0:
		m.setIdentity(); // front
		break;
	case 1:
		m.rotX(Math.PI/2); // side edge-centered
		break;
	case 2:
		m.rotY(Math.PI/n); // side face-centered
		Matrix3d m1 = new Matrix3d();
		m1.rotX(Math.PI/2);
		m.mul(m1);
		break;
	case 3:
		m.set(flipX()); // back
		break;
	default:
		throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
	}
	return m;
}
Example 3
Source File: EarthVector.java    From geowave with Apache License 2.0 5 votes vote down vote up 
/**
 * Rotates this coordinate about the input vector through the input angle (radians - because we
 * usually use this internally)
 *
 * @param rotAxis The axis of rotation
 * @param angle The angle of rotation (in radians)
 */
public Vector3d rotate(final Vector3d rotAxis, final double angle) {
  final Vector3d thisVec = new Vector3d(ecfVector);
  final Vector3d axis = new Vector3d(rotAxis);
  axis.normalize();

  final Matrix3d trans = new Matrix3d();
  trans.set(new AxisAngle4d(axis, angle));

  trans.transform(thisVec);

  return thisVec;
}
Example 4
Source File: EarthVector.java    From geowave with Apache License 2.0 4 votes vote down vote up 
/**
 * Locate a coordinate at a specific distance (km), elevation angle (radians), and heading
 * (radians) from this one.
 */
public EarthVector findPoint(
    final double distanceKM,
    final double azimuth,
    final double elevAngle) {
  // convert distance to radians
  // final double distR = distanceKM / KMPerDegree() / DPR;
  final double lon = getLongitude();
  final double lat = getLatitude();
  // convert local enu to ecf to get east and north vectors
  // east vector
  final Vector3d eastVec = new Vector3d(1, 0, 0);
  final Vector3d northVec = new Vector3d(0, 1, 0);
  final double sinLon = Math.sin(lon);
  final double cosLon = Math.cos(lon);
  final double sinLat = Math.sin(lat);
  final double cosLat = Math.cos(lat);
  final Matrix3d enuToEcf = new Matrix3d();
  enuToEcf.m00 = -sinLon;
  enuToEcf.m01 = -(sinLat * cosLon);
  enuToEcf.m02 = cosLat * cosLon;
  enuToEcf.m10 = cosLon;
  enuToEcf.m11 = -(sinLat * sinLon);
  enuToEcf.m12 = cosLat * sinLon;
  enuToEcf.m20 = 0;
  enuToEcf.m21 = cosLat;
  enuToEcf.m22 = sinLat;
  enuToEcf.transform(eastVec);
  enuToEcf.transform(northVec);
  eastVec.normalize();
  northVec.normalize();
  northVec.scale(distanceKM);
  final Matrix3d elevTrans = new Matrix3d();
  elevTrans.set(new AxisAngle4d(eastVec, elevAngle));

  elevTrans.transform(northVec);
  final Matrix3d azTrans = new Matrix3d();
  final Vector3d unitEcf = new Vector3d(ecfVector);
  unitEcf.normalize();
  azTrans.set(new AxisAngle4d(unitEcf, azimuth));
  azTrans.transform(northVec);
  final Vector3d transformedEcf = new Vector3d();
  transformedEcf.add(ecfVector, northVec);
  final EarthVector transformedEv = new EarthVector(transformedEcf);
  return transformedEv;
}
Example 5
Source File: DynamicObject.java    From jMAVSim with BSD 3-Clause "New" or "Revised" License 4 votes vote down vote up 
@Override
public void update(long t) {
    if (lastTime >= 0) {
        double dt = (t - lastTime) / 1000.0;
        if (dt < 0.001) {
            dt = 0.001; // Limit min dt to 1ms
        }
        // Position
        Vector3d dPos = new Vector3d(velocity);
        dPos.scale(dt);
        position.add(dPos);
        // Velocity
        acceleration = getForce();
        acceleration.scale(1.0 / mass);
        acceleration.add(getWorld().getEnvironment().getG());
        if (position.z >= getWorld().getEnvironment().getGroundLevel(position) &&
                velocity.z + acceleration.z * dt >= 0.0) {
            // On ground
            acceleration.x = -velocity.x / dt;
            acceleration.y = -velocity.y / dt;
            acceleration.z = -velocity.z / dt;
            position.z = getWorld().getEnvironment().getGroundLevel(position);
            //rotationRate.set(0.0, 0.0, 0.0);
        }
        Vector3d dVel = new Vector3d(acceleration);
        dVel.scale(dt);
        velocity.add(dVel);
        // Rotation
        if (rotationRate.length() > 0.0) {
            Matrix3d r = new Matrix3d();
            Vector3d rotationAxis = new Vector3d(rotationRate);
            rotationAxis.normalize();
            r.set(new AxisAngle4d(rotationAxis, rotationRate.length() * dt));
            rotation.mulNormalize(r);
        }
        // Rotation rate
        Vector3d Iw = new Vector3d(rotationRate);
        momentOfInertia.transform(Iw);
        Vector3d angularAcc = new Vector3d();
        angularAcc.cross(rotationRate, Iw);
        angularAcc.negate();
        angularAcc.add(getTorque());
        momentOfInertiaInv.transform(angularAcc);
        angularAcc.scale(dt);
        rotationRate.add(angularAcc);
    }
    lastTime = t;
}