From 4741dca55f325eb3b56090bb3941ffa4152dfed8 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 30 May 2017 17:56:52 -0700 Subject: Add angleaxisfrom2vect() --- redist/linmath.c | 39 +++++++++++++++++++++++++++++++++++++++ redist/linmath.h | 2 ++ src/poser_turveytori.c | 45 +++++++++++++++++++++++++++++---------------- 3 files changed, 70 insertions(+), 16 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index b3896ff..5d51708 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -104,6 +104,45 @@ void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) 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) +{ + FLT v0[3]; + FLT v1[3]; + normalize3d(v0, src); + normalize3d(v1, dest); + + 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) + { + axis[0] = 0; + axis[1] = 1; + axis[2] = 0; + *angle = 0; + return; + } + else if (FLT_FABS(d + 1) < DEFAULT_EPSILON) + { + axis[0] = 0; + axis[1] = 1; + axis[2] = 0; + *angle = LINMATHPI; + return; + } + + FLT v0Len = magnitude3d(v0); + FLT v1Len = magnitude3d(v1); + + *angle = FLT_ACOS(d / (v0Len * v1Len)); + + //cross3d(c, v0, v1); + cross3d(axis, v1, v0); + +} + + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. diff --git a/redist/linmath.h b/redist/linmath.h index 6f0bf60..8d6cf05 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -71,6 +71,8 @@ FLT magnitude3d(const FLT * a ); 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); + //Quaternion things... void quatsetnone( FLT * q ); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 481a499..45a1122 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1471,22 +1471,22 @@ static void QuickPose(SurviveObject *so, int lh) // it basically ignores all the logic to find the most reliable data points // and just grabs a sample and uses it. - static int countdown = 5; - - if (countdown > 0 && so->ctx->objs[0] == so) - { - SolveForLighthouse(pos, quat, to, so, 0, lh, 1); - countdown--; - } - else - { - SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - } + //static int countdown = 5; + + //if (countdown > 0 && so->ctx->objs[0] == so) + //{ + // SolveForLighthouse(pos, quat, to, so, 0, lh, 1); + // countdown--; + //} + //else + //{ + // SolveForLighthouse(pos, quat, to, so, 0, lh, 0); + //} - //SolveForLighthouse(pos, quat, to, so, 0, lh, 0); + SolveForLighthouse(pos, quat, to, so, 0, lh, 0); printf("!\n"); } @@ -1600,6 +1600,11 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) //quatfrom2vectors(downQuat, negZ, td->down); quatfrom2vectors(downQuat, td->down, negZ); + FLT angle; + FLT axis[3]; + angleaxisfrom2vect(&angle, &axis, td->down, negZ); + //angleaxisfrom2vect(&angle, &axis, negZ, td->down); + { int sensorCount = 0; @@ -1611,8 +1616,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + + rotatearoundaxis(norm, norm, axis, angle); + rotatearoundaxis(point, point, axis, angle); + to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; @@ -1643,8 +1652,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + + rotatearoundaxis(norm, norm, axis, angle); + rotatearoundaxis(point, point, axis, angle); + to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; -- cgit v1.2.3