OpenTK Ray Picking

Question

I just can't get ray picking to work (I need to drop old working faedback mode, and replace it with color pick, but because my scene can have up to 300K+ objects it is reasonable to filter (it's a hierarchy) visible objects by ray picking first to limit the number of drawn bounding boxes).

Here it goes. Why oh why this is failing me (I'm out of ideas, I don't see some obvious error...):

public class MouseRay
{
    private Vector3 _start;
    private Vector3 _end;

    public Vector3 Start { get { return _start; } }

    public Vector3 End { get { return _end; } }

    public MouseRay(Point mouse)
        : this(mouse.X, mouse.Y)
    {
    }

    public MouseRay(int x, int y)
    {
        int[] viewport = new int[4];
        Matrix4 modelMatrix, projMatrix;

        // get matrix and viewport:
        GL.GetFloat(GetPName.ModelviewMatrix, out modelMatrix);
        GL.GetFloat(GetPName.ProjectionMatrix, out projMatrix);
        GL.GetInteger(GetPName.Viewport, viewport);

        _start = new Vector3(0, 0, 0);
        _end = new Vector3(0, 0, 0);

        Glu.UnProject(new Vector3(x, y, 0.0f), modelMatrix, projMatrix, viewport, ref _start);
        Glu.UnProject(new Vector3(x, y, 1.0f), modelMatrix, projMatrix, viewport, ref _end);
    }

    public bool HitSphere(IDrawable drawable)
    {
        return HitSphereAt(drawable) != Vector3.Zero;
    }

    public Vector3 HitSphereAt(IDrawable drawable)
    {
        Vector3 pos = drawable.RealTranslation;
        float radius = drawable.BoundingRadius;

        Vector3 d = _end - _start;
        float a = Vector3.Dot(d, d);
        float b = 2.0f * Vector3.Dot(d, _start - pos);
        float c = 2.0f * Vector3.Dot(pos, pos) + Vector3.Dot(_start, _start) - 2.0f * Vector3.Dot(pos, _start) - radius * radius;

        float test = b * b - 4.0f * a * c;

        if (test >= 0.0)
        {
            // Hit (according to Treebeard, "a fine hit").
            float u = (-b - (float)Math.Sqrt(test)) / (2.0f * a);
            return _start + u * (_end - _start);
        }

        return Vector3.Zero;
    }
}

Here is unproject func (not my code, but I think I understand it):

public static class Glu
{
    //....

    public static int UnProject(Vector3 win, ref Vector3 obj)
    {
        Matrix4 modelMatrix;
        GL.GetFloat(GetPName.ModelviewMatrix, out modelMatrix);

        Matrix4 projMatrix;
        GL.GetFloat(GetPName.ProjectionMatrix, out projMatrix);

        int[] viewport = new int[4];
        GL.GetInteger(GetPName.Viewport, viewport);

        return UnProject(win, modelMatrix, projMatrix, viewport, ref obj);
    }

    public static int UnProject(Vector3 win, Matrix4 modelMatrix, Matrix4 projMatrix, int[] viewport, ref Vector3 obj)
    {
        return gluUnProject(win.X, win.Y, win.Z, modelMatrix, projMatrix, viewport, ref obj.X, ref obj.Y, ref obj.Z);
    }

    private static int gluUnProject(float winx, float winy, float winz, Matrix4 modelMatrix, Matrix4 projMatrix, int[] viewport, ref float objx, ref float objy, ref float objz)
    {
        Matrix4 finalMatrix;
        Vector4 _in;
        Vector4 _out;

        finalMatrix = Matrix4.Mult(modelMatrix, projMatrix);

        //if (!__gluInvertMatrixd(finalMatrix, finalMatrix)) return(GL_FALSE);
        finalMatrix.Invert();

        _in.X = winx;
        _in.Y = viewport[3] - winy;
        _in.Z = winz;
        _in.W = 1.0f;

        /* Map x and y from window coordinates */
        _in.X = (_in.X - viewport[0]) / viewport[2];
        _in.Y = (_in.Y - viewport[1]) / viewport[3];

        /* Map to range -1 to 1 */
        _in.X = _in.X * 2 - 1;
        _in.Y = _in.Y * 2 - 1;
        _in.Z = _in.Z * 2 - 1;

        //__gluMultMatrixVecd(finalMatrix, _in, _out);
        // check if this works:
        _out = Vector4.Transform(_in, finalMatrix);

        if (_out.W == 0.0)
            return (0);
        _out.X /= _out.W;
        _out.Y /= _out.W;
        _out.Z /= _out.W;
        objx = _out.X;
        objy = _out.Y;
        objz = _out.Z;
        return (1);
    }

    //....
}

Answer 1

Solved it. Maybe this will be usable for someone:

public class MouseRay
{
    private Vector3 _start;
    private Vector3 _end;

    public Vector3 Start { get { return _start; } }

    public Vector3 End { get { return _end; } }

    public MouseRay(Point mouse)
        : this(mouse.X, mouse.Y)
    {
    }

    public MouseRay(int x, int y)
    {
        int[] viewport = new int[4];
        Matrix4 modelMatrix, projMatrix;

        GL.GetFloat(GetPName.ModelviewMatrix, out modelMatrix);
        GL.GetFloat(GetPName.ProjectionMatrix, out projMatrix);
        GL.GetInteger(GetPName.Viewport, viewport);

        _start = new Vector3(x, y, 0.0f).UnProject(projMatrix, modelMatrix, new Size(viewport[2], viewport[3]));
        _end = new Vector3(x, y, 1.0f).UnProject(projMatrix, modelMatrix, new Size(viewport[2], viewport[3]));
    }

    public bool HitSphere(IDrawable drawable)
    {
        return Vector3.Cross(Vector3.Subtract(drawable.RealTranslation, _start), Vector3.Subtract(drawable.RealTranslation, _end)).Length / Vector3.Subtract(_end, _start).Length < drawable.BoundingRadius;
    }
}

Extensions
{
    public static Vector3 UnProject(this Vector3 mouse, Matrix4 projection, Matrix4 view, Size viewport)
    {
        Vector4 vec;

        vec.X = 2.0f * mouse.X / (float)viewport.Width - 1;
        vec.Y = -(2.0f * mouse.Y / (float)viewport.Height - 1);
        vec.Z = mouse.Z;
        vec.W = 1.0f;

        Matrix4 viewInv = Matrix4.Invert(view);
        Matrix4 projInv = Matrix4.Invert(projection);

        Vector4.Transform(ref vec, ref projInv, out vec);
        Vector4.Transform(ref vec, ref viewInv, out vec);

        if (vec.W > 0.000001f || vec.W < -0.000001f)
        {
            vec.X /= vec.W;
            vec.Y /= vec.W;
            vec.Z /= vec.W;
        }

        return vec.Xyz;
    }
}