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;
}