00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "linearEulerIntegrator.h"
00016 #include "forceNode.h"
00017 #include "physicalNode.h"
00018 #include "config_physics.h"
00019
00020
00021
00022
00023
00024
00025 LinearEulerIntegrator::
00026 LinearEulerIntegrator() {
00027 }
00028
00029
00030
00031
00032
00033
00034 LinearEulerIntegrator::
00035 ~LinearEulerIntegrator() {
00036 }
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 void LinearEulerIntegrator::
00055 child_integrate(Physical *physical,
00056 LinearForceVector& forces,
00057 PN_stdfloat dt) {
00058
00059
00060
00061
00062
00063 precompute_linear_matrices(physical, forces);
00064 const MatrixVector &matrices = get_precomputed_linear_matrices();
00065 #ifndef NDEBUG
00066 MatrixVector::const_iterator mi;
00067 for (mi = matrices.begin(); mi != matrices.end(); ++mi) {
00068 nassertv(!(*mi).is_nan());
00069 }
00070 #endif // NDEBUG
00071
00072
00073 PN_stdfloat viscosityDamper=1.0f-physical->get_viscosity();
00074
00075
00076
00077
00078
00079
00080
00081
00082 PhysicsObject::Vector::const_iterator current_object_iter;
00083 current_object_iter = physical->get_object_vector().begin();
00084 for (; current_object_iter != physical->get_object_vector().end();
00085 ++current_object_iter) {
00086 PhysicsObject *current_object = *current_object_iter;
00087
00088
00089
00090 if (current_object == (PhysicsObject *) NULL) {
00091 continue;
00092 }
00093
00094 if (current_object->get_active() == false) {
00095 continue;
00096 }
00097
00098 LVector3 md_accum_vec;
00099 LVector3 non_md_accum_vec;
00100 LVector3 accel_vec;
00101 LVector3 vel_vec;
00102
00103
00104 md_accum_vec.set(0.0f, 0.0f, 0.0f);
00105 non_md_accum_vec.set(0.0f, 0.0f, 0.0f);
00106
00107
00108 LVector3 f;
00109
00110
00111 LinearForceVector::const_iterator f_cur;
00112
00113
00114 f_cur = forces.begin();
00115 int index = 0;
00116 for (; f_cur != forces.end(); ++f_cur) {
00117 LinearForce *cur_force = *f_cur;
00118
00119
00120 if (cur_force->get_active() == false) {
00121 continue;
00122 }
00123
00124
00125 f = cur_force->get_vector(current_object) * matrices[index++];
00126
00127 physics_spam("child_integrate "<<f);
00128
00129 if (cur_force->get_mass_dependent() == true) {
00130 md_accum_vec += f;
00131 } else {
00132 non_md_accum_vec += f;
00133 }
00134 }
00135
00136
00137 f_cur = physical->get_linear_forces().begin();
00138 for (; f_cur != physical->get_linear_forces().end(); ++f_cur) {
00139 LinearForce *cur_force = *f_cur;
00140
00141
00142 if (cur_force->get_active() == false) {
00143 continue;
00144 }
00145
00146
00147 f = cur_force->get_vector(current_object) * matrices[index++];
00148
00149 physics_spam("child_integrate "<<f);
00150
00151 if (cur_force->get_mass_dependent() == true) {
00152 md_accum_vec += f;
00153 } else {
00154 non_md_accum_vec += f;
00155 }
00156 }
00157
00158
00159 LPoint3 pos = current_object->get_position();
00160 vel_vec = current_object->get_velocity();
00161 PN_stdfloat mass = current_object->get_mass();
00162
00163
00164
00165 nassertv(mass != 0.0f);
00166 accel_vec = md_accum_vec / mass;
00167 accel_vec += non_md_accum_vec;
00168
00169 #if 0 //[
00170
00171 vel_vec += accel_vec * dt;
00172
00173
00174 PN_stdfloat len = vel_vec.length();
00175
00176 if (len > current_object->get_terminal_velocity()) {
00177
00178 vel_vec *= current_object->get_terminal_velocity() / len;
00179 }
00180
00181 pos += vel_vec * dt;
00182 #else //][
00183 assert(current_object->get_position()==current_object->get_last_position());
00184
00185 accel_vec*=viscosityDamper;
00186
00187
00188 pos += vel_vec * dt + 0.5 * accel_vec * dt * dt;
00189
00190 vel_vec += accel_vec * dt;
00191 #endif //]
00192
00193
00194 if (!pos.is_nan()) {
00195 current_object->set_position(pos);
00196 }
00197 if (!vel_vec.is_nan()) {
00198 current_object->set_velocity(vel_vec);
00199 }
00200 }
00201 }
00202
00203
00204
00205
00206
00207
00208
00209 void LinearEulerIntegrator::
00210 output(ostream &out) const {
00211 #ifndef NDEBUG //[
00212 out<<"LinearEulerIntegrator";
00213 #endif //] NDEBUG
00214 }
00215
00216
00217
00218
00219
00220
00221
00222 void LinearEulerIntegrator::
00223 write(ostream &out, unsigned int indent) const {
00224 #ifndef NDEBUG //[
00225 out.width(indent);
00226 out<<""<<"LinearEulerIntegrator:\n";
00227 LinearIntegrator::write(out, indent+2);
00228 #endif //] NDEBUG
00229 }
00230
00231
00232
00233
00234
00235
00236
00237