1. #include "Euler.h"
  2. rab::Euler::Euler()
  3. {
  4. 	currentSimTime=0;
  5. }
  6.  
  7. rab::Euler::Euler(rab::vector3D startFval)
  8. {
  9. 	currentSimTime=0;
  10. 	tees.push_back(0);
  11. 	effs.push_back(startFval);
  12. }
  13.  
  14. void rab::Euler::clearHistory()
  15. {
  16. 	currentSimTime=0;
  17. 	tees.clear();
  18. 	effs.clear();
  19. }
  20. void rab::Euler::setStartEff(rab::vector3D startFval)
  21. {
  22. 	if(effs.empty())
  23. 	{
  24. 		tees.push_back(0);
  25. 		effs.push_back(startFval);
  26. 	}
  27.  
  28. }
  29. void rab::Euler::setStepLength(double t)
  30. {
  31. 	if(t>0)	stepLength=t;
  32. 	else stepLength=1;
  33. }
  34.  
  35. vector<double> rab::Euler::getTees()
  36. {
  37. 	return tees;
  38. }
  39. vector<rab::vector3D> rab::Euler::getEffs()
  40. {
  41. 	return effs;
  42. }
  43. void rab::Euler::updateTo(double d)
  44. {
  45. 	while(currentSimTime<d)
  46. 	{		
  47. 		step();
  48.  
  49.  
  50.  
  51. 	}
  52. }
  53. rab::vector3D rab::Euler::evaluateAt(double d)
  54. {
  55. 	if(d<0) return effs.at(0);
  56. 	if(currentSimTime<d) updateTo(d);
  57. 	if(tees.back()==d) return effs.back();
  58. 	else
  59. 	{
  60. 		//track back along tees until find first t <d
  61. 		vector<double>::reverse_iterator itTee=tees.rbegin();
  62. 		vector<rab::vector3D>::reverse_iterator itF=effs.rbegin();
  63. 		while(*itTee>d && itTee<(tees.rend()-1))
  64. 		{
  65. 			++itTee;
  66. 			++itF;
  67. 		}
  68. 		//use iteration equation to find eff at d 
  69. 		double tempStep=d-*itTee;
  70. 		if(tempStep<0) cout<<"Warning evaluate call made with out of range parameter"<<endl;
  71. 		return *itF +tempStep*RHS(*itF,*itTee);
  72. 	}
  73. }
  74.  
  75. void rab::Euler::step()
  76. {
  77. 	effs.push_back(effs.back()+stepLength*RHS(effs.back(),tees.back()));
  78. 	currentSimTime+=stepLength;
  79. 	tees.push_back(currentSimTime);
  80. 	//if the vectors are too long remove half of the data
  81. 		if(effs.size()>MAXRECORDS)
  82. 		{
  83. 			int removed=MAXRECORDS/2;
  84. 			vector<rab::vector3D>::const_iterator it=effs.begin();
  85. 			vector<rab::vector3D>::const_iterator it1=it;
  86. 			it1+=removed;
  87. 			effs.erase(it,it1);
  88.  
  89. 			vector<double>::const_iterator it3=tees.begin();
  90. 			vector<double>::const_iterator it4=it3;
  91. 			it4+=removed;
  92. 			tees.erase(it3,it4);
  93. 		}
  94. 	//cout<<"Updating euler step "<<currentSimTime<<endl;//debug line
  95.  
  96. }
  97.  
  98. rab::vector3D rab::Euler::getCurrentFValue()
  99. {
  100. 	return effs.back();
  101. }
  102.  
  103. rab::MotionSolver::MotionSolver()
  104. {
  105. }
  106. rab::MotionSolver::MotionSolver(rab::vector3D pos,rab::vector3D vel ,Euler* pE)
  107. 	{
  108. 		pVelSol=pE;
  109. 		pVelSol->setStartEff(vel);
  110. 		setStartEff(pos);
  111. 	}
  112. 	void rab::MotionSolver::setStepLength(double step)
  113. 	{
  114. 		stepLength=step;
  115. 		pVelSol->setStepLength(step);
  116. 	}
  117.  
  118.  
  119. 	rab::vector3D rab::MotionSolver::RHS(rab::vector3D v,double d)
  120. 	{
  121. 		return pVelSol->evaluateAt(d);
  122. 	}
  123.  
  124.  
  125. 	rab::vector3D rab::MotionSolver::getVelocityAt(double d)
  126. 	{
  127. 		return pVelSol->evaluateAt(d);
  128. 	}
  129.  
  130. 	rab::vector3D rab::MotionSolver::getCurrentVelocity()
  131. 	{
  132. 		return pVelSol->getCurrentFValue();
  133. 	}
  134. 	rab::vector3D rab::MotionSolver::getCurrentPosition()
  135. 	{
  136. 		return getCurrentFValue();
  137. 	}
  138.  
  139. 	void rab::MotionSolver::step()
  140. 	{
  141. 		effs.push_back(effs.back()+stepLength*(pVelSol->getCurrentFValue()));
  142. 		currentSimTime+=stepLength;
  143. 		tees.push_back(currentSimTime);
  144. 		pVelSol->step();
  145. 		//if the vectors are too long remove half of the data
  146. 		if(effs.size()>MAXRECORDS)
  147. 		{
  148. 			int removed=MAXRECORDS/2;
  149. 			vector<rab::vector3D>::const_iterator it=effs.begin();
  150. 			vector<rab::vector3D>::const_iterator it1=it;
  151. 			it1+=removed;
  152. 			effs.erase(it,it1);
  153.  
  154. 			vector<double>::const_iterator it3=tees.begin();
  155. 			vector<double>::const_iterator it4=it3;
  156. 			it4+=removed;
  157. 			tees.erase(it3,it4);
  158. 		}
  159. 		//cout<<"Updating solver step "<<currentSimTime<<endl;//debug line
  160. 	}
  161.  
  162. //-----------------------------------------------------------------------------------
  163. //implementation of user defined classes below here
  164.  
  165. rab::vector3D EulerSimple::RHS(rab::vector3D v,double t)
  166. {
  167. 	return -1*v;
  168. }
  169.  
  170. //the EulerWindBullet class methods------------------------
  171.  
  172. void EulerWindBullet::setBulletMass(double m){bulletMass=m;}
  173. void EulerWindBullet::setARcoeff(double coeff){ARcoeff=coeff;}
  174. void EulerWindBullet::setWRcoeff(double coeff){WRcoeff=coeff;}
  175. void EulerWindBullet::setVeePow(double pow){veePow=pow;}
  176. void EulerWindBullet::setWindVel(rab::vector3D vel){windVel=vel;}
  177.  
  178. //you must write the next method - it has a holding function supplied 
  179. //to allow the code to compile
  180. rab::vector3D EulerWindBullet::RHS(rab::vector3D v,double t)
  181. {
  182. 	double mass = bulletMass;
  183. 	double gravity = 10; 
  184. 	double x = ( 1.F/mass ) *  ( windVel.getX() * WRcoeff ) - (ARcoeff* v.getX());
  185. 	double y = ( 1.F/mass ) *  ( windVel.getY() * WRcoeff ) - (ARcoeff* v.getY());
  186. 	double z = ( 1.F/mass ) *  ( windVel.getZ() * WRcoeff ) - (ARcoeff* v.getZ());
  187. 	return rab::vector3D( x, y-gravity, z);
  188.  
  189.  
  190. }