Java Code Examples for org.lwjgl.util.vector.Matrix4f#transform()

public void drawProtractorGL33(Composite3D c3d, GLShader shader, BigDecimal x1c, BigDecimal y1c, BigDecimal z1c, BigDecimal x2c, BigDecimal y2c, BigDecimal z2c, BigDecimal x3c, BigDecimal y3c, BigDecimal z3c) {
    GL20.glUniform3f(shader.getUniformLocation("color"), r, g, b); //$NON-NLS-1$

    final java.text.DecimalFormat NUMBER_FORMAT2F = new java.text.DecimalFormat(View.NUMBER_FORMAT2F, new DecimalFormatSymbols(MyLanguage.LOCALE));
    final float zoom = 1f / c3d.getZoom();

    final Vector4f textOrigin = new Vector4f(x1, y1, z1, 1f);
    Matrix4f.transform(c3d.getRotation(), textOrigin, textOrigin);

    Vector3d va = new Vector3d(x1c, y1c, z1c);
    Vector3d vb = new Vector3d(x2c, y2c, z2c);
    Vector3d vc = new Vector3d(x3c, y3c, z3c);
    vb = Vector3d.sub(va, vb);
    vc = Vector3d.sub(va, vc);
    double angle = Vector3d.angle(vb, vc);
    BigDecimal ang = new BigDecimal(angle);
    String angle_s = NUMBER_FORMAT2F.format(ang) + "°"; //$NON-NLS-1$

    drawNumberGL33(angle_s, textOrigin.x, textOrigin.y, textOrigin.z, zoom);
}
 

public void pushTo(SubMesh destMesh, final float xOffset, final float yOffset, final float zOffset, Rotation horizRotation, final float horizAngleDeg, Rotation vertRotation, final float vertAngleDeg)
{
	Matrix4f transform = createTransform(horizRotation, horizAngleDeg, vertRotation, vertAngleDeg);
	
	for (int i=0; i<positions.size(); i++)
	{
		Vector3f pos = new Vector3f( positions.get(i) );
		
		if (transform != null)
		{
			Vector4f dest = new Vector4f();
			Matrix4f.transform(transform, new Vector4f(pos.x, pos.y, pos.z, 1.0f), dest);
			
			pos.x = dest.x / dest.w;
			pos.y = dest.y / dest.w;
			pos.z = dest.z / dest.w;
		}
		
		destMesh.positions.add( new Vector3f(positions.get(i) ));
		destMesh.colours.add( new Vector4f(colours.get(i) ));
		destMesh.texCoords.add( new Vector2f(texCoords.get(i) ));
	}
}
 

private GData3 checkNormal(GData3 g3, Matrix4f vport) {
    Vertex[] v = triangles.get(g3);

    Vector4f n = new Vector4f();
    n.setW(1f);
    n.setX((v[2].y - v[0].y) * (v[1].z - v[0].z) - (v[2].z - v[0].z) * (v[1].y - v[0].y));
    n.setY((v[2].z - v[0].z) * (v[1].x - v[0].x) - (v[2].x - v[0].x) * (v[1].z - v[0].z));
    n.setZ((v[2].x - v[0].x) * (v[1].y - v[0].y) - (v[2].y - v[0].y) * (v[1].x - v[0].x));
    Matrix4f.transform(vport, n, n);
    Vector4f.sub(n, new Vector4f(vport.m03, vport.m13, vport.m23, 0f), n);
    if (n.z > 0f ^ Editor3DWindow.getWindow().hasBfcToggle()) {
        return new GData3(g3.colourNumber, g3.r, g3.g, g3.b, g3.a, v[0], v[2], v[1], View.DUMMY_REFERENCE, linkedDatFile, g3.isTriangle);
    } else {
        return null;
    }

}
 

private void readPositions() {
	String positionsId = meshData.getChild("vertices").getChild("input").getAttribute("source").substring(1);
	XmlNode positionsData = meshData.getChildWithAttribute("source", "id", positionsId).getChild("float_array");
	int count = Integer.parseInt(positionsData.getAttribute("count"));
	String[] posData = positionsData.getData().split(" ");
	for (int i = 0; i < count/3; i++) {
		float x = Float.parseFloat(posData[i * 3]);
		float y = Float.parseFloat(posData[i * 3 + 1]);
		float z = Float.parseFloat(posData[i * 3 + 2]);
		Vector4f position = new Vector4f(x, y, z, 1);
		Matrix4f.transform(CORRECTION, position, position);
		vertices.add(new Vertex(vertices.size(), new Vector3f(position.x, position.y, position.z), vertexWeights.get(vertices.size())));
	}
}
 

