Projection to screen

Hi!

I want to project a 3D point to the screen. I want to mark the position of a certain Node on the hud (even when the node itself is not visible). This is so you can select an enemy as the current target and have it marked even when it is on the other side of a hill or buildning.

I found an old thread about this and copied the code from it. But I can't get it to work. The point is a little bit wrong. I'm not (yet) good enough at the matrix operations of openGL to understand what is happening.

The method below is not exactly the one described on for instance this page. But I tried that one as well, didn't get it to work.
http://pyopengl.sourceforge.net/documentation/manual/gluProject.3G.xml

Code:

import org.openmali.vecmath2.*;
import org.openmali.FastMath;
import org.xith3d.loop.InputAdapterRenderLoop;
import org.xith3d.loop.RenderLoop;
import org.xith3d.base.Xith3DEnvironment;
import org.xith3d.scenegraph.BranchGroup;
import org.xith3d.scenegraph.TransformGroup;
import org.xith3d.scenegraph.Transform3D;
import org.xith3d.scenegraph.StaticTransform;
import org.xith3d.scenegraph.primitives.Cube;
import org.xith3d.render.RenderPass;
import org.xith3d.render.Canvas3DFactory;
import org.xith3d.render.Canvas3D;
import org.xith3d.render.util.WindowClosingListener;
import org.xith3d.ui.hud.HUD;
import org.xith3d.ui.hud.widgets.LineWidget;
import org.jagatoo.input.events.KeyPressedEvent;
import org.jagatoo.input.devices.components.Key;
import org.jagatoo.input.InputSystem;
import org.jagatoo.input.InputSystemException;

public class Test extends InputAdapterRenderLoop implements WindowClosingListener {
private Xith3DEnvironment env;
private BranchGroup root;
private RenderPass scenePass;
private Canvas3D canvas;
private Cube cube;
private TransformGroup cubeTG;
private LineWidget line;

public static void main(String[] args) {
Test test = new Test();
test.begin(RenderLoop.RunMode.RUN_IN_SAME_THREAD);
}

public Test() {
super(30);

env = new Xith3DEnvironment(this);
env.getView().setFrontClipDistance(0.2f);
env.getView().setBackClipDistance(1500f);

root = new BranchGroup();

scenePass = env.addPerspectiveBranch(root);

canvas = Canvas3DFactory.createWindowed(800, 600, "Project");
env.addCanvas(canvas);
canvas.addWindowClosingListener(this);
try {
InputSystem.getInstance().registerNewKeyboardAndMouse(canvas.getPeer());
} catch (InputSystemException e) {
e.printStackTrace();
}

cube = new Cube(1);
StaticTransform.translate(cube, 0, 3, 0);
cubeTG = new TransformGroup();
cubeTG.addChild(cube);
root.addChild(cubeTG);

canvas.getView().lookAt(new Tuple3f(10, 0, 0), Point3f.ZERO, Vector3f.POSITIVE_Y_AXIS);

HUD hud = new HUD(800, 600, 800);
env.addHUD(hud);

line = new LineWidget(new Vector2f(0, 5), 5f, Colorf.RED);
hud.addWidget(line);
line.setZIndex(1);
}

@Override
/**
Rotates the cube around zero and tries to mark it's position (or rather, the center of it's worldbounds)
with a lineWidget.
*/
public void update(long gameTime, long frameTime, TimingMode timingMode) {
super.update(gameTime, frameTime, timingMode);

Transform3D t3d = cubeTG.getTransform();
t3d.rotZ(timingMode.getSecondsAsFloat(gameTime));
cubeTG.setTransform(t3d);

Point3f centerPoint = Point3f.fromPool();
cube.getWorldBounds().getCenter(centerPoint);
Point2f point = worldToScreen(centerPoint);
line.setLocation(point.getX(), point.getY());
Point3f.toPool(centerPoint);
}

public Point2f worldToScreen (Point3f p) {
// get the model-view and the projection matrix
Matrix4f mP = canvas.getView().calculatePerspective(800, 600).getMatrix4f();
Matrix4f mM = canvas.getView().getModelViewTransform(true).getMatrix4f();

// convert the point into a vector of length 4
Vector4f v = new Vector4f(p.getX(), p.getY(), p.getZ(), 1);

// compute the distance between the eye and the view plane
float vpd = 1.0f / FastMath.tan(canvas.getView().getFieldOfView());

// v' = P x M x v
v.mul(mM, v);
v.mul(mP, v);

// scale the resulting vector so that it lies in the view plane
float x = v.getX() * vpd / v.getZ();
float y = v.getY() * vpd / v.getZ();

// convert normalized view plane coordinates into screen coordinates
float xs = (float) 800 * (x + 1f) / 2f;
float ys = (float) 600 / 2f * (1f - y);
return new Point2f(xs, ys);
}

public void onWindowCloseRequested(Canvas3D canvas) {
this.end();
}

public void onKeyPressed(KeyPressedEvent e, Key key) {
switch (key.getKeyID()) {
case ESCAPE:
this.end();
break;
}
}

}

Edit:
This is the old thread:
http://xith.org/forum/index.php/topic,464.15.html

Edit 2:
Sorry, just noticed I posted this in General. Should be in support of course. So I deleted the first and posted it here.