Panda3D
|
00001 // Filename: linearEulerIntegrator.cxx 00002 // Created by: charles (13Jun00) 00003 // 00004 //////////////////////////////////////////////////////////////////// 00005 // 00006 // PANDA 3D SOFTWARE 00007 // Copyright (c) Carnegie Mellon University. All rights reserved. 00008 // 00009 // All use of this software is subject to the terms of the revised BSD 00010 // license. You should have received a copy of this license along 00011 // with this source code in a file named "LICENSE." 00012 // 00013 //////////////////////////////////////////////////////////////////// 00014 00015 #include "linearEulerIntegrator.h" 00016 #include "forceNode.h" 00017 #include "physicalNode.h" 00018 #include "config_physics.h" 00019 00020 //////////////////////////////////////////////////////////////////// 00021 // Function : LinearEulerIntegrator 00022 // Access : Public 00023 // Description : constructor 00024 //////////////////////////////////////////////////////////////////// 00025 LinearEulerIntegrator:: 00026 LinearEulerIntegrator() { 00027 } 00028 00029 //////////////////////////////////////////////////////////////////// 00030 // Function : LinearEulerIntegrator 00031 // Access : Public 00032 // Description : destructor 00033 //////////////////////////////////////////////////////////////////// 00034 LinearEulerIntegrator:: 00035 ~LinearEulerIntegrator() { 00036 } 00037 00038 //////////////////////////////////////////////////////////////////// 00039 // Function : Integrate 00040 // Access : Public 00041 // Description : Integrate a step of motion (based on dt) by 00042 // applying every force in force_vec to every object 00043 // in obj_vec. 00044 // 00045 // physical, 00046 // The objects being acted upon and the 00047 // set of local forces that are applied 00048 // after the global forces. 00049 // forces, 00050 // Global forces to be applied first. 00051 // dt, 00052 // The delta time of this integration step. 00053 //////////////////////////////////////////////////////////////////// 00054 void LinearEulerIntegrator:: 00055 child_integrate(Physical *physical, 00056 LinearForceVector& forces, 00057 PN_stdfloat dt) { 00058 // perform the precomputation. Note that the vector returned by 00059 // get_precomputed_matrices() has the matrices loaded in order of force 00060 // type: first global, then local. If you're using this as a guide to write 00061 // another integrator, be sure to process your forces global, then local. 00062 // otherwise your transforms will be VERY bad. 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 // Get the greater of the local or global viscosity: 00073 PN_stdfloat viscosityDamper=1.0f-physical->get_viscosity(); 00074 00075 // Loop through each object in the set. This processing occurs in O(pf) time, 00076 // where p is the number of physical objects and f is the number of 00077 // forces. Unfortunately, no precomputation of forces can occur, as 00078 // each force is possibly contingent on such things as the position and 00079 // velocity of each physicsobject in the set. Accordingly, we have 00080 // to grunt our way through each one. wrt caching of the xform matrix 00081 // should help. 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 // bail out if this object doesn't exist or doesn't want to be 00089 // processed. 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; // mass dependent accumulation vector. 00099 LVector3 non_md_accum_vec; 00100 LVector3 accel_vec; 00101 LVector3 vel_vec; 00102 00103 // reset the accumulation vectors for this object 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 // run through each acting force and sum it 00108 LVector3 f; 00109 // LMatrix4 force_to_object_xform; 00110 00111 LinearForceVector::const_iterator f_cur; 00112 00113 // global forces 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 // make sure the force is turned on. 00120 if (cur_force->get_active() == false) { 00121 continue; 00122 } 00123 00124 // now we go from force space to our object's space. 00125 f = cur_force->get_vector(current_object) * matrices[index++]; 00126 00127 physics_spam("child_integrate "<<f); 00128 // tally it into the accum vectors. 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 // local forces 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 // make sure the force is turned on. 00142 if (cur_force->get_active() == false) { 00143 continue; 00144 } 00145 00146 // go from force space to object space 00147 f = cur_force->get_vector(current_object) * matrices[index++]; 00148 00149 physics_spam("child_integrate "<<f); 00150 // tally it into the accum vectors 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 // get this object's physical info 00159 LPoint3 pos = current_object->get_position(); 00160 vel_vec = current_object->get_velocity(); 00161 PN_stdfloat mass = current_object->get_mass(); 00162 00163 // we want 'a' in F = ma 00164 // get it by computing F / m 00165 nassertv(mass != 0.0f); 00166 accel_vec = md_accum_vec / mass; 00167 accel_vec += non_md_accum_vec; 00168 00169 #if 0 //[ 00170 // step the position and velocity 00171 vel_vec += accel_vec * dt; 00172 00173 // cap terminal velocity 00174 PN_stdfloat len = vel_vec.length(); 00175 00176 if (len > current_object->get_terminal_velocity()) { 00177 //cout << "Capping terminal velocity at: " << current_object->get_terminal_velocity() << endl; 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 // x = x + v * t + 0.5 * a * t * t 00188 pos += vel_vec * dt + 0.5 * accel_vec * dt * dt; 00189 // v = v + a * t 00190 vel_vec += accel_vec * dt; 00191 #endif //] 00192 00193 // and store them back. 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 // Function : output 00205 // Access : Public 00206 // Description : Write a string representation of this instance to 00207 // <out>. 00208 //////////////////////////////////////////////////////////////////// 00209 void LinearEulerIntegrator:: 00210 output(ostream &out) const { 00211 #ifndef NDEBUG //[ 00212 out<<"LinearEulerIntegrator"; 00213 #endif //] NDEBUG 00214 } 00215 00216 //////////////////////////////////////////////////////////////////// 00217 // Function : write 00218 // Access : Public 00219 // Description : Write a string representation of this instance to 00220 // <out>. 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