00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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.0, 0.0, 0.0, 0.0));
00087 else
00088 {
00089 rot /= len;
00090 real sine = sin(len);
00091 set_orientation(ctQuaternion(cos(len / 2), sine*rot[0], sine*rot[1],
00092 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
00136
00137
00138
00139 Precalculate ();
00140 ctMatrix3 I(R * Ibody * R.get_transpose());
00141 ang = I * new_omega;
00142 precalculated = false;
00143 }
00144
00145
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 real *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
00166 ctQuatRigidBodyConnector *new_connector (ctVector3 offs);
00167
00168
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__