diff options
| author | Justin Berger <j.david.berger@gmail.com> | 2018-03-15 14:43:58 -0600 |
|---|---|---|
| committer | Justin Berger <j.david.berger@gmail.com> | 2018-03-15 14:48:29 -0600 |
| commit | 2e980136715acd4692dd61be4442dc823219764d (patch) | |
| tree | 14c7df0eef9e8b0794766d54d5994ae6efd2420d /redist | |
| parent | 3f8346dcc4b38116ea15543b62a1e859b6a47e85 (diff) | |
| download | libsurvive-2e980136715acd4692dd61be4442dc823219764d.tar.gz libsurvive-2e980136715acd4692dd61be4442dc823219764d.tar.bz2 | |
Results out of epnp
Diffstat (limited to 'redist')
| -rw-r--r-- | redist/json_helpers.c | 5 | ||||
| -rw-r--r-- | redist/linmath.c | 31 | ||||
| -rw-r--r-- | redist/linmath.h | 1 |
3 files changed, 33 insertions, 4 deletions
diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 109708a..4aeb399 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -185,9 +185,6 @@ void json_load_file(const char* path) { char* tag = substr(JSON_STRING, tag_t->start, tag_t->end, JSON_STRING_LEN); char* value = substr(JSON_STRING, value_t->start, value_t->end, JSON_STRING_LEN); - printf("%d %d c:%d %d %s \n", tag_t->start, tag_t->end, tag_t->size, tag_t->type, tag); - - if (value_t->type == JSMN_ARRAY) { i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children } else if (value_t->type == JSMN_OBJECT) { @@ -244,4 +241,4 @@ int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) { return count; -}
\ No newline at end of file +} diff --git a/redist/linmath.c b/redist/linmath.c index 76a723d..c302f5b 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -300,6 +300,37 @@ void quattomatrix(FLT * matrix44, const FLT * qin) matrix44[15] = 1; } +void quatfrommatrix33(FLT *q, const FLT *m) { + FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8]; + + FLT tr = m00 + m11 + m22; + + if (tr > 0) { + FLT S = sqrt(tr + 1.0) * 2; // S=4*qw + q[0] = 0.25 * S; + q[1] = (m21 - m12) / S; + q[2] = (m02 - m20) / S; + q[3] = (m10 - m01) / S; + } else if ((m00 > m11) & (m00 > m22)) { + FLT S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*q[1] + q[0] = (m21 - m12) / S; + q[1] = 0.25 * S; + q[2] = (m01 + m10) / S; + q[3] = (m02 + m20) / S; + } else if (m11 > m22) { + FLT S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*q[2] + q[0] = (m02 - m20) / S; + q[1] = (m01 + m10) / S; + q[2] = 0.25 * S; + q[3] = (m12 + m21) / S; + } else { + FLT S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*q[3] + q[0] = (m10 - m01) / S; + q[1] = (m02 + m20) / S; + q[2] = (m12 + m21) / S; + q[3] = 0.25 * S; + } +} void quatfrommatrix( FLT * q, const FLT * matrix44 ) { diff --git a/redist/linmath.h b/redist/linmath.h index ff00c94..57b98d3 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -87,6 +87,7 @@ FLT quatinvsqmagnitude( const FLT * q ); void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out. void quattomatrix( FLT * matrix44, const FLT * q ); void quatfrommatrix( FLT * q, const FLT * matrix44 ); +void quatfrommatrix33(FLT *q, const FLT *matrix33); void quatgetconjugate( FLT * qout, const FLT * qin ); void quatgetreciprocal( FLT * qout, const FLT * qin ); void quatsub( FLT * qout, const FLT * a, const FLT * b ); |