public void translateViewport(float dx, float dy, Matrix4f viewport_translation, Matrix4f viewport_rotation,
        PerspectiveCalculator perspective) {
    Vector4f xAxis4f_translation = new Vector4f(dx, 0, 0, 1.0f);
    Vector4f yAxis4f_translation = new Vector4f(0, dy, 0, 1.0f);
    Matrix4f ovr_inverse2 = Matrix4f.invert(viewport_rotation, null);
    Matrix4f.transform(ovr_inverse2, xAxis4f_translation, xAxis4f_translation);
    Matrix4f.transform(ovr_inverse2, yAxis4f_translation, yAxis4f_translation);
    Vector3f xAxis3 = new Vector3f(xAxis4f_translation.x, xAxis4f_translation.y, xAxis4f_translation.z);
    Vector3f yAxis3 = new Vector3f(yAxis4f_translation.x, yAxis4f_translation.y, yAxis4f_translation.z);
    Matrix4f.load(old_viewport_translation, viewport_translation);
    Matrix4f.translate(xAxis3, old_viewport_translation, viewport_translation);
    Matrix4f.translate(yAxis3, viewport_translation, viewport_translation);
    perspective.calculateOriginData();
}
 

public Particle(float angle) {
	Matrix4f rot_matrix = new Matrix4f();
	Vector3f axis = new Vector3f();
	Vector4f uv_vector = new Vector4f();
	Vector4f transform_uv_vector = new Vector4f();
	
	rot_matrix.setIdentity();
	axis.set(0f, 0f, 1f);
	rot_matrix.rotate(angle, axis);

	uv_vector.set(-.5f, -.5f, 0f, 0f);
	Matrix4f.transform(rot_matrix, uv_vector, transform_uv_vector);
	u1 = transform_uv_vector.getX() + .5f;
	v1 = transform_uv_vector.getY() + .5f;
	
	uv_vector.set(.5f, -.5f, 0f, 0f);
	Matrix4f.transform(rot_matrix, uv_vector, transform_uv_vector);
	u2 = transform_uv_vector.getX() + .5f;
	v2 = transform_uv_vector.getY() + .5f;

	uv_vector.set(.5f, .5f, 0f, 0f);
	Matrix4f.transform(rot_matrix, uv_vector, transform_uv_vector);
	u3 = transform_uv_vector.getX() + .5f;
	v3 = transform_uv_vector.getY() + .5f;

	uv_vector.set(-.5f, .5f, 0f, 0f);
	Matrix4f.transform(rot_matrix, uv_vector, transform_uv_vector);
	u4 = transform_uv_vector.getX() + .5f;
	v4 = transform_uv_vector.getY() + .5f;
}
 

/**
 * Calculates the origin axis coordinates for the actual viewport
 * perspective
 */
private void calculateOrigin(Matrix4f realViewport) {
    Rectangle bounds = c3d.getBounds();
    z_eff = (float) ((c3d.getzFar() + c3d.getzNear()) / -2.0 * c3d.getZoom());
    width = (float) bounds.width / (float) bounds.height;
    Vector3f[] axes = c3d.getViewportOriginAxis();
    offset.set(0, 0, 0, 1f);
    Matrix4f.transform(realViewport, offset, offset);
    axes[0].set(-width, offset.y, z_eff);
    axes[1].set(width, offset.y, z_eff);
    axes[2].set(offset.x, -1f, z_eff);
    axes[3].set(offset.x, 1f, z_eff);
}
 

private void a(Vector3f var, Vector3f var1, Matrix4f var2, Vector3f var3) {
	Vector4f var4 = new Vector4f(var.x - var1.x, var.y - var1.y, var.z - var1.z, 1.0F);
	Matrix4f.transform(var2, var4, var4);
	var4.x *= var3.x;
	var4.y *= var3.y;
	var4.z *= var3.z;
	var.set(var4.x + var1.x, var4.y + var1.y, var4.z + var1.z);
}
 

