kernel/Overhauser.h
Go to the documentation of this file.00001 #ifndef OverhauserHPP 00002 #define OverhauserHPP 00003 00004 #include <iostream> 00005 #include <string> 00006 #include <vector> 00007 #include <set> 00008 #include <map> 00009 #include <stdlib.h> 00010 #include <stdio.h> 00011 00012 #include "Point.h" 00013 #include "Operations.h" 00014 00015 #define overbose 0 00016 #define BOUNDS(pp) { if (pp < 0) pp = 0; else if (pp >= int(nodes.size())-1) pp = int(nodes.size())-1; } 00017 00024 template <class DataType, int dim> 00025 class Spline {//<DataType,dim> { 00026 00027 public: 00028 00029 // Constructors and destructor 00030 Spline() { 00031 ltemp.resize(dim,0.); 00032 length = 0.; 00033 slength.clear(); 00034 } 00035 00036 Spline(const Spline& s) { 00037 for (int i = 0; i < (int)s.nodes.size(); i++) 00038 nodes.push_back(s.nodes[i]); 00039 delta_t = s.delta_t; 00040 ltemp.resize(3,0.); 00041 length = 0.; 00042 slength = s.slength; 00043 } 00044 00045 ~Spline() {} 00046 00048 virtual BPType Eq(DataType t, const BPType& p1, const BPType& p2, const BPType& p3, const BPType& p4) { 00049 DataType t2 = t * t; 00050 DataType t3 = t2 * t; 00051 00052 DataType b1 = .5 * ( -t3 + 2*t2 - t); 00053 DataType b2 = .5 * ( 3*t3 - 5*t2 + 2); 00054 DataType b3 = .5 * (-3*t3 + 4*t2 + t); 00055 DataType b4 = .5 * ( t3 - t2 ); 00056 00057 return (p1*b1 + p2*b2 + p3*b3 + p4*b4); 00058 } 00059 00060 virtual PType Eq(DataType t, const PType& p1, const PType& p2, const PType& p3, const PType& p4) { 00061 BPType Bp1(3,0.), Bp2(3,0.), Bp3(3,0.), Bp4(3,0.), result(3,0.); 00062 PType answer(0.); 00063 toBoost3(p1,Bp1); 00064 toBoost3(p2,Bp2); 00065 toBoost3(p3,Bp3); 00066 toBoost3(p4,Bp4); 00067 result = Eq(t,Bp1,Bp2,Bp3,Bp4); 00068 toCPT3(result,answer); 00069 return answer; 00070 } 00071 00072 virtual void AddSplineNode(const PType& v) 00073 { 00074 nodes.push_back(v);//tmp); 00075 delta_t = (DataType) 1.; 00076 delta_s = (DataType) 1.; 00077 length = (DataType) 0.; 00078 00079 if ( nodes.size() == 1) { 00080 delta_t = (DataType) 1.; 00081 delta_s = (DataType) 1.; 00082 length = (DataType) 0.; 00083 } 00084 else if (nodes.size() > 1) { 00085 length = slength[slength.size()-1] + (DataType)(norm_2( nodes[nodes.size()-1] - nodes[nodes.size()-2] )); 00086 delta_s = ((DataType)1 / length ); 00087 delta_t = (DataType)1 / (DataType)(nodes.size()-1); 00088 } 00089 slength.push_back(length); 00090 if (overbose) { 00091 for (int l=0; l < int(nodes.size()); l++) 00092 ( mklog()<< "\tslength[" << l << "]=" << slength[l] << std::endl 00093 <<"Length = "<<length<<std::endl).flush(); 00094 } 00095 00096 } 00097 00099 virtual PType GetInterpolatedSplineNode(DataType t) 00100 { 00101 // Find out in which interval we are on the spline 00102 int p = (int)(t / delta_t); 00103 00104 if (overbose) printf("GetInterpolatedSplineNode t=%g delta_t=%g p=%d nodes.size()=%lu",t,delta_t,p,nodes.size()); 00105 00106 // Compute local control point indices 00107 int p0 = p - 1; BOUNDS(p0); 00108 int p1 = p; BOUNDS(p1); 00109 int p2 = p + 1; BOUNDS(p2); 00110 int p3 = p + 2; BOUNDS(p3); 00111 if (p >= int(nodes.size())-3) { 00112 printf("\tSHIFT Low\n"); 00113 int p0 = p - 2; BOUNDS(p0); 00114 int p1 = p - 1; BOUNDS(p1); 00115 int p2 = p; BOUNDS(p2); 00116 int p3 = p + 1; BOUNDS(p3); 00117 } 00118 // Relative (local) time 00119 DataType lt = (t - delta_t*(DataType)p) / delta_t; 00120 if (overbose) printf(" lt=%g\n",lt); 00121 // Interpolate 00122 if (overbose) printf(" p0=%d p1=%d p2=%d p3=%d\n",p0,p1,p2,p3); 00123 return Eq(lt, nodes[p0], nodes[p1], nodes[p2], nodes[p3]); 00124 } 00125 00127 virtual PType GetInterpolatedSplineNode_dist(DataType t) 00128 { 00129 // Find out in which interval we are on the spline 00130 int p = (int)(t / delta_t); 00131 int k=0; 00132 00133 for (k = 0; k < int(slength.size()); k++) { 00134 if (t < slength[k]/length ) { 00135 if (overbose) printf("k=%d t=%g slength[%d]=%g ratio=%g\n",k,t,k,slength[k],slength[k]/length); 00136 break; 00137 } 00138 } 00139 00140 if (overbose) printf("\nGetInterpolatedSplineNode_dist t=%g delta_t=%g p=%d k=%d nodes.size()=%lu",t,delta_t,p,k,nodes.size()); 00141 00142 p = k-1; 00143 // Compute local control point indices 00144 int p0 = p - 1; BOUNDS(p0); 00145 int p1 = p; BOUNDS(p1); 00146 int p2 = p + 1; BOUNDS(p2); 00147 int p3 = p + 2; BOUNDS(p3); 00148 if (p >= int(nodes.size())-3) { 00149 if (overbose) printf("\tSHIFT Low\n"); 00150 int p0 = p - 2; BOUNDS(p0); 00151 int p1 = p - 1; BOUNDS(p1); 00152 int p2 = p; BOUNDS(p2); 00153 int p3 = p + 1; BOUNDS(p3); 00154 } 00155 // Relative (local) time 00156 DataType ls = (t*length - slength[k-1]) / (slength[k]-slength[k-1]); 00157 if (overbose) printf("p=%d ls=%g\n",p,ls); 00158 // Interpolate 00159 if (overbose) printf(" p0=%d p1=%d p2=%d p3=%d\n",p0,p1,p2,p3); 00160 return Eq(ls, nodes[p0], nodes[p1], nodes[p2], nodes[p3]); 00161 } 00162 00164 virtual BPType linear(DataType t, const BPType& p1, const BPType& p2 ) { //, const BPType& p3, const BPType& p4) { 00165 BPType tmp = p2-p1; 00166 tmp = normalize(tmp); 00167 tmp *= t; 00168 tmp += p1; 00169 00170 return tmp; //( p1 + t*norm_2(p2-p1) );// (p1*b1 + p2*b2 + p3*b3 + p4*b4); 00171 } 00172 00173 virtual PType linear(DataType t, const PType& p1, const PType& p2) { //, const PType& p3, const PType& p4) { 00174 BPType Bp1(3,0.), Bp2(3,0.), result(3,0.); //, Bp3(3,0.), Bp4(3,0.), result(3,0.); 00175 PType answer(0.); 00176 toBoost3(p1,Bp1); 00177 toBoost3(p2,Bp2); 00178 result = linear(t,Bp1,Bp2); 00179 toCPT3(result,answer); 00180 return answer; 00181 } 00182 00184 virtual PType GetInterpolatedSplineNode_distLinear(DataType t) 00185 { 00186 // Find out in which interval we are on the spline 00187 int p = (int)(t / delta_t); 00188 int k=0; 00189 00190 for (k = 0; k < int(slength.size()); k++) { 00191 if (t < slength[k]/length ) { 00192 if (overbose) printf("k=%d t=%g slength[%d]=%g ratio=%g\n",k,t,k,slength[k],slength[k]/length); 00193 break; 00194 } 00195 } 00196 00197 if (overbose) printf("\nGetInterpolatedSplineNode_dist t=%g delta_t=%g p=%d k=%d nodes.size()=%lu",t,delta_t,p,k,nodes.size()); 00198 00199 p = k-1; 00200 // Compute local control point indices 00201 int p1 = p; BOUNDS(p1); 00202 int p2 = p + 1; BOUNDS(p2); 00203 00204 // Relative (local) time 00205 DataType ls = (t*length - slength[p]) / (slength[k]-slength[p]); 00206 if (overbose) printf("p=%d ls=%g\n",p,ls); 00207 // Interpolate 00208 if (overbose) printf(" p1=%d p2=%d \n",p1,p2);//,p2,p3); 00209 return linear(ls,nodes[p1],nodes[p2]); 00210 } 00211 00212 virtual int GetNumSplineNodes() 00213 { 00214 return nodes.size(); 00215 } 00216 00217 virtual PType GetNthSplineNode(int n) 00218 { 00219 return nodes[n]; 00220 } 00221 00222 virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) { 00223 #ifdef CHECKRESTART_DEBUG_ON 00224 ( mklog() << " Overhauser Restart\n" ).flush(); 00225 #endif 00226 ifs.seekg(pos); 00227 ifs.read((char*)<emp(0),sizeof(DataType)); 00228 ifs.read((char*)<emp(1),sizeof(DataType)); 00229 ifs.read((char*)<emp(2),sizeof(DataType)); 00230 ifs.read((char*)&delta_t,sizeof(DataType)); 00231 ifs.read((char*)&delta_s,sizeof(DataType)); 00232 00233 ifs.read((char*)&num_nodes,sizeof(int)); 00234 ifs.read((char*)&num_slength,sizeof(int)); 00235 for (register int i=0; i < num_nodes; i++) { 00236 ifs.read((char*)&nodes[i],sizeof(int)); 00237 } 00238 for (register int i=0; i < num_slength; i++) { 00239 ifs.read((char*)&slength[i],sizeof(int)); 00240 } 00241 pos = ifs.tellg(); 00242 } 00243 00244 virtual void Checkpointing(std::ofstream& ofs) { 00245 #ifdef CHECKRESTART_DEBUG_ON 00246 ( mklog() << "\nCHECKPOINTING Overhauser " << std::endl ).flush(); 00247 #endif 00248 ofs.write((char*)<emp(0),sizeof(DataType)); 00249 ofs.write((char*)<emp(1),sizeof(DataType)); 00250 ofs.write((char*)<emp(2),sizeof(DataType)); 00251 ofs.write((char*)&delta_t,sizeof(DataType)); 00252 ofs.write((char*)&delta_s,sizeof(DataType)); 00253 00254 num_nodes=nodes.size(); 00255 num_slength=slength.size(); 00256 ofs.write((char*)&num_nodes,sizeof(int)); 00257 ofs.write((char*)&num_slength,sizeof(int)); 00258 for (register int i=0; i < num_nodes; i++) { 00259 ofs.write((char*)&nodes[i],sizeof(int)); 00260 } 00261 for (register int i=0; i < num_slength; i++) { 00262 ofs.write((char*)&slength[i],sizeof(int)); 00263 } 00264 } 00265 00266 public: 00267 std::vector<PType> nodes; 00268 std::vector <DataType> slength; 00269 DataType delta_t, delta_s, length; 00270 BPType ltemp; 00271 int num_nodes, num_slength; 00272 }; 00273 00274 #endif
Generated on Thu Jun 30 02:18:58 2016 for Motion - rigid body dynamics solver by
1.6.3