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

/**
 * Returns the vertices of an n-fold polygon of given radius and center
 * @param n
 * @param radius
 * @param center
 * @return
 */
@Override
public  Point3d[] getVertices() {
	double x = getSideLengthFromCircumscribedRadius(circumscribedRadius)/2;
	double z = x/Math.sqrt(2);
	Point3d[] tetrahedron = new Point3d[4];
	tetrahedron[0] = new Point3d(-x,  0, -z);
	tetrahedron[1] = new Point3d( x,  0, -z);
	tetrahedron[2] = new Point3d( 0, -x,  z);
	tetrahedron[3] = new Point3d( 0,  x,  z);
	Point3d centroid = CalcPoint.centroid(tetrahedron);

	// rotate tetrahedron to align one vertex with the +z axis
	Matrix3d m = new Matrix3d();
	m.rotX(0.5 * TETRAHEDRAL_ANGLE);
	for (Point3d p: tetrahedron) {
		p.sub(centroid);
		m.transform(p);
	}
	return tetrahedron;
}
 

/**
 * Returns the vertices of an n-fold polygon of given radius and center
 * @return
 */
@Override
public Point3d[] getVertices() {
	Point3d[] polygon = new Point3d[2*n];
	Matrix3d m = new Matrix3d();

	Point3d center = new Point3d(0, 0, height/2);

	for (int i = 0; i < n; i++) {
		polygon[i] = new Point3d(0, circumscribedRadius, 0);
		m.rotZ(i*2*Math.PI/n);
		m.transform(polygon[i]);
		polygon[n+i] = new  Point3d(polygon[i]);
		polygon[i].sub(center);
		polygon[n+i].add(center);
	}

	return polygon;
}
 

/**
 * Generic quadcopter constructor.
 *
 * @param world          world where to place the vehicle
 * @param modelName      filename of model to load, in .obj format
 * @param orientation    "x" or "+"
 * @param armLength      length of arm from center
 * @param rotorThrust    full thrust of one rotor
 * @param rotorTorque    torque at full thrust of one rotor
 * @param rotorTimeConst spin-up time of rotor
 * @param rotorsOffset   rotors positions offset from gravity center
 * @throws FileNotFoundException if .obj model file not found
 */
public Quadcopter(World world, String modelName, String orientation, double armLength, double rotorThrust,
                  double rotorTorque, double rotorTimeConst, Vector3d rotorsOffset) throws FileNotFoundException {
    super(world, modelName);
    rotorPositions[0] = new Vector3d(0.0, armLength, 0.0);
    rotorPositions[1] = new Vector3d(0.0, -armLength, 0.0);
    rotorPositions[2] = new Vector3d(armLength, 0.0, 0.0);
    rotorPositions[3] = new Vector3d(-armLength, 0.0, 0.0);
    if (orientation.equals("x") || orientation.equals("X")) {
        Matrix3d r = new Matrix3d();
        r.rotZ(-Math.PI / 4);
        for (int i = 0; i < rotorsNum; i++) {
            r.transform(rotorPositions[i]);
        }
    } else if (orientation.equals("+")) {
    } else {
        throw new RuntimeException("Unknown orientation: " + orientation);
    }
    for (int i = 0; i < rotors.length; i++) {
        rotorPositions[i].add(rotorsOffset);
        Rotor rotor = rotors[i];
        rotor.setFullThrust(rotorThrust);
        rotor.setFullTorque(i < 2 ? -rotorTorque : rotorTorque);
        rotor.setTimeConstant(rotorTimeConst);
    }
}
 

/**
 * Calculate the angles and call them back to the app
 * @param inFunction function to call for export
 */
private void callbackRender(Export3dFunction inFunction)
{
	Transform3D trans3d = new Transform3D();
	_orbit.getViewingPlatform().getViewPlatformTransform().getTransform(trans3d);
	Matrix3d matrix = new Matrix3d();
	trans3d.get(matrix);
	Point3d point = new Point3d(0.0, 0.0, 1.0);
	matrix.transform(point);
	// Set up initial rotations
	Transform3D firstTran = new Transform3D();
	firstTran.rotY(Math.toRadians(-INITIAL_Y_ROTATION));
	Transform3D secondTran = new Transform3D();
	secondTran.rotX(Math.toRadians(-INITIAL_X_ROTATION));
	// Apply inverse rotations in reverse order to the test point
	Point3d result = new Point3d();
	secondTran.transform(point, result);
	firstTran.transform(result);

	// Give the settings to the rendering function
	inFunction.setCameraCoordinates(result.x, result.y, result.z);
	inFunction.setAltitudeExaggeration(_altFactor);
	inFunction.setTerrainDefinition(_terrainDefinition);
	inFunction.setImageDefinition(_imageDefinition);

	inFunction.begin();
}
 

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

