Fix axis interpolation to work with scaling

[libs/gl.git] / source / animation.cpp

diff --git a/source/animation.cpp b/source/animation.cpp
index efb67c972ded72ad916c286def76bd34bf5de28c..8135cb5995a406e5f9b722f5ca242b3c60eecb96 100644 (file)
--- a/source/animation.cpp
+++ b/source/animation.cpp
@@ -3,6 +3,7 @@
 #include <msp/time/units.h>
 #include "animation.h"
 #include "armature.h"
+#include "error.h"
 #include "keyframe.h"
 #include "pose.h"
 
@@ -58,18 +59,17 @@ Animation::AxisInterpolation::AxisInterpolation(const double *axis1, const doubl
 {
        // Compute a normalized vector halfway between the two endpoints
        double half[3];
-       double len = 0;
+       double a1_len = 0;
+       double h_len = 0;
        for(unsigned i=0; i<3; ++i)
        {
                half[i] = (axis1[i]+axis2[i])/2;
-               len += half[i]*half[i];
+               a1_len += axis1[i]*axis1[i];
+               h_len += half[i]*half[i];
        }
-       len = sqrt(len);
-       for(unsigned i=0; i<3; ++i)
-               half[i] /= len;
 
        // Compute correction factors for smooth interpolation
-       double cos_half = axis1[0]*half[0]+axis1[1]*half[1]+axis1[2]*half[2];
+       double cos_half = (axis1[0]*half[0]+axis1[1]*half[1]+axis1[2]*half[2])/sqrt(a1_len*h_len);
        double angle = acos(cos_half);
        slope = (angle ? angle/tan(angle) : 1);
        scale = cos_half;
@@ -206,7 +206,7 @@ Matrix Animation::Iterator::get_matrix() const
 Matrix Animation::Iterator::get_pose_matrix(unsigned link) const
 {
        if(!animation.armature)
-               throw logic_error("Animation::Iterator::get_pose_matrix");
+               throw invalid_operation("Animation::Iterator::get_pose_matrix");
        if(link>animation.armature->get_max_link_index())
                throw out_of_range("Animation::Iterator::get_pose_matrix");