6
« on: April 23, 2014, 11:42:13 AM »
How does one rotate a point in 3D space without gimbal lock?
All I have encountered is to use Quaternions so here is what I have tried.
#include <iostream>
#include <GLFW/glfw3.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#define PI 3.14159265
double getTime() {
return glfwGetTime();
}
float lastTime = glfwGetTime();
float getDelta() {
float time = getTime();
float delta = (time - lastTime);
lastTime = getTime();
return delta;
}
struct vector3f{
float x,y,z;
};
struct quaternion{
float x,y,z,w;
};
float getqlength(quaternion q1) {
return sqrt(q1.x*q1.x+q1.y*q1.y+q1.z*q1.z+q1.w*q1.w);
}
quaternion normalizeq(quaternion q1) {
quaternion nq;
float q1length = getqlength(q1);
nq.x = q1.x / q1length;
nq.y = q1.y / q1length;
nq.z = q1.z / q1length;
nq.w = q1.w / q1length;
return nq;
}
quaternion conjugateq(quaternion q1) {
quaternion nq;
nq.x = -q1.x;
nq.y = -q1.y;
nq.z = -q1.z;
nq.w = q1.w;
return nq;
}
quaternion mulq(quaternion q1, quaternion q2) {
quaternion nq;
nq.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
nq.w = q1.x * q2.w + q1.w * q2.x + q1.y * q2.z - q1.z * q2.y;
nq.w = q1.y * q2.w + q1.w * q2.y + q1.z * q2.x - q1.x * q2.z;
nq.w = q1.z * q2.w + q1.w * q2.z + q1.x * q2.y - q1.y * q2.x;
return nq;
}
quaternion mulqv(quaternion q1, vector3f v) {
quaternion nq;
nq.w = -q1.x * v.x - q1.y * v.y - q1.z * v.z;
nq.w = q1.w * v.x + q1.y * v.z - q1.z * v.y;
nq.w = q1.w * v.y + q1.z * v.x - q1.x * v.z;
nq.w = q1.w * v.z + q1.x * v.y - q1.y * v.x;
return nq;
}
vector3f rotatev(vector3f v, vector3f axis, float angle) {
vector3f nv;
float sha = sin( (angle / 2)*(PI / 180));
float cha = cos( (angle / 2)*(PI / 180));
quaternion rotation;
rotation.x = axis.x * sha;
rotation.y = axis.y * sha;
rotation.z = axis.z * sha;
rotation.w = cha;
quaternion w = mulq(mulqv(rotation,v),conjugateq(rotation));
std::cout << w.x << "," << w.y << "," << w.z << " " << angle << " " << sha << " " << cha << "\n";
nv.x = w.x;
nv.y = w.y;
nv.z = w.z;
return nv;
}
vector3f p1;
vector3f p2;
vector3f pa;
float pang;
void init() {
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
glFrontFace(GL_CCW);
glCullFace(GL_BACK);
glEnable(GL_CULL_FACE);
glEnable(GL_DEPTH_TEST);
p1.x = 0.0;
p1.y = 0.0;
p1.z = 0.0;
p2.x = 0.0;
p2.y = 0.2;
p2.z = 0.0;
pa.x = 0.0;
pa.y = 0.0;
pa.z = 1.0;
pang = 0;
}
void update(float delta) {
pang += 90 * delta;
if(pang > 360) {
pang -= 360;
}
p2 = rotatev(p2,pa,pang);
}
void render() {
glBegin(GL_POINTS);
glColor3f(0.0, 1.0, 0.0);
glVertex3f(p1.x, p1.y, p1.z);
glVertex3f(p2.x, p2.y, p2.z);
glEnd();
}
static void error_callback(int error, const char* description) {
fputs(description, stderr);
}
static void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods) {
if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS)
glfwSetWindowShouldClose(window, GL_TRUE);
}
int main(void) {
GLFWwindow* window;
glfwSetErrorCallback(error_callback);
if (!glfwInit())
exit(EXIT_FAILURE);
window = glfwCreateWindow(640, 480, "", NULL, NULL);
if (!window)
{
glfwTerminate();
exit(EXIT_FAILURE);
}
glfwMakeContextCurrent(window);
glfwSetKeyCallback(window, key_callback);
init();
while (!glfwWindowShouldClose(window))
{
float ratio;
int width, height;
glfwGetFramebufferSize(window, &width, &height);
ratio = width / (float) height;
glViewport(0, 0, width, height);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-ratio, ratio, -1.f, 1.f, 1.f, -1.f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
update(getDelta());
render();
glfwSwapBuffers(window);
glfwPollEvents();
}
glfwDestroyWindow(window);
glfwTerminate();
exit(EXIT_SUCCESS);
}So to my understanding that should give me a window with two points, one rotating around the other.
But, all i'm getting is one dot and the location of another point at 4.61497e+033,1.26933e-038,3.76462e-039 apparently...
What have I done wrong and/or how do I manage 3D rotation?
Show Posts