diff options
| -rw-r--r-- | redist/linmath.c | 32 | ||||
| -rw-r--r-- | src/poser_turveytori.c | 41 |
2 files changed, 46 insertions, 27 deletions
diff --git a/redist/linmath.c b/redist/linmath.c index 5fefe1e..b3896ff 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -259,35 +259,37 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 ) } +// Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ void quattomatrix33(FLT * matrix33, const FLT * qin) { FLT q[4]; quatnormalize(q, qin); //Reduced calulation for speed - FLT xx = 2 * q[0] * q[0]; - FLT xy = 2 * q[0] * q[1]; - FLT xz = 2 * q[0] * q[2]; - FLT xw = 2 * q[0] * q[3]; + FLT xx = 2 * q[1] * q[1]; + FLT xy = 2 * q[1] * q[2]; + FLT xz = 2 * q[1] * q[3]; + FLT xw = 2 * q[1] * q[0]; + + FLT yy = 2 * q[2] * q[2]; + FLT yz = 2 * q[2] * q[3]; + FLT yw = 2 * q[2] * q[0]; - FLT yy = 2 * q[1] * q[1]; - FLT yz = 2 * q[1] * q[2]; - FLT yw = 2 * q[1] * q[3]; + FLT zz = 2 * q[3] * q[3]; + FLT zw = 2 * q[3] * q[0]; - FLT zz = 2 * q[2] * q[2]; - FLT zw = 2 * q[2] * q[3]; //opengl major matrix33[0] = 1 - yy - zz; - matrix33[1] = xy - zw; - matrix33[2] = xz + yw; + matrix33[1] = xy + zw; + matrix33[2] = xz - yw; - matrix33[3] = xy + zw; + matrix33[3] = xy - zw; matrix33[4] = 1 - xx - zz; - matrix33[5] = yz - xw; + matrix33[5] = yz + xw; - matrix33[6] = xz - yw; - matrix33[7] = yz + xw; + matrix33[6] = xz + yw; + matrix33[7] = yz - xw; matrix33[8] = 1 - xx - yy; } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d5f36cf..0222a46 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -386,13 +386,20 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) return dist; } +int compareFlts(const void * b, const void * a) +{ + FLT a2 = *(const FLT*)a; + FLT b2 = *(const FLT*)b; + return (a2 > b2) - (a2 < b2); +} + FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint) { FLT fitness; FLT resultSum = 0; FLT *fitnesses = alloca(sizeof(FLT) * pnaCount); - int i=0, j=0; + int i = 0, j = 0; FLT worstFitness = 0; @@ -414,18 +421,18 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu } } - for (size_t i = 0; i < pnaCount; i++) + + qsort(fitnesses, pnaCount, sizeof(FLT), compareFlts); + + //printf("wf[%f]\n", worstFitness); + + + // Note that we're only using the best 70% of the tori. + // This is to remove any "bad" outliers. + // TODO: better algorithms exist. + for (size_t i = 0; i < (size_t)(pnaCount * 0.70); i++) { - // TODO: This is an UGLY HACK!!! It is NOT ROBUST and MUST BE BETTER - // Right now, we're just throwing away the single worst fitness value - // this works frequently, but we REALLY need to do a better job of determing - // exactly when we should throw away a bad value. I'm thinking that decision - // alone could be a master's thesis, so lots of work to be done. - // This is just a stupid general approach that helps in a lot of cases, - // but is NOT suitable for long term use. - //if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j) - if (fitnesses[i] != worstFitness) - resultSum += SQUARED(fitnesses[i]); + resultSum += SQUARED(fitnesses[i]); } return 1 / FLT_SQRT(resultSum); } @@ -1523,6 +1530,16 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) if (counter % 4 == 0) QuickPose(so, 0); } + if (1 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so, 1); + } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; |