1 #pragma once 2 #include "dbl.h" 3 #include "quaternion.h" 4 #include "mathvector.h" 5 #include "matrix3.h" 6 7 //#define NSV 8 //#define MODIFIEDVERLET 9 #define SUVAT 10 //#define EULER 11 //Samuel R. Buss second order angular momentum(and energy) preserving integrator 12 //#define SECOND_ORDER 13 14 15 class ROTATIONALFRAME 16 { 17 private: 18 //primary 19 QUATERNION<Dbl> orientation; 20 MATHVECTOR<Dbl,3> angular_momentum; 21 MATHVECTOR<Dbl,3> torque; 22 23 //secondary 24 MATHVECTOR<Dbl,3> old_torque; //this is only state information for the verlet-like integrators 25 MATRIX3 <Dbl> orientmat; ///< 3x3 orientation matrix generated during inertia tensor rotation to worldspace and cached here 26 MATRIX3 <Dbl> world_inverse_inertia_tensor; ///< inverse inertia tensor in worldspace, cached here 27 MATRIX3 <Dbl> world_inertia_tensor; 28 MATHVECTOR<Dbl,3> angular_velocity; ///< calculated from angular_momentum, cached here 29 30 // constants 31 MATRIX3 <Dbl> inverse_inertia_tensor; //used for calculations every frame 32 MATRIX3 <Dbl> inertia_tensor; //used for the GetInertia function only 33 34 //housekeeping 35 bool have_old_torque; 36 int integration_step; 37 RecalculateSecondary()38 void RecalculateSecondary() 39 { 40 old_torque = torque; 41 have_old_torque = true; 42 orientation.GetMatrix3(orientmat); 43 world_inverse_inertia_tensor = orientmat.Transpose().Multiply(inverse_inertia_tensor).Multiply(orientmat); 44 world_inertia_tensor = orientmat.Transpose().Multiply(inertia_tensor).Multiply(orientmat); 45 angular_velocity = GetAngularVelocityFromMomentum(angular_momentum); 46 } 47 48 ///this call depends on having orientmat and world_inverse_inertia_tensor calculated GetAngularVelocityFromMomentum(const MATHVECTOR<Dbl,3> & moment)49 MATHVECTOR<Dbl,3> GetAngularVelocityFromMomentum(const MATHVECTOR<Dbl,3> & moment) const 50 { 51 return world_inverse_inertia_tensor.Multiply(moment); 52 } 53 GetSpinFromMomentum(const MATHVECTOR<Dbl,3> & ang_moment)54 QUATERNION<Dbl> GetSpinFromMomentum(const MATHVECTOR<Dbl,3> & ang_moment) const 55 { 56 const MATHVECTOR<Dbl,3> ang_vel = GetAngularVelocityFromMomentum(ang_moment); 57 QUATERNION<Dbl> qav = QUATERNION<Dbl> (ang_vel[0], ang_vel[1], ang_vel[2], 0); 58 return (qav * orientation) * 0.5; 59 } 60 61 public: ROTATIONALFRAME()62 ROTATIONALFRAME() : have_old_torque(false),integration_step(0) {} 63 SetInertia(const MATRIX3<Dbl> & inertia)64 void SetInertia(const MATRIX3 <Dbl> & inertia) 65 { 66 //inertia.DebugPrint(std::cout); 67 MATHVECTOR<Dbl,3> angvel_old = GetAngularVelocityFromMomentum(angular_momentum); 68 inertia_tensor = inertia; 69 inverse_inertia_tensor = inertia_tensor.Inverse(); 70 world_inverse_inertia_tensor = orientmat.Transpose().Multiply(inverse_inertia_tensor).Multiply(orientmat); 71 world_inertia_tensor = orientmat.Transpose().Multiply(inertia_tensor).Multiply(orientmat); 72 angular_momentum = world_inertia_tensor.Multiply(angvel_old); 73 angular_velocity = GetAngularVelocityFromMomentum(angular_momentum); 74 } 75 GetInertia()76 const MATRIX3 <Dbl> & GetInertia() const 77 { 78 return world_inertia_tensor; 79 } GetInertiaConst()80 const MATRIX3 <Dbl> & GetInertiaConst() const 81 { 82 return inertia_tensor; 83 } 84 SetOrientation(const QUATERNION<Dbl> & neworient)85 void SetOrientation(const QUATERNION<Dbl> & neworient) 86 { 87 orientation = neworient; 88 } 89 SetAngularVelocity(const MATHVECTOR<Dbl,3> & newangvel)90 void SetAngularVelocity(const MATHVECTOR<Dbl,3> & newangvel) 91 { 92 angular_momentum = world_inertia_tensor.Multiply(newangvel); 93 angular_velocity = newangvel; 94 } 95 GetAngularVelocity()96 const MATHVECTOR<Dbl,3> GetAngularVelocity() const 97 { 98 return angular_velocity; 99 } 100 101 ///this is a modified velocity verlet integration method that relies on a two-step calculation. 102 /// both steps must be executed each frame and forces can only be set between steps 1 and 2 Integrate1(const Dbl & dt)103 void Integrate1(const Dbl & dt) 104 { 105 assert(integration_step == 0); 106 107 assert (have_old_torque); //you'll get an assert problem unless you call SetInitialTorque at the beginning of the simulation 108 109 #ifdef MODIFIEDVERLET 110 orientation = orientation + GetSpinFromMomentum(angular_momentum + old_torque*dt*0.5)*dt; 111 orientation.Normalize(); 112 angular_momentum = angular_momentum + old_torque * dt * 0.5; 113 RecalculateSecondary(); 114 #endif 115 116 integration_step++; 117 } 118 119 ///this is a modified velocity verlet integration method that relies on a two-step calculation. 120 /// both steps must be executed each frame and forces must be set between steps 1 and 2 Integrate2(const Dbl & dt)121 void Integrate2(const Dbl & dt) 122 { 123 assert(integration_step == 1); 124 #ifdef MODIFIEDVERLET 125 angular_momentum = angular_momentum + torque * dt * 0.5; 126 #endif 127 128 #ifdef NSV 129 //simple NSV integration 130 angular_momentum = angular_momentum + torque * dt; 131 orientation = orientation + GetSpinFromMomentum(angular_momentum)*dt; 132 orientation.Normalize(); 133 #endif 134 135 #ifdef EULER 136 orientation = orientation + GetSpinFromMomentum(angular_momentum)*dt; 137 orientation.Normalize(); 138 angular_momentum = angular_momentum + torque * dt; 139 #endif 140 141 #ifdef SUVAT 142 //orientation = orientation + GetSpinFromMomentum(angular_momentum)*dt + GetSpinFromMomentum(torque)*dt*dt*0.5; 143 orientation = orientation + GetSpinFromMomentum(angular_momentum + torque*dt*0.5)*dt; 144 orientation.Normalize(); 145 angular_momentum = angular_momentum + torque * dt; 146 #endif 147 148 #ifdef SECOND_ORDER 149 MATHVECTOR<Dbl,3> ang_acc = 150 world_inverse_inertia_tensor.Multiply(torque - angular_velocity.cross(angular_momentum)); 151 MATHVECTOR<Dbl,3> avg_rot = 152 angular_velocity + ang_acc * dt/2.0 + ang_acc.cross(angular_velocity) * dt * dt/12.0; 153 QUATERNION<Dbl> dq = 154 QUATERNION<Dbl>(avg_rot[0], avg_rot[1], avg_rot[2], 0) * orientation * 0.5 * dt; 155 orientation = orientation + dq; 156 orientation.Normalize(); 157 #endif 158 // update angular velocity, inertia 159 RecalculateSecondary(); 160 161 integration_step = 0; 162 torque.Set(0.0); 163 } 164 165 ///this must only be called between integrate1 and integrate2 steps GetLockUpTorque(const Dbl dt)166 const MATHVECTOR<Dbl,3> GetLockUpTorque(const Dbl dt) const 167 { 168 #ifdef MODIFIEDVERLET 169 return -angular_momentum * 2 / dt; 170 #else 171 return -angular_momentum / dt; 172 #endif 173 } 174 175 ///this must only be called between integrate1 and integrate2 steps ApplyTorque(const MATHVECTOR<Dbl,3> & t)176 void ApplyTorque(const MATHVECTOR<Dbl,3> & t) 177 { 178 assert(integration_step == 1); 179 torque = torque + t; 180 } 181 SetTorque(const MATHVECTOR<Dbl,3> & t)182 void SetTorque(const MATHVECTOR<Dbl,3> & t) 183 { 184 assert(integration_step == 1); 185 torque = t; 186 } 187 GetTorque()188 const MATHVECTOR<Dbl,3> & GetTorque() 189 { 190 return old_torque; 191 } 192 193 ///this must be called once at sim start to set the initial torque present SetInitialTorque(const MATHVECTOR<Dbl,3> & t)194 void SetInitialTorque(const MATHVECTOR<Dbl,3> & t) 195 { 196 assert(integration_step == 0); 197 198 old_torque = t; 199 have_old_torque = true; 200 } 201 GetOrientation()202 const QUATERNION<Dbl> & GetOrientation() const 203 { 204 return orientation; 205 } 206 }; 207