core/vcsl/vcsl_spatial_transformation.cxx Source File

Go to the documentation of this file.
00001 // This is core/vcsl/vcsl_spatial_transformation.cxx
00002 #include "vcsl_spatial_transformation.h"
00003 #include <vcl_cmath.h> // for acos(), sin()
00004 #include <vcl_cassert.h>
00005 
00006 //---------------------------------------------------------------------------
00007 // Is `time' between the two time bounds ?
00008 //---------------------------------------------------------------------------
00009 bool vcsl_spatial_transformation::valid_time(double time) const
00010 {
00011   if (beat_.size() == 0) return true;
00012   return (beat_[0]<=time)&&(time<=beat_[beat_.size()-1]);
00013 }
00014 
00015 //---------------------------------------------------------------------------
00016 // Return the index of the beat inferior or equal to `time'
00017 // REQUIRE: valid_time(time)
00018 //---------------------------------------------------------------------------
00019 int vcsl_spatial_transformation::matching_interval(double time) const
00020 {
00021   // require
00022   assert(valid_time(time));
00023 
00024   // Dichotomic research of the index
00025   int inf=0;
00026   int sup=beat_.size()-1;
00027   while (sup-inf > 1)
00028   {
00029     int mid=(inf+sup)/2;
00030     if (beat_[mid]>time)
00031       sup=mid;
00032     else
00033       inf=mid;
00034   }
00035   return inf;
00036 }
00037 
00038 //---------------------------------------------------------------------------
00039 // Empty the time clock and interpolators, thereby making the transf static
00040 //---------------------------------------------------------------------------
00041 void vcsl_spatial_transformation::set_static()
00042 {
00043   beat_.clear();
00044   interpolator_.clear();
00045 }
00046 
00047 //---------------------------------------------------------------------------
00048 // Linear interpolation on scalar values
00049 //---------------------------------------------------------------------------
00050 double vcsl_spatial_transformation::lsi(double v0,
00051                                         double v1,
00052                                         int index,
00053                                         double time) const
00054 {
00055   assert(index>=0 && (unsigned)index+1<beat_.size());
00056   double t0=beat_[index];
00057   double t1=beat_[index+1];
00058 
00059   return (v0*(t1-time)+v1*(time-t0))/(t1-t0);
00060 }
00061 
00062 //---------------------------------------------------------------------------
00063 // Linear interpolation on vnl_vectors
00064 //---------------------------------------------------------------------------
00065 vnl_vector<double>
00066 vcsl_spatial_transformation::lvi(const vnl_vector<double> &v0,
00067                                  const vnl_vector<double> &v1,
00068                                  int index,
00069                                  double time) const
00070 {
00071   int size=v0.size();
00072   assert(index>=0 && (unsigned)index+1<beat_.size());
00073   double t0=beat_[index];
00074   double t1=beat_[index+1];
00075 
00076   double denominator=1/(t1-t0);
00077   double dt1=(t1-time)*denominator;
00078   double dt0=(time-t0)*denominator;
00079 
00080   vnl_vector<double> result(size);
00081   for (int i=0;i<size;++i)
00082     result.put(i,v0.get(i)*dt1+v1.get(i)*dt0);
00083 
00084   return result;
00085 }
00086 
00087 //---------------------------------------------------------------------------
00088 // Linear interpolation on vnl_matrices
00089 //---------------------------------------------------------------------------
00090 vnl_matrix<double>
00091 vcsl_spatial_transformation::lmi(const vnl_matrix<double> &m0,
00092                                  const vnl_matrix<double> &m1,
00093                                  int index,
00094                                  double time) const
00095 {
00096   int rows=m0.rows();
00097   int cols=m0.cols();
00098   assert(index>=0 && (unsigned)index+1<beat_.size());
00099   double t0=beat_[index];
00100   double t1=beat_[index+1];
00101 
00102   double denominator=1/(t1-t0);
00103   double dt1=(t1-time)*denominator;
00104   double dt0=(time-t0)*denominator;
00105 
00106   vnl_matrix<double> result(rows,cols);
00107   for (int i=0;i<rows;++i)
00108   for (int j=0;j<cols;++j)
00109     result.put(i,j,m0.get(i,j)*dt1+m1.get(i,j)*dt0);
00110 
00111   return result;
00112 }
00113 
00114 //---------------------------------------------------------------------------
00115 // Linear interpolation on quaternions
00116 //---------------------------------------------------------------------------
00117 vnl_quaternion<double>
00118 vcsl_spatial_transformation::lqi(const vnl_quaternion<double> &v0,
00119                                  const vnl_quaternion<double> &v1,
00120                                  int index,
00121                                  double time) const
00122 {
00123   assert(index>=0 && (unsigned)index+1<beat_.size());
00124   double t0=beat_[index];
00125   double t1=beat_[index+1];
00126   double t=(time-t0)/(t1-t0);
00127 
00128   double cosangle=dot_product(v0.as_ref(), v1.as_ref());
00129   double angle=vcl_acos(cosangle);
00130   double invsin=1/vcl_sin(angle);
00131   double coef1=vcl_sin((1-t)*angle)*invsin;
00132   double coef2=vcl_sin(t*angle)*invsin;
00133 
00134   return vnl_quaternion<double>(v0.x()*coef1+v1.x()*coef2,
00135                                 v0.y()*coef1+v1.y()*coef2,
00136                                 v0.z()*coef1+v1.z()*coef2,
00137                                 v0.r()*coef1+v1.r()*coef2);
00138 }