public void pushTo(Mesh mesh, final float xOffset, final float yOffset, final float zOffset, Rotation horizRotation, final float horizAngleDeg, Rotation vertRotation, final float vertAngleDeg)
{
	Matrix4f transform = createTransform(horizRotation, horizAngleDeg, vertRotation, vertAngleDeg);
	
	for (int i=0; i<positions.size(); i++)
	{
		Vector3f pos = new Vector3f( positions.get(i) );
		Vector2f tex = texCoords.get(i);
		Vector4f col = colours.get(i);
		
		if (transform != null)
		{
			Vector4f dest = new Vector4f();
			Matrix4f.transform(transform, new Vector4f(pos.x, pos.y, pos.z, 1.0f), dest);
			
			pos.x = dest.x / dest.w;
			pos.y = dest.y / dest.w;
			pos.z = dest.z / dest.w;
		}
		
		pos.x += xOffset;
		pos.y += yOffset;
		pos.z += zOffset;
		
		mesh.addVertex(pos, col, tex.x, tex.y);
	}
}
 

private void a(Vector3f var, Vector3f var1, Matrix4f var2, Vector3f var3) {
	Vector4f var4 = new Vector4f(var.x - var1.x, var.y - var1.y, var.z - var1.z, 1.0F);
	Matrix4f.transform(var2, var4, var4);
	var4.x *= var3.x;
	var4.y *= var3.y;
	var4.z *= var3.z;
	var.set(var4.x + var1.x, var4.y + var1.y, var4.z + var1.z);
}
 

private void a(Vector3f var, Vector3f var1, Matrix4f var2, Vector3f var3) {
	Vector4f var4 = new Vector4f(var.x - var1.x, var.y - var1.y, var.z - var1.z, 1.0F);
	Matrix4f.transform(var2, var4, var4);
	var4.x *= var3.x;
	var4.y *= var3.y;
	var4.z *= var3.z;
	var.set(var4.x + var1.x, var4.y + var1.y, var4.z + var1.z);
}
 

private void a(Vector3f var, Vector3f var1, Matrix4f var2, Vector3f var3) {
	Vector4f var4 = new Vector4f(var.x - var1.x, var.y - var1.y, var.z - var1.z, 1.0F);
	Matrix4f.transform(var2, var4, var4);
	var4.x *= var3.x;
	var4.y *= var3.y;
	var4.z *= var3.z;
	var.set(var4.x + var1.x, var4.y + var1.y, var4.z + var1.z);
}
 

public ArrayList<ParsingResult> checkForFlatScaling(GData1 ref) {
    ArrayList<ParsingResult> result = new ArrayList<ParsingResult>();

    Matrix4f tMatrix = (Matrix4f) ref.accurateLocalMatrix.getMatrix4f().invert();

    boolean plainOnX = true;
    boolean plainOnY = true;
    boolean plainOnZ = true;

    Set<VertexInfo> verts = lineLinkedToVertices.get(ref);
    if (verts == null) return result;
    for (VertexInfo vi : verts) {
        Vector4f vert = vi.vertex.toVector4f();
        vert.setX(vert.x / 1000f);
        vert.setY(vert.y / 1000f);
        vert.setZ(vert.z / 1000f);
        Vector4f vert2 = Matrix4f.transform(tMatrix, vert, null);

        if (plainOnX && Math.abs(vert2.x) > 0.001f) {
            plainOnX = false;
        }
        if (plainOnY && Math.abs(vert2.y) > 0.001f) {
            plainOnY = false;
        }
        if (plainOnZ && Math.abs(vert2.z) > 0.001f) {
            plainOnZ = false;
        }
        if (!plainOnX && !plainOnY && !plainOnZ) {
            return result;
        }
    }

    Matrix TMatrix2 = ref.accurateLocalMatrix;
    final BigDecimal lengthX =  plainOnX ? MathHelper.sqrt(TMatrix2.M00.multiply(TMatrix2.M00).add(TMatrix2.M01.multiply(TMatrix2.M01)).add(TMatrix2.M02.multiply(TMatrix2.M02))).subtract(BigDecimal.ONE).abs() : null;
    final BigDecimal lengthY =  plainOnY ? MathHelper.sqrt(TMatrix2.M10.multiply(TMatrix2.M10).add(TMatrix2.M11.multiply(TMatrix2.M11)).add(TMatrix2.M12.multiply(TMatrix2.M12))).subtract(BigDecimal.ONE).abs() : null;
    final BigDecimal lengthZ =  plainOnZ ? MathHelper.sqrt(TMatrix2.M20.multiply(TMatrix2.M20).add(TMatrix2.M21.multiply(TMatrix2.M21)).add(TMatrix2.M22.multiply(TMatrix2.M22))).subtract(BigDecimal.ONE).abs() : null;
    // Epsilon is 0.000001 / DATHeader default value is 0.0005
    final BigDecimal epsilon = new BigDecimal("0.000001"); //$NON-NLS-1$
    if (plainOnX && epsilon.compareTo(lengthX) < 0) {
        result.add(new ParsingResult(I18n.VM_FlatScaledX, "[W02] " + I18n.DATPARSER_Warning, ResultType.WARN)); //$NON-NLS-1$
    }
    if (plainOnY && epsilon.compareTo(lengthY) < 0) {
        result.add(new ParsingResult(I18n.VM_FlatScaledY, "[W03] " + I18n.DATPARSER_Warning, ResultType.WARN)); //$NON-NLS-1$
    }
    if (plainOnZ && epsilon.compareTo(lengthZ) < 0) {
        result.add(new ParsingResult(I18n.VM_FlatScaledZ, "[W04] " + I18n.DATPARSER_Warning, ResultType.WARN)); //$NON-NLS-1$
    }

    return result;
}
 

