Skip to content

Instantly share code, notes, and snippets.

@mmozeiko

mmozeiko/bench.c Secret

Last active Aug 4, 2017
Embed
What would you like to do?
quaternion vs gibbs
#include <stdio.h>
#include <math.h>
#include <windows.h>
#define deg(x) ((x) * 57.295779513)
#define rad(x) ((x) * 0.017453292)
typedef struct {
float x, y, z;
} v3;
typedef struct {
float x, y, z, w;
} v4;
static inline v3 neg(v3 v) {
return (v3){-v.x, -v.y, -v.z};
}
static inline float length(v3 v) {
return sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
}
static inline v3 normalize(v3 v) {
float l = length(v);
if (l != 0) {
v.x /= l;
v.y /= l;
v.z /= l;
}
return v;
}
static inline v4 qmul(v4 a, v4 b) {
v4 o = {
a.x * b.w + b.x * a.w + (a.y * b.z - b.y * a.z),
a.y * b.w + b.y * a.w + (a.z * b.x - b.z * a.x),
a.z * b.w + b.z * a.w + (a.x * b.y - b.x * a.y),
a.w * b.w - (a.x * b.x + a.y * b.y + a.z * b.z)
};
return o;
}
#ifdef _MSC_VER
#define unlikely(x) (x)
#else
#define unlikely(x) __builtin_expect(!!(x), 0)
#endif
static inline v3 gmul(v3 a, v3 b) {
float dot = (a.x * b.x + a.y * b.y + a.z * b.z);
if (unlikely(dot==1.0f)) return (v3){0,0,0}; /* a == b */
float w = 1.0f/(1.0f - dot);
return (v3){
(a.x + b.x + (a.y * b.z - b.y * a.z)) * w,
(a.y + b.y + (a.z * b.x - b.z * a.x)) * w,
(a.z + b.z + (a.x * b.y - b.x * a.y)) * w,
};
}
static inline v3 qrot(v3 v, v4 q) {
v4 o = qmul(qmul(q, (v4){v.x, v.y, v.z, 0}), (v4){-q.x, -q.y, -q.z, q.w});
return (v3){o.x, o.y, o.z};
}
static inline v3 grot(v3 v, v3 g) {
v3 o = gmul(gmul(g, v), (v3){-g.x, -g.y, -g.z});
return o;
}
static inline v4 qaxisrad(v3 axis, float rad) {
float s = (float)sin(rad / 2.0);
float c = (float)cos(rad / 2.0);
v4 o;
o.x = s * axis.x;
o.y = s * axis.y;
o.z = s * axis.z;
o.w = c;
return o;
}
static inline v3 gaxisrad(v3 axis, float rad) {
float t = (float)tan(rad / 2.0);
v3 o;
o.x = t * axis.x;
o.y = t * axis.y;
o.z = t * axis.z;
return o;
}
static inline v3 qtog(v4 q) {
return (v3){
q.x / q.w,
q.y / q.w,
q.z / q.w
};
}
static inline v3 qaxis(v4 q) {
return normalize((v3){q.x, q.y, q.z});
}
static inline v3 gaxis(v3 g) {
return normalize(g);
}
static inline float qrad(v4 q) {
return atan2(length((v3){q.x, q.y, q.z}), q.w) * 2.0;
}
static inline float grad(v3 g) {
return atan(length(g)) * 2.0;
}
#define COUNT (16*1024*1024)
int main() {
v3 g;
v4 q;
static v3 src[COUNT];
v3 axis = normalize((v3){
((float)rand() / RAND_MAX) * 2.f - 1.f,
((float)rand() / RAND_MAX) * 2.f - 1.f,
((float)rand() / RAND_MAX) * 2.f - 1.f,
});
float angle = rad(((float)rand() / RAND_MAX) * 360.f);
g = gaxisrad(axis, angle);
q = qaxisrad(axis, angle);
for (int i=0; i<COUNT; i++)
{
src[i].x = ((float)rand() / RAND_MAX) * 2.f - 1.f;
src[i].y = ((float)rand() / RAND_MAX) * 2.f - 1.f;
src[i].z = ((float)rand() / RAND_MAX) * 2.f - 1.f;
}
volatile float tmp = 0.f;
LARGE_INTEGER f, t1, t2, t3;
QueryPerformanceFrequency(&f);
for (int k=0; k<2; k++)
{
QueryPerformanceCounter(&t1);
for (int i=0; i<COUNT; i++)
{
v3 dst = qrot(src[i], q);
tmp += dst.x + dst.y + dst.z;
}
QueryPerformanceCounter(&t2);
for (int i=0; i<COUNT; i++)
{
v3 dst = grot(src[i], g);
tmp += dst.x + dst.y + dst.z;
}
QueryPerformanceCounter(&t3);
}
printf("quats = %.2fmsec\n", 1000.f * (t2.QuadPart - t1.QuadPart) / f.QuadPart);
printf("gibbs = %.2fmsec\n", 1000.f * (t3.QuadPart - t2.QuadPart) / f.QuadPart);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment