aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Makefile20
-rw-r--r--include/libsurvive/survive.h1
-rw-r--r--redist/linmath.c339
-rw-r--r--redist/linmath.h71
-rw-r--r--redist/os_generic.c337
-rw-r--r--redist/os_generic.h369
-rw-r--r--src/poser.c7
-rw-r--r--src/poser_epnp.c16
-rw-r--r--src/poser_sba.c33
-rw-r--r--src/survive_charlesbiguator.c35
-rw-r--r--src/survive_sensor_activations.c4
-rw-r--r--tools/lightlengthparams/Makefile15
-rw-r--r--tools/lightlengthparams/lightlengthparams.c112
-rw-r--r--tools/viz/survive_viewer.js19
14 files changed, 699 insertions, 679 deletions
diff --git a/Makefile b/Makefile
index a6d139b..f95ca61 100644
--- a/Makefile
+++ b/Makefile
@@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test
CC?=gcc
-CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable
+CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable
CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm
@@ -35,7 +35,7 @@ endif
SBA:=redist/sba/sba_chkjac.o redist/sba/sba_crsm.o redist/sba/sba_lapack.o redist/sba/sba_levmar.o redist/sba/sba_levmar_wrap.o
POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o src/poser_epnp.o src/poser_sba.o
-REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o redist/minimal_opencv.o
+REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_opencv.o
ifeq ($(UNAME), Darwin)
REDISTS:=$(REDISTS) redist/hid-osx.c
endif
@@ -66,23 +66,23 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
testCocoa : testCocoa.c
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-test : test.c ./lib/libsurvive.so redist/os_generic.o
+test : test.c ./lib/libsurvive.so
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-simple_pose_test : simple_pose_test.c ./lib/libsurvive.so redist/os_generic.o $(DRAWFUNCTIONS)
+simple_pose_test : simple_pose_test.c ./lib/libsurvive.so $(DRAWFUNCTIONS)
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-data_recorder : data_recorder.c ./lib/libsurvive.so redist/os_generic.c
+data_recorder : data_recorder.c ./lib/libsurvive.so
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-calibrate : calibrate.c ./lib/libsurvive.so redist/os_generic.c $(DRAWFUNCTIONS)
+calibrate : calibrate.c ./lib/libsurvive.so $(DRAWFUNCTIONS)
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
-calibrate_client : calibrate_client.c ./lib/libsurvive.so redist/os_generic.c $(GRAPHICS_LOFI)
+calibrate_client : calibrate_client.c ./lib/libsurvive.so $(GRAPHICS_LOFI)
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS)
## Still not working!!! Don't use.
-static_calibrate : calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) $(LIBSURVIVE_C)
+static_calibrate : calibrate.c $(DRAWFUNCTIONS) $(LIBSURVIVE_C)
tcc -o $@ $^ $(CFLAGS) $(LDFLAGS) -DTCC
./redist/dclhelpers_debuggable.c : ./redist/dclhelpers.c ./redist/dclhelpers.h ./redist/dclapack.h
@@ -91,7 +91,7 @@ static_calibrate : calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) $(LIBSURVIVE
sed -i 's/#/\/\/#/g' ./redist/dclhelpers_debuggable.c
-test_dcl: ./redist/test_dcl.c ./redist/dclhelpers.c ./redist/dclhelpers.h ./redist/dclapack.h redist/os_generic.c ./redist/minimal_opencv.c ./src/epnp/epnp.c
+test_dcl: ./redist/test_dcl.c ./redist/dclhelpers.c ./redist/dclhelpers.h ./redist/dclapack.h ./redist/minimal_opencv.c ./src/epnp/epnp.c
$(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS_RELEASE) -DFLT=double
test_dcl_debug: ./redist/test_dcl.c ./redist/dclhelpers_debuggable.c ./redist/dclhelpers.h ./redist/dclapack.h redist/os_generic.c
@@ -114,7 +114,7 @@ lib/libsurvive.so : $(LIBSURVIVE_O)
calibrate_tcc : $(LIBSURVIVE_C)
- tcc -DRUNTIME_SYMNUM $(CFLAGS) -o $@ $^ $(LDFLAGS) calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) redist/symbol_enumerator.c
+ tcc -DRUNTIME_SYMNUM $(CFLAGS) -o $@ $^ $(LDFLAGS) calibrate.c $(DRAWFUNCTIONS) redist/symbol_enumerator.c
clean :
rm -rf */*/*.o *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so test_minimal_cv test_epnp test_epnp_ocv calibrate_client redist/*.o redist/*~ tools/data_server/data_server tools/lighthousefind/lighthousefind tools/lighthousefind_tori/lighthousefind-tori tools/plot_lighthouse/plot_lighthouse tools/process_rawcap/process_to_points redist/jsmntest redist/lintest
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index b1c32cd..8a1474b 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -16,6 +16,7 @@ extern "C" {
typedef struct {
FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space)
uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks
+ uint32_t lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks
FLT accel[3];
FLT gyro[3];
diff --git a/redist/linmath.c b/redist/linmath.c
index d005074..c57410f 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -1,130 +1,120 @@
-//Copyright 2013,2016 <>< C. N. Lohr. This file licensed under the terms of the MIT license.
+// Copyright 2013,2016 <>< C. N. Lohr. This file licensed under the terms of the MIT license.
-#include <math.h>
#include "linmath.h"
#include <float.h>
+#include <math.h>
#include <string.h>
-void cross3d( FLT * out, const FLT * a, const FLT * b )
-{
- out[0] = a[1]*b[2] - a[2]*b[1];
- out[1] = a[2]*b[0] - a[0]*b[2];
- out[2] = a[0]*b[1] - a[1]*b[0];
+void cross3d(FLT *out, const FLT *a, const FLT *b) {
+ out[0] = a[1] * b[2] - a[2] * b[1];
+ out[1] = a[2] * b[0] - a[0] * b[2];
+ out[2] = a[0] * b[1] - a[1] * b[0];
}
-void sub3d( FLT * out, const FLT * a, const FLT * b )
-{
+void sub3d(FLT *out, const FLT *a, const FLT *b) {
out[0] = a[0] - b[0];
out[1] = a[1] - b[1];
out[2] = a[2] - b[2];
}
-void add3d( FLT * out, const FLT * a, const FLT * b )
-{
+void add3d(FLT *out, const FLT *a, const FLT *b) {
out[0] = a[0] + b[0];
out[1] = a[1] + b[1];
out[2] = a[2] + b[2];
}
-void scale3d( FLT * out, const FLT * a, FLT scalar )
-{
+void scale3d(FLT *out, const FLT *a, FLT scalar) {
out[0] = a[0] * scalar;
out[1] = a[1] * scalar;
out[2] = a[2] * scalar;
}
-void normalize3d( FLT * out, const FLT * in )
-{
+void normalize3d(FLT *out, const FLT *in) {
FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);
out[0] = in[0] * r;
out[1] = in[1] * r;
out[2] = in[2] * r;
}
-FLT dot3d( const FLT * a, const FLT * b )
-{
- return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
-}
-
-int compare3d( const FLT * a, const FLT * b, FLT epsilon )
-{
- if( !a || !b ) return 0;
- if( a[2] - b[2] > epsilon ) return 1;
- if( b[2] - a[2] > epsilon ) return -1;
- if( a[1] - b[1] > epsilon ) return 1;
- if( b[1] - a[1] > epsilon ) return -1;
- if( a[0] - b[0] > epsilon ) return 1;
- if( b[0] - a[0] > epsilon ) return -1;
+FLT dot3d(const FLT *a, const FLT *b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }
+
+int compare3d(const FLT *a, const FLT *b, FLT epsilon) {
+ if (!a || !b)
+ return 0;
+ if (a[2] - b[2] > epsilon)
+ return 1;
+ if (b[2] - a[2] > epsilon)
+ return -1;
+ if (a[1] - b[1] > epsilon)
+ return 1;
+ if (b[1] - a[1] > epsilon)
+ return -1;
+ if (a[0] - b[0] > epsilon)
+ return 1;
+ if (b[0] - a[0] > epsilon)
+ return -1;
return 0;
}
-void copy3d( FLT * out, const FLT * in )
-{
+void copy3d(FLT *out, const FLT *in) {
out[0] = in[0];
out[1] = in[1];
out[2] = in[2];
}
-FLT magnitude3d(const FLT * a )
-{
- return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
-}
+FLT magnitude3d(const FLT *a) { return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); }
-FLT anglebetween3d( FLT * a, FLT * b )
-{
+FLT anglebetween3d(FLT *a, FLT *b) {
FLT an[3];
FLT bn[3];
- normalize3d( an, a );
- normalize3d( bn, b );
+ normalize3d(an, a);
+ normalize3d(bn, b);
FLT dot = dot3d(an, bn);
- if( dot < -0.9999999 ) return LINMATHPI;
- if( dot > 0.9999999 ) return 0;
+ if (dot < -0.9999999)
+ return LINMATHPI;
+ if (dot > 0.9999999)
+ return 0;
return FLT_ACOS(dot);
}
// algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/
-void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle)
-{
+void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) {
// TODO: this really should be external.
normalize3d(axis, axis);
FLT s = FLT_SIN(angle);
FLT c = FLT_COS(angle);
- FLT u=axis[0];
- FLT v=axis[1];
- FLT w=axis[2];
+ FLT u = axis[0];
+ FLT v = axis[1];
+ FLT w = axis[2];
- FLT x=invec3[0];
- FLT y=invec3[1];
- FLT z=invec3[2];
+ FLT x = invec3[0];
+ FLT y = invec3[1];
+ FLT z = invec3[2];
- outvec3[0] = u*(u*x + v*y + w*z)*(1-c) + x*c + (-w*y + v*z)*s;
- outvec3[1] = v*(u*x + v*y + w*z)*(1-c) + y*c + ( w*x - u*z)*s;
- outvec3[2] = w*(u*x + v*y + w*z)*(1-c) + z*c + (-v*x + u*y)*s;
+ outvec3[0] = u * (u * x + v * y + w * z) * (1 - c) + x * c + (-w * y + v * z) * s;
+ outvec3[1] = v * (u * x + v * y + w * z) * (1 - c) + y * c + (w * x - u * z) * s;
+ outvec3[2] = w * (u * x + v * y + w * z) * (1 - c) + z * c + (-v * x + u * y) * s;
}
-void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
-{
+void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) {
FLT v0[3];
FLT v1[3];
normalize3d(v0, src);
normalize3d(v1, dest);
- FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
+ FLT d = dot3d(v0, v1); // v0.dotProduct(v1);
// If dot == 1, vectors are the same
// If dot == -1, vectors are opposite
- if (FLT_FABS(d - 1) < DEFAULT_EPSILON)
- {
+ if (FLT_FABS(d - 1) < DEFAULT_EPSILON) {
axis[0] = 0;
axis[1] = 1;
axis[2] = 0;
*angle = 0;
return;
- }
- else if (FLT_FABS(d + 1) < DEFAULT_EPSILON)
- {
+ } else if (FLT_FABS(d + 1) < DEFAULT_EPSILON) {
axis[0] = 0;
axis[1] = 1;
axis[2] = 0;
@@ -137,33 +127,28 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
*angle = FLT_ACOS(d / (v0Len * v1Len));
- //cross3d(c, v0, v1);
+ // cross3d(c, v0, v1);
cross3d(axis, v1, v0);
-
}
-
-void axisanglefromquat(FLT *angle, FLT *axis, FLT *q)
-{
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) {
// this way might be fine, too.
- //FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
+ // FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
//
//*angle = 2 * FLT_ATAN2(dist, q[0]);
- //axis[0] = q[1] / dist;
- //axis[1] = q[2] / dist;
- //axis[2] = q[3] / dist;
-
+ // axis[0] = q[1] / dist;
+ // axis[1] = q[2] / dist;
+ // axis[2] = q[3] / dist;
// Good mathematical foundation for this algorithm found here:
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
- FLT tmp[4] = { q[0], q[1], q[2], q[3] };
+ FLT tmp[4] = {q[0], q[1], q[2], q[3]};
quatnormalize(tmp, q);
- if (FLT_FABS(q[0] - 1) < FLT_EPSILON)
- {
+ if (FLT_FABS(q[0] - 1) < FLT_EPSILON) {
// we have a degenerate case where we're rotating approx. 0 degrees
*angle = 0;
axis[0] = 1;
@@ -180,11 +165,14 @@ void axisanglefromquat(FLT *angle, FLT *axis, FLT *q)
}
/////////////////////////////////////QUATERNIONS//////////////////////////////////////////
-//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
-//Under the mit/X11 license.
+// Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
+// Under the mit/X11 license.
void quatsetnone(LinmathQuat q) {
- q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
+ q[0] = 1;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = 0;
}
void quatcopy(LinmathQuat qout, const LinmathQuat qin) {
@@ -195,9 +183,9 @@ void quatcopy(LinmathQuat qout, const LinmathQuat qin) {
}
void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) {
- FLT X = euler[0]/2.0f; //roll
- FLT Y = euler[1]/2.0f; //pitch
- FLT Z = euler[2]/2.0f; //yaw
+ FLT X = euler[0] / 2.0f; // roll
+ FLT Y = euler[1] / 2.0f; // pitch
+ FLT Z = euler[2] / 2.0f; // yaw
FLT cx = FLT_COS(X);
FLT sx = FLT_SIN(X);
@@ -206,17 +194,17 @@ void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) {
FLT cz = FLT_COS(Z);
FLT sz = FLT_SIN(Z);
- //Correct according to
- //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
- q[0] = cx*cy*cz+sx*sy*sz;//q1
- q[1] = sx*cy*cz-cx*sy*sz;//q2
- q[2] = cx*sy*cz+sx*cy*sz;//q3
- q[3] = cx*cy*sz-sx*sy*cz;//q4
- quatnormalize( q, q );
+ // Correct according to
+ // http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
+ q[0] = cx * cy * cz + sx * sy * sz; // q1
+ q[1] = sx * cy * cz - cx * sy * sz; // q2
+ q[2] = cx * sy * cz + sx * cy * sz; // q3
+ q[3] = cx * cy * sz - sx * sy * cz; // q4
+ quatnormalize(q, q);
}
void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) {
- //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
+ // According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]));
euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1]));
euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]));
@@ -224,15 +212,15 @@ void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) {
void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians) {
FLT v[3];
- normalize3d( v, axis );
-
+ normalize3d(v, axis);
+
FLT sn = FLT_SIN(radians / 2.0f);
q[0] = FLT_COS(radians / 2.0f);
q[1] = sn * v[0];
q[2] = sn * v[1];
q[3] = sn * v[2];
- quatnormalize( q, q );
+ quatnormalize(q, q);
}
FLT quatmagnitude(const LinmathQuat q) {
@@ -240,19 +228,19 @@ FLT quatmagnitude(const LinmathQuat q) {
}
FLT quatinvsqmagnitude(const LinmathQuat q) {
- return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+ return ((FLT)1.) / FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
}
void quatnormalize(LinmathQuat qout, const LinmathQuat qin) {
- FLT imag = quatinvsqmagnitude( qin );
- quatscale( qout, qin, imag );
+ FLT imag = quatinvsqmagnitude(qin);
+ quatscale(qout, qin, imag);
}
void quattomatrix(FLT *matrix44, const LinmathQuat qin) {
FLT q[4];
quatnormalize(q, qin);
- //Reduced calulation for speed
+ // Reduced calulation for speed
FLT xx = 2 * q[1] * q[1];
FLT xy = 2 * q[1] * q[2];
FLT xz = 2 * q[1] * q[3];
@@ -265,7 +253,7 @@ void quattomatrix(FLT *matrix44, const LinmathQuat qin) {
FLT zz = 2 * q[3] * q[3];
FLT zw = 2 * q[3] * q[0];
- //opengl major
+ // opengl major
matrix44[0] = 1 - yy - zz;
matrix44[1] = xy - zw;
matrix44[2] = xz + yw;
@@ -320,16 +308,16 @@ void quatfrommatrix33(FLT *q, const FLT *m) {
}
void quatfrommatrix(LinmathQuat q, const FLT *matrix44) {
- //Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+ // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
FLT tr = matrix44[0] + matrix44[5] + matrix44[10];
if (tr > 0) {
- FLT S = FLT_SQRT(tr+1.0) * 2.; // S=4*qw
+ FLT S = FLT_SQRT(tr + 1.0) * 2.; // S=4*qw
q[0] = 0.25f * S;
q[1] = (matrix44[9] - matrix44[6]) / S;
q[2] = (matrix44[2] - matrix44[8]) / S;
q[3] = (matrix44[4] - matrix44[1]) / S;
- } else if ((matrix44[0] > matrix44[5])&&(matrix44[0] > matrix44[10])) {
+ } else if ((matrix44[0] > matrix44[5]) && (matrix44[0] > matrix44[10])) {
FLT S = FLT_SQRT(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2.; // S=4*qx
q[0] = (matrix44[9] - matrix44[6]) / S;
q[1] = 0.25f * S;
@@ -350,13 +338,12 @@ void quatfrommatrix(LinmathQuat q, const FLT *matrix44) {
}
}
-
// Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
void quattomatrix33(FLT *matrix33, const LinmathQuat qin) {
FLT q[4];
quatnormalize(q, qin);
- //Reduced calulation for speed
+ // Reduced calulation for speed
FLT xx = 2 * q[1] * q[1];
FLT xy = 2 * q[1] * q[2];
FLT xz = 2 * q[1] * q[3];
@@ -369,8 +356,7 @@ void quattomatrix33(FLT *matrix33, const LinmathQuat qin) {
FLT zz = 2 * q[3] * q[3];
FLT zw = 2 * q[3] * q[0];
-
- //opengl major
+ // opengl major
matrix33[0] = 1 - yy - zz;
matrix33[1] = xy + zw;
matrix33[2] = xz - yw;
@@ -393,8 +379,8 @@ void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin) {
void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin) {
FLT m = quatinvsqmagnitude(qin);
- quatgetconjugate( qout, qin );
- quatscale( qout, qout, m );
+ quatgetconjugate(qout, qin);
+ quatscale(qout, qout, m);
}
void quatsub(LinmathQuat qout, const FLT *a, const FLT *b) {
@@ -412,11 +398,11 @@ void quatadd(LinmathQuat qout, const FLT *a, const FLT *b) {
}
void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q2) {
- //NOTE: Does not normalize
- qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
- qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
- qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]);
- qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]);
+ // NOTE: Does not normalize
+ qout[0] = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]);
+ qout[1] = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]);
+ qout[2] = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]);
+ qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]);
}
void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s) {
@@ -427,96 +413,87 @@ void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s) {
}
FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb) {
- return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]);
+ return (qa[0] * qb[0]) + (qa[1] * qb[1]) + (qa[2] * qb[2]) + (qa[3] * qb[3]);
}
void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) {
- outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]);
- outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]);
- outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]);
+ outvec3[0] = (qa[0] * qb[1]) - (qa[1] * qb[0]) - (qa[2] * qb[3]) + (qa[3] * qb[2]);
+ outvec3[1] = (qa[0] * qb[2]) + (qa[1] * qb[3]) - (qa[2] * qb[0]) - (qa[3] * qb[1]);
+ outvec3[2] = (qa[0] * qb[3]) - (qa[1] * qb[2]) + (qa[2] * qb[1]) - (qa[3] * qb[0]);
}
void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb) {
- q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]);
- q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]);
- q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]);
- q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]);
+ q[0] = (qa[0] * qb[0]) - (qa[1] * qb[1]) - (qa[2] * qb[2]) - (qa[3] * qb[3]);
+ q[1] = (qa[0] * qb[1]) + (qa[1] * qb[0]);
+ q[2] = (qa[0] * qb[2]) + (qa[2] * qb[0]);
+ q[3] = (qa[0] * qb[3]) + (qa[3] * qb[0]);
}
void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) {
- outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]);
- outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]);
- outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]);
+ outvec3[0] = (qa[2] * qb[3]) - (qa[3] * qb[2]);
+ outvec3[1] = (qa[3] * qb[1]) - (qa[1] * qb[3]);
+ outvec3[2] = (qa[1] * qb[2]) - (qa[2] * qb[1]);
}
void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) {
FLT an[4];
FLT bn[4];
- quatnormalize( an, qa );
- quatnormalize( bn, qb );
- FLT cosTheta = quatinnerproduct(an,bn);
+ quatnormalize(an, qa);
+ quatnormalize(bn, qb);
+ FLT cosTheta = quatinnerproduct(an, bn);
FLT sinTheta;
- //Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll
+ // Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll
// cause SQRT to produce not a number, and screw everything up.
- if ( 1 - (cosTheta*cosTheta) <= 0 )
+ if (1 - (cosTheta * cosTheta) <= 0)
sinTheta = 0;
else
- sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta));
+ sinTheta = FLT_SQRT(1 - (cosTheta * cosTheta));
- FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions
+ FLT Theta = FLT_ACOS(cosTheta); // Theta is half the angle between the 2 MQuaternions
if (FLT_FABS(Theta) < DEFAULT_EPSILON)
- quatcopy( q, qa );
- else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON)
- {
- quatadd( q, qa, qb );
- quatscale( q, q, 0.5 );
- }
- else
- {
+ quatcopy(q, qa);
+ else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) {
+ quatadd(q, qa, qb);
+ quatscale(q, q, 0.5);
+ } else {
FLT aside[4];
FLT bside[4];
- quatscale( bside, qb, FLT_SIN(t * Theta));
- quatscale( aside, qa, FLT_SIN((1 - t)*Theta));
- quatadd( q, aside, bside );
- quatscale( q, q, ((FLT)1.)/sinTheta );
+ quatscale(bside, qb, FLT_SIN(t * Theta));
+ quatscale(aside, qa, FLT_SIN((1 - t) * Theta));
+ quatadd(q, aside, bside);
+ quatscale(q, q, ((FLT)1.) / sinTheta);
}
}
void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in) {
- //See: http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/
+ // See: http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/
FLT tmp[3];
FLT tmp2[3];
- cross3d( tmp, &quat[1], vec3in );
+ cross3d(tmp, &quat[1], vec3in);
tmp[0] += vec3in[0] * quat[0];
tmp[1] += vec3in[1] * quat[0];
tmp[2] += vec3in[2] * quat[0];
- cross3d( tmp2, &quat[1], tmp );
+ cross3d(tmp2, &quat[1], tmp);
vec3out[0] = vec3in[0] + 2 * tmp2[0];
vec3out[1] = vec3in[1] + 2 * tmp2[1];
vec3out[2] = vec3in[2] + 2 * tmp2[2];
}
-
// Matrix Stuff
-Matrix3x3 inverseM33(const Matrix3x3 mat)
-{
+Matrix3x3 inverseM33(const Matrix3x3 mat) {
Matrix3x3 newMat;
- for (int a = 0; a < 3; a++)
- {
- for (int b = 0; b < 3; b++)
- {
+ for (int a = 0; a < 3; a++) {
+ for (int b = 0; b < 3; b++) {
newMat.val[a][b] = mat.val[a][b];
}
}
- for (int i = 0; i < 3; i++)
- {
- for (int j = i + 1; j < 3; j++)
- {
+ for (int i = 0; i < 3; i++) {
+ for (int j = i + 1; j < 3; j++) {
FLT tmp = newMat.val[i][j];
newMat.val[i][j] = newMat.val[j][i];
newMat.val[j][i] = tmp;
@@ -526,8 +503,7 @@ Matrix3x3 inverseM33(const Matrix3x3 mat)
return newMat;
}
-void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3])
-{
+void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) {
FLT q[4];
quatfrom2vectors(q, v1, v2);
@@ -535,8 +511,7 @@ void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3])
quattomatrix33(&(m->val[0][0]), q);
}
-void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot)
-{
+void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot) {
out[0] = rot.val[0][0] * in[0] + rot.val[1][0] * in[1] + rot.val[2][0] * in[2];
out[1] = rot.val[0][1] * in[0] + rot.val[1][1] * in[1] + rot.val[2][1] * in[2];
out[2] = rot.val[0][2] * in[0] + rot.val[1][2] * in[1] + rot.val[2][2] * in[2];
@@ -544,7 +519,6 @@ void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot)
return;
}
-
// This function based on code from Object-oriented Graphics Rendering Engine
// Copyright(c) 2000 - 2012 Torus Knot Software Ltd
// under MIT license
@@ -557,8 +531,7 @@ If you call this with a dest vector that is close to the inverse
of this vector, we will rotate 180 degrees around a generated axis if
since in this case ANY axis of rotation is valid.
*/
-void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
-{
+void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) {
// Based on Stan Melax's article in Game Programming Gems
// Copy, since cannot modify local
@@ -567,32 +540,26 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
normalize3d(v0, src);
normalize3d(v1, dest);
- FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
+ FLT d = dot3d(v0, v1); // v0.dotProduct(v1);
// If dot == 1, vectors are the same
- if (d >= 1.0f)
- {
+ if (d >= 1.0f) {
quatsetnone(q);
return;
}
- if (d < (1e-6f - 1.0f))
- {
+ if (d < (1e-6f - 1.0f)) {
// Generate an axis
- FLT unitX[3] = { 1, 0, 0 };
- FLT unitY[3] = { 0, 1, 0 };
-
+ FLT unitX[3] = {1, 0, 0};
+ FLT unitY[3] = {0, 1, 0};
+
FLT axis[3];
- cross3d(axis, unitX, src); // pick an angle
- if ((axis[0] < 1.0e-35f) &&
- (axis[1] < 1.0e-35f) &&
- (axis[2] < 1.0e-35f)) // pick another if colinear
+ cross3d(axis, unitX, src); // pick an angle
+ if ((axis[0] < 1.0e-35f) && (axis[1] < 1.0e-35f) && (axis[2] < 1.0e-35f)) // pick another if colinear
{
cross3d(axis, unitY, src);
}
normalize3d(axis, axis);
quatfromaxisangle(q, axis, LINMATHPI);
- }
- else
- {
+ } else {
FLT s = FLT_SQRT((1 + d) * 2);
FLT invs = 1 / s;
@@ -608,13 +575,9 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
}
}
-void matrix44copy(FLT * mout, const FLT * minm )
-{
- memcpy( mout, minm, sizeof( FLT ) * 16 );
-}
+void matrix44copy(FLT *mout, const FLT *minm) { memcpy(mout, minm, sizeof(FLT) * 16); }
-void matrix44transpose(FLT * mout, const FLT * minm )
-{
+void matrix44transpose(FLT *mout, const FLT *minm) {
mout[0] = minm[0];
mout[1] = minm[4];
mout[2] = minm[8];
@@ -634,7 +597,6 @@ void matrix44transpose(FLT * mout, const FLT * minm )
mout[13] = minm[7];
mout[14] = minm[11];
mout[15] = minm[15];
-
}
void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin) {
@@ -654,5 +616,18 @@ void InvertPose(LinmathPose *poseout, const LinmathPose *pose) {
scale3d(poseout->Pos, poseout->Pos, -1);
}
+void PoseToMatrix(FLT *matrix44, const LinmathPose *pose_in) {
+ quattomatrix(matrix44, pose_in->Rot);
+
+ /*
+ matrix44[12] = pose_in->Pos[0];
+ matrix44[13] = pose_in->Pos[1];
+ matrix44[14] = pose_in->Pos[2];
+ */
+ matrix44[3] = pose_in->Pos[0];
+ matrix44[7] = pose_in->Pos[1];
+ matrix44[11] = pose_in->Pos[2];
+}
+
LinmathQuat LinmathQuat_Identity = {1.0};
LinmathPose LinmathPose_Identity = {.Rot = {1.0}};
diff --git a/redist/linmath.h b/redist/linmath.h
index cacb1c6..1a73a06 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -1,18 +1,18 @@
-//Copyright 2013,2016 <>< C. N. Lohr. This file licensed under the terms of the MIT/x11 license.
+// Copyright 2013,2016 <>< C. N. Lohr. This file licensed under the terms of the MIT/x11 license.
#ifndef _LINMATH_H
#define _LINMATH_H
-//Yes, I know it's kind of arbitrary.
+// Yes, I know it's kind of arbitrary.
#define DEFAULT_EPSILON 0.001
-//For printf
+// For printf
#define PFTHREE(x) (x)[0], (x)[1], (x)[2]
#define PFFOUR(x) (x)[0], (x)[1], (x)[2], (x)[3]
#define LINMATHPI ((FLT)3.141592653589)
-//uncomment the following line to use double precision instead of single precision.
+// uncomment the following line to use double precision instead of single precision.
//#define USE_DOUBLE
#ifdef USE_DOUBLE
@@ -20,11 +20,11 @@
#define FLT double
#define FLT_SQRT sqrt
#define FLT_TAN tan
-#define FLT_SIN sin
-#define FLT_COS cos
-#define FLT_ACOS acos
-#define FLT_ASIN asin
-#define FLT_ATAN2 atan2
+#define FLT_SIN sin
+#define FLT_COS cos
+#define FLT_ACOS acos
+#define FLT_ASIN asin
+#define FLT_ATAN2 atan2
#define FLT_FABS__ fabs
#else
@@ -32,24 +32,24 @@
#define FLT float
#define FLT_SQRT sqrtf
#define FLT_TAN tanf
-#define FLT_SIN sinf
-#define FLT_COS cosf
-#define FLT_ACOS acosf
-#define FLT_ASIN asinf
-#define FLT_ATAN2 atan2f
+#define FLT_SIN sinf
+#define FLT_COS cosf
+#define FLT_ACOS acosf
+#define FLT_ASIN asinf
+#define FLT_ATAN2 atan2f
#define FLT_FABS__ fabsf
#endif
#ifdef TCC
-#define FLT_FABS(x) (((x)<0)?(-(x)):(x))
+#define FLT_FABS(x) (((x) < 0) ? (-(x)) : (x))
#else
#define FLT_FABS FLT_FABS__
#endif
typedef FLT LinmathQuat[4]; // This is the [wxyz] quaternion, in wxyz format.
typedef FLT LinmathPoint3d[3];
-typedef FLT linmathVec3d[3];
+typedef FLT LinmathVec3d[3];
typedef struct LinmathPose {
LinmathPoint3d Pos;
@@ -59,33 +59,33 @@ typedef struct LinmathPose {
extern LinmathQuat LinmathQuat_Identity;
extern LinmathPose LinmathPose_Identity;
-//NOTE: Inputs may never be output with cross product.
-void cross3d( FLT * out, const FLT * a, const FLT * b );
+// NOTE: Inputs may never be output with cross product.
+void cross3d(FLT *out, const FLT *a, const FLT *b);
-void sub3d( FLT * out, const FLT * a, const FLT * b );
+void sub3d(FLT *out, const FLT *a, const FLT *b);
-void add3d( FLT * out, const FLT * a, const FLT * b );
+void add3d(FLT *out, const FLT *a, const FLT *b);
-void scale3d( FLT * out, const FLT * a, FLT scalar );
+void scale3d(FLT *out, const FLT *a, FLT scalar);
-void normalize3d( FLT * out, const FLT * in );
+void normalize3d(FLT *out, const FLT *in);
-FLT dot3d( const FLT * a, const FLT * b );
+FLT dot3d(const FLT *a, const FLT *b);
-//Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned.
-int compare3d( const FLT * a, const FLT * b, FLT epsilon );
+// Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned.
+int compare3d(const FLT *a, const FLT *b, FLT epsilon);
-void copy3d( FLT * out, const FLT * in );
+void copy3d(FLT *out, const FLT *in);
-FLT magnitude3d(const FLT * a );
+FLT magnitude3d(const FLT *a);
-FLT anglebetween3d( FLT * a, FLT * b );
+FLT anglebetween3d(FLT *a, FLT *b);
void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest);
void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat);
-//Quaternion things...
+// Quaternion things...
typedef FLT LinmathEulerAngle[3];
@@ -126,10 +126,10 @@ void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const Linma
// by definition.
void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in);
+void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in);
// Matrix Stuff
-typedef struct
-{
+typedef struct {
FLT val[3][3]; // row, column
} Matrix3x3;
@@ -137,12 +137,7 @@ void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot);
void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]);
Matrix3x3 inverseM33(const Matrix3x3 mat);
-
-void matrix44copy(FLT * mout, const FLT * minm );
-void matrix44transpose(FLT * mout, const FLT * minm );
-
+void matrix44copy(FLT *mout, const FLT *minm);
+void matrix44transpose(FLT *mout, const FLT *minm);
#endif
-
-
-
diff --git a/redist/os_generic.c b/redist/os_generic.c
deleted file mode 100644
index 3191357..0000000
--- a/redist/os_generic.c
+++ /dev/null
@@ -1,337 +0,0 @@
-#include "os_generic.h"
-
-#ifdef USE_WINDOWS
-
-#include <windows.h>
-
-void OGSleep( int is )
-{
- Sleep( is*1000 );
-}
-
-void OGUSleep( int ius )
-{
- Sleep( ius/1000 );
-}
-
-double OGGetAbsoluteTime()
-{
- static LARGE_INTEGER lpf;
- LARGE_INTEGER li;
-
- if( !lpf.QuadPart )
- {
- QueryPerformanceFrequency( &lpf );
- }
-
- QueryPerformanceCounter( &li );
- return (double)li.QuadPart / (double)lpf.QuadPart;
-}
-
-
-double OGGetFileTime( const char * file )
-{
- FILETIME ft;
-
- HANDLE h = CreateFile(file, GENERIC_READ, FILE_SHARE_READ, NULL, OPEN_EXISTING, 0, NULL);
-
- if( h==INVALID_HANDLE_VALUE )
- return -1;
-
- GetFileTime( h, 0, 0, &ft );
-
- CloseHandle( h );
-
- return ft.dwHighDateTime + ft.dwLowDateTime;
-}
-
-
-og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter )
-{
- return (og_thread_t)CreateThread( 0, 0, (LPTHREAD_START_ROUTINE)routine, parameter, 0, 0 );
-}
-
-void * OGJoinThread( og_thread_t ot )
-{
- WaitForSingleObject( ot, INFINITE );
- CloseHandle( ot );
- return 0;
-}
-
-void OGCancelThread( og_thread_t ot )
-{
- CloseHandle( ot );
-}
-
-og_mutex_t OGCreateMutex()
-{
- return CreateMutex( 0, 0, 0 );
-}
-
-void OGLockMutex( og_mutex_t om )
-{
- WaitForSingleObject(om, INFINITE);
-}
-
-void OGUnlockMutex( og_mutex_t om )
-{
- ReleaseMutex(om);
-}
-
-void OGDeleteMutex( og_mutex_t om )
-{
- CloseHandle( om );
-}
-
-
-
-og_sema_t OGCreateSema()
-{
- HANDLE sem = CreateSemaphore( 0, 0, 32767, 0 );
- return (og_sema_t)sem;
-}
-
-int OGGetSema( og_sema_t os )
-{
- typedef LONG NTSTATUS;
- HANDLE sem = (HANDLE)os;
- typedef NTSTATUS (NTAPI *_NtQuerySemaphore)(
- HANDLE SemaphoreHandle,
- DWORD SemaphoreInformationClass, /* Would be SEMAPHORE_INFORMATION_CLASS */
- PVOID SemaphoreInformation, /* but this is to much to dump here */
- ULONG SemaphoreInformationLength,
- PULONG ReturnLength OPTIONAL
- );
-
- typedef struct _SEMAPHORE_BASIC_INFORMATION {
- ULONG CurrentCount;
- ULONG MaximumCount;
- } SEMAPHORE_BASIC_INFORMATION;
-
-
- static _NtQuerySemaphore NtQuerySemaphore;
- SEMAPHORE_BASIC_INFORMATION BasicInfo;
- NTSTATUS Status;
-
- if( !NtQuerySemaphore )
- {
- NtQuerySemaphore = (_NtQuerySemaphore)GetProcAddress (GetModuleHandle ("ntdll.dll"), "NtQuerySemaphore");
- if( !NtQuerySemaphore )
- {
- return -1;
- }
- }
-
-
- Status = NtQuerySemaphore (sem, 0 /*SemaphoreBasicInformation*/,
- &BasicInfo, sizeof (SEMAPHORE_BASIC_INFORMATION), NULL);
-
- if (Status == ERROR_SUCCESS)
- {
- return BasicInfo.CurrentCount;
- }
-
- return -2;
-}
-
-void OGLockSema( og_sema_t os )
-{
- WaitForSingleObject( (HANDLE)os, INFINITE );
-}
-
-void OGUnlockSema( og_sema_t os )
-{
- ReleaseSemaphore( (HANDLE)os, 1, 0 );
-}
-
-void OGDeleteSema( og_sema_t os )
-{
- CloseHandle( os );
-}
-
-#else
-
-#ifndef _GNU_SOURCE
-# define _GNU_SOURCE
-#endif
-
-#include <sys/stat.h>
-#include <stdlib.h>
-#include <pthread.h>
-#include <sys/time.h>
-#include <semaphore.h>
-#include <unistd.h>
-
-pthread_mutex_t g_RawMutexStart = PTHREAD_MUTEX_INITIALIZER;
-
-void OGSleep( int is )
-{
- sleep( is );
-}
-
-void OGUSleep( int ius )
-{
- usleep( ius );
-}
-
-double OGGetAbsoluteTime()
-{
- struct timeval tv;
- gettimeofday( &tv, 0 );
- return ((double)tv.tv_usec)/1000000. + (tv.tv_sec);
-}
-
-double OGGetFileTime( const char * file )
-{
- struct stat buff;
-
- int r = stat( file, &buff );
-
- if( r < 0 )
- {
- return -1;
- }
-
- return buff.st_mtime;
-}
-
-
-
-og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter )
-{
- pthread_t * ret = (pthread_t *)malloc( sizeof( pthread_t ) );
- int r = pthread_create( ret, 0, routine, parameter );
- if( r )
- {
- free( ret );
- return 0;
- }
- return (og_thread_t)ret;
-}
-
-void * OGJoinThread( og_thread_t ot )
-{
- void * retval;
- if( !ot )
- {
- return 0;
- }
- pthread_join( *(pthread_t*)ot, &retval );
- free( ot );
- return retval;
-}
-
-void OGCancelThread( og_thread_t ot )
-{
- if( !ot )
- {
- return;
- }
- pthread_cancel( *(pthread_t*)ot );
- free( ot );
-}
-
-og_mutex_t OGCreateMutex()
-{
- pthread_mutexattr_t mta;
- og_mutex_t r = malloc( sizeof( pthread_mutex_t ) );
-
- pthread_mutexattr_init(&mta);
- pthread_mutexattr_settype(&mta, PTHREAD_MUTEX_RECURSIVE);
-
- pthread_mutex_init( (pthread_mutex_t *)r, &mta );
-
- return r;
-}
-
-void OGLockMutex( og_mutex_t om )
-{
- if( !om )
- {
- return;
- }
- pthread_mutex_lock( (pthread_mutex_t*)om );
-}
-
-void OGUnlockMutex( og_mutex_t om )
-{
- if( !om )
- {
- return;
- }
- pthread_mutex_unlock( (pthread_mutex_t*)om );
-}
-
-void OGDeleteMutex( og_mutex_t om )
-{
- if( !om )
- {
- return;
- }
-
- pthread_mutex_destroy( (pthread_mutex_t*)om );
- free( om );
-}
-
-
-
-
-og_sema_t OGCreateSema()
-{
- sem_t * sem = (sem_t *)malloc( sizeof( sem_t ) );
- sem_init( sem, 0, 0 );
- return (og_sema_t)sem;
-}
-
-int OGGetSema( og_sema_t os )
-{
- int valp;
- sem_getvalue( (sem_t *)os, &valp );
- return valp;
-}
-
-
-void OGLockSema( og_sema_t os )
-{
- sem_wait( (sem_t *)os );
-}
-
-void OGUnlockSema( og_sema_t os )
-{
- sem_post( (sem_t *)os );
-}
-
-void OGDeleteSema( og_sema_t os )
-{
- sem_destroy( (sem_t *)os );
- free(os);
-}
-
-
-
-#endif
-
-//Date Stamp: 2012-02-15
-
-/*
- Copyright (c) 2011-2012 <>< Charles Lohr
-
- Permission is hereby granted, free of charge, to any person obtaining a
- copy of this software and associated documentation files (the "Software"),
- to deal in the Software without restriction, including without limitation
- the rights to use, copy, modify, merge, publish, distribute, sublicense,
- and/or sell copies of the Software, and to permit persons to whom the
- Software is furnished to do so, subject to the following conditions:
-
- The above copyright notice and this permission notice shall be included in
- all copies or substantial portions of this file.
-
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
- IN THE SOFTWARE.
-*/
-
diff --git a/redist/os_generic.h b/redist/os_generic.h
index 0924030..853440a 100644
--- a/redist/os_generic.h
+++ b/redist/os_generic.h
@@ -1,76 +1,293 @@
-#ifndef _OS_GENERIC_H
-#define _OS_GENERIC_H
-
-#if defined( WIN32 ) || defined (WINDOWS) || defined( _WIN32)
-#define USE_WINDOWS
-#endif
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-//Things that shouldn't be macro'd
-double OGGetAbsoluteTime();
-void OGSleep( int is );
-void OGUSleep( int ius );
-double OGGetFileTime( const char * file );
-
-//Threads and Mutices
-typedef void* og_thread_t;
-typedef void* og_mutex_t;
-typedef void* og_sema_t;
-
-og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter );
-void * OGJoinThread( og_thread_t ot );
-void OGCancelThread( og_thread_t ot );
-
-//Always a recrusive mutex.
-og_mutex_t OGCreateMutex();
-void OGLockMutex( og_mutex_t om );
-void OGUnlockMutex( og_mutex_t om );
-void OGDeleteMutex( og_mutex_t om );
-
-//Always a semaphore
-og_sema_t OGCreateSema(); //Create a semaphore, comes locked initially. NOTE: Max count is 32767
-void OGLockSema( og_sema_t os );
-int OGGetSema( og_sema_t os ); //if <0 there was a failure.
-void OGUnlockSema( og_sema_t os );
-void OGDeleteSema( og_sema_t os );
-
-#ifdef __cplusplus
-};
-#endif
-
-
-
-#endif
-
-
-//Date Stamp: 2012-02-15
-
-/*
- NOTE: Portions (namely the top section) are part of headers from other
- sources.
-
- Copyright (c) 2011-2012 <>< Charles Lohr
-
- Permission is hereby granted, free of charge, to any person obtaining a
- copy of this software and associated documentation files (the "Software"),
- to deal in the Software without restriction, including without limitation
- the rights to use, copy, modify, merge, publish, distribute, sublicense,
- and/or sell copies of the Software, and to permit persons to whom the
- Software is furnished to do so, subject to the following conditions:
-
- The above copyright notice and this permission notice shall be included in
- all copies or substantial portions of this file.
-
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
- FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
- IN THE SOFTWARE.
-*/
-
+#ifndef _OS_GENERIC_H
+#define _OS_GENERIC_H
+/*
+ "osgeneric" Generic, platform independent tool for the following operations:
+
+ Delay functions:
+ void OGSleep( int is );
+ void OGUSleep( int ius );
+
+ Getting current time (may be time from program start, boot, or epoc)
+ double OGGetAbsoluteTime();
+ double OGGetFileTime( const char * file );
+
+ Thread functions
+ og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter );
+ void * OGJoinThread( og_thread_t ot );
+ void OGCancelThread( og_thread_t ot );
+
+ Mutex functions, used for protecting data structures.
+ (recursive on platforms where available.)
+ og_mutex_t OGCreateMutex();
+ void OGLockMutex( og_mutex_t om );
+ void OGUnlockMutex( og_mutex_t om );
+ void OGDeleteMutex( og_mutex_t om );
+
+//Always a semaphore (not recursive)
+// og_sema_t OGCreateSema(); //Create a semaphore, comes locked initially. NOTE: Max count is 32767
+// void OGLockSema( og_sema_t os );
+// int OGGetSema( og_sema_t os ); //if <0 there was a failure.
+// void OGUnlockSema( og_sema_t os );
+// void OGDeleteSema( og_sema_t os );
+
+
+
+ Copyright (c) 2011-2012,2013,2016,2018 <>< Charles Lohr
+ This file may be licensed under the MIT/x11 license or the NewBSD license.
+
+ Permission is hereby granted, free of charge, to any person obtaining a
+ copy of this software and associated documentation files (the "Software"),
+ to deal in the Software without restriction, including without limitation
+ the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ and/or sell copies of the Software, and to permit persons to whom the
+ Software is furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of this file.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ IN THE SOFTWARE.
+
+ Date Stamp: 2018-03-25: Switched to header-only format.
+*/
+
+#define OSG_INLINE static inline
+
+// Threads and Mutices
+typedef void *og_thread_t;
+typedef void *og_mutex_t;
+typedef void *og_sema_t;
+
+#if defined(WIN32) || defined(WINDOWS) || defined(_WIN32)
+#define USE_WINDOWS
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef USE_WINDOWS
+
+#include <windows.h>
+
+OSG_INLINE void OGSleep(int is) { Sleep(is * 1000); }
+
+OSG_INLINE void OGUSleep(int ius) { Sleep(ius / 1000); }
+
+OSG_INLINE double OGGetAbsoluteTime() {
+ static LARGE_INTEGER lpf;
+ LARGE_INTEGER li;
+
+ if (!lpf.QuadPart) {
+ QueryPerformanceFrequency(&lpf);
+ }
+
+ QueryPerformanceCounter(&li);
+ return (double)li.QuadPart / (double)lpf.QuadPart;
+}
+
+OSG_INLINE double OGGetFileTime(const char *file) {
+ FILETIME ft;
+
+ HANDLE h = CreateFile(file, GENERIC_READ, FILE_SHARE_READ, NULL, OPEN_EXISTING, 0, NULL);
+
+ if (h == INVALID_HANDLE_VALUE)
+ return -1;
+
+ GetFileTime(h, 0, 0, &ft);
+
+ CloseHandle(h);
+
+ return ft.dwHighDateTime + ft.dwLowDateTime;
+}
+
+OSG_INLINE og_thread_t OGCreateThread(void *(routine)(void *), void *parameter) {
+ return (og_thread_t)CreateThread(0, 0, (LPTHREAD_START_ROUTINE)routine, parameter, 0, 0);
+}
+
+OSG_INLINE void *OGJoinThread(og_thread_t ot) {
+ WaitForSingleObject(ot, INFINITE);
+ CloseHandle(ot);
+ return 0;
+}
+
+OSG_INLINE void OGCancelThread(og_thread_t ot) { CloseHandle(ot); }
+
+OSG_INLINE og_mutex_t OGCreateMutex() { return CreateMutex(0, 0, 0); }
+
+OSG_INLINE void OGLockMutex(og_mutex_t om) { WaitForSingleObject(om, INFINITE); }
+
+OSG_INLINE void OGUnlockMutex(og_mutex_t om) { ReleaseMutex(om); }
+
+OSG_INLINE void OGDeleteMutex(og_mutex_t om) { CloseHandle(om); }
+
+OSG_INLINE og_sema_t OGCreateSema() {
+ HANDLE sem = CreateSemaphore(0, 0, 32767, 0);
+ return (og_sema_t)sem;
+}
+
+OSG_INLINE int OGGetSema(og_sema_t os) {
+ typedef LONG NTSTATUS;
+ HANDLE sem = (HANDLE)os;
+ typedef NTSTATUS(NTAPI * _NtQuerySemaphore)(
+ HANDLE SemaphoreHandle, DWORD SemaphoreInformationClass, /* Would be SEMAPHORE_INFORMATION_CLASS */
+ PVOID SemaphoreInformation, /* but this is to much to dump here */
+ ULONG SemaphoreInformationLength, PULONG ReturnLength OPTIONAL);
+
+ typedef struct _SEMAPHORE_BASIC_INFORMATION {
+ ULONG CurrentCount;
+ ULONG MaximumCount;
+ } SEMAPHORE_BASIC_INFORMATION;
+
+ static _NtQuerySemaphore NtQuerySemaphore;
+ SEMAPHORE_BASIC_INFORMATION BasicInfo;
+ NTSTATUS Status;
+
+ if (!NtQuerySemaphore) {
+ NtQuerySemaphore = (_NtQuerySemaphore)GetProcAddress(GetModuleHandle("ntdll.dll"), "NtQuerySemaphore");
+ if (!NtQuerySemaphore) {
+ return -1;
+ }
+ }
+
+ Status =
+ NtQuerySemaphore(sem, 0 /*SemaphoreBasicInformation*/, &BasicInfo, sizeof(SEMAPHORE_BASIC_INFORMATION), NULL);
+
+ if (Status == ERROR_SUCCESS) {
+ return BasicInfo.CurrentCount;
+ }
+
+ return -2;
+}
+
+OSG_INLINE void OGLockSema(og_sema_t os) { WaitForSingleObject((HANDLE)os, INFINITE); }
+
+OSG_INLINE void OGUnlockSema(og_sema_t os) { ReleaseSemaphore((HANDLE)os, 1, 0); }
+
+OSG_INLINE void OGDeleteSema(og_sema_t os) { CloseHandle(os); }
+
+#else
+
+#define _GNU_SOURCE
+
+#include <pthread.h>
+#include <semaphore.h>
+#include <stdlib.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+OSG_INLINE void OGSleep(int is) { sleep(is); }
+
+OSG_INLINE void OGUSleep(int ius) { usleep(ius); }
+
+OSG_INLINE double OGGetAbsoluteTime() {
+ struct timeval tv;
+ gettimeofday(&tv, 0);
+ return ((double)tv.tv_usec) / 1000000. + (tv.tv_sec);
+}
+
+OSG_INLINE double OGGetFileTime(const char *file) {
+ struct stat buff;
+
+ int r = stat(file, &buff);
+
+ if (r < 0) {
+ return -1;
+ }
+
+ return buff.st_mtime;
+}
+
+OSG_INLINE og_thread_t OGCreateThread(void *(routine)(void *), void *parameter) {
+ pthread_t *ret = malloc(sizeof(pthread_t));
+ int r = pthread_create(ret, 0, routine, parameter);
+ if (r) {
+ free(ret);
+ return 0;
+ }
+ return (og_thread_t)ret;
+}
+
+static void *OGJoinThread(og_thread_t ot) {
+ void *retval;
+ if (!ot) {
+ return 0;
+ }
+ pthread_join(*(pthread_t *)ot, &retval);
+ free(ot);
+ return retval;
+}
+
+OSG_INLINE void OGCancelThread(og_thread_t ot) {
+ if (!ot) {
+ return;
+ }
+ pthread_cancel(*(pthread_t *)ot);
+ free(ot);
+}
+
+OSG_INLINE og_mutex_t OGCreateMutex() {
+ pthread_mutexattr_t mta;
+ og_mutex_t r = malloc(sizeof(pthread_mutex_t));
+
+ pthread_mutexattr_init(&mta);
+ pthread_mutexattr_settype(&mta, PTHREAD_MUTEX_RECURSIVE);
+
+ pthread_mutex_init((pthread_mutex_t *)r, &mta);
+
+ return r;
+}
+
+OSG_INLINE void OGLockMutex(og_mutex_t om) {
+ if (!om) {
+ return;
+ }
+ pthread_mutex_lock((pthread_mutex_t *)om);
+}
+
+OSG_INLINE void OGUnlockMutex(og_mutex_t om) {
+ if (!om) {
+ return;
+ }
+ pthread_mutex_unlock((pthread_mutex_t *)om);
+}
+
+OSG_INLINE void OGDeleteMutex(og_mutex_t om) {
+ if (!om) {
+ return;
+ }
+
+ pthread_mutex_destroy((pthread_mutex_t *)om);
+ free(om);
+}
+
+OSG_INLINE og_sema_t OGCreateSema() {
+ sem_t *sem = malloc(sizeof(sem_t));
+ sem_init(sem, 0, 0);
+ return (og_sema_t)sem;
+}
+
+OSG_INLINE int OGGetSema(og_sema_t os) {
+ int valp;
+ sem_getvalue(os, &valp);
+ return valp;
+}
+
+OSG_INLINE void OGLockSema(og_sema_t os) { sem_wait(os); }
+
+OSG_INLINE void OGUnlockSema(og_sema_t os) { sem_post(os); }
+
+OSG_INLINE void OGDeleteSema(og_sema_t os) {
+ sem_destroy(os);
+ free(os);
+}
+
+#endif
+
+#endif
diff --git a/src/poser.c b/src/poser.c
index 1c638f8..9a0de24 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -1,6 +1,7 @@
#include "math.h"
#include <linmath.h>
#include <stdint.h>
+#include <stdio.h>
#include <survive.h>
#define _USE_MATH_DEFINES // for C
@@ -21,6 +22,12 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
} else {
const FLT up[3] = {0, 0, 1};
+ if (quatmagnitude(lighthouse_pose->Rot) == 0) {
+ SurviveContext *ctx = so->ctx;
+ SV_INFO("Pose func called with invalid pose.");
+ return;
+ }
+
// Assume that the space solved for is valid but completely arbitrary. We are going to do a few things:
// a) Using the gyro data, normalize it so that gravity is pushing straight down along Z
// c) Assume the object is at origin
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index f5fa127..c294c0c 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -27,6 +27,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
CvMat R = cvMat(3, 3, CV_64F, r);
CvMat T = cvMat(3, 1, CV_64F, rtn.Pos);
+
+ // Super degenerate inputs will project us basically right in the camera. Detect and reject
+ if (magnitude3d(rtn.Pos) < 0.25) {
+ return rtn;
+ }
+
// Requested output is camera -> world, so invert
if (cameraToWorld) {
FLT tmp[3];
@@ -81,7 +87,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
}
SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+
+ if (quatmagnitude(lighthouse2object.Rot) != 0.0) {
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+ }
epnp_dtor(&pnp);
}
@@ -123,8 +132,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct);
add_correspondences(so, &pnp, scene, lightData);
+ static int required_meas = -1;
+ if (required_meas == -1)
+ required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4);
- if (pnp.number_of_correspondences > 4) {
+ if (pnp.number_of_correspondences > required_meas) {
SurvivePose pose = solve_correspondence(so, &pnp, false);
diff --git a/src/poser_sba.c b/src/poser_sba.c
index a1fdea6..069e1d0 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -37,6 +37,9 @@ typedef struct SBAData {
int failures_to_reset_cntr;
int successes_to_reset;
int successes_to_reset_cntr;
+
+ int required_meas;
+
} SBAData;
void metric_function(int j, int i, double *aj, double *xij, void *adata) {
@@ -175,7 +178,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
SurvivePose *camera = &so->ctx->bsd[lh].Pose;
survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij);
}
-
+#if 0
static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl,
SurviveObject *so, SurviveSensorActivations *scene, int acode,
int lh, int max_iterations /* = 50*/,
@@ -187,7 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh);
static int failure_count = 500;
- if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) {
+ if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) {
if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
SurviveContext *ctx = so->ctx;
SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
@@ -261,7 +264,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
SurviveContext *ctx = so->ctx;
// Docs say info[0] should be divided by meas; I don't buy it really...
static int cnt = 0;
- if (cnt++ > 1000 || meas_size < 8) {
+ if (cnt++ > 1000 || meas_size < d->required_meas) {
SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size);
SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
cnt = 0;
@@ -270,7 +273,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
return info[1] / meas_size * 2;
}
-
+#endif
static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl,
SurviveObject *so, SurviveSensorActivations *scene,
int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
@@ -281,7 +284,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas);
static int failure_count = 500;
- if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) {
+ if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) {
if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
SurviveContext *ctx = so->ctx;
SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
@@ -361,7 +364,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
SurviveContext *ctx = so->ctx;
// Docs say info[0] should be divided by meas; I don't buy it really...
static int cnt = 0;
- if (cnt++ > 1000 || meas_size < 8) {
+ if (cnt++ > 1000 || meas_size < d->required_meas) {
SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
cnt = 0;
@@ -436,8 +439,12 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
if (status >= 0) {
SurvivePose additionalTx = {0};
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose);
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose);
+ for (int i = 0; i < NUM_LIGHTHOUSES; i++) {
+ if (quatmagnitude(sbactx.camera_params[i].Rot) != 0) {
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, i, &additionalTx, &sbactx.camera_params[i],
+ &sbactx.obj_pose);
+ }
+ }
} else {
SurviveContext *ctx = so->ctx;
SV_INFO("SBA was unable to run %d", status);
@@ -456,16 +463,20 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
}
int PoserSBA(SurviveObject *so, PoserData *pd) {
+ SurviveContext *ctx = so->ctx;
if (so->PoserData == 0) {
so->PoserData = calloc(1, sizeof(SBAData));
SBAData *d = so->PoserData;
d->failures_to_reset_cntr = 0;
- d->failures_to_reset = 5;
+ d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
d->successes_to_reset_cntr = 0;
- d->successes_to_reset = 20;
+ d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1);
+
+ d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
+
+ SV_INFO("Initializing SBA with %d required measurements", d->required_meas);
}
SBAData *d = so->PoserData;
- SurviveContext *ctx = so->ctx;
switch (pd->pt) {
case POSERDATA_LIGHT: {
// No poses if calibration is ongoing
diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c
index fbba888..83b3681 100644
--- a/src/survive_charlesbiguator.c
+++ b/src/survive_charlesbiguator.c
@@ -1,6 +1,7 @@
//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
//
#include "survive_internal.h"
+#include <assert.h>
#include <math.h> /* for sqrt */
#include <stdint.h>
#include <string.h>
@@ -15,12 +16,16 @@ static int32_t decode_acode(uint32_t length, int32_t main_divisor) {
if (acode & 1)
return -1;
- return (acode >> 1) - 6;
+ int32_t rtn = (acode >> 1) - 6;
+ if (rtn > 7 || rtn < 0) {
+ return -1;
+ }
+ return rtn;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////The charles disambiguator. Don't use this, mostly here for
-///debugging.///////////////////////////////////////////////////////
+/// debugging.///////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) {
@@ -169,21 +174,23 @@ void DisambiguatorCharles(SurviveObject *so, LightcapElement *le) {
int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz.
int acode = decode_acode(so->last_sync_length[0], main_divisor);
- int whichlh;
- if (acode < 0)
- whichlh = 1;
- else
- whichlh = (acode >> 2);
- int32_t dl = so->last_sync_time[whichlh];
- if (!so->did_handle_ootx)
- HandleOOTX(ctx, so);
+ // If acode isn't right; don't even think of emitting an event
+ if (acode >= 0) {
+ int whichlh = (acode >> 2);
+ assert(whichlh <= 1);
+ int32_t dl = so->last_sync_time[whichlh];
- int32_t offset_from = le->timestamp - dl + le->length / 2;
+ if (!so->did_handle_ootx)
+ HandleOOTX(ctx, so);
- // Make sure pulse is in valid window
- if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time) {
- ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+ int32_t offset_from = le->timestamp - dl + le->length / 2;
+
+ // Make sure pulse is in valid window
+ if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time &&
+ offset_from > so->pulse_in_clear_time) {
+ ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+ }
}
} else {
// printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time -
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 4d1801c..e42b50e 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -21,9 +21,11 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
int axis = (lightData->acode & 1);
uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];
FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis];
+ uint32_t *length = &self->lengths[lightData->sensor_id][lightData->lh][axis];
*angle = lightData->angle;
*data_timecode = lightData->timecode;
+ *length = lightData->length * 48000000;
}
-uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); \ No newline at end of file
+uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000);
diff --git a/tools/lightlengthparams/Makefile b/tools/lightlengthparams/Makefile
new file mode 100644
index 0000000..b743f4f
--- /dev/null
+++ b/tools/lightlengthparams/Makefile
@@ -0,0 +1,15 @@
+all : lightlengthparams
+
+SRT:=../..
+
+LIBSURVIVE:=$(SRT)/lib/libsurvive.so
+
+CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE
+LDFLAGS:=-lm -lpthread -llapacke -lcblas
+
+lightlengthparams : lightlengthparams.c $(LIBSURVIVE)
+ gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean :
+ rm -rf lightlengthparams
+
diff --git a/tools/lightlengthparams/lightlengthparams.c b/tools/lightlengthparams/lightlengthparams.c
new file mode 100644
index 0000000..3b28b3f
--- /dev/null
+++ b/tools/lightlengthparams/lightlengthparams.c
@@ -0,0 +1,112 @@
+#include <assert.h>
+#include <libsurvive/survive.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+uint32_t current_timecode;
+
+void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length,
+ uint32_t lighthouse) {
+ current_timecode = timecode;
+ survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse);
+}
+
+void raw_pose_process(SurviveObject *so, uint8_t _lh, SurvivePose *pose2w) {
+ survive_default_raw_pose_process(so, _lh, pose2w);
+
+ /*printf("Pose: ");
+ for(int i = 0;i < 7;i++) {
+ printf("%f, ", pose2w->Pos[i]);
+ };
+ printf("\n");
+ */
+ for (int lh = 0; lh < 2; lh++) {
+ SurvivePose pose2lh = {};
+ SurvivePose w2lh = {};
+ InvertPose(&w2lh, &so->ctx->bsd[lh].Pose);
+ ApplyPoseToPose(&pose2lh, &w2lh, pose2w);
+
+ SurviveSensorActivations *scene = &so->activations;
+ for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
+ if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance / 2,
+ current_timecode, sensor_idx, lh)) {
+ uint32_t *lengths = scene->lengths[sensor_idx][lh];
+
+ const FLT *sensor_location = so->sensor_locations + 3 * sensor_idx;
+ const FLT *sensor_normals = so->sensor_normals + 3 * sensor_idx;
+
+ LinmathPoint3d sensor = {}, sensorN = {};
+
+ scale3d(sensorN, sensor_normals, .01);
+ add3d(sensorN, sensor_location, sensor_normals);
+
+ /*
+ printf("\n");
+ printf("%f, %f, %f\n", sensor_location[0], sensor_location[1], sensor_location[2]);
+ printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]);
+
+ FLT m[4][4];
+ PoseToMatrix((FLT*)m, &pose2lh);
+
+ for(int i = 0;i < 7;i++) {
+ printf("%f, ", pose2lh.Pos[i]);
+ };
+ printf("\n");
+ for(int i = 0;i < 4;i++) {
+ for(int j = 0;j < 4;j++) {
+ printf("%f, ", m[i][j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ */
+ ApplyPoseToPoint(sensor, &pose2lh, sensor_location);
+ ApplyPoseToPoint(sensorN, &pose2lh, sensorN);
+
+ // printf("%f, %f, %f\n", sensor[0], sensor[1], sensor[2]);
+ // printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]);
+ FLT dist = magnitude3d(sensor);
+ LinmathVec3d zout = {0, 0, -dist};
+ LinmathQuat r;
+ quatfrom2vectors(r, sensor, zout);
+ quatrotatevector(sensorN, r, sensorN);
+ sub3d(sensorN, sensorN, zout);
+
+ FLT dot = dot3d(sensor, sensorN);
+ if (dist > 20 || dist < 0.25)
+ continue;
+
+ FLT angs[2] = {};
+ for (int axis = 0; axis < 2; axis++) {
+ angs[axis] = cos(atan2(fabs(sensorN[axis ? 0 : 1]), sensorN[2]));
+ }
+ FLT area = angs[0] * angs[1];
+
+ printf("%u\t%u\t%lu\t%f\t%f\t%f\t%f\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", current_timecode, lh,
+ sensor_idx, dist, angs[0], angs[1], area, lengths[0], lengths[1], dot, sensor[0], sensor[1],
+ sensor[2], sensorN[0], sensorN[1], sensorN[2]);
+
+ // assert(angs[0] >= 0);
+ }
+ }
+ }
+}
+
+int main(int argc, char **argv) {
+ SurviveContext *ctx = survive_init(argc, argv);
+ if (ctx == 0) // implies -help or similiar
+ return 0;
+
+ printf("timecode\tlh\tsensor_idx\tdistance\tnorm_x\tnorm_y\tarea\tlength_x\tlength_y\tdot\tx\ty\tz\tnx\tny\tnz\n");
+
+ survive_startup(ctx);
+
+ survive_install_raw_pose_fn(ctx, raw_pose_process);
+ survive_install_light_fn(ctx, light_process);
+ while (survive_poll(ctx) == 0) {
+ }
+
+ survive_close(ctx);
+ return 0;
+}
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js
index 70aecfa..67e65f0 100644
--- a/tools/viz/survive_viewer.js
+++ b/tools/viz/survive_viewer.js
@@ -164,16 +164,20 @@ function create_tracked_object(info) {
group.sensors = [];
if (info.config && info.config.lighthouse_config) {
for (var idx in info.config.lighthouse_config.modelPoints) {
- var p = info.config.lighthouse_config.modelPoints[idx];
- var color = 0xFFFFFF; // / info.points.length * idx;
- if (idx === 10)
+ var p = info.config.lighthouse_config.modelPoints[idx];
+ var pn = info.config.lighthouse_config.modelNormals[idx];
+ var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF;
+ if (idx === 0)
color = 0x00ff00;
- if (idx === 12)
- color = 0x0000ff;
var sensorMaterial = new THREE.MeshBasicMaterial({color : color});
var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial);
newSensor.position.set(p[0], p[1], p[2]);
+ var normalGeom = new THREE.Geometry();
+ normalGeom.vertices.push(newSensor.position,
+ new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02));
+ var normal= new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00}));
+ group.add(normal);
group.sensors[idx] = sensorMaterial;
group.add(newSensor);
}
@@ -236,7 +240,6 @@ var survive_log_handlers = {
objs[obj.tracker].position.set(obj.position[0], obj.position[1], obj.position[2]);
objs[obj.tracker].quaternion.set(obj.quat[1], obj.quat[2], obj.quat[3], obj.quat[0]);
objs[obj.tracker].verticesNeedUpdate = true;
- timecode[obj.tracker] = obj.timecode;
if (trails) {
@@ -296,7 +299,7 @@ var survive_log_handlers = {
downAxes[obj.tracker] = new THREE.Geometry();
downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0));
- var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
+ var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
scene.add(line);
}
@@ -333,7 +336,7 @@ $(function() {
var url = new URL(window.location.href);
var remote = url.searchParams.get("remote");
- if (remote.length) {
+ if (remote && remote.length) {
survive_ws = new WebSocket("ws://" + remote + "/ws");
} else if (window.location.protocol === "file:") {
survive_ws = new WebSocket("ws://localhost:8080/ws");