@Override
public Point3d[] getVertices() {
	Point3d[] icosahedron = new Point3d[12];
	// see http://answers.yahoo.com/question/index?qid=20080108041441AAJCjEu
	double c = circumscribedRadius * 1 / Math.sqrt(5);
	double s = 2 * c; // golden ratio
	double c1 = Math.sqrt((3-Math.sqrt(5))/8); // cos(2Pi/5)
	double s1 = Math.sqrt((5+Math.sqrt(5))/8); // sin(2Pi/5)
	double c2 = Math.sqrt((3+Math.sqrt(5))/8); // cos(Pi/5)
	double s2 = Math.sqrt((5-Math.sqrt(5))/8); // sin(Pi/5)

	icosahedron[0] = new Point3d(0, 0, circumscribedRadius);
	icosahedron[1] = new Point3d(s, 0, c);
	icosahedron[2] = new Point3d(s*c1, s*s1, c);
	icosahedron[3] = new Point3d(-s*c2, s*s2, c);
	icosahedron[4] = new Point3d(-s*c2, -s*s2, c);
	icosahedron[5] = new Point3d(s*c1, -s*s1, c);
	for (int i = 0; i < 6; i++) {
		icosahedron[i+6] = new Point3d(icosahedron[i]);
		icosahedron[i+6].negate();
	}

	Matrix3d m = new Matrix3d();
	m.rotZ(Math.PI/10);
	for (Point3d p: icosahedron) {
		m.transform(p);
	}

	return icosahedron;
}
 

/**
 * Returns the vertices of an n-fold polygon of given radius and center
 * @return
 */
public static Point3d[] getPolygonVertices(int n, double radius, Point3d center) {
	Point3d[] polygon = new Point3d[n];
	Matrix3d m = new Matrix3d();

	for (int i = 0; i < n; i++) {
		polygon[i] = new Point3d(0, radius, 0);
		m.rotZ(i*2*Math.PI/n);
		m.transform(polygon[i]);
		polygon[i].add(center);
	}
	return polygon;
}
 

/**
 * Generic hexacopter constructor.
 *
 * @param world          world where to place the vehicle
 * @param modelName      filename of model to load, in .obj format
 * @param orientation    "x" or "+"
 * @param armLength      length of arm from center
 * @param rotorThrust    full thrust of one rotor
 * @param rotorTorque    torque at full thrust of one rotor
 * @param rotorTimeConst spin-up time of rotor
 * @param rotorsOffset   rotors positions offset from gravity center
 * @throws FileNotFoundException if .obj model file not found
 */
public Hexacopter(World world, String modelName, String orientation, double armLength, double rotorThrust,
                  double rotorTorque, double rotorTimeConst, Vector3d rotorsOffset) throws FileNotFoundException {
    super(world, modelName);
    rotorPositions[0] = new Vector3d(armLength, 0.0, 0.0);
    rotorPositions[1] = new Vector3d(-armLength, 0.0, 0.0);
    rotorPositions[2] = new Vector3d(-armLength * Math.cos(Math.PI / 3), -armLength * Math.sin(Math.PI / 3), 0.0);
    rotorPositions[3] = new Vector3d(armLength * Math.cos(Math.PI / 3), armLength * Math.sin(Math.PI / 3), 0.0);
    rotorPositions[4] = new Vector3d(armLength * Math.cos(Math.PI / 3), -armLength * Math.sin(Math.PI / 3), 0.0);
    rotorPositions[5] = new Vector3d(-armLength * Math.cos(Math.PI / 3), armLength * Math.sin(Math.PI / 3), 0.0);
    if (orientation.equals("x") || orientation.equals("X")) {
        Matrix3d r = new Matrix3d();
        r.rotZ(Math.PI / 2);
        for (int i = 0; i < rotorsNum; i++) {
            r.transform(rotorPositions[i]);
        }
    } else if (orientation.equals("+")) {
    } else {
        throw new RuntimeException("Unknown orientation: " + orientation);
    }
    for (int i = 0; i < rotors.length; i++) {
        rotorPositions[i].add(rotorsOffset);
        Rotor rotor = rotors[i];
        rotor.setFullThrust(rotorThrust);
        rotor.setFullTorque((i == 1 || i == 3 || i == 4) ? -rotorTorque : rotorTorque);
        rotor.setTimeConstant(rotorTimeConst);
    }
}
 

@Override
public Vector3d getAcc() {
    Vector3d accBody = new Vector3d(object.getAcceleration());
    accBody.sub(object.getWorld().getEnvironment().getG());
    Matrix3d rot = new Matrix3d(object.getRotation());
    rot.transpose();
    rot.transform(accBody);
    return accBody;
}
 

