Documentation AnimaL |
Tutorial |
Documentation |
Documentation 00001 00005 #include "odeImplicitSolver.h" 00006 #include "odeSolver.inl" 00007 #include <iostream> 00008 //#include <animal/numerics.h> 00009 #include <animal/value.h> 00010 #include <animal/io.h> 00011 using std::cerr; using std::endl; 00012 00013 namespace animal { 00014 00015 template<class P,class V,class R> 00016 OdeImplicitSolver<P,V,R>::OdeImplicitSolver() 00017 : _dampingRatio(0.1) 00018 , _maxIter(5) 00019 , _smallDenominatorThreshold( 1.0e-12) 00020 { 00021 _method = IMPLICIT_EULER; 00022 } 00023 00024 00030 template<class P,class V,class R> 00031 void OdeImplicitSolver<P,V,R>::solveODE( Positions& pos, Vector& vel, Real dt ) 00032 { 00033 //std::cerr<<"OdeImplicitSolver<P,V,R>::solveODE apply method " << _method << std::endl; 00034 if( _method == IMPLICIT_EULER ){ 00035 //std::cerr<<"OdeImplicitSolver<P,V,R>::apply implicit euler" << std::endl; 00036 implicit_euler(pos,vel,dt); 00037 } 00038 else OdeSolver<P,V,R>::solveODE(pos,vel,dt); 00039 } 00040 00041 00042 00043 00044 00045 template<class P,class V,class R> 00046 void OdeImplicitSolver<P,V,R>::implicit_euler( 00047 Positions& pos, 00048 Vector& vel, 00049 Real h 00050 ) 00051 { 00052 // -- resize auxiliary vectors 00053 Vector& b = da1; b.resize (pos.size ()); 00054 Vector& r = da2; r.resize(pos.size ()); 00055 Vector& x = da3; x.resize (pos.size ()); 00056 Vector& p = dv1; p.resize (pos.size ()); 00057 Vector& q = dv2; q.resize (pos.size ()); 00058 Vector& z = dv3; z.resize (pos.size ()); 00059 00060 // -- right-hand term of equation 00061 // computeForce( b, pos, vel ); // b==f 00062 // v_peq_h_dfdx_x( b, h, vel ); // b==f+hGv 00063 // v_teq(b,h); // b==h(f+hGv) 00064 // applyForces(b,b); // b==invMass * h(f+hGv) 00065 // ::std::cerr<<"OdeImplicitSolver<P,V,R>::implicit_euler h*acceleration: "<<v_output(b)<<std::endl; 00066 computeAccelerationsAndStiffness( b, pos, vel ); // b==invMass*f 00067 v_eq_h_dfdx_x( r, h, vel ); // r==hGv 00068 applyForces(r,r); // r==invMass*hGv 00069 v_peq(b,r); // b==invMass*(f+hGv) 00070 v_teq(b,h); // b==invMass * h(f+hGv) 00071 //::std::cerr<<"OdeImplicitSolver<P,V,R>::implicit_euler h*acceleration: "<<v_norm(b)<<std::endl; 00072 //::std::cerr<<"OdeImplicitSolver<P,V,R>::implicit_euler h*acceleration: "<<b<<std::endl; 00073 00074 00075 // -- solve the system using a conjugate gradient solution 00076 Real rho, rho_1=0, alpha, beta; 00077 v_assign( x, Value<typename container_traits<V>::value_type>::zero() ); 00078 v_eq(r,b); // initial residual 00079 00080 for( int nb_iter=1; nb_iter<=_maxIter; nb_iter++ ) 00081 { 00082 v_eq( z, r ); // no precond 00083 //cerr << "OdeImplicitSolver<P,V,R>::implicit_euler z =" << v_output(z) << std::endl; 00084 rho = v_dot(r,z); 00085 if( nb_iter==1 ) 00086 v_eq(p,z); 00087 else { 00088 beta = rho / rho_1; 00089 v_eq(p,z,beta,p); 00090 } 00091 //cerr << "OdeImplicitSolver<P,V,R>::implicit_euler p =" << v_output(p) << std::endl; 00092 multImplicitMatrix( q, p, h ); 00093 //cerr << "OdeImplicitSolver<P,V,R>::implicit_euler q =" << v_output(q) << std::endl; 00094 Real den = v_dot(p,q); 00095 if( fabs(den)<_smallDenominatorThreshold ) break; 00096 //cerr<<"den = "<<den<<endl; 00097 alpha = rho/den; 00098 //cerr<<"alpha = "<<alpha<<endl; 00099 v_peq(x,alpha,p); 00100 v_peq(r,-alpha,q); 00101 rho_1 = rho; 00103 // std::cerr << "\n================= OdeImplicitSolver<P,V,R>::implicit_euler end iteration "<< nb_iter << "\n"; 00104 // std::cerr<< std::endl; 00105 // std::cerr<< "OdeImplicitSolver<P,V,R>::implicit_euler p = " << v_output(p) << std::endl; 00106 // std::cerr<< "OdeImplicitSolver<P,V,R>::implicit_euler q = " << v_output(q) << std::endl; 00107 // std::cerr<< "OdeImplicitSolver<P,V,R>::implicit_euler residual = " << v_output(r) << std::endl; 00108 //std::cerr<< "OdeImplicitSolver<P,V,R>::implicit_euler current solution = " << v_norm(x) << std::endl; 00109 //std::cerr<< "OdeImplicitSolver<P,V,R>::implicit_euler residual norm = " << v_norm(r); 00110 //std::cerr<< std::endl; 00112 } 00113 // x is the solution of the system 00114 00115 00116 // apply the solution 00117 // std::cerr << "OdeImplicitSolver<P,V,R>::implicit_euler solution: "<< v_output(x) << std::endl; 00118 //std::cerr<<"implicit_euler 1 "<<vel[0].y<<std::endl; 00119 v_peq( vel, x ); 00120 //std::cerr<<"implicit_euler 2 "<<vel[0].y<<std::endl; 00121 v_peq( pos, h, vel ); 00122 //std::cerr<<"implicit_euler 3 "<<vel[0].y<<std::endl; 00123 } 00124 00125 template<class P,class V,class R> 00126 void OdeImplicitSolver<P,V,R>::compute_implicit_euler_step( 00127 Positions& dpos, 00128 Vector& dvel, 00129 const Positions& pos, 00130 const Vector& vel, 00131 Real h 00132 ) 00133 { 00134 // -- resize auxiliary vectors 00135 Vector& b = da1; b.resize (pos.size ()); 00136 Vector& r = da2; r .resize(pos.size ()); 00137 Vector& x = da3; x.resize (pos.size ()); 00138 Vector& p = dv1; p.resize (pos.size ()); 00139 Vector& q = dv2; q.resize (pos.size ()); 00140 Vector& z = dv3; z.resize (pos.size ()); 00141 00142 // -- right-hand term of equation 00143 // computeForce( b, pos, vel ); // b==f 00144 // v_peq_h_dfdx_x( b, h, vel ); // b==f+hGv 00145 // v_teq(b,h); // b==h(f+hGv) 00146 // applyForces(b,b); // b==invMass * h(f+hGv) 00147 // ::std::cerr<<"OdeImplicitSolver<P,V,R>::implicit_euler h*acceleration: "<<v_output(b)<<std::endl; 00148 computeAccelerationsAndStiffness( b, pos, vel ); // b==invMass*f 00149 v_eq_h_dfdx_x( r, h, vel ); // r==hGv 00150 applyForces(r,r); // r==invMass*hGv 00151 v_peq(b,r); // b==invMass*(f+hGv) 00152 v_teq(b,h); // b==invMass * h(f+hGv) 00153 //::std::cerr<<"OdeImplicitSolver<P,V,R>::implicit_euler h*acceleration: "<<v_output(b)<<std::endl; 00154 00155 // -- solve the system using a conjugate gradient solution 00156 Real rho, rho_1=0, alpha, beta; 00157 v_assign( x, Value<typename container_traits<V>::value_type>::zero() ); 00158 v_eq(r,b); // initial residual 00159 00160 for( int nb_iter=1; nb_iter<=_maxIter; nb_iter++ ) 00161 { 00162 v_eq( z, r ); // no precond 00163 //cerr << "OdeImplicitSolver<P,V,R>::implicit_euler z =" << v_output(z) << std::endl; 00164 rho = v_dot(r,z); 00165 if( nb_iter==1 ) 00166 v_eq(p,z); 00167 else { 00168 beta = rho / rho_1; 00169 v_eq(p,z,beta,p); 00170 } 00171 //cerr << "OdeImplicitSolver<P,V,R>::implicit_euler p =" << v_output(p) << std::endl; 00172 multImplicitMatrix( q, p, h ); 00173 //cerr << "OdeImplicitSolver<P,V,R>::implicit_euler q =" << v_output(q) << std::endl; 00174 Real den = v_dot(p,q); 00175 if( fabs(den)<_smallDenominatorThreshold ) break; 00176 alpha = rho/den; 00177 v_peq(x,alpha,p); 00178 v_peq(r,-alpha,q); 00179 rho_1 = rho; 00181 //std::cerr << "\n================= OdeImplicitSolver<P,V,R>::implicit_euler end iteration "<< nb_iter << "\n" 00182 // << std::endl 00183 // << "OdeImplicitSolver<P,V,R>::implicit_euler p = " << v_output(p) << std::endl 00184 // << "OdeImplicitSolver<P,V,R>::implicit_euler q = " << v_output(q) << std::endl 00185 // << "OdeImplicitSolver<P,V,R>::implicit_euler residual = " << v_output(r) << std::endl 00186 // << "OdeImplicitSolver<P,V,R>::implicit_euler current solution = " << v_output(x) << std::endl 00187 // << "OdeImplicitSolver<P,V,R>::implicit_euler residual norm = " << v_norm(r) 00188 // << std::endl; 00190 } 00191 // x is the solution of the system 00192 00193 // apply the solution 00194 // std::cerr << "OdeImplicitSolver<P,V,R>::implicit_euler solution: "<< v_output(x) << std::endl; 00195 v_eq( dvel, x ); 00196 v_eq( dpos, h, vel ); 00197 v_peq( dpos, h, dvel ); 00198 } 00199 00200 00201 template<class P,class V,class R> 00202 void OdeImplicitSolver<P,V,R>::multImplicitMatrix( Vector& y, const Vector& x, Real h) 00203 { 00204 // cerr<<"OdeImplicitSolver<P,V,R>::multImplicitMatrix, dampingRatio= " << _dampingRatio << endl; 00205 // cerr<<"OdeImplicitSolver<P,V,R>::multImplicitMatrix, h= " << h << endl; 00206 // cerr<< "OdeImplicitSolver<P,V,R>::multImplicitMatrix, x = " << x << endl; 00207 // cerr<< "OdeImplicitSolver<P,V,R>::damping ratio = " << _dampingRatio << endl; 00208 v_eq_h_dfdx_x(y, -h*(h+_dampingRatio), x); // y == -h(h+r)Gx 00209 //cerr<< "-h(h+r)Gx = " << v_norm(y) << endl; 00210 applyForces(y,y); // y == -h(h+r)M^{-1}Gx 00211 // cerr<< "-h(h+r)M^{-1}Gx = " << v_norm(y) << endl; 00212 v_peq(y,x); // y == x -h(h+r)M^{-1}Gx 00213 // cerr<< "x - h(h+r)M^{-1}Gx = " << v_norm(y) << endl <<"----------------" <<endl; 00214 } 00215 00216 00217 }//animal
1.3.6