Skip to content

M
MasterArbeit
Commit 2c11e009 by Kai Westerkamp
Options

Camera

parent 9c55c1f0
Showing with 603 additions and 135 deletions
3_PointCloud/Camera.cpp 0 → 100644
#include "Camera.h"
#include <iostream>
#include "glm/gtx/string_cast.hpp"
#include "quaternion_utils.h"
Camera::Camera()
{
// rotation = glm::gtx::quaternion::angleAxis(degrees(45), RotationAxis);
position = glm::vec3(20, 10, 0);
//direction = normalize(-position);
rotate(0, 0); //sets direction
up = glm::vec3(0.0f, 1.0f, 0.0f);
}
Camera::~Camera()
{
}
void Camera::move(glm::vec3 relative)
{
glm::vec3 right = cross(direction, up);
vec3 uplocal = cross(right, direction);
position += (relative.x*speed* direction)+ (relative.y*speed*uplocal)+ (relative.z*speed* right);
}
void Camera::rotate(int dx, int dy)
{
horizontalAngle += mouseSpeed * float(dx);
verticalAngle += mouseSpeed * float(dy);
direction = glm::vec3(
cos(verticalAngle) * sin(horizontalAngle),
sin(verticalAngle),
cos(verticalAngle) * cos(horizontalAngle)
);
}
void Camera::applyMatrix()
{
glm::vec3 p = position + direction;
gluLookAt(position.x, position.y, position.z,
p.x, p.y, p.z, up.x, up.y, up.z);
}
3_PointCloud/Camera.h 0 → 100644
#pragma once
#define _USE_MATH_DEFINES
#include <glm/gtx/quaternion.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/norm.hpp>
#include "glut.h"
#include <math.h>
class Camera
{
public:
glm::vec3 direction;
glm::vec3 position;
glm::vec3 up;
float speed = 0.3f;
float mouseSpeed = 0.005f;
float horizontalAngle = -M_PI/2;
float verticalAngle = -M_PI/8;
Camera();
~Camera();
void move(glm::vec3 relative);
void rotate(int dx, int dy);
void applyMatrix();;
};
3_PointCloud/KinectTutorial3.vcxproj
...@@ -87,17 +87,24 @@ ...@@ -87,17 +87,24 @@
</Link> </Link>
</ItemDefinitionGroup> </ItemDefinitionGroup>
<ItemGroup> <ItemGroup>
<ClCompile Include="Camera.cpp" />
<ClCompile Include="glut.cpp" /> <ClCompile Include="glut.cpp" />
<ClCompile Include="main.cpp" /> <ClCompile Include="main.cpp" />
<ClCompile Include="quaternion_utils.cpp" />
<ClCompile Include="SteamTracking.cpp" /> <ClCompile Include="SteamTracking.cpp" />
<ClCompile Include="SteamVrTrackingServer.cpp" /> <ClCompile Include="SteamVrTrackingServer.cpp" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClInclude Include="Camera.h" />
<ClInclude Include="glut.h" /> <ClInclude Include="glut.h" />
<ClInclude Include="quaternion_utils.h" />
<ClInclude Include="SteamTracking.h" /> <ClInclude Include="SteamTracking.h" />
<ClInclude Include="SteamVrTrackingServer.h" /> <ClInclude Include="SteamVrTrackingServer.h" />
<ClInclude Include="main.h" /> <ClInclude Include="main.h" />
</ItemGroup> </ItemGroup>
<ItemGroup>
<ResourceCompile Include="KinectTutorial3.rc" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets"> <ImportGroup Label="ExtensionTargets">
</ImportGroup> </ImportGroup>
......
3_PointCloud/KinectTutorial3.vcxproj.filters
...@@ -27,6 +27,12 @@ ...@@ -27,6 +27,12 @@
<ClCompile Include="SteamTracking.cpp"> <ClCompile Include="SteamTracking.cpp">
<Filter>Source Files</Filter> <Filter>Source Files</Filter>
</ClCompile> </ClCompile>
<ClCompile Include="Camera.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="quaternion_utils.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClInclude Include="glut.h"> <ClInclude Include="glut.h">
...@@ -41,5 +47,16 @@ ...@@ -41,5 +47,16 @@
<ClInclude Include="SteamTracking.h"> <ClInclude Include="SteamTracking.h">
<Filter>Header Files</Filter> <Filter>Header Files</Filter>
</ClInclude> </ClInclude>
<ClInclude Include="Camera.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="quaternion_utils.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ResourceCompile Include="KinectTutorial3.rc">
<Filter>Resource Files</Filter>
</ResourceCompile>
</ItemGroup> </ItemGroup>
</Project> </Project>
\ No newline at end of file
3_PointCloud/SteamTracking.cpp
...@@ -63,8 +63,8 @@ SteamTracking::~SteamTracking() ...@@ -63,8 +63,8 @@ SteamTracking::~SteamTracking()
void SteamTracking::printVRDevices() { void SteamTracking::printVRDevices() {
std::cout << "List of available VR devices:" << std::endl; std::cout << "List of available VR devices:" << std::endl;
if (!vr_system_) { if (!vr_system_) {
std::cout << "\tNot connected to VR System, no devices"; std::cout << "\tNot connected to VR System, no devices"<< std::endl;
return;
} }
for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice) for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
{ {
...@@ -86,6 +86,8 @@ void SteamTracking::printVRDevices() { ...@@ -86,6 +86,8 @@ void SteamTracking::printVRDevices() {
void SteamTracking::processEvents() void SteamTracking::processEvents()
{ {
if (!vr_system_) return;
vr::VREvent_t event; vr::VREvent_t event;
while (vr_system_->PollNextEvent(&event, sizeof(event))) while (vr_system_->PollNextEvent(&event, sizeof(event)))
{ {
......
3_PointCloud/glut.cpp
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
#include "main.h" #include "main.h"
void draw() { void draw() {
drawKinectData(); mainRenderLoop();
glutSwapBuffers(); glutSwapBuffers();
} }
...@@ -16,6 +16,8 @@ bool init(int argc, char* argv[]) { ...@@ -16,6 +16,8 @@ bool init(int argc, char* argv[]) {
glutInitWindowSize(width,height); glutInitWindowSize(width,height);
glutCreateWindow("Kinect VR Point Cloud"); glutCreateWindow("Kinect VR Point Cloud");
glutKeyboardFunc(keyPressed); glutKeyboardFunc(keyPressed);
glutMouseFunc(mouseButton);
glutMotionFunc(mouseMove);
glutDisplayFunc(draw); glutDisplayFunc(draw);
glutIdleFunc(draw); glutIdleFunc(draw);
glewInit(); glewInit();
......
3_PointCloud/main.cpp
#include "main.h" #include "main.h"
#include "glut.h" #include "glut.h"
#include "SteamVRTrackingServer.h"
#include "SteamTracking.h" #include "SteamTracking.h"
#include <cmath> #include <cmath>
...@@ -18,29 +17,36 @@ ...@@ -18,29 +17,36 @@
#include <glm/gtc/type_ptr.hpp> #include <glm/gtc/type_ptr.hpp>
#include <Kinect.h> #include <Kinect.h>
#include "Camera.h"
// We'll be using buffer objects to store the kinect point cloud // We'll be using buffer objects to store the kinect point cloud
GLuint vboId;
GLuint cboId;
GLuint vboIdExport;
GLuint cboIdExport;
float arrowPoints[] = { 1, 0 , 0, 0, 1, 0, 0, 0, 1 struct drawData {
GLuint vboID;
GLuint cboID;
GLsizei size;
}; };
drawData framePoints;
drawData gridData;
drawData exportData;
drawData arrowData;
// Intermediate Buffers // Intermediate Buffers
unsigned char rgbimage[colorwidth*colorheight*4]; // Stores RGB color image unsigned char rgbimage[colorwidth*colorheight*4]; // Stores RGB color image
ColorSpacePoint depth2rgb[width*height]; // Maps depth pixels to rgb pixels ColorSpacePoint depth2rgb[width*height]; // Maps depth pixels to rgb pixels
CameraSpacePoint depth2xyz[width*height]; // Maps depth pixels to 3d coordinates CameraSpacePoint depth2xyz[width*height]; // Maps depth pixels to 3d coordinates
//buffers of the current frame
glm::vec3 framePositions[width*height]; glm::vec3 framePositions[width*height];
float frameColors[width*height * 3]; unsigned char frameColors[width*height * 3];
bool frameValidPoints[width*height];
//export Buffers;
std::vector<glm::vec3> points; std::vector<glm::vec3> points;
std::vector<unsigned char> colors; std::vector<unsigned char> colors;
std::vector<glm::vec3> snapshotPoints;
std::vector<unsigned char> snapshotColors;
// Kinect Variables // Kinect Variables
IKinectSensor* sensor; // Kinect sensor IKinectSensor* sensor; // Kinect sensor
...@@ -49,9 +55,12 @@ ICoordinateMapper* mapper; // Converts between depth, color, and 3d coor ...@@ -49,9 +55,12 @@ ICoordinateMapper* mapper; // Converts between depth, color, and 3d coor
SteamTracking* steamTracking = nullptr; SteamTracking* steamTracking = nullptr;
glm::mat4x4* currentControlerPos = nullptr; glm::mat4x4* currentControlerPos = nullptr;
Camera mainCam;
static bool writeFile = false; static bool writeFile = false;
static bool captureFrame = false; static bool captureFrame = false;
int lastX, lastY;
void drawArrow() { void drawArrow() {
...@@ -128,7 +137,7 @@ void getDepthData(IMultiSourceFrame* frame) { ...@@ -128,7 +137,7 @@ void getDepthData(IMultiSourceFrame* frame) {
if (depthframe) depthframe->Release(); if (depthframe) depthframe->Release();
} }
void getRgbData(IMultiSourceFrame* frame, bool writeFrameInVector) { void getRgbData(IMultiSourceFrame* frame) {
IColorFrame* colorframe; IColorFrame* colorframe;
IColorFrameReference* frameref = NULL; IColorFrameReference* frameref = NULL;
frame->get_ColorFrameReference(&frameref); frame->get_ColorFrameReference(&frameref);
...@@ -137,41 +146,28 @@ void getRgbData(IMultiSourceFrame* frame, bool writeFrameInVector) { ...@@ -137,41 +146,28 @@ void getRgbData(IMultiSourceFrame* frame, bool writeFrameInVector) {
if (!colorframe) return; if (!colorframe) return;
if (writeFrameInVector) {
snapshotPoints.clear();
snapshotPoints.reserve(width*height);
snapshotColors.clear();
snapshotColors.reserve(width*height * 3);
}
// Get data from frame // Get data from frame
colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba); colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba);
// Write color array for vertices // Write color array for vertices
float* fdest = (float*)frameColors; unsigned char* fdest = (unsigned char*)frameColors;
bool* fvalid = (bool*)frameValidPoints;
for (int i = 0; i < width*height; i++) { for (int i = 0; i < width*height; i++) {
ColorSpacePoint p = depth2rgb[i]; ColorSpacePoint p = depth2rgb[i];
// Check if color pixel coordinates are in bounds // Check if color pixel coordinates are in bounds
if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) { if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
*fdest++ = 1; *fdest++ = 255;
*fdest++ = 0; *fdest++ = 0;
*fdest++ = 0; *fdest++ = 0;
*fvalid++ = false;
} }
else { else {
int idx = (int)p.X + colorwidth*(int)p.Y; int idx = (int)p.X + colorwidth*(int)p.Y;
*fdest++ = rgbimage[4 * idx + 0] / 255.f; *fdest++ = rgbimage[4 * idx + 0];
*fdest++ = rgbimage[4 * idx + 1] / 255.f; *fdest++ = rgbimage[4 * idx + 1];
*fdest++ = rgbimage[4 * idx + 2] / 255.f; *fdest++ = rgbimage[4 * idx + 2];
*fvalid++ = true;
if (writeFrameInVector) {
//copy points with valid Color
snapshotPoints.push_back(framePositions[i]);
snapshotColors.push_back(rgbimage[4 * idx + 0]);
snapshotColors.push_back(rgbimage[4 * idx + 1]);
snapshotColors.push_back(rgbimage[4 * idx + 2]);
}
} }
// Don't copy alpha channel // Don't copy alpha channel
} }
...@@ -179,27 +175,52 @@ void getRgbData(IMultiSourceFrame* frame, bool writeFrameInVector) { ...@@ -179,27 +175,52 @@ void getRgbData(IMultiSourceFrame* frame, bool writeFrameInVector) {
if (colorframe) colorframe->Release(); if (colorframe) colorframe->Release();
} }
void getKinectData(bool writeFrameInVector) { void getKinectData() {
IMultiSourceFrame* frame = NULL; IMultiSourceFrame* frame = NULL;
if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) { if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
delete currentControlerPos;
currentControlerPos = steamTracking->getTransformationForDevice(1);
getDepthData(frame); getDepthData(frame);
getRgbData(frame, writeFrameInVector); getRgbData(frame);
glBindBuffer(GL_ARRAY_BUFFER, vboId); glBindBuffer(GL_ARRAY_BUFFER, framePoints.vboID);
glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW); glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW);
glUnmapBuffer(GL_ARRAY_BUFFER); glUnmapBuffer(GL_ARRAY_BUFFER);
glBindBuffer(GL_ARRAY_BUFFER, cboId); glBindBuffer(GL_ARRAY_BUFFER, framePoints.cboID);
glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), frameColors, GL_DYNAMIC_DRAW); glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(unsigned char), frameColors, GL_DYNAMIC_DRAW);
glUnmapBuffer(GL_ARRAY_BUFFER); glUnmapBuffer(GL_ARRAY_BUFFER);
} }
if (frame) frame->Release(); if (frame) frame->Release();
} }
void processCurrentFrameForExport() {
points.clear();
colors.clear();
points.reserve(points.size() + width*height);
colors.reserve(colors.size() + width*height * 3);
for (unsigned int i = 0; i < width*height; i++) {
if (frameValidPoints[i]) {
points.push_back(*currentControlerPos * glm::vec4(framePositions[i], 1.0f));
colors.push_back(frameColors[i * 3]);
colors.push_back(frameColors[i * 3 + 1]);
colors.push_back(frameColors[i * 3 + 2]);
}
}
//update Buffers
glBindBuffer(GL_ARRAY_BUFFER, exportData.vboID);
glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(glm::vec3), &points[0], GL_DYNAMIC_DRAW);
glUnmapBuffer(GL_ARRAY_BUFFER);
glBindBuffer(GL_ARRAY_BUFFER, exportData.cboID);
glBufferData(GL_ARRAY_BUFFER, colors.size() * sizeof(unsigned char), &colors[0], GL_DYNAMIC_DRAW);
exportData.size = points.size();
glUnmapBuffer(GL_ARRAY_BUFFER);
}
void writePointCloud() { void writePointCloud() {
if (points.size() == 0) { if (points.size() == 0) {
std::cout << "No points, scipping File write" << std::endl; std::cout << "No points, scipping File write" << std::endl;
...@@ -236,47 +257,38 @@ void rotateCamera() { ...@@ -236,47 +257,38 @@ void rotateCamera() {
static double angle = 0.; static double angle = 0.;
static double radius = 20.; static double radius = 20.;
double x = radius*sin(angle); double x = radius*sin(angle);
double z = radius*(1-cos(angle)) - radius/2; double z = radius*(1-cos(angle))-radius/2;
//gluLookAt(0, 0, 0, 0, 5, 0, 0, 1, 0); //gluLookAt(0, 0, 0, 0, 5, 0, 0, 1, 0);
gluLookAt(x,0,z,0,0,radius/2,0,1,0); gluLookAt(x,10,z, 0,0,0, 0,1,0);
//angle += 0.002; angle += 0.002;
} }
void drawKinectData() { void drawElementArray(drawData data, GLenum mode) {
if (steamTracking) glBindBuffer(GL_ARRAY_BUFFER, data.vboID);
steamTracking->processEvents(); glVertexPointer(3, GL_FLOAT, 0, NULL);
getKinectData(captureFrame); glBindBuffer(GL_ARRAY_BUFFER, data.cboID);
glColorPointer(3, GL_UNSIGNED_BYTE, 0, NULL);
//add points to point cloud glDrawArrays(mode, 0, data.size);
if (captureFrame) { }
//get controler pos
points.clear();
colors.clear();
points.reserve(points.size() + width*height);
colors.reserve(colors.size() + width*height * 3);
for (unsigned int i = 0; i < snapshotPoints.size(); i++) {
points.push_back(*currentControlerPos * glm::vec4(snapshotPoints[i], 1.0f));
colors.push_back(snapshotColors[i * 3]);
colors.push_back(snapshotColors[i * 3 + 1]);
colors.push_back(snapshotColors[i * 3 + 2]);
}
/*
glBindBuffer(GL_ARRAY_BUFFER, vboIdExport);
glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(glm::vec3), &points[0], GL_DYNAMIC_DRAW);
glUnmapBuffer(GL_ARRAY_BUFFER);
glBindBuffer(GL_ARRAY_BUFFER, cboIdExport); void mainRenderLoop() {
glBufferData(GL_ARRAY_BUFFER, colors.size() * sizeof(GLfloat), &colors[0], GL_DYNAMIC_DRAW); steamTracking->processEvents();
glUnmapBuffer(GL_ARRAY_BUFFER);*/ //get controler pos
delete currentControlerPos;
currentControlerPos = steamTracking->getTransformationForDevice(1);
getKinectData();
} //add points to point cloud
if (captureFrame) {
processCurrentFrameForExport();
captureFrame = false; captureFrame = false;
}
if (writeFile) { if (writeFile) {
writePointCloud(); writePointCloud();
...@@ -285,32 +297,43 @@ void drawKinectData() { ...@@ -285,32 +297,43 @@ void drawKinectData() {
glMatrixMode(GL_MODELVIEW); glMatrixMode(GL_MODELVIEW);
glLoadIdentity(); glLoadIdentity();
rotateCamera(); //rotateCamera(); //todo
if(currentControlerPos) mainCam.applyMatrix();
glMultMatrixf(glm::value_ptr(*currentControlerPos));
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnableClientState(GL_VERTEX_ARRAY); glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY); glEnableClientState(GL_COLOR_ARRAY);
glBindBuffer(GL_ARRAY_BUFFER, vboId);
glVertexPointer(3, GL_FLOAT, 0, NULL);
glBindBuffer(GL_ARRAY_BUFFER, cboId); //draw Grid
glColorPointer(3, GL_FLOAT, 0, NULL); glLineWidth(1.f);
drawElementArray(gridData, GL_LINES);
//draw stored Point cloud
//draw current Frame
if(currentControlerPos) //applay controler Transformation
glMultMatrixf(glm::value_ptr(*currentControlerPos));
glPointSize(1.f); glPointSize(1.f);
glDrawArrays(GL_POINTS, 0, width*height); drawElementArray(framePoints, GL_POINTS);
drawElementArray(arrowData, GL_TRIANGLES);
// draw hud arrow
glPushMatrix(); glPushMatrix();
float scale = 1.0f / 10; glTranslatef(0.0f, 1.5f, 0.0f);
float scale = 1.0f ;
glScalef(scale, scale, scale); glScalef(scale, scale, scale);
glDisableClientState(GL_COLOR_ARRAY);
//drawArrow(); //drawArrow();
glColor4f(1.0f, 0.0f, 0.0f, 1.0f); glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
glutSolidTeapot(1); glutSolidTeapot(1);
glPopMatrix(); glPopMatrix();
glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY); glDisableClientState(GL_COLOR_ARRAY);
...@@ -319,14 +342,35 @@ void drawKinectData() { ...@@ -319,14 +342,35 @@ void drawKinectData() {
void keyPressed(unsigned char key, int x, int y) void keyPressed(unsigned char key, int x, int y)
{ {
switch (key) { switch (key) {
case 'a': case 'y':
std::cout << "Caturing Frame Keyboard"<< std::endl; std::cout << "Capturing Frame Keyboard"<< std::endl;
captureFrame = true; captureFrame = true;
break; break;
case 's': case 'x':
std::cout << "writing File" << std::endl; std::cout << "writing File" << std::endl;
writeFile = true; writeFile = true;
break; break;
case 'w':
mainCam.move(glm::vec3(1.0f,0.0f,0.0f));
break;
case 's':
mainCam.move(glm::vec3(-1.0f, 0.0f, 0.0f));
break;
case 'a':
mainCam.move(glm::vec3(0.0f, 0.0f, -1.0f));
break;
case 'd':
mainCam.move(glm::vec3(0.0f, 0.0f, 1.0f));
break;
case 'q':
mainCam.move(glm::vec3(0.0f, 1.0f, 0.0f));
break;
case 'e':
mainCam.move(glm::vec3(0.0f, -1.0f, 0.0f));
break;
case 27: // Escape key
exit(0);
break;
default: default:
break; break;
} }
...@@ -335,6 +379,19 @@ void keyPressed(unsigned char key, int x, int y) ...@@ -335,6 +379,19 @@ void keyPressed(unsigned char key, int x, int y)
instantly */ instantly */
} }
void mouseButton(int button, int state, int x, int y) {
lastX = x;
lastY = y;
}
void mouseMove(int x, int y) {
mainCam.rotate(lastX-x, lastY-y);
lastX = x;
lastY = y;
}
void captureNextFrame(vr::VREvent_t event) void captureNextFrame(vr::VREvent_t event)
{ {
if (event.trackedDeviceIndex == 1 && event.data.controller.button == 33) { if (event.trackedDeviceIndex == 1 && event.data.controller.button == 33) {
...@@ -344,63 +401,28 @@ void captureNextFrame(vr::VREvent_t event) ...@@ -344,63 +401,28 @@ void captureNextFrame(vr::VREvent_t event)
} }
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
std::cout << "Starting Kinect VR Point Cloud creator"; std::cout << "Starting Kinect VR Point Cloud creator\n";
if (!init(argc, argv)) return 1; if (!init(argc, argv)) return 1;
if (!initKinect()) return 1; if (!initKinect()) return 1;
if (!initVR())
{
shutdownVR();
}
steamTracking = new SteamTracking(); steamTracking = new SteamTracking();
steamTracking->printVRDevices(); steamTracking->printVRDevices();
steamTracking->onButtonPress = captureNextFrame; steamTracking->onButtonPress = captureNextFrame;
// OpenGL setup // OpenGL setup
glClearColor(0,0,0,0); glClearColor(0,0,0,0);
glClearDepth(1.0f); glClearDepth(1.0f);
snapshotPoints.clear(); setupFrameBuffer();
snapshotColors.clear();
// Write color array for vertices
float* fdest = (float*)frameColors;
float* fdest2 = (float*)framePositions;
for (int j = 0; j < height; j++) {
for (int i = 0; i < width; i++) {
*fdest++ = 1.0f * i / width;
*fdest++ = 1.0f *j / height;
*fdest++ = 1;
snapshotPoints.push_back(glm::vec3(1.0 * i / width, 1.0 *j / height, 0));
snapshotColors.push_back((unsigned char) 1.0 *i / width * 255);
snapshotColors.push_back((unsigned char) 1.0 *j / height*255);
snapshotColors.push_back(0);
*fdest2++ = 1.0f *i / width;
*fdest2++ = 1.0f *j / height;;
*fdest2++ = 0;
}
}
// Set up array buffers
const int dataSize = width*height * 3 * 4;
glGenBuffers(1, &vboId);
glBindBuffer(GL_ARRAY_BUFFER, vboId);
glBufferData(GL_ARRAY_BUFFER, dataSize, framePositions, GL_DYNAMIC_DRAW);
glGenBuffers(1, &cboId);
glBindBuffer(GL_ARRAY_BUFFER, cboId);
glBufferData(GL_ARRAY_BUFFER, dataSize, frameColors, GL_DYNAMIC_DRAW);
setupGrid();
glGenBuffers(1, &exportData.vboID);
glGenBuffers(1, &exportData.cboID);
exportData.size = 0;
glUnmapBuffer(GL_ARRAY_BUFFER);
// Camera setup // Camera setup
glViewport(0, 0, width, height); glViewport(0, 0, width, height);
...@@ -409,22 +431,191 @@ int main(int argc, char* argv[]) { ...@@ -409,22 +431,191 @@ int main(int argc, char* argv[]) {
gluPerspective(45, width /(GLdouble) height, 0.1, 1000); gluPerspective(45, width /(GLdouble) height, 0.1, 1000);
glMatrixMode(GL_MODELVIEW); glMatrixMode(GL_MODELVIEW);
glLoadIdentity(); glLoadIdentity();
gluLookAt(0,0,0,0,0,1,0,1,0);
// Main loop // Main loop
atexit(close); atexit(close);
execute(); execute();
return 0;
}
void setupFrameBuffer()
{
{ // init depth Image
// Write color array for vertices
unsigned char* fcolor = (unsigned char*)frameColors;
glm::vec3* fvec3 = (glm::vec3*)framePositions;
for (int j = 0; j < height; j++) {
for (int i = 0; i < width; i++) {
*fvec3++ = glm::vec3(1.0 * i / width, 1.0 *j / height, 0);
*fcolor++ = (unsigned char)(1.0 * i / width * 255);
*fcolor++ = (unsigned char)(1.0 * j / height * 255);
*fcolor++ = 0;
}
}
return 0; // Set up array buffers
glGenBuffers(1, &framePoints.vboID);
glBindBuffer(GL_ARRAY_BUFFER, framePoints.vboID);
glBufferData(GL_ARRAY_BUFFER, width*height * sizeof(glm::vec3), framePositions, GL_DYNAMIC_DRAW);
glGenBuffers(1, &framePoints.cboID);
glBindBuffer(GL_ARRAY_BUFFER, framePoints.cboID);
glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(unsigned char), frameColors, GL_DYNAMIC_DRAW);
framePoints.size = width*height;
}
} }
void close() { void setupGrid()
delete steamTracking; {
{ //init Grid
std::vector<glm::vec3> grid;
std::vector<unsigned char> gridcolors;
for (int i = -20; i < 21; i++)
{
grid.push_back(glm::vec3(20, 0, i));
grid.push_back(glm::vec3(-20, 0, i));
gridcolors.push_back(255);
gridcolors.push_back(i == 0 ? 0 : 255);
gridcolors.push_back(i == 0 ? 0 : 255);
gridcolors.push_back(255);
gridcolors.push_back(i == 0 ? 0 : 255);
gridcolors.push_back(i == 0 ? 0 : 255);
}
for (int i = -20; i < 21; i++)
{
grid.push_back(glm::vec3(i, 0, 20));
grid.push_back(glm::vec3(i, 0, -20));
gridcolors.push_back(255);
gridcolors.push_back(i == 0 ? 0 : 255);
gridcolors.push_back(i == 0 ? 0 : 255);
gridcolors.push_back(255);
gridcolors.push_back(i == 0 ? 0 : 255);
gridcolors.push_back(i == 0 ? 0 : 255);
}
grid.push_back(glm::vec3(0, -20, 0));
grid.push_back(glm::vec3(0, 20, 0));
gridcolors.push_back(255);
gridcolors.push_back(0);
gridcolors.push_back(0);
gridcolors.push_back(255);
gridcolors.push_back(0);
gridcolors.push_back(0);
glGenBuffers(1, &gridData.vboID);
glBindBuffer(GL_ARRAY_BUFFER, gridData.vboID);
glBufferData(GL_ARRAY_BUFFER, grid.size() * sizeof(glm::vec3), &grid[0], GL_STATIC_DRAW);
glGenBuffers(1, &gridData.cboID);
glBindBuffer(GL_ARRAY_BUFFER, gridData.cboID);
glBufferData(GL_ARRAY_BUFFER, gridcolors.size() * sizeof(unsigned char), &gridcolors[0], GL_STATIC_DRAW);
gridData.size = grid.size();
}
} }
void drawGround() void setupArrow() {
{ static GLfloat g_vertex_buffer_data[] = {
-1.0f,-1.0f,-1.0f, // triangle 1 : begin
-1.0f,-1.0f, 1.0f,
-1.0f, 1.0f, 1.0f, // triangle 1 : end
1.0f, 1.0f,-1.0f, // triangle 2 : begin
-1.0f,-1.0f,-1.0f,
-1.0f, 1.0f,-1.0f, // triangle 2 : end
1.0f,-1.0f, 1.0f,
-1.0f,-1.0f,-1.0f,
1.0f,-1.0f,-1.0f,
1.0f, 1.0f,-1.0f,
1.0f,-1.0f,-1.0f,
-1.0f,-1.0f,-1.0f,
-1.0f,-1.0f,-1.0f,
-1.0f, 1.0f, 1.0f,
-1.0f, 1.0f,-1.0f,
1.0f,-1.0f, 1.0f,
-1.0f,-1.0f, 1.0f,
-1.0f,-1.0f,-1.0f,
-1.0f, 1.0f, 1.0f,
-1.0f,-1.0f, 1.0f,
1.0f,-1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f,-1.0f,-1.0f,
1.0f, 1.0f,-1.0f,
1.0f,-1.0f,-1.0f,
1.0f, 1.0f, 1.0f,
1.0f,-1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f,-1.0f,
-1.0f, 1.0f,-1.0f,
1.0f, 1.0f, 1.0f,
-1.0f, 1.0f,-1.0f,
-1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
-1.0f, 1.0f, 1.0f,
1.0f,-1.0f, 1.0f
};
static const GLfloat g_color_buffer_data[] = {
0.583f, 0.771f, 0.014f,
0.609f, 0.115f, 0.436f,
0.327f, 0.483f, 0.844f,
0.822f, 0.569f, 0.201f,
0.435f, 0.602f, 0.223f,
0.310f, 0.747f, 0.185f,
0.597f, 0.770f, 0.761f,
0.559f, 0.436f, 0.730f,
0.359f, 0.583f, 0.152f,
0.483f, 0.596f, 0.789f,
0.559f, 0.861f, 0.639f,
0.195f, 0.548f, 0.859f,
0.014f, 0.184f, 0.576f,
0.771f, 0.328f, 0.970f,
0.406f, 0.615f, 0.116f,
0.676f, 0.977f, 0.133f,
0.971f, 0.572f, 0.833f,
0.140f, 0.616f, 0.489f,
0.997f, 0.513f, 0.064f,
0.945f, 0.719f, 0.592f,
0.543f, 0.021f, 0.978f,
0.279f, 0.317f, 0.505f,
0.167f, 0.620f, 0.077f,
0.347f, 0.857f, 0.137f,
0.055f, 0.953f, 0.042f,
0.714f, 0.505f, 0.345f,
0.783f, 0.290f, 0.734f,
0.722f, 0.645f, 0.174f,
0.302f, 0.455f, 0.848f,
0.225f, 0.587f, 0.040f,
0.517f, 0.713f, 0.338f,
0.053f, 0.959f, 0.120f,
0.393f, 0.621f, 0.362f,
0.673f, 0.211f, 0.457f,
0.820f, 0.883f, 0.371f,
0.982f, 0.099f, 0.879f
};
std::vector<unsigned char> gridcolors;
for (int i = 0; i < 108; i++)
{
gridcolors.push_back((unsigned char)(g_color_buffer_data[i]*255));
}
glGenBuffers(1, &arrowData.vboID);
glBindBuffer(GL_ARRAY_BUFFER, gridData.vboID);
glBufferData(GL_ARRAY_BUFFER, 108* sizeof(float), g_vertex_buffer_data, GL_STATIC_DRAW);
glGenBuffers(1, &gridData.cboID);
glBindBuffer(GL_ARRAY_BUFFER, gridData.cboID);
glBufferData(GL_ARRAY_BUFFER, gridcolors.size() * sizeof(unsigned char), &gridcolors[0], GL_STATIC_DRAW);
arrowData.size = 108;
}
void close() {
delete steamTracking;
} }
3_PointCloud/main.h
#pragma once #pragma once
#include <openvr.h> #include <openvr.h>
const int width = 512; const int width = 512;
const int height = 424; const int height = 424;
const int colorwidth = 1920; const int colorwidth = 1920;
const int colorheight = 1080; const int colorheight = 1080;
void drawKinectData(); void mainRenderLoop();
void keyPressed(unsigned char key, int x, int y); void keyPressed(unsigned char key, int x, int y);
void mouseButton(int button, int state, int x, int y);
void mouseMove(int x, int y);
void captureNextFrame(vr::VREvent_t event); void captureNextFrame(vr::VREvent_t event);
void writePointCloud(); void writePointCloud();
void close(); void close();
void drawGround();
void setupGrid();
void setupFrameBuffer();
struct pntsHeader { struct pntsHeader {
......
3_PointCloud/quaternion_utils.cpp 0 → 100644
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/quaternion.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/norm.hpp>
#include "quaternion_utils.h"
// Returns a quaternion such that q*start = dest
quat RotationBetweenVectors(vec3 start, vec3 dest) {
start = normalize(start);
dest = normalize(dest);
float cosTheta = dot(start, dest);
vec3 rotationAxis;
if (cosTheta < -1 + 0.001f) {
// special case when vectors in opposite directions :
// there is no "ideal" rotation axis
// So guess one; any will do as long as it's perpendicular to start
// This implementation favors a rotation around the Up axis,
// since it's often what you want to do.
rotationAxis = cross(vec3(0.0f, 0.0f, 1.0f), start);
if (length2(rotationAxis) < 0.01) // bad luck, they were parallel, try again!
rotationAxis = cross(vec3(1.0f, 0.0f, 0.0f), start);
rotationAxis = normalize(rotationAxis);
return angleAxis(180.0f, rotationAxis);
}
// Implementation from Stan Melax's Game Programming Gems 1 article
rotationAxis = cross(start, dest);
float s = sqrt((1 + cosTheta) * 2);
float invs = 1 / s;
return quat(
s * 0.5f,
rotationAxis.x * invs,
rotationAxis.y * invs,
rotationAxis.z * invs
);
}
// Returns a quaternion that will make your object looking towards 'direction'.
// Similar to RotationBetweenVectors, but also controls the vertical orientation.
// This assumes that at rest, the object faces +Z.
// Beware, the first parameter is a direction, not the target point !
quat LookAt(vec3 direction, vec3 desiredUp) {
if (length2(direction) < 0.0001f)
return quat();
// Recompute desiredUp so that it's perpendicular to the direction
// You can skip that part if you really want to force desiredUp
vec3 right = cross(direction, desiredUp);
desiredUp = cross(right, direction);
// Find the rotation between the front of the object (that we assume towards +Z,
// but this depends on your model) and the desired direction
quat rot1 = RotationBetweenVectors(vec3(0.0f, 0.0f, 1.0f), direction);
// Because of the 1rst rotation, the up is probably completely screwed up.
// Find the rotation between the "up" of the rotated object, and the desired up
vec3 newUp = rot1 * vec3(0.0f, 1.0f, 0.0f);
quat rot2 = RotationBetweenVectors(newUp, desiredUp);
// Apply them
return rot2 * rot1; // remember, in reverse order.
}
// Like SLERP, but forbids rotation greater than maxAngle (in radians)
// In conjunction to LookAt, can make your characters
quat RotateTowards(quat q1, quat q2, float maxAngle) {
if (maxAngle < 0.001f) {
// No rotation allowed. Prevent dividing by 0 later.
return q1;
}
float cosTheta = dot(q1, q2);
// q1 and q2 are already equal.
// Force q2 just to be sure
if (cosTheta > 0.9999f) {
return q2;
}
// Avoid taking the long path around the sphere
if (cosTheta < 0) {
q1 = q1*-1.0f;
cosTheta *= -1.0f;
}
float angle = acos(cosTheta);
// If there is only a 2� difference, and we are allowed 5�,
// then we arrived.
if (angle < maxAngle) {
return q2;
}
// This is just like slerp(), but with a custom t
float t = maxAngle / angle;
angle = maxAngle;
quat res = (sin((1.0f - t) * angle) * q1 + sin(t * angle) * q2) / sin(angle);
res = normalize(res);
return res;
}
void tests() {
glm::vec3 Xpos(+1.0f, 0.0f, 0.0f);
glm::vec3 Ypos(0.0f, +1.0f, 0.0f);
glm::vec3 Zpos(0.0f, 0.0f, +1.0f);
glm::vec3 Xneg(-1.0f, 0.0f, 0.0f);
glm::vec3 Yneg(0.0f, -1.0f, 0.0f);
glm::vec3 Zneg(0.0f, 0.0f, -1.0f);
// Testing standard, easy case
// Must be 90� rotation on X : 0.7 0 0 0.7
quat X90rot = RotationBetweenVectors(Ypos, Zpos);
// Testing with v1 = v2
// Must be identity : 0 0 0 1
quat id = RotationBetweenVectors(Xpos, Xpos);
// Testing with v1 = -v2
// Must be 180� on +/-Y axis : 0 +/-1 0 0
quat Y180rot = RotationBetweenVectors(Xpos, Xneg);
// Testing with v1 = -v2, but with a "bad first guess"
// Must be 180� on +/-Y axis : 0 +/-1 0 0
quat X180rot = RotationBetweenVectors(Zpos, Zneg);
}
\ No newline at end of file
3_PointCloud/quaternion_utils.h 0 → 100644
#pragma once
//https://github.com/opengl-tutorials/ogl/blob/master/common/quaternion_utils.hpp
#include <glm/gtx/quaternion.hpp>
using namespace glm;
quat RotationBetweenVectors(vec3 start, vec3 dest);
quat LookAt(vec3 direction, vec3 desiredUp);
quat RotateTowards(quat q1, quat q2, float maxAngle);
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment