File content as of revision 3:14d241e29be3:

#include "position.h"    
#include <cstring>
    
position::position() {
}

float position::interpAngle(float value1, float weight1, float value2, float weight2) {
    float delta = (value1-value2);
    if ((delta >= -180) && (delta <= 180)) // no wraparound
        return value1*weight1 + value2*weight2;
    
    if (delta < -180) { // val2 far larger than val1 
        float result = (value1+360)*weight1 + value2*weight2;
        if (result >= 360)
            return result-360;
        return result;
    } else {
        float result = (value1-360)*weight1 + value2*weight2;
        if (result < 0)
            return result+360;
        return result;
    }    
}

bool position::interp(position* output, position *pos1, position *pos2) {
if (pos1->ID != pos2->ID)
    return false;

if (output==NULL) {
    printf(" BO ");
    return false;
}
if (pos1==NULL) {
    printf(" B1 ");
    return false;
}
if (pos2==NULL) {
    printf(" B2 ");
    return false;
}

if (output->time == pos1->time) {
    memcpy(output,pos1,sizeof(position));
    printf(" C1 ");
    return true;
}

if (output->time == pos2->time) {
    memcpy(output,pos2,sizeof(position));
    printf(" C2 ");
    return true;
}

if (pos1->time == pos2->time) {
    printf(" E %ld",pos1->time);
    return false;
}

float pos2Weight = (pos1->time - pos2->time)/(float)(pos1->time - output->time);
float pos1Weight = 1-pos2Weight;
output->X = pos1->X*pos1Weight + pos2->X*pos2Weight;
output->Y = pos1->Y*pos1Weight + pos2->Y*pos2Weight;
output->Height = pos1->Height*pos1Weight + pos2->Height*pos2Weight;
output->roll = position::interpAngle(pos1->roll,pos1Weight,pos2->roll,pos2Weight);
output->pitch = position::interpAngle(pos1->pitch,pos1Weight,pos2->pitch,pos2Weight);
output->yaw =position::interpAngle(pos1->yaw,pos1Weight,pos2->yaw,pos2Weight);
output->ID = pos1->ID;
output->focus = pos1->focus*pos1Weight + pos2->focus*pos2Weight +0.5f;
output->iris = pos1->iris*pos1Weight + pos2->iris*pos2Weight +0.5f;
output->zoom = pos1->zoom*pos1Weight + pos2->zoom*pos2Weight +0.5f;

return true;
}