public Axis isFlatOnAxis(GData1 ref) {

        if (ref == null) return Axis.NONE;

        Matrix4f tMatrix = (Matrix4f) ref.accurateLocalMatrix.getMatrix4f().invert();

        boolean plainOnX = true;
        boolean plainOnY = true;
        boolean plainOnZ = true;

        Set<VertexInfo> verts = lineLinkedToVertices.get(ref);
        if (verts == null) return Axis.NONE;
        for (VertexInfo vi : verts) {
            Vector4f vert = vi.vertex.toVector4f();
            vert.setX(vert.x / 1000f);
            vert.setY(vert.y / 1000f);
            vert.setZ(vert.z / 1000f);
            Vector4f vert2 = Matrix4f.transform(tMatrix, vert, null);

            if (plainOnX && Math.abs(vert2.x) > 0.001f) {
                plainOnX = false;
            }
            if (plainOnY && Math.abs(vert2.y) > 0.001f) {
                plainOnY = false;
            }
            if (plainOnZ && Math.abs(vert2.z) > 0.001f) {
                plainOnZ = false;
            }
            if (!plainOnX && !plainOnY && !plainOnZ) {
                return Axis.NONE;
            }
        }

        if (plainOnX) {
            return Axis.X;
        } else if (plainOnY) {
            return Axis.Y;
        } else if (plainOnZ) {
            return Axis.Z;
        } else {
            return Axis.NONE;
        }
    }
 

