Help with collision detection with PickTool in J3D!

Hello!

I got a small app where I have implanted some collision detection. But The collision detection doesn't work perfect. So if any one could take a look and give me an advice.

There is 2 ColorCubes. One small and one Big, The small one is navigable with the arrow keys. It should not be possible to "drive" through the big ColorCube but with my code it unfortunatly is.

The small cube is supposed to stop immediatly outside the big cube before it run through it.

Best Regards
Fredrik

Below is the hole code to test this (2 files):
//file 1
package java3d;

import java.applet.*;
import java.awt.*;
import java.awt.Frame;
import java.awt.event.*;
import com.sun.j3d.utils.applet.MainFrame;
import com.sun.j3d.utils.universe.*;
import com.sun.j3d.utils.geometry.*;
import com.sun.j3d.utils.behaviors.keyboard.*;
import javax.media.j3d.*;
import javax.vecmath.*;
import javax.swing.*;

import java.util.*;

//import com.mnstarfire.loaders3d.Inspector3DS;

public class Test extends Applet
{
    BranchGroup branchGroup;
    ColorCube colorCube1 = new ColorCube(0.4); //Den som styrs
    ColorCube colorCube2 = new ColorCube(0.6);

    public void init()
    {
        setLayout(new BorderLayout());
        GraphicsConfiguration config = SimpleUniverse.getPreferredConfiguration();
        Canvas3D canvas3D = new Canvas3D(config);
        add("Center", canvas3D);
        SimpleUniverse simpleUniverse = new SimpleUniverse(canvas3D);

        branchGroup = new BranchGroup();
        branchGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        branchGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        branchGroup.setCapability(Shape3D.ALLOW_GEOMETRY_READ);

        //The small cube
        TransformGroup transformGroup1 = new TransformGroup();
        Transform3D transform3D1 = new Transform3D();
        transform3D1.set(new Vector3f(0.0f, 0.0f, -20.0f));
        transformGroup1.setTransform(transform3D1);
        transformGroup1.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        transformGroup1.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        transformGroup1.setPickable(false);
        transformGroup1.addChild(colorCube1);
        branchGroup.addChild(transformGroup1);

        canvas3D.addKeyListener( new TestListener(transformGroup1, this) );

        //The big cube
        TransformGroup transformGroup2 = new TransformGroup();
        Transform3D transform3D2 = new Transform3D();
        transform3D2.set(new Vector3f(0.0f, 2.0f, -20.0f));
        transformGroup2.setTransform(transform3D2);
        colorCube2.setPickable(true);
        transformGroup2.addChild(colorCube2);
        branchGroup.addChild(transformGroup2);

        branchGroup.compile();

        simpleUniverse.addBranchGraph(branchGroup);
    }

    public static void main(String[] args)
    {
        Frame frame = new MainFrame(new Test(), 600, 400);
    }
}

//file 2
package java3d;

import java.awt.event.*;
import javax.media.j3d.*;
import javax.vecmath.*;
import java.awt.Frame;
import com.sun.j3d.utils.applet.MainFrame;

import com.sun.j3d.utils.picking.*;
import com.sun.j3d.utils.geometry.*;

public class TestListener implements KeyListener
{
    final static float DISTANCE     = 0.1f;
    final static double TURNANGLE     = 0.1;
    float x = 0.0f;
    float y = 0.0f;
    float z = -20.0f;
    private double angle = 0.0;

    TransformGroup transformGroup;
    Transform3D positionTransform3D = new Transform3D();
    Transform3D angleTransform3D = new Transform3D();

    Test test;

    PickTool pickTool;
    Point3d point3d;
    Vector3d vector3d;
    Transform3D transform3D;

    public TestListener(TransformGroup tfg, Test t)
    {
        test = t;
        transformGroup = tfg;
        pickTool = new PickTool(test.branchGroup);
        pickTool.setCapabilities(test.colorCube2, PickTool.INTERSECT_FULL);
        positionTransform3D.set(new Vector3f(x, y, z));
    }

    public void keyTyped(KeyEvent e)
    {
    }

    public void keyPressed(KeyEvent e)
    {
        if( e.getKeyCode() == KeyEvent.VK_UP )
        {
            if(isMovePossible(DISTANCE))
            {
                Transform3D temp = new Transform3D();
                temp.set(new Vector3f(0, DISTANCE, 0));
                positionTransform3D.mul(temp);
                transformGroup.setTransform( positionTransform3D );
                //printOutCordinates(transformGroup);
            }
        }
        else if( e.getKeyCode() == KeyEvent.VK_DOWN )
        {
            if(isMovePossible(-DISTANCE))
            {
                Transform3D temp = new Transform3D();
                temp.set(new Vector3f(0, -DISTANCE, 0));
                positionTransform3D.mul(temp);
                transformGroup.setTransform( positionTransform3D );
                //printOutCordinates(transformGroup);
            }
        }
        else if( e.getKeyCode() == KeyEvent.VK_LEFT )
        {
            angle = angle + TURNANGLE;
            angleTransform3D.rotZ(TURNANGLE);
            positionTransform3D.mul(angleTransform3D);
            transformGroup.setTransform( positionTransform3D );

            //printOutCordinates(transformGroup);
        }
        else if( e.getKeyCode() == KeyEvent.VK_RIGHT )
        {
            angle = angle - TURNANGLE;
            angleTransform3D.rotZ(-TURNANGLE);
            positionTransform3D.mul(angleTransform3D);
            transformGroup.setTransform( positionTransform3D );

            //printOutCordinates(transformGroup);
        }
    }

    public void keyReleased(KeyEvent e)
    {
    }

    public boolean isMovePossible(float distance)
    {
        boolean retValue = true;
        point3d = new Point3d();
        vector3d = new Vector3d(0, distance, 0);
        transform3D = new Transform3D();
        transform3D.setTranslation(new Vector3d(0, distance, 0)); //Funkar det inte kan denna rad vara fel
        transform3D.mul(positionTransform3D);
        transform3D.transform(point3d);
        pickTool.setShapeRay(point3d, vector3d);
        PickResult pickResult = pickTool.pickClosest();
        if(pickResult != null)
        {
            Node node = pickResult.getObject();
            if(node instanceof ColorCube )
            {
                //System.out.println("ColorCube");
            }
            PickIntersection pickIntersection = pickResult.getIntersection(0);
            if(pickIntersection != null)
            {
                Point3d checkPoint = pickIntersection.getPointCoordinates();
                if(checkPoint != null)
                {
                    System.out.println("Boink " + checkPoint.x + " " + checkPoint.y);
                    retValue = true;
                }
            }
        }
        return retValue;
    }

    public static void main(String[] args)
    {
        Frame frame = new MainFrame(new Test(), 600, 350);
    }

    public void printOutCordinates(TransformGroup transformGroup)
    {
        Transform3D printOutTransform3D = new Transform3D();
        transformGroup.getTransform( printOutTransform3D );
        float[] cordinates = new float[16];
        printOutTransform3D.get(cordinates);
        System.out.println("x: " + cordinates[0] + " y: " + cordinates[1] + " z: " + cordinates[2]);

        Vector3d v = new Vector3d(cordinates[0], cordinates[1], cordinates[2]);
        Point3d oft = new Point3d(cordinates[0], cordinates[1]+DISTANCE, cordinates[2]);
        //test.checkForCollision(v, oft);
    }
}