void qrot_vec(vec dest, quat *q, vec x) {
quat qc, qx, tmpdest;
memcpy(&qc, q, sizeof(quat));
qconj(&qc);
qx.s = 0;
memcpy(qx.v, x, sizeof(vec));
qmul(&tmpdest, q, &qx);
qmul(&qx, &tmpdest, &qc);
memcpy(dest, qx.v, sizeof(vec));
}
void qinverse(quat *dest, quat *q) {
memcpy(dest, q, sizeof(quat));
qconj(dest);
vmul(dest->v, 1.0/qlengthsquare(q));
}
int main(int argc, char *argv[]) try
{
std::cout << "TestDQ\n";
Pose camera = { { 0, 0, 8 }, { 0, 0, 0, 1 } };
bool showaxis = true;
float3 focuspoint(0, 0, 0);
float3 mousevec_prev;
float4 model_orientation(0, 0, 0, 1);
Pose p0 = { { -3, 0, 0 }, { 0, 0, 0, 1 } };
Pose p1 = { { 3, 0, 0 }, { 0, 0, sqrtf(0.5f),sqrtf(0.5f) } };
float dt = 0.01f, t = 0;
Pose *selected = NULL;
std::vector<float4> planes = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { -1, 0, 0, 0 }, { 0, -1, 0, 0 }, { 0, 0, -1, 0 } };
for (auto &p : planes)
p.w = -0.25f;
GLWin glwin("Dual Quaternion Pose Interpolation");
glwin.keyboardfunc = [&](int key, int, int)
{
showaxis = key == 'a' != showaxis;
};
while (glwin.WindowUp())
{
t = t + dt; // advance our global time t is in 0..1
if (t > 1.0f)
t = 0.0f;
Pose pt = dqinterp(p0,p1,t); // And here we show our dual quaterion usage
// some extras to help visualize the axis of rotation, not the best math to get the result, but oh well
float4 aq = qmul(dot(p0.orientation, p1.orientation) < 0 ? -p1.orientation : p1.orientation, qconj(p0.orientation));
float3 axis = normalize(aq.xyz()*(aq.w < 0 ? -1.0f : 1.0f)); // direction of the axis of rotation
float3 axisp = cross(axis, p1.position - p0.position) / 2.0f * sqrtf(1/dot(aq.xyz(),aq.xyz())-1); // origin projected onto the axis of rotation
// user interaction:
float3 ray = qrot(camera.orientation, normalize(glwin.MouseVector)); // for mouse selection
float3 v1 = camera.position + ray*100.0f;
if (!glwin.MouseState) // note that we figure out what is being selected only when the mouse is up
{
selected = NULL;
for (Pose *p : { &p0, &p1 })
{
if (auto h = ConvexHitCheck(planes, *p, camera.position, v1))
{
selected = p;
v1 = h.impact;
}
}
}
else // if (glwin.MouseState)
{
if (selected)
selected->orientation = qmul(VirtualTrackBall(camera.position, selected->position, qrot(camera.orientation, mousevec_prev), qrot(camera.orientation, glwin.MouseVector)), selected->orientation);
else
camera.orientation = qmul(camera.orientation, qconj(VirtualTrackBall(float3(0, 0, 1), float3(0, 0, 0), mousevec_prev, glwin.MouseVector))); // equation is non-typical we are orbiting the camera, not rotating the object
}
camera.position = focuspoint + qzdir(camera.orientation)*magnitude(camera.position - focuspoint);
camera.position -= focuspoint;
camera.position *= powf(1.1f, (float)glwin.mousewheel);
camera.position += focuspoint;
mousevec_prev = glwin.MouseVector;
// Render the scene
glPushAttrib(GL_ALL_ATTRIB_BITS);
glViewport(0, 0, glwin.Width, glwin.Height); // Set up the viewport
glClearColor(0.1f, 0.1f, 0.15f, 1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glPushMatrix(); glLoadIdentity();
gluPerspective(glwin.ViewAngle, (double)glwin.Width / glwin.Height, 0.25, 250);
glMatrixMode(GL_MODELVIEW);
glPushMatrix(); glLoadIdentity();
glMultMatrixf(camera.Inverse().Matrix());
glDisable(GL_LIGHTING);
glAxis();
glGridxy(4.0f);
if (showaxis)
{
glPushAttrib(GL_ALL_ATTRIB_BITS);
glLineWidth(3.0f);
glBegin(GL_LINES);
glColor3f(1, 1, 1);
for (auto p : { p0.position, p1.position, pt.position ,axisp})
glVertex3fv(p - axis*0.5f), glVertex3fv(p + axis*0.5f); // note the comma
glEnd();
glPopAttrib();
glColor3f(1, 1, 0);
glBegin(GL_LINES);
glVertex3fv(axisp + axis*dot(axis, p0.position)), glVertex3fv(axisp + axis*dot(axis, p1.position));
glVertex3fv(axisp + axis*dot(axis, p0.position)), glVertex3fv(p0.position);
glVertex3fv(axisp + axis*dot(axis, p1.position)), glVertex3fv(p1.position);
glVertex3fv(axisp + axis*dot(axis, pt.position)), glVertex3fv(pt.position);
glEnd();
}
glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_COLOR_MATERIAL);
for (auto p : { p0, p1, pt })
glcolorbox(0.25f, p);
glPopMatrix(); //should be currently in modelview mode
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopAttrib();// Restore state
glwin.PrintString({ 0, 0 }, "ESC to quit.");
glwin.PrintString({ 0, 1 }, "'a' show axis (%s)", showaxis ? "on" : "off");
glwin.PrintString({ 0, 2 }, "%selected: %s", glwin.MouseState?"S":"s", (selected) ? ((selected==&p0)?"box0":"box1") : "none");
glwin.SwapBuffers();
}
std::cout << "\n";
return 0;
}
catch (std::exception e)
{
std::cerr << e.what() << "\n";
}
int main(int argc, char *argv[])
{
(void)argc; (void)argv;
#ifdef __SSE__
printf("SSE ");
#endif
#ifdef __SSE2__
printf("SSE2 ");
#endif
#ifdef __SSE3__
printf("SSE3 ");
#endif
#ifdef __SSE4__
printf("SSE4 ");
#endif
#ifdef __SSE4_1__
printf("SSE4.1 ");
#endif
#ifdef __SSE4_2__
printf("SSE4.2 ");
#endif
#ifdef __AVX__
printf("AVX ");
#endif
#ifdef __FMA4__
printf("FMA4 ");
#endif
printf("\n");
printv(vec(1, 2, 3, 4));
printv(vzero());
printm(mzero());
printm(midentity());
vec4 a = { 1, 2, 3, 4 }, b = { 5, 6, 7, 8 };
printv(a);
printv(b);
printf("\nshuffles:\n");
printv(vshuffle(a, a, 0, 1, 2, 3));
printv(vshuffle(a, a, 3, 2, 1, 0));
printv(vshuffle(a, b, 0, 1, 0, 1));
printv(vshuffle(a, b, 2, 3, 2, 3));
printf("\ndot products:\n");
printv(vdot(a, b));
printv(vdot(b, a));
printv(vdot3(a, b));
printv(vdot3(b, a));
//vec4 blendmask = { 1, -1, 1, -1 };
//printv(vblend(x, y, blendmask));
vec4 x = { 1, 0, 0, 0 }, y = { 0, 1, 0, 0 }, z = { 0, 0, 1, 0 }, w = { 0, 0, 0, 1 };
printf("\ncross products:\n");
printv(vcross(x, y));
printv(vcross(y, x));
printv(vcross_scalar(x, y));
printv(vcross_scalar(y, x));
printf("\nquaternion products:\n");
printv(qprod(x, y));
printv(qprod(y, x));
printv(qprod_mad(x, y));
printv(qprod_mad(y, x));
printv(qprod_scalar(x, y));
printv(qprod_scalar(y, x));
printf("\nquaternion conjugates:\n");
printv(qconj(x));
printv(qconj(y));
printv(qconj(z));
printv(qconj(w));
printf("\nmat from quat:\n");
printm(quat_to_mat(w));
printm(quat_to_mat_mmul(w));
printm(quat_to_mat_scalar(w));
vec4 angles = { 0.0, 0.0, 0.0, 0.0 };
printf("\neuler to quaternion:\n");
printv(quat_euler(angles));
printv(quat_euler_scalar(angles));
printv(quat_euler_gems(angles));
printf("\neuler to matrix:\n");
printm(mat_euler(angles));
printm(mat_euler_scalar(angles));
printm(quat_to_mat(quat_euler(angles)));
printf("\nperspective matrix:\n");
printm(mat_perspective_fovy(M_PI/4.0, 16.0/9.0, 0.1, 100.0));
printm(mat_perspective_fovy_inf_z(M_PI/4.0, 16.0/9.0, 0.1));
printm(mat_perspective_fovy_scalar(M_PI/4.0, 16.0/9.0, 0.1, 100.0));
printf("\northogonal matrix:\n");
printm(mat_ortho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0));
printm(mat_ortho(-1.0, 2.0, -1.0, 2.0, -1.0, 2.0));
printf("\ntranslate matrix:\n");
printm(mtranslate(a));
printf("\nscale matrix:\n");
printm(mscale(a));
return 0;
}
Pose dqpose(const float4x2 &d) { return{ qmul(d[1], qconj(d[0])).xyz()*2.0f, d[0] }; }
