|
qtrigid.h00001 /* 00002 Dynamics/Kinematics modeling and simulation library. 00003 Copyright (C) 1999 by Michael Alexander Ewert and Noah Gibbs 00004 00005 This library is free software; you can redistribute it and/or 00006 modify it under the terms of the GNU Library General Public 00007 License as published by the Free Software Foundation; either 00008 version 2 of the License, or (at your option) any later version. 00009 00010 This library is distributed in the hope that it will be useful, 00011 but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00013 Library General Public License for more details. 00014 00015 You should have received a copy of the GNU Library General Public 00016 License along with this library; if not, write to the Free 00017 Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00018 00019 */ 00020 00021 #ifndef __CT_QUATRIGIDBODY_H__ 00022 #define __CT_QUATRIGIDBODY_H__ 00023 00024 #include "csphyzik/phyztype.h" 00025 #include "csphyzik/entity.h" 00026 #include "csphyzik/ctquat.h" 00027 #include "csgeom/quaterni.h" 00028 00029 #define QUATRIGID_STATE_SIZE 13 00030 00031 class ctQuatRigidBodyConnector; 00032 00033 class ctQuatRigidBody : public ctEntity 00034 { 00035 public: 00037 ctQuatRigidBody (ctVector3 x = ctVector3(0,0,0), real M = 0); 00038 ~ctQuatRigidBody () {} 00039 00041 real get_mass () 00042 { return mass; } 00043 00044 void set_mass (real m) 00045 { mass = m; precalculated = false; } 00046 00047 ctVector3 get_center () 00048 { return pos; } 00049 00050 void set_center (ctVector3 x) 00051 { pos = x; } 00052 00053 ctVector3 get_linear_velocity () 00054 { return mom / mass; } 00055 00056 void set_linear_velocity (ctVector3 vel) 00057 { mom = vel * mass; } 00058 00059 ctVector3 get_linear_momentum (void) 00060 { return mom; } 00061 00062 void set_linear_momentum (ctVector3 newmom) 00063 { mom = newmom; } 00064 00065 ctQuaternion get_orientation () 00066 { return quat; } 00067 00068 void set_orientation (ctQuaternion ori) 00069 { 00070 quat = ori; 00071 quat.Normalize(); 00072 precalculated = false; 00073 } 00074 00075 ctVector3 get_rotation() 00076 { 00077 real angle = 2.0 * acos(quat.r); 00078 real fact = angle / sin(angle); 00079 return ctVector3(quat.x * fact, quat.y * fact, quat.z*fact); 00080 } 00081 00082 void set_rotation (ctVector3 rot) 00083 { 00084 real len = rot.Norm (); 00085 if (len <= 3*MIN_REAL) 00086 set_orientation(ctQuaternion(1.0f, 0.0f, 0.0f, 0.0f)); 00087 else 00088 { 00089 rot /= len; 00090 float sine = (float) sin(len); 00091 set_orientation(ctQuaternion((float) cos(len / 2.0f), 00092 sine * rot[0], sine * rot[1], sine * rot[2])); 00093 } 00094 } 00095 00096 ctMatrix3 get_R () 00097 { 00098 Precalculate(); 00099 return R; 00100 } 00101 00102 void set_R (ctMatrix3& new_R) 00103 { 00104 quat.from_matrix(new_R); 00105 precalculated = false; 00106 } 00107 00108 ctMatrix3 get_Ibody (void) 00109 { return Ibody; } 00110 00111 void set_Ibody (const ctMatrix3& Ib) 00112 { 00113 Ibody = Ib; 00114 Ibodyinv = Ibody.inverse(); 00115 precalculated = false; 00116 } 00117 00118 ctMatrix3 get_Ibodyinv (void) 00119 { return Ibodyinv; } 00120 00121 ctMatrix3 get_Iinv (void) 00122 { 00123 Precalculate(); 00124 return Iinv; 00125 } 00126 00127 ctVector3 get_angular_velocity () 00128 { 00129 Precalculate(); 00130 return omega; 00131 } 00132 00133 void set_angular_velocity (ctVector3 new_omega) 00134 { 00135 // Omega = Iinv * ang_mom 00136 // Iinv * ang_mom = new_omega 00137 // ang_mom = I * new_omega 00138 // I = R * Ibody * R^T 00139 Precalculate (); 00140 ctMatrix3 I(R * Ibody * R.get_transpose()); 00141 ang = I * new_omega; 00142 precalculated = false; 00143 } 00144 00145 // Workhorse functions 00146 void init_state () 00147 { 00148 F[0] = F[1] = F[2] = 0.0; 00149 T[0] = T[1] = T[2] = 0.0; 00150 } 00151 00152 int get_state_size () 00153 { return QUATRIGID_STATE_SIZE; } 00154 00155 int get_state (const float *sa); 00156 int set_state (real *sa); 00157 int set_delta_state (real *sa); 00158 00159 void apply_F (ctVector3 newF) 00160 { F += newF; } 00161 00162 void apply_T (ctVector3 newT) 00163 { T += newT; } 00164 00165 // Connector method 00166 ctQuatRigidBodyConnector *new_connector (ctVector3 offs); 00167 00168 // Shorthand methods, just call other methods 00169 inline ctVector3 get_velocity () 00170 { return get_linear_velocity(); } 00171 00172 inline void set_velocity (ctVector3 vel) 00173 { set_linear_velocity(vel); } 00174 00175 inline ctVector3 get_omega () 00176 { return get_angular_velocity(); } 00177 00178 inline void set_omega (ctVector3 new_omega) 00179 { set_angular_velocity(new_omega); } 00180 00181 protected: 00183 void Precalculate (); 00184 00185 real mass; 00187 ctVector3 pos; 00189 ctVector3 mom; 00191 ctQuaternion quat; 00193 ctVector3 ang; 00194 00196 ctVector3 F; 00198 ctVector3 T; 00200 ctMatrix3 Ibody; 00201 00203 bool precalculated; 00205 ctMatrix3 Ibodyinv; 00206 ctMatrix3 R; 00208 ctMatrix3 Iinv; 00210 ctVector3 omega; 00211 00212 }; 00213 00214 #endif // __CT_QUATRIGIDBODY_H__ Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000 |