Go to the documentation of this file.00001
00002 #include "vcsl_displacement.h"
00003 #include <vcl_cassert.h>
00004
00005
00006
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
00016
00017
00018 vnl_vector<double> vcsl_displacement::execute(const vnl_vector<double> &v,
00019 double time) const
00020 {
00021
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
00054
00055
00056
00057 vnl_vector<double> vcsl_displacement::inverse(const vnl_vector<double> &v,
00058 double time) const
00059 {
00060
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
00094
00095 vnl_vector<double> vcsl_displacement::vector_value(double time) const
00096 {
00097 if (this->duration()==0)
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>();
00118 }