Mouse picking, rayTest

Post Reply
mgs_oleg
Posts: 10
Joined: Mon Jun 08, 2015 6:25 pm

Mouse picking, rayTest

Post by mgs_oleg » Thu Mar 29, 2018 7:45 pm

Hi All,

I am trying to get rayTest working for me, but it fails.
I am really does not know what is the issue.

In general I am creating convex hull based on the same coordinates that I has for my QUADS, looks like start and end of the ray calculated correctly....

Here is only source code.

Please help someone as I get in stack with that and see no light... :(

Code: Select all

//

#include "stdafx.h"
#include <GL/glut.h>  

#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>


#include <glm\glm.hpp>
#include <glm\gtc\matrix_transform.hpp>
#include <glm\gtc\quaternion.hpp>
#include <glm\gtx\quaternion.hpp>
#include <glm\gtx\euler_angles.hpp> 


btConvexHullShape* collisionShape;
btDiscreteDynamicsWorld* dynamicsWorld;
/* Global variables */
char title[] = "3D Shapes";

/* Initialize OpenGL Graphics */
void initGL() {
	glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // Set background color to black and opaque
	glClearDepth(1.0f);                   // Set background depth to farthest
	glEnable(GL_DEPTH_TEST);   // Enable depth testing for z-culling
	glDepthFunc(GL_LEQUAL);    // Set the type of depth-test
	glShadeModel(GL_SMOOTH);   // Enable smooth shading
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);  // Nice perspective corrections
}

/* Handler for window-repaint event. Called back when the window first appears and
whenever the window needs to be re-painted. */
void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear color and depth buffers
	glMatrixMode(GL_MODELVIEW);     // To operate on model-view matrix

	// Render a color-cube consisting of 6 quads with different colors
	glLoadIdentity();                 // Reset the model-view matrix
	glTranslatef(1.5f, 0.0f, -7.0f);  // Move right and into the screen

	glBegin(GL_QUADS);                // Begin drawing the color cube with 6 quads
	// Top face (y = 1.0f)
	// Define vertices in counter-clockwise (CCW) order with normal pointing out
	glColor3f(0.0f, 1.0f, 0.0f);     // Green
	glVertex3f(1.0f, 1.0f, -1.0f);
	glVertex3f(-1.0f, 1.0f, -1.0f);
	glVertex3f(-1.0f, 1.0f, 1.0f);
	glVertex3f(1.0f, 1.0f, 1.0f);

	// Bottom face (y = -1.0f)
	glColor3f(1.0f, 0.5f, 0.0f);     // Orange
	glVertex3f(1.0f, -1.0f, 1.0f);
	glVertex3f(-1.0f, -1.0f, 1.0f);
	glVertex3f(-1.0f, -1.0f, -1.0f);
	glVertex3f(1.0f, -1.0f, -1.0f);

	// Front face  (z = 1.0f)
	glColor3f(1.0f, 0.0f, 0.0f);     // Red
	glVertex3f(1.0f, 1.0f, 1.0f);
	glVertex3f(-1.0f, 1.0f, 1.0f);
	glVertex3f(-1.0f, -1.0f, 1.0f);
	glVertex3f(1.0f, -1.0f, 1.0f);

	// Back face (z = -1.0f)
	glColor3f(1.0f, 1.0f, 0.0f);     // Yellow
	glVertex3f(1.0f, -1.0f, -1.0f);
	glVertex3f(-1.0f, -1.0f, -1.0f);
	glVertex3f(-1.0f, 1.0f, -1.0f);
	glVertex3f(1.0f, 1.0f, -1.0f);

	// Left face (x = -1.0f)
	glColor3f(0.0f, 0.0f, 1.0f);     // Blue
	glVertex3f(-1.0f, 1.0f, 1.0f);
	glVertex3f(-1.0f, 1.0f, -1.0f);
	glVertex3f(-1.0f, -1.0f, -1.0f);
	glVertex3f(-1.0f, -1.0f, 1.0f);

	// Right face (x = 1.0f)
	glColor3f(1.0f, 0.0f, 1.0f);     // Magenta
	glVertex3f(1.0f, 1.0f, -1.0f);
	glVertex3f(1.0f, 1.0f, 1.0f);
	glVertex3f(1.0f, -1.0f, 1.0f);
	glVertex3f(1.0f, -1.0f, -1.0f);
	glEnd();  // End of drawing color-cube

		glutSwapBuffers();  // Swap the front and back frame buffers (double buffering)
}

/* Handler for window re-size event. Called back when the window first appears and
whenever the window is re-sized with its new width and height */
void reshape(GLsizei width, GLsizei height) {  // GLsizei for non-negative integer
	// Compute aspect ratio of the new window
	if (height == 0) height = 1;                // To prevent divide by 0
	GLfloat aspect = (GLfloat)width / (GLfloat)height;

	// Set the viewport to cover the new window
	glViewport(0, 0, width, height);

	// Set the aspect ratio of the clipping volume to match the viewport
	glMatrixMode(GL_PROJECTION);  // To operate on the Projection matrix
	glLoadIdentity();             // Reset
	// Enable perspective projection with fovy, aspect, zNear and zFar
	gluPerspective(45.0f, aspect, 0.1f, 100.0f);
}