private final void expandAnimation(AnimationInfo[] animations, float[][][] tmp_vertices, float[][][] tmp_normals, float[] initial_pose_vertices, float[] initial_pose_normals, byte[][] skin_names, float[][] skin_weights, BoundingBox[] bounding_boxes) {
		int num_bones = animations[0].getFrames()[0].length/12;
		Matrix4f[] frame_bones = new Matrix4f[num_bones];
		for (int bone = 0; bone < frame_bones.length; bone++)
			frame_bones[bone] = new Matrix4f();
		Vector4f v = new Vector4f();
		Vector4f n = new Vector4f();
		Vector4f temp = new Vector4f();
		FloatBuffer matrix_buffer = FloatBuffer.allocate(16);
		matrix_buffer.put(15, 1f);
		for (int anim = 0; anim < animations.length; anim++) {
			BoundingBox bounding_box = bounding_boxes[anim];
			int num_frames = animations[anim].getFrames().length;
			tmp_vertices[anim] = new float[num_frames][num_vertices*3];
			tmp_normals[anim] = new float[num_frames][num_vertices*3];
			for (int frame = 0; frame < num_frames; frame++) {
				float[] frame_animation = animations[anim].getFrames()[frame];
				for (int bone = 0; bone < num_bones; bone++) {
					matrix_buffer.clear();
					matrix_buffer.put(frame_animation, bone*12, 12);
					matrix_buffer.rewind();
					frame_bones[bone].loadTranspose(matrix_buffer);
				}
				float[] frame_normals = tmp_normals[anim][frame];
				float[] frame_vertices = tmp_vertices[anim][frame];
				float bmax_x = Float.NEGATIVE_INFINITY;
				float bmax_y = Float.NEGATIVE_INFINITY;
				float bmax_z = Float.NEGATIVE_INFINITY;
				float bmin_x = Float.POSITIVE_INFINITY;
				float bmin_y = Float.POSITIVE_INFINITY;
				float bmin_z = Float.POSITIVE_INFINITY;
				for (int vertex = 0; vertex < num_vertices; vertex++) {
					float x = initial_pose_vertices[vertex*3 + 0];
					float y = initial_pose_vertices[vertex*3 + 1];
					float z = initial_pose_vertices[vertex*3 + 2];
					float nx = initial_pose_normals[vertex*3 + 0];
					float ny = initial_pose_normals[vertex*3 + 1];
					float nz = initial_pose_normals[vertex*3 + 2];
					float result_x = 0, result_y = 0, result_z = 0, result_nx = 0, result_ny = 0, result_nz = 0;
					v.set(x, y, z, 1);
					n.set(nx, ny, nz, 0);
					byte[] vertex_skin_names = skin_names[vertex];
					float[] vertex_skin_weights = skin_weights[vertex];
					for (int bone = 0; bone < vertex_skin_names.length; bone++) {
						float weight = vertex_skin_weights[bone];
						Matrix4f bone_matrix = frame_bones[vertex_skin_names[bone]];
						Matrix4f.transform(bone_matrix, v, temp);
						result_x += temp.x*weight;
						result_y += temp.y*weight;
						result_z += temp.z*weight;
						// Assume matrix is only translation and scaling
						Matrix4f.transform(bone_matrix, n, temp);
						result_nx += temp.x*weight;
						result_ny += temp.y*weight;
						result_nz += temp.z*weight;
					}
					// Use Math.sqrt here for efficiency. Only used for normals (not gamestate affecting) anyway.
					float vec_len_inv = 1f/(float)Math.sqrt(result_nx*result_nx + result_ny*result_ny + result_nz*result_nz);
					result_nx *= vec_len_inv;
					result_ny *= vec_len_inv;
					result_nz *= vec_len_inv;
					frame_normals[vertex*3 + 0] = result_nx;
					frame_normals[vertex*3 + 1] = result_ny;
					frame_normals[vertex*3 + 2] = result_nz;

//result_x = x; result_y = y; result_z = z;
					if (result_x < bmin_x)
						bmin_x = result_x;
					else if (result_x > bmax_x)
						bmax_x = result_x;
					if (result_y < bmin_y)
						bmin_y = result_y;
					else if (result_y > bmax_y)
						bmax_y = result_y;
					if (result_z < bmin_z)
						bmin_z = result_z;
					else if (result_z > bmax_z)
						bmax_z = result_z;
					frame_vertices[vertex*3 + 0] = result_x;
					frame_vertices[vertex*3 + 1] = result_y;
					frame_vertices[vertex*3 + 2] = result_z;
				}
				bounding_box.checkBounds(bmin_x, bmax_x, bmin_y, bmax_y, bmin_z, bmax_z);
			}
		}
	}
 

