00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "angularEulerIntegrator.h"
00016 #include "forceNode.h"
00017 #include "physicalNode.h"
00018 #include "config_physics.h"
00019
00020
00021
00022
00023
00024
00025 AngularEulerIntegrator::
00026 AngularEulerIntegrator() {
00027 }
00028
00029
00030
00031
00032
00033
00034 AngularEulerIntegrator::
00035 ~AngularEulerIntegrator() {
00036 }
00037
00038
00039
00040
00041
00042
00043
00044
00045 void AngularEulerIntegrator::
00046 child_integrate(Physical *physical,
00047 AngularForceVector& forces,
00048 PN_stdfloat dt) {
00049
00050
00051
00052
00053
00054
00055
00056 PhysicsObject::Vector::const_iterator current_object_iter;
00057 current_object_iter = physical->get_object_vector().begin();
00058 for (; current_object_iter != physical->get_object_vector().end();
00059 ++current_object_iter) {
00060 PhysicsObject *current_object = *current_object_iter;
00061
00062
00063
00064 if (current_object == (PhysicsObject *) NULL) {
00065 continue;
00066 }
00067
00068 if (current_object->get_active() == false) {
00069 continue;
00070 }
00071
00072 LRotation accum_quat(0, 0, 0, 0);
00073
00074
00075 AngularForceVector::const_iterator f_cur;
00076
00077 LRotation f;
00078
00079
00080 f_cur = forces.begin();
00081
00082 for (; f_cur != forces.end(); ++f_cur) {
00083 AngularForce *cur_force = *f_cur;
00084
00085
00086 if (cur_force->get_active() == false) {
00087 continue;
00088 }
00089
00090
00091 f = cur_force->get_quat(current_object);
00092 accum_quat += f;
00093 }
00094
00095 LOrientation orientation = current_object->get_orientation();
00096
00097 f_cur = physical->get_angular_forces().begin();
00098 for (; f_cur != physical->get_angular_forces().end(); ++f_cur) {
00099 AngularForce *cur_force = *f_cur;
00100
00101
00102 if (cur_force->get_active() == false) {
00103 continue;
00104 }
00105
00106 f = cur_force->get_quat(current_object);
00107
00108
00109
00110 accum_quat += orientation.xform(f);
00111 }
00112
00113
00114
00115
00116 accum_quat = current_object->get_inertial_tensor() * accum_quat;
00117
00118
00119 LRotation rot_quat = current_object->get_rotation();
00120 #if 0
00121 rot_quat += accum_quat * dt;
00122
00123 if (rot_quat.normalize()) {
00124 LOrientation old_orientation = current_object->get_orientation();
00125 LOrientation new_orientation = old_orientation * rot_quat;
00126 new_orientation.normalize();
00127
00128
00129 current_object->set_orientation(new_orientation);
00130 current_object->set_rotation(rot_quat);
00131 }
00132 #else
00133
00134
00135
00136
00137
00138 orientation = orientation * ((rot_quat * dt) * (accum_quat * (0.5 * dt * dt)));
00139
00140 rot_quat = rot_quat + (accum_quat * dt);
00141
00142
00143 if (orientation.normalize() && rot_quat.normalize()) {
00144
00145 current_object->set_orientation(orientation);
00146 current_object->set_rotation(rot_quat);
00147 }
00148 #endif
00149 }
00150 }
00151
00152
00153
00154
00155
00156
00157
00158 void AngularEulerIntegrator::
00159 output(ostream &out) const {
00160 #ifndef NDEBUG //[
00161 out<<"AngularEulerIntegrator (id "<<this<<")";
00162 #endif //] NDEBUG
00163 }
00164
00165
00166
00167
00168
00169
00170
00171 void AngularEulerIntegrator::
00172 write(ostream &out, unsigned int indent) const {
00173 #ifndef NDEBUG //[
00174 out.width(indent); out<<""; out<<"AngularEulerIntegrator:\n";
00175 AngularIntegrator::write(out, indent+2);
00176 #endif //] NDEBUG
00177 }