void initiDynamics(){

	btBroadphaseInterface* broadphase = new btDbvtBroadphase();

	// Set up the collision configuration and dispatcher
	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

	// The actual physics solver
	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

	// The world.
	dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
	dynamicsWorld->setGravity(btVector3(0, -9.81f, 0));/// I DO NOT UNDERSTAND KURWA !!!!!!!!

	collisionShape = new btConvexHullShape();

	//top
	collisionShape->addPoint(btVector3(1.0f, 1.0f, -1.0f));
	collisionShape->addPoint(btVector3(-1.0f, 1.0f, -1.0f));
	collisionShape->addPoint(btVector3(-1.0f, 1.0f, 1.0f));
	collisionShape->addPoint(btVector3(1.0f, 1.0f, 1.0f));

	// Bottom face (btVector3(y = -1.0f))

	collisionShape->addPoint(btVector3(1.0f, -1.0f, 1.0f));
	collisionShape->addPoint(btVector3(-1.0f, -1.0f, 1.0f));
	collisionShape->addPoint(btVector3(-1.0f, -1.0f, -1.0f));
	collisionShape->addPoint(btVector3(1.0f, -1.0f, -1.0f));

	// Front face  (btVector3(z = 1.0f))

	collisionShape->addPoint(btVector3(1.0f, 1.0f, 1.0f));
	collisionShape->addPoint(btVector3(-1.0f, 1.0f, 1.0f));
	collisionShape->addPoint(btVector3(-1.0f, -1.0f, 1.0f));
	collisionShape->addPoint(btVector3(1.0f, -1.0f, 1.0f));

	// Back face (btVector3(z = -1.0f))

	collisionShape->addPoint(btVector3(1.0f, -1.0f, -1.0f));
	collisionShape->addPoint(btVector3(-1.0f, -1.0f, -1.0f));
	collisionShape->addPoint(btVector3(-1.0f, 1.0f, -1.0f));
	collisionShape->addPoint(btVector3(1.0f, 1.0f, -1.0f));

	// Left face (btVector3(x = -1.0f))

	collisionShape->addPoint(btVector3(-1.0f, 1.0f, 1.0f));
	collisionShape->addPoint(btVector3(-1.0f, 1.0f, -1.0f));
	collisionShape->addPoint(btVector3(-1.0f, -1.0f, -1.0f));
	collisionShape->addPoint(btVector3(-1.0f, -1.0f, 1.0f));

	// Right face (btVector3(x = 1.0f))

	collisionShape->addPoint(btVector3(1.0f, 1.0f, -1.0f));
	collisionShape->addPoint(btVector3(1.0f, 1.0f, 1.0f));
	collisionShape->addPoint(btVector3(1.0f, -1.0f, 1.0f));
	collisionShape->addPoint(btVector3(1.0f, -1.0f, -1.0f));

	//glTranslatef(1.5f, 0.0f, -7.0f);
	btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
		btQuaternion(0.0f, 0.0f, 0.0f, 1),
		btVector3(1.5f, 0.0f, -7.0f)
		));




	btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
		0,                  // mass, in kg. 0 -> Static object, will never move.
		motionstate,
		collisionShape,  // collision shape of body
		btVector3(0, 0, 0)    // local inertia
		);

	btRigidBody *rigidBody = new btRigidBody(rigidBodyCI);
	dynamicsWorld->addRigidBody(rigidBody);
	rigidBody->setUserPointer((int*)2);


	

}