private static Integer selectCSG_helper(Composite3D c3d, Event event) {
    final PowerRay powerRay = new PowerRay();
    final DatFile df = c3d.getLockableDatFileReference();
    final HashBiMap<Integer, GData> dpl = df.getDrawPerLine_NOCLONE();
    registeredData.putIfAbsent(df, new HashSet<GDataCSG>());

    PerspectiveCalculator perspective = c3d.getPerspectiveCalculator();
    Matrix4f viewport_rotation = c3d.getRotation();
    Vector4f zAxis4f = new Vector4f(0, 0, -1f, 1f);
    Matrix4f ovr_inverse2 = Matrix4f.invert(viewport_rotation, null);
    Matrix4f.transform(ovr_inverse2, zAxis4f, zAxis4f);
    Vector4f rayDirection = (Vector4f) new Vector4f(zAxis4f.x, zAxis4f.y, zAxis4f.z, 0f).normalise();
    rayDirection.w = 1f;

    Vertex[] triQuadVerts = new Vertex[3];

    Vector4f orig = perspective.get3DCoordinatesFromScreen(event.x, event.y);
    Vector4f point = new Vector4f(orig);

    double minDist = Double.MAX_VALUE;
    final double[] dist = new double[1];
    Integer result = null;
    GDataCSG resultObj = null;
    for (CSG csg : linkedCSG.putIfAbsent(df, new HashMap<String, CSG>()).values()) {
        for(Entry<GData3, IdAndPlane> pair : csg.getResult(df).entrySet()) {
            final GData3 triangle = pair.getKey();

            triQuadVerts[0] = new Vertex(triangle.x1, triangle.y1, triangle.z1);
            triQuadVerts[1] = new Vertex(triangle.x2, triangle.y2, triangle.z2);
            triQuadVerts[2] = new Vertex(triangle.x3, triangle.y3, triangle.z3);

            if (powerRay.TRIANGLE_INTERSECT(orig, rayDirection, triQuadVerts[0], triQuadVerts[1], triQuadVerts[2], point, dist)) {
                if (dist[0] < minDist) {
                    Integer result2 = pair.getValue().id;
                    if (result2 != null) {
                        for (GDataCSG c : registeredData.get(df)) {
                            if (dpl.containsValue(c) && idToGDataCSG.putIfAbsent(df, new HashBiMap<Integer, GDataCSG>()).containsKey(result2)) {
                                if (c.type == CSG.TRANSFORM && c.ref1 != null && c.ref2 != null || c.ref1 != null && c.ref2 == null && c.ref3 == null && c.type != CSG.COMPILE) {
                                    resultObj = idToGDataCSG.get(df).getValue(result2);
                                    if (resultObj != null && resultObj.ref1 != null && resultObj.ref1.endsWith("#>null")) { //$NON-NLS-1$
                                        minDist = dist[0];
                                        result = result2;
                                        break;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    selectedBodyMap.putIfAbsent(df, new HashSet<GDataCSG>());
    if (!(c3d.getKeys().isCtrlPressed() || (Cocoa.isCocoa && c3d.getKeys().isCmdPressed()))) {
        selectedBodyMap.get(df).clear();
    }
    selectedBodyMap.get(df).add(resultObj);
    return result;
}
 

public void drawProtractor_GL20(boolean selected, Composite3D c3d, BigDecimal x1c, BigDecimal y1c, BigDecimal z1c, BigDecimal x2c, BigDecimal y2c, BigDecimal z2c, BigDecimal x3c, BigDecimal y3c, BigDecimal z3c) {
    final java.text.DecimalFormat NUMBER_FORMAT2F = new java.text.DecimalFormat(View.NUMBER_FORMAT2F, new DecimalFormatSymbols(MyLanguage.LOCALE));
    final OpenGLRenderer20 renderer = (OpenGLRenderer20) c3d.getRenderer();
    final float zoom = 1f / c3d.getZoom();

    GL11.glDisable(GL11.GL_LIGHTING);

    GL11.glLineWidth(View.lineWidthGL[0]);
    if (selected) {
        GL11.glColor4f(View.vertex_selected_Colour_r[0], View.vertex_selected_Colour_g[0], View.vertex_selected_Colour_b[0], 1f);
        GL11.glBegin(GL11.GL_LINES);
        GL11.glVertex3f(x1, y1, z1);
        GL11.glVertex3f(x2, y2, z2);
        GL11.glEnd();
    } else {
        final float s = ((r + g + b) / 3 + .5f) % 1f;
        GL11.glColor4f(s, s, s, 1f);
        GL11.glBegin(GL11.GL_LINES);
        GL11.glVertex3f(x1, y1, z1);
        GL11.glVertex3f(x2, y2, z2);
        GL11.glEnd();
    }
    GL11.glColor4f(r, g, b, 1f);
    GL11.glBegin(GL11.GL_LINES);
    GL11.glVertex3f(x1, y1, z1);
    GL11.glVertex3f(x3, y3, z3);
    GL11.glEnd();

    final Vector4f textOrigin = new Vector4f(x1, y1, z1, 1f);
    Matrix4f.transform(c3d.getRotation(), textOrigin, textOrigin);

    Vector3d va = new Vector3d(x1c, y1c, z1c);
    Vector3d vb = new Vector3d(x2c, y2c, z2c);
    Vector3d vc = new Vector3d(x3c, y3c, z3c);
    vb = Vector3d.sub(va, vb);
    vc = Vector3d.sub(va, vc);
    double angle = Vector3d.angle(vb, vc);
    BigDecimal ang = new BigDecimal(angle);
    String angle_s = NUMBER_FORMAT2F.format(ang) + "°"; //$NON-NLS-1$

    float sx1 = x1 + (x2 - x1) * .2f;
    float sy1 = y1 + (y2 - y1) * .2f;
    float sz1 = z1 + (z2 - z1) * .2f;
    float sx2 = x1 + (x3 - x1) * .2f;
    float sy2 = y1 + (y3 - y1) * .2f;
    float sz2 = z1 + (z3 - z1) * .2f;
    float sx1t = x1 + (x2 - x1) * .25f;
    float sy1t = y1 + (y2 - y1) * .25f;
    float sz1t = z1 + (z2 - z1) * .25f;
    float sx2t = x1 + (x3 - x1) * .25f;
    float sy2t = y1 + (y3 - y1) * .25f;
    float sz2t = z1 + (z3 - z1) * .25f;
    float sx1tt = x1 + (x2 - x1) * .24f;
    float sy1tt = y1 + (y2 - y1) * .24f;
    float sz1tt = z1 + (z2 - z1) * .24f;
    float sx2tt = x1 + (x3 - x1) * .24f;
    float sy2tt = y1 + (y3 - y1) * .24f;
    float sz2tt = z1 + (z3 - z1) * .24f;
    float sx3 = sx1t * .5f + sx2t * .5f;
    float sy3 = sy1t * .5f + sy2t * .5f;
    float sz3 = sz1t * .5f + sz2t * .5f;
    float sx3r = sx1tt * .7f + sx2tt * .3f;
    float sy3r = sy1tt * .7f + sy2tt * .3f;
    float sz3r = sz1tt * .7f + sz2tt * .3f;
    float sx3l = sx1tt * .3f + sx2tt * .7f;
    float sy3l = sy1tt * .3f + sy2tt * .7f;
    float sz3l = sz1tt * .3f + sz2tt * .7f;

    GL11.glBegin(GL11.GL_LINES);
    GL11.glVertex3f(sx1, sy1, sz1);
    GL11.glVertex3f(sx3r, sy3r, sz3r);
    GL11.glVertex3f(sx3r, sy3r, sz3r);
    GL11.glVertex3f(sx3, sy3, sz3);
    GL11.glVertex3f(sx3, sy3, sz3);
    GL11.glVertex3f(sx3l, sy3l, sz3l);
    GL11.glVertex3f(sx3l, sy3l, sz3l);
    GL11.glVertex3f(sx2, sy2, sz2);
    GL11.glEnd();

    GL11.glPushMatrix();
    GL11.glMultMatrixf(renderer.getRotationInverse());

    GL11.glDisable(GL11.GL_CULL_FACE);
    GL11.glDisable(GL11.GL_DEPTH_TEST);
    PGData3.beginDrawText();
    GL11.glColor4f(r, g, b, 1f);
    drawNumber(angle_s, textOrigin.x, textOrigin.y, textOrigin.z, zoom);
    PGData3.endDrawText();
    GL11.glEnable(GL11.GL_DEPTH_TEST);
    GL11.glEnable(GL11.GL_CULL_FACE);
    GL11.glPopMatrix();

    GL11.glEnable(GL11.GL_LIGHTING);
}
 

public Vector4f getUntransformed(float x, float y, float z) {
    return Matrix4f.transform(resultinv, new Vector4f(x, y, z, 1f), null);
}
 

/**
 * Transforms 3D space coordinates to screen coordinates
 *
 * @param x
 *            x-screen coordinate
 * @param y
 *            y-screen coordinate
 * @param z
 *            z-screen coordinate
 * @return vector position on screen
 */
public Vector4f getScreenCoordinatesFrom3D(float x, float y, float z) {
    Point cSize = c3d.getSize();
    Vector4f relPos = new Vector4f(x, y, z, 1f);
    Matrix4f.transform(c3d.getViewport(), relPos, relPos);
    float cursor_x = 0.5f * cSize.x - relPos.x * View.PIXEL_PER_LDU;
    float cursor_y = 0.5f * cSize.y + relPos.y * View.PIXEL_PER_LDU;
    relPos.set(cursor_x, cursor_y, 0f, 1f);
    return relPos;
}
 

/**
 * Transforms 3D space coordinates to screen coordinates with Z axis
 *
 * @param x
 *            x-screen coordinate
 * @param y
 *            y-screen coordinate
 * @param z
 *            z-screen coordinate
 * @return vector position on screen
 */
public Vector4f getScreenCoordinatesFrom3DonlyZ(float x, float y, float z) {
    Vector4f relPos = new Vector4f(x, y, z, 1f);
    Matrix4f.transform(c3d.getViewport(), relPos, relPos);
    return relPos;
}