core/vcsl/vcsl_displacement.cxx
Go to the documentation of this file.
00001 // This is core/vcsl/vcsl_displacement.cxx
00002 #include "vcsl_displacement.h"
00003 #include <vcl_cassert.h>
00004 
00005 //---------------------------------------------------------------------------
00006 // Set the point for a static displacement
00007 //---------------------------------------------------------------------------
00008 void vcsl_displacement::set_static_point(vnl_vector<double> const& new_point)
00009 {
00010   point_.clear(); point_.push_back(new_point);
00011   vcsl_spatial_transformation::set_static();
00012 }
00013 
00014 //---------------------------------------------------------------------------
00015 // Image of `v' by `this'
00016 // REQUIRE: is_valid()
00017 //---------------------------------------------------------------------------
00018 vnl_vector<double> vcsl_displacement::execute(const vnl_vector<double> &v,
00019                                               double time) const
00020 {
00021   // require
00022   assert(is_valid());
00023   assert((is_2d()&&v.size()==2)||(is_3d()&&v.size()==3));
00024 
00025   vnl_vector<double> translation=vector_value(time);
00026 
00027   vnl_vector_fixed<double,3> result;
00028 
00029   if (mode_2d_)
00030   {
00031     result.put(0,v.get(0)-translation.get(0));
00032     result.put(1,v.get(1)-translation.get(1));
00033     result.put(2,0);
00034   }
00035   else
00036     result=v-translation;
00037 
00038   vnl_quaternion<double> q=quaternion(time);
00039   result = q.rotate(result);
00040 
00041   if (mode_2d_)
00042   {
00043     vnl_vector<double> tmp(2);
00044     tmp.put(0,result.get(0)+translation.get(0));
00045     tmp.put(1,result.get(1)+translation.get(1));
00046     return tmp;
00047   }
00048   else
00049     return result+translation;
00050 }
00051 
00052 //---------------------------------------------------------------------------
00053 // Image of `v' by the inverse of `this'
00054 // REQUIRE: is_valid()
00055 // REQUIRE: is_invertible(time)
00056 //---------------------------------------------------------------------------
00057 vnl_vector<double> vcsl_displacement::inverse(const vnl_vector<double> &v,
00058                                               double time) const
00059 {
00060   // require
00061   assert(is_valid());
00062   assert(is_invertible(time));
00063   assert((is_2d()&&v.size()==2)||(is_3d()&&v.size()==3));
00064 
00065   vnl_vector<double> translation=vector_value(time);
00066 
00067   vnl_vector_fixed<double,3> result;
00068 
00069   if (mode_2d_)
00070   {
00071     result.put(0,v.get(0)-translation.get(0));
00072     result.put(1,v.get(1)-translation.get(1));
00073     result.put(2,0);
00074   }
00075   else
00076     result=v-translation;
00077 
00078   vnl_quaternion<double> q=quaternion(time);
00079   result = q.conjugate().rotate(result);
00080 
00081   if (mode_2d_)
00082   {
00083     vnl_vector<double> tmp(2);
00084     tmp.put(0,result.get(0)+translation.get(0));
00085     tmp.put(1,result.get(1)+translation.get(1));
00086     return tmp;
00087   }
00088   else
00089     return result+translation;
00090 }
00091 
00092 //---------------------------------------------------------------------------
00093 // Compute the value of the vector at time `time'
00094 //---------------------------------------------------------------------------
00095 vnl_vector<double> vcsl_displacement::vector_value(double time) const
00096 {
00097   if (this->duration()==0) // static
00098     return point_[0];
00099   else
00100   {
00101     int i=matching_interval(time);
00102     switch (interpolator_[i])
00103     {
00104      case vcsl_linear:
00105       return lvi(point_[i],point_[i+1],i,time);
00106      case vcsl_cubic:
00107       assert(!"vcsl_cubic net yet implemented");
00108       break;
00109      case vcsl_spline:
00110       assert(!"vcsl_spline net yet implemented");
00111       break;
00112      default:
00113       assert(!"This is impossible");
00114       break;
00115     }
00116   }
00117   return vnl_vector<double>(); // never reached if asserts are in effect
00118 }