@Override
public Vector3d getMag() {
    Vector3d mag = new Vector3d(object.getWorld().getEnvironment().getMagField(object.getPosition()));
    Matrix3d rot = new Matrix3d(object.getRotation());
    rot.transpose();
    rot.transform(mag);
    return mag;
}
 

/**
 * Regenerate the optimized rendering buffers for the fixed function pipeline.
 * Also recalculate the bounding box.
 * @param gl2
 */
private void updateBuffers(GL2 gl2) {
	int numVertexes = vertexArray.size()/3;
	Iterator<Float> fi;
	int j=0;

	Point3d boundBottom = new Point3d(Double.MAX_VALUE,Double.MAX_VALUE,Double.MAX_VALUE);
	Point3d boundTop = new Point3d(-Double.MAX_VALUE,-Double.MAX_VALUE,-Double.MAX_VALUE);

	FloatBuffer vertices = FloatBuffer.allocate(vertexArray.size());
	fi = vertexArray.iterator();
	Point3d p = new Point3d();
	while(fi.hasNext()) {
		p.x = fi.next().floatValue();
		p.y = fi.next().floatValue();
		p.z = fi.next().floatValue();
		adjust.transform(p);
		vertices.put(j++, (float)p.x);
		vertices.put(j++, (float)p.y);
		vertices.put(j++, (float)p.z);
		
		// also recalculate the bounding limits			
		if(boundBottom.x>p.x) boundBottom.x=p.x;
		if(boundBottom.y>p.y) boundBottom.y=p.y;
		if(boundBottom.z>p.z) boundBottom.z=p.z;
		if(boundTop.x<p.x) boundTop.x=p.x;
		if(boundTop.y<p.y) boundTop.y=p.y;
		if(boundTop.z<p.z) boundTop.z=p.z;
	}
	
	cuboid.setBounds(boundTop, boundBottom);

	int s=(Float.SIZE/8);  // bits per float / bits per byte = bytes per float
	int totalBufferSize = numVertexes*3*s;
	int vboIndex=0;
	
	// bind a buffer
	vertices.rewind();
	gl2.glBindBuffer(GL2.GL_ARRAY_BUFFER, VBO[vboIndex]);
    // Write out vertex buffer to the currently bound VBO.
    gl2.glBufferData(GL2.GL_ARRAY_BUFFER, totalBufferSize, vertices, GL2.GL_STATIC_DRAW);
    vboIndex++;
    
	if(hasNormals) {
		j=0;
	    // repeat for normals
		Matrix3d pose = new Matrix3d();
		adjust.get(pose);
		FloatBuffer normals = FloatBuffer.allocate(normalArray.size());
		fi = normalArray.iterator();
		while(fi.hasNext()) {
			p.x = fi.next().floatValue();
			p.y = fi.next().floatValue();
			p.z = fi.next().floatValue();
			pose.transform(p);
			normals.put(j++, (float)p.x);
			normals.put(j++, (float)p.y);
			normals.put(j++, (float)p.z);
		}
		
		normals.rewind();
		gl2.glBindBuffer(GL2.GL_ARRAY_BUFFER, VBO[vboIndex]);
	    gl2.glBufferData(GL2.GL_ARRAY_BUFFER, totalBufferSize, normals, GL2.GL_STATIC_DRAW);
	    vboIndex++;
	}

	if(hasColors) {
	    // repeat for colors
		FloatBuffer colors = FloatBuffer.allocate(colorArray.size());
		fi = colorArray.iterator();
		while(fi.hasNext()) {
			colors.put(fi.next().floatValue());
		}
		
		colors.rewind();
		gl2.glBindBuffer(GL2.GL_ARRAY_BUFFER, VBO[vboIndex]);
	    gl2.glBufferData(GL2.GL_ARRAY_BUFFER, totalBufferSize, colors, GL2.GL_STATIC_DRAW);
	    vboIndex++;
	}
	
	if(hasUVs) {
	    // repeat for textures
		FloatBuffer texCoords = FloatBuffer.allocate(texCoordArray.size());
		fi = texCoordArray.iterator();
		while(fi.hasNext()) {
			texCoords.put(fi.next().floatValue());
		}
		
	    texCoords.rewind();
		gl2.glBindBuffer(GL2.GL_ARRAY_BUFFER, VBO[vboIndex]);
	    gl2.glBufferData(GL2.GL_ARRAY_BUFFER, numVertexes*2*s, texCoords, GL2.GL_STATIC_DRAW);
	    vboIndex++;
	}
}
 

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