void ScreenPosToWorldRay(
	int mouseX, int mouseY,             // Mouse position, in pixels, from bottom-left corner of the window
	int screenWidth, int screenHeight,  // Window size, in pixels
	glm::mat4 ViewMatrix,               // Camera position and orientation
	glm::mat4 ProjectionMatrix,         // Camera parameters (ratio, field of view, near and far planes)
	glm::vec3& out_origin,              // Ouput : Origin of the ray. /!\ Starts at the near plane, so if you want the ray to start at the camera's position instead, ignore this.
	glm::vec3& out_direction            // Ouput : Direction, in world space, of the ray that goes "through" the mouse.
	){

	// The ray Start and End positions, in Normalized Device Coordinates (Have you read Tutorial 4 ?)
	glm::vec4 lRayStart_NDC(
		((float)mouseX / (float)screenWidth - 0.5f) * 2.0f, // [0,1024] -> [-1,1]
		((float)mouseY / (float)screenHeight - 0.5f) * 2.0f, // [0, 768] -> [-1,1]
		-1.0, // The near plane maps to Z=-1 in Normalized Device Coordinates
		1.0f
		);
	glm::vec4 lRayEnd_NDC(
		((float)mouseX / (float)screenWidth - 0.5f) * 2.0f,
		((float)mouseY / (float)screenHeight - 0.5f) * 2.0f,
		0.0,
		1.0f
		);


	// The Projection matrix goes from Camera Space to NDC.
	// So inverse(ProjectionMatrix) goes from NDC to Camera Space.
	glm::mat4 InverseProjectionMatrix = glm::inverse(ProjectionMatrix);

	// The View Matrix goes from World Space to Camera Space.
	// So inverse(ViewMatrix) goes from Camera Space to World Space.
	glm::mat4 InverseViewMatrix = glm::inverse(ViewMatrix);

	glm::vec4 lRayStart_camera = InverseProjectionMatrix * lRayStart_NDC;    lRayStart_camera /= lRayStart_camera.w;
	glm::vec4 lRayStart_world = InverseViewMatrix       * lRayStart_camera; lRayStart_world /= lRayStart_world.w;
	glm::vec4 lRayEnd_camera = InverseProjectionMatrix * lRayEnd_NDC;      lRayEnd_camera /= lRayEnd_camera.w;
	glm::vec4 lRayEnd_world = InverseViewMatrix       * lRayEnd_camera;   lRayEnd_world /= lRayEnd_world.w;


	// Faster way (just one inverse)
	//glm::mat4 M = glm::inverse(ProjectionMatrix * ViewMatrix);
	//glm::vec4 lRayStart_world = M * lRayStart_NDC; lRayStart_world/=lRayStart_world.w;
	//glm::vec4 lRayEnd_world   = M * lRayEnd_NDC  ; lRayEnd_world  /=lRayEnd_world.w;


	glm::vec3 lRayDir_world(lRayEnd_world - lRayStart_world);
	lRayDir_world = glm::normalize(lRayDir_world);


	out_origin = glm::vec3(lRayStart_world);
	out_direction = glm::normalize(lRayDir_world);
}



void mouse(int button, int state, int x, int y) {

	glm::vec3 position = glm::vec3(1.5f, 0.0f, -7.0f);;
	//glm::vec4 perspective(45.0f, 640.0f / 480.0f, 0.1f, 100.0f); //taken from reshape 
	glm::mat4 projectionMatrix = glm::perspective(glm::radians(45.0f), 640.0f / 480.0f, 0.1f, 100.f);
	glm::mat4 identity(1.0f);
	//glm::mat4 projectionMatrix = identity * perspective;//
	// Up vector
	
	glm::mat4 viewMatrix = glm::translate(glm::mat4(1.0f), position);

	
	
	glm::vec3 out_origin;
	glm::vec3 out_direction;



	ScreenPosToWorldRay(
		x, y,
		640, 480,
		viewMatrix,
		projectionMatrix,
		out_origin,
		out_direction
		);

	glm::vec3 out_end = out_origin + out_direction*1000.0f;
	//printf(" x y z vec %f %f %f  ", out_origin.x, out_origin.y, out_origin.z);
	//printf(" x y z vec %f %f %f  ", out_direction.x, out_direction.y, out_direction.z);

	btCollisionWorld::ClosestRayResultCallback RayCallback(btVector3(out_origin.x, out_origin.y, out_origin.z), btVector3(out_end.x, out_end.y, out_end.z));
	dynamicsWorld->rayTest(btVector3(out_origin.x, out_origin.y, out_origin.z), btVector3(out_end.x, out_end.y, out_end.z), RayCallback);

	if (RayCallback.hasHit()) {
		int* i;
		i = (int*)RayCallback.m_collisionObject->getUserPointer();
		printf(" DETECTED = %d \n", i);
	}
}

/* Main function: GLUT runs as a console application starting at main() */
int main(int argc, char** argv) {
	glutInit(&argc, argv);            // Initialize GLUT
	glutInitDisplayMode(GLUT_DOUBLE); // Enable double buffered mode
	glutInitWindowSize(640, 480);   // Set the window's initial width & height
	glutInitWindowPosition(50, 50); // Position the window's initial top-left corner
	glutCreateWindow("TEST");          // Create window with the given title
	glutDisplayFunc(display);       // Register callback handler for window re-paint event
	glutReshapeFunc(reshape);       // Register callback handler for window re-size event
	
	glutMouseFunc(mouse);

	initGL();                       // Our own OpenGL initialization
	initiDynamics();
	glutMainLoop();                 // Enter the infinite event-processing loop
	return 0;
}

hyyou
Posts: 96
Joined: Wed Mar 16, 2016 10:11 am

Re: Mouse picking, rayTest

Post by hyyou » Sat Mar 31, 2018 2:16 am

Whoo.... the code is so long. Sorry for not read it.

I stuck with the same issue. Here is how I solved :-
1. Erase all Graphics code. Erase all mouse-related code.
2. Create empty world.
3. Create 1 very simple btBoxShape & 1 btRigidbody with all 1 mask at position (0,0,0).
4. Try to ray test only 1 time : from position (10,0,0) to another position (0,0,0). That is easy to debug.
5. I would rarely face any problem with such < 100 lines code. If it works, I would try to add more code.

Hope it helps.

mgs_oleg
Posts: 10
Joined: Mon Jun 08, 2015 6:25 pm

Re: Mouse picking, rayTest

Post by mgs_oleg » Sun Apr 01, 2018 9:43 am

Thanks! Will try